王國勝， 夏 凡， 呂 強， 劉 峰

(裝甲兵工程學院控制工程系，北京 100072)

基于動力學與運動學的四輪全向移動機器人軌跡跟蹤控制

王國勝， 夏 凡， 呂 強， 劉 峰

(裝甲兵工程學院控制工程系，北京 100072)

建立了四輪全向移動機器人的運動學模型和動力學模型，并分析了四輪全向移動機器人執(zhí)行器的機械特性。在此基礎(chǔ)上，利用反饋控制設(shè)計了四輪全向移動機器人的運動學控制器，利用逆動力學補償控制設(shè)計了四輪全向移動機器人的動力學控制器，實現(xiàn)了基于動力學與運動學的四輪全向移動機器人軌跡跟蹤控制系統(tǒng)設(shè)計。最后，利用Matlab/Simulink完成了基于動力學與運動學的四輪全向移動機器人軌跡跟蹤控制仿真實驗，實驗結(jié)果驗證了該軌跡跟蹤控制方法的有效性。

四輪全向移動機器人；軌跡跟蹤；運動學；動力學

由于輪式機器人具有結(jié)構(gòu)簡單、狀態(tài)穩(wěn)定等特點，現(xiàn)已廣泛應用于工業(yè)和服務(wù)等領(lǐng)域。輪式機器人的類型多種多樣，其中全向移動機器人因其可以在任意狀態(tài)下向任意方向移動而備受關(guān)注[1-2]。

早期的全向移動機器人控制策略是基于運動學模型設(shè)計的[3]，而未考慮全向移動機器人的動力學模型。Morin等[4]提出了可以忽略動力學模型的假設(shè)條件；但是Martins等[5]提出了當系統(tǒng)存在不確定性、時變參數(shù)時或者當機器人快速移動時，不可以忽略機器人的動力學模型，從而使得基于動力學的機器人控制研究成為熱點。Song等[6]提出了一種逆動力學補償控制策略，其將經(jīng)典控制和魯棒控制相結(jié)合，實現(xiàn)了機器人控制；Kao等[7]利用動力學模型提出了基于積分滑模變結(jié)構(gòu)的機器人控制；Ebrahim等[8]提出了基于一階和二階滑模的逆動力學補償控制方法。

滑?？刂剖且环N魯棒性較強的控制方法，在機器人控制中取得了廣泛的應用。Hwang等[9]將模糊控制和滑?？刂葡嘟Y(jié)合，實現(xiàn)了移動機器人軌跡跟蹤控制；Zhang等[10]針對非完整性移動機器人設(shè)計了動態(tài)滑?？刂撇呗?。因此，本文將動力學模型和運動學模型相結(jié)合，利用經(jīng)典PID控制和滑?？刂茖崿F(xiàn)四輪全向移動機器人逆動力學控制，完成其軌跡跟蹤控制設(shè)計并通過仿真實驗驗證了其控制效果。

1 四輪全向移動機器人的數(shù)學模型

1.1 四輪全向移動機器人的運動學模型

四輪全向移動機器人的運動學坐標系如圖1所示，其中:Ow-XwYw是以現(xiàn)場的特定點為原點的基準坐標系；Or-XrYr為相對坐標系；(xw,yw,θw)、(xr,yr,θr)分別為基準坐標系、相對坐標系下四輪全向移動機器人的位姿；vi(i=1,2,3,4)為四輪全向移動機器人的輪胎線速度；θ為四輪全向移動機器人的方位角，其中θ=θr=θw；L為全向輪到機器人中心的距離。

圖1 四輪全向移動機器人的運動學坐標系

對圖1進行分析可知：四輪全向移動機器人的4個輪胎線速度分別為

(1)

相對坐標系下坐標與基準坐標系下坐標之間滿足以下轉(zhuǎn)換關(guān)系：

(2)

將式(2)代入式(1)，可得四輪全向移動機器人的輪胎線速度為

(3)

1.2 四輪全向移動機器人的動力學模型

四輪全向移動機器人的動力學坐標系如圖2所示，其中fi(i=1,2,3,4)為4個全向輪上電機的驅(qū)動力。

圖2 四輪全向移動機器人的動力學坐標系

四輪全向移動機器人的總能量由動能、勢能及能量損失3部分組成，其中:動能包括平移運動動能、旋轉(zhuǎn)運動動能和電機旋轉(zhuǎn)動能;勢能為重力勢能。對圖2進行分析，可知四輪全向移動機器人的平移運動動能T1、旋轉(zhuǎn)運動動能T2和電機旋轉(zhuǎn)動能T3分別為[11]

(4)

(5)

(6)

(7)

(8)

(9)

式中：mm為機器人主體的質(zhì)量；ml為全向輪的質(zhì)量；2Lc為底座的一邊長度；τg為電機的轉(zhuǎn)矩；R為輪胎半徑。

