全向移動小車編隊軌跡跟蹤及防碰撞控制系統(tǒng)研究

張曉峰，于登秀，馮 喆，徐 浩

（1.西北工業(yè)大學(xué)無人系統(tǒng)技術(shù)研究院，西安710072；2.西安科技大學(xué)電氣與控制工程學(xué)院，西安710600）

1 引言

多智能體系統(tǒng)編隊控制成為當(dāng)前國際和國內(nèi)控制問題研究領(lǐng)域的一個熱點，其在航空航天、環(huán)境監(jiān)測、社會學(xué)和生物學(xué)等領(lǐng)域有著非常廣闊的應(yīng)用前景。本文主要借助人工勢場法和滑?？刂品椒▉硌芯炕邴溈思{姆輪的全向移動小車系統(tǒng)的編隊控制問題。

近年來，全向移動小車引起了工業(yè)界與相關(guān)學(xué)者的廣泛關(guān)注，并且在社會的各個方面得到了很好的應(yīng)用［1-4］。文獻(xiàn)［5-6］主要研究了麥克納姆輪全向移動小車的運動學(xué)和動力學(xué)模型，解決了小車由于未知擾動和模型不確定引起的不穩(wěn)定問題。文獻(xiàn)［7］提出了一種基于模糊小波網(wǎng)絡(luò)的非奇異終端滑?？刂品椒?，用于麥克納姆輪全向移動小車的軌跡跟蹤和穩(wěn)定，其中模糊小波網(wǎng)絡(luò)被用來逼近控制器設(shè)計中的一些不確定非線性項。上述文獻(xiàn)均是對一臺全向移動小車的控制進(jìn)行研究，然而，隨著任務(wù)復(fù)雜度的增加，一些任務(wù)需要多臺全向移動小車協(xié)同完成。

多臺全向移動小車的協(xié)同控制屬于多智能體系統(tǒng)的研究范疇，已經(jīng)取得了一定的研究成果。文獻(xiàn)［8］考慮了當(dāng)系統(tǒng)拓?fù)鋱D是有向圖時的一致性問題，并且指出如果系統(tǒng)拓?fù)鋱D包含有生成樹，則該系統(tǒng)將會達(dá)到一致。文獻(xiàn)［9］應(yīng)用圖論和矩陣?yán)碚摻鉀Q了基于合作架構(gòu)的一致性協(xié)議收斂問題。文獻(xiàn)［10］研究了具有定向通信拓?fù)涞亩A非線性多智能體系統(tǒng)的漸進(jìn)一致性問題和有限時間一致性問題，運用滑?？刂评碚?，提出兩種新的分布式控制策略并進(jìn)行仿真，證明了提出的方法有效。但是以上研究均未考慮智能體之間的防碰撞問題，該問題在多智能體系統(tǒng)的實際應(yīng)用中至關(guān)重要。

人工勢場法原理簡單、實施方便，是實現(xiàn)多智能體系統(tǒng)防碰撞控制的有效解決方案之一［11-12］。人工勢能場法的基本思想為：智能體在一個虛擬的人工勢能場中運動，環(huán)境中的障礙物對智能體產(chǎn)生排斥力，從而使智能體能夠遠(yuǎn)離障礙物。智能體在所受力的作用下，將會沿著勢能場中勢能最小的方向運動。文獻(xiàn)［13］提出了一種使用人工勢能場法和帶有虛擬領(lǐng)導(dǎo)者的解決多智能體系統(tǒng)協(xié)作與分布式控制的框架，在該框架中，相鄰智能體之間的相互作用力驅(qū)動智能體運動。文獻(xiàn)［14］針對一類二階非線性多智能體系統(tǒng)，結(jié)合人工勢場法，提出了一種帶避碰策略的自適應(yīng)先導(dǎo)跟蹤編隊控制方法。但上文所述對多智能體系統(tǒng)的研究中，其研究對象一般為二階線性/非線性控制模型，據(jù)我們所知，目前對全向移動小車編隊防碰撞控制方面的研究甚少，本文的研究能夠推動多臺全向移動小車的實際應(yīng)用，具有重要的指導(dǎo)意義。

