欠驅動AUV自適應編隊控制策略

黃海，張強，張樹迪，萬磊，龐永杰

(哈爾濱工程大學水下機器人技術重點實驗室船舶工程學院，黑龍江哈爾濱150001)

自主式水下機器人(autonomous underwater vehicles，AUVs)在軍事上可廣泛應用于水域偵查、中繼通訊、區域探雷、智能攻擊等。在民用方面，可用于海底生物資源探查，礦產資源采樣，海底地形勘測，沉物打撈，地震地熱活動監測，海洋環境監測等［1］。編隊控制可以更好的利用AUV的傳感器系統實現海洋環境的協作探測和作業［2-3］。近年來，人們針對AUV的編隊控制進行了深入的研究，并提出了許多控制方法，例如V.Djapic等基于機器人智能提出了行為控制的編隊方法［4-5］，西北工業大學基于領航員-跟隨法［6］使用前饋加反饋控制器研究了多 AUV 的主從編隊控制［7］，Z.H.Ismail提出使用人工勢場法［8-9］保持多AUV編隊的幾何形狀等。而由于AUV的欠驅動特性，它的隊形保持通常需要基于模型的的非線性路徑跟蹤控制方法作為基礎［10-11］。E.Yang等針對多個非完整的鏈式AUV的領航員－跟隨者編隊保持，提出了非線性的時變光滑反饋控制律［12］。Saing Paul Hou等提出了考慮AUV的動力學模型，提出自適應比例和微分控制器用于多AUV的編隊控制［13］。在異構AUV的編隊中，通常采用領航員－跟隨者編隊形式來實現編隊控制。在這種方法中，通常是由領航員跟蹤一個預定義的路徑，而跟隨AUV將與領航員保持期望的幾何構型［2，14］。考慮到AUV的欠驅動特性，本文將基于人工勢場原理定義多AUV編隊的期望區域，建立自適應編隊控制策略跟蹤預定軌跡，并保持編隊的幾何構型。

1 自適應編隊策略

1.1 坐標系的建立

在多AUV的編隊中，對每個AUV在其中心點上建立局部坐標系(xi，yi，zi)和絕對坐標系(x0，y0，z0)。對于第i個AUV，如果不考慮橫搖，運動學模型可以用下面的等式表示:

圖1 欠驅動AUV編隊的系統結構Fig.1 Control architecture for under-actuated AUVs formation

但由于欠驅動AUV和其他類型的水下航行器相比有較大的差別，它主要通過一個主推螺旋槳輔以舵翼的運動實現AUV的前進、下潛和轉艏，其基礎控制內容主要包括速度控制、艏向控制和下潛深度控制(由AUV前進的同時縱傾控制實現)，而軌跡控制也是綜合上述3種控制所得到的結果。

根據文獻［15］的單個AUV運動控制，本文建立了欠驅動AUV控制系統結構。如圖1所示，單個AUV的控制體系結構包括感知層、運動控制層和執行層。多AUV編隊控制器作為規劃層，通常是在運動控制層的上層向單個AUV發送期望速度、期望角度和期望下潛深度指令，再由單個AUV的基礎控制加以完成。

所以本文將根據AUV的欠驅動特性建立基于柱坐標系的運動學模型，使自適應編隊控制器所得到的AUV期望區域或期望點更為直觀地表達為單個AUV的期望速度、期望角度和期望下潛深度，以有利于編隊規劃層指令的發送。

1.2 勢函數的建立

勢場法是機器人編隊控制常用的方法。勢場函數的梯度可以幫助多機器人系統調整其控制輸入，使它們沿著梯度方向移動，并最終穩定在勢能函數的局部極小值(期望點)。但對于AUV的編隊控制復雜系統，需要設定特定的期望區域，以利用區域控制策略［16］實現編隊控制。

根據AUV在柱坐標系的運動學方程，設第i個AUV qi的期望區域是以期望形心為圓心，半徑介于R1、R2之間，高度為h的環狀區域，其中，

