[發(fā)明專利]一種基于自適應UKF的水下機器人狀態(tài)和參數(shù)聯(lián)合估計方法

有效

• 申請?zhí)枺?/td>: 201110190512.5
• 申請日: 2011-07-08
• 公開（公告）號: CN102862666A
• 公開（公告）日: 2013-01-09
• 發(fā)明（設計）人: 劉開周;程大軍;李一平;封錫盛
• 申請（專利權）人: 中國科學院沈陽自動化研究所
• 主分類號: B63C11/52
• 分類號: B63C11/52
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 許宗富

技術領域

本發(fā)明涉及一種基于自適應無色卡爾曼濾波(UKF，Unscented?Kalman?Filter)的水下機器人狀態(tài)和參數(shù)聯(lián)合估計方法，特別涉及一種基于自適應UKF的水下機器人狀態(tài)和推進器故障參數(shù)聯(lián)合估計方法。

技術背景

隨著海洋開發(fā)事業(yè)的迅速發(fā)展，水下施工和建設項目越來越多，對于水下作用手段的性能要求也越來越高。由于水下機器人能夠在水下進行觀察、攝影、打撈和施工作業(yè)，因此在海洋開發(fā)中得到廣泛的應用。而推進器的健康狀況決定著水下機器人能否很好完成預期的任務。傳統(tǒng)預編程的水下機器人在檢測到推進器有故障發(fā)生時一般采取拋載上浮等措施，其實在很多情況下，水下機器人的推進器并非完全失效，如果能夠實時的辨識出其效率損失因子，然后對推力進行重新分配，水下機器人仍可能完成預期的任務。

在水下機器人系統(tǒng)的噪聲信息已知的情況下，采用UKF算法能夠達到很好的估計效果，而在系統(tǒng)噪聲信息未知時，如果設置的噪聲參數(shù)與實際的噪聲相差過大，就會導致有偏估計甚至濾波算法的發(fā)散，因此要設計和采用更適合于估計系統(tǒng)噪聲信息的濾波算法，以達到實時的估計水下機器人系統(tǒng)的噪聲信息，在噪聲信息未知的情況下仍可以達到很好的估計效果。

發(fā)明內容

針對上述存在的問題，本發(fā)明提供了一種能實時估計推進器故障信息和系統(tǒng)噪聲信息，并具有實用性好、估算精度高的基于自適應UKF的水下機器人狀態(tài)和參數(shù)聯(lián)合估計方法

為實現(xiàn)本發(fā)明的目的，本發(fā)明采用如下方案：

一種基于自適應UKF的水下機器人狀態(tài)和參數(shù)聯(lián)合估計方法，其特征在于包括以下步驟：

在水下機器人動力學模型的基礎上，對推進器故障進行建模，得到離線的擴展參考模型；根據(jù)位姿傳感器探測的在線位姿信息y_k，采用自適應UKF的主濾波器為標準UKF濾波估計算法，結合水下機器人離線的擴展參考模型為擴展的系統(tǒng)狀態(tài)方程，對系統(tǒng)狀態(tài)和推進器故障組成的擴展狀態(tài)傳遞和更新，實時估計出系統(tǒng)的狀態(tài)和參數(shù)信息，得到系統(tǒng)估算的結果值均值和方差P_k；這里系統(tǒng)的狀態(tài)信息包括經(jīng)過濾波后的位姿信息s_k和實時估計出來的速度信息參數(shù)信息為系統(tǒng)推進器的故障參數(shù)b_k，同時，依據(jù)主濾波器計算估計的新息信息ξ_k計算出輔助濾波器的測量值Sq_k；建立UKF輔助濾波器的狀態(tài)方程和測量方程，依據(jù)測量值Sq_k作為系統(tǒng)的觀測信號，采用自適應UKF的輔助濾波器KF對水下機器人系統(tǒng)的噪聲信息進行實時估計，得到系統(tǒng)噪聲協(xié)方差陣D的對角線元素和方的估算值，這里的噪聲信息包括過程噪聲w_k和測量噪聲v_k，它們的協(xié)方差矩陣分別為D^w和D^v。

主濾波器擴展系統(tǒng)方程形成過程：

擴展非線性系統(tǒng)時，含有未知/時變推進器的故障參數(shù)b_k的系統(tǒng)狀態(tài)空間方程為：

x_k+1＝f(x_k，b_k，u_k)+w_k

b_k+1＝f_b(x_k，b_k，u_k)+w_kb

式中，b_k是k時刻的未知/時變推進器故障參數(shù)，w_kb為故障參數(shù)模型的系統(tǒng)推進器故障噪聲矢量，是零均值的高斯噪聲；

當故障參數(shù)b_k變化規(guī)律未知，則可將b_k視為不相關的隨機漂移矢量，其遞歸表達式為：

b_k＝b_k-1+w_bk

基于自適應UKF的聯(lián)合估計，將系統(tǒng)推進器的故障參數(shù)追加到系統(tǒng)的真實狀態(tài)后，組成擴展的狀態(tài)矩陣，即把系統(tǒng)方程組成為擴展系統(tǒng)狀態(tài)方程為：