全驅(qū)動(dòng)型AUV 三維路徑跟蹤控制系統(tǒng)設(shè)計(jì)及分析

姚金藝，曾慶軍，趙強(qiáng)，朱志宇，戴文文

江蘇科技大學(xué)電子信息學(xué)院，江蘇鎮(zhèn)江212003

0 引 言

自主式水下機(jī)器人（Autonomous Underwater Vehicle，AUV）廣泛應(yīng)用于軍民用領(lǐng)域，是海洋開(kāi)發(fā)中使用的重要工具之一。其具有機(jī)動(dòng)性好和巡航范圍大等優(yōu)點(diǎn)，在水下觀測(cè)、制圖、定位和深海探測(cè)中扮演著重要角色［1］。而三維路徑跟蹤作為AUV 的重要功能，對(duì)于其能否精準(zhǔn)遂行規(guī)定的使命任務(wù)、順利完成回收和布放等都有著重要意義［2］。

AUV 是一種無(wú)纜自主式水下機(jī)器人，配備有電池及各種傳感器，可水下自主作業(yè)。迄今，研制的AUV 多以欠驅(qū)動(dòng)型為主。例如，美國(guó)的Hydroid公司的Remus 6000［3］配備有測(cè)深儀、側(cè)掃聲吶、水下攝像機(jī)等多種傳感器，可用于海底測(cè)繪等任務(wù)；美國(guó)金槍魚(yú)機(jī)器人公司的Bluefin 21［4］可攜帶多種傳感器及有效載荷，配備有大容量電池，主要用于近海勘探、搜救及反水雷等軍事用途。雖然欠驅(qū)動(dòng)型AUV 制造成本較低、航速快、推進(jìn)效率高，但定位精度和對(duì)危險(xiǎn)環(huán)境的應(yīng)對(duì)能力不強(qiáng)［5］。而相比于欠驅(qū)動(dòng)型AUV，全驅(qū)動(dòng)型AUV 操控性能則較好，運(yùn)動(dòng)控制靈活。其中，三維路徑跟蹤控制作為衡量AUV 控制性能的主要指標(biāo)，是AUV 自主作業(yè)的關(guān)鍵技術(shù)。

三維路徑跟蹤控制主要是將其解耦為水平面（橫向）和垂直面（垂向）控制，并分別設(shè)計(jì)轉(zhuǎn)艏控制器、深度控制器等，以實(shí)現(xiàn)三維路徑跟蹤控制。由于AUV 本身存在不確定性和非線性的特點(diǎn)，加上海流等干擾，給控制器的設(shè)計(jì)帶來(lái)了很大難度。目前，國(guó)內(nèi)研究人員多圍繞上述問(wèn)題針對(duì)AUV 的三維路徑跟蹤開(kāi)展研究。例如，Qi［6］采用神經(jīng)網(wǎng)絡(luò)進(jìn)行非線性重建，引入自適應(yīng)方法使控制器具備了較好的魯棒性和抗干擾性，但是研究未考慮到AUV 是離散系統(tǒng)；王曉偉等［7］采用反步法設(shè)計(jì)了水平面跟蹤控制器，利用微分器對(duì)未知狀態(tài)和不確定項(xiàng)進(jìn)行估計(jì)，但是這種方法的計(jì)算量較大，對(duì)于實(shí)際的AUV 系統(tǒng)無(wú)法實(shí)時(shí)處理；劉昌鑫等［8］采用非線性模型預(yù)測(cè)控制方法設(shè)計(jì)了AUV 的約束路徑跟蹤控制律，以解決有約束路徑跟蹤控制問(wèn)題，但仍存在計(jì)算量大的問(wèn)題。由此可見(jiàn)，簡(jiǎn)化控制器的設(shè)計(jì)，使其具有較好的魯棒性和抗干擾性，是目前AUV 三維路徑跟蹤控制器的設(shè)計(jì)目標(biāo)。

