吳長進(jìn)，劉慧婷

(1.揚(yáng)子江船業(yè)集團(tuán)公司，江蘇 靖江 214500; 2.江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212003)

MC-ROV導(dǎo)航系統(tǒng)研究

吳長進(jìn)1，劉慧婷2

(1.揚(yáng)子江船業(yè)集團(tuán)公司，江蘇 靖江 214500; 2.江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212003)

針對研制的新型模態(tài)切換水下機(jī)器人(Mode-Converted ROV, MC-ROV)，設(shè)計(jì)了一套以MEMS器件為主的微慣性導(dǎo)航系統(tǒng)，包括三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)及高精度深度傳感器；基于加速度計(jì)和陀螺儀的頻域互補(bǔ)原理，系統(tǒng)首先運(yùn)用互補(bǔ)濾波算法融合傳感器數(shù)據(jù)，補(bǔ)償陀螺漂移，提高系統(tǒng)動(dòng)態(tài)性能；然后分析了四元數(shù)微分方程，以四元數(shù)估算水下機(jī)器人的角度大小和方向，可顯著減小計(jì)算量；最后采用了改進(jìn)的漸消記憶指數(shù)加權(quán)自適用卡爾曼濾波器，增大新近數(shù)據(jù)的作用，相應(yīng)減小陳舊數(shù)據(jù)的作用，有效避免濾波發(fā)散，提高導(dǎo)航精度；水池實(shí)驗(yàn)表明，在持續(xù)震動(dòng)和電磁干擾等惡劣環(huán)境下，利用互補(bǔ)濾波和改進(jìn)的自適應(yīng)卡爾曼濾波，水下機(jī)器人依然能夠獲得準(zhǔn)確的、高可靠性的水下姿態(tài)信息。

水下機(jī)器人；微慣性導(dǎo)航；四元數(shù)；自適應(yīng)卡爾曼

0 引言

開發(fā)海洋資源、進(jìn)行漁業(yè)監(jiān)測、對水下機(jī)構(gòu)定期檢測等等都對當(dāng)今水下作業(yè)工具的功能和性能提出了更高的要求。由于人工作業(yè)危險(xiǎn)性大、成本高，因此開發(fā)靈活、可靠的水下工具成為當(dāng)今研究的一個(gè)熱點(diǎn)。

有纜遙控水下機(jī)器人(remotely operated vehicle，ROV)是一種通過臍帶電纜與水面支持平臺聯(lián)系的水下作業(yè)機(jī)器人，是海洋工程水下結(jié)構(gòu)安全檢測及維護(hù)的關(guān)鍵裝備[1]。文中所述水下機(jī)器人，是模態(tài)切換水下機(jī)器人(mode-converted ROV, MC-ROV)，可在浮游和爬壁之間切換，是應(yīng)用于科考船的特種水下機(jī)器人。

精確導(dǎo)航在水下機(jī)器人系統(tǒng)中占據(jù)重要地位，是其正常工作的前提和保證?，F(xiàn)有的水下導(dǎo)航技術(shù)中，單一的方法都有各自固有的缺陷，而利用兩種或以上技術(shù)組合成的組合導(dǎo)航可以互相彌補(bǔ)缺點(diǎn)，提高精度，成為當(dāng)今導(dǎo)航首選，備受青睞，是未來導(dǎo)航技術(shù)的重要發(fā)展方向[2-4]。在組合導(dǎo)航中，利用各種低精度的微機(jī)電系統(tǒng)(MEMS)器件，可設(shè)計(jì)精巧的、高精度和動(dòng)態(tài)性能的導(dǎo)航系統(tǒng)，因此被眾多研究人員重視[5-6]。

系統(tǒng)基于MEMS傳感器件，利用互補(bǔ)濾波和改進(jìn)的自適應(yīng)卡爾曼濾波對MC-ROV進(jìn)行導(dǎo)航姿態(tài)更新和估算，降低軸間干擾，抑制姿態(tài)角漂移，獲得良好的導(dǎo)航效果。

1 導(dǎo)航系統(tǒng)結(jié)構(gòu)及原理

通常來講，慣性導(dǎo)航是指測量運(yùn)動(dòng)載體的加速度和角速度，通過積分等方法推算出運(yùn)動(dòng)物體的導(dǎo)航信息，包括位置和角度。運(yùn)動(dòng)物體的坐標(biāo)系稱為載體坐標(biāo)系，其參照的坐標(biāo)系為參考坐標(biāo)系。

