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

?

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

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

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

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

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

0 引言

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

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

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

1 單個AUV三自由度模型

圖1 載體坐標系和地面坐標系

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

則AUV三自由度水平面運動學和動力學方程

載體坐標系下航行器的軸向、側向及偏航速度矢量定義為

載體坐標系到地面坐標系的坐標轉換矩陣

科里奧利-向心矩陣

系統(tǒng)慣性矩陣

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

水動力阻尼矩陣

2 Jacobi向量

隊形中心點位置

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

3 MAUV編隊系統(tǒng)動力學模型

同理, 可將式(2)寫成

式(13)兩邊取微分可得

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

則, 式(20)可寫為

應用式(23)可得MAUV編隊系統(tǒng)動力學模型

其中

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

4 ODIN編隊控制律設計

4.1 多ODIN編隊系統(tǒng)動力學方程

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

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

點運動方程

航行器的轉向動力學方程

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

4.2 編隊控制律設計

4.3 轉向控制律設計

不妨假設航行器不受側向力, 即=0, 且令

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

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

5 仿真試驗

5.1 試驗1

圖4 跟蹤直線時編隊系統(tǒng)的運動軌跡

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

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

5.2 試驗2

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

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

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

6 結束語

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

圖8 跟蹤正弦曲線時編隊系統(tǒng)的運動軌跡

[1] 王銀濤, 鄭美云, 嚴衛(wèi)生. 一種新的AUV路徑跟蹤控制方法[J]. 西北工業(yè)大學學報, 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] 李向舜, 方華京. 基于分數(shù)階PDμ控制器的群體編隊控制[J]. 華中科技大學學報: 自然科學版, 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] 陳楊楊, 田玉平. 多智能體沿多條給定路徑編隊運動的有向協(xié)同控制[J]. 自動化學報, 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)東. 移動機器人編隊自修復的切換拓撲控制[J].控制理論與應用, 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] 楊波, 方華京.具有通信約束的分布式水下航行器群編隊控制[J]. 華中科技大學學報: 自然科學版, 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.

水下信號與控制重點實驗室基金資助(9140C2305041001).

張 靜(1983-), 女, 在讀碩士, 研究方向為水下航行器協(xié)同控制.

(責任編輯: 楊力軍)