鑒此，本文將以國(guó)內(nèi)自主研發(fā)的“探海I型”全驅(qū)動(dòng)型AUV 為對(duì)象，介紹系統(tǒng)的組成，研究并建立該AUV 的推進(jìn)器、水平面和垂直面的數(shù)學(xué)模型；設(shè)計(jì)三維路徑跟蹤控制器，采用S 面控制三維路徑跟蹤控制器設(shè)計(jì)部分，契合AUV 自身的非線性特性，并引入積分項(xiàng)，以增強(qiáng)控制器的抗干擾性能，旨在較好地解決簡(jiǎn)化控制器設(shè)計(jì)問(wèn)題。

1 “探海I 型”AUV 數(shù)學(xué)模型

本文所述的數(shù)學(xué)模型主要來(lái)源于Remus 模型。為更直觀地描述水下機(jī)器人的水動(dòng)力模型，一般建立兩種坐標(biāo)系：慣性（固定）坐標(biāo)系E-ξηζ和載體（運(yùn)動(dòng)）坐標(biāo)系O（G）-xyz，如圖1 所示。

圖1 AUV 坐標(biāo)系示意圖Fig.1 Definition of AUV coordinate system

AUV 的運(yùn)動(dòng)學(xué)方程將浮心作為載體坐標(biāo)系原點(diǎn)，通過(guò)長(zhǎng)期的理論分析和工程實(shí)踐得到如下方程：式中：m為AUV 本體質(zhì)量；xG，yG，zG為AUV 重心坐標(biāo)；Ix，Iy，Iz為AUV 在3 個(gè)軸方向的轉(zhuǎn)動(dòng)慣量；u，v，w 為AUV 線速度在載體坐標(biāo)系3 個(gè)軸方向的分量；p，q，r 為AUV 角速度在載體坐標(biāo)系3 個(gè)軸方向的分量［9］；X，Y，Z分別為3 個(gè)軸方向的力；K，M，N分別為3 個(gè)軸方向的力矩。

1.1 推進(jìn)器模型

本文“探海Ⅰ型”AUV 屬于全驅(qū)動(dòng)型AUV，采用魚(yú)雷外型，設(shè)計(jì)有5 個(gè)推進(jìn)器，如圖2 所示。其中：2，3 號(hào)電機(jī)為側(cè)向推進(jìn)器；4，5 號(hào)電機(jī)為垂向推進(jìn)器；1 號(hào)電機(jī)為艉部主推進(jìn)器。6 自由度（x 軸向進(jìn)退、y 軸向平移、z 軸向浮潛、x 軸向橫傾、y 軸向縱傾、z軸向轉(zhuǎn)艏）狀態(tài)變量為[ξ，η，ζ，φ，θ，ψ]T，其中ξ，η，ζ為AUV 質(zhì)心在慣性坐標(biāo)系中的位置；φ，θ，ψ為AUV 相對(duì)于慣性坐標(biāo)系的姿態(tài)角，即橫傾角、縱傾角、轉(zhuǎn)艏角。橫傾角φ不予考慮。

圖2 全驅(qū)動(dòng)型AUV 推進(jìn)器布置Fig.2 Propeller configuration of fully-actuated AUV

假設(shè)推進(jìn)器為線性模型，則推進(jìn)器推力XT表示為

式中：D為螺旋槳直徑；n為螺旋槳轉(zhuǎn)速；ρ為流體密度；tr為推力減額系數(shù)（很小，可忽略）；KT為無(wú)因次推力系數(shù)。

假設(shè)已知各推進(jìn)器的推力/力矩向量為T(mén)1，T2，T3，T4，T5，同一個(gè)平面的電機(jī)旋向相反，因未達(dá)到電機(jī)的最大功率，推進(jìn)器反向效率不予考慮，則各推力和力矩由下式表示。

進(jìn)退推力：

平移推力：

浮潛推力：

水平力矩：

垂向力矩：

式中，d1，d2分別為T(mén)1，T2的推力距。將式（3）～式（7）整合，得到

式（8）中，若主推進(jìn)器轉(zhuǎn)速恒定，則可將其視為軸向阻尼二次項(xiàng)系數(shù)Xu||u與速度的二次項(xiàng)關(guān)系，即