對 MC-ROV，載體坐標(biāo)系位于其幾何中心，參考坐標(biāo)系為常用的東北天坐標(biāo)系，兩坐標(biāo)系遵循右手螺旋定則。通過測量載體坐標(biāo)系的加速度和角速度，積分可獲得位置和角度，再將其變換到參考坐標(biāo)系，即可得到常規(guī)意義上的位置和角度。

MC-ROV導(dǎo)航系統(tǒng)由磁力計(jì)、陀螺儀、加速度計(jì)、深度傳感器、導(dǎo)航微處理器組成。陀螺儀和加速度計(jì)作為慣性傳感器，采集水下機(jī)器人的實(shí)時(shí)角速度和加速度，以估算位置和角度；磁力計(jì)測量磁場強(qiáng)度，對水下機(jī)器人的方位角進(jìn)行矯正；深度計(jì)通過壓力測算潛水深度。

MC-ROV導(dǎo)航系統(tǒng)主要用于實(shí)時(shí)估算姿態(tài)角和方位角，以實(shí)現(xiàn)各種復(fù)雜控制。各個(gè)傳感器的重要參數(shù)，即靈敏度和量程等信息如表1所示。

表1 導(dǎo)航器件

2 MC-ROV微慣性導(dǎo)航算法分析

系統(tǒng)首先運(yùn)用互補(bǔ)濾波降低陀螺漂移，再此基礎(chǔ)上，采用漸消記憶指數(shù)加權(quán)的自適應(yīng)卡爾曼濾波算法對MC-ROV導(dǎo)航姿態(tài)實(shí)時(shí)解算。導(dǎo)航算法如圖1所示。

圖1 導(dǎo)航算法框圖

2.1 互補(bǔ)濾波

陀螺儀具有良好的動(dòng)態(tài)響應(yīng)特性，但是隨著時(shí)間的增長累積誤差越來越大；磁力計(jì)和加速度傳感器雖然沒有累積誤差，但動(dòng)態(tài)響應(yīng)能力不足。在頻域上它們互補(bǔ)，因此，可以采用互補(bǔ)濾波器融合傳感器的數(shù)據(jù)，提高測量精度的同時(shí)也提高系統(tǒng)的動(dòng)態(tài)性能[7-8]。

以R表示MC-ROV的真實(shí)姿態(tài)的方向余弦矩陣，R0代表由加速度傳感器觀測到的姿態(tài)方向余弦矩陣，uH表示高頻觀測噪聲，則R0=R+uH。Rc代表由陀螺儀計(jì)算得到的姿態(tài)，uL表示Rc中的低頻累積誤差，則有Rc=R+uL。

GL(s)uH(s)+GH(s)uL(s)≈R(s)

(1)

由上式可知，信號中一部分的低頻和高頻干擾可同時(shí)被互補(bǔ)濾波濾掉。設(shè)Ω為陀螺儀測量值構(gòu)成的反對稱矩陣，則有:

(2)

2.2 四元數(shù)微分方程

設(shè)ω=[ωxωyωz]Τ，表示經(jīng)過歸一化的水下機(jī)器人的三軸角速度。令Q=[q0q1q2q3]Τ，四元數(shù)q0,q1,q2和q3與角加速度有如下關(guān)系[9-10]:

(3)

其矩陣形式為:

(4)

由此可知四元數(shù)更新方程為:

(5)

2.3 漸消記憶指數(shù)加權(quán)的自適應(yīng)卡爾曼濾波

最初的卡爾曼濾波是一種線性最小方差估計(jì)方法，可對目標(biāo)狀態(tài)進(jìn)行有效估計(jì)。而在實(shí)際環(huán)境中，系統(tǒng)的噪聲統(tǒng)計(jì)特性是時(shí)變的、非線性的，大大的降低了濾波的精度，甚至可能引起濾波發(fā)散。

為了解決這種缺陷，應(yīng)加強(qiáng)新近數(shù)據(jù)的比重，減弱陳舊數(shù)據(jù)的比重，這可以通過漸消記憶指數(shù)加權(quán)方法來實(shí)現(xiàn)。傳統(tǒng)的衰減記憶卡爾曼濾波就是基于此來防止濾波發(fā)散的一種濾波方法[11]。

與常規(guī)卡爾曼濾波類似，漸消記憶指數(shù)加權(quán)自適應(yīng)卡爾曼濾波過程如式(6)～(10)所示：

(6)

(7)

(8)

Pk/k=(I-KkHk)Pk/k-1

(9)

Xk/k=(I-KkHk)Φk,k-1+KkZk

(10)

