基于衛星/INS的仿生機器魚組合導航系統

周 勇，文小玲，胡 慧，羅心睿

（1.武漢工程大學 電氣信息學院，武漢430205；2.武漢工程大學 藝術與設計學院，武漢430205；3.武漢紡織大學 電子與電氣工程學院，武漢430073）

導航定位和路徑規劃是仿生機器魚的關鍵技術。由于紅外、聲納、電磁信號在水下會嚴重衰減，使得一些適用于地面和天空的導航定位方法不適用于仿生機器魚的導航定位，因此仿生機器魚的導航與定位已成為仿生機器魚研究的熱點問題[1]。慣性導航系統（inertial navigation system，INS）具有短時間內精度高的特點，可用于水下機器魚的定位。INS 通過對安裝在仿生機器魚體內的3 個相互垂直的加速度計和3 個相互垂直的陀螺儀的測量值進行積分來獲得仿生機器魚的瞬時速度和位置[2]。但是，由于加速度計和陀螺儀的漂移，仿生機器魚的速度和姿態角誤差隨時間呈線性累積、位置誤差隨時間呈平方累積[3]，因此需要將INS 和其他導航方法組合來消除INS 的誤差，提高機器魚的定位精度。常見的組合導航方法有INS 和全球定位系統、INS 和聲學定位系統、INS 和地球物理導航系統、INS 和多普勒測速儀系統、INS 和及時定位與地圖重建系統[4]。

考慮到仿生機器魚的體積和應用場景，本文選擇INS 和衛星組合的導航方法，并且通過卡爾曼濾波算法得到比較精準的導航信息。

1 導航信息測量方法

ATGM332 測量位置和速度方法主要是測量衛星和仿生機器魚之間的距離，通過距離計算出機器魚的位置信息，再通過位移的導數求得瞬時速度[5]。距離方程可表示為

式中：x1，y1，z1為衛星1 的位置信息；x0，y0，z0為機器魚的位置信息；r1為衛星1 和機器魚之間的距離，可以通過計算衛星和機器魚內ATGM332 信號的時間差乘以光速得出；Δr 為時間誤差引起的距離誤差。由于x0，z0，Δr 均未知，故最少需要4 顆衛星才能得到機器魚位置信息。

衛星定位原理如圖1所示，當芯片接收到多個衛星信號時，每4 個衛星為一組求解方程，從中選出誤差最小的一組作為位置信息。在得到位置信息之后，可通過對短時間內的位移求導得到速度信息。機器魚內部的ATGM332 遵循NMEA 協議，因此可以通過NMEA 協議將位置和速度信息解碼成地理坐標系下的經緯高和速度[6]。

圖1 衛星定位原理圖Fig.1 Satellite positioning schematic diagram

INS 通過直接使用安裝在仿生機器魚上的加速度計和陀螺儀得到加速度和角速度，通過角速度得到載體參考坐標系下的四元數，通過載體參考坐標系四元數反求出載體參考坐標系的歐拉角以及姿態矩陣，使用角速度更新姿態矩陣，根據加速度和姿態矩陣計算導航參考坐標系下的加速度，再對加速度和角速度積分，即可得到瞬時速度和瞬時位置[7]。

首先引入載體參考系姿態四元數：

已知四元數的情況下，將ZYX 歐拉角繞Z 軸、Y 軸、X 軸分別旋轉α，β，γ 角度，將旋轉角度轉換成四元數得：

根據公式（3）求逆解得載體坐標系下的歐拉角為

在組合導航系統中，需要把載體坐標系中的加速度、速度、角速度轉換成導航坐標系中的加速度。

導航坐標系常選用地理坐標系，則根據ZYX 旋轉坐標變換得到變換矩陣為[8]

式中：s 表示sin；c 表示cos。則導航坐標系下的加速度an為

式中：ab為載體坐標系下的加速度；為載體的姿態矩陣。

2 組合導航和數據融合

本文采取的定位方法為衛星和INS 組合導航定位方法。組合導航的總體結構如圖2所示，圖中IMU 測量得到的位置是相對ATGM332 測量的初始位置，將ATGM332 和MPU6050 測量的速度和位置輸入到數據濾波器中，將濾波后的數據輸出作為機器魚的實際導航信息，可以實現衛星和INS 組合定位的功能[9]。

圖2 組合導航原理圖Fig.2 Combined navigation schematic

由于仿生機器魚在水下情況多變，且控制芯片的數據存儲量較小，符合卡爾曼濾波器在處理動態數據時的高效性以及求解時不需要存儲上一時刻數據的特點，故圖2中數據濾波器采用卡爾曼濾波器。通過卡爾曼濾波算法設計位置和速度融合算法，仿生機器魚在水下可分為水平勻速和上浮下潛運動，則ATGM332 測量的位移為X1=［xx，xy，xz］，IMU測量的加速度積分為X2=［Δxx，Δxy，Δxz］，即系統狀態量可變成：

