崔秀芳,石福孝,李培培,陳剛,王宏宇

(上海海洋大學 工程學院,上海 201306)

0 引 言

北斗衛(wèi)星導航系統(tǒng)(BDS)是我國立足于國家經(jīng)濟和安全保護戰(zhàn)略,獨立研制運營的衛(wèi)星導航系統(tǒng)[1],具有高精度的導航定位功能,BDS的導航精度可達10 m,延時低于10 ns,測速精度0.2m/s,還能進行短報文通信,已經(jīng)具備全球服務能力．BDS還能兼容美國的GPS系統(tǒng)、俄羅斯的GLONASS以及歐盟的Galileo．BDS功能的日趨完善,促進了BDS在工業(yè)、氣象、農(nóng)業(yè)、汽車行業(yè)以及智能家具等相關行業(yè)的大范圍使用[2]．

仿生機器魚的設計整體本著安全高效,穩(wěn)定性好,靈活度高等原則[3]．它是一種智能化的產(chǎn)物,可以完成自主游動和集群游動,可以根據(jù)使用需求,對各個探測模塊進行自由組合,以實現(xiàn)不同的功能．仿生機器魚作為探測海底世界的機械,能夠在水下環(huán)境檢測、海底資源探索及開發(fā)利用等方面發(fā)揮自身特有的優(yōu)勢,而且隨著算法、人工智能(AI)以及定位導航技術(shù)的快速發(fā)展,其技術(shù)獲得進一步發(fā)展．其中,定位導航技術(shù)最為關鍵．BDS具有精準定位,短報文通信等功能,能夠與其他定位導航技術(shù)結(jié)合,應用于水下仿生機器魚的導航定位中,為仿生機器魚提供精準的導航定位及通信,為路徑規(guī)劃、位姿解算等提供技術(shù)上的支持．

張鈞凱等[4]提出采用多導航方式組合技術(shù)來提升水下機械定位導航的實時性和精確度,本文將文獻[4]中提出的技術(shù)運用于水下仿生機器魚,設計實驗驗證其相對于單一慣性導航具有更好的精確性．崔秀芳等[5]論述了BDS在船舶監(jiān)測領域中的應用,研究了BDS與船舶監(jiān)測系統(tǒng)的通信方式以及如何通過上位機將檢測信息由BDS衛(wèi)星發(fā)送給船舶控制系統(tǒng)．本文以文獻[5]中BDS與船舶監(jiān)測系統(tǒng)的通信方式為基礎,設計出BDS與仿生機器魚的通信過程．

1 BDS在仿生機器魚中應用的基本工作原理

1.1 仿生機器魚水下導航定位的基本原理

機器魚工作在水下,衛(wèi)星信號很難穿透水面,因此，本文以慣性導航技術(shù)和水聲定位技術(shù)作為BDS導航定位技術(shù)的補充,建立多導航方式組合系統(tǒng),使BDS導航定位技術(shù)成功地應用于水下.該系統(tǒng)通過四顆BDS衛(wèi)星接收浮標發(fā)出的信號,浮標與機器魚通過超聲波通信,然后通過BDS衛(wèi)星計算出機器魚的方位． 本組合系統(tǒng)中水面以上部分通過BDS收發(fā)信息,水面以下部分是水聲定位系統(tǒng)與仿生機器魚通過超聲波通信,仿生機器魚自身還運用了慣性導航定位技術(shù)．圖1為多導航方式組合技術(shù)圖．通過上述導航方式組合,并用卡爾曼濾波法進行濾波,可以大大提高仿生機器魚定位的精確性與實時性．

圖1 多導航方式組合技術(shù)圖

BDS衛(wèi)星定位原理具體如下:設仿生機器魚的坐標為(X0,Y0,Z0)為未知參數(shù),而用于定位的四顆衛(wèi)星瞬時位置坐標已知,分別為(Xi,Yi,Zi),i=1,2,3,4,BDS浮標的坐標為(X5,Y5,Z5),以已知的衛(wèi)星坐標為圓心,以四顆衛(wèi)星坐標到浮標坐標的距離為半徑,使用三維空間距離交匯的方法,先計算出浮標的三維空間坐標位置,具體計算公式如下:

[(X1-X5)2+(Y1-Y5)2+(Z1-Z5)2]1/2+

C(Vt1-Vt5)=D1,

(1)

[(X2-X5)2+(Y2-Y5)2+(Z2-Z5)2]1/2+

C(Vt2-Vt5)=D2,

(2)

[(X3-X5)2+(Y3-Y5)2+(Z3-Z5)2]1/2+

C(Vt3-Vt5)=D3,

(3)

[(X4-X5)2+(Y4-Y5)2+(Z4-Z5)2]1/2+

C(Vt4-Vt5)=D4,

