基于Jacobi幾何向量的多AUV編隊(duì)控制方法

張 靜, 楊惠珍, 郝莉莉, 康鳳舉

?

基于Jacobi幾何向量的多AUV編隊(duì)控制方法

張 靜1,2, 楊惠珍1,2, 郝莉莉1,2, 康鳳舉1,2

(1. 西北工業(yè)大學(xué) 航海學(xué)院, 陜西 西安, 710072; 2. 水下信息與控制重點(diǎn)實(shí)驗(yàn)室, 陜西 西安, 710072)

研究了多自主水下航行器(MAUV)系統(tǒng)的水平面動(dòng)力學(xué)建模和系統(tǒng)編隊(duì)控制問(wèn)題。從單個(gè)自主水下航行器的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)模型出發(fā), 通過(guò)引入Jacobi幾何向量建立了MAUV系統(tǒng)的水平面動(dòng)力學(xué)模型; 結(jié)合典型水下航行器全方位智能導(dǎo)器(ODIN)的流體動(dòng)力參數(shù), 將系統(tǒng)模型解耦為3個(gè)線性子系統(tǒng)——編隊(duì)隊(duì)形、編隊(duì)運(yùn)動(dòng)(即編隊(duì)中心點(diǎn)的運(yùn)動(dòng))及航行器轉(zhuǎn)向子系統(tǒng), 并采用線性狀態(tài)反饋法設(shè)計(jì)了運(yùn)動(dòng)控制器和轉(zhuǎn)向控制器。仿真結(jié)果表明, 該算法可以控制MAUV系統(tǒng)在保持編隊(duì)隊(duì)形的基礎(chǔ)上跟蹤已知路徑。

多自主水下航行器(MAUV); 編隊(duì)控制; Jacobi幾何向量; 全方位智能導(dǎo)航器(ODIN); 線性反饋控制

0 引言

自主水下航行器(autonomous underwater vehicle, AUV)是一種智能化、自主航行、可根據(jù)任務(wù)使命要求進(jìn)行模塊優(yōu)化組合、實(shí)現(xiàn)多種功能的集成系統(tǒng)[1], 其應(yīng)用領(lǐng)域遍及海洋水文探測(cè)、海洋石油勘探與開采、水下考古和水下搜救等。多自主水下航行器（MAUV）系統(tǒng)通過(guò)各AUV間的協(xié)調(diào)合作可更可靠、快速地完成工作任務(wù)。因此受到國(guó)內(nèi)外學(xué)者的高度重視。編隊(duì)控制作為研究MAUV系統(tǒng)的基礎(chǔ)性問(wèn)題, 近年來(lái)取得了大量的研究成果[2-10]。

目前編隊(duì)控制常見做法是將控制對(duì)象的運(yùn)動(dòng)模型簡(jiǎn)化為2階粒子模型[2-8]。由于控制對(duì)象的實(shí)際模型遠(yuǎn)比2階微分方程復(fù)雜, 因而編隊(duì)控制方法在實(shí)際工程應(yīng)用中仍面臨相當(dāng)大的挑戰(zhàn)。文獻(xiàn)[9]利用AUV三自由度水平面運(yùn)動(dòng)學(xué)方程, 根據(jù)領(lǐng)航者的位置和轉(zhuǎn)向信息設(shè)計(jì)了一個(gè)虛擬leader, 并以其信息為參考, 設(shè)計(jì)了主從式隊(duì)形控制策略。文獻(xiàn)[10]研究了一種魚雷型水下航行器水平面2D運(yùn)動(dòng)模型, 并基于人工勢(shì)函數(shù)設(shè)計(jì)了多航行器的編隊(duì)控制。文獻(xiàn)[11]考慮小型無(wú)人機(jī)六自由度非線性模型, 利用滑模控制算法實(shí)現(xiàn)了多無(wú)人機(jī)的任意編隊(duì)控制。文獻(xiàn)[12]建立(remotely operated vehicle, ROV)水下航行器的六自由度空間動(dòng)力學(xué)方程, 提出具有通信約束的分布式水下航行器群編隊(duì)控制算法。文獻(xiàn)[13]利用水下航行器全方位智能導(dǎo)航器(omni-directional intelligent navigator, ODIN)六自由度動(dòng)力學(xué)模型設(shè)計(jì)比例-微分控制器, 實(shí)現(xiàn)了多航行器的編隊(duì)控制。

