水下滑翔機定位導航算法研究

摘 要： 介紹了水下滑翔機定位導航的特點，提出了水面-水下導航定位算法。通過Matlab進行擴展卡爾曼濾波仿真處理，證明了該算法的有效性及魯棒性。

關鍵字： 水下滑翔機； 導航定位； 卡爾曼濾波； Matlab仿真

中圖分類號： TN96?34 文獻標識碼： A 文章編號： 1004?373X（2014）01?0054?03

0 引 言

水下滑翔機是一種無外掛推進系統、通過內置執行機構調整重心位置和凈浮力來控制其運動的新型水下航行器，主要用于長時間、大范圍的海洋環境監測。與傳統水下航行器相比，水下滑翔機具有作業時間長、航行距離大、作業費用低和對母船的依靠性小等優點[1]。精確的導航能力是水下滑翔機有效應用和安全回收的一個關鍵技術[2]。然而，水下滑翔機成本較低，所攜帶的傳感器數量和種類較少，同時需要長時間，大航程巡航，實現精確導航定位是不易的。

本文針對水下滑翔機導航定位的特點，提出了水面?水下導航定位算法，即水面通過GPS提供滑翔機的位置信息，水下采用電子羅盤和AHRS提供的滑翔機運動姿態信息[3]，結合數據融合理論[4]，運用擴展卡爾曼濾波對滑翔機進行航位推算。通過Matlab進行仿真處理，證明了該算法的有效性及魯棒性。

1 導航基本算法

導航定位算法示意圖如圖1所示。

圖1中設滑翔機位于[P（λ1，φ1）]點，搜索目標點[D（λ2，φ2），]其中[（λ，φ）]為點的經度、緯度。滑翔機要航行的東向和北向距離（單位：m）分別為：

[dy=kφ（φ2-φ1）] （1）

[dx=kλ（λ2-λ1）] （2）

式中：[kφ，kλ]為距離標度因子（單位：m/s）：

[kφ=30.636 7+0.005 19φ] （3）

[kλ=kφcosφ+0.250 67-0.003 9φ] （4）

圖1 導航算法示意圖

2 坐標系的變換

由于水下滑翔機所攜帶的傳感器中，GPS使用的是WGS?84坐標系，電子羅盤和AHRS使用的是載體坐標系，這就給數據處理帶來了諸多不便。為解決這一問題，必須進行坐標系轉換，將所用的導航數據全部轉換到選定的坐標系中[5]。 以下坐標系變換都是將滑翔機從三位空間坐標[XYZ]投影到二維平面坐標系[XOY]中進行的。

2.1 地球坐標系

如圖2所示，該坐標系與地球固聯，坐標原點在地心，[Z]軸沿地球自轉軸且指向北極，[X]軸與[Y]軸在地球赤道平面內，[X]軸指向零子午線，[Y]軸指向東經90°方向。運動體在該坐標系內的定位采用經度[λ]和緯度[φ]及距地心距離[R]來標定。

圖2 地球坐標系

2.2 GPS坐標系轉換為地球坐標系

GPS使用的是WGS?84地心大地坐標系，將其中的點[（φ，λ，h）]轉換為地球坐標系中的點[（x，y，z）]的轉換關系如下：

[x=（N+h）cosφcosλy=（N+h）cosφsinλz=[N（1-e2）+h]sinφ] （5）

式中：[a]為地球長半軸，[b]為短半軸，[e2=（a2-b2）a2]為第一偏心率平方；[N=a（1-e2sin2φ）12]為東西圓曲率半徑[6]。

2.3 載體坐標系轉換為地球坐標系

水下滑翔機攜帶的電子羅盤和AHRS都是采用的載體坐標系（以當前航向所在方向作為[x]軸）。如圖3所示，[xy]表示載體坐標系，[XY]表示地球坐標系，三角形代表載體。載體坐標系轉換為地球坐標的推導如下：

[ΔY=Δxsinα+Δycosα] （6）

[ΔX=Δxcosα-Δysinα] （7）

[φK+1=φK+ΔXKφλK+1=λK+ΔYKλ] （8）

式中：從[k]時刻到[k+1]時刻，[Δx]表示載體在航向上的距離改變量，可以由AHRS在載體坐標系[x]軸方向上的加速度對時間積分求得，即：[Δx=12a2x；][Δy]表示在把航向逆時針旋轉90°后的垂直航向方向上的距離改變量， 可以由AHRS在載體坐標系[y]軸方向上的加速度對時間積分求得，即：[Δy=12a2y；][Δα]表示[k]時刻地球坐標系[X]軸正方向在逆時針方向上與載體坐標系[x]軸的正方向的夾角，[α=α+Δθ。]

圖3 載體坐標系與地球坐標系轉換

3 數據融合過程[7]

（1） 狀態向量：