(4)

Di=CΔti,(i=1,2,3,4).

式中:C為光速;即信號的傳播速度;Di為四顆衛(wèi)星分別與浮標之間的距離;Δti為衛(wèi)星信號到達浮標信號接收裝置的時間;Vti為衛(wèi)星時鐘差,由衛(wèi)星星歷提供；Vt0為浮標時鐘差．再計算出仿生機器魚相對于浮標的坐標(X6,Y6,Z6)具體計算公式如下:

X=VΔt0,

(5)

X6=Xcosθ,

(6)

Y6=Xsinθ,

(7)

Z6=Xcos(90°-θ),

(8)

式中:X為浮標與仿生機器魚的距離;V為聲波在水中的傳播速度;Δt0為浮標的聲波信號到達機器魚接收裝置的時間;θ為浮標與機器魚連成的直線與水面的夾角．

X0=X5+X6,

(9)

Y0=Y5+Y6,

(10)

Z0=Z5+Z6.

(11)

計算出仿生機器魚相對于浮標的坐標后,再由式(9)～(11)即可計算出仿生機器魚的三維坐標(X0,Y0,Z0)．

1.2 實現(xiàn)BDS水下定位的BDS浮標設計

浮標是實現(xiàn)BDS與水聲定位系統(tǒng)聯(lián)合導航定位的關鍵設備,是實現(xiàn)多導航方式組合的關鍵環(huán)節(jié)．該種浮標稱為BDS智能浮標,其自身帶有控制系統(tǒng),將BDS衛(wèi)星應答器連接到控制器上,用來接收BDS衛(wèi)星信號,將聲吶設備也連接在控制器上,接收機器魚發(fā)出的超聲波信號,浮標通過無線藍牙設備與上位機通信．圖2為BDS浮標系統(tǒng)結(jié)構(gòu)圖．浮標為BDS與機器魚起到了通信中繼的作用,該設備是BDS應用于水下必不可少的一環(huán)．

圖2 BDS浮標系統(tǒng)結(jié)構(gòu)圖

2 BDS在水下仿生機器魚中的具體應用

BDS應用于仿生機器魚主要涉及到導航(路徑規(guī)劃)、定位(位姿解算)、傳感器數(shù)據(jù)融合、運動控制等幾項子技術(shù),本文主要論述了仿生機器魚在前兩方面的具體應用．

2.1 仿生機器魚系統(tǒng)組成結(jié)構(gòu)

本文所述仿生機器魚為本實驗室自主設計,主

圖3 機器魚的總體布局圖

要有三個部分:頭部、身體骨架和尾鰭．整個身體采用流線型,圖3所示為機器魚的總體布局圖．電池、控制元件與導航定位模塊(慣性導航模塊和超聲波接收器)安裝于機器魚身中間的上盒體內(nèi),其中,導航定位模塊是整條機器魚的心臟,它負責整個機器魚的通信以及導航定位,浮潛舵機和重塊安裝在機器魚的下盒體內(nèi),這樣可以降低整個機器魚的重心,使機器魚在水下保持直立而不傾覆,機器魚尾部采用兩關節(jié)骨架,使整個尾部具有柔韌性,提高了仿生效果．

2.2 位姿解算

基于BDS的仿生機器魚的一個重要應用是位姿解算,通過BDS快速定位與全球地理信息系統(tǒng)(GIS)結(jié)合,再運用相應的算法實時解算出運動中機器魚的實時姿態(tài)信息．由于仿生機器魚處于水下,電磁波在水下會慢慢減弱,而聲波不會,所以依靠水為介質(zhì),能夠很好地傳遞信息,這點和陸地機器人位姿解算有所區(qū)別.針對此特點,本文將尋找一種適用于水下機械通訊的位姿解算算法．文獻[6]中提出了幾種位姿解算的典型算法,比如:四元數(shù)法、方向余弦算法等,其中特別適合水下環(huán)境的是四元數(shù)算法．

本文仿生機器魚位姿解算算法使用的是四元數(shù)法,四元數(shù)法在水下受到的干擾較小,所以特別適合仿生機器魚．四元數(shù)的四個分量與機器魚相對姿態(tài)角θ(俯仰)、φ(滾動)、ψ(偏航)的關系表達式如下:

θ=arcsin[2(q0q2-q1q3)],

(12)

(13)

(14)

式中:q0、q1、q2、q3表示四元數(shù)的4個分量,其值是由機器魚的定位坐標和四元數(shù)微分方程聯(lián)合計算所得.通過公式解出θ、φ、ψ,即可得出仿生機器魚的運動姿態(tài)．

2.3 路徑規(guī)劃

