專利名稱：一種小型水下機器人組合導(dǎo)航定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及的是一種小型水下機器人自主導(dǎo)航與自主校正方法。
背景技術(shù)：
小型水下機器人執(zhí)行水下任務(wù)過程中，需為其控制系統(tǒng)和制導(dǎo)系統(tǒng)提供準確、平 滑的位置、深度、姿態(tài)以及載體系下的速度和加速度信息，并能自動上浮至水面進行自主校 正。小型水下機器人導(dǎo)航定位系統(tǒng)往往存在由海流或海浪干擾導(dǎo)致的系統(tǒng)模型失配問題， 并且導(dǎo)航定位系統(tǒng)一般僅能提供未經(jīng)濾波的加速度、深度信息和導(dǎo)航系下的速度信息，這 些信息需經(jīng)濾波和坐標變換到艇體系后方可被機器人控制系統(tǒng)使用。而取多路導(dǎo)航信號 的導(dǎo)航方法通常是簡單導(dǎo)航信息的備份，而沒有進行有效的信息融合。本發(fā)明采用強跟蹤 無跡卡爾曼濾波算法有效地解決了小型水下機器人導(dǎo)航定位系統(tǒng)的模型失配問題，對多路 導(dǎo)航設(shè)備提供的導(dǎo)航信息進行融合，提高了定位精度，平滑了航向、高度信息，并能直接為 控制系統(tǒng)提供平滑的艇體系下的速度與加速度信息?！犊刂婆c決策》(1990，第5期)“非線 性系統(tǒng)的帶次優(yōu)漸消因子的擴展卡爾曼濾波”提出了強跟蹤濾波器有效解決了 EKF由于模 型失配導(dǎo)致的魯棒性差、濾波發(fā)散等問題，但要求非線性系統(tǒng)狀態(tài)函數(shù)和量測函數(shù)必須是 連續(xù)可微的，而且強跟蹤濾波器需要計算非線性函數(shù)的雅可比矩陣，不但過程繁瑣易出錯， 而且對非線性函數(shù)的線性化近似精度通常偏低，當系統(tǒng)具有強非線性時精度嚴重下降，甚 至發(fā)散。本發(fā)明采用的強跟蹤無跡卡爾曼濾波算法通過采用UT變換克服了上述問題?！逗?空電子技術(shù)》0008，第39卷第4期)“強跟蹤UKF濾波在SINS/GPS組合導(dǎo)航中的應(yīng)用研 究”，針對大機動條件下飛行器SINS/GPS組合導(dǎo)航系統(tǒng)的濾波發(fā)散問題，設(shè)計了強跟蹤無跡 卡爾曼濾波算法，通過漸消因子在線調(diào)整卡爾曼濾波增益從而提高無跡卡爾曼濾波算法的 跟蹤性能。《儀器儀表學(xué)報》0008，第四卷第8期)“強跟蹤UKF方法及其在故障辨識中 的應(yīng)用”也基于強跟蹤無跡卡爾曼濾波算法設(shè)計了 GNSS/SINS組合導(dǎo)航系統(tǒng)。本發(fā)明采用 的強跟蹤無跡卡爾曼濾波算法同上述算法的漸消因子的計算方法不同，且更為合理。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種既能有效解決小型水下機器人因海流或海浪干擾而 導(dǎo)致的導(dǎo)航定位系統(tǒng)模型失配問題，又能平滑小型水下機器人控制系統(tǒng)所需的航向、深度 以及在艇體系下的速度與加速度信息的集自主水下導(dǎo)航定位與自主水面位置校正功能的 小型水下機器人導(dǎo)航定位方法。本發(fā)明的目的是這樣實現(xiàn)的本發(fā)明的小型水下機器人組合導(dǎo)航定位方法主要包括如下步驟1)采用強跟蹤 無跡卡爾曼濾波器進行組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合；幻取導(dǎo)航系下的位置矢量和航向角與 載體系下速度矢量和加速度矢量作為濾波器狀態(tài)矢量，濾波結(jié)果被機器人控制與制導(dǎo)系統(tǒng) 直接使用；3)取深度計輸出的深度信息同航姿參考系統(tǒng)輸出的航向角、姿態(tài)角和載體系下 的加速度信息與載體系下多普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主導(dǎo)航與數(shù)據(jù)濾波；4)取GPS接收機輸出的水平位置信息，航姿參考系統(tǒng) 輸出的航向、姿態(tài)及載體系下加速度信息與載體系下多普勒測速聲納的速度信息，通過強 跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主校正與數(shù)據(jù)濾波力)通過GPS接收機輸出信 號的有效位實現(xiàn)水上和水下兩個不同量測方程的切換。1、所述采用強跟蹤無跡卡爾曼濾波器進行組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合的步驟如下系統(tǒng)噪聲方差陣為Q，觀測噪聲方差陣為R ；(1)選擇對稱點采樣策略，根據(jù)X(A)、P (k I k)計算k時刻Sigma點集{ χ J，其中i 1、...、L<;(2)計算經(jīng)過非線性狀態(tài)方程++播后的Sigma點 Xi(k+l|k)，由X i (k+11 k)計算得狀態(tài)向量X (k) —步預(yù)測估計+
χ. (k +1 μ) = /(χ (k I k)) + GXsm (k)
權(quán)利要求
1.一種小型水下機器人組合導(dǎo)航定位方法，其特征是主要包括如下步驟1)采用強跟 蹤無跡卡爾曼濾波器進行組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合；幻取導(dǎo)航系下的位置矢量和航向角 與載體系下速度矢量和加速度矢量作為濾波器狀態(tài)矢量，濾波結(jié)果被機器人控制與制導(dǎo)系 統(tǒng)直接使用；；3)取深度計輸出的深度信息同航姿參考系統(tǒng)輸出的航向角、姿態(tài)角和載體系 下的加速度信息與載體系下多普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實 現(xiàn)水下機器人的自主導(dǎo)航與數(shù)據(jù)濾波；4)取GPS接收機輸出的水平位置信息，航姿參考系 統(tǒng)輸出的航向、姿態(tài)及載體系下加速度信息與載體系下多普勒測速聲納的速度信息，通過 強跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主校正與數(shù)據(jù)濾波力)通過GPS接收機輸出 信號的有效位實現(xiàn)水上和水下兩個不同量測方程的切換。
2.根據(jù)權(quán)利要求1所述的小型水下機器人組合導(dǎo)航定位方法，其特征是所述采用強跟 蹤無跡卡爾曼濾波器進行組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合的步驟如下系統(tǒng)噪聲方差陣為Q，觀測噪聲方差陣為R ；(1)選擇對稱點采樣策略，根據(jù)X@)、P(k |k)計算k時刻Sigma點集{ xj，其中i = 1、...、L<;(2)計算經(jīng)過非線性狀態(tài)方程+= F(X(k)) + GXSUB(k)傳播后的Sigma點 Xi(k+l|k)，由χ i (k+11 k)計算得狀態(tài)向量X (k) —步預(yù)測估計+
3.根據(jù)權(quán)利要求2所述的小型水下機器人組合導(dǎo)航定位方法，其特征是所述取導(dǎo)航系 下的位置矢量和航向角與載體系下速度矢量和加速度矢量作為濾波器狀態(tài)矢量，濾波結(jié)果 被機器人控制與制導(dǎo)系統(tǒng)直接使用的步驟如下采用NED導(dǎo)航坐標系0-ΧΥΖ，以0作為原點，OZ軸指向地心，OX軸指北，OY軸指東；艇 體坐標系o-xyz定義如下，ox軸沿機器人縱軸，oy軸平行于基平面與ox軸垂直指向艇體 右舷，OZ軸垂直于基平面并與ox軸、oy軸構(gòu)成右手直角坐標系；選取導(dǎo)航系下位置矢量 P和航向角Ψ與艇體系下的速度矢量V、加速度矢量A作為小型水下機器人運動連續(xù)時間 模型的狀態(tài)矢量；其中位置矢量為P= [PN(k)pE(k)pD(k)]T，速度矢量為V= [vxb(k) vyb(k) vzb(k)]T，加速度矢量為A=[知㈨ ㈨知㈨]、定義SAUV的橫滾角為Y，縱搖角為Θ， 航向角為Ψ，經(jīng)歐拉角計算得到艇體系b到水平面坐標系h的姿態(tài)矩陣 cos0 sin θ sin γ sin θ cos γQa =0 cos 7 - sin γ -sin0 cos 0 sin 7 cos 0 cos 7無跡卡爾曼濾波器的狀態(tài)空間模型為
4.根據(jù)權(quán)利要求3所述的小型水下機器人組合導(dǎo)航定位方法，其特征是取深度計輸出 的深度信息同航姿參考系統(tǒng)輸出的航向角、姿態(tài)角和載體系下的加速度信息與載體系下多 普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主導(dǎo)航與數(shù) 據(jù)濾波的步驟如下在機器人水下工作階段選取Z (t) = [pD (t) Vxb (t) Vyb (t) ^b (t) axb (t) ayb (t) b (t) Ψ (t) ]τ 作為觀測向量，觀測模型為Z(t) = HX (t) +V (t)H — [O8X2I8X8]；V(t)是均值為零與系統(tǒng)噪聲w(t)及狀態(tài)x(t)相互獨立的觀測白噪聲，滿足 cov (V (k)) = R。
5.根據(jù)權(quán)利要求4所述的小型水下機器人組合導(dǎo)航定位方法，其特征是取GPS接收機 輸出的水平位置信息，航姿參考系統(tǒng)輸出的航向、姿態(tài)及載體系下加速度信息與載體系下 多普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主校正與 數(shù)據(jù)濾波的步驟如下在機器人水面校正階段選取Z(t) = [pN (t) pE (t) pD (t) vxb (t) vyb(t)vzb (t) axb (t) ayb (t) azb (t) ￥(0]工作為觀測向量，觀測模型為Z(t) = HX (t) +V (t)H — IlOXlO ；V(t)是均值為零與系統(tǒng)噪聲W(t)及狀態(tài)X(t)相互獨立的觀測白噪聲，滿足 cov (V (k)) = R。
6.根據(jù)權(quán)利要求5所述的小型水下機器人組合導(dǎo)航定位方法，其特征是通過GPS接收 機輸出信號的有效位進行控制具體步驟如下(1)通過串口實時采集GPS信號，按照數(shù)據(jù)格式從緩存中讀取GPS提供的位置信息；(2)判斷耐壓GPS接收機的導(dǎo)航數(shù)據(jù)有效位；(3)如果在連續(xù)10秒鐘內(nèi)GPS導(dǎo)航數(shù)據(jù)有效則將觀測模型切換至水面校正模式即令H =I1(IX1。，否則保持觀測模型的水下導(dǎo)航模式H= [O8x2I8x8]不變。
全文摘要
本發(fā)明提供的是一種小型水下機器人組合導(dǎo)航定位方法。采用強跟蹤無跡卡爾曼濾波器對各種導(dǎo)航設(shè)備的測量數(shù)據(jù)加以融合；取導(dǎo)航系下的位置矢量和航向角與載體系下速度矢量和加速度矢量作為濾波器狀態(tài)矢量；通過強跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主導(dǎo)航與數(shù)據(jù)濾波；取GPS接收機輸出的水平位置信息，通過強跟蹤無跡卡爾曼濾波器實現(xiàn)水下機器人的自主校正與數(shù)據(jù)濾波；通過GPS接收機輸出信號的有效位實現(xiàn)水上和水下兩個不同量測方程的切換。本發(fā)明可以融合各種導(dǎo)航設(shè)備測量到的小型水下機器人的位置、深度和姿態(tài)信息，在存在海流或海浪的干擾的情況下，實現(xiàn)小型水下機器人的自主導(dǎo)航與自主校正。
文檔編號G01C21/20GK102052924SQ20101055936
公開日2011年5月11日 申請日期2010年11月25日 優(yōu)先權(quán)日2010年11月25日
發(fā)明者萬磊, 孫玉山, 張國成, 張強, 王建國 申請人:哈爾濱工程大學(xué)