本文研究內(nèi)容如下：首先建立了全向移動小車控制模型；其次，通過圖論建立了多智能體系統(tǒng)控制模型；接著，采用滑模控制方法設(shè)計了編隊軌跡跟蹤控制器，采用人工勢場法實現(xiàn)了小車間的防碰撞，并對系統(tǒng)的穩(wěn)定性進(jìn)行了分析；最后，通過數(shù)值仿真證明了控制器的有效性。

2 背景知識

無向圖G=(V，E)由它的頂點和它的無向邊eij=(vi，vj)∈E?V×V所給出。注意對無向圖而言，eij=(vi，vj)，eji=(vj，vi)標(biāo)記的是同一條邊。圖G的兩個定點vi、vj稱為相鄰的頂點或者鄰居，如果滿足邊eij=(vi，vj)∈E，頂點vi的度vi∈V定義為與他直接相鄰的頂點數(shù)目。

對于給定的無向圖，其鄰接矩陣A的元素aij取值為1或0，即：

頂點vi的度定義為：

圖G的Laplacian 矩陣L定義為：L=D-A，其中D= diag(d1，…，dN)。

本文中，定義一個領(lǐng)導(dǎo)者的鄰接權(quán)重矩陣B=diag[b1b2…bN]T，其中bi代表小車i和虛擬領(lǐng)導(dǎo)者之間的通信拓?fù)錂?quán)重，如果小車i和虛擬領(lǐng)導(dǎo)者之間有通信，則bi＞0；否則bi= 0。

引理1：對于無向圖而言，其拉普拉斯矩陣是半正定矩陣，所以L+B為正定矩陣。

引理2［15］：函數(shù)V(t)是一個連續(xù)函數(shù)，?t＞0，V(t) ≥0，若V˙(t) ≤-γV(t) + κ則：

其中γ、κ為正常數(shù)。

3 編隊系統(tǒng)描述

3.1 全向移動小車的運動學(xué)模型和動力學(xué)模型

全向移動小車框架圖如圖1所示，根據(jù)其結(jié)構(gòu)特征以及動力學(xué)特征我們可以建立全向移動小車的的運動學(xué)及動力學(xué)模型，模型建立過程如下：

圖1 全向移動小車框架圖Fig.1 Frame diagram of omnidirectional mobile vehicle

VW=[VW1VW2VW3VW4]T∈R4×1為四個輪子的速度矢量，V0=[Vx Vy Vθ]T為小車自身坐標(biāo)系下整體的速度矢量，R為輪子的半徑，α為輥子軸線與X軸正半軸的夾角，麥克納姆輪一般為45°。對小車進(jìn)行整體的運動學(xué)分析可得系統(tǒng)的逆運動學(xué)模型：

系統(tǒng)的正運動學(xué)模型為：

其中，

將式（2）轉(zhuǎn)變?yōu)槭澜缱鴺?biāo)下系統(tǒng)的運動學(xué)模型：

其中

世界坐標(biāo)下車輛的位置和方向為x=[xw yw θ]T而J(θ)為逆運動學(xué)的雅可比矩陣，當(dāng)雅可比矩陣不滿秩時，系統(tǒng)存在運動學(xué)奇異點，此時系統(tǒng)失去可控性。本文取α= 45°、l1≠l2，此時雅可比矩陣滿秩，因此全向移動小車通過調(diào)節(jié)各麥克納姆輪的轉(zhuǎn)速，即可改變運動狀態(tài)，實現(xiàn)運動可控。

全向移動小車的動能方程為：

其中Vx，Vy，ωz含義同上，m是小車整體的質(zhì)量，Jz是車體的轉(zhuǎn)動慣量，Jω為車輪轉(zhuǎn)動慣量。

考慮因為靜摩擦導(dǎo)致的能量損失為：

其中Dθ為麥克納姆輪的粘性摩擦系數(shù)。

引入拉格朗日函數(shù)L=T1+T2+T3-V。全向移動小車位于平面水平運動，故V= 0。

利用歐拉-拉格朗日方程：

