吳長進，劉慧婷

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

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

吳長進1，劉慧婷2

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

表1 導航器件

2 MC-ROV微慣性導航算法分析

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

圖1 導航算法框圖

2.1 互補濾波

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

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

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

(1)

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

(2)

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

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

(3)

其矩陣形式為:

(4)

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

(5)

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

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

為了解決這種缺陷，應(yīng)加強新近數(shù)據(jù)的比重，減弱陳舊數(shù)據(jù)的比重，這可以通過漸消記憶指數(shù)加權(quán)方法來實現(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ù)等相同的條件下，研究了超濾時間對膜通量及壓力的影響，結(jié)果如圖5所示。

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

(11)

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

,...

(12)

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

(13)

3 MC-ROV水池實驗和仿真

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

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

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

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

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

圖3 MC-ROV定航

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

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

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

4 結(jié)論

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

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

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

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

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

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

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

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

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

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

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

[11] 張朝飛, 羅建軍, 龔柏春,等. 一種復雜多介質(zhì)環(huán)境下的視覺/慣性自適應(yīng)組合導航方法[J]. 中國慣性技術(shù)學報, 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。

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

1671-4598(2017)04-0159-03

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

TP242

A