式中：Tx為x 軸向推力；uobj與 ||uobj分別為目標(biāo)速度及其絕對(duì)值。

1.2 垂直面控制模型

基于AUV 的6 自由度模型，可得到AUV 垂直面控制模型。

首先，得到簡(jiǎn)化的AUV垂直面運(yùn)動(dòng)學(xué)方程如下。

z軸向浮潛運(yùn)動(dòng)學(xué)方程（一般情況下，yg=0）：

y 軸向縱傾運(yùn)動(dòng)學(xué)方程（恒速，u?=0）：

式中：Fi為高斯白噪聲；Zw||w，Zq||q，Zw?，Zq?，Zuq，Zuw，Mw||w，Mq||q，Mw?，Mq?，Muq，Muw均為水動(dòng)力參數(shù)，其中Zw?，Zq?為多元矩陣函數(shù)G中相應(yīng)分量的一階偏導(dǎo)數(shù)，Zw||w，Zq||q為多元矩陣函數(shù)G中的二階混合偏導(dǎo)數(shù)；Zg，Mg為重力；Zprop，Mprop分別為z 軸的推力和y 軸推力矩；xg，zg為重心坐標(biāo)。

然后，將“探海I型”模型參數(shù)代入垂直面控制模型，得到如下AUV 垂直面運(yùn)動(dòng)學(xué)方程。

z軸向浮潛運(yùn)動(dòng)學(xué)方程：

y 軸向縱傾運(yùn)動(dòng)學(xué)方程：

1.3 水平面模型

若假設(shè)AUV 的航行深度未發(fā)生改變，僅航向、航跡發(fā)生變化，則認(rèn)為AUV 的重心保持在水平面上。水平面內(nèi)，AUV 在慣性坐標(biāo)系的坐標(biāo)變換關(guān)系即可表示為

其中，慣性坐標(biāo)系以水平面一點(diǎn)E為原點(diǎn)，ξ軸指向地理北向，η軸指向地理東向，ζ軸指向地心。

當(dāng)w=0，p=0，q=0 時(shí)，首先簡(jiǎn)化得到以下AUV 的水平面運(yùn)動(dòng)學(xué)方程。

x 軸向進(jìn)退運(yùn)動(dòng)學(xué)方程（一般情況下，yg=0）：

y 軸向平移運(yùn)動(dòng)學(xué)方程（一般情況下，yg=0）：

z軸向轉(zhuǎn)艏運(yùn)動(dòng)學(xué)方程（一般情況下，yg=0）：

式中：Xu||u，Xu?，Xvr，Xrr，Yv||v，Yv?，Yvr，Yuv，Nv||v，Nr||r，Nr?，Nur，Nuv均 為 水 動(dòng) 力 參 數(shù)；Xprop，Yprop，Nprop為各自方向的力和力矩。

然后，將“探海I型”模型參數(shù)代入水平面控制模型，得到如下AUV 水平面運(yùn)動(dòng)學(xué)方程。

x 軸向進(jìn)退運(yùn)動(dòng)學(xué)方程：

y 軸向平移運(yùn)動(dòng)學(xué)方程：

z軸向轉(zhuǎn)艏運(yùn)動(dòng)學(xué)方程：

2 AUV 三維路徑跟蹤控制器設(shè)計(jì)

2.1 引入智能積分的S 面控制器設(shè)計(jì)

在工程實(shí)際中，模糊控制器和PID 控制器應(yīng)用得最為廣泛［10］。但是，AUV 模糊控制器非常依賴于設(shè)計(jì)人員的實(shí)際經(jīng)驗(yàn)，且AUV 是非線性系統(tǒng)，而基于線性模型的PID 控制器控制效果往往不令人滿意。因此，結(jié)合上述兩種控制器的優(yōu)缺點(diǎn)，劉學(xué)敏等［11］提出了改進(jìn)的S 面控制方法，該方法被廣泛應(yīng)用于水下機(jī)器人的控制領(lǐng)域，取得了很好的控制效果。S 面控制器的控制模型為

