水下機器人抗波浪擾動動力定位研究

劉慧婷，張　明，曾慶軍，眭　翔，宋振文

(1.江蘇科技大學 電子信息學院，江蘇 鎮(zhèn)江　212003; 2.常州信息職業(yè)技術學院機電工程學院，江蘇 常州　213164；3.江蘇科技大學 計算機科學與工程學院，江蘇 鎮(zhèn)江　212003)

?

水下機器人抗波浪擾動動力定位研究

劉慧婷1，張明1，曾慶軍1，眭翔2，宋振文3

(1.江蘇科技大學 電子信息學院，江蘇 鎮(zhèn)江212003; 2.常州信息職業(yè)技術學院機電工程學院，江蘇 常州213164；3.江蘇科技大學 計算機科學與工程學院，江蘇 鎮(zhèn)江212003)

針對水下機器人近水面運動受到波浪干擾而產生的不確定性問題，結合所研制的新型海洋工程模態(tài)切換機器人MC-ROV，文章基于CFD法建立動力學模型，進行6DOF動力定位分析；根據(jù)定位誤差設計力和力矩分配策略，采用非奇異終端滑膜控制(NTSM)對推力予以補償，以減小由波浪等隨機帶來的影響，并采用無味卡爾曼(UKF)實時估計水下機器人的狀態(tài)；仿真結果表明，該方法具有良好的動力定位效果，并且在受擾動后可以迅速調整動力分配策略，減小其帶來的負面影響。

水下機器人；動力定位；擾動；非奇異終端滑動模型；無味卡爾曼

0　引言

水下機器人被用于各種水下作業(yè)，在探索、檢修、監(jiān)控等方面發(fā)揮極為重要的作用。水下機器人動力定位是利用一系列使動裝置(主要為推進器)使其保持在某一特定位置和角度。動力定位系統(tǒng)由傳感器、控制器、濾波算法和推進器等組成，不僅要克服系統(tǒng)本身的高度非線性和耦合性，還要注意外部干擾如波浪、海流等[1]。由于水下機器人的動態(tài)行為在很大程度上決定于推力的大小和方向，因此對推進器實施恰當?shù)目刂撇呗允种匾?/p>

目前，已有眾多的學者研究水下機器人動力定位方法，取得了豐碩成果。一直以來，擴展卡爾曼(extended kalman filter，EKF)和PID常被用于動力定位控制，Balchen與Grimble等人都曾提出相關的改進算法并獲得良好效果[2]。然而其缺陷在于增益調節(jié)較難，無法靈活的適應環(huán)境變化，而且魯棒性難以得到保障。2001年，Tannuri Donha和Pesce應用滑動模型控制(sliding mode control，SMC)理論進行動力定位研究，并證明其可行性和魯棒性[3]。文獻[4]提出一種滑?？刂品椒?，驗證與PID相比，其對與多數(shù)環(huán)境具有良好的特性和穩(wěn)定性。文獻[5]基于一種異步數(shù)據(jù)算法設計了擴展卡爾曼濾波器，使水下機器人定位誤差減小20%。文獻[6]介紹了一套包括硬件、軟件和算法的水下機器人控制系統(tǒng)，對水下機器人的航向和其他自由度運動分開控制，取得不錯的實驗效果。文獻[7]驗證了在有效的動態(tài)補償下，超短基線在水下機器人動力定位中的可行性。文獻[8]針對水下機器人推理的動態(tài)過程，分析并采用一種模糊滑膜控制方法進行動力補償，Lyapuno方法證明其可在有限時間內收斂并能保證魯棒性。其他如模糊控制、神經網絡控制及其改進算法等等都已取得不錯的成果。

考慮水下機器人水動力特性和環(huán)境干擾，本文以自主研制的模態(tài)切換水下機器人(Mode-converted ROV，MC-ROV)為對象，建立較為完整的6DOF運動模型，采用終端滑動模型(terminal sliding mode，TSM)、無味卡爾曼(unscented kalman filter，UKF)方法進行動力分配和位置計算，使MC-ROV動態(tài)地保持在目標位置。

1　動力學模型

1.1MC-ROV簡述

MC-ROV是開架式小型水下機器人，航速3節(jié)，重80 kg，具有水中浮游和爬壁兩種工作模式，可以根據(jù)工作需要進行勘察或清污作業(yè)。其模態(tài)切換功能依靠一個垂向的吸附推進器和壓縮彈簧來實現(xiàn)：吸附推進器為小車輪和清污滾輪提供動力，可以沿壁爬行清污；壓縮彈簧解除爬壁模式，此時可以浮游作業(yè)。

1.2動力學模型

