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

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

0 引 言

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

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

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

1 BDS在仿生機(jī)器魚中應(yīng)用的基本工作原理

1.1 仿生機(jī)器魚水下導(dǎo)航定位的基本原理

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

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

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

[(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)星分別與浮標(biāo)之間的距離;Δti為衛(wèi)星信號到達(dá)浮標(biāo)信號接收裝置的時(shí)間;Vti為衛(wèi)星時(shí)鐘差,由衛(wèi)星星歷提供；Vt0為浮標(biāo)時(shí)鐘差．再計(jì)算出仿生機(jī)器魚相對于浮標(biāo)的坐標(biāo)(X6,Y6,Z6)具體計(jì)算公式如下:

X=VΔt0,

(5)

X6=Xcosθ,

(6)

Y6=Xsinθ,

(7)

Z6=Xcos(90°-θ),

(8)

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

X0=X5+X6,

(9)

Y0=Y5+Y6,

(10)

Z0=Z5+Z6.

(11)

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

1.2 實(shí)現(xiàn)BDS水下定位的BDS浮標(biāo)設(shè)計(jì)

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

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

2 BDS在水下仿生機(jī)器魚中的具體應(yīng)用

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

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

本文所述仿生機(jī)器魚為本實(shí)驗(yàn)室自主設(shè)計(jì),主

圖3 機(jī)器魚的總體布局圖

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

2.2 位姿解算

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

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

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

(12)

(13)

(14)

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

2.3 路徑規(guī)劃

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

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

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

3 基于BDS的仿生機(jī)器魚硬件設(shè)計(jì)組成

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

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

圖5 仿生機(jī)器魚硬件設(shè)計(jì)圖

4 仿真與實(shí)驗(yàn)

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

圖6 兩種導(dǎo)航方式的緯度位置誤差

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

5 結(jié)束語

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