基于生物啟發和環境感知的多AUV編隊控制

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

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

0 引言

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

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

1 數學模型的建立

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

在AUV的六自由度運動學模型的基礎上，建立欠驅動AUV的四自由度運動學模型。本文只考慮AUV的位置量（x,y,z）、三個方向的速度（u,v,w）、艏向角φ以及艏向角速度r[15]。可以得到AUV的運動學模型：

1.2 領航--跟隨法編隊模型的建立

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

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

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

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

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

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

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

2 MAUV隊形控制設計

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

2.1 反步控制器設計

為了消除位置誤差，首先設計Lyapunov函數：

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

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

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

接下來，對虛擬速度進行鎮定，結合V1，構造Lyapunov函數：

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

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

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

2.2 生物啟發模型

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

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

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

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

本文利用生物啟發模型，構造的速度如下：

將生物啟發模型引入到控制器中，可以得到系統的運動學控制器為：

3 仿真實驗

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

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

在MATLAB環境下控制三個AUV做三角形編隊實驗，各AUV的初始姿態分別為[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均處于靜止狀態。編隊分為三個階段，第一個階段AUV0從初始位置駛向目標點[200，200，5]，AUV1和 AUV2分別按照期望隊形為

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

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

圖2

圖3 未加入生物啟發的編隊誤差

圖4

圖5 引入生物啟發的編隊誤差

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

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

4 結語

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