欠驅(qū)動AUV自適應(yīng)編隊控制策略

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

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

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

1 自適應(yīng)編隊策略

1.1 坐標(biāo)系的建立

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

圖1 欠驅(qū)動AUV編隊的系統(tǒng)結(jié)構(gòu)Fig.1 Control architecture for under-actuated AUVs formation

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

根據(jù)文獻［15］的單個AUV運動控制，本文建立了欠驅(qū)動AUV控制系統(tǒng)結(jié)構(gòu)。如圖1所示，單個AUV的控制體系結(jié)構(gòu)包括感知層、運動控制層和執(zhí)行層。多AUV編隊控制器作為規(guī)劃層，通常是在運動控制層的上層向單個AUV發(fā)送期望速度、期望角度和期望下潛深度指令，再由單個AUV的基礎(chǔ)控制加以完成。

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

1.2 勢函數(shù)的建立

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

根據(jù)AUV在柱坐標(biāo)系的運動學(xué)方程，設(shè)第i個AUV qi的期望區(qū)域是以期望形心為圓心，半徑介于R1、R2之間，高度為h的環(huán)狀區(qū)域，其中，

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

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

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

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

可見，當(dāng)AUV在期望區(qū)域外時，勢函數(shù)對AUV產(chǎn)生引力。

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

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

避碰勢能函數(shù)定義為

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

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

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

1.3 自適應(yīng)編隊控制器

第i個AUV動力學(xué)方程可表示為

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

式中:αi和γ是正常數(shù)。并設(shè)定

定義第i個AUV的滑動面為

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

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

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

式中:Ksi和Kp為控制器的可調(diào)控制參數(shù)，Ksi和Kp為正定陣。在控制中通過調(diào)整Ksi和Kp的值，確定AUV編隊中期望編隊區(qū)域和避碰區(qū)域的權(quán)重。估計參數(shù)被更新為:

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

式中:Δ δi=δi－。

圖2 自適應(yīng)編隊控制器框圖Fig.2 Diagram of adaptive formation controller

由圖2，自適應(yīng)編隊控制器建立在控制層上層，主要通過建立和調(diào)整避碰區(qū)域和期望區(qū)域的權(quán)重規(guī)劃出適合欠驅(qū)動AUV的軌跡。所以為了證明編隊控制器的穩(wěn)定性，定義和期望區(qū)域、避碰區(qū)域及閉環(huán)控制相關(guān)的二次型函數(shù)之和作為多AUV系統(tǒng)的李亞普諾夫函數(shù):

2 仿真結(jié)果

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

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

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

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

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

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

3 結(jié)束語

本文根據(jù)AUV的欠驅(qū)動特性，使用柱坐標(biāo)系建立了AUV的運動學(xué)模型和多AUV的期望編隊區(qū)域。提出并證明了自適應(yīng)編隊控制策略，通過人工勢場能函數(shù)，使領(lǐng)航員AUV完成了預(yù)定軌跡的路徑跟隨，同時實現(xiàn)了跟隨中多AUV的編隊和隊形保持。通過建立3個欠驅(qū)動AUV編隊的仿真平臺，分析了欠驅(qū)動AUV在編隊過程的特點，完成了空間折線和螺旋線的跟蹤和編隊，并使AUV之間的相對距離誤差逐漸趨于零并保持穩(wěn)定，證明了編隊控制策略，對欠驅(qū)動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］仲宇，顧國昌，張汝波.一種新的水下機器人集群路徑規(guī)劃方法［J］.哈爾濱工程大學(xué)學(xué)報，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］.魚雷技術(shù)，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］賈秋玲，李廣文，閆建國.水下編隊航行機器人的前饋加反饋控制器設(shè)計［J］.兵工學(xué)報，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］甘永.水下機器人運動控制系統(tǒng)體系結(jié)構(gòu)的研究［D］.哈爾濱:哈爾濱工程大學(xué).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］.哈爾濱工程大學(xué)學(xué)報，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.