袁健，張文霞，周忠海

(1.國(guó)家海洋監(jiān)測(cè)設(shè)備工程技術(shù)研究中心，山東 青島，266001;2.山東省海洋環(huán)境監(jiān)測(cè)技術(shù)重點(diǎn)實(shí)驗(yàn)室，山東 青島，266001;3.山東省科學(xué)院海洋儀器儀表研究所，山東 青島，266001;4.青島理工大學(xué) 琴島學(xué)院，山東 青島，266100)

多個(gè)自主水下航行器的協(xié)同控制對(duì)于海洋科學(xué)考察、海洋開(kāi)發(fā)以及軍事應(yīng)用等方面都具有重要的理論和現(xiàn)實(shí)意義。多自主水下航行器的編隊(duì)控制是多機(jī)器人協(xié)調(diào)控制中的一個(gè)典型科學(xué)問(wèn)題。與空間以及陸地多機(jī)器人編隊(duì)控制技術(shù)相比，水下多AUV編隊(duì)控制非常困難。除了AUV自身運(yùn)動(dòng)控制技術(shù)比較難實(shí)現(xiàn)外，在信號(hào)傳輸方面水聲通信方式隨著通信距離的增加通信質(zhì)量顯著下降，主要表現(xiàn)在信號(hào)的延遲、衰減和失真［1］。當(dāng)前關(guān)于自主水下航行器的編隊(duì)控制的研究，主要是借鑒了陸地多機(jī)器人編隊(duì)控制策略，多采用分布控制器的設(shè)計(jì)思路，而忽略多自主水下航行器間的耦合關(guān)系，涉及的主要編隊(duì)控制方法有虛擬結(jié)構(gòu)法［1-6］、遞階控制法［7-8］、跟隨領(lǐng)航法［9］、基于行為的智能控制方法［10］等;或者將多AUVs建模為多智能體，而忽略其姿態(tài)特性(橫滾角、俯仰角和偏航角)。本文考慮到三維空間AUV的位置和姿態(tài)協(xié)調(diào)控制問(wèn)題，提出一種有限時(shí)間一致性算法實(shí)現(xiàn)多個(gè)全驅(qū)動(dòng)式自主水下航行器的編隊(duì)控制。利用各AUV的動(dòng)力學(xué)方程和一致性算法得到的速度和位置來(lái)求得各AUV的推力和推力矩，并通過(guò)仿真驗(yàn)證了控制策略的有效性。

1 AUV三維運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型

1.1 單位的書(shū)寫(xiě)規(guī)則

定義2個(gè)直角坐標(biāo)系，分別是慣性坐標(biāo)系O-xyz和AUV本體坐標(biāo)系E-ξηζ。慣性坐標(biāo)的原點(diǎn)O定義為海平面內(nèi)的某點(diǎn)，O-x軸和O-y軸位于水平面內(nèi)，O-x軸與經(jīng)線方向平行且指向北，O-y軸與緯線方向平行且指向東，O-z軸垂直于水平面且指向地心。E-ξ、E-η和E-ζ是AUV的慣性主軸，本體坐標(biāo)系的原點(diǎn)E定義在AUV的重心，E-ξ定義在縱向中剖面內(nèi)且指向AUV線速度方向，E-η與縱向中剖面垂直且指向右手邊，E-ζ在縱向中剖面內(nèi)且指向下。AUVi在慣性坐標(biāo)系和本體坐標(biāo)系下的示意圖，如圖1所示。

圖1 AUVi在2個(gè)坐標(biāo)系下的示意圖Fig.1 Schematic of AUViin two coordinate systems

慣性坐標(biāo)系到本體坐標(biāo)系的轉(zhuǎn)換矩陣為

式中:

式中:αi為AUVi的橫滾角，βi為AUVi的俯仰角，γi為AUVi的偏航角。

在AUVi本體坐標(biāo)系下，定義

式中:為AUVi的浮心相對(duì)于重心的偏移。

AUVi在本體坐標(biāo)系下的動(dòng)力學(xué)方程為

式中:

式中:ΣFi和 ΣMi、和、和、和、和分別表示AUVi受到的合力和合力矩、重力和重力矩、浮力和浮力矩、推力和推力矩以及水流阻力和阻力矩。

1)重力和重力矩

