基于方位信息的輪式機器人動態編隊控制

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

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

引用格式：，等.基于方位信息的輪式機器人動態編隊控制[J].重慶大學學報，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

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

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

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

1準備工作

1.1 移動機器人模型

1.2 圖論相關知識

1.4基于方位編隊的唯一性

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

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

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

2.1 控制目標

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

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

2.2 編隊控制算法設計

2.3 收斂性分析

3仿真

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

3.1 無干擾的自標編隊控制

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

1）長方形編隊

設定各移動機器人的初始位置為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 加入干擾的編隊控制

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

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

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

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

表1定量對比表

4結束語

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

參考文獻

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

[2］王帥，周樂來李貽斌，等.多移動機器人編隊領航跟隨方法研究進展[J].無人系統技術，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］郭鵬軍，張睿，高關根，等.基于相對速度和位置輔助的無人機編隊協同導航[J].上海交通大學學報，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］鄭重，劉帥，錢默抒，等.航天器編隊系統相對位置自適應分布式控制[J].中國慣性技術學報，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].飛行力學，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]孟蕾.基于距離約束的單領航者多智能體編隊控制[J].電光與控制，2021，28（7）：48. Meng L.Multi-agentleader-follower formationcontrolbasedondistanceconstraints[]ElectronicsOpticsamp;Control，2021， 28（7）： 48.（in Chinese）

[12］葉結松，龔柏春，李爽，等.基于相對方位信息和單間距測量的多智能體編隊協同控制[J].航空學報，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].秦皇島：燕山大學，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]李剛，周尚波，郭尚志等.通信延遲下自適應容錯控制及其在航天器編隊中的應用[J].重慶大學學報，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].西安：西北工業大學，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].控制理論與應用，2021，38（7）：1043-1050. TanY，MeiJFormationcntrolofmobileobotsusingbearingnlymeasurements[]ControlTheoryamp;Applications021， 38（7）：1043-1050. （in Chinese）

（編輯侯湘）