基于全向移動平臺的多機器人編隊控制研究

李金芝，張志安，程 志，江 濤

(南京理工大學機械工程學院，江蘇 南京 210094)

1 引言

目前，多機器人協(xié)同控制系統(tǒng)已成為控制領(lǐng)域和系統(tǒng)工程的研究熱點之一，受到機器人、數(shù)學、物理、生物等相關(guān)領(lǐng)域?qū)＜业膹V泛關(guān)注[1]。為提高機器人任務完成的效率，通常在執(zhí)行任務過程中，采取協(xié)同編隊的控制方法[2]。

在目前的編隊控制算法中，領(lǐng)航-跟隨編隊控制法[3]采用鏈式拓撲結(jié)構(gòu)，跟隨者跟蹤領(lǐng)航者形成隊形，具有數(shù)學分析簡單、易保持隊形、通信壓力小等優(yōu)點，被廣泛應用于多機器人系統(tǒng)編隊中。但是該算法針對位置環(huán)境下的編隊控制研究甚少，缺少整體路徑規(guī)劃功能，特別是領(lǐng)航機器人如何在線進行路徑規(guī)劃、實時自主避障有待更進一步研究；人工勢場法是在機器人周圍設(shè)計一種類似電場的勢場，機器人的運動是靠勢場力驅(qū)動。在生成路徑時只需要局部和靜態(tài)的信息、有著很強的靈活性和穩(wěn)定性，尤其在復雜環(huán)境下[4]。同時人工勢場法也存在著缺陷，文獻[5]指出該法依托局部環(huán)境信息運算，缺少全局信息的指導，容易陷入局部極小值。

針對上述方法存在的問題，本文采用基于改進領(lǐng)航-跟隨法和改進人工勢場法相結(jié)合的編隊控制算法。領(lǐng)航-跟隨法負責整體編隊的隊形控制，引入虛擬跟隨機器人構(gòu)成虛擬領(lǐng)航-跟隨結(jié)構(gòu)隊形，確保編隊的穩(wěn)定性；采用改進人工勢場法進行在線局部路徑規(guī)劃，引入虛擬斥力旋轉(zhuǎn)勢場，避免局部極小值。最終實現(xiàn)多機器人編隊的無碰撞路徑到達目標點。

2 全向移動平臺建立

本實驗研究采用基于麥克納姆輪的全向移動機器人作為研究平臺，該平臺能夠?qū)崿F(xiàn)前進、后退、左移、右移以及旋轉(zhuǎn)等運動，具有水平、垂直和旋轉(zhuǎn)三個自由度，相比于其它移動機器人平臺具有更高的靈活性。每個輪子由多個按照固定角度安裝的被動滾輪組成，四個輪子所提供的軸外力可以使其進行全方位運動[6][7]。

運動學建模是從理論上分析麥克納姆輪全向移動的原理，為此建立圖1的運動分析示意圖。

圖1 麥克納姆輪運動分析示意圖

當輪軸線和棍子軸線夾角α為45度時，四個車輪的轉(zhuǎn)速與平臺中心速度之間的映射關(guān)系算為

(1)

式(1)中，l1、l2-輪子中心到X軸和Y軸的距離；Vω-每個車輪的轉(zhuǎn)速；Vx、Vy-平臺在X軸和Y軸方向上的分速度；ωz-yaw軸自轉(zhuǎn)角速度。圖1表明了機器人速度和位置狀態(tài)間的關(guān)系，在給定平臺的驅(qū)動速度和機器人初始位姿[xiyiθi]下，可以求得任意時刻t的機器人位姿[xi(t)yi(t)θi(t)]。

通常情況下，多機器人編隊系統(tǒng)所用到的坐標系主要分為：全局坐標系、機器人自身局部坐標系以及傳感器坐標系。全局坐標系定義為xoy，坐標原點在機器人起始位置處，第i個機器人t時刻在全局坐標系下的位姿可表達為：zi=[xi(t)yi(t)θi(t)]；此外機器人還擁有自己的局部坐標XOY，用來描述機器人之間相對位置關(guān)系，通過傳感器獲取的坐標和運動控制指令的下達都是基于局部坐標系進行的。機器人在雙重坐標系下的運動示意圖如圖2所示。

圖2 機器人在全局坐標系下的位姿

3 多機器人編隊控制算法

3.1 傳統(tǒng)領(lǐng)航-跟隨法

