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

袁健，張文霞，周忠海

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

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

1 AUV三維運動學和動力學模型

1.1 單位的書寫規則

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

圖1 AUVi在2個坐標系下的示意圖Fig.1 Schematic of AUViin two coordinate systems

慣性坐標系到本體坐標系的轉換矩陣為

式中:

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

在AUVi本體坐標系下，定義

式中:為AUVi的浮心相對于重心的偏移。

AUVi在本體坐標系下的動力學方程為

式中:

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

1)重力和重力矩

由于AUVi本體坐標原點定義在重心，不產生重力矩。其在本體坐標系下的表達式為

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

2)浮力和浮力矩

AUVi產生浮力和浮力矩反映到本體坐標系下，表示為

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

3)推力和推力矩

AUVi實現6自由度運動，需要安裝3對具有正反推力的推進器。沿著3個坐標軸的方向進行布局，如圖2所示。其中推進器T1和T2位于E-ξη平面并且相對于ξ軸對稱，其可以實現沿ξ軸的屏東以及繞ζ軸的轉動;推進器T3和T4位于E-ηζ平面并且相對于η軸對稱，其可以實現沿η軸的平動以及繞ξ軸的轉動;推進器T5和T6位于E-ξζ平面并且相對于ζ軸對稱，其可以實現沿ζ軸的平動以及繞η軸的轉動。

設AUVi第k個推進器的螺旋槳轉速為，螺旋槳直徑為，第k個推進器的推力系數為，水密度為ρi，則第k個推進器產生的推力為

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

式中:為AUVi第k個推進器和與之平行的坐標軸之間的垂直距離。

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

4)水流阻力和阻力矩

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

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

七歲時，波爾進了當地頗有名氣的伽莫霍姆小學，在學校里，他聰明好學，各門功課都是優秀。課余時間，爸爸常常帶著尼爾斯·波爾和他弟弟海拉德·波爾一塊去遠足、郊游，邊走邊由爸爸給他們哥倆介紹一些各方面的知識。

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

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

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

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

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

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

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

為實現多個自主水下航行器在有限時間內達到預定的編隊形狀，需要對系統的運動學方程進行有限時間一致性設計，采用二階有限時間一致性協議，可實現多個AUV的狀態在有限時間內達到預定的編隊形狀。

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

令

所以式(11)可以表示為

下面證明所提一致性控制協議作用下閉環系統的穩定性。

證明:

定義矢量李亞普諾夫函數:

沿著閉環系統(1)的軌線求v的時間導數，并考慮到為奇函數，得

由

所以

證畢。

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

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

3 數值仿真

以7個AUV編隊為例。設AUVi到虛擬領航者的距離其表達式為

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

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

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

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

4 結束語

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

［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，FIORELLI 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.