模型的復(fù)雜性使設(shè)計(jì)的編隊(duì)控制器在理論證明上有一定困難。本文從單個(gè)AUV的動(dòng)力學(xué)模型出發(fā), 將文獻(xiàn)[14]中用于粒子編隊(duì)系統(tǒng)的Jacobi圖形理論推廣到MAUV編隊(duì)系統(tǒng), 建立了編隊(duì)系統(tǒng)的動(dòng)力學(xué)模型。該模型描述了MAUV編隊(duì)系統(tǒng)的編隊(duì)隊(duì)形和編隊(duì)中心點(diǎn)運(yùn)動(dòng)過(guò)程。將ODIN的流體動(dòng)力學(xué)參數(shù)代入該模型[15], 得到了3個(gè)解耦的線性子系統(tǒng), 即編隊(duì)隊(duì)形子系統(tǒng)、編隊(duì)中心運(yùn)動(dòng)子系統(tǒng)和AUV轉(zhuǎn)[U4] 子系統(tǒng)。采用線性反饋控制方法設(shè)計(jì)了各子系統(tǒng)的控制律。仿真結(jié)果表明，該方法可控制MAUV編隊(duì)系統(tǒng)跟蹤指定路徑并同時(shí)保持預(yù)定的隊(duì)形。

1 單個(gè)AUV三自由度模型

圖1 載體坐標(biāo)系和地面坐標(biāo)系

Fig.1 Body-fixed and earth-fixed reference coordinates

則AUV三自由度水平面運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程

載體坐標(biāo)系下航行器的軸向、側(cè)向及偏航速度矢量定義為

載體坐標(biāo)系到地面坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣

科里奧利-向心矩陣

系統(tǒng)慣性矩陣

航行器側(cè)向力、軸向力及偏航力矩的外部控制輸入

水動(dòng)力阻尼矩陣

2 Jacobi向量

隊(duì)形中心點(diǎn)位置

圖2 用Jacobi向量描述由3個(gè)AUV組成的編隊(duì)系統(tǒng)隊(duì)形

3 MAUV編隊(duì)系統(tǒng)動(dòng)力學(xué)模型

同理, 可將式(2)寫成

式(13)兩邊取微分可得

將式(14)~式(16)代入式(20), 且定義

則, 式(20)可寫為

應(yīng)用式(23)可得MAUV編隊(duì)系統(tǒng)動(dòng)力學(xué)模型

其中

下節(jié)將航行器ODIN的流體動(dòng)力學(xué)參數(shù)代入式(25), 得到多ODIN編隊(duì)系統(tǒng)的動(dòng)力學(xué)模型, 然后進(jìn)行控制律設(shè)計(jì)。

4 ODIN編隊(duì)控制律設(shè)計(jì)

4.1 多ODIN編隊(duì)系統(tǒng)動(dòng)力學(xué)方程

ODIN是美國(guó)夏威夷大學(xué)研制的一種球形水下航行器, 如圖3所示。

則可將系統(tǒng)(25)解耦為編隊(duì)隊(duì)形方程和中心

點(diǎn)運(yùn)動(dòng)方程

航行器的轉(zhuǎn)向動(dòng)力學(xué)方程

顯然, 式(29)，式(30)為線性方程。所以多ODIN編隊(duì)系統(tǒng)是個(gè)可解耦的線性系統(tǒng), 可分別設(shè)計(jì)控制律控制Jacobi向量和隊(duì)形中心點(diǎn)位置向量。

4.2 編隊(duì)控制律設(shè)計(jì)

4.3 轉(zhuǎn)向控制律設(shè)計(jì)

不妨假設(shè)航行器不受側(cè)向力, 即=0, 且令

易驗(yàn)證該系統(tǒng)完全可控可觀測(cè), 所有由線性狀態(tài)反饋得偏航力矩控制律為