領(lǐng)航-跟隨法主要有兩種編隊模式：l-φ和l-l，其中l(wèi)-φ應用較多，主要原理是跟隨者和領(lǐng)航者之間保持一定的距離和角度，才能形成期望隊形。如圖3所示：控制目標是limt→∞(ld-l)=0和limt→∞(φd-φ)=0，其中l(wèi)d和φd是跟隨機器人和領(lǐng)航機器人間期望的距離和角度，l和φ是實際測得的相對距離和角度。

圖3 l-φ領(lǐng)航-跟隨編隊結(jié)構(gòu)模型

3.2 虛擬結(jié)構(gòu)領(lǐng)航跟隨法

本文在傳統(tǒng)領(lǐng)航跟隨編隊法的控制思想上，基于l-φ的編隊模式，提出了虛擬結(jié)構(gòu)的領(lǐng)航-跟隨法。虛擬結(jié)構(gòu)描述了機器人編隊的剛性隊形，領(lǐng)航機器人作為隊形中一個固定的參考對象，跟隨機器人跟隨剛性隊形中相應的虛擬機器人保證隊形。將編隊控制問題轉(zhuǎn)化為跟隨機器人跟隨虛擬機器人的跟蹤問題，通過不斷改變l、φ值，可以達到變換任意隊形的目的，具有一定的可行性。以三臺機器人為例，本文提出的虛擬結(jié)構(gòu)的領(lǐng)航-跟隨隊形的結(jié)構(gòu)的模型如圖4所示。

圖4 虛擬結(jié)構(gòu)的領(lǐng)航-跟隨型結(jié)構(gòu)模型

其中ZL為領(lǐng)航機器人，ZF1和ZF2為跟隨機器人，并且ZFV1、ZFV2表示ZF1、ZF2的期望虛擬機器人，通過轉(zhuǎn)化與計算領(lǐng)航者ZL的位姿，可以得出兩個虛擬機器人ZFV1、ZFV2的位姿表達式為：

(2)

(3)

ZL(xL，yL，θL)為領(lǐng)航者位姿，ZF1(xF1，yF1，θF1)、ZF2(xF2，yF2，θF2)是跟隨機器人位姿，跟隨機器人各自的虛擬跟隨機器人位姿為ZFV1(xFV1，yFV1，θFV1)、ZFV2(xFV2，yFV2，θFV2)，三者構(gòu)成了虛擬剛性三角形隊形。

以跟隨機器人ZF1為例，其中還可以得出跟隨機器人與虛擬機器人的位姿誤差表達式如下：

(4)

在本文搭建的全向移動平臺上，根據(jù)麥克納姆輪速度解算公式(1)，將平臺在X軸上的分速度Vx看作是跟隨機器人與虛擬跟隨機器人之間距離在X軸上的誤差分量Δx；平臺在Y軸上的分速度Vy看作是跟隨機器人與虛擬跟隨機器人之間距離在X軸上的誤差分量Δy；平臺圍繞yaw軸自轉(zhuǎn)角速度ωz看作是跟隨機器人與虛擬跟隨機器人之間的角度誤差Δθ。將公式(1)和公式(3)進行融合，四個麥輪的速度如下：

(5)

通過改變領(lǐng)航機器人和跟隨機器人之間的距離l和角度值φ，即可改變四個麥輪的速度輸入值。選擇的控制行為(l，φ)T需要使得Δx、Δy、Δθ都逼近于零，即limt→∞ΔFiVi(t)=0，i∈[1，n]，當上式成立時，意味著編隊中每一個跟隨機器人都到達其對應的虛擬跟隨機器人位置，最終形成期望的隊形完成編隊任務。

4 改進人工勢場法的編隊避障控制

4.1 傳統(tǒng)人工勢場法

1986年KHATB首次提出了人工勢場法，將機器人所處的環(huán)境虛擬為人工勢場，機器人在虛擬環(huán)境中運動，障礙物周圍存在斥力場對其產(chǎn)生斥力，目標點周圍存在引力場對其產(chǎn)生引力，在引力與斥力的合力作用下，機器人沿最小化勢能方向運動[8]。人工勢場法具有簡單的數(shù)學模型、依賴信息少等優(yōu)勢，尤其是在避障方面的獨特優(yōu)勢，非常適合未知復雜環(huán)境下的在線局部路徑規(guī)劃任務。但正是由于能夠?qū)崿F(xiàn)快速優(yōu)化的特點，該方法在尋找最優(yōu)解的過程中往往容易陷入非真正最優(yōu)的次優(yōu)位置，即局部最小值[9]。在python中仿真如圖5所示。

圖5 陷入局部最小值示意圖

4.2 虛擬斥力旋轉(zhuǎn)勢場

上述局部最小值的形成主要是人工勢場產(chǎn)生的斥力和引力反向共線，如圖6所示。