基于BDS的仿生機器魚另一個重要應用是路徑規(guī)劃,該功能在本系統(tǒng)中起著重要作用．BDS能夠計算出機器魚游動路線中特定點的坐標,然后用相應的算法為機器魚制定出最優(yōu)或者次優(yōu)路徑,最后由浮標發(fā)送給機器魚執(zhí)行．路徑規(guī)劃簡單來講,就是人們預設了路徑,讓機器魚在沿途躲過障礙物而無碰撞地到達終點[7]．和陸地機器人相似,仿生機器魚路徑規(guī)劃也可以分成兩種,一種是環(huán)境已知的情況下,比如投籃機器人投籃的過程均為預先設計的路線,此過程為全局路徑規(guī)劃;另一種是環(huán)境未知的情況下,例如仿生探索機器魚等在傳感器信息的引導下自主地選擇出最優(yōu)的探測路徑,此過程為局部路徑規(guī)劃．基于BDS的仿生機器魚適合很多路徑規(guī)劃算法,比如:模糊法、遺傳算法、數(shù)學幾何法等．

本文仿生機器魚路徑規(guī)劃功能實現(xiàn)使用的是模糊邏輯算法,其過程為:先利用BDS采集機器魚定位數(shù)據(jù),然后將數(shù)據(jù)模糊化,再按照模糊規(guī)則進行解模糊,最終實現(xiàn)運動控制．模糊邏輯算法的流程圖如圖4所示．其工作的主要決定對象是在未知情況下進行的,該算法計算量較小,原理簡單,易實現(xiàn)．

圖4 模糊邏輯算法的流程圖

3 基于BDS的仿生機器魚硬件設計組成

仿生機器魚在淺海地區(qū)的游動過程,涉及到的硬件有通信模塊、失控自救模塊、能源及控制模塊、探測模塊等．只有所有模塊全部正常工作的情形下,機器魚才能完整地完成預設的各項動作,更好地滿足消費者的需求．

基于BDS的仿生機器魚,利用 BDS定位導航模塊來解算其狀態(tài),進而進行姿態(tài)解算和控制動力部件——舵機,BDS定位導航模塊將機器魚的位置、姿態(tài)等相關數(shù)據(jù)和參數(shù)傳給上位機進行反饋,通過上位機發(fā)送指令來協(xié)調(diào)機器魚當前執(zhí)行的作業(yè)任務,為機器魚規(guī)劃出最優(yōu)航行路線,然后將最優(yōu)航行路線發(fā)送給BDS浮標,BDS浮標將控制信息發(fā)送給仿生機器魚控制模塊,控制模塊將信息分析處理后發(fā)送給相應的舵機,讓各個舵機實現(xiàn)不同角度的旋轉(zhuǎn),從而按照事先規(guī)劃好的路徑進行游動．基于BDS的仿生機器魚硬件設計如圖5所示．

圖5 仿生機器魚硬件設計圖

4 仿真與實驗

為了驗證本文所述以BDS技術(shù)為主,以水聲定位和慣性導航技術(shù)為補充的組合導航的定位精度[8],現(xiàn)設計仿真實驗.由于水下物體的深度可以用水深傳感器測出,用此方法測得的深度結(jié)果精度較高,而且能夠減小定位系統(tǒng)的計算量[9],因此，用作對比驗證．基于此方法對本文所述仿生機器魚的北向與東向的定位實施仿真．在仿真過程中,設系統(tǒng)噪音和觀察噪音都是高斯白噪音,陀螺時間漂移為0.1°/h,陀螺白噪聲漂移為0.1°/h,加速度計零偏為10-4g．以東北向的地表坐標系為定位坐標系,為了驗證單一慣性導航的發(fā)散性,將組合導航誤差和單一慣性導航誤差曲線列出,圖6、7分別示出了兩種導航方式的經(jīng)度及緯度位置誤差的仿真結(jié)果．

圖6 兩種導航方式的緯度位置誤差

圖7 兩種導航方式的經(jīng)度位置誤差

5 結(jié)束語

本文介紹了BDS應用于水下仿生機器魚定位導航的基本原理,及以BDS為主的多導航方式組合技術(shù),重點論述了BDS導航技術(shù)在仿生機器魚中的具體應用,包括本實驗室自主設計的仿生機器魚的硬件設計組成、基于BDS來控制仿生機器魚的運動、在自主設計的仿生機器魚身上實現(xiàn)路徑規(guī)劃和位姿解算等功能．本文所介紹的以BDS為主的多導航方式組合導航技術(shù)可推進水下導航技術(shù)的發(fā)展,為高精度定位的仿生機器魚定位系統(tǒng)的研究和各種控制算法的實現(xiàn)提供參考．隨著通信以及機器人技術(shù)的迅速發(fā)展,定位導航方式將會越來越多元化,高精度定位的水下仿生機器魚定位導航系統(tǒng)也將會更加智能化和普及化．