反步軌跡跟蹤控制的移動機器人隊形成形控制

張文霞，袁 健

（1.青島理工大學琴島學院 機電工程系，青島 266100；2.國家海洋監測設備工程技術研究中心 山東省海洋環境監測技術重點實驗室 山東省科學院海洋儀器儀表研究所，青島 266001）

多個移動機器人的隊形成形控制是多機器人協調合作中的一個典型問題，比如自動化工廠中的多移動陸路機器人（AGV）協同搬運。多個移動機器人的隊形成形控制可以完成單個機器人無法完成的作業任務，顯著提高任務的完成效率。虛擬結構法和跟隨領航者方法是當前主要采用的研究方法。文獻[1-3]采用了分布式的虛擬結構編隊框架實現了機器人的隊形成形控制。文獻[4]研究了通訊受限情況下陸地機器人的隊形成形控制問題，文獻[5]研究了非完整機器人的隊形成形控制問題，文獻[6]等研究了非完整機器人協作控制，設計了分布式控制器和估計器。但虛擬結構法要求機器人以剛體上的相應點的位置和姿態信息作為各自的跟蹤目標來形成一定的隊形，機器人之間存在較強的耦合關系，無法實現靈活的隊形變換[1]。而跟隨領航者方法要求控制1臺機器人以一定的角度和距離跟蹤1個或者幾個機器人，能夠實現編隊變形或隊形變換，對障礙具有好的適應性，但要求機器人間的實時通信，對角度和距離測量傳感器的性能要求很高，隨著距離增加通信量急劇增大。

軌跡跟蹤控制方法不同于路徑跟蹤控制方法，其既有嚴格時間上的跟蹤要求，又有軌跡跟蹤誤差上的要求[5]，可以確保在每一個規定的時刻每個機器人運動到預定的跟蹤軌跡點，從而整個機器人能夠形成預定的軌跡形狀。本文基于backstepping軌跡跟蹤控制方法提出一種多陸路移動機器人成形控制方法。首先根據多移動機器人不同的搬運任務設置不同的運動軌跡，并將設定的多個移動機器人的不同運動軌跡預編程到每個機器人控制器中，然后對每個機器人設計控制律實現對預定義軌跡的跟蹤控制，這樣不但簡化了機器人之間的通信聯系，并且可以保證每個機器人在預定的時刻跟蹤上期望軌跡點，從而實現期望的控制隊形。通過3次不同的運動軌跡，仿真驗證了所設計控制方法的有效性。

1 移動機器人運動學模型建模

將多個移動陸路機器人協同搬運問題轉化為多移動機器人的軌跡跟蹤控制問題。要實現多個移動機器人的協同搬運，首先要對每個機器人的期望運動軌跡和期望運動速度進行設置，作為軌跡跟蹤控制的期望軌跡和期望速度。考慮到軌跡跟蹤控制對跟蹤時刻和跟蹤精度的要求，通過對每個機器人分別設計控制律實現對預定義軌跡的跟蹤即可實現期望運動隊形，如圖1所示，圖中實線框表示機器人的實際位置和駛向，虛線框表示機器人1的期望位置和駛向。

首先建立2個坐標系，使用柵格定位方法，分別為柵格坐標系和機器人體坐標系。移動機器人通過2個輪子驅動電機的不同轉速來控制機器人的行進速度和轉向，機器人的運動狀態由其兩驅動輪的中點在柵格平面坐標系下的位置及駛向角（即前進方向和x軸正方向的夾角）來表示。

圖1 移動機器人的期望/實際位姿及誤差表示方法Fig.1 Desired/true position and attitude of the mobile robots

為表示移動機器人在坐標下的位置和駛向，令p=（x，y，θ）T，q=（ν，ω）T，其中 x，y 為機器人在柵格坐標系下的位置坐標，θ為機器人的駛向角。ν，ω為控制量，分別為機器人的行駛直線速度和轉向運動角速度，則移動機器人的運動學方程可以表示為

參照圖1的系統期望/實際位姿及誤差表示方法，進一步通過坐標系轉換，機器人的運動位姿誤差可以表示為

為使機器人跟蹤上其預定義軌跡坐標，即使得對任意的初始位置和姿態誤差（xe（0），ye（0），θe（0）），使實際移動軌跡坐標（x，y，θ）與期望軌跡坐標（xd，yd，θd）之差為零。 移動機器人的軌跡跟蹤問題即通過控制機器人的直線運動速度和旋轉運動速度q=（ν，ω）T，使系統誤差方程（3）趨向于零，即‖（xe，ye，θe）T‖=0，并且直線運動速度和旋轉運動速度ν，ω有大小限制，其中直線運動速度表示為ν=v的函數，旋轉運動速度表示為的函數。

2 移動機器人反步軌跡跟蹤控制器設計

設移動機器人直線運動速度ν和旋轉運動速度ω可以連續變化，并且受驅動電機功率的限制有大小約束，則可以通過直線運動速度ν和旋轉運動速度ω的控制律設計使得位姿系統方程式（3）對于任意的機器人初始位姿誤差（xe（0），ye（0），θe（0）），隨著時間的推移xe，ye和θe逐漸趨向于零，即‖（xe，ye，θe）T‖=0，這樣就實現了移動機器人在預定時刻上的軌跡跟蹤控制，其具體設計步驟如下：

