基于方位信息的輪式機器人動態(tài)編隊控制

中圖分類號：TP242 文獻標(biāo)志碼：A 文章編號：1000-582X（2025）04-084-13

doi：10.11835/j.issn.1000-582X.2025.04.007

引用格式：，等.基于方位信息的輪式機器人動態(tài)編隊控制[J].重慶大學(xué)學(xué)報，2025，48（4）：84-96.

Dynamic formation control of wheeled robots based on azimuth information

LI Yana， HE Binbina，LI Minghuib， DAI Qingyua （a.School of Electricaland Control Engineering； b. School of Mechanicaland Electrical Engineering， Shaanxi University of Science amp; Technology， Xi'an 710021， P. R. China）

Abstract： Addressing the scenario in which each robot can only acquire the azimuth information of adjacent robots in dynamic formations with incomplete constraints，this paper proposes a distributed PID formation control algorithm based solely onazimuth information.Withconsidering thatthe pilotrobot is susceptible todisturbances such as wind direction or road surface irregularities， which may disrupt formation maintenance， the algorithm introduces relative position and velocity feedback of thefollower robots.This approach effctively eliminates steady-state error， suppresses the influence of disturbances， improves system dynamic performance， and ensures global system stability.Then，the Routh-Hurwitz stability criterion is used for stability analysis，verifying the global stability ofthe formation system.Finally， simulation experiments compare the performance of the proposed control law with control laws based on pure proportional and proportional-integral strategies in terms of convergence speed and disturbancerejection.Theresults show that the proposedcontrollaw enables theformation to recover after disturbances and achieve rapid trajectory tracking of the leader， with the relative maximum deviation of the total azimuth error reduced by 5.4% ：

Keywords： azimuth information； wheeled robots； formation control； PID control algorithm

