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

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

?

水下機(jī)器人抗波浪擾動(dòng)動(dòng)力定位研究

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

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

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

水下機(jī)器人；動(dòng)力定位；擾動(dòng)；非奇異終端滑動(dòng)模型；無味卡爾曼

0　引言

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

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

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

1　動(dòng)力學(xué)模型

1.1MC-ROV簡述

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

1.2動(dòng)力學(xué)模型

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

表1　參數(shù)定義

在靜水中，可用下述方程描述水下機(jī)器人動(dòng)力學(xué)特性[9]：

(1)

其中：

η=[xyzφθψ]Τ；

v=[uvwpqr]Τ；

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

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

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

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

τ是水下機(jī)器人推進(jìn)器提供的推力，τ∈R6×1。

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

(2)

1.3模型參數(shù)

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

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

M=

(3)

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

(4)

式中,

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

(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。

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

(6)

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

2　定位算法分析

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

2.1非奇異終端滑模

定義運(yùn)算

(7)

則本算法的NTSM設(shè)計(jì)如下[14]

(8)

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

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

(9)

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

對(duì)于動(dòng)力定位系統(tǒng)，控制器終端變量設(shè)計(jì)為

(10)

2.2無味卡爾曼

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

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

(11)

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

(12)

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

Unscented卡爾曼濾波與基本卡爾曼濾波一樣，也是由時(shí)間更新和測(cè)量更新構(gòu)成的，

按如下過程進(jìn)行。

時(shí)間更新：

測(cè)量更新：

2.3動(dòng)力分配方法

假設(shè)水下機(jī)器人的三軸直線運(yùn)動(dòng)誤差和三軸旋轉(zhuǎn)誤差為e=[exeyezeφeθeψ]Τ。針對(duì)誤差設(shè)計(jì)推力如下

(13)

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

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

(14)

其中，T1、T2、T3、T4、T5分別為5個(gè)推進(jìn)器的推力，a=0.253 m，b=0.341 m。

由NTSM補(bǔ)償?shù)膭?dòng)力為

(15)

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

最終的動(dòng)力分配策略為

(16)

3　算法仿真

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

PM譜在實(shí)際使用時(shí)難以預(yù)知，為簡便起見，以縱向?yàn)槔捎萌缦路匠毯唵蚊枋霾ɡ诉\(yùn)動(dòng)：

(17)

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

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

(18)

(19)

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

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

圖1　位置曲線

圖2　姿態(tài)曲線

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

圖3　力和力矩

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

圖4　位置曲線(擾動(dòng)時(shí))

圖5　姿態(tài)曲線(擾動(dòng)時(shí))

圖6　力和力矩(擾動(dòng)時(shí))

4　結(jié)論

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

[1]范士波. 深海作業(yè)型 ROV 水動(dòng)力試驗(yàn)及運(yùn)動(dòng)控制技術(shù)研究[D]. 上海：上海交通大學(xué), 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動(dòng)力定位中應(yīng)用的可行性研究[J]. 機(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] 鄧志剛, 朱大奇, 方建安. 水下機(jī)器人動(dòng)力學(xué)模型參數(shù)辨識(shí)方法綜述[J]. 上海海事大學(xué)學(xué)報(bào), 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] 袁雷, 肖飛, 沈建清, 等. 基于擾動(dòng)觀測(cè)器的不確定非線性系統(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] 曹永輝. 復(fù)雜環(huán)境下自主式水下航行器動(dòng)力定位技術(shù)研究[D]. 西安：西北工業(yè)大學(xué), 2006.

[18] 孫行衍, 付明玉, 施小成,等. 基于UKF聯(lián)邦濾波的動(dòng)力定位船舶運(yùn)動(dòng)狀態(tài)估計(jì)[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。

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

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

1671-4598(2016)01-0199-05

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

TP273

A

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