將上式中的(6)和(9)相結(jié)合得到：

在料液濃度、料液體積流量、濃縮倍數(shù)等相同的條件下，研究了超濾時(shí)間對膜通量及壓力的影響，結(jié)果如圖5所示。

Xk/k=(I-KkHk)Φk,k-1Xk-1/k-1+KkZk

(11)

從上式可以看出，λ的引入使得濾波增益Kk增大，即加強(qiáng)了新近測量值的權(quán)重。而對于Xk-1/k-1，其系數(shù)減小，即以前時(shí)刻的狀態(tài)對當(dāng)前時(shí)刻的狀態(tài)影響減小。為了體現(xiàn)新近數(shù)據(jù)對濾波的作用，文章設(shè)計(jì)了一個(gè)自適應(yīng)計(jì)算衰減因子λk對傳統(tǒng)算法進(jìn)行改進(jìn)，即用一個(gè)指數(shù)型系數(shù)替換傳統(tǒng)衰減記憶卡爾曼的常值系數(shù)，用公式中的每項(xiàng)乘以不同的加權(quán)系數(shù)。

,...

(12)

式中的衰減因子λk為指數(shù)型函數(shù)，表示新近測量值權(quán)重逐漸增大，體現(xiàn)了自適應(yīng)性。將λk代入式(6)～(10)得到改進(jìn)后的濾波方程組。其中式(7)變形為:

(13)

3 MC-ROV水池實(shí)驗(yàn)和仿真

MC-ROV水下機(jī)器人長寬高為80cm、50cm和50cm，重約90kg，屬于開架式小型水下機(jī)器人。為驗(yàn)證本系統(tǒng)算法的有效性，在水池分別進(jìn)行了靜止、定航和旋轉(zhuǎn)實(shí)驗(yàn)。

將水下機(jī)器人靜置于水平地面，運(yùn)用互補(bǔ)濾波、四元數(shù)更新算法及自適應(yīng)卡爾曼濾波計(jì)算其姿態(tài)角，結(jié)果如圖2。

圖2 MC-ROV靜置時(shí)的姿態(tài)角

由圖2可知，MC-ROV在靜態(tài)時(shí)，其航向角(yaw)、俯仰角(pitch)、橫滾角(roll)誤差絕對值基本小于0.5度，且沒有發(fā)生漂移，表現(xiàn)出良好的穩(wěn)定性和精度。

對水下機(jī)器人進(jìn)行定航實(shí)驗(yàn)，分別應(yīng)用了無味卡爾曼和改進(jìn)的自適應(yīng)卡爾曼，結(jié)果如圖3所示。從圖中我們可以看出，MC-ROV在18s時(shí)都達(dá)到了規(guī)定的角度，但自適應(yīng)卡爾曼的收斂速度要明顯快于標(biāo)準(zhǔn)爾曼。

圖3 MC-ROV定航

如圖4所示，為水下機(jī)器人在水中的小幅度旋轉(zhuǎn)運(yùn)動(dòng)示意圖，控制其先正轉(zhuǎn)再反轉(zhuǎn)，得到上述姿態(tài)角變化曲線。圖(a)中，俯仰角(pitch)、橫滾角(roll)發(fā)生漂移，這是由機(jī)械安裝誤差和算法固有特性造成的。通過添加自適應(yīng)卡爾曼濾波，使得MC-ROV不再沿著一個(gè)方向持續(xù)漂移，并使俯仰角(pitch)、橫滾角(roll)誤差絕對值基本小于0.2°。

圖4 MC-ROV旋轉(zhuǎn)

從上述實(shí)驗(yàn)可知，利用互補(bǔ)濾波和改進(jìn)的漸消記憶指數(shù)加權(quán)自適應(yīng)卡爾曼濾波能夠提高動(dòng)態(tài)響應(yīng)速度、改善姿態(tài)漂移特性。對比圖4中兩圖可知，與標(biāo)準(zhǔn)卡爾曼相比，改進(jìn)算法后，俯仰角誤差降低5倍，并且抑制了角度漂移；橫滾角的誤差降低近一倍，波動(dòng)更小。當(dāng)然，采用漸消記憶指數(shù)加權(quán)自適應(yīng)卡爾曼濾波后，算法對干擾更加敏感，表現(xiàn)出微小的波動(dòng)，后續(xù)研究可以考慮改善權(quán)值或進(jìn)行硬件濾波等方法處理。

4 結(jié)論