該系統(tǒng)完全可控可觀測(cè), 故卡爾曼濾波器方程

5 仿真試驗(yàn)

5.1 試驗(yàn)1

圖4 跟蹤直線時(shí)編隊(duì)系統(tǒng)的運(yùn)動(dòng)軌跡

圖5 航行器的軸向速度及橫向速度

圖6 航行器的偏航角及偏航角速度

5.2 試驗(yàn)2

系統(tǒng)由6個(gè)ODIN組成, Jacobi幾何向量定義為

設(shè)Jacobi幾何向量的期望值為即期望的編隊(duì)隊(duì)形為如圖7所示的六邊形。跟蹤軌跡為正弦曲線。

圖8所示的仿真結(jié)果表明，每個(gè)航行器都很好地完成了軌跡跟蹤。

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

本文從水下航行器的三自由度水平面運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程出發(fā), 基于Jacobi幾何向量建立了MAUV系統(tǒng)的動(dòng)力學(xué)模型。由于航行器的對(duì)稱性, 多ODIN系統(tǒng)的動(dòng)力學(xué)模型解耦為編隊(duì)隊(duì)形、編隊(duì)中心點(diǎn)運(yùn)動(dòng)及轉(zhuǎn)[U5] 運(yùn)動(dòng)3個(gè)獨(dú)立的線性子系統(tǒng), 用線性狀態(tài)反饋設(shè)計(jì)方法實(shí)現(xiàn)了各子系統(tǒng)的控制律設(shè)計(jì)。仿真結(jié)果表明, 應(yīng)用此算法可實(shí)現(xiàn)多ODIN系統(tǒng)的隊(duì)形保持和路徑追蹤。

圖8 跟蹤正弦曲線時(shí)編隊(duì)系統(tǒng)的運(yùn)動(dòng)軌跡

[1] 王銀濤, 鄭美云, 嚴(yán)衛(wèi)生. 一種新的AUV路徑跟蹤控制方法[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 2009, 27(4): 517-520. Wang Yin-tao, Zheng Mei-yun, Yan Wei-sheng. A New Meth- od of Path Following Control of AUV (Autonomous Under- water Vehicle)[J]. Journal of Northwestern Polythechnical University, 2009, 27(4): 717-520.

[2] Lewis M, Tan K H. High Precision Formation Control of Mobile Robots Using Virtual Structures[J]. Autonomous Robots, 1997, 4(4): 387- 403.

[3] Zhang F, Leonard N E. Cooperative Kalman Filters for Coo- perative Exploration[C]//Proceedings of 2008 American Con- trol Conference.Washington, 2008: 2654-2659.

[4] 李向舜, 方華京. 基于分?jǐn)?shù)階PDμ控制器的群體編隊(duì)控制[J]. 華中科技大學(xué)學(xué)報(bào): 自然科學(xué)版, 2009, 37(10): 32-35. Li Xiang-shun, Fang Hua-jing. PDμ-based Controller For- mation Control of Networked Swarm Agents[J]. Journal Hua- zhong University of Science and Technology: Natural Scien- ce Edition, 2009, 37(10): 32-35.

[5] 陳楊楊, 田玉平. 多智能體沿多條給定路徑編隊(duì)運(yùn)動(dòng)的有向協(xié)同控制[J]. 自動(dòng)化學(xué)報(bào), 2009, 35(12): 1541- 1549. Chen Yang-yang, Tian Yu-ping. Directed Coordinated Con- trol for Multi-agent Formation Motion on a Set of Given Curves[J]. Acta Automatica Sinica, 2009, 35(12): 1541-1549.

[6] Cui R, Ge S S. Ren B. Synchronized Altitude Tracking Con- trol of Multiple Unmanned Helicopters[C]//Proceedings of 2010 American Control Conference, 2010: 4433- 4438.

[7] 張飛, 陳衛(wèi)東. 移動(dòng)機(jī)器人編隊(duì)自修復(fù)的切換拓?fù)淇刂芠J].控制理論與應(yīng)用, 2010, 27(3): 289-295. Zhang Fei, Chen Wei-dong. Switched Topology Control for Self-healing of Mobile Robot Formation[J]. Control Theory & Applications, 2010, 27(3): 289-295.

