基于生物啟發(fā)和環(huán)境感知的多AUV編隊(duì)控制

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

（沈陽(yáng)理工大學(xué)自動(dòng)化與電氣工程學(xué)院，沈陽(yáng)110159）

0 引言

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

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

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

1.1 三維空間的AUV運(yùn)動(dòng)學(xué)模型建立

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

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

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

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

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

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

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

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

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

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

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

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

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

顯然，當(dāng)且僅當(dāng)ex=ey=ez=0時(shí)，V1=0；否則V1＞0恒成立，對(duì)式（9）求導(dǎo)可得：

為了使V1收斂，則必須為負(fù)值，則設(shè)計(jì)期望的運(yùn)動(dòng)學(xué)控制率為：

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

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

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

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

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

2.2 生物啟發(fā)模型

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

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

后來(lái)經(jīng)過(guò)Grossberg進(jìn)一步改進(jìn)，簡(jiǎn)化了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)的輸出，對(duì)于任何激勵(lì)與抑制信號(hào)都能控制?i在[-D,B]范圍內(nèi)變化，使輸出信號(hào)?i平滑，可以用于解決編隊(duì)過(guò)程中速度跳變。

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

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

3 仿真實(shí)驗(yàn)

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

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

在MATLAB環(huán)境下控制三個(gè)AUV做三角形編隊(duì)實(shí)驗(yàn)，各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]，初始時(shí)刻所有AUV均處于靜止?fàn)顟B(tài)。編隊(duì)分為三個(gè)階段，第一個(gè)階段AUV0從初始位置駛向目標(biāo)點(diǎn)[200，200，5]，AUV1和 AUV2分別按照期望隊(duì)形為

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

圖1 多水下機(jī)器人編隊(duì)模型

圖2

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

圖4

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

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

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

4 結(jié)語(yǔ)

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