首先令 xe=c3ωye，θe=0，則可使系統式（3）中的 ye系統趨向于零，于是可以引入一個新變量ωye，其中 c1∈R+。 對變量求導，可得：

考慮到位姿系統方程式（3）的系統特性，基于反步控制律設計方法，設計的直線運動速度ν和旋轉運動速度ω的控制律為

式中：cx和cθ為移動機器人控制器的軌跡跟蹤收斂速度調節參數，決定著位姿誤差收斂到零的速度，cx和cθ越大則機器人跟蹤上期望軌跡坐標點的速度越快。使用該控制律可以使機器人跟蹤上期望軌跡坐標點，實現對預定義軌跡點在預定時刻的軌跡跟蹤。

3 數值仿真

要實現多個陸路移動機器人協同搬運，根據其協作操作任務性質，選擇橫向隊形（即3個機器人平行前進）作為預期隊形，期望跟蹤軌跡為直線，選定工作場地內某一點為坐標原點，實現了3個移動機器人的直線軌跡跟蹤控制。期望跟蹤軌跡與x軸正方向的夾角分別為π/4 rad，0 rad，-π/4 rad，進行了3次隊形成形仿真，3個機器人Robot1、Robot2、Robot3 的初始位置狀態分別為3次仿真中機器人的位置初值保持不變，Robot1預先指定的跟蹤軌跡的位置初值為Robot2預先指定的跟蹤軌跡的位置初值為預先指定的跟蹤軌跡的位置初值為三者間保持0.2 m的橫向距離。為防止機器人打滑，將其期望角速度設置為 0，選取 ωd=0，νd=0.2 m/s，設置控制器參數為 cx=3，cθ=10，采樣時間 τ=8 ms。 針對 3次不同的期望軌跡，分別進行仿真實驗，期望跟蹤軌跡與 x軸正方向的夾角分別為 π/4 rad，0 rad，-π/4 rad，如圖2所示。圖2中分別標注了3次仿真中3個機器人在200 s，500 s，700 s時的位置，其中實線表示機器人的期望運動軌跡，虛線表示機器人的真實運動軌跡，仿真表明在500 s時移動機器人基本跟蹤上了期望運動軌跡并且姿態誤差也收斂到0；圖3為跟蹤軌跡與x軸正方向的夾角為0 rad時的Robot3的位置和姿態誤差，在700 s時位姿誤差收斂到0，可見700 s時完全跟蹤上了期望運動軌跡和姿態從而實現了比較精確的橫向編隊；圖4為跟蹤軌跡與x軸正方向的夾角為0 rad時的Robot3控制量，可以保證3個機器人以同樣的運動速度和運動姿態并行前進。

圖2 軌跡跟蹤控制3次運動仿真Fig.2 Three simulations for the trajectory-tracking control

圖3 跟蹤軌跡與x軸正方向的夾角為0 rad時的Robot3的位姿誤差Fig.3 Position and attitude error of the third robot when the angle is 0 rad between the desired trajectory and the x-axis

圖4 跟蹤軌跡與x軸正方向的夾角為0 rad時的 Robot3控制量（ν，ω）Fig.4 Control（ν，ω）of the third robot when the angle is 0 rad between the desired trajectory and the x-axis

4 結語

研究了多個陸路移動機器人隊形成形控制問題。為實現機器人的在時間上的精確隊形控制，設計了基于反步控制的機器人的軌跡跟蹤控制方法，通過選取李雅譜諾夫函數設計了機器人的軌跡跟蹤控制律，實現了多個機器人對預期軌跡的跟蹤，從而形成了期望的隊形。通過3個不同軌跡的仿真實驗，表明所提控制方法能夠形成預期隊形。注意到由于控制律是漸近收斂到期望軌跡，導致過渡時間較長，研究能在有限時間內實現軌跡跟蹤的控制方法是以后工作的重點。

[1]Wang P K C.Navigation strategies for multiple autonomous mobile robots moving in formation[J].Journal of Robotic Systems，1991，8（2）：177-195.

[2]Lawton J，Beard R W，Young B J.A decentralized approach to formation maneuvers[J].IEEE Trans on Robotics and Automation，2003，19（6）：933-941.

[3]Leonard N E，Fiorelli E.Virtual leader，artificial potentials and coordinated control of groups[C]//IEEE Conference on Decision and Control，2001.

[4]Ren W，Beard R W.A decentralized scheme for spacecraft formation flying via the virtual structure approach[J].AIAA Journal of Guidance，Control and Dynamics，2004，27（1）：73-82.

[5]Jiang Z P，Pomet J B.Backstepping-based adaptive controller for uncertain nonholonomic systems[C]//In Proceeding of 34th IEEE Conference on Decision and Control，New Orleans，LA，1995.

[6]Do K D.Formation tracking control of unicycle-type mobile robots with limited sensing ranges[J].IEEE Transactions on Control Systems Technology，2008，16（3）：527-538.