是期望編隊區域的中心。因此，期望編隊區域可以定義為如下的不等式:

式中:δ qiok=qi－，是第k個期望區域的恒定參考矢量點，k=1、2、3;θh是允許的艏向角誤差;fSk(δ qiok)是具有連續偏導數的標量函數。

在約束條件(2)、(3)的描述下，期望編隊區域的勢能函數定義為

因此對式(4)的勢能函數微分，則第i個AUV與期望區域的勢能函數偏差定義為

可見，當AUV在期望區域外時，勢函數對AUV產生引力。

為了在編隊中保證AUV之間的距離，使AUV之間的距離越近，勢函數所產生的斥力越大，還定義了AUV的分層避碰區域。其中避碰函數為

式中:δ qij=qi－qj;g1ij、g2ij、…、gLij分別為避碰勢函數第1層，第2層和第L層(最內層)，并且這些層次連續可微，r1、r2、rL分別表示第一層，第二層和最內層的半徑。

避碰勢能函數定義為

式中:kLij＞…＞k2ij＞k1ij，并且都是正常數。第i個和第j個AUV之間的避碰勢能為

對式(8)的勢能函數微分，得到第i個AUV在避碰區域內勢能偏差為

因此，如果設定式(2)為AUV編隊的期望中心，則AUV的期望位置，編隊形狀和避障區域都可以通過方程(3)～(6)定義。

1.3 自適應編隊控制器

第i個AUV動力學方程可表示為

式中:Mi(qi)是AUV的6×6質量和慣量陣，矢量是 6×6 的離心加速度和科氏加速度矩陣，Di(qi)是阻尼矩陣，gi(qi)是重力加速度矢量，Ti包含推力器推力和舵翼的力矩。

式中:αi和γ是正常數。并設定

定義第i個AUV的滑動面為

微分方程(11)對于時間的表達式為

將式(11)、(12)代入式(10)，得到:

根據自適應控制原理，式(13)可表示為［16］

式中:Ksi和Kp為控制器的可調控制參數，Ksi和Kp為正定陣。在控制中通過調整Ksi和Kp的值，確定AUV編隊中期望編隊區域和避碰區域的權重。估計參數被更新為:

式中:Li是正定陣。如果把式 (15)代入式 (13)，則得到系統控制的閉環動力學方程:

式中:Δ δi=δi－。

圖2 自適應編隊控制器框圖Fig.2 Diagram of adaptive formation controller

由圖2，自適應編隊控制器建立在控制層上層，主要通過建立和調整避碰區域和期望區域的權重規劃出適合欠驅動AUV的軌跡。所以為了證明編隊控制器的穩定性，定義和期望區域、避碰區域及閉環控制相關的二次型函數之和作為多AUV系統的李亞普諾夫函數:

2 仿真結果

為了驗證前面所提到的自適應編隊控制策略，本文通過串聯3個欠驅動AUV的控制平臺和操縱性仿真平臺，搭建了自適應編隊控制的仿真平臺。其中AUV的控制平臺采用文獻［17］的S面控制方法，經過推力分配分別控制AUV的主推螺旋槳、舵、翼實現速度、艏向和深度控制。AUV的操縱性仿真平臺基于潛水器的六自由度動力學方程，動力學方程中包含水動力試驗獲得的AUV水動力系數，輸入AUV主推螺旋槳的控制電壓、舵翼的控制角度，通過推力電壓曲線將主推螺旋槳換算為螺旋槳推力，輸出為AUV實時位置。編隊的期望路徑由仿真平臺進行規劃，編隊過程中各AUV位置由AUV之間的通信進行傳遞，并通過仿真平臺計算AUV之間的距離，為每個AUV發送期望速度、艏向和深度信息。AUV主要在靜水的仿真環境中完成編隊，由于并未設定障礙物，系統的干擾源主要來自不同AUV之間的相互干擾。