因此卡爾曼濾波器的狀態方程和測量方程為

式中：Xk表示k 時刻的狀態；Zk表示k 時刻的測量狀態；A 為狀態變化矩陣。根據速度和位移的關系，得到，I3表示三維單位矩陣；Δt 表示采樣時間，根據仿生機器魚的運行流程可設定采樣時間為1 s，H 為觀測變換矩陣，根據測量值和實際值的關系，得到H=I6，I6表示六維單位矩陣。p（w），p（v）為估計誤差和測量誤差，都是零均值白噪聲，服從正態分布p（w）～N（0，Q），p（v）～N（0，R）。

卡爾曼濾波算法如下[10]：

3 仿真與水下實驗

3.1 算法仿真

仿生機器魚的游動可分為水平游動和上浮下潛游動。當機器魚在水下水平勻速游動時，有白噪聲誤差，設定初始狀態為［0 0 0.2 0.2］，由于估計誤差相對于測量誤差較小，因此設定測量誤差ν=［2.5 2.5 0.1 0.1 ］T，估計誤差ω=0.01 ［1 1 1 1 ］T，則Q=0.0001·I4，R=］，P0=I4，將上述參數輸入到卡爾曼濾波器，取前500 個點，仿真結果如圖3所示。

圖3 xy 軸數據融合仿真圖Fig.3 XY axis data fusion simulation diagram

當機器魚上浮下潛游動時，有白噪聲誤差，設定初始狀態為［0 0.05］，由于估計誤差相對于測量誤差較小，故設定測量誤差ν= ［2.5 0.1 ］T，估計誤差ω=0.01 ［1 1］T，則Q=0.01·0.01·I2，R=P0=I2，將上述參數輸入到卡爾曼濾波器，取前500 個點，仿真結果如圖4所示。

圖4 z 軸數據融合仿真圖Fig.4 Z axis data fusion simulation diagram

從圖3和圖4可得出，在測量值誤差較大的情況下，濾波后的速度和位移也能接近真實值，說明在ATGM332 信號較弱或者INS 累計誤差較大的情況下，依靠卡爾曼濾波算法也能得到較為精確的值。

3.2 水下實驗

本文使用的ATGM332 采用GPS 和北斗雙定位系統，外接一根有源天線。ATGM332 的性能指標如表1所示。ATGM332 和單片機STM32 相連，通過單片機使用NMEA 協議解碼ATGM332 接收的信息，得到ATGM332 所在的位置、速度等導航信息。

表1 ATGM332 性能指標Tab.1 ATGM332 performance indicator

IMU 使用的是MPU6050 模塊，該模塊能夠同時測量芯片的三軸加速度、角速度等運動數據。單片機可以獲得由芯片內部DMP 模塊數據融合濾波之后的值，根據濾波之后的值可以計算出姿態角、導航坐標系下的加速度以及姿態轉移矩陣。MPU6050的性能指標如表2所示。

表2 MPU6050 性能指標Tab.2 MPU6050 performance indicator

仿生機器魚的控制模式可分為手動控制模式和自動巡航避障模式，在實驗中使用手動控制模式，內部搭載無線LORA 模塊，用于接收控制信號以及向上位機傳輸相關的信息，通過控制舵機的轉動以及偏角可以實現前進和轉向，機器魚的直行速度最大為0.2 m/s，機器魚底部搭載大容量鋰電池，工作時間最長可達30 min。ATGM332，MPU6050 及仿生機器魚的實物如圖5所示。

圖5 實物圖Fig.5 Picture of real products

實驗地點選擇武漢工程大學靜心湖水域，控制機器魚在水下游動，將機器魚的導航信息通過LORA 模塊發送到上位機保存，進行仿真測試和水中定位實驗，仿真圖如圖6所示。從仿真圖可以看出，加速度積分的累計誤差越來越大，通過組合導航和卡爾曼濾波器能夠得到較為精準的導航信息。

圖6 水下實驗仿真圖Fig.6 Underwater experiment simulation diagram

4 結語

為了解決仿生機器魚水下定位困難的問題，本文使用ATGM332 和INS 組合導航的方法，并將測量的速度和位移數據進行卡爾曼濾波，以提高測量精度。仿真實驗以及水下實驗的結果表明，在因ATGM332 信號弱或INS 累計誤差較大而導致定位精度不準時，可以通過卡爾曼濾波算法，得到較為精準的值，提高了機器魚的定位精度。本方法雖然存在一些問題，但通過改進濾波器算法，可以實現更高精度的定位功能。