式中：u為控制輸出，在AUV 里考慮其為對(duì)應(yīng)推進(jìn)器的推力和轉(zhuǎn)矩；Δu為調(diào)整項(xiàng)，可以將其考慮為一段時(shí)間內(nèi)的固定干擾力或是其他調(diào)整因素；e和e?為控制輸入信息，其中e在AUV 里考慮為深度和轉(zhuǎn)艏角誤差信息，e?在AUV 里考慮為深度和轉(zhuǎn)艏角誤差變化率；k1和k2為對(duì)應(yīng)偏差和偏差率的控制系數(shù)，可以將其類(lèi)比為PID 控制器中的PD 系數(shù)。

S 面控制器本質(zhì)上是一種特殊的非線性PID控制器，這里并沒(méi)有引入積分項(xiàng)，所以對(duì)洋流的抗干擾能力較弱。結(jié)合PID 的特點(diǎn)，對(duì)S 面控制器的Δu調(diào)整項(xiàng)設(shè)計(jì)成積分項(xiàng)，控制模型即變?yōu)?/p>

式中，k3為控制系數(shù)。

對(duì)于式（22），當(dāng)e(t)*e?(t)＞0 或e?(t)=0 時(shí)，對(duì)S面控制進(jìn)行積分；當(dāng)e(t)*e?(t)＜0 或e(t)=0 時(shí)，不對(duì)S 面控制進(jìn)行積分。由此，通過(guò)引入積分項(xiàng)可以減少跟蹤時(shí)的穩(wěn)態(tài)誤差［12］。

2.2 穩(wěn)性分析

首先，若只考慮水平面縱向和轉(zhuǎn)艏運(yùn)動(dòng)，則AUV 水平面運(yùn)動(dòng)學(xué)方程為

式中，Nr為多元矩陣函數(shù)G的一階偏導(dǎo)數(shù)。

轉(zhuǎn)艏力矩Nprop可以寫(xiě)為

式中，e?為艏向角誤差變化率的一階導(dǎo)數(shù)。

取Lyapunov 函數(shù)（V）：

求導(dǎo)得到

因C2＞0 ，z 軸 向 速 度u＞0 ，水 動(dòng) 力 參 數(shù)Nr||r＜0，Nr＜0，故可知轉(zhuǎn)艏角控制器是穩(wěn)定的［13］。同理，可證明縱、垂向面的深度控制器也是穩(wěn)定的。

3 全驅(qū)動(dòng)控制系統(tǒng)概述

鑒于欠驅(qū)動(dòng)型AUV 在操縱性方面存在缺陷，自主研發(fā)了全驅(qū)動(dòng)型AUV 樣機(jī)。該型AUV 具有位置跟蹤精度高、控制靈活等特點(diǎn)。圖3 所示為該型AUV 實(shí)體樣機(jī)，具體參數(shù)見(jiàn)表1。

圖3 全驅(qū)動(dòng)AUV 實(shí)體樣機(jī)Fig.3 The full-scale prototype of fully-actuated AUV

表1 全驅(qū)動(dòng)型AUV 技術(shù)參數(shù)Table 1 Specifications of fully-actuated AUV

圖4 所示為該AUV 主要設(shè)備及各段的組成。其中，水面設(shè)備主要由光端機(jī)、無(wú)線數(shù)傳電臺(tái)、光滑環(huán)、光纖絞盤(pán)和岸基控制單元等組成。艙段包括艏段、艏部推進(jìn)段、電子艙段、艉部推進(jìn)段和主推進(jìn)段。AUV 控制設(shè)備主要包括光端機(jī)、網(wǎng)絡(luò)交換機(jī)、自動(dòng)駕駛儀、PC104 攝像機(jī)、圖像采集卡、電機(jī)驅(qū)動(dòng)器、照明燈控制板和電池節(jié)點(diǎn)控制器，以及多普勒計(jì)程儀、光纖慣導(dǎo)、深度傳感器、漏水傳感器和入水傳感器等［14］。

