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

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

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

考慮到仿生機器魚的體積和應(yīng)用場景，本文選擇INS 和衛(wèi)星組合的導(dǎo)航方法，并且通過卡爾曼濾波算法得到比較精準(zhǔn)的導(dǎo)航信息。

1 導(dǎo)航信息測量方法

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

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

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

圖1 衛(wèi)星定位原理圖Fig.1 Satellite positioning schematic diagram

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

首先引入載體參考系姿態(tài)四元數(shù)：

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

根據(jù)公式（3）求逆解得載體坐標(biāo)系下的歐拉角為

在組合導(dǎo)航系統(tǒng)中，需要把載體坐標(biāo)系中的加速度、速度、角速度轉(zhuǎn)換成導(dǎo)航坐標(biāo)系中的加速度。

導(dǎo)航坐標(biāo)系常選用地理坐標(biāo)系，則根據(jù)ZYX 旋轉(zhuǎn)坐標(biāo)變換得到變換矩陣為[8]

式中：s 表示sin；c 表示cos。則導(dǎo)航坐標(biāo)系下的加速度an為

式中：ab為載體坐標(biāo)系下的加速度；為載體的姿態(tài)矩陣。

2 組合導(dǎo)航和數(shù)據(jù)融合

本文采取的定位方法為衛(wèi)星和INS 組合導(dǎo)航定位方法。組合導(dǎo)航的總體結(jié)構(gòu)如圖2所示，圖中IMU 測量得到的位置是相對ATGM332 測量的初始位置，將ATGM332 和MPU6050 測量的速度和位置輸入到數(shù)據(jù)濾波器中，將濾波后的數(shù)據(jù)輸出作為機器魚的實際導(dǎo)航信息，可以實現(xiàn)衛(wèi)星和INS 組合定位的功能[9]。

圖2 組合導(dǎo)航原理圖Fig.2 Combined navigation schematic

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

因此卡爾曼濾波器的狀態(tài)方程和測量方程為

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

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

3 仿真與水下實驗

3.1 算法仿真

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

圖3 xy 軸數(shù)據(jù)融合仿真圖Fig.3 XY axis data fusion simulation diagram

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

圖4 z 軸數(shù)據(jù)融合仿真圖Fig.4 Z axis data fusion simulation diagram

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

3.2 水下實驗

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

表1 ATGM332 性能指標(biāo)Tab.1 ATGM332 performance indicator

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

表2 MPU6050 性能指標(biāo)Tab.2 MPU6050 performance indicator

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

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

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

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

4 結(jié)語

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