由于AUVi本體坐標(biāo)原點(diǎn)定義在重心，不產(chǎn)生重力矩。其在本體坐標(biāo)系下的表達(dá)式為

式中:Gi為AUVi所受的重力。

2)浮力和浮力矩

AUVi產(chǎn)生浮力和浮力矩反映到本體坐標(biāo)系下，表示為

式中:Bi為AUVi受到的浮力。

3)推力和推力矩

AUVi實(shí)現(xiàn)6自由度運(yùn)動(dòng)，需要安裝3對(duì)具有正反推力的推進(jìn)器。沿著3個(gè)坐標(biāo)軸的方向進(jìn)行布局，如圖2所示。其中推進(jìn)器T1和T2位于E-ξη平面并且相對(duì)于ξ軸對(duì)稱，其可以實(shí)現(xiàn)沿ξ軸的屏東以及繞ζ軸的轉(zhuǎn)動(dòng);推進(jìn)器T3和T4位于E-ηζ平面并且相對(duì)于η軸對(duì)稱，其可以實(shí)現(xiàn)沿η軸的平動(dòng)以及繞ξ軸的轉(zhuǎn)動(dòng);推進(jìn)器T5和T6位于E-ξζ平面并且相對(duì)于ζ軸對(duì)稱，其可以實(shí)現(xiàn)沿ζ軸的平動(dòng)以及繞η軸的轉(zhuǎn)動(dòng)。

設(shè)AUVi第k個(gè)推進(jìn)器的螺旋槳轉(zhuǎn)速為，螺旋槳直徑為，第k個(gè)推進(jìn)器的推力系數(shù)為，水密度為ρi，則第k個(gè)推進(jìn)器產(chǎn)生的推力為

設(shè)沿著E-ξ、E-η和E-ζ軸方向上的合力分別為、和，作用于E-ξ、E-η 和E-ζ軸上的合力矩分別為、和，分別表示為

式中:為AUVi第k個(gè)推進(jìn)器和與之平行的坐標(biāo)軸之間的垂直距離。

圖2 AUVi推力和推力矩示意圖Fig.2 Thrusts and thrust torques

4)水流阻力和阻力矩

AUVi受到水動(dòng)力作用，由此產(chǎn)生的力和力矩要反映到局部坐標(biāo)系中。設(shè)水流在慣性坐標(biāo)系下的速度為，則水流速度在 AUVi本體坐標(biāo)系下的速度可以表示為:，則水流相對(duì)于AUVi的速度可以表示為

水流阻力與AUVi相對(duì)水流速度的平方成正比。沿E-ξ、E-η和E-ζ軸方向的水流阻力分別為、和，可以表示為

七歲時(shí)，波爾進(jìn)了當(dāng)?shù)仡H有名氣的伽莫霍姆小學(xué)，在學(xué)校里，他聰明好學(xué)，各門(mén)功課都是優(yōu)秀。課余時(shí)間，爸爸常常帶著尼爾斯·波爾和他弟弟海拉德·波爾一塊去遠(yuǎn)足、郊游，邊走邊由爸爸給他們哥倆介紹一些各方面的知識(shí)。

式中:ρi為水密度，Cdi為軸向阻力系數(shù)，Sxi、Syi、Szi為AUVi分別垂直于E-ξ、E-η 和E-ζ軸的橫截面積。

水流產(chǎn)生的阻力矩與AUVi角速度平方成正比。阻力矩系數(shù)Kxi、Kyi、Kzi由試驗(yàn)測(cè)得，可以求出流體阻力矩在E-ξ、E-η和E-ζ軸方向的分量分別為

2 有限時(shí)間二階一致性的編隊(duì)控制律

提出一種基于虛擬領(lǐng)航者位置信息的有限時(shí)間一致性算法實(shí)現(xiàn)多自主水下航行器的編隊(duì)控制。該編隊(duì)控制方法要求AUV利用自身的位置和其到虛擬領(lǐng)航者的距離hi之差進(jìn)行一致性協(xié)同，就是使每個(gè)AUV所擁有的虛擬領(lǐng)航者的位置矢量信息(這里(1:3)－h(huán)i，其中xi(1:3)表示AUVi的位置信息分量)在有限時(shí)間內(nèi)達(dá)到一致:→，i≠j，如圖 3 所示。

圖3 基于虛擬領(lǐng)航者位置信息的7個(gè)AUV編隊(duì)控制矢量關(guān)系示意圖Fig.3 Vector relationship between seven AUVs for formation control based on positions of the virtual leader