圖4 AUV 系統(tǒng)組成示意圖Fig.4 Schematic diagram of AUV system composition

該型AUV 軟件系統(tǒng)基于自動(dòng)駕駛儀PC104上的Linux 架構(gòu)平臺(tái)，使用美國(guó)麻省理工學(xué)院的水下機(jī)器人開(kāi)源庫(kù)（Mission Orientated Operating Suite，MOOS）進(jìn)行開(kāi)發(fā)，通過(guò)核心控制單元MOOSDB 進(jìn)行信息交互［15］。MOOS 開(kāi)源庫(kù)采用星型拓?fù)浣Y(jié)構(gòu)，具有模塊化、可維護(hù)性強(qiáng)等特點(diǎn)，整個(gè)軟件結(jié)構(gòu)包括中央控制單元MOOSDB、傳感器數(shù)據(jù)采集單元、運(yùn)動(dòng)控制單元、pHelm 行為控制單元和岸基控制單元等。

4 仿真及湖試結(jié)果分析

4.1 仿真分析

本文分別設(shè)計(jì)了PID 控制器和智能積分S 面控制器，并對(duì)AUV 進(jìn)行了三維路徑跟蹤仿真。仿真采用的是“探海I 型”AUV 水動(dòng)力等模型參數(shù)。主推進(jìn)器給定恒定推力，目標(biāo)深度為3 m，三維跟蹤初始位置（單位：m）為（10，150，0），六邊形的6個(gè)定點(diǎn)坐標(biāo)分別為（130，50，3）、（160，90，3）、（210，95，3）、（240，55，3）、（210，15，3）、（160，10，3），仿真結(jié)果如圖5～圖8 所示。仿真中，在0～3 200 s 范圍均采用智能積分S 面控制器，在3 200 s 時(shí)AUV 到達(dá)六邊形跟蹤初始位置（160，90，3），施加x 軸正向0.1 m/s 洋流干擾，并切換控制器進(jìn)行對(duì)比。

圖5 智能積分S 面三維路徑跟蹤圖Fig.5 3D path-following map by intelligent integral S-plane controller

圖6 PID 三維路徑跟蹤圖Fig.6 3D path-following map by PID controller

圖7 智能積分S 面水平面航線圖Fig.7 Horizontal route map by intelligent integral S-plane controller

圖8 PID 水平面航線圖Fig.8 Horizontal route map by PID controller

仿真結(jié)果表明：智能積分S 面控制器和PID 控制器雖然都能完成三維路徑跟蹤任務(wù)，但是在深度控制方面，PID 深度控制誤差大于智能積分S 面控制（圖9）；在水平面跟蹤方面，智能積分S 面控制器收斂速度快于PID 控制器（圖10）；AUV 切換目標(biāo)航線時(shí)會(huì)產(chǎn)生一定的超調(diào)，而出現(xiàn)超調(diào)會(huì)使AUV 的跟蹤路線產(chǎn)生誤差，從而導(dǎo)致這2 種控制器的跟蹤時(shí)間不一致；智能積分S 面控制器在海流干擾下的三維路徑跟蹤性能優(yōu)于PID 控制器。表2 給出了智能積分S 面和PID 跟蹤六邊形時(shí)每條邊跟蹤的平均誤差。

圖9 深度曲線對(duì)比圖Fig.9 Contrast map of depth curves

圖10 位置誤差對(duì)比圖Fig.10 Contrast map of position errors

表2 AUV 六邊形跟蹤平均誤差Table 2 The average errors of AUV hexagon tracking

4.2 湖試結(jié)果分析