圖3 欠驅動AUV到達期望區域Fig.3 Under-actuated AUV arriving at desired formation region

AUV的欠驅動特性和一般的機器人不同，其轉艏和下潛是在前進中完成的，AUV并不能直接完成原地轉艏和下潛，這些都給多AUV的協同編隊造成了一定的困難。在圖3中3個AUV的初始位置分別為(0，0，0)，(40，40，0)和(60，60，0)，AUV 的初始方向為東向，期望編隊為下潛6 m，轉艏90°后沿北向前進的一段直線。由于AUV是欠驅動的所以其下潛和轉艏均需要AUV同時前進配合完成，AUV在轉艏和下潛的同時還要完成編隊，即AUV之間相對位置的改變和保持。欠驅動AUV下潛和轉艏都是通過舵翼來完成的，沒有辦法實現垂直下潛或側移，需要一定的速度和回轉半徑。所以為了在實驗中充分體現欠驅動AUV編隊同時轉艏和下潛這一耦合運動的特點，只對AUV給出了一個中間目標點和一個最終目標點，分別為柱坐標系下和，AUV之間的相對距離由初始的AUV1與AUV2之間的56 m，AUV1與AUV3之間的84 m變為25 m的等腰三角形，其編隊過程是一個期望目標點和相對距離都存在較大突變的多耦合調整過程，而勢函數場力為距離誤差的平方數量級，雖然可以減小誤差但也造成了AUV2在跟隨時由于斥力過大造成的一定的波動。而AUV2的波動反而會對AUV1和AUV3產生引力，進而產生AUV之間的相互干擾作用，造成AUV3穩定后的波動，但也正是自適應編隊控制器的作用，讓AUV3的波動迅速消除了。所以要經過一段調整才使3個AUV之間的相對距離誤差達到穩定和期望狀態。

圖4 欠驅動編隊跟蹤空間折線Fig.4 Under-actuated AUV formation tracking 3D folding line

圖5 欠驅動編隊跟蹤空間曲線Fig.5 Under-actuated AUV formation tracking 3D curve

在圖4和圖5中，3個AUV從同樣的初始位置分別跟蹤空間折線和螺旋線。在跟蹤的過程中，其初始位置并不是理想的編隊位置。AUV1(領航AUV)在仿真開始時，所受到的其他AUV的引力大于目標路徑的引力，所以它盡可能地保持自己的位置，等待其他AUV和自己靠近，同時AUV2和AUV3受到AUV1的引力，將盡快趨近期望編隊區域，當所受到其他AUV的引力與目標路徑的引力平衡時AUV1從起始點開始跟蹤路徑，并在跟蹤的過程中其他兩個AUV進一步趨近AUV1，經過一段時間的相對速度和艏向調整，通過隊形中的引力和斥力作用，到達理想的編隊位置，并在空間曲線跟蹤中使AUV之間的相對距離誤差逐漸趨于零并保持穩定，從而完成了編隊控制和相對位置保持。

3 結束語

本文根據AUV的欠驅動特性，使用柱坐標系建立了AUV的運動學模型和多AUV的期望編隊區域。提出并證明了自適應編隊控制策略，通過人工勢場能函數，使領航員AUV完成了預定軌跡的路徑跟隨，同時實現了跟隨中多AUV的編隊和隊形保持。通過建立3個欠驅動AUV編隊的仿真平臺，分析了欠驅動AUV在編隊過程的特點，完成了空間折線和螺旋線的跟蹤和編隊，并使AUV之間的相對距離誤差逐漸趨于零并保持穩定，證明了編隊控制策略，對欠驅動AUV的編隊研究具有重要的意義。

［1］CUI Rongxin，GE Shuzhi，VOON H B ，et al.Leader-follower formation control of underactuated autonomous underwater vehicles［J］.Ocean Engineering，2010，37:1491-1502.