其中τ=[τ1τ2τ3τ4]T，θ=[θ1θ2θ3θ4]T，為各輪所受的靜摩擦力，并且

。

其中J+(θ)為J(θ)的逆矩陣，滿足J(θ)J+(θ) =I3。

由上式可推出全向移動小車個體模型：

其中u(t) =(RJ+(θ)M-1)τ為其控制輸入函數(shù)；f(t)為麥克納姆輪型全向移動小車個體非線性動態(tài)，且

3.2 小車編隊設(shè)計

以多智能體二階一致性算法為基礎(chǔ)，結(jié)合上述的小車動力學(xué)模型建立了一個由虛擬領(lǐng)導(dǎo)者引導(dǎo)的小車編隊系統(tǒng)。采用滑?？刂评碚摚O(shè)計了一種分布式控制器，以保證隨著時間的推移，小車編隊控制可以得到滿足。

定義一個參考信號，使虛擬領(lǐng)導(dǎo)者沿著參考信號的軌跡運行，它的運動學(xué)模型為：

其中xl=[xlx xly xlθ]T、vl=[vlx vly vlθ]T分別表示的是參考信號的位置和速度。ul為參考信號的控制輸入。

定義系統(tǒng)位置和速度誤差為：

其中δi=[δix δiy δiθ]T代表的是小車i與虛擬領(lǐng)導(dǎo)者的相對位置向量。

并且

誤差模型采用矩陣形式表示為：

取純培養(yǎng)的藻液20 μL置于無菌載玻片上，用結(jié)晶紫染色固定后在光學(xué)顯微鏡下觀察微藻形態(tài)特征。參考《中國淡水藻類-系統(tǒng)、分類及生態(tài)》對微藻進(jìn)行初步分類鑒定。

根據(jù)公式（12），我們可以得到：

3.3 避碰問題

在系統(tǒng)中，全向移動小車之間，需要保持一定的間距，以保證小車之間不會發(fā)生碰撞，建立如圖2所示的避碰范圍。

圖2 避碰范圍Fig.2 The range of collision avoidance

在小車運動過程中，當(dāng)小車間的范圍小于rout時，它們之間可能會存在碰撞，它們之間的距離小于rin，則認(rèn)為小車間已經(jīng)發(fā)生碰撞。所以對于每個小車在碰撞鄰域內(nèi)都有一組碰撞鄰居Nli=。

其中k為勢能函數(shù)系數(shù)，并且

其中?xi為xi的梯度。

4 控制器設(shè)計及穩(wěn)定性證明

控制輸入設(shè)計的目的是為了使小車編隊系統(tǒng)以固定的幾何隊形來實現(xiàn)漸進(jìn)一致性，同時在運行過程中小車個體之間避免碰撞。

根據(jù)滑模控制理論，為了使小車編隊實現(xiàn)漸進(jìn)一致性，我們設(shè)計整體編隊的滑模流形為：

其中，μ為一正定值。

設(shè)計編隊軌跡跟隨控制器為：

其中，c1、c2為正定值。

設(shè)計碰撞避免項的控制器為：

其中uC，j是第i個全向移動小車的碰撞避免控制項，且。

故系統(tǒng)總控制器u為：

定理1.對于公式（17）所示的誤差系統(tǒng)，分布式控制器（20）能夠保證集群系統(tǒng)跟蹤誤差的漸近收斂，即集群系統(tǒng)能夠?qū)崿F(xiàn)預(yù)期的編隊軌跡跟蹤及防碰撞控制效果。

證明.設(shè)計李雅普諾夫函數(shù)：

對函數(shù)V進(jìn)行求導(dǎo)：

將總的控制輸入u代入可得：

根據(jù)牛頓第三運動定律，任何兩個小車之間所受的虛擬避碰斥力大小相等方向相反。對于無向圖而言，拉普拉斯矩陣是對稱的，所以有(L+B)?ImuC= 0。因此：

由此可得系統(tǒng)最終可以達(dá)到穩(wěn)定狀態(tài)。

當(dāng)達(dá)到滑模面上，即S= 0 時，με1+ε2= 0，故：。

