譚東旭，徐紅麗，唐磊生，吳函，付麗君

（沈陽理工大學自動化與電氣工程學院，沈陽110159）

0 引言

自主水下機器人（Autonomous Underwater Vehicle，AUV）作為水下作業(yè)的重要工具，可以用于海洋勘測、水下搜救、管道檢查等作業(yè)，甚至可以應(yīng)用于軍事上，用于巡邏、監(jiān)測、淺灘排雷等作業(yè)。但是面對復雜的任務(wù)需求，單個機器人無法滿足其任務(wù)需求，所以越來越多的研究人員開始研究多水下機器人系統(tǒng)（Multiple Autonomous Underwater Vehicle，MAUV）。而編隊控制是MAUV協(xié)調(diào)控制的基本問題。目前MAUV隊形控制的方法主要有：領(lǐng)航跟隨法、基于行為法、虛擬結(jié)構(gòu)法、人工勢場法等[1-3]。

領(lǐng)航跟隨法又稱主從式編隊，只通過領(lǐng)航者的信息和編隊隊形來實時調(diào)整跟隨者的狀態(tài)，從而形成不同的網(wǎng)絡(luò)拓撲結(jié)構(gòu)，這種方法結(jié)構(gòu)簡單，穩(wěn)定性好，被廣泛應(yīng)用[4-5]。基于行為法可以對于環(huán)境的適應(yīng)性較強，但是隊形的穩(wěn)定性較差[6-7]。虛擬結(jié)構(gòu)法將機器人看成是剛體上的點，根據(jù)環(huán)境變化改變虛擬結(jié)構(gòu)的位姿。MAUV的通信主要依靠水聲通信，但是水聲通信的穩(wěn)定性差、效率低，尤其是在MAUV通信上，隨著AUV數(shù)量的增加，通信效率會顯著降低。而且MAUV在一些軍事方面的應(yīng)用，要求在拒止環(huán)境進行作業(yè)[8]?？紤]到這些情況下，對于MAUV的水下編隊而言，通信是一個不可避免地問題。所以本文研究通過雙目攝像頭或者聲納、姿態(tài)傳感器等傳感器來獲取領(lǐng)航AUV的相對位姿，從而完成MAUV編隊的方法[9-10]。采用領(lǐng)航跟隨法建立編隊運動學模型，再利用Lyapunov函數(shù)設(shè)計反步控制器[11]，最后為避免出現(xiàn)大的速度、角度跳變，引入生物啟發(fā)模型進行處理[12-14]。

1 數(shù)學模型的建立

1.1 三維空間的AUV運動學模型建立

在AUV的六自由度運動學模型的基礎(chǔ)上，建立欠驅(qū)動AUV的四自由度運動學模型。本文只考慮AUV的位置量（x,y,z）、三個方向的速度（u,v,w）、艏向角φ以及艏向角速度r[15]?？梢缘玫紸UV的運動學模型：

1.2 領(lǐng)航--跟隨法編隊模型的建立

為了進一步描述，定義在參考坐標下t時刻的AUVi（i=0，1，2...）的狀態(tài)向量為：

則P0(t)表示領(lǐng)航AUV0的姿態(tài)。P1(t)表示跟隨AUV1的姿態(tài)。其中航向角可由位姿得到：

采用領(lǐng)航跟隨法中的l-φ隊形控制方法，通過控制AUV0和跟隨者AUV1的距離和角度實現(xiàn)隊形控制[16]。則AUV0和AUV1的編隊隊形為(l01,φ01)，期望隊形為

則AUV1可根據(jù)探測到的AUV0的艏向和相對位置，確定自己在AUV0坐標系下的位置(lx,ly)如下：

利用坐標的同胚變換可將位姿誤差轉(zhuǎn)換到跟隨AUV1的載體坐標系下的位姿誤差：

式中Reφ=S-1|φ=eφ,θ=0,ψ=0，S是三維載體坐標系與參考坐標系的旋轉(zhuǎn)矩陣，矩陣Reφ[17]的形式如下：

因此，跟隨AUV1實現(xiàn)期望編隊，則只需ex、ey、ez、eφ則收斂到原點，將式（4）-式（7）代入到EP(t)并對其求導，可以得到基于領(lǐng)航-跟隨的編隊誤差模型：

2 MAUV隊形控制設(shè)計

為了形成期望隊形，跟隨AUV1要保持和領(lǐng)航AUV0之間的距離l和角度φ。通過得到的編隊誤差模型，根據(jù)Lyapunov函數(shù)設(shè)計跟隨AUV1運動學控制率，來實時調(diào)節(jié)跟隨AUV1的速度、角速度（u，w，r）T，從而盡可能保持編隊誤差趨近于0。

2.1 反步控制器設(shè)計

為了消除位置誤差，首先設(shè)計Lyapunov函數(shù)：

顯然，當且僅當ex=ey=ez=0時，V1=0；否則V1＞0恒成立，對式（9）求導可得：

為了使V1收斂，則必須為負值，則設(shè)計期望的運動學控制率為：

而對于欠驅(qū)動的AUV一般沒有橫向推進器，所以設(shè)計虛擬速度控制變量，產(chǎn)生控制信號r1，則虛擬控制變量期望值設(shè)計為：

接下來，對虛擬速度進行鎮(zhèn)定，結(jié)合V1，構(gòu)造Lyapunov函數(shù)：

為了使則V˙2為負值，同時也為了避免設(shè)計的控制率出現(xiàn)奇異值，即分母中出現(xiàn)cos(φe+φ0)，則可以得到控制率r1：