MC-ROV在水中做6DOF運動，即三軸移動和三軸旋轉。為方便計算，水下機器人運動參數(shù)定義如表1所示。

表1　參數(shù)定義

在靜水中，可用下述方程描述水下機器人動力學特性[9]：

(1)

其中：

η=[xyzφθψ]Τ；

v=[uvwpqr]Τ；

M為水下機器人慣性矩陣，M∈R6×6；

C(v)為水下機器人科氏及向心力矩陣，C(v)∈R6×6；

D(v)是水下機器人流體阻力矩陣，D(v)∈R6×6；

g(η)是由重力和浮力組成的回復力矩陣，g(η)∈R6×1；

τ是水下機器人推進器提供的推力，τ∈R6×1。

而在實際運行時，還要考慮周圍環(huán)境干擾，如水流、波浪等帶來的影響。假設ξd為干擾力，vd表示伴隨干擾力產生的速度，則實際動力學模型應調整為[10]

(2)

1.3模型參數(shù)

水下機器人動力學模型計算涉及眾多的水動力系數(shù)，目前主要有3種辨識方法：經驗法、試驗法和CFD法[11]。由于采用CFD可以方便的在計算機上實現(xiàn)各種流場、速度場等環(huán)境下的計算，成本低，速度快，因此這里基于FLUENT軟件進行CFD數(shù)字模擬辨識參數(shù)。水下機器人在水中運動時，主要考慮流體慣性類系數(shù)和粘性類系數(shù)。其中慣性類水動力系數(shù)與加速度線性相關，用附加質量描述，而粘性類水動力系數(shù)與速度的一次項和二次項相關。考慮MC-ROV的低速運行、上下對稱、開架式結構等，同時選擇機體坐標系原點位于重心，浮心與重心基本重合，忽略眾多的耦合項和高于三階的項，可以大大簡化水動力模型[12]。

令MC-ROV重心為xG=yG=zG=0，式(a)中慣性矩陣為

M=

(3)

C(v)由科氏向心力矩陣和附加質量矩陣組成，表示為

(4)

式中,

阻尼力D(v)主要由阻力一次項和二次項組成，即

(5)

其中，各自由度二次阻尼系數(shù)為：

Xu|u|=-189 N·m-2s2，Yv|v|=-292.98 N·m-2s2，Zw|w|=-314.58 N·m-2s2，

Kp|p|=-46 N·m-2s2，Mq|q|=-76 N·m-2s2，Nr|r|=-38 N·m-2s2。

一次阻力系數(shù)為

Xu=-148.18 N·m-2s2，Yv=-785.69 N·m-2s2，Zw=

-414.09 N·m-2s2，

Kp=-79.21 N·m-2s2，Mq=-103.22 N·m-2s2，Nr=-44.41 N·m-2s2。

回復力矩陣g(η)為重力和浮力向量：

(6)

其中：W和B分別為重力和浮力，xB、yB和zB為MC-ROV浮心。經過調平衡，MC-ROV的重力最后為W=888.174N，浮力為B=892.878N，重心和浮心之間的關系可描述為xB-xG=0，yB-yG=0，zB-zG=0.154 6 m。

2　定位算法分析

在對水下機器人實時控制時，要求能夠及時適應突變的運動環(huán)境，即在受到波浪、海流等影響時迅速改變控制量。本文根據(jù)定位誤差有序地進行初次動力分配，然后利用NTSM對外部干擾和不確定性具有魯棒性對推力做動態(tài)補償。同時，運用UKF濾波器估計水下機器人實時位置，以進行下次動力分配。

2.1非奇異終端滑模

定義運算

(7)

則本算法的NTSM設計如下[14]

(8)

式中，β=diag{β1,···β6}，βi>0，1<γi<2，i=1,2,…,6。

MC-ROV由某一狀態(tài)趨近qd時按如下規(guī)定到達

(9)

其中：k1、k2為常數(shù)項，ρi>0，i=1,2,···,6。

對于動力定位系統(tǒng)，控制器終端變量設計為

(10)

2.2無味卡爾曼

UKF，即無味卡爾曼濾波(unscented kalman filter，UKF)是20世紀90年代出現(xiàn)的一種非線性濾波方法，具有估計精度高、收斂速度快等優(yōu)點，而且不必對非線性函數(shù)求導，在動力定位中具有廣泛的應用[15]。UKF的基礎是Unscented變換(unscented transformation，UT),其基本思想是用一組確定的離散采樣點(即Sigma點)來近似狀態(tài)變量的分布。UKF假定狀態(tài)滿足高斯分布，因此只需逼近其均值和方差[16]。

