王 恒， 胡軍中， 閆永寶， 張舒陽

(1. 裝甲兵工程學(xué)院控制工程系，北京100072； 2. 浙江美科斯叉車有限公司，浙江 杭州 311407)

由于無人地面車輛(Unmanned Ground Vehicle，UGV)具備遙控和自主駕駛能力，并能對信息進(jìn)行采集和處理，因而在炸藥引爆、場地勘探、安全巡邏、戰(zhàn)場營救等領(lǐng)域發(fā)揮著重要作用，而且隨著技術(shù)研究的深入，其應(yīng)用范圍得到不斷拓展[1-3]。

軌跡跟蹤是UGV運動控制問題的研究重點，也是實現(xiàn)許多其他功能的前提。在給定參考軌跡的情況下，通過對車輛位姿信息的采集和處理，經(jīng)控制算法運算，將得出的控制量賦予車輛執(zhí)行機構(gòu)，能夠?qū)崿F(xiàn)對給定軌跡的跟蹤。而軌跡跟蹤的關(guān)鍵是控制算法的制定，基于不同的思想，為改進(jìn)控制算法，提高跟蹤效果，許多學(xué)者進(jìn)行了深入研究[4-8]。在運動學(xué)控制算法中，如何調(diào)整車的行駛方向是一個重要的問題，其直接影響到跟蹤誤差收斂的速度。文獻(xiàn)[9]給出了一種始終保持車體朝向參考點位置坐標(biāo)的算法，雖簡單易行，但跟蹤效果不是總能達(dá)到要求[10]。也有學(xué)者提出使用2個模糊控制器分別控制線速度與角速度，進(jìn)而得到調(diào)整車體朝向角的策略[11]，雖有一定效果，但控制規(guī)則的制定需大量經(jīng)驗基礎(chǔ)，實現(xiàn)和優(yōu)化起來比較困難。

為此，筆者基于優(yōu)化車體方向角調(diào)整策略的思想，提出了一種運動學(xué)控制算法，并與文獻(xiàn)[12]給出的基于反步法的控制律進(jìn)行了仿真跟蹤對比，證明了該算法的有效性。

1 運動學(xué)模型及跟蹤控制問題描述

為便于分析，運動學(xué)建模過程中假定UGV同側(cè)車輪轉(zhuǎn)速保持一致，車體為剛性框架，車輪為剛性輪，重心與幾何中心重合，且與地面間為純滾動，忽略滑轉(zhuǎn)與滑移。跟蹤位姿狀態(tài)如圖1所示，在全局坐標(biāo)系XOY中，UGV當(dāng)前位姿坐標(biāo)為(Xc,Yc,θc),參考點位姿坐標(biāo)為(Xr,Yr,θr)；在局部坐標(biāo)系xoy中，當(dāng)前位姿和參考點位姿坐標(biāo)分別為(xc,yc,θc)和(xr,yr,θr)。

圖1 跟蹤位姿狀態(tài)

令(Xe,Ye,θe)和(xe,ye,θe)分別為全局坐標(biāo)系和局部坐標(biāo)系下的位姿誤差,由圖1可得

令車體的線速度為v，角速度為w，則速度與位姿坐標(biāo)之間存在如下關(guān)系[13]：

進(jìn)而得到

(1)

2 控制律設(shè)計及穩(wěn)定性分析

2.1 跟蹤控制律設(shè)計

定義輔助變量如下：

(2)

(3)

式中:L為當(dāng)前位置與參考點之間的距離；φ為L與車體當(dāng)前朝向到參考點轉(zhuǎn)過的角度。極坐標(biāo)系下的誤差可用L、φ和θe表示，即

(4)

結(jié)合式(1)、(4)，可以得到[12]

(5)

在軌跡跟蹤問題中，首要考慮的是當(dāng)前點與參考點之間的距離，即L的大小。為使L趨近于0，可將UGV的線速度視作2個方向速度的合成，即

ν=f(νl,νr)，

式中：|νl|=KνL，νl的方向是從當(dāng)前點指向參考點，起到逐步減小L的作用，Kν為一個恒大于0的參數(shù)；νr為參考點的線速度，作用是確保跟蹤上參考軌跡。如圖1所示，定義輔助變量ν′,其大小及與X軸方向的夾角分別為

(6)

(7)

控制量ν的大小取為νl和νr在車體縱向速度方向上的投影之和，即

ν=νlcosφ+νrcosθe，

(8)

將|νl|=KνL代入式(8)，得到線速度的控制律為

ν=KνLcosφ+νrcosθe。

(9)

由圖1可見：要使車輛沿ν′方向行駛，須通過給定合適的控制量ω來調(diào)整車體方向角與θ′重合。給出角速度的控制律如下：

(10)

式中：Kω為一個恒大于0的參數(shù)。

由式(7)可得

(11)

(12)

式中：

(13)

(14)

由vl=KvL和式(5)可得

(15)

綜合式(10)-(15)，得到角速度的控制律如下：

ω= [β2+v′(Kω(vlsinφ+vrsinθe)+

Lsinφ)]/(1-β1)。

(16)

2.2 穩(wěn)定性分析

為證明式(9)、(16)所給出控制律的穩(wěn)定性，定義李雅普諾夫函數(shù)如下：

V=L2/2+1-cos(θ′-θc)。

(17)

由式(17)可見：V≥0，當(dāng)且僅當(dāng)qe=0時，V=0。對V求導(dǎo)，可得

(yeω-ν+νrcosθe)xe+(-xeω+νrsinθe)ye+

Lcosφ(νrcosθe-ν)+Lνrsinφsinθe+