隨著機器人被應(yīng)用于各個領(lǐng)域，多智能體協(xié)同控制引起了廣泛關(guān)注。多智能體的編隊控制是協(xié)同控制領(lǐng)域的重要研究課題之一。根據(jù)感知和控制變量的類型，現(xiàn)有的編隊控制方法大致分為3類[：基于相對位置基于距離[和基于方位[12-17。與其他2種編隊方法相比，基于方位的編隊方法因具有縮放便利、對智能體的感知能力要求更低等優(yōu)點被廣泛應(yīng)用。文獻[18]基于相對方位信息提出一種有限時間的圓形編隊分布式控制律，有效減少了方位角誤差。文獻[19]提出一種基于局部方位測量和相對角度約束的分布式三角形編隊控制算法，編隊控制效果良好，但只適用于3個智能體的情況。為了進一步擴展智能體個數(shù)，Bishop又在文獻[20]中，提出一種具有純方位約束的四邊形編隊控制的分布式控制律，將智能體的個數(shù)擴展到4個及以上，提高編隊控制的穩(wěn)定性。

上述文獻研究的控制律僅適用于解決靜止編隊場景，當(dāng)應(yīng)用于跟蹤運動目標(biāo)編隊時，這些控制律將無法消除動態(tài)編隊中產(chǎn)生的較大跟蹤誤差。因此，如何使用方位信息實現(xiàn)對運動目標(biāo)跟蹤的動態(tài)編隊控制是當(dāng)前研究的難點。目前的動態(tài)編隊控制根據(jù)智能體的動力學(xué)模型可分為單積分器、雙積分器、非完整動力學(xué)模型。輪式機器人就屬于其中的非完整動力學(xué)模型。文獻[21]將基于方位的控制方法應(yīng)用于非完整輪式機器人模型，提出純方位編隊控制律，解決該模型下編隊控制的隊形縮放問題。文獻[22]提出動態(tài)方位編隊跟蹤控制律，利用局部參考坐標(biāo)系中的方位信息實現(xiàn)期望的運動編隊，解決局部坐標(biāo)系下非完整動力學(xué)模型的引導(dǎo)跟蹤、編隊跟蹤問題。文獻[23]提出新的自適應(yīng)編隊控制律，考慮在領(lǐng)航者速度恒定情況下機器人之間的避碰問題，實現(xiàn)6個非完整約束機器人系統(tǒng)的純方位編隊跟蹤控制。以上文獻都默認在理想環(huán)境下，只考慮領(lǐng)航者速度不變的情況。但實際應(yīng)用中，領(lǐng)航機器人速度必然會因受到不同類型的干擾，發(fā)生不規(guī)律改變，無法保持編隊隊形。因此，為了使跟隨機器人能快速響應(yīng)并形成指定目標(biāo)隊形，必須考慮提高動態(tài)編隊控制算法的抗干擾能力，保證領(lǐng)航機器人受到擾動速度不規(guī)律變化時，編隊能穩(wěn)定形成和縮放。

為解決以上問題，文獻[24]利用無跡卡爾曼濾波算法對領(lǐng)航機器人系統(tǒng)的狀態(tài)進行估計，設(shè)計了輸入-輸出狀態(tài)反饋控制規(guī)律，提高對非線性系統(tǒng)和不確定的適應(yīng)性。但該方法需要對純角度觀測信息進行無跡卡爾曼濾波，增加了計算復(fù)雜度和延遲。文獻[25]考慮領(lǐng)航者速度時變的情況，在控制律中引入積分項實現(xiàn)對領(lǐng)航者速度變化的補償，從而實現(xiàn)期望編隊，該方法的系統(tǒng)穩(wěn)定性和抗干擾能力有待改進。因此，針對多移動機器人動態(tài)編隊中領(lǐng)航者速度受擾動隨機改變的情況，提出了一種基于方位的抗擾能力強的PID（proportion integration derivative）編隊控制算法。該算法利用方位信息作為控制輸入，結(jié)合PID控制器的比例、積分和微分作用，有效抑制領(lǐng)航者速度擾動對編隊系統(tǒng)的影響，提高了編隊系統(tǒng)的穩(wěn)定性和魯棒性。

1準(zhǔn)備工作

1.1 移動機器人模型

1.2 圖論相關(guān)知識

1.4基于方位編隊的唯一性

與傳統(tǒng)基于位置的編隊控制方法不同，基于方位的編隊控制方法由于缺少距離信息，在同樣的領(lǐng)導(dǎo)者位置與方位約束條件下，有不同的編隊隊形，如圖4中（a）（b）所示。當(dāng)只采用方位信息對期望隊形進行描述時，對于同樣的期望隊形，不同的方位約束條件的選擇會對隊形的唯一性產(chǎn)生不同影響。因此，為了保證編隊的唯一性，需要確保所構(gòu)建的目標(biāo)隊形框架為無窮小方位剛性的。方位剛性矩陣被定義為方位函數(shù)的雅可比矩陣。為滿足所構(gòu)建目標(biāo)隊形的框架為無窮小方位剛性的，通過計算方位剛性矩陣的秩來檢測，求方位剛性矩陣的秩首先要對方位函數(shù)求偏導(dǎo)，再求出雅可比矩陣的秩。因此，對于具有多節(jié)點的多智能體編隊，工作量較大也不夠直觀。

研究采用以下判斷條件：只要一個隊形框架的邊數(shù) m 和頂點數(shù) n 滿足 m？2n-3 關(guān)系，該框架是無窮小距離剛性的，由于在二維空間中，無窮小距離剛性圖等價于無窮小方位剛性圖。因此，借助以上判斷條件，可以簡潔直觀地構(gòu)建出唯一的目標(biāo)隊形。如圖4所示，圖4中（a）（b）不滿足無窮小方位剛性條件，故幾何形狀不唯一；圖4中（c）（d）所示的框架，滿足無窮小方位剛性條件，因此，幾何形狀唯一，只是平移和縮放的條件不同。

2基于方位信息的移動機器人編隊控制算法

2.1 控制目標(biāo)

研究將多智能體中雙積分器模型的基于方位編隊控制方法應(yīng)用于非完整約束多機器人模型編隊控制中。對滿足方位約束 的編隊 ，根據(jù)相對位置測量值 設(shè)計跟隨機器人的控制律 ，使實

際的方位收斂到期望值，多機器人形成目標(biāo)隊形且以期望速度整體移動，完成多機器人系統(tǒng)編隊的形成、保持和跟蹤。

2.2 編隊控制算法設(shè)計

2.3 收斂性分析

3仿真

為了驗證所提控制律的有效性，使用MATLAB軟件對多機器人動態(tài)編隊控制問題進行模擬仿真研究。研究采用4個輪式移動機器人所組成的系統(tǒng)，所有輪式移動機器人的動力學(xué)均為相同的非完整約束動力模型。

3.1 無干擾的自標(biāo)編隊控制

首先選取期望隊形，根據(jù)編隊唯一性設(shè)置合適的方位約束與領(lǐng)航者。本次仿真實驗分別采用長方形和菱形的期望隊形進行驗證，編隊期望隊形的平面示意圖如圖6所示。其中，機器人 是第1領(lǐng)航者，機器人 是第2領(lǐng)航者，機器人 和 均為編隊中的跟隨者。根據(jù)編隊的信息拓撲結(jié)構(gòu)：領(lǐng)航機器人 和 之間是單向信息交互，跟隨機器人 和 與領(lǐng)航機器人 和 也是單向信息交互。而跟隨機器人 和 之間的信息交互是雙向。

1）長方形編隊

設(shè)定各移動機器人的初始位置為

本次仿真主要采用基于Ziegler-Nlichols方法進行PID的參數(shù)整定。Ziegler-Nichols方法是一種經(jīng)驗性的PID控制器整定方法，相較于傳統(tǒng)的辨識法整定方法，它的優(yōu)勢是不需要知道系統(tǒng)的數(shù)學(xué)模型，只需要觀察系統(tǒng)的輸出變化。并且可以適用于比例、比例-積分、比例-積分-微分等多種類型的控制器。

在此次PID整定中，首先只調(diào)節(jié) ，從一個較小的值開始逐漸增大，直到 時，系統(tǒng)響應(yīng)速度和穩(wěn)態(tài)誤差達到滿意的程度。然后固定 ，分別依次調(diào)節(jié) ，直至系統(tǒng)到達最佳狀態(tài)。經(jīng)過多次調(diào)整PID控制器參數(shù)，最后得到無干擾編隊情況下Z-N方法整定后的PID參數(shù)值為： 。

如圖7和圖8所示，初始位置不滿足期望隊形。實驗首先令各機器人到達初始期望位置點后，跟隨機器人在所提出的控制律式（6）中，利用方位信息完成編隊的形成與跟蹤任務(wù)，對2個領(lǐng)航機器人則采用方位信息進行跟蹤控制。此外，在10s加人放縮信號，驗證整個編隊通過操縱2個領(lǐng)導(dǎo)者實現(xiàn)隊伍平移和縮放，同時保持所需的編隊隊形，這表明所提出的動態(tài)編隊控制解決方案可用于避免碰撞或通過狹窄通道。

不同的初始位置會影響編隊控制的難度和效果。因此，為了保證編隊控制算法的可靠性和安全性，筆者選擇了3種不同的初始位置，考慮領(lǐng)航者軌跡不同的情況。如圖9和圖10所示，盡管3次初始位置不同，但并不影響不同編隊隊形的形成，且形成過程中無碰撞情況，仿真證明了編隊算法可靠性和安全性。但是當(dāng)跟隨者初始位置與期望編隊位置相距過遠時，形成編隊的時間會更長。

3）不同編隊控制方法的對比實驗

為進一步驗證所提出編隊控制方法的可靠性，在形成長方形編隊過程中，設(shè)計如下2組對比仿真實驗：

A1：領(lǐng)航者速度不變情況下，采用PI控制的方位角誤差；

A2：領(lǐng)航者速度不變情況下，采用PD控制的方位角誤差；

A3：領(lǐng)航者速度不變情況下，采用PID控制的方位角誤差；

B1：領(lǐng)航者速度隨時間變化情況下，采用PI控制的方位角誤差；

B2：領(lǐng)航者速度隨時間變化情況下，采用PD控制的方位角誤差；

B3：領(lǐng)航者速度隨時間變化情況下，采用PID控制的方位角誤差。

圖11和圖12中在領(lǐng)航者速度保持不變和隨時間變化2種情況下，對比了PI控制算法、PD控制算法和PID控制算法的編隊總方位誤差

從圖11中看出，在領(lǐng)航者速度不變時，加入微分環(huán)節(jié)，尤其是A組第3s時PI控制算法的相對方位角誤差為 35% ，PID控制算法的方位角誤差為 30% ，下降 5% 。從圖12中看出，在領(lǐng)航者速度隨時間變化時，較于PD算法，PID控制的響應(yīng)曲線沒有明顯的尖峰和波動，更平滑。同時控制更快地收斂，沒有明顯的延遲和震蕩。而在圖7和圖9軌跡圖中，紫色線為PID算法，黃色線為PI算法的。由圖也可看出，采用PID的控制方法比以往只用PI控制器的跟隨者軌跡更平滑。

3.2 加入干擾的編隊控制

在實際情況中，領(lǐng)航者易受其他動態(tài)障礙物、風(fēng)向或路面平整度等擾動。因此，在領(lǐng)航者速度規(guī)律變化情況下，10～15 s加人干擾，設(shè)計如下2組對比仿真實驗：

C1：加人隨機干擾，采用PI控制的方位角誤差；C2：加入隨機干擾，采用PID控制的方位角誤差；D1：加入干擾 的情況下，采用PI控制的方位角誤差D2：加入干擾 cost的情況下，采用PID控制的方位角誤差。

采用長方形編隊跟蹤進行驗證，圖13展示了領(lǐng)航機器人速度受到隨機干擾時的隊形保持與變換，圖14～15展示了C，D2組實驗在編隊過程中總方位角誤差變化情況。對圖14與圖15中的數(shù)據(jù)的穩(wěn)態(tài)過程進行定量對比分析，選取第 10～40s 的數(shù)據(jù)，分別對圖中各量求取最大偏差和均方根誤差，計算結(jié)果如表1所示。對比圖14與圖15，發(fā)現(xiàn)在受到未知擾動，系統(tǒng)產(chǎn)生振蕩時，設(shè)計的非線性控制律可以在更短的時間內(nèi)用更合理的控制量進行校正。對比表1，可以發(fā)現(xiàn)2種環(huán)境下，采用PID控制律的方位角偏差都有所下降，加入隨機干擾情況下最為明顯，相對最大偏差降低了 5.4% 。這說明在所設(shè)計控制律的作用下，多機器人系統(tǒng)形成的編隊更趨近于期望編隊，且波動更小，較為穩(wěn)定。

選取第10～40s的數(shù)據(jù)，分別對圖中各量求取最大偏差和均方根誤差

表1定量對比表

4結(jié)束語

筆者主要研究了多機器人的動態(tài)編隊控制，提出一種適用于二維非完整多移動機器人系統(tǒng)的分布式純方位PID編隊控制律，通過Routh-Hurwitz穩(wěn)定性判據(jù)證明了所提方法的穩(wěn)定性。仿真和實驗結(jié)果驗證了所設(shè)計方法的有效性。基于方位的方法因其易于實現(xiàn)隊形縮放在機器人避障中發(fā)揮著重要作用，在未來的研究中，計劃進一步深人探討多移動機器人的避障問題。

參考文獻

[1］劉銀萍，楊宜民.多機器人編隊控制的研究綜述[J].控制工程，2010，17（S3）：182-186. LiuYP，YangYM.Frmationcontrolofmulti-robotresearch[]ControlEngineeringofChina17（S3）8-86（in Chinese）

[2］王帥，周樂來李貽斌，等.多移動機器人編隊領(lǐng)航跟隨方法研究進展[J].無人系統(tǒng)技術(shù)，2019，2（5）：1-8. Wang S，ZhouLL，LiYB，etal.Advances intheleader-folowingmethodofmulti-mobilerobotsformation[]Unmanned Systems Technology， 2019， 2（5）： 1-8. （in Chinese）

[3]Yang ZQPanXFZhangQetalFormationontroloffirstdermulti-agents withregionconstraint[]Automatika，20， 61（4）： 651-656.

[4]Oh K K，Park MC，Ahn H S.A survey of multi-agent formation control[J].Automatica，2015，53：424-440.

[5]ZhaoSYZelazoDBearingrigiditytheoryanditsapplicationsforcontrolandestimationofnetworksystems：life beyond distance rigidity[J]. IEEE Control Systems Magazine， 2019， 39（2）： 66-83.

[6］郭鵬軍，張睿，高關(guān)根，等.基于相對速度和位置輔助的無人機編隊協(xié)同導(dǎo)航[J].上海交通大學(xué)學(xué)報，2022，56（11）：1438-1446. Guo P J， Zhang R， Gao GG，et al.Cooperative navigationof UAV formation based on relative velocity and position assistance[J].Journal of Shanghai Jiaotong University， 2022，56（11）： 1438-1446.（in Chinese）

[7］鄭重，劉帥，錢默抒，等.航天器編隊系統(tǒng)相對位置自適應(yīng)分布式控制[J].中國慣性技術(shù)學(xué)報，2019，27（1）：129-135. Zheng Z，LiuQianMStaldaptiveistibutedtrolforelativepositiofspacecraffoatissem[]uof Chinese Inertial Technology， 2019， 27（1）： 129-135.（in Chinese）

[8]OgawaTSakuramaK，akataniSetalRelativepositionestimatinforformatncntrolwiththefusioofprdicted futue informationndeasuementdata[ICEJualofCtolMeasurementandSystemItegrat.

[9］劉樹光，王歡，劉榮華.基于距離的無人機編隊路徑跟蹤控制[J].飛行力學(xué)，2023，41（1）：27-33. Liu SG，WangH，LiuRH.Distance-basedcontrolforUAVsformation pathtracking[]FlightDynamics，23，41（1）：27-33. （in Chinese）

[10]Chen Z，JiangC，GuoYDistancebased formationcontrolofathree-robot system[C]/19ChineseControland Decision Conference （CCDC）.Nanchang，China：IEEE，2019： 5501-5507.

[11]孟蕾.基于距離約束的單領(lǐng)航者多智能體編隊控制[J].電光與控制，2021，28（7）：48. Meng L.Multi-agentleader-follower formationcontrolbasedondistanceconstraints[]ElectronicsOpticsamp;Control，2021， 28（7）： 48.（in Chinese）

[12］葉結(jié)松，龔柏春，李爽，等.基于相對方位信息和單間距測量的多智能體編隊協(xié)同控制[J].航空學(xué)報，2021，42（7）：324610. YeJS，Gong BC，LiS，etalMulti-agentformationcooperativecontrolusingrelativebearing informationand single-spacing measurement[J].Acta Aeronautica et Astronautica Sinica， 2021，42（7）： 324610.（in Chinese）

[13］李憲珞.基于方位信息的非完整性多智能體編隊控制[D].秦皇島：燕山大學(xué)，2020. Li X L.Formation control of nonholonomic multi-agent systems with bearing-only information[D].Qinhuangdao：Yanshan University， 2020. （in Chinese）

[14]HanZMGuoKXXieLHetaltegedelatocazndadfolorfatol[EETac Automatic Control， 2019， 64（1）： 20-34.

[15]ChYHKimDistanebasedfomattolithgoaassgntforglobaasyptotictabilityfmultitsem[] IEEE Robotics and Automation Letters， 2021， 6（2）： 2020-2027.

[16]Dong WJFarrellJA.Coperativeontrolof multiplenonholonomicmobileagents[J]EEETransactionsonAutomatic Control， 2008， 53（6）： 1434-1448.

[17]李剛，周尚波，郭尚志等.通信延遲下自適應(yīng)容錯控制及其在航天器編隊中的應(yīng)用[J].重慶大學(xué)學(xué)報，2024，47（4）：104-113. LiG，ZhouSBGuoSZetal.Adaptivefault-tolerantcontrolwithcommunicationdelaysanditsapplicationinspacecraft formation[J].Journal of Chongqing University， 2024， 47（4）： 104-113. （in Chinese）

[18]ZhaoSYLinFengKtal.stutedtrolofangle-nstradcclicfmatusingagnlyeasts[] Systems amp; Control Letters， 2014， 63： 12-24.

[19]BasiriMBishoANJensfelt PDistributedcontroloftriangularformationswithangle-nlyonstraints[]ystemsamp; Control Letters， 2010，59（2）： 147-154.

[20]BishoANDistrbutedbearingnlyquadrilateralformationntrol[ACrocedingsolumes4（1-1.

[21]ZhaoSY，LiZHDingZTearingnlyformatntrackingctrolofutagetsstems[]EEETansactisoAutic Control， 2019， 64（11）： 4541-4554.

[22]ZhaoJNuX，LiXWetalBearignlyfomatiotrackingtrolofmultiagentsstemswithlocalrferencefamsand constant-velocity leaders[J]. IEEE Control Systems Letters， 2021， 5（1）： 1-6.

[23]VanTranQ，KimJBearing-constrainedformationtrackingcontrolofnonholonomicagents withoutinter-agentcommunication [J].IEEE Control Systems Letters， 2022， 6： 2401-2406.

[24]韓青.基于純角度觀測信息的多機器人編隊控制方法研究[D].西安：西北工業(yè)大學(xué)，2018. Han Q.Bearing-onlyobservations multi-robotformationcontrol[D].Xi'an：Northwestern PolytechnicalUniversity18.（in Chinese）

[25]LiXL，LuoXYWang JG，etal.Distributedbearing-basedformationcontrolofnetworkedthrust-propelledehicles[]. Journal of the Franklin Institute， 2019， 356（9）： 4907-4927.

[26］譚瑤，梅杰.利用方位角信息的移動機器人編隊控制[J].控制理論與應(yīng)用，2021，38（7）：1043-1050. TanY，MeiJFormationcntrolofmobileobotsusingbearingnlymeasurements[]ControlTheoryamp;Applications021， 38（7）：1043-1050. （in Chinese）

（編輯侯湘）