反步軌跡跟蹤控制的移動(dòng)機(jī)器人隊(duì)形成形控制

張文霞，袁 健

（1.青島理工大學(xué)琴島學(xué)院 機(jī)電工程系，青島 266100；2.國(guó)家海洋監(jiān)測(cè)設(shè)備工程技術(shù)研究中心 山東省海洋環(huán)境監(jiān)測(cè)技術(shù)重點(diǎn)實(shí)驗(yàn)室 山東省科學(xué)院海洋儀器儀表研究所，青島 266001）

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

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

1 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型建模

將多個(gè)移動(dòng)陸路機(jī)器人協(xié)同搬運(yùn)問(wèn)題轉(zhuǎn)化為多移動(dòng)機(jī)器人的軌跡跟蹤控制問(wèn)題。要實(shí)現(xiàn)多個(gè)移動(dòng)機(jī)器人的協(xié)同搬運(yùn)，首先要對(duì)每個(gè)機(jī)器人的期望運(yùn)動(dòng)軌跡和期望運(yùn)動(dòng)速度進(jìn)行設(shè)置，作為軌跡跟蹤控制的期望軌跡和期望速度?？紤]到軌跡跟蹤控制對(duì)跟蹤時(shí)刻和跟蹤精度的要求，通過(guò)對(duì)每個(gè)機(jī)器人分別設(shè)計(jì)控制律實(shí)現(xiàn)對(duì)預(yù)定義軌跡的跟蹤即可實(shí)現(xiàn)期望運(yùn)動(dòng)隊(duì)形，如圖1所示，圖中實(shí)線框表示機(jī)器人的實(shí)際位置和駛向，虛線框表示機(jī)器人1的期望位置和駛向。

首先建立2個(gè)坐標(biāo)系，使用柵格定位方法，分別為柵格坐標(biāo)系和機(jī)器人體坐標(biāo)系。移動(dòng)機(jī)器人通過(guò)2個(gè)輪子驅(qū)動(dòng)電機(jī)的不同轉(zhuǎn)速來(lái)控制機(jī)器人的行進(jìn)速度和轉(zhuǎn)向，機(jī)器人的運(yùn)動(dòng)狀態(tài)由其兩驅(qū)動(dòng)輪的中點(diǎn)在柵格平面坐標(biāo)系下的位置及駛向角（即前進(jìn)方向和x軸正方向的夾角）來(lái)表示。

圖1 移動(dòng)機(jī)器人的期望/實(shí)際位姿及誤差表示方法Fig.1 Desired/true position and attitude of the mobile robots

為表示移動(dòng)機(jī)器人在坐標(biāo)下的位置和駛向，令p=（x，y，θ）T，q=（ν，ω）T，其中 x，y 為機(jī)器人在柵格坐標(biāo)系下的位置坐標(biāo)，θ為機(jī)器人的駛向角。ν，ω為控制量，分別為機(jī)器人的行駛直線速度和轉(zhuǎn)向運(yùn)動(dòng)角速度，則移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)方程可以表示為

參照?qǐng)D1的系統(tǒng)期望/實(shí)際位姿及誤差表示方法，進(jìn)一步通過(guò)坐標(biāo)系轉(zhuǎn)換，機(jī)器人的運(yùn)動(dòng)位姿誤差可以表示為

為使機(jī)器人跟蹤上其預(yù)定義軌跡坐標(biāo)，即使得對(duì)任意的初始位置和姿態(tài)誤差（xe（0），ye（0），θe（0）），使實(shí)際移動(dòng)軌跡坐標(biāo)（x，y，θ）與期望軌跡坐標(biāo)（xd，yd，θd）之差為零。 移動(dòng)機(jī)器人的軌跡跟蹤問(wèn)題即通過(guò)控制機(jī)器人的直線運(yùn)動(dòng)速度和旋轉(zhuǎn)運(yùn)動(dòng)速度q=（ν，ω）T，使系統(tǒng)誤差方程（3）趨向于零，即‖（xe，ye，θe）T‖=0，并且直線運(yùn)動(dòng)速度和旋轉(zhuǎn)運(yùn)動(dòng)速度ν，ω有大小限制，其中直線運(yùn)動(dòng)速度表示為ν=v的函數(shù)，旋轉(zhuǎn)運(yùn)動(dòng)速度表示為的函數(shù)。

2 移動(dòng)機(jī)器人反步軌跡跟蹤控制器設(shè)計(jì)

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

首先令 xe=c3ωye，θe=0，則可使系統(tǒng)式（3）中的 ye系統(tǒng)趨向于零，于是可以引入一個(gè)新變量ωye，其中 c1∈R+。 對(duì)變量求導(dǎo)，可得：

考慮到位姿系統(tǒng)方程式（3）的系統(tǒng)特性，基于反步控制律設(shè)計(jì)方法，設(shè)計(jì)的直線運(yùn)動(dòng)速度ν和旋轉(zhuǎn)運(yùn)動(dòng)速度ω的控制律為

式中：cx和cθ為移動(dòng)機(jī)器人控制器的軌跡跟蹤收斂速度調(diào)節(jié)參數(shù)，決定著位姿誤差收斂到零的速度，cx和cθ越大則機(jī)器人跟蹤上期望軌跡坐標(biāo)點(diǎn)的速度越快。使用該控制律可以使機(jī)器人跟蹤上期望軌跡坐標(biāo)點(diǎn)，實(shí)現(xiàn)對(duì)預(yù)定義軌跡點(diǎn)在預(yù)定時(shí)刻的軌跡跟蹤。

3 數(shù)值仿真

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

圖2 軌跡跟蹤控制3次運(yùn)動(dòng)仿真Fig.2 Three simulations for the trajectory-tracking control

圖3 跟蹤軌跡與x軸正方向的夾角為0 rad時(shí)的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時(shí)的 Robot3控制量（ν，ω）Fig.4 Control（ν，ω）of the third robot when the angle is 0 rad between the desired trajectory and the x-axis

4 結(jié)語(yǔ)

研究了多個(gè)陸路移動(dòng)機(jī)器人隊(duì)形成形控制問(wèn)題。為實(shí)現(xiàn)機(jī)器人的在時(shí)間上的精確隊(duì)形控制，設(shè)計(jì)了基于反步控制的機(jī)器人的軌跡跟蹤控制方法，通過(guò)選取李雅譜諾夫函數(shù)設(shè)計(jì)了機(jī)器人的軌跡跟蹤控制律，實(shí)現(xiàn)了多個(gè)機(jī)器人對(duì)預(yù)期軌跡的跟蹤，從而形成了期望的隊(duì)形。通過(guò)3個(gè)不同軌跡的仿真實(shí)驗(yàn)，表明所提控制方法能夠形成預(yù)期隊(duì)形。注意到由于控制律是漸近收斂到期望軌跡，導(dǎo)致過(guò)渡時(shí)間較長(zhǎng)，研究能在有限時(shí)間內(nèi)實(shí)現(xiàn)軌跡跟蹤的控制方法是以后工作的重點(diǎn)。

[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，F(xiàn)iorelli 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.