基于全向移動(dòng)平臺(tái)的多機(jī)器人編隊(duì)控制研究
2021-11-17 04:31:54李金芝張志安
計(jì)算機(jī)仿真 2021年2期
李金芝，張志安，程 志，江 濤
(南京理工大學(xué)機(jī)械工程學(xué)院，江蘇 南京 210094)
1 引言
目前，多機(jī)器人協(xié)同控制系統(tǒng)已成為控制領(lǐng)域和系統(tǒng)工程的研究熱點(diǎn)之一，受到機(jī)器人、數(shù)學(xué)、物理、生物等相關(guān)領(lǐng)域?qū)＜业膹V泛關(guān)注[1]。為提高機(jī)器人任務(wù)完成的效率，通常在執(zhí)行任務(wù)過程中，采取協(xié)同編隊(duì)的控制方法[2]。
在目前的編隊(duì)控制算法中，領(lǐng)航-跟隨編隊(duì)控制法[3]采用鏈?zhǔn)酵負(fù)浣Y(jié)構(gòu)，跟隨者跟蹤領(lǐng)航者形成隊(duì)形，具有數(shù)學(xué)分析簡單、易保持隊(duì)形、通信壓力小等優(yōu)點(diǎn)，被廣泛應(yīng)用于多機(jī)器人系統(tǒng)編隊(duì)中。但是該算法針對(duì)位置環(huán)境下的編隊(duì)控制研究甚少，缺少整體路徑規(guī)劃功能，特別是領(lǐng)航機(jī)器人如何在線進(jìn)行路徑規(guī)劃、實(shí)時(shí)自主避障有待更進(jìn)一步研究；人工勢(shì)場(chǎng)法是在機(jī)器人周圍設(shè)計(jì)一種類似電場(chǎng)的勢(shì)場(chǎng)，機(jī)器人的運(yùn)動(dòng)是靠勢(shì)場(chǎng)力驅(qū)動(dòng)。在生成路徑時(shí)只需要局部和靜態(tài)的信息、有著很強(qiáng)的靈活性和穩(wěn)定性，尤其在復(fù)雜環(huán)境下[4]。同時(shí)人工勢(shì)場(chǎng)法也存在著缺陷，文獻(xiàn)[5]指出該法依托局部環(huán)境信息運(yùn)算，缺少全局信息的指導(dǎo)，容易陷入局部極小值。
針對(duì)上述方法存在的問題，本文采用基于改進(jìn)領(lǐng)航-跟隨法和改進(jìn)人工勢(shì)場(chǎng)法相結(jié)合的編隊(duì)控制算法。領(lǐng)航-跟隨法負(fù)責(zé)整體編隊(duì)的隊(duì)形控制，引入虛擬跟隨機(jī)器人構(gòu)成虛擬領(lǐng)航-跟隨結(jié)構(gòu)隊(duì)形，確保編隊(duì)的穩(wěn)定性；采用改進(jìn)人工勢(shì)場(chǎng)法進(jìn)行在線局部路徑規(guī)劃，引入虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)，避免局部極小值。最終實(shí)現(xiàn)多機(jī)器人編隊(duì)的無碰撞路徑到達(dá)目標(biāo)點(diǎn)。
2 全向移動(dòng)平臺(tái)建立
本實(shí)驗(yàn)研究采用基于麥克納姆輪的全向移動(dòng)機(jī)器人作為研究平臺(tái)，該平臺(tái)能夠?qū)崿F(xiàn)前進(jìn)、后退、左移、右移以及旋轉(zhuǎn)等運(yùn)動(dòng)，具有水平、垂直和旋轉(zhuǎn)三個(gè)自由度，相比于其它移動(dòng)機(jī)器人平臺(tái)具有更高的靈活性。每個(gè)輪子由多個(gè)按照固定角度安裝的被動(dòng)滾輪組成，四個(gè)輪子所提供的軸外力可以使其進(jìn)行全方位運(yùn)動(dòng)[6][7]。
運(yùn)動(dòng)學(xué)建模是從理論上分析麥克納姆輪全向移動(dòng)的原理，為此建立圖1的運(yùn)動(dòng)分析示意圖。
圖1 麥克納姆輪運(yùn)動(dòng)分析示意圖
當(dāng)輪軸線和棍子軸線夾角α為45度時(shí)，四個(gè)車輪的轉(zhuǎn)速與平臺(tái)中心速度之間的映射關(guān)系算為
(1)
式(1)中，l1、l2-輪子中心到X軸和Y軸的距離；Vω-每個(gè)車輪的轉(zhuǎn)速；Vx、Vy-平臺(tái)在X軸和Y軸方向上的分速度；ωz-yaw軸自轉(zhuǎn)角速度。圖1表明了機(jī)器人速度和位置狀態(tài)間的關(guān)系，在給定平臺(tái)的驅(qū)動(dòng)速度和機(jī)器人初始位姿[xiyiθi]下，可以求得任意時(shí)刻t的機(jī)器人位姿[xi(t)yi(t)θi(t)]。
通常情況下，多機(jī)器人編隊(duì)系統(tǒng)所用到的坐標(biāo)系主要分為：全局坐標(biāo)系、機(jī)器人自身局部坐標(biāo)系以及傳感器坐標(biāo)系。全局坐標(biāo)系定義為xoy，坐標(biāo)原點(diǎn)在機(jī)器人起始位置處，第i個(gè)機(jī)器人t時(shí)刻在全局坐標(biāo)系下的位姿可表達(dá)為：zi=[xi(t)yi(t)θi(t)]；此外機(jī)器人還擁有自己的局部坐標(biāo)XOY，用來描述機(jī)器人之間相對(duì)位置關(guān)系，通過傳感器獲取的坐標(biāo)和運(yùn)動(dòng)控制指令的下達(dá)都是基于局部坐標(biāo)系進(jìn)行的。機(jī)器人在雙重坐標(biāo)系下的運(yùn)動(dòng)示意圖如圖2所示。
圖2 機(jī)器人在全局坐標(biāo)系下的位姿
3 多機(jī)器人編隊(duì)控制算法
3.1 傳統(tǒng)領(lǐng)航-跟隨法
領(lǐng)航-跟隨法主要有兩種編隊(duì)模式：l-φ和l-l，其中l(wèi)-φ應(yīng)用較多，主要原理是跟隨者和領(lǐng)航者之間保持一定的距離和角度，才能形成期望隊(duì)形。如圖3所示：控制目標(biāo)是limt→∞(ld-l)=0和limt→∞(φd-φ)=0，其中l(wèi)d和φd是跟隨機(jī)器人和領(lǐng)航機(jī)器人間期望的距離和角度，l和φ是實(shí)際測(cè)得的相對(duì)距離和角度。
圖3 l-φ領(lǐng)航-跟隨編隊(duì)結(jié)構(gòu)模型
3.2 虛擬結(jié)構(gòu)領(lǐng)航跟隨法
本文在傳統(tǒng)領(lǐng)航跟隨編隊(duì)法的控制思想上，基于l-φ的編隊(duì)模式，提出了虛擬結(jié)構(gòu)的領(lǐng)航-跟隨法。虛擬結(jié)構(gòu)描述了機(jī)器人編隊(duì)的剛性隊(duì)形，領(lǐng)航機(jī)器人作為隊(duì)形中一個(gè)固定的參考對(duì)象，跟隨機(jī)器人跟隨剛性隊(duì)形中相應(yīng)的虛擬機(jī)器人保證隊(duì)形。將編隊(duì)控制問題轉(zhuǎn)化為跟隨機(jī)器人跟隨虛擬機(jī)器人的跟蹤問題，通過不斷改變l、φ值，可以達(dá)到變換任意隊(duì)形的目的，具有一定的可行性。以三臺(tái)機(jī)器人為例，本文提出的虛擬結(jié)構(gòu)的領(lǐng)航-跟隨隊(duì)形的結(jié)構(gòu)的模型如圖4所示。
圖4 虛擬結(jié)構(gòu)的領(lǐng)航-跟隨型結(jié)構(gòu)模型
其中ZL為領(lǐng)航機(jī)器人，ZF1和ZF2為跟隨機(jī)器人，并且ZFV1、ZFV2表示ZF1、ZF2的期望虛擬機(jī)器人，通過轉(zhuǎn)化與計(jì)算領(lǐng)航者ZL的位姿，可以得出兩個(gè)虛擬機(jī)器人ZFV1、ZFV2的位姿表達(dá)式為：
(2)
(3)
ZL(xL，yL，θL)為領(lǐng)航者位姿，ZF1(xF1，yF1，θF1)、ZF2(xF2，yF2，θF2)是跟隨機(jī)器人位姿，跟隨機(jī)器人各自的虛擬跟隨機(jī)器人位姿為ZFV1(xFV1，yFV1，θFV1)、ZFV2(xFV2，yFV2，θFV2)，三者構(gòu)成了虛擬剛性三角形隊(duì)形。
以跟隨機(jī)器人ZF1為例，其中還可以得出跟隨機(jī)器人與虛擬機(jī)器人的位姿誤差表達(dá)式如下：
(4)
在本文搭建的全向移動(dòng)平臺(tái)上，根據(jù)麥克納姆輪速度解算公式(1)，將平臺(tái)在X軸上的分速度Vx看作是跟隨機(jī)器人與虛擬跟隨機(jī)器人之間距離在X軸上的誤差分量Δx；平臺(tái)在Y軸上的分速度Vy看作是跟隨機(jī)器人與虛擬跟隨機(jī)器人之間距離在X軸上的誤差分量Δy；平臺(tái)圍繞yaw軸自轉(zhuǎn)角速度ωz看作是跟隨機(jī)器人與虛擬跟隨機(jī)器人之間的角度誤差Δθ。將公式(1)和公式(3)進(jìn)行融合，四個(gè)麥輪的速度如下：
(5)
通過改變領(lǐng)航機(jī)器人和跟隨機(jī)器人之間的距離l和角度值φ，即可改變四個(gè)麥輪的速度輸入值。選擇的控制行為(l，φ)T需要使得Δx、Δy、Δθ都逼近于零，即limt→∞ΔFiVi(t)=0，i∈[1，n]，當(dāng)上式成立時(shí)，意味著編隊(duì)中每一個(gè)跟隨機(jī)器人都到達(dá)其對(duì)應(yīng)的虛擬跟隨機(jī)器人位置，最終形成期望的隊(duì)形完成編隊(duì)任務(wù)。
4 改進(jìn)人工勢(shì)場(chǎng)法的編隊(duì)避障控制
4.1 傳統(tǒng)人工勢(shì)場(chǎng)法
1986年KHATB首次提出了人工勢(shì)場(chǎng)法，將機(jī)器人所處的環(huán)境虛擬為人工勢(shì)場(chǎng)，機(jī)器人在虛擬環(huán)境中運(yùn)動(dòng)，障礙物周圍存在斥力場(chǎng)對(duì)其產(chǎn)生斥力，目標(biāo)點(diǎn)周圍存在引力場(chǎng)對(duì)其產(chǎn)生引力，在引力與斥力的合力作用下，機(jī)器人沿最小化勢(shì)能方向運(yùn)動(dòng)[8]。人工勢(shì)場(chǎng)法具有簡單的數(shù)學(xué)模型、依賴信息少等優(yōu)勢(shì)，尤其是在避障方面的獨(dú)特優(yōu)勢(shì)，非常適合未知復(fù)雜環(huán)境下的在線局部路徑規(guī)劃任務(wù)。但正是由于能夠?qū)崿F(xiàn)快速優(yōu)化的特點(diǎn)，該方法在尋找最優(yōu)解的過程中往往容易陷入非真正最優(yōu)的次優(yōu)位置，即局部最小值[9]。在python中仿真如圖5所示。
圖5 陷入局部最小值示意圖
4.2 虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)
上述局部最小值的形成主要是人工勢(shì)場(chǎng)產(chǎn)生的斥力和引力反向共線，如圖6所示。
圖6 局部最小值形成示意圖
由圖6可知，當(dāng)機(jī)器人、障礙物和目標(biāo)點(diǎn)位于同一條直線時(shí)，機(jī)器人所受的斥力和引力共線。當(dāng)Fa
針對(duì)以上出現(xiàn)的局部最小值情況，本文在傳統(tǒng)人工勢(shì)場(chǎng)法的基礎(chǔ)上添加了一種虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)，如圖7所示。
圖7 虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)
當(dāng)機(jī)器人運(yùn)動(dòng)到位于目標(biāo)點(diǎn)和障礙物連線上，即位于障礙物的背面，此時(shí)除了原有斥力勢(shì)場(chǎng)和引力勢(shì)場(chǎng)作用外，還將受到水平于障礙物影響范圍邊緣的虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)的作用，在三力的共同作用下逐漸遠(yuǎn)離障礙物的影響區(qū)域。因此編隊(duì)中的領(lǐng)航者在平面區(qū)域中受到的總勢(shì)場(chǎng)函數(shù)為
(6)
式中：ΣU(x)-機(jī)器人在環(huán)境中x位置時(shí)的綜合勢(shì)場(chǎng)，Ua(x)-引力勢(shì)場(chǎng)，Uobs(x)-斥力勢(shì)場(chǎng)，Ur(x)-虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)，F(xiàn)(x)-總勢(shì)場(chǎng)力。
在圖7中，領(lǐng)航機(jī)器人RL受到的引力勢(shì)場(chǎng)函數(shù)為
(7)
式中，(x，y)-領(lǐng)航機(jī)器人實(shí)時(shí)位置；xgoal-目標(biāo)點(diǎn)位置；ka-引力增益系數(shù)。
同樣，機(jī)器人只有在和障礙物間的距離小于某常數(shù)D，才被視為進(jìn)入斥力勢(shì)場(chǎng)作用范圍內(nèi)。對(duì)勢(shì)場(chǎng)函數(shù)求解負(fù)梯度，即可得到斥力勢(shì)場(chǎng)力的表達(dá)式。RL和障礙物間的斥力勢(shì)場(chǎng)函數(shù)為
(8)
(9)
式中：f(x)-領(lǐng)航機(jī)器人處于x位置時(shí)傳感器探測(cè)與障礙物間距離，γ-斥力增益系數(shù)，D-障礙物斥力勢(shì)場(chǎng)影響區(qū)域半徑。
同時(shí)機(jī)器人還將受到虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)的作用。虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)的作用就是為打破引力和斥力之間的平衡，改變機(jī)器人受到的合力方向，使得機(jī)器人能夠跳出局部最小值陷阱，朝著目標(biāo)點(diǎn)前進(jìn)。勢(shì)場(chǎng)函數(shù)如下
(10)
Fr(x)=-?Ur(x)
(11)
圖8 虛擬斥力勢(shì)場(chǎng)曲線圖
本文運(yùn)用改進(jìn)的人工勢(shì)場(chǎng)法局部避障時(shí)，領(lǐng)航者根據(jù)目標(biāo)點(diǎn)產(chǎn)生的引力、自身的行駛速度，來計(jì)算下一時(shí)刻領(lǐng)航者以及跟隨者的坐標(biāo)軌跡。根據(jù)車載傳感器不斷探測(cè)周圍環(huán)境，當(dāng)傳感器探測(cè)距離f(x)大于D時(shí)，機(jī)器人只受到目標(biāo)點(diǎn)產(chǎn)生的引力作用；由圖8可得，當(dāng)傳感器探測(cè)距離f(x)小于D且Fa(x)=Fobs(x)時(shí)，此時(shí)機(jī)器人走進(jìn)局部極小值陷阱，本文引入的斥力旋轉(zhuǎn)勢(shì)場(chǎng)力陡然增大，方向和原有的斥力方向互相垂直，領(lǐng)航機(jī)器人受到的合力就會(huì)發(fā)生改變，在引力、斥力和虛擬勢(shì)場(chǎng)力的共同作用下，打破引力和斥力反向共線等值的局面，逐漸逃離局部極小值的陷阱區(qū)域；而當(dāng)傳感器探測(cè)距離f(x)小于D、Fa(x)≠Fobs(x)且二者方向不共線時(shí)，即引力和斥力平衡不存在時(shí)，斥力旋轉(zhuǎn)勢(shì)場(chǎng)則會(huì)逐漸減小，對(duì)機(jī)器人造成的影響可忽略不計(jì)，領(lǐng)航者就會(huì)帶領(lǐng)跟隨者在朝著目標(biāo)的合力作用下慢慢靠近目標(biāo)點(diǎn)。
5 仿真校驗(yàn)
本文利用python中的pygame模塊搭建了仿真環(huán)境，選用三臺(tái)全方位移動(dòng)機(jī)器人來驗(yàn)證該領(lǐng)航-跟隨型編隊(duì)控制方法的有效性。分別在無障礙環(huán)境和有障礙環(huán)境下驗(yàn)證編隊(duì)算法的可行性，不僅可以形成任意隊(duì)形、保持隊(duì)形穩(wěn)定，還可以隨時(shí)切換隊(duì)形，并且在遇到障礙物時(shí)能夠及時(shí)躲避并順利到達(dá)目的地，從而實(shí)現(xiàn)優(yōu)化編隊(duì)控制的目的。
5.1 無障礙環(huán)境下編隊(duì)仿真
在無障礙環(huán)境下，通過設(shè)定三種期望隊(duì)形：三角形、柱形和線性的編隊(duì)隊(duì)形，隊(duì)形的l、φ和機(jī)器人起始點(diǎn)參數(shù)見表1。在改進(jìn)的領(lǐng)航-跟隨編隊(duì)控制算法下，機(jī)器人從起始點(diǎn)出發(fā)形成目標(biāo)隊(duì)形，仿真結(jié)果如圖9所示。
表1 預(yù)設(shè)隊(duì)形的參數(shù)
圖9 編隊(duì)隊(duì)形形成
其中，R1是領(lǐng)航者，R2、R3是跟隨者，Rv2、Rv3是虛擬跟隨者。由仿真可得，在構(gòu)建的400×400地圖上，跟隨機(jī)器人能夠通過自身與虛擬跟隨機(jī)器人的位置誤差，不斷調(diào)整行進(jìn)步長，最終到達(dá)虛擬機(jī)器人的位置點(diǎn)，形成期望隊(duì)形，驗(yàn)證了該算法的有效性。
為了進(jìn)一步驗(yàn)證本文提出的虛擬結(jié)構(gòu)領(lǐng)航-跟隨法的高效性，現(xiàn)進(jìn)行隊(duì)形變換仿真。三臺(tái)機(jī)器人位于地圖左下角，通過隊(duì)形變換到達(dá)目的地。仿真結(jié)果如圖所10示。
圖10 編隊(duì)隊(duì)形變換過程
由圖10可得，在構(gòu)建的650×600地圖上，R1(160，590)是領(lǐng)航者，R2(100，590)和R3(230，530)是跟隨者，A(230，80)點(diǎn)和B(400，40)點(diǎn)是隊(duì)形變換點(diǎn)，C(500，400)點(diǎn)是目標(biāo)點(diǎn)，圖中虛點(diǎn)圓是三臺(tái)機(jī)器人的運(yùn)動(dòng)軌跡。在提出的虛擬結(jié)構(gòu)領(lǐng)航-跟隨編隊(duì)控制法下，機(jī)器人隊(duì)伍首先形成三角形編隊(duì)隊(duì)形，在A點(diǎn)變換為線形編隊(duì)隊(duì)形，接著在B點(diǎn)變換為柱形編隊(duì)隊(duì)形，最后到達(dá)目的地C點(diǎn)。通過仿真實(shí)驗(yàn)，體現(xiàn)了隊(duì)形切換的靈活性和隊(duì)形保持的穩(wěn)定性，從而驗(yàn)證了本文改進(jìn)算法的高效性。
5.2 障礙環(huán)境下編隊(duì)避障仿真
在靜態(tài)障礙物環(huán)境下，運(yùn)用本文提出的虛擬結(jié)構(gòu)領(lǐng)航-跟隨法進(jìn)行編隊(duì)控制，改進(jìn)人工勢(shì)場(chǎng)算法進(jìn)行避障仿真實(shí)驗(yàn)，仿真時(shí)設(shè)定ka=0.8，γ=0.9，ε=0.5，D=10，實(shí)驗(yàn)結(jié)果如圖11所示。
圖11 復(fù)雜環(huán)境下機(jī)器人隊(duì)伍避障實(shí)驗(yàn)
首先構(gòu)建600×600的環(huán)境地圖，并在地圖上安排好3臺(tái)機(jī)器人的出發(fā)點(diǎn)以及目標(biāo)點(diǎn)。R1(130，540)是領(lǐng)航者，R2(80，540)和R3(180，560)是跟隨者，圖中的虛點(diǎn)圓是三臺(tái)機(jī)器人運(yùn)動(dòng)過程中的軌跡。從圖11可知，在障礙物環(huán)境下，通過本文提出的結(jié)合算法，領(lǐng)航機(jī)器人依然能夠規(guī)劃一條無碰撞路徑，兩臺(tái)跟隨機(jī)器人通過不斷跟隨虛擬跟隨機(jī)器人，從起始點(diǎn)順利到達(dá)目標(biāo)點(diǎn)。機(jī)器人隊(duì)伍首先從起始點(diǎn)出發(fā)形成三角形編隊(duì)隊(duì)形，并保持該隊(duì)形結(jié)構(gòu)前行，在遇到拱形障礙物A(200，245)時(shí)，切換隊(duì)伍形狀為柱形和線形來脫離拱形障礙物，在B(310，240)處以柱形編隊(duì)隊(duì)形穿過狹窄通道，并在C(370，240)處遇到了局部最小值點(diǎn)，此時(shí)在本文提出的虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)的作用下擺脫局部極小值，如圖12所。
圖12 改進(jìn)人工勢(shì)場(chǎng)法擺脫局部極小點(diǎn)過程
由圖12可知，在C處機(jī)器人、障礙物和目標(biāo)點(diǎn)位于同一直線，即局部極小值點(diǎn)，在原有的引力場(chǎng)Ua(x)和斥力場(chǎng)Uobs(x)作用下，機(jī)器人隊(duì)伍并不能擺脫局部極小值，此時(shí)在虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)Ur(x)作用下，機(jī)器人隊(duì)伍能夠跳出局部極小點(diǎn)，朝著目標(biāo)點(diǎn)前進(jìn)，并恢復(fù)到開始的三角隊(duì)形到達(dá)目的地。
6 結(jié)論
本文主要在基于麥克納姆輪的全向移動(dòng)平臺(tái)上研究了多機(jī)器人的編隊(duì)控制和協(xié)同避障問題，提出了虛擬結(jié)構(gòu)領(lǐng)航-跟隨法實(shí)現(xiàn)多機(jī)器人編隊(duì)隊(duì)形控制，在傳統(tǒng)人工勢(shì)場(chǎng)法的基礎(chǔ)上設(shè)計(jì)了一種虛擬斥力旋轉(zhuǎn)勢(shì)場(chǎng)，幫助多機(jī)器人隊(duì)伍跳出傳統(tǒng)人工勢(shì)場(chǎng)法中的局部最小點(diǎn)的陷阱，達(dá)到多機(jī)器人編隊(duì)無碰撞避障。通過兩組仿真證明了算法的可行性，多機(jī)器人隊(duì)伍不僅能夠形成任意隊(duì)形，保持隊(duì)形的穩(wěn)定性，同時(shí)在障礙環(huán)境下成功避開障礙物，進(jìn)一步驗(yàn)證了算法的有效性和穩(wěn)定性。
