專利名稱：一種小型水下機器人組合導航定位方法
技術領域：
本發明涉及的是一種小型水下機器人自主導航與自主校正方法。
背景技術：
小型水下機器人執行水下任務過程中，需為其控制系統和制導系統提供準確、平 滑的位置、深度、姿態以及載體系下的速度和加速度信息，并能自動上浮至水面進行自主校 正。小型水下機器人導航定位系統往往存在由海流或海浪干擾導致的系統模型失配問題， 并且導航定位系統一般僅能提供未經濾波的加速度、深度信息和導航系下的速度信息，這 些信息需經濾波和坐標變換到艇體系后方可被機器人控制系統使用。而取多路導航信號 的導航方法通常是簡單導航信息的備份，而沒有進行有效的信息融合。本發明采用強跟蹤 無跡卡爾曼濾波算法有效地解決了小型水下機器人導航定位系統的模型失配問題，對多路 導航設備提供的導航信息進行融合，提高了定位精度，平滑了航向、高度信息，并能直接為 控制系統提供平滑的艇體系下的速度與加速度信息。《控制與決策》(1990，第5期)“非線 性系統的帶次優漸消因子的擴展卡爾曼濾波”提出了強跟蹤濾波器有效解決了 EKF由于模 型失配導致的魯棒性差、濾波發散等問題，但要求非線性系統狀態函數和量測函數必須是 連續可微的，而且強跟蹤濾波器需要計算非線性函數的雅可比矩陣，不但過程繁瑣易出錯， 而且對非線性函數的線性化近似精度通常偏低，當系統具有強非線性時精度嚴重下降，甚 至發散。本發明采用的強跟蹤無跡卡爾曼濾波算法通過采用UT變換克服了上述問題。《航 空電子技術》0008，第39卷第4期)“強跟蹤UKF濾波在SINS/GPS組合導航中的應用研 究”，針對大機動條件下飛行器SINS/GPS組合導航系統的濾波發散問題，設計了強跟蹤無跡 卡爾曼濾波算法，通過漸消因子在線調整卡爾曼濾波增益從而提高無跡卡爾曼濾波算法的 跟蹤性能。《儀器儀表學報》0008，第四卷第8期)“強跟蹤UKF方法及其在故障辨識中 的應用”也基于強跟蹤無跡卡爾曼濾波算法設計了 GNSS/SINS組合導航系統。本發明采用 的強跟蹤無跡卡爾曼濾波算法同上述算法的漸消因子的計算方法不同，且更為合理。
發明內容
本發明的目的在于提供一種既能有效解決小型水下機器人因海流或海浪干擾而 導致的導航定位系統模型失配問題，又能平滑小型水下機器人控制系統所需的航向、深度 以及在艇體系下的速度與加速度信息的集自主水下導航定位與自主水面位置校正功能的 小型水下機器人導航定位方法。本發明的目的是這樣實現的本發明的小型水下機器人組合導航定位方法主要包括如下步驟1)采用強跟蹤 無跡卡爾曼濾波器進行組合導航系統的數據融合；幻取導航系下的位置矢量和航向角與 載體系下速度矢量和加速度矢量作為濾波器狀態矢量，濾波結果被機器人控制與制導系統 直接使用；3)取深度計輸出的深度信息同航姿參考系統輸出的航向角、姿態角和載體系下 的加速度信息與載體系下多普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實現水下機器人的自主導航與數據濾波；4)取GPS接收機輸出的水平位置信息，航姿參考系統 輸出的航向、姿態及載體系下加速度信息與載體系下多普勒測速聲納的速度信息，通過強 跟蹤無跡卡爾曼濾波器實現水下機器人的自主校正與數據濾波力)通過GPS接收機輸出信 號的有效位實現水上和水下兩個不同量測方程的切換。1、所述采用強跟蹤無跡卡爾曼濾波器進行組合導航系統數據融合的步驟如下系統噪聲方差陣為Q，觀測噪聲方差陣為R ；(1)選擇對稱點采樣策略，根據X(A)、P (k I k)計算k時刻Sigma點集{ χ J，其中i 1、...、L<;(2)計算經過非線性狀態方程++播后的Sigma點 Xi(k+l|k)，由X i (k+11 k)計算得狀態向量X (k) —步預測估計+
χ. (k +1 μ) = /(χ (k I k)) + GXsm (k)
權利要求
1.一種小型水下機器人組合導航定位方法，其特征是主要包括如下步驟1)采用強跟 蹤無跡卡爾曼濾波器進行組合導航系統的數據融合；幻取導航系下的位置矢量和航向角 與載體系下速度矢量和加速度矢量作為濾波器狀態矢量，濾波結果被機器人控制與制導系 統直接使用；；3)取深度計輸出的深度信息同航姿參考系統輸出的航向角、姿態角和載體系 下的加速度信息與載體系下多普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實 現水下機器人的自主導航與數據濾波；4)取GPS接收機輸出的水平位置信息，航姿參考系 統輸出的航向、姿態及載體系下加速度信息與載體系下多普勒測速聲納的速度信息，通過 強跟蹤無跡卡爾曼濾波器實現水下機器人的自主校正與數據濾波力)通過GPS接收機輸出 信號的有效位實現水上和水下兩個不同量測方程的切換。
2.根據權利要求1所述的小型水下機器人組合導航定位方法，其特征是所述采用強跟 蹤無跡卡爾曼濾波器進行組合導航系統數據融合的步驟如下系統噪聲方差陣為Q，觀測噪聲方差陣為R ；(1)選擇對稱點采樣策略，根據X@)、P(k |k)計算k時刻Sigma點集{ xj，其中i = 1、...、L<;(2)計算經過非線性狀態方程+= F(X(k)) + GXSUB(k)傳播后的Sigma點 Xi(k+l|k)，由χ i (k+11 k)計算得狀態向量X (k) —步預測估計+
3.根據權利要求2所述的小型水下機器人組合導航定位方法，其特征是所述取導航系 下的位置矢量和航向角與載體系下速度矢量和加速度矢量作為濾波器狀態矢量，濾波結果 被機器人控制與制導系統直接使用的步驟如下采用NED導航坐標系0-ΧΥΖ，以0作為原點，OZ軸指向地心，OX軸指北，OY軸指東；艇 體坐標系o-xyz定義如下，ox軸沿機器人縱軸，oy軸平行于基平面與ox軸垂直指向艇體 右舷，OZ軸垂直于基平面并與ox軸、oy軸構成右手直角坐標系；選取導航系下位置矢量 P和航向角Ψ與艇體系下的速度矢量V、加速度矢量A作為小型水下機器人運動連續時間 模型的狀態矢量；其中位置矢量為P= [PN(k)pE(k)pD(k)]T，速度矢量為V= [vxb(k) vyb(k) vzb(k)]T，加速度矢量為A=[知㈨ ㈨知㈨]、定義SAUV的橫滾角為Y，縱搖角為Θ， 航向角為Ψ，經歐拉角計算得到艇體系b到水平面坐標系h的姿態矩陣 cos0 sin θ sin γ sin θ cos γQa =0 cos 7 - sin γ -sin0 cos 0 sin 7 cos 0 cos 7無跡卡爾曼濾波器的狀態空間模型為
4.根據權利要求3所述的小型水下機器人組合導航定位方法，其特征是取深度計輸出 的深度信息同航姿參考系統輸出的航向角、姿態角和載體系下的加速度信息與載體系下多 普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實現水下機器人的自主導航與數 據濾波的步驟如下在機器人水下工作階段選取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)是均值為零與系統噪聲w(t)及狀態x(t)相互獨立的觀測白噪聲，滿足 cov (V (k)) = R。
5.根據權利要求4所述的小型水下機器人組合導航定位方法，其特征是取GPS接收機 輸出的水平位置信息，航姿參考系統輸出的航向、姿態及載體系下加速度信息與載體系下 多普勒測速聲納的速度信息，通過強跟蹤無跡卡爾曼濾波器實現水下機器人的自主校正與 數據濾波的步驟如下在機器人水面校正階段選取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)是均值為零與系統噪聲W(t)及狀態X(t)相互獨立的觀測白噪聲，滿足 cov (V (k)) = R。
6.根據權利要求5所述的小型水下機器人組合導航定位方法，其特征是通過GPS接收 機輸出信號的有效位進行控制具體步驟如下(1)通過串口實時采集GPS信號，按照數據格式從緩存中讀取GPS提供的位置信息；(2)判斷耐壓GPS接收機的導航數據有效位；(3)如果在連續10秒鐘內GPS導航數據有效則將觀測模型切換至水面校正模式即令H =I1(IX1。，否則保持觀測模型的水下導航模式H= [O8x2I8x8]不變。
全文摘要
本發明提供的是一種小型水下機器人組合導航定位方法。采用強跟蹤無跡卡爾曼濾波器對各種導航設備的測量數據加以融合；取導航系下的位置矢量和航向角與載體系下速度矢量和加速度矢量作為濾波器狀態矢量；通過強跟蹤無跡卡爾曼濾波器實現水下機器人的自主導航與數據濾波；取GPS接收機輸出的水平位置信息，通過強跟蹤無跡卡爾曼濾波器實現水下機器人的自主校正與數據濾波；通過GPS接收機輸出信號的有效位實現水上和水下兩個不同量測方程的切換。本發明可以融合各種導航設備測量到的小型水下機器人的位置、深度和姿態信息，在存在海流或海浪的干擾的情況下，實現小型水下機器人的自主導航與自主校正。
文檔編號G01C21/20GK102052924SQ20101055936
公開日2011年5月11日 申請日期2010年11月25日 優先權日2010年11月25日
發明者萬磊, 孫玉山, 張國成, 張強, 王建國 申請人:哈爾濱工程大學