圖6 局部最小值形成示意圖

由圖6可知，當機器人、障礙物和目標點位于同一條直線時，機器人所受的斥力和引力共線。當FaFr時，即引力大于斥力，機器人會遇到障礙物；當Fa=Fr時，即引力和斥力相等，機器人則會認為到達目標點。最終機器人會在局部最小值處徘徊，無法到達目標點。

針對以上出現(xiàn)的局部最小值情況，本文在傳統(tǒng)人工勢場法的基礎(chǔ)上添加了一種虛擬斥力旋轉(zhuǎn)勢場，如圖7所示。

圖7 虛擬斥力旋轉(zhuǎn)勢場

當機器人運動到位于目標點和障礙物連線上，即位于障礙物的背面，此時除了原有斥力勢場和引力勢場作用外，還將受到水平于障礙物影響范圍邊緣的虛擬斥力旋轉(zhuǎn)勢場的作用，在三力的共同作用下逐漸遠離障礙物的影響區(qū)域。因此編隊中的領(lǐng)航者在平面區(qū)域中受到的總勢場函數(shù)為

(6)

式中：ΣU(x)-機器人在環(huán)境中x位置時的綜合勢場，Ua(x)-引力勢場，Uobs(x)-斥力勢場，Ur(x)-虛擬斥力旋轉(zhuǎn)勢場，F(xiàn)(x)-總勢場力。

在圖7中，領(lǐng)航機器人RL受到的引力勢場函數(shù)為

(7)

式中，(x，y)-領(lǐng)航機器人實時位置；xgoal-目標點位置；ka-引力增益系數(shù)。

同樣，機器人只有在和障礙物間的距離小于某常數(shù)D，才被視為進入斥力勢場作用范圍內(nèi)。對勢場函數(shù)求解負梯度，即可得到斥力勢場力的表達式。RL和障礙物間的斥力勢場函數(shù)為

(8)

(9)

式中：f(x)-領(lǐng)航機器人處于x位置時傳感器探測與障礙物間距離，γ-斥力增益系數(shù)，D-障礙物斥力勢場影響區(qū)域半徑。

同時機器人還將受到虛擬斥力旋轉(zhuǎn)勢場的作用。虛擬斥力旋轉(zhuǎn)勢場的作用就是為打破引力和斥力之間的平衡，改變機器人受到的合力方向，使得機器人能夠跳出局部最小值陷阱，朝著目標點前進。勢場函數(shù)如下

(10)

Fr(x)=-?Ur(x)

(11)

圖8 虛擬斥力勢場曲線圖

本文運用改進的人工勢場法局部避障時，領(lǐng)航者根據(jù)目標點產(chǎn)生的引力、自身的行駛速度，來計算下一時刻領(lǐng)航者以及跟隨者的坐標軌跡。根據(jù)車載傳感器不斷探測周圍環(huán)境，當傳感器探測距離f(x)大于D時，機器人只受到目標點產(chǎn)生的引力作用；由圖8可得，當傳感器探測距離f(x)小于D且Fa(x)=Fobs(x)時，此時機器人走進局部極小值陷阱，本文引入的斥力旋轉(zhuǎn)勢場力陡然增大，方向和原有的斥力方向互相垂直，領(lǐng)航機器人受到的合力就會發(fā)生改變，在引力、斥力和虛擬勢場力的共同作用下，打破引力和斥力反向共線等值的局面，逐漸逃離局部極小值的陷阱區(qū)域；而當傳感器探測距離f(x)小于D、Fa(x)≠Fobs(x)且二者方向不共線時，即引力和斥力平衡不存在時，斥力旋轉(zhuǎn)勢場則會逐漸減小，對機器人造成的影響可忽略不計，領(lǐng)航者就會帶領(lǐng)跟隨者在朝著目標的合力作用下慢慢靠近目標點。

5 仿真校驗

本文利用python中的pygame模塊搭建了仿真環(huán)境，選用三臺全方位移動機器人來驗證該領(lǐng)航-跟隨型編隊控制方法的有效性。分別在無障礙環(huán)境和有障礙環(huán)境下驗證編隊算法的可行性，不僅可以形成任意隊形、保持隊形穩(wěn)定，還可以隨時切換隊形，并且在遇到障礙物時能夠及時躲避并順利到達目的地，從而實現(xiàn)優(yōu)化編隊控制的目的。

5.1 無障礙環(huán)境下編隊仿真