MC-ROV水下機器人的控制模型按式(2)進行，控制過程中加入不確定環(huán)境干擾。將式(2)整理變換為

(11)

考慮Unscented卡爾曼濾波算法基于如下非線性離散狀態(tài)空間模型：

(12)

式中，xk=[ηv]Τ，B=[06×6M-1]Τ，u∈R6×1是推進器推力，hk(xk)=Hxk，fk(xk)=[J(η)v-M-1CRBv-M-1CAvξ-M-1Dvξ-M-1g]Τ。

Unscented卡爾曼濾波與基本卡爾曼濾波一樣，也是由時間更新和測量更新構成的，

按如下過程進行。

時間更新：

測量更新：

2.3動力分配方法

假設水下機器人的三軸直線運動誤差和三軸旋轉誤差為e=[exeyezeφeθeψ]Τ。針對誤差設計推力如下

(13)

式中，n1、n2為調節(jié)因子，F(xiàn)max是推進器最大推力，T是單個推進器推力，-Fmax

在6DOF動力定位時，采用有序定位的方法，即按照先定艏向，再定位置，最后定姿態(tài)的順序依次完成整個定位系統(tǒng)。這樣可以使動力分配更加條理，避免動力分配混亂的情況。水下機器人MC-ROV的推進器共5個，兩個水平推進器和3個垂向推進器。因此，在計算出單個推進器的推力后，需按下式分配推力

(14)

其中，T1、T2、T3、T4、T5分別為5個推進器的推力，a=0.253 m，b=0.341 m。

由NTSM補償?shù)膭恿?/p>

(15)

其中:A6×6為可調系數(shù)矩陣。

最終的動力分配策略為

(16)

3　算法仿真

水下機器人在近水面航行時，主要受海流(current)和波浪(wave)影響。由于航行速度和時間有限，航行水域也較小，可以近似認為海流不變[17]。而波浪的運動是較為復雜的隨機過程，常利用PM譜(pierson-moskowitz spectrum)進行分析、計算波浪運動[18]。

PM譜在實際使用時難以預知，為簡便起見，以縱向為例，采用如下方程簡單描述波浪運動：

(17)

式中，ai表示波浪幅值，ki為波數(shù)，ωi、εi為頻率和初相。

其在水平和垂直方向上的速度為

(18)

(19)

首先考慮無干擾情況，假設MC-ROV在無限深廣的靜水中航行，初始位置定義為為原點，目標位置為(x,y,z)=(20,20,-5)。對于TSM，β=0.4、γ=1.4、k1=3、k2=6、ρ=0.9。對于UKF，狀態(tài)初值x0=012×1，方差初值P0=1 000I12×12，系統(tǒng)噪聲和量測噪聲分別為

圖1～2為靜水中動力定位的位置和姿態(tài)曲線。圖中，x、y、z為縱軸、橫軸和垂向上的位移，roll、pitch、yaw為橫滾角、俯仰角和航向角。由圖可知，水下機器人在靜水中基本能夠快速、平穩(wěn)得到達指定位置，只有俯仰角在水下機器人下潛時產生小幅度波動。

圖1　位置曲線

圖2　姿態(tài)曲線

圖3為水下機器人推力和力矩變化曲線圖。

圖3　力和力矩

