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

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

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

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

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

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

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

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

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

INS 通過直接使用安裝在仿生機(jī)器魚上的加速度計和陀螺儀得到加速度和角速度，通過角速度得到載體參考坐標(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ù)輸出作為機(jī)器魚的實際導(dǎo)航信息，可以實現(xiàn)衛(wèi)星和INS 組合定位的功能[9]。

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

由于仿生機(jī)器魚在水下情況多變，且控制芯片的數(shù)據(jù)存儲量較小，符合卡爾曼濾波器在處理動態(tài)數(shù)據(jù)時的高效性以及求解時不需要存儲上一時刻數(shù)據(jù)的特點，故圖2中數(shù)據(jù)濾波器采用卡爾曼濾波器。通過卡爾曼濾波算法設(shè)計位置和速度融合算法，仿生機(jī)器魚在水下可分為水平勻速和上浮下潛運動，則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ù)仿生機(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 算法仿真

仿生機(jī)器魚的游動可分為水平游動和上浮下潛游動。當(dāng)機(jī)器魚在水下水平勻速游動時，有白噪聲誤差，設(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)機(jī)器魚上浮下潛游動時，有白噪聲誤差，設(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 和單片機(jī)STM32 相連，通過單片機(jī)使用NMEA 協(xié)議解碼ATGM332 接收的信息，得到ATGM332 所在的位置、速度等導(dǎo)航信息。

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

IMU 使用的是MPU6050 模塊，該模塊能夠同時測量芯片的三軸加速度、角速度等運動數(shù)據(jù)。單片機(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

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

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

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

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

4 結(jié)語

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