[8] Zhang F. Cooperative Shape Control of Particle Forma- tions[C]//Proceedings of the 46th IEEE Conference on Deci- sion and Control, 2007: 2516-2521.

[9] Cui R, Ge S S, How B E, et al. Leader-follower Formation Control of Underactuated AUVs with Leader Position Mea- surement[C]//Proceedings of IEEE International Conference on Robotics and Automation, 2009: 979-984.

[10] Li J H, Jun B H, Lee P M. Formation Control of Multiple Un- deractuated AUVs[C]//Proceedings of IEEE Oceans. Europe, 2007: 1-5.

[11] Fahimi F. Full Formation Control for Autonomous Helicopter Groups[J]. Robotica, 2008, 26(2): 143-156.

[12] 楊波, 方華京.具有通信約束的分布式水下航行器群編隊(duì)控制[J]. 華中科技大學(xué)學(xué)報(bào): 自然科學(xué)版, 2009, 37(2): 57-60. Yang Bo, Fang Hua-jing. Distributed-control of Maneuvers of Underwater of Underwater Vehicle Groups with Communi- cation Constraint[J]. Journal Huazhong University of Science and Technology: Natural Science Edition, 2009, 37(2): 57-60.

[13] Hou S P, Cheah C C. PD Control Scheme for Formation Con- trol of Multiple Autonomous Underwater Vehicles[C]//Pro- ceedings of IEEE/ASME International Conference on Advan- ced Intelligent Mechatronics. Singapore, 2009: 356-361.

[14] Zhang F, Goldgeler M, Krishnaprasad P S. Control of Small Formations Using Shape Coordinates[C]//Proceedings of the 2003 IEEE International Conference on Robotics and Auto- mation. Taipei, 2003: 14-19.

[15] Choi S K, Tahashige G Y, Yuh J. Experimental Study on an Underwater Robotic Vehicle: ODIN[C]//Proceedings of the 1994 Symposium on Autonomous Underwater Vehicle Tech- nology. Cambridge, 1994: 79-84.

[16] Fossen T I. Guidance and Control of Ocean Vehicles[M]. Chichester: John Wiley and Sons, 1994.

[17] Antonelli G. Underwater Robits-Motion and Force Control of Vehicle-Manipulator Systems[M]. 2th ed. Berlin: Springer- Verlag, 2006.

A Formation Control Algorithm for Multiple Autonomous Underwater Vehicles Based on Jacobi Shape Theory

ZHANG Jing1,2, YANG Hui-zhen1,2, HAO Li-li1,2, KANG Feng-ju1,2

(1.College of Marine Engineering, Northwestern Polythechnical University, Xi′an 710072, China; 2. Science and Technology on Underwater Information and Control Laboratory, Xi′an 710072, China)

Dynamics modeling and formation control of multiple autonomous underwater vehicles (MAUVs) system are addressed. Based on dynamics and kinematics models of a single AUV, a horizontaldynamics model of MAUV system is established by applying the Jacobi geometric vector. With the hydrodynamic parameters of the typical underwater vehicle’s omni-directional intelligent navigator (ODIN), the formation dynamics model is reasonably simplified and decoupled into three linear subsystems——formation shape, formation movement (i.e. movement of the formation center), and Vehicle orientation subsystem. Motion controller and steering controller are designed for the decoupled linear subsystem by using the linear state feedback approach. Simulation results demonstrate that the algorithm can control the formation center of MAUV system to track known path while keeping formation shape.

multiple autonomous underwater vehicle(MAUV); formation control; Jacobi geometric vector; omni-directional intelligent navigator(ODIN); linear feedback control

TJ630.33; U674.76

A

1673-1948(2012)02-0111-06

2011-07-03;

2011-10-31.

水下信號(hào)與控制重點(diǎn)實(shí)驗(yàn)室基金資助(9140C2305041001).

張 靜(1983-), 女, 在讀碩士, 研究方向?yàn)樗潞叫衅鲄f(xié)同控制.

(責(zé)任編輯: 楊力軍)