[Xv=（x，y，α，ax，ay）T]

式中：[x，y]是地球坐標系下滑翔機的位置坐標；[α]地球坐標系[X]軸正方向在逆時針方向上與載體坐標系[x]軸的正方向的夾角；[ax，ay]為滑翔機載體坐標系下東向和北向的分加速度。

（2） 狀態方程：由[k-1]時刻狀態預測[k]時刻的狀態，對各個狀態參量求導：

[Gv=?f?Xxyαvxvy=x+ΔXy+ΔYα+Δθvx+axvy+ay]

（3） 觀測向量：

[Z=（ax，ay，Δθ）T]

式中：[ax，ay]可以由AHRS[8]測得[Δθ]為轉向角，可由電子羅盤[9]的俯仰測得。觀測向量誤差協方差：

[R=diag（δ2axδ2ayδ2Δθ）T]

觀測矩陣：

[H=0000000000000000001000001]

將狀態預測和觀測預測聯系起來，觀測預測：

[Z=H*XK]

（4） 控制量：

[u=（ax，ay）T]

對控制量求導：[Gu=?f?u]

控制量協方差矩陣：[Q=]diag（[δ2axδ2ay]）T

4 擴展卡爾曼濾波的執行過程

程序執行的流程圖如圖4所示。

圖4 流程圖

由上述過程最終得到EKF遞歸方程[10]。

觀測預測：[Z=H*XK]

新息：[V=][Z-][Z]

預測方程：狀態參量協方差估計

[PX=Gv*PX*G′v+Gu*Q*G′u]

更新方程：卡爾曼增益

[Kg=PX*H*H*PX*H+R-1]

滑翔機姿態更新：

[XK=XK+Kg*V]

狀態參量協方差矩陣更新：

[PX=PX-Kg*H*PX]

5 Matlab仿真及結果分析

系統初始化如下：

下潛速度為20 m/min，每30 min進行一次航位推算。傾角為20°，航向角為15°，俯仰角為0°。

初始位置點為：120.36°E，36.15°N。

經過3 h的航位推算（擴展卡爾曼濾波器）得到數據見表1。

由任意兩點[A（α1，β1），][B（α2，β2）]的距離公式：

[AB=R?arccos[cos β1cos β2cos（α1-α2）+sinβ1sinβ2]]

其中[R]為地球半徑，[α，β]分別為經度和緯度。

可以求出[S1-S2=]864 m，由預先設定可知水下滑翔機在虛擬水平面與初始點的距離為30×30×cos 20°=845 m，推算距離與實際距離相差19 m，符合系統對水下定位的要求。

表1 航位推算得到的數據表

[推算次數＼1＼2＼3＼4＼5＼6＼推算經度＼120.360 2＼120.362 7＼120.363 5＼120.365 4＼120.367 1＼120.371 5＼推算緯度＼36.157 5＼36.164 1＼36.168 3＼36.174 2＼36.185 7＼36.201 1＼]

對余下5個測試點進行驗證，可得其與實際距離差分別為：25 m，21 m，22 m，27 m，20 m，平均誤差在23 m左右。

仿真結果說明在模擬的環境中，水下滑翔機航位推算算法的有效性，同時擴展卡爾曼濾波器保證了航位推算誤差不隨時間增加而擴散，使得系統穩定。

參考文獻

[1] 程雪梅.水下滑翔機研究進展及關鍵技術[J].魚類技術，2009，17（6）：1?6.

[2] 董緒榮.一種低成本的GPS組合導航技術[J].指揮技術學院學報，2000（5）：18?20.

[3] FEEZOR M D， YATES S F， BLANKINSHIP P R， et al， Autonomous underwater vehicle homing/docking via electromagnetic guidance [J]. IEEE Journal of Oceanic Engineering， 2001， 26（4）： 515?521.

[4] 張愛軍.水下潛器組合導航定位及數據融合技術[D].南京：南京理工大學，2009.

[5] 鄧正隆.慣性技術[M]. 哈爾濱：哈爾濱工業大學出版社，2006.

[6] 朱海，莫軍.水下導航信息融合技術[M].北京：國防工業出版社， 2002.

[7] 莫軍，丁寧.GPS與基于海流數據庫的推算船位的數據融合[J].中國航海，2002，50（1）：31?36.

[8] SIPOS M， PACES P， REINSTEIN M， et al. Flight attitude track reconstruction using two AHRS units under laboratory conditions [C]// Proceedings of 2009 IEEE Sensor. Christchurch， New Zealand： IEEE， 2009： 675?678.

[9] 蔣賢志.數字電子羅盤誤差分析及校正技術研究[J].現代雷達，2005，27（6）：39?41.

[10] 付夢印，鄧志紅.Kalman濾波理論及其在導航系統中的應用[M].北京：科學出版社，2010.