[發明專利]一種基于自適應UKF的水下機器人狀態和參數聯合估計方法

有效

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

1.一種基于自適應UKF的水下機器人狀態和參數聯合估計方法，其特征在于包括以下步驟：

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

2.根據權利要求1所述的一種基于自適應UKF的水下機器人狀態和參數聯合估計方法，其特征在于：主濾波器擴展系統方程形成過程：

擴展非線性系統時，含有未知/時變推進器的故障參數b_k的系統狀態空間方程為：

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時刻的未知/時變推進器故障參數，w_kb為故障參數模型的系統推進器故障噪聲矢量，是零均值的高斯噪聲；

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

b_k＝b_k-1+w_bk

基于自適應UKF的聯合估計，將系統推進器的故障參數追加到系統的真實狀態后，組成擴展的狀態矩陣，即把系統方程組成為擴展系統狀態方程為：

xk+1a=f~(xka,uk)+wka]]>

式中，為擴展的系統過程噪聲矢量，其中為系統過程噪聲矢量，為估計推進器故障噪聲矢量。

3.根據權利要求1或2所述的一種基于自適應UKF的水下機器人狀態和參數聯合估計方法，其特征在于：擴展參考模型中故障參數b_k的表示為k時刻第i個推進器的效率損失因子，取0～1數值表示推進器故障發生失效的程度。