[((β1-1)ω+β2)(vrsinθe+vlsinφ)]/v′。

(18)

將νl、ν和ω的表達(dá)式代入式(18)，可得

Lvrsinφsinθe+Kω(vlsinφ+vrsinθe)+

Lsinφ(KvLsinφ+vrsinθe)=KνL2-

Kω(KνLsinφ+vrsinθe)2，

(19)

3 算法仿真

筆者運用Matlab/Simulink對本文提出的算法和Kanayama算法[12]進(jìn)行了圓形軌跡仿真跟蹤對比，以驗證本文算法的跟蹤效果。

參考曲線參數(shù)為：νr=1 m/s,ωr=0.1 rad/s，(Xr(0),Yr(0),θr(0))=(30,0,π/2)； UGV初始點位姿為(25,0,0)；試驗中控制參數(shù)取為Kv=0.8，Kω=2。跟蹤仿真效果如圖2-8所示。

從仿真結(jié)果可以看出：本文提出的跟蹤算法能有效跟蹤圓形軌跡，與Kanayama算法相比，誤差收斂速度更快，且所需速度與角速度分配也更加合理。

圖2 跟蹤圓形軌跡結(jié)果

圖3 距離誤差變化曲線

圖4 X方向誤差變化曲線

圖5 Y方向誤差變化曲線

圖6 方向角誤差變化曲線

圖7 角速度變化曲線

圖8 線速度變化曲線

4 結(jié)論

針對UGV軌跡跟蹤問題,本文提出了一種基于極坐標(biāo)運動學(xué)的控制算法。該算法的核心思想是通過對車體方向角調(diào)整算法的優(yōu)化，使UGV時刻沿著更為合適的方向前進(jìn)，進(jìn)而達(dá)到提高跟蹤效果之目的。由于控制算法中只需調(diào)節(jié)2個參數(shù)，因而與模糊控制等需要大量經(jīng)驗基礎(chǔ)的算法相比，實現(xiàn)和優(yōu)化的過程更加簡單。從仿真結(jié)果來看：該算法能夠有效實現(xiàn)UGV的軌跡跟蹤，誤差收斂速度較快。

參考文獻(xiàn)：

[1] 岳松堂，張志杰.外軍無人地面車輛發(fā)展分析[J].現(xiàn)代軍事，2007(11)：53-56.

[2] 劉向平，薛杰.無人地面車輛：地面戰(zhàn)爭的新利器[J].國外坦克，2013(3)：45-52.

[3] 滿益明，陳慧巖，胡玉文，等.無人地面平臺研究概況及其關(guān)鍵技術(shù)[C]∥中國汽車工程學(xué)會越野車技術(shù)分會.2008年學(xué)術(shù)年會論文集.北京：北京理工大學(xué)出版社，2008：55-64.

[4] Moon H C, Lee J C，Kim J A, et al. Development of Unmanned Ground Vehicles Available of Urban Drive[C]∥Proceedings of International Conference on Advanced Intelligent Mechatronics. Singapore: Institute of Electrical and Electronics Engineers Inc,2009: 14-17.

[5] Woo H J, Yoon B J, Cho B G, et al. Research into Navigation Algorithm for Unmanned Ground Vehicle Using Real Time Kinematic (RTK)-GPS [C]∥Proceedings of ICROS-SICE International Joint Conference. Japan:Society of Instrument and Control Engineers, 2009:18-21.

[6] Filipescu A, Minzu V, Dumitrascu B, et al. Trajectory-tracking and Discret-time Sliding-mode Control of Wheeled Mobile Robots [C]∥Proceedings of IEEE International Conference on Information and Automation. Piscataway, USA: IEEE Computer Society Press,2011:27-32.

[7] Kao Y F, Chien Y H, Li I H, et al. Design and Implementation of Adaptive Dynamic Controllers for Wheeled Mobile Robots[C]∥Proceedings of ICSSE 2013 IEEE International Conference on System Science and Engineering. Oxford,United Kingdom: Elsevier Ltd, 2013: 195-199.

[8] Al-Araji A S, Abbod M F,Al-Raweshidy H S,et al. Design of an Adaptive Nonlinear PID Controller for Nonholonomic Mobile Robot Based on Posture Identifier[C]∥Proceedings of IEEE International Conference on Control System, Computing and Engineering. Piscataway, USA: IEEE Computer Society Press,2011: 337-342.

[9] Chwa D. Tracking Control of Differential-drive Wheeled Mobile Robots Using a Backstepping-like Feedback Linearization[J].IEEE Transactions on Systems, Man, and Cybernetics,2010,40(6): 1285-1295.

[10] Amoozgar M H, Zhang Y M. Trajectory Tracking of Wheeled Mobile Robots: A Kinematical Approach[C]∥Proceedings of 2012 8th IEEE/ASME International Conference on Mechatronic and Embedded Systems and Applications.Washington,DC,USA: IEEE Computer Society Press,2012: 275-280.

[11] Amoozgar M H, Sadati S H, Alipour K. Trajectory Tracking of Wheeled Mobile Robots Using a Kinematical Fuzzy Controller[J].International Journal of Robotics and Automation,2012, 27(1): 49-56.

[12] Kanayama Y, Kimura Y, Miyazaki F. A Stable Tracking Control Method for Autonomous Mobile Robot [C]∥Proceedings of 1990 IEEE International Conference on Robotics and Automation. Cincinnati, USA:IEEE Computer Society Press,1990: 384-389.

[13] 徐俊艷, 張培仁. 非完整輪式移動機器人軌跡跟蹤控制研究[J].中國科學(xué)技術(shù)大學(xué)學(xué)報，2004，34(3)：376-380.