設(shè)計李雅普諾夫函數(shù)為：

對其進(jìn)行求導(dǎo)得：

由引理2 可得：當(dāng)t→∞時，ε1→0 同時ε2→0。

注1.文中uA能夠驅(qū)動多麥克納姆輪車系統(tǒng)實現(xiàn)預(yù)期的編隊軌跡跟蹤控制效果，uC能夠保證系統(tǒng)中的麥克納姆輪車之間不會發(fā)生碰撞，并且該部分控制輸入僅在任意兩臺麥克納姆輪車之間的距離時起作用。

5 仿真實驗

為了驗證所提出控制策略的有效性，我們進(jìn)行了仿真實驗。在實驗中，小車編隊共有六個小車。他們的通信拓?fù)浣Y(jié)構(gòu)如圖3所示，并且在表1 中列出小車個體的主要參數(shù)值。

圖3 小車編隊拓?fù)鋱DFig.3 Topology of vehicles formation

表1 小車的主要參數(shù)Table 1 The main parameters of the vehicles

此外給定六個小車的初始位置分別為：x1，0=［0，0，0］T，x2，0=［-5，-5，0］T，x3，0=［5，-5，0］T，x4，0=［0，-10，0］T，x5，0=［-10，-10，0］T，x6，0=［10，-10，0］T；小車在初始位置保持靜止。給定參考信號的信息為：xl=[5cos(0.1πt)，5sin(0.1πt)，0]T，分別設(shè)定每個小車與虛擬領(lǐng)導(dǎo)者之間的位置偏差向量為δ1，0=[0，0，0]T，δ2，0=[-6，-6，0]T，δ3，0=[6，-6，0]T，δ4，0=[0，-12，0]T，δ5，0=［-12，-12，0］T，δ6，0=[12，-12，0]T。

設(shè)置滑?？刂茀?shù)為c1= 5；c2= 1；μ= 2，設(shè)置防碰撞控制參數(shù)為rin= 2；rout= 5。

以x方向上的編隊狀態(tài)為例，圖4 為小車編隊的位置跟蹤軌跡，圖5 為小車編隊的位置跟蹤誤差，圖6為小車編隊的速度跟蹤軌跡，圖7為小車編隊的速度跟蹤誤差。由圖5 可以看出小車編隊過程中可以相互之間保持固定的隊形。由圖4 可以看出編隊中的每一個小車都可以追蹤參考信號，按照參考信號的軌跡運行。由圖6和圖7可以看出在編隊運行過程中，小車編隊可以按照同一速度運行。由圖8所示，兩輛小車之間的最小距離在編隊運動開始的階段進(jìn)入到防碰撞控制區(qū)域，但在人工勢場以及編隊控制算法的共同作用下迅速脫離，并且任意輛小車之間的最小距離始終與rin保持一定的偏差，說明在編隊控制過程中并沒有發(fā)生碰撞。

圖4 小車編隊在x方向的位置跟蹤軌跡Fig.4 Position tracking trajectory of vehicles formation in x direction

圖5 小車編隊在x方向的位置跟蹤誤差Fig.5 Position tracking errors of vehicles formation in x direction

圖6 小車編隊在x方向的速度跟蹤軌跡Fig.6 Velocity tracking trajectory of vehicles formation in x direction

圖7 小車編隊在x方向的速度跟蹤誤差Fig.7 Velocity tracking errors of vehicles formation in x direction

圖8 小車之間的最小距離Fig.8 Minimum distance between of vehicles

6 結(jié)論

本文針對全向移動小車編隊的軌跡跟蹤及防碰撞問題，采用滑模控制理論設(shè)計了一種分布式控制器，使小車編隊以固定的幾何隊形逐漸到達(dá)期望的位置，并且設(shè)計了一種人工勢能函數(shù)。在編隊運行過程中使小車保持距離避免相互碰撞，本研究以全向移動小車為研究對象，解決了編隊軌跡跟蹤過程中的防碰撞控制問題，有助于加快多智能體系統(tǒng)的實用化進(jìn)程。