將式（15）、（19）帶入到式（18）中，又k1、k2、k3、k4均為大于零的常數(shù)，所以可以得到

所以整個系統(tǒng)可以達到全局漸進穩(wěn)定的，接下來將生物啟發(fā)模型引入到跟隨AUV1的反步控制器u1、w1、r1，做平滑處理。

2.2 生物啟發(fā)模型

生物啟發(fā)模型最早是由英國科學家霍奇金為了研究槍烏賊的神經(jīng)元的動作電位提出的。該類神經(jīng)元電位的數(shù)學模型也稱H-H模型。該模型中細胞膜的膜電壓Vm的動態(tài)特性方程如下：

式中：Vm是細胞膜的膜電壓；Cm為細胞膜的膜電容；EP、ENa、EK分別是細胞膜的負電流、鈉離子、鉀離子在細胞膜中對應(yīng)的能量；gP、gNa、gK分別是負電流、鈉離子、鉀離子所對應(yīng)的電導。

后來經(jīng)過Grossberg進一步改進，簡化了H-H模型的系數(shù)，令Cm=1，?=Ep+Vm，A=gP，B=ENa+Ep，D=Ek-Ep，f(ei)=max(ei,0)，g(ei)=max(-ei,0)可以獲得生物啟發(fā)模型：

式中，f(ei)是興奮性輸入，g(ei)是抑制性輸入。神經(jīng)元?i的膜電壓是系統(tǒng)的輸出，對于任何激勵與抑制信號都能控制?i在[-D,B]范圍內(nèi)變化，使輸出信號?i平滑，可以用于解決編隊過程中速度跳變。

本文利用生物啟發(fā)模型，構(gòu)造的速度如下：

將生物啟發(fā)模型引入到控制器中，可以得到系統(tǒng)的運動學控制器為：

3 仿真實驗

對于跟隨AUV，可以通過姿態(tài)傳感器等來自身獲取航向角φ0、速度u0等信息。可以通過雙目視覺的運動物體方向識別算法檢測到領(lǐng)航AUV的相對航向φ、相對速度u。與自身航向航速進行計算，可得到領(lǐng)航AUV的航向φ0、速度u0。而領(lǐng)航AUV的位置可以通過雙目攝像頭來進行點匹配，通過三角測量的原理，計算像素之間的偏移來獲取領(lǐng)航AUV的位置信息（X0，Y0，Z0）。領(lǐng)航AUV和跟隨AUV的隊形可以根據(jù)測得位姿信息計算的出。

為了方便仿真，假設(shè)所有需要的向量都能被準確地探測到，領(lǐng)航AUV根據(jù)預定的軌跡航行，跟隨AUV則根據(jù)預定的隊形，完成編隊控制，達到期望隊形。

在MATLAB環(huán)境下控制三個AUV做三角形編隊實驗，各AUV的初始姿態(tài)分別為[x0(0),y0(0),z0(0),φ0(0)]=[0,0,2,0]、[x1(0),y1(0),z1(0),φ1(0)]=[-5,-5,2,0]、[x2(0),y2(0),z2(0),φ2(0)]=[5，-5，2，0]，初始時刻所有AUV均處于靜止狀態(tài)。編隊分為三個階段，第一個階段AUV0從初始位置駛向目標點[200，200，5]，AUV1和 AUV2分別按照期望隊形為

編隊軌跡和編隊誤差可見圖1-4，圖1-2是沒有將編隊控制器導入生物啟發(fā)模型的結(jié)果，圖3-4是引入生物啟發(fā)的編隊控制器。

圖1 多水下機器人編隊模型

圖2

圖3 未加入生物啟發(fā)的編隊誤差

圖4

圖5 引入生物啟發(fā)的編隊誤差

通過對比可以看出，兩種方法雖然都可以實現(xiàn)編隊控制，但是引入了生物啟發(fā)的控制算法有更好的編隊效果。雖然在兩種方法在大多在形成隊形以后都能保持隊形，但是在開始和拐點處，引入了生物啟發(fā)的控制算法的位姿誤差更小，形成編隊的速度更快一些。并且在拐點處，如果不引入生物啟發(fā)，會出現(xiàn)較大的速度、航向跳變，并且變化有超調(diào)量，需要調(diào)節(jié)一段時間才能形成隊形，編隊誤差才會趨于零。而引入生物啟發(fā)以后，速度、航向的變化更小、更平滑，所以編隊誤差更小、運動軌跡更平滑。

通過兩個仿真結(jié)果可知，由于引入生物啟發(fā)以后的編隊控制器具有連續(xù)平滑并且輸出有界的特性，所以編隊效果更好，可以有效避免MAUV在編隊過程中出現(xiàn)較大的速度、航向跳變，從而使AUV的推進器需要提供超過推進器推力上限的推力。

4 結(jié)語

由于水聲通信延遲大、易受干擾，而通過雙目攝像頭等傳感器獲取編隊信息，AUV之間不需要進行通信也可以進行編隊?？梢詰?yīng)用于一些拒止環(huán)境作業(yè)，或者編隊過程中水聲通信失效的情況下。采用領(lǐng)航跟隨控制方法研究MAUV的編隊控制問題，并且為了避免出現(xiàn)較大的速度、角度的跳變，導致推進器的推力飽和，引入了生物啟發(fā)進行平滑處理，通過仿真驗證了MAUV編隊的可以有效地形成期望隊形，達到全局漸進穩(wěn)定。