該方法要求所有AUV在慣性坐標(biāo)系下的線速度、角速度和角位移矢量在有限時(shí)間內(nèi)達(dá)到一致，并使每個(gè)AUV所有用的虛擬領(lǐng)航者的位置信息在有限時(shí)間內(nèi)達(dá)到一致，這就要求存在一個(gè)時(shí)刻ts，使得t≥ts時(shí)

式中:Nj為與AUVi有通信聯(lián)系的其他AUV集合，表示矢量的范數(shù)，hi為AUVi到虛擬領(lǐng)航者的距離，表示AUVi的虛擬領(lǐng)航者的平移位置矢量，xi(4:6)表示AUVi的角位移信息分量，vi表示AUVi的速度信息矢量。

為實(shí)現(xiàn)多個(gè)自主水下航行器在有限時(shí)間內(nèi)達(dá)到預(yù)定的編隊(duì)形狀，需要對(duì)系統(tǒng)的運(yùn)動(dòng)學(xué)方程進(jìn)行有限時(shí)間一致性設(shè)計(jì)，采用二階有限時(shí)間一致性協(xié)議，可實(shí)現(xiàn)多個(gè)AUV的狀態(tài)在有限時(shí)間內(nèi)達(dá)到預(yù)定的編隊(duì)形狀。

式中:0＜a，b≤1為與收斂性速度相關(guān)的系數(shù)，表示矢量每個(gè)元素的絕對(duì)值，sgn(·)為矢量符號(hào)函數(shù)，其表達(dá)式為

令

所以式(11)可以表示為

下面證明所提一致性控制協(xié)議作用下閉環(huán)系統(tǒng)的穩(wěn)定性。

證明:

定義矢量李亞普諾夫函數(shù):

沿著閉環(huán)系統(tǒng)(1)的軌線求v的時(shí)間導(dǎo)數(shù)，并考慮到為奇函數(shù)，得

由

所以

證畢。

利用控制律(4)帶入式(1)求出和，再帶入式(2)，可以求出合力和合力矩ΣFi和ΣMi，由于和、和、和均為已知或可測(cè)定，所以利用式(3)求出編隊(duì)情形下各個(gè)AUV的推進(jìn)器的驅(qū)動(dòng)力和，其表達(dá)式為

再利用式(3)的6個(gè)方程，從而可以求出沿著E-ξ、E-η和E-ζ軸方向上的合力、和和合力矩、和。

3 數(shù)值仿真

以7個(gè)AUV編隊(duì)為例。設(shè)AUVi到虛擬領(lǐng)航者的距離其表達(dá)式為

式中:r為AUV到虛擬領(lǐng)航者的選定的單位距離，這里r=12 m。AUVi質(zhì)量mi=1 000 kg，轉(zhuǎn)動(dòng)慣量=100 kg·m2，慣性積100 kg·m2。浮心在本體坐標(biāo)系下的坐標(biāo)［1 1 1］T。重力加速度g=9.8 N/kg。水流相對(duì)于AUVi的速度［2 3 4］T，水密度 ρi=1 000 kg/m3，軸向阻力系數(shù)Cdi=1，截面積Sxi=2 m2，Syi=10 m2，Szi=10 m2;阻力矩系數(shù)Kxi=1、Kyi=1、Kzi=1。由于AUV通信范圍的有限性，僅與在其通信范圍內(nèi)(記為SetDis)的其他AUV進(jìn)行通信以進(jìn)行信息交換，這里 SetDis=30 m，各個(gè)AUV的最大速度和角度具有約束，最大速度約束為，最大角度度約束為30 rad/s。所以在編隊(duì)成形過(guò)程中，其通信拓?fù)錇闀r(shí)變的，即連接矩陣A(t)為時(shí)變的，對(duì)于每個(gè)AUV存儲(chǔ)的連接矩陣，需要實(shí)時(shí)調(diào)整A(t)的元素取值:在通信范圍之內(nèi)的對(duì)應(yīng)A(t)的元素置為1，在通信范圍之外的對(duì)應(yīng)A(t)的元素置為0。選取a=1，b=1，χ=0.3。以正方體表示具有速度和姿態(tài)的全驅(qū)動(dòng)式自主水下航行器。仿真結(jié)果如圖4、5所示。