在同樣的條件下，加入兩次波浪干擾，一次在定位過程中(25 s

圖4　位置曲線(擾動時)

圖5　姿態(tài)曲線(擾動時)

圖6　力和力矩(擾動時)

4　結論

本文基于NTSM和UKF設計了一種水下機器人6DOF動力定位方法，根據(jù)定位誤差分配推力，運用NTSM方法進行推力補償，同時不斷以UKF實時估計水下機器人的狀態(tài)。從無干擾和有干擾兩種環(huán)境下的仿真結果得知，該方法能夠快速、有效到達指定位置，具有一定的抗干擾能力。該方法對其他環(huán)境干擾如海流等的抵抗能力還需進一步研究。

[1]范士波. 深海作業(yè)型 ROV 水動力試驗及運動控制技術研究[D]. 上海：上海交通大學, 2013:50.

[2] Balchen J G, Jenssen N A, Salid S. Dynamic positioning using Kalman filtering and optimal control theory[A].IFAC/IFIP symposium on automation in offshore oil field operation[C]. 1976, 183.

[3] Donha D C, Tannuri E A. Non-linear semi-submersible positioning system using an H-infinity controller[A].Proceedings of the control applications in marine systems conference (IFAC-CAMS 2001)[C]. Glasgow, Scotland, CD-ROM. 2001.

[4] Tannuri E A, Agostinho A C, Morishita H M, et al. Dynamic positioning systems: An experimental analysis of sliding mode control[J]. Control engineering practice, 2010, 18(10): 1121-1132.

[5] Steinke D M, Buckham B J. A kalman filter for the navigation of remotely operated vehicles[C].OCEANS, 2005. Proceedings of MTS/IEEE. IEEE, 2005: 581-588.

[6] Dukan F, Ludvigsen M, Sorensen A J. Dynamic positioning system for a small size ROV with experimental results[A].OCEANS, 2011 IEEE-Spain[C]. IEEE, 2011: 1-10.

[7] 邢志偉, 于開洋, 王曉輝. 超短基線定位系統(tǒng)在ROV動力定位中應用的可行性研究[J]. 機器人, 2002, 24(6):487-491.

[8] Bessa W M, Dutra M S, Kreuzer E. Dynamic Positioning of Underwater Robotic Vehicles with Thruster Dynamics Compensation[J]. International Journal of Advanced Robotic Systems, 2013, 10:1-8.

[9] Sorensen A J. A survey of dynamic positioning control systems [J]. Annual reviews in control, 2011, 35(1): 123-136.

[10] Fossen T I. Marine control systems: guidance, navigation and control of ships, rigs and underwater vehicles [M]. Marine Cybernetics, 2002:123-138.

[11] 鄧志剛, 朱大奇, 方建安. 水下機器人動力學模型參數(shù)辨識方法綜述[J]. 上海海事大學學報, 2014, 35(2): 74-80.

[12] Chin C S, Lau M W S, Low E, et al. Software for modelling and simulation of a remotely-operated vehicle (ROV)[J]. Professional Paper 5 (2006), 2006, 3: 114-125.

[13] 袁雷, 肖飛, 沈建清, 等. 基于擾動觀測器的不確定非線性系統(tǒng)非奇異終端滑模控制 [J]. 控制與決策, 2014, 29(2): 353-357.

[14] Yu S, Yu X, Shirinzadeh B, et al. Continuous finite-time control for robotic manipulators with terminal sliding mode [J]. Automatica, 2005, 41(11): 1957-1964.

[15] Cadet O. Introduction to Kalman Filter and its use in dynamic positioning systems [A].Proceedings of Dynamic Positioning Conference[C]. 2003: 16-17.

[16] Krishnamurthy P, Khorrami F. A self-aligning underwater navigation system based on fusion of multiple sensors including DVL and IMU [A].Control Conference (ASCC), 2013 9th Asian[C]. IEEE, 2013: 1-6.

[17] 曹永輝. 復雜環(huán)境下自主式水下航行器動力定位技術研究[D]. 西安：西北工業(yè)大學, 2006.

[18] 孫行衍, 付明玉, 施小成,等. 基于UKF聯(lián)邦濾波的動力定位船舶運動狀態(tài)估計[J]. 中國造船, 2013 (1):114-128.

Research on Dynamic Positioning of ROV Anti-waves

Liu Huiting1，Zhang Ming1, Zeng Qingjun1, Sui Xiang2, Song Zhenwen3

(1.School of Electronic and Information，Jiangsu University of Science and Technology, Zhenjiang212003,China;2.School of Mechanical and Electrical Engineering, Changzhou College of Information， Changzhou213164,China;3.School of Computer Science and Engineering, Jiangsu University of Science and Technology, Zhenjiang212003, China)

In terms of the uncertainties induced by wave disturbances when an underwater vehicle is operated by the surface of water and combined with the novelty Model-Converted Remotely Operated Vehicle (MC-ROV) of ocean engineering, a dynamic model based on CFD is built to analyse 6DOF positioning. A strategy of force and moment allocations is designed according to positioning error, then compensating it with an algorithm known as Non-singular Terminal Sliding Model (NTSM) to low the impact brought by waves likewise. Meanwhile Unscented Kalman Filter (UKF) is utilized to estimate the real-time states of the vehicle. Simulation results present that this algorithm makes the underwater vehicle reach a pre-given position rapidly and effectively, and it can adjust the strategy of force and moment promptly when encounters with perturbations so that reduces the negative effects.

remotely operated vehicle; dynamic positioning; disturbance; NTSM; UKF

2015-06-28；

2015-09-06。

國家自然科學基金項目(11204109)；江蘇省產學研前瞻性聯(lián)合研究項目(BY2012181)；江蘇科技大學海洋裝備研究院2015年科研項目A類。

劉慧婷(1990-)，碩士研究生,主要從事導航定位與水下機器人研究工作。

1671-4598(2016)01-0199-05

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

TP273

A

曾慶軍(1969-)，博士，教授,主要從事智能電氣與系統(tǒng)與水下機器人研究工作。