全驅(qū)動式自主水下航行器有限時間編隊控制

袁健，張文霞，周忠海

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

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

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

1.1 單位的書寫規(guī)則

定義2個直角坐標(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個坐標(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的浮心相對于重心的偏移。

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

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

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

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

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

4)水流阻力和阻力矩

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

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

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

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

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

2 有限時間二階一致性的編隊控制律

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

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

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

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

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

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

令

所以式(11)可以表示為

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

證明:

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

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

由

所以

證畢。

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

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

3 數(shù)值仿真

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

式中:r為AUV到虛擬領(lǐng)航者的選定的單位距離，這里r=12 m。AUVi質(zhì)量mi=1 000 kg，轉(zhuǎn)動慣量=100 kg·m2，慣性積100 kg·m2。浮心在本體坐標(biāo)系下的坐標(biāo)［1 1 1］T。重力加速度g=9.8 N/kg。水流相對于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，各個AUV的最大速度和角度具有約束，最大速度約束為，最大角度度約束為30 rad/s。所以在編隊成形過程中，其通信拓?fù)錇闀r變的，即連接矩陣A(t)為時變的，對于每個AUV存儲的連接矩陣，需要實時調(diào)整A(t)的元素取值:在通信范圍之內(nèi)的對應(yīng)A(t)的元素置為1，在通信范圍之外的對應(yīng)A(t)的元素置為0。選取a=1，b=1，χ=0.3。以正方體表示具有速度和姿態(tài)的全驅(qū)動式自主水下航行器。仿真結(jié)果如圖4、5所示。

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

圖4 7個AUVs的編隊控制Fig.4 Formation control of seven AUVs

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

4 結(jié)束語

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