［2］YANG Huizhen，ZHANG Fumin.Geometric formation control for autonomous underwater vehicles［C］//2010 IEEE International Conference on Robotics and Automation Anchorage Convention District，Anchorage，USA，2010:4288-4293.

［3］仲宇，顧國昌，張汝波.一種新的水下機器人集群路徑規劃方法［J］.哈爾濱工程大學學報，2003，24(2):166-169.ZHONG Yu，GU Guochang，ZHANG Rubo.New way of path planning for underwater vehicle group［J］.Journal of Harbin Engineering University，2003，24(2):166-169.

［4］DJAPIC V.Unifying behavior based control design and hybrid stability theory for AUV application［D］.Riversider:U-niversity of California，2009:72-105.

［5］JIANG Dapeng，PANG Yongjie，QIN Zaibai.Coordinated control of multiple autonomous underwater vehicle system［C］//Proceedings of 2010 8th World Congress on Intelligent Control and Automation.Jinan，China，2010:4901-4906.

［6］崔海英，石秀華，崔榮鑫.自主水下航行器主從式編隊控制［J］.魚雷技術，2007，15(4):42-44.CUI Haiying，SHI Xiuhua，CUI Rongxin.Leader-follower formation control of autonomous underwater vehicles［J］.Torpedo Technology，2007，15(4):42-44.

［7］賈秋玲，李廣文，閆建國.水下編隊航行機器人的前饋加反饋控制器設計［J］.兵工學報，2007，28(8):1008-1010.JIA Qiuling，LI Guangwen，YAN Jianguo.Feedforword/feedback controller for formation control of multiple autonomous underwater vehicles［J］.Acta Armamentarii，2007，28(8):1008-1010.

［8］ISMAIL Z H，DUNNIGAN M W.A region boundary-based geometric formation control scheme for multiple autonomous underwater vehicles［C］//International Conference on Electrical，Control and Computer Engineering.Pahang，Malaysia，2011:491-496.

［9］GU Dongbing，WANG Zongyao.Leader-follower flocking:algorithms and experiments［J］.IEEE Transactions on Control Systems Technology，2009，17:1211-1219.

［10］WOOLSLEY C A，TECHY L.Cross-track control of a slender，underactuated AUV using potential shaping nonlinear oscillations［J］.Ocean Engineering，2009，36:82-91.

［11］LAPIERRE L，SOETANTO D.Nonlinear path-following control of an AUV［J］.Ocean Engineering，2007，34:1734-1744.

［12］YANG Erfu，GU Dongbing.Nonlinear formation-keeping and mooring control of multiple autonomous underwater vehicles［J］.IEEE/ASME Transactions on Mechatronics，2007，12:164-178.

［13］HOU S P，CHEAH C C.Can a simple control scheme work for a formation control of multiple autonomous underwater vehicles［J］.IEEE Transactions on Control Systems Technology，2011，17:1090-1100.

［14］CUI Rongxin，SAM S G，HOW B V E，et al.Leader-follower formation control of underactuated AUVs with leader position measurement［C］//2009 IEEE International Conference on Robotics and Automation.Kobe，Japan，2009:979-984.

［15］甘永.水下機器人運動控制系統體系結構的研究［D］.哈爾濱:哈爾濱工程大學.2007:10-18.GAN Yong.Research on motion control system architecture of underwater vehicle［D］.Harbin:Harbin Engineering University，2007:10-18.

［16］CHEAH C C，HOU S P，SLOTINE J J E.Region-based shape control for a swarm of robots［J］.Automatica，2009，45:2406-2411.

［17］劉建成，于華南，徐玉如.水下機器人改進的S面控制方法［J］.哈爾濱工程大學學報，2002，23(1):33-36.LIU Jiancheng，YU Huanan，XU Yuru.Improved S plane control algorithm for underwater vehicles［J］.Journal of Harbin Engineering University，2002，23(1):33-36.