圖11 所示為湖試中的AUV，試驗(yàn)湖區(qū)符合試驗(yàn)條件要求。其中，水深為5～40 m，水流流速不超過(guò)2 kn。在湖試中，給定巡航中心點(diǎn)和運(yùn)動(dòng)圖形，水下采用航位推算系統(tǒng)自主巡航。使命規(guī)劃如下：航速0.8 kn，潛深5m，初始位置GPS 經(jīng)緯度（120.317 245°，31.109 611°），起始航跡點(diǎn)GPS 經(jīng)緯度（120.318 199°，31.109 256°），回收航跡點(diǎn)GPS 經(jīng)緯度（120.317 280°，31.109 557°），巡航圖形為六邊形，巡航中心位置GPS經(jīng)緯度（120.317 247°，31.109 610°），自主導(dǎo)航時(shí)間7 800 s。表3 給出了水下六邊形自主巡航試驗(yàn)的部分?jǐn)?shù)據(jù)。

圖11 現(xiàn)場(chǎng)實(shí)驗(yàn)Fig.11 Field experiment

表3 自主巡航試驗(yàn)部分?jǐn)?shù)據(jù)Table 3 The partial experimental data acquired from autonomous cruise

根據(jù)航位推算系統(tǒng)得到經(jīng)緯度，繪制出AUV巡航的軌跡圖，如圖12和圖13所示。圖14和圖15給出了航向角和深度曲線。

由圖12 和圖13 可以看出，AUV 自主巡航的穩(wěn)定性很高，在巡航結(jié)束后浮出水面返回回收點(diǎn)的過(guò)程中，湖面起風(fēng)，水流波動(dòng)較大，導(dǎo)致了AUV部分軌跡雜亂，但總體上使用水下航位推算系統(tǒng)得到的AUV 自主導(dǎo)航航跡辨識(shí)度較高。

圖12 水下六邊形自主巡航經(jīng)緯度軌跡三維圖Fig.12 3D diagram of longitude and latitude trajectory of underwater hexagonal autonomous cruise

圖13 水下六邊形自主巡航經(jīng)緯度軌跡平面圖Fig.13 Plane diagram of longitude and latitude trajectory of underwater hexagonal autonomous cruising

圖14 航向角曲線Fig.14 Curves of azimuth angle

圖15 深度曲線圖Fig.15 Depth curves

由圖14 和圖15 分別可以看出，AUV 在路徑跟蹤時(shí)的航向角有一定的超調(diào)，這是因?yàn)榍袚Q目標(biāo)航線時(shí)引起了較大的轉(zhuǎn)艏角和一定的水流干擾所致；AUV 深度曲線存在一定的波動(dòng)，這是AUV路徑跟蹤時(shí)因?yàn)榉€(wěn)心差而引起晃動(dòng)，但總體上還是能夠較好地跟蹤目標(biāo)深度。

綜上所述，AUV 在執(zhí)行路徑跟蹤使命時(shí)，通過(guò)GPS、光纖慣導(dǎo)、深度計(jì)和多普勒計(jì)程儀等多種傳感器進(jìn)行航位推算，以及由自主決策單元計(jì)算出期望深度、期望航向角等數(shù)據(jù)，能夠很好地解決傳感器自身誤差及安裝誤差造成AUV 自主導(dǎo)航精度達(dá)不到實(shí)際工程應(yīng)用要求的問(wèn)題。

5 結(jié) 語(yǔ)

本文對(duì)國(guó)內(nèi)自主研發(fā)的“探海I 型”全驅(qū)動(dòng)型AUV 的推進(jìn)器、垂直面和水平面建立了數(shù)學(xué)模型，設(shè)計(jì)的智能積分S 面控制器契合了AUV 自身的非線性特性，并引入積分項(xiàng)，增強(qiáng)了控制器的抗干擾性能，較好地解決了簡(jiǎn)化控制器的設(shè)計(jì)問(wèn)題。仿真分析和湖試結(jié)果表明，該AUV 運(yùn)行穩(wěn)定，所設(shè)計(jì)的控制器有效，能夠較好地完成三維路徑跟蹤任務(wù)，滿足了全驅(qū)動(dòng)型AUV 水下應(yīng)用的實(shí)際需求。采用的三維路徑跟蹤控制器設(shè)計(jì)方法可為全驅(qū)動(dòng)型AUV 三維路徑跟蹤研究奠定基礎(chǔ)。