從圖4中可以看出盡管在初始時(shí)刻各個(gè)AUV的初始位置、姿態(tài)和速度狀態(tài)不同，但采用提出的有限時(shí)間一致性控制后，多AUV系統(tǒng)在有限時(shí)間內(nèi)實(shí)現(xiàn)了所要求的編隊(duì)形狀，并且從圖4中可以看出所有AUV的姿態(tài)和速度也在有限時(shí)間內(nèi)達(dá)到了一致，從而實(shí)現(xiàn)了在有限時(shí)間內(nèi)的編隊(duì)形狀。由于通信距離的約束和最大速率約束，導(dǎo)致在編隊(duì)控制過(guò)程中的編隊(duì)軌跡發(fā)生了改變，這也是多個(gè)AUV之間相互通信協(xié)調(diào)控制的結(jié)果。圖5為編隊(duì)控制過(guò)程中AUV1的驅(qū)動(dòng)控制力和控制力矩。

圖4 7個(gè)AUVs的編隊(duì)控制Fig.4 Formation control of seven AUVs

圖5 AUV1推力和推力矩Fig.5 Thrusts and thrust torques of AUV1

4 結(jié)束語(yǔ)

本文研究了全驅(qū)動(dòng)式水下航行器的有限時(shí)間編隊(duì)控制問(wèn)題。仿真結(jié)果表明，盡管在初始時(shí)刻各個(gè)AUV的初始狀態(tài)不同，但采用有限時(shí)間一致性控制后，所有AUV的位置和姿態(tài)都在有限時(shí)間內(nèi)達(dá)到了一致，驗(yàn)證了提出的有限時(shí)間一致性控制策略的有效性。通常情況，由于AUV驅(qū)動(dòng)控制力受到驅(qū)動(dòng)電機(jī)的約束，速度不可能任意調(diào)節(jié)，而是具有一定約束的控制輸入，必定會(huì)加大系統(tǒng)同步所需的時(shí)間或者由于控制輸入約束而導(dǎo)致甚至無(wú)法實(shí)現(xiàn)期望的編隊(duì)隊(duì)形和姿態(tài)，研究充分考慮驅(qū)動(dòng)電機(jī)驅(qū)動(dòng)能力特性的具有控制約束情形下的自主水下航行器的編隊(duì)控制問(wèn)題是下一步研究的重點(diǎn)。

［1］LEWIS M A，TAN K H.High precision formation control of mobile robots using virtual structure approach［J］.Autonomous Robots，1997，4:387-403.

［2］LEONARD N E，F(xiàn)IORELLI E.Virtual leader，artificial potentials and coordinated control of groups［C］//Proceedings of IEEE Conference on Decision and Control.Orlando，USA，2001:2968-2973.

［3］LAWTON A R，YOUNG B J，BEARD R W.A decentralized approach to elementary formation maneuvers［C］//IEEE International Conference on Robotics and Automation.San Francisco，USA，2000:2728-2733.

［4］LAWTON J R，BEARD R W，YOUNG B J.A decentralized approach to formation maneuvers［J］.IEEE Transactions on Robotics and Automation，2003，19(6):933-941.

［5］REN W，SORENSEN N.Distributed coordination architecture for multi-robot formation control［J］.Robotics and Autonomous Systems，2008，56(4):324-333.

［6］REN W，BEARD R W.Decentralized scheme for spacecraft formation flying via the virtual structure approach［J］.Journal of Guidance，Control and Dynamics，2004，27(1):73-82.

［7］XIANG X B，GUAN Z H.Coordinated control for multi-AUV systems based on hybrid automata［C］//Proceedings of the 2007 IEEE International Conference on Robotics and Biomimetics.Sanya，China，2007:2121-2126.

［8］TANGIRALA S，KUMAR R，BHATTACHARYYA S，et al.Hybrid-model based hierarchical mission control architecture for autonomous underwater vehicles［C］//American Control Conference.Portland，USA，2005:668-673.

［9］YANG E F，GU D B.Nonlinear formation-keeping and mooring control of multiple autonomous underwater vehicles［J］.IEEE/ASME Transactions on Mechatronics，2007，2(2):164-178.

［10］KALANTAR S，ZIMMER U R.Distributed shape control of homogeneous swarms of autonomous underwater vehicles［J］.Autonomous Robots，2007，22(1):37-53.