基于MEMS的導(dǎo)航系統(tǒng)應(yīng)用越來越廣，已成為水下導(dǎo)航領(lǐng)域的重要研究方向。MEMS微慣性組合導(dǎo)航，具有體積小、易實(shí)現(xiàn)、干擾低等優(yōu)點(diǎn)，是一種極佳的水下導(dǎo)航方法。本文結(jié)合研制的MC-ROV，在微慣性導(dǎo)航基礎(chǔ)上，采用互補(bǔ)濾波、四元數(shù)法、漸消記憶指數(shù)加權(quán)的自適應(yīng)卡爾曼等算法得到了較高精度的姿態(tài)信息，解決了姿態(tài)漂移等問題，具有良好的穩(wěn)定性和實(shí)時(shí)性。

[1] 封錫盛. 從有纜遙控水下機(jī)器人到自治水下機(jī)器人[J]. 中國工程科學(xué), 2000, 2(12):29-33.

[2]WendelJ,MeisterO,SchlaileC,etal.AnintegratedGPS/MEMS-IMUnavigationsystemforanautonomoushelicopter[J].AerospaceScience&Technology, 2006, 10(6):527-533.

[3] 孫玉山, 代天嬌, 趙志平. 水下機(jī)器人航位推算導(dǎo)航系統(tǒng)及誤差分析[J]. 船舶工程, 2010, 32 (5): 67-72.

[4] 唐國元, 徐正武, 黃道敏,等. 基于姿態(tài)控制力矩陀螺的水下航行體運(yùn)動(dòng)建模方法研究[J]. 中國造船, 2014(2):18-27.

[5] 丁 君. 基于微慣性傳感器的姿態(tài)算法研究[D]. 上海：上海交通大學(xué), 2013.

[6] 楊 磊, 龐 碩, 楊耀民,等. 基于9DOFIMU的AUV慣性導(dǎo)航技術(shù)研究[J]. 計(jì)算機(jī)測量與控制, 2016, 24(3):133-135.

[7] 梁延德, 程 敏, 何福本,等. 基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J]. 傳感器與微系統(tǒng), 2011, 30(11):56-58.

[8] 張 明. 水下結(jié)構(gòu)檢測ROV的研制及導(dǎo)航定位研究[D]. 江蘇科技大學(xué), 2015.

[9] 劉建業(yè)，曾慶化，趙 偉,等. 導(dǎo)航系統(tǒng)理論與應(yīng)用[M]. 西北工業(yè)大學(xué)出版社, 2010,20-23.

[10] 張 明, 劉慧婷, 眭 翔,等. 模態(tài)切換水下機(jī)器人微慣性導(dǎo)航分析[J]. 計(jì)算機(jī)輔助工程, 2015, 24(3):88-94.

[11] 張朝飛, 羅建軍, 龔柏春,等. 一種復(fù)雜多介質(zhì)環(huán)境下的視覺/慣性自適應(yīng)組合導(dǎo)航方法[J]. 中國慣性技術(shù)學(xué)報(bào), 2016, 24(2):190-195.

Research of MC-ROV Navigation System

Wu Changjin1，Liu Huiting2

(1.Yangzijiang Shipbuilding (Holidings) Co.Ltd, Jingjiang 214500, China;
2.School of Electronic and Information, Jiangsu University of Science and Technology, Zhenjiang 212003, China)

For the novelty Mode-Converted ROV(MC-ROV), a set of MEMS-based integrated inertial navigation system has been developed, including 3-axis gyroscope, 3-axis accelerometer, 3-axis magnetic compass and high-precision depth sensor. In terms of the compensation infrequency domain between accelerometer and gyroscope, the system firstly utilizes complementary filter to merge the sensors in order to restrain the drifts of gyroscope and improve the dynamic performence. Then quaternions‘ differential equation is analised and further to be the estimated values of MC-ROV’s position and angles, which apparentely reduce the computing. This paper applies an improved fading memory index-weighted adaptive Kalman filter that stresses the effects of new data while weakening that of old data to avoid divergence and further enhance the navigation precision. The pool experimental results present that using complementary filter and the improved Kalman filter can aquire underwater angles with high precision and reliability, though in harsh ambience such as continuous vibration and electromagnet interference.

underwater vehicle; micro-inertial navigation; quaternion; AKF

2016-11-01；

2016-12-01。

吳長進(jìn)(1990-)，男，江蘇靖江人，大學(xué)，主要從事船舶設(shè)計(jì)與導(dǎo)航方向的研究。

1671-4598(2017)04-0159-03

10.16526/j.cnki.11-4762/tp.2017.04.044

TP242

A