系統(tǒng)中存在能量損失主要是由系統(tǒng)中電機旋轉(zhuǎn)時受摩擦阻力和底座旋轉(zhuǎn)時受地面摩擦阻力所致。四輪全向移動機器人的能量損失方程為

(10)

式中：Da為底座旋轉(zhuǎn)時與地面的摩擦因數(shù)；Dm為電機的黏性摩擦因數(shù)。

因為四輪全向移動機器人在平面上運動，所以其重力勢能V=0。設(shè)

則第二類Lagrange方程為

(11)

式中:τ=(Fx,Fy,Fθ)T,為機器人所受的驅(qū)動合力，包括平移力和旋轉(zhuǎn)力矩；q=(x(t),y(t),θ(t))T,為機器人在基準坐標系下坐標。

綜合式(4)-(10)，可知式(11)中左端3項分別為

(12)

(13)

(14)

將式(12)-(14)代入式(11)，可得

(15)

對減速直流電機特性進行分析，可知四輪全向移動機器人的驅(qū)動力為

(16)

式中:Kt為電機轉(zhuǎn)矩常數(shù)；Ui為電機的電壓；Ra為電機的電樞電阻；n為電機的齒輪減速比；ωi為輪子轉(zhuǎn)速。

對圖2進行分析，可知四輪全向移動機器人所受的驅(qū)動合力τ與4個驅(qū)動力的關(guān)系式為

(17)

將式(17)寫成如下矩陣形式：

(18)

將式(16)、(18)代入式(15)，可得四輪全向移動機器人的動力學模型為

(19)

式中：U=(U1,U2,U3,U4)T，ω=(ω1,ω2,ω3,ω4)T。

2 四輪全向移動機器人的軌跡跟蹤控制設(shè)計

基于運動學和動力學的四輪全向移動機器人軌跡跟蹤控制流程如圖3所示。動力學控制的目的是使機器人在驅(qū)動力的控制下跟蹤由運動學控制得到的速度控制信號;運動學控制的作用是根據(jù)位姿的誤差得出速度信號,使得機器人可以在速度信號的控制下跟蹤目標位置。

圖3 基于運動學和動力學的四輪全向移動機器人軌跡跟蹤控制流程

2.1 四輪全向移動機器人的運動學控制設(shè)計

定義期望軌跡為

式中：xd(t)、yd(t)分別為機器人在Xw、Yw軸上的期望位置；θd(t)為期望姿態(tài)角。

定義期望速度為

式中：vxd、vyd分別為機器人在Xw、Yw軸上的分速度；wd為機器人繞中心旋轉(zhuǎn)的旋轉(zhuǎn)速度。

當機器人在期望軌跡上運動時，滿足如下關(guān)系：

令四輪全向移動機器人的軌跡跟蹤誤差為

(20)

對式(20)求導，可得

(21)

針對式(21)，取反饋控制為

(22)

式中：kx、ky和kθ為運動學參數(shù)，均大于0。由式(22)可得

(23)

由

可知跟蹤誤差漸近穩(wěn)定，從而全向移動機器人在運動控制(23)作用下可跟蹤期望軌跡。

2.2 四輪全向移動機器人的動力學控制設(shè)計

因為vi=Rωi，其中ωi為機器人全向輪的角速度。由式(3)可知

(24)

將式(24)代入式(19)，可得

(25)

式中:

到了山西，不吃碗正宗的刀削面，那絕對算空走一趟。山西刀削面一講刀工，二講澆頭，小小一碗面，里面有很多門道。削面功夫全憑手的勁頭掌控，削出的面似空中飛舞的柳葉，又似水中暢游的銀魚，中厚邊薄，均勻有致，長短六寸，方為上品。

如實際系統(tǒng)存在擾動，則式(25)可寫為

(26)

取控制律

U=G+(r1+r2)，

(27)

(28)

由式(27)可得

r1=GU-r2，

(29)

由式(26)可得

(30)

基于式(29)、(30)，可得

(31)

由式(28)可得

(32)

由式(31)、(32)可得

(33)

將式(33)寫成如下狀態(tài)方程形式：

(34)

式中:

選取滑模面為σ=TX,其中

(35)

式中：

3 仿真實驗

假設(shè)四輪全向移動機器人的運動學模型和動力學模型的參數(shù)為L=0.12 m,m=2 kg,I=1.013 kg·m2,Kt=0.014,R=0.03 m,Ra=1.9 Ω。設(shè)定期望軌跡為

設(shè)定四輪全向移動機器人的初始位置為x=1.36,y=0.63,θ=0。選取運動學參數(shù)為kx=ky=kθ=0.8。選取動力學參數(shù)為ρ=40，H0=0.7H，d=(0,0,0)T,

圖4 四輪全向移動機器人的速度誤差隨時間變化的曲線

圖5 四輪全向移動機器人的動力學控制電壓隨時間變化的曲線

圖6 四輪全向移動機器人的動力學控制力矩隨時間變化的曲線

圖7 四輪全向移動機器人的軌跡跟蹤效果隨時間變化的曲線

圖8 四輪全向移動機器人的軌跡跟蹤誤差隨時間變化的曲線

由圖4可知：四輪全向移動機器人在動力學控制下速度誤差逐漸穩(wěn)定且無穩(wěn)態(tài)誤差。由圖5、6可知：四輪全向移動機器人在動力學控制下電機提供的電壓及電機輸出的力矩基本呈對應關(guān)系。由圖7、8可知：該仿真實驗中四輪全向移動機器人軌跡跟蹤準確且響應速度快。

4 結(jié)論

本文基于四輪全向移動機器人的運動學和動力學模型，提出了經(jīng)典PID控制和滑?？刂葡嘟Y(jié)合的逆動力學補償控制策略,通過仿真實驗驗證了該策略在四輪全向移動機器人軌跡跟蹤中的有效性，結(jié)果表明：該策略能滿足四輪全向移動機器人軌跡跟蹤控制的快速性和穩(wěn)定性要求。

[1] 王國勝, 呂紅濤, 呂強. 四輪全向移動機器人的雙閉環(huán)速度控制系統(tǒng)設(shè)計[J]. 裝甲兵工程學院學報, 2013, 27(1): 59-64.

[2] Wang G S, Xia F, Guo F, et al. Design of Positioning System for Indoor Mobile Multi-robot Platform Based on Image Features[C]∥Proceedings of the 11th World Congress on Intelligent Control and Automation. Shenyang, China: IEEE, 2014: 5149-5154.

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

[4] Morin P, Samson C. Motion Control of WMRs [J]. Handbook of Robotics, 2008, 4(3): 799-826.

[5] Martins F N, Celeste W C, Carelli R, et al. An Adaptive Dynamic Controller for Autonomous Mobile Robot Trajectory Tracking [J]. Control Engineering Practice, 2008, 16(8): 1354-1363.

[6] Song Y D, Mitchell T L, Lai H Y. Control of a Type of Nonlinear Uncertain Systems via Compensated Inverse Dynamics Approach [J]. IEEE Transactions on Automatic Control, 1994, 39(9): 1866-1871.

[7] Kao S T, Chiou W J, Ho M T. Integral Sliding Mode Control for Trajectory Tracking Control of an Omnidirectional Mobile Robot[C]∥Proceedings of Asian Control Conference. Kaohsiung, Taiwan: IEEE, 2011: 765-770.

[8] Ebrahim S E, Martins N A, Edson R P. On a Wheeled Mobile Robot Trajectory Tracking Control: 1st and 2nd Order Sliding Modes Applied to a Compensated Inverse Dynamics[C]∥Proceedings of 11th Pan-American Congress of Applied Mechanics. Brazil: IEEE, 2010: 345-349.

[9] Hwang C L, Wu H M. Hybrid Fuzzy Sliding-mode Under-actuated Control for Trajectory Tracking of Mobile Robot in the Presence of Friction and Uncertainty[C]∥Proceedings of IEEE World Congress on Computational Intelligence. Brisbane, Australia: IEEE, 2012: 117-123.

[10] Zhang R L, Zhang Y P, Xue Z H, et al. The Review and Implementation of Multi-robot System Simulation Software[C]∥Proceedings of the 26th Chinese Control and Decision Conference. Changsha, China: IEEE, 2014: 3869-3874.

[11] Xiao J, Xie L H, Khanna N, et al. Cooperative Control in HNMSim:A 3D Hybrid Networked MAS Simulator[C]∥Proceedings of Control Automation Robotics & Vision. Guangzhou, China: IEEE, 2012:626-631.

(責任編輯:尚彩娟)

Trajectory Tracking Control of Four-wheels Omnidirectional Mobile Robot Based on Dynamics and Kinematics

WANG Guo-sheng, XIA Fan, Lü Qiang, LIU Feng

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

The kinematics model and the dynamic model of four-wheels omnidirectional mobile robot are established, and the actuators’ mechanical characteristics of four-wheels omnidirectional mobile robot are analyzed. In order to realize the trajectory tracking control based on kinematics and dynamics of four-wheels mobile omnidirectional robot, the kinematics controller of four-wheels omnidirectional mobile robot is designed by using the feedback control method and the dynamics controller of four-wheels omnidirectional mobile robot is designed by using the inverse dynamic compensation control method. Finally, the trajectory tracking control experiment of four-wheels omnidirectional mobile robot based on kinematics and dynamics is made by using Matlab/Simulink and the simulation results validate the effectiveness of the proposed trajectory tracking control method.

four-wheels omnidirectional mobile robot; trajectory tracking; kinematics; dynamics

1672-1497(2015)01-0054-06

2014- 11- 05

王國勝(1975-)，男，副教授，博士。

TP242

A

10.3969/j.issn.1672-1497.2015.01.011