在無障礙環(huán)境下，通過設(shè)定三種期望隊形：三角形、柱形和線性的編隊隊形，隊形的l、φ和機器人起始點參數(shù)見表1。在改進的領(lǐng)航-跟隨編隊控制算法下，機器人從起始點出發(fā)形成目標隊形，仿真結(jié)果如圖9所示。

表1 預設(shè)隊形的參數(shù)

圖9 編隊隊形形成

其中，R1是領(lǐng)航者，R2、R3是跟隨者，Rv2、Rv3是虛擬跟隨者。由仿真可得，在構(gòu)建的400×400地圖上，跟隨機器人能夠通過自身與虛擬跟隨機器人的位置誤差，不斷調(diào)整行進步長，最終到達虛擬機器人的位置點，形成期望隊形，驗證了該算法的有效性。

為了進一步驗證本文提出的虛擬結(jié)構(gòu)領(lǐng)航-跟隨法的高效性，現(xiàn)進行隊形變換仿真。三臺機器人位于地圖左下角，通過隊形變換到達目的地。仿真結(jié)果如圖所10示。

圖10 編隊隊形變換過程

由圖10可得，在構(gòu)建的650×600地圖上，R1(160，590)是領(lǐng)航者，R2(100，590)和R3(230，530)是跟隨者，A(230，80)點和B(400，40)點是隊形變換點，C(500，400)點是目標點，圖中虛點圓是三臺機器人的運動軌跡。在提出的虛擬結(jié)構(gòu)領(lǐng)航-跟隨編隊控制法下，機器人隊伍首先形成三角形編隊隊形，在A點變換為線形編隊隊形，接著在B點變換為柱形編隊隊形，最后到達目的地C點。通過仿真實驗，體現(xiàn)了隊形切換的靈活性和隊形保持的穩(wěn)定性，從而驗證了本文改進算法的高效性。

5.2 障礙環(huán)境下編隊避障仿真

在靜態(tài)障礙物環(huán)境下，運用本文提出的虛擬結(jié)構(gòu)領(lǐng)航-跟隨法進行編隊控制，改進人工勢場算法進行避障仿真實驗，仿真時設(shè)定ka=0.8，γ=0.9，ε=0.5，D=10，實驗結(jié)果如圖11所示。

圖11 復雜環(huán)境下機器人隊伍避障實驗

首先構(gòu)建600×600的環(huán)境地圖，并在地圖上安排好3臺機器人的出發(fā)點以及目標點。R1(130，540)是領(lǐng)航者，R2(80，540)和R3(180，560)是跟隨者，圖中的虛點圓是三臺機器人運動過程中的軌跡。從圖11可知，在障礙物環(huán)境下，通過本文提出的結(jié)合算法，領(lǐng)航機器人依然能夠規(guī)劃一條無碰撞路徑，兩臺跟隨機器人通過不斷跟隨虛擬跟隨機器人，從起始點順利到達目標點。機器人隊伍首先從起始點出發(fā)形成三角形編隊隊形，并保持該隊形結(jié)構(gòu)前行，在遇到拱形障礙物A(200，245)時，切換隊伍形狀為柱形和線形來脫離拱形障礙物，在B(310，240)處以柱形編隊隊形穿過狹窄通道，并在C(370，240)處遇到了局部最小值點，此時在本文提出的虛擬斥力旋轉(zhuǎn)勢場的作用下擺脫局部極小值，如圖12所。

圖12 改進人工勢場法擺脫局部極小點過程

由圖12可知，在C處機器人、障礙物和目標點位于同一直線，即局部極小值點，在原有的引力場Ua(x)和斥力場Uobs(x)作用下，機器人隊伍并不能擺脫局部極小值，此時在虛擬斥力旋轉(zhuǎn)勢場Ur(x)作用下，機器人隊伍能夠跳出局部極小點，朝著目標點前進，并恢復到開始的三角隊形到達目的地。

6 結(jié)論

本文主要在基于麥克納姆輪的全向移動平臺上研究了多機器人的編隊控制和協(xié)同避障問題，提出了虛擬結(jié)構(gòu)領(lǐng)航-跟隨法實現(xiàn)多機器人編隊隊形控制，在傳統(tǒng)人工勢場法的基礎(chǔ)上設(shè)計了一種虛擬斥力旋轉(zhuǎn)勢場，幫助多機器人隊伍跳出傳統(tǒng)人工勢場法中的局部最小點的陷阱，達到多機器人編隊無碰撞避障。通過兩組仿真證明了算法的可行性，多機器人隊伍不僅能夠形成任意隊形，保持隊形的穩(wěn)定性，同時在障礙環(huán)境下成功避開障礙物，進一步驗證了算法的有效性和穩(wěn)定性。