基于領(lǐng)航跟隨的多機(jī)器人編隊(duì)控制方法

高繼勛，黃全振，趙媛媛

（1.河南工程學(xué)院計(jì)算機(jī)學(xué)院，河南 鄭州 451191; 2.河南工程學(xué)院電氣信息學(xué)院，河南 鄭州 451191;3.鄭州工程技術(shù)學(xué)院，河南 鄭州450044）

0 引 言

隨著人工智能時(shí)代的到來(lái)，多機(jī)器人的控制已逐漸成為研究熱點(diǎn)[1-6]，其應(yīng)用場(chǎng)景包括但不限于農(nóng)業(yè)、軍事、航空等領(lǐng)域。在多機(jī)器人編隊(duì)的協(xié)同控制過(guò)程中，通常所期望隊(duì)形是可變的，以便于應(yīng)對(duì)單個(gè)機(jī)器人無(wú)法完成的復(fù)雜任務(wù)。在極坐標(biāo)下，利用線性反饋的方法建立“距離-角度”控制器模型，完成對(duì)多機(jī)器人編隊(duì)控制[7]。由于奇異點(diǎn)存在，該控制器存在一定問(wèn)題?？赏ㄟ^(guò)將極坐標(biāo)投影到笛卡爾坐標(biāo)系下，重新建立控制模型解決奇異點(diǎn)問(wèn)題[8-9]。鐘宜生等[10]提出，將多機(jī)器人可變編隊(duì)問(wèn)題描述為“不變的隊(duì)形在可變的坐標(biāo)系中運(yùn)動(dòng)”的編隊(duì)控制問(wèn)題。在上述基礎(chǔ)上，李清東[11]提出用多個(gè)不變隊(duì)形互相切換，以完成編隊(duì)控制。劉磊[12]采用以差速輪式機(jī)器人為研究對(duì)象，通過(guò)仿真驗(yàn)證該控制方法的可行性。

目前對(duì)于解決多機(jī)器人一般性編隊(duì)控制問(wèn)題，公開(kāi)文獻(xiàn)中尚未見(jiàn)到成熟方案?，F(xiàn)階段主要有人工勢(shì)場(chǎng)法[13]、基于行為法[14]、虛擬結(jié)構(gòu)法[15]和領(lǐng)航-跟隨法[16]。上述方法各有其優(yōu)缺點(diǎn)，相比較而言，領(lǐng)航-跟隨法的控制更簡(jiǎn)單，只需調(diào)整跟隨者與領(lǐng)航者的運(yùn)動(dòng)參數(shù)即可完成跟蹤控制，具有較好的穩(wěn)定性和靈活性[17-20]。

根據(jù)上述情況，本文提出了一種新穎的機(jī)器人編隊(duì)控制方法，該方法基于領(lǐng)航-跟隨法，引入“虛擬領(lǐng)航機(jī)器人”，將其他機(jī)器人作為跟隨者，建立控制模型，從而將編隊(duì)問(wèn)題轉(zhuǎn)化為跟蹤控制問(wèn)題。根據(jù)提出的控制算法，本文設(shè)計(jì)了跟隨誤差控制器，分析跟隨誤差及其收斂性。通過(guò)仿真與實(shí)驗(yàn)結(jié)果分析，驗(yàn)證本文提出的改進(jìn)型領(lǐng)航-跟隨控制方法在一致性控制中的有效性與穩(wěn)定性。

1 問(wèn)題描述和模型構(gòu)建

1.1 問(wèn)題描述

基于領(lǐng)航-跟隨法，可將編隊(duì)問(wèn)題轉(zhuǎn)化為跟隨機(jī)器人對(duì)虛擬領(lǐng)航機(jī)器人的軌跡跟蹤，該方法只需獲得虛擬領(lǐng)航者的運(yùn)動(dòng)參數(shù)和位姿即可實(shí)現(xiàn)跟蹤控制，一定程度上減少相互之間信息冗余及計(jì)算量。通過(guò)上述方法，建立虛擬領(lǐng)航機(jī)器人與跟隨機(jī)器人運(yùn)動(dòng)模型，如圖1所示。

圖1 領(lǐng)航-跟隨結(jié)構(gòu)示意圖

圖中，跟隨機(jī)器人R1與虛擬領(lǐng)航機(jī)器人R0的距離是ρ0，二者中心線連線與虛擬領(lǐng)航機(jī)器人R0軸心線的夾角是φ0。通過(guò)虛擬領(lǐng)航者的引入，將編隊(duì)控制轉(zhuǎn)化為跟隨移動(dòng)機(jī)器人R1對(duì)虛擬領(lǐng)航機(jī)器人R0的軌跡跟蹤，即跟隨機(jī)器人R1在一定時(shí)間內(nèi)跟隨虛擬領(lǐng)航機(jī)器人R0運(yùn)動(dòng)就可組成相應(yīng)的編隊(duì)。

1.2 模型構(gòu)建

非完整移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型[15]如下式：

由圖1可知，當(dāng)跟隨機(jī)器人R1與虛擬領(lǐng)航機(jī)器人R0之間的位姿（位置和姿態(tài)）誤差趨近于零，即：

假設(shè)在t時(shí)刻，虛擬領(lǐng)航機(jī)器人與期望軌跡運(yùn)動(dòng)示意圖如圖2所示。

圖2 跟蹤位置示意圖

其中，V為期望軌跡上P點(diǎn)的運(yùn)動(dòng)方向，與X軸正方向夾角為θ。P0P為虛擬領(lǐng)航機(jī)器人與期望軌跡上P點(diǎn)的連線。P點(diǎn)狀態(tài)表示為（X,Y,θ）。

虛擬領(lǐng)航機(jī)器人與期望軌跡誤差參數(shù)表示為：

通過(guò)對(duì)參數(shù)λ的調(diào)整，可完成跟隨機(jī)器人相對(duì)于期望軌跡的控制。

虛擬領(lǐng)航機(jī)器人運(yùn)動(dòng)學(xué)模型可表示為：

跟隨機(jī)器人參考軌跡模型為：

根據(jù)期望軌跡P點(diǎn)狀態(tài)及公式（4）可得到虛擬領(lǐng)航機(jī)器人位姿，其數(shù)學(xué)表達(dá)式如下：

2 控制器設(shè)計(jì)

跟隨機(jī)器人R1與虛擬領(lǐng)航機(jī)器人R0位姿誤差為ei，則ei可表示為：

其中，K1，K2，K3為正數(shù)。

定義Lyapunov函數(shù)VL為：

以上構(gòu)建了基于領(lǐng)航跟隨運(yùn)動(dòng)控制方法的控制器，并且證明了該控制器的穩(wěn)定性。下面對(duì)該方法進(jìn)行仿真與實(shí)驗(yàn)研究。

3 仿真分析與實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證系統(tǒng)及控制算法的正確性和有效性，利用仿真軟件和硬件平臺(tái)構(gòu)建模擬/物理實(shí)驗(yàn)系統(tǒng)進(jìn)行驗(yàn)證。仿真采取與期望軌跡重合（追蹤）、與期望軌跡保持固定隊(duì)形（編隊(duì)）兩種情況進(jìn)行，便于在不同控制場(chǎng)景下對(duì)結(jié)果分析。

3.1 仿真分析

仿真研究基于Gazebo機(jī)器人仿真軟件構(gòu)建的運(yùn)動(dòng)學(xué)與控制仿真平臺(tái)。仿真固定參數(shù)以實(shí)際硬件平臺(tái)為參考，其固定參數(shù)值如表1所示。

表1 仿真基本參數(shù)

圖3 λ=[0 0]T跟隨機(jī)器人仿真運(yùn)動(dòng)軌跡

跟隨機(jī)器人從任意初始位置開(kāi)始運(yùn)動(dòng)，速度變化曲線如圖4中虛線所示。由圖可以看出，時(shí)間T從5 s左右速度開(kāi)始變化，隨后減??；從20 s左右開(kāi)始，跟隨機(jī)器人速度趨于穩(wěn)定；30 s時(shí)，跟隨機(jī)器人速度穩(wěn)定在 0.8 m/s。

圖4 λ=[0 0]T跟隨機(jī)器人仿真速度曲線

設(shè)定虛擬領(lǐng)航機(jī)器人與期望路徑距離為0.5 m，與速度方向夾角為π/6，即λ=[0.5 π/6]T。假設(shè)虛擬領(lǐng)航機(jī)器人速度為0.8 m/s，角速度為16 rad/s。即在某時(shí)刻，虛擬領(lǐng)航機(jī)器人中心線與期望軌跡連線長(zhǎng)度為0.5 m，與期望軌跡該點(diǎn)切線方向夾角為π/6。跟隨機(jī)器人運(yùn)動(dòng)軌跡如圖5所示，其中，X、Y為機(jī)器人所處平面坐標(biāo)，三角形為期望結(jié)構(gòu)，端點(diǎn)為運(yùn)動(dòng)軌跡與虛擬機(jī)器人期望位置。

圖5 λ=[0.5 π/6]T跟隨機(jī)器人仿真運(yùn)動(dòng)軌跡

跟隨機(jī)器人速度變化曲線如圖6中虛線所示。當(dāng)時(shí)間T從25 s開(kāi)始，跟隨機(jī)器人速度趨于穩(wěn)定；40 s時(shí)，跟隨機(jī)器人速度穩(wěn)定在0.8 m/s。

圖6 λ=[0.5 π/6]T跟隨機(jī)器人仿真速度曲線

3.2 實(shí)驗(yàn)驗(yàn)證

為移動(dòng)機(jī)器人控制構(gòu)建物理實(shí)驗(yàn)系統(tǒng)。該實(shí)驗(yàn)系統(tǒng)的硬件部分主要由雙輪差速驅(qū)動(dòng)底盤、IMU模塊、差分GPS定位模塊（RTK）和主計(jì)算機(jī)（Raspberry Pi4B）組成。

主計(jì)算機(jī)主要通過(guò)傳感器模塊采集到的數(shù)據(jù)進(jìn)行運(yùn)動(dòng)狀態(tài)計(jì)算，并且通過(guò)輸出控制指令完成對(duì)底盤的運(yùn)動(dòng)控制；底盤執(zhí)行主計(jì)算機(jī)的控制指令，按照控制指令完成運(yùn)動(dòng)；IMU模塊為九軸慣性測(cè)量模塊，用于測(cè)量機(jī)器人的運(yùn)動(dòng)狀態(tài)，包括加速度和航向角等；差分GPS定位模塊實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)定位功能，具體實(shí)現(xiàn)為：將差分GPS移動(dòng)接收端安裝于機(jī)器人的底盤上，通過(guò)接收衛(wèi)星與基準(zhǔn)站數(shù)據(jù)進(jìn)行差分計(jì)算，從而提高機(jī)器人的定位精度。

實(shí)驗(yàn)平臺(tái)示意圖及實(shí)物圖如圖7、圖8所示。

圖7 實(shí)驗(yàn)平臺(tái)示意圖

圖8 實(shí)驗(yàn)平臺(tái)實(shí)物圖

程序流程如圖9所示。首先對(duì)跟隨機(jī)器人進(jìn)行狀態(tài)初始化；然后根據(jù)GPS和IMU回傳數(shù)據(jù)計(jì)算，并于期望位姿做姿態(tài)解算；通過(guò)結(jié)算結(jié)果，得到控制輸入?yún)?shù)；最后輸出給控制單元，并實(shí)時(shí)對(duì)跟隨機(jī)器人狀態(tài)進(jìn)行監(jiān)測(cè)，判斷其是否達(dá)到預(yù)期位姿。

圖9 跟隨機(jī)器人控制實(shí)驗(yàn)流程圖

根據(jù)預(yù)設(shè)期望軌跡與虛擬領(lǐng)航機(jī)器人的當(dāng)前位姿誤差，計(jì)算跟隨機(jī)器人所需控制輸入?yún)?shù)，然后通過(guò)串口將運(yùn)動(dòng)控制指令發(fā)送給運(yùn)動(dòng)底盤。如此循環(huán)。

設(shè)定虛擬領(lǐng)航機(jī)器人與期望軌跡重合，即λ=[0 0]T。假設(shè)任意初始位置，虛擬領(lǐng)航機(jī)器人以速度為0.8 m/s，角速度為0勻速運(yùn)動(dòng)。跟隨機(jī)器人運(yùn)動(dòng)軌跡如圖10所示。其中，X、Y為機(jī)器人所處二維平面。

圖10 λ=[0 0]T跟隨機(jī)器人實(shí)驗(yàn)運(yùn)動(dòng)軌跡

跟隨機(jī)器人速度變化曲線如圖11中虛線所示。當(dāng)時(shí)間T從20 s開(kāi)始，跟隨機(jī)器人速度趨于穩(wěn)定；35 s時(shí)，跟隨機(jī)器人速度穩(wěn)定在0.8 m/s。

圖11 λ=[0 0]T跟隨機(jī)器人實(shí)驗(yàn)速度曲線

設(shè)定虛擬領(lǐng)航機(jī)器人與期望路徑距離為0.5 m，與速度方向夾角為π/6，即λ=[0.5 π/6]T。假設(shè)虛擬領(lǐng)航機(jī)器人速度為0.8 m/s，角速度為16 rad/s。即在某時(shí)刻，虛擬領(lǐng)航機(jī)器人中心線與期望軌跡連線長(zhǎng)度為0.5 m，與期望軌跡該點(diǎn)切線方向夾角為π/6。跟隨機(jī)器人運(yùn)動(dòng)軌跡如圖12所示，其中，X、Y為機(jī)器人所處平面坐標(biāo)，三角形為期望結(jié)構(gòu)，端點(diǎn)為運(yùn)動(dòng)軌跡與虛擬機(jī)器人期望位置。

圖12 λ=[0.5 π/6]T跟隨機(jī)器人實(shí)驗(yàn)運(yùn)動(dòng)軌跡

領(lǐng)航機(jī)器人以0.8 m/s速度運(yùn)動(dòng)時(shí)，跟隨機(jī)器人變化曲線如圖13虛線所示。當(dāng)時(shí)間趨于50 s時(shí)，跟隨機(jī)器人速度穩(wěn)定在0.8 m/s。

圖13 λ=[0.5 π/6]T跟隨機(jī)器人實(shí)驗(yàn)速度曲線

通過(guò)對(duì)比表2及前文中速度曲線可以看出，跟隨機(jī)器人速度、位姿誤差在仿真中收斂較快。并且，跟隨期望軌跡比保持固定距離收斂耗時(shí)較少。

表2 仿真、實(shí)驗(yàn)結(jié)果

在一定時(shí)間內(nèi)，跟隨機(jī)器人運(yùn)動(dòng)速度逐漸收斂至預(yù)期，V0?V1在一定時(shí)間內(nèi)趨于穩(wěn)定。同時(shí)，跟隨機(jī)器人與虛擬領(lǐng)航機(jī)器人位姿誤差也趨近于穩(wěn)定，系統(tǒng)達(dá)到穩(wěn)定狀態(tài)。

4 結(jié)束語(yǔ)

本文以輪式差分驅(qū)動(dòng)機(jī)器人為實(shí)驗(yàn)對(duì)象，其由兩個(gè)獨(dú)立驅(qū)動(dòng)的后輪控制，完成姿態(tài)變化。在此基礎(chǔ)上，通過(guò)對(duì)差分驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型分析，設(shè)計(jì)了控制器并生成相應(yīng)控制算法。利用仿真和硬件實(shí)驗(yàn)平臺(tái)，在兩種不同運(yùn)動(dòng)情況下，完成對(duì)控制算法的驗(yàn)證。仿真和實(shí)驗(yàn)結(jié)果表明：位姿誤差收斂及速度收斂均能滿足一般跟隨控制需求，驗(yàn)證了控制算法與控制器的有效性與穩(wěn)定性。

此外，本文提出的控制策略易于實(shí)現(xiàn)，只需合理設(shè)置 λ參數(shù)即可完成對(duì)機(jī)器人編隊(duì)控制狀態(tài)的調(diào)整，可為多種復(fù)雜場(chǎng)合下實(shí)際應(yīng)用提供有益借鑒。