四輪移動(dòng)機(jī)器人跟隨控制

劉子龍，丁玉靜，江艷霞

（上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海，200093）

四輪移動(dòng)機(jī)器人跟隨控制

劉子龍，丁玉靜，江艷霞

（上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海，200093）

針對輪式移動(dòng)機(jī)器人路徑跟隨問題，提出一種多移動(dòng)機(jī)器人路徑跟隨控制方案。首先對其運(yùn)動(dòng)學(xué)模型經(jīng)過適當(dāng)增補(bǔ)，將基于輸入輸出反饋線性化的橫向位置跟蹤控制方案和基于古典校正的縱向跟隨控制方案相結(jié)合，這2種方案互為前提，相互依托，進(jìn)而制定出相應(yīng)的多移動(dòng)機(jī)器人路面跟隨控制方案。研究結(jié)果表明：該方法簡單實(shí)用，無論在橫向位置跟蹤和縱向位置跟隨上都得到較好的控制效果，動(dòng)態(tài)特性好，穩(wěn)態(tài)精度高。

輪式移動(dòng)機(jī)器人；跟隨控制；反饋線性化；古典校正

輪式移動(dòng)機(jī)器人是重要的物流工具之一，在無人自動(dòng)駕駛車輛、火星探測車等領(lǐng)域得到廣泛應(yīng)用。移動(dòng)機(jī)器人研究是20世紀(jì)80年代以后興起的一個(gè)新的研究領(lǐng)域，它是21世紀(jì)自動(dòng)化技術(shù)研究和發(fā)展的重要方面。四輪移動(dòng)機(jī)器人其模型可簡化為兩輪自行車模型，行駛在路面上跟蹤地面標(biāo)明的軌線。由于在路面上行駛，不免存在多個(gè)移動(dòng)機(jī)器人跟隨現(xiàn)象，即在橫向上跟蹤路面軌跡，在縱向上多個(gè)機(jī)器人之間保持一定等間距有序跟隨。由于路面軌線上各個(gè)點(diǎn)的切線斜率各不相同，在橫向跟蹤的同時(shí)又要縱向跟隨，所以，聯(lián)排控制方法并不適用[1?2]；又由于移動(dòng)機(jī)器人其動(dòng)力學(xué)模型具有嚴(yán)重非線性特點(diǎn)，且模型參數(shù)難以測量，致使基于動(dòng)力學(xué)模型的控制效果并不理想[3?4]；模糊控制對難以建模的被控對象有較好的控制效果，但是，對于復(fù)雜的路況很難建立統(tǒng)一的控制律[5?7]；在研究方法上，很多學(xué)者將機(jī)器人位置坐標(biāo)建立在全局坐標(biāo)系內(nèi)，這對于大范圍路面來說，難于準(zhǔn)確測量和定位，因此，在全局坐標(biāo)系內(nèi)設(shè)計(jì)控制方案的方法并不適合[8?9]。在行駛過程中，移動(dòng)機(jī)器人的縱向速度會(huì)因路況不同而不同，人們對其在縱向速度時(shí)變情況下的控制研究較少。在此，本文作者在自然坐標(biāo)系內(nèi)，兼顧車體運(yùn)動(dòng)的“柔和”性，增補(bǔ)移動(dòng)機(jī)器人車體運(yùn)動(dòng)學(xué)模型。在車體縱向速度時(shí)變的情況下，在橫向上采用一種基于輸入輸出反饋線性化的橫向位置跟蹤控制方法；在縱向上，將車體航向角的變化看成一種干擾量，進(jìn)而采用古典校正的跟隨控制方法，將這2種方法相互結(jié)合、互為前提，以實(shí)現(xiàn)路面車輛跟隨控制。

1 控制系統(tǒng)體系結(jié)構(gòu)

移動(dòng)機(jī)器人控制系統(tǒng)體系結(jié)構(gòu)如圖1所示。電荷耦合元件（CCD）攝像頭起對視覺信號采集、對所要跟蹤路徑的感知和識(shí)別作用，經(jīng)過主處理器計(jì)算，以車體相對于路徑的橫向位移偏差與航向角偏差作為結(jié)果，并且主處理器進(jìn)行路徑規(guī)劃，將規(guī)劃結(jié)果傳遞到執(zhí)行電機(jī)，使執(zhí)行電機(jī)帶動(dòng)車輪跟蹤規(guī)劃結(jié)果。

圖1 控制系統(tǒng)框圖Fig.1 Diagram of control system

2 運(yùn)動(dòng)學(xué)模型的建立

2.1 平面直角坐標(biāo)系下的運(yùn)動(dòng)學(xué)模型

四輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型如圖2所示。圖2中：

其中：vfrontwheel（t）為車體兩前輪中心點(diǎn)的速度，m/s；vrearwheel（t）為車體兩后輪中心M點(diǎn)的速度，m/s；θ（t）為車體前輪轉(zhuǎn)向角，rad；ψ（t）為車體航向角，rad；y（t）為車體兩后輪中心點(diǎn)M距X軸的距離，m；x（t）為車體兩后輪中心點(diǎn)M距Y軸的距離，m；d為車體前、后輪間的距離，m；M為車體兩后輪中心點(diǎn)和橫向被控點(diǎn)。由式（1）和（2）得：

式（3）～（5）為車體本身固有的運(yùn)動(dòng)學(xué)模型，其中：式（3）為車體固有縱向運(yùn)動(dòng)學(xué)模型；式（4）和（5）為車體固有橫向運(yùn)動(dòng)學(xué)模型。

圖2 平面直角坐標(biāo)系下移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型Fig.2 Kinematics model of mobile robot in plane right-angle coordinates

由式（3）可知：vrearwheel（t）為縱向運(yùn)動(dòng)控制量。由式（5）可知：tanθ（t）為橫向運(yùn)動(dòng)控制量，若直接將該2項(xiàng)作為控制量，則車體運(yùn)動(dòng)不夠“柔和”， 因此，需要在原模型的基礎(chǔ)上進(jìn)行增補(bǔ)：

其中：ulatitude（t）為橫向控制輸入；ulongitude（t）為縱向控制輸入。式（3）和（7）為車體增補(bǔ)后的縱向運(yùn)動(dòng)學(xué)模型，式（4）～（6）為車體增補(bǔ)后的橫向運(yùn)動(dòng)學(xué)模型。

2.2 自然坐標(biāo)系下的運(yùn)動(dòng)學(xué)模型

移動(dòng)機(jī)器人行駛在路面上，跟蹤路面標(biāo)明的軌跡，一些研究者采用的是全局坐標(biāo)系，即以行駛平面內(nèi)某一固定點(diǎn)為坐標(biāo)系原點(diǎn)，進(jìn)而建立平面直角坐標(biāo)系,相應(yīng)的運(yùn)動(dòng)學(xué)模型建立和控制算法的制定都是以該平面直角坐標(biāo)系為基礎(chǔ)。但這不適合于大范圍的定位和控制。

在車體行進(jìn)過程中，在被跟蹤軌跡上建立自然坐標(biāo)系，即以路面軌跡上的被測點(diǎn)為坐標(biāo)系的原點(diǎn)，X軸為過該被測點(diǎn)的切線，正方向?yàn)檐圀w正方向，Y軸按逆時(shí)針垂直X軸，如圖3所示。

圖3 自然坐標(biāo)系下移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型Fig.3 Kinematics model of mobile robot in natural reference

在橫向上，只要車體能夠時(shí)刻跟蹤X軸，則能夠跟蹤被跟蹤軌線；在縱向上通過激光雷達(dá)等傳感器，測量距前車距離，進(jìn)而制定相應(yīng)的縱向跟隨控制方案，保證縱向間距恒定。這2種控制方案互為前提、相互依托，共同實(shí)現(xiàn)縱向跟隨控制。

由以上論述可知平面直角坐標(biāo)系與自然坐標(biāo)系的區(qū)別只是坐標(biāo)系的原點(diǎn)不同，而車體的運(yùn)動(dòng)學(xué)方程相同。這給控制方案的制定帶來了便利，降低了控制方案制定的難度。

3 控制方案的制定

路面跟隨控制時(shí)，在橫向上跟蹤路面標(biāo)明的軌跡或磁釘，在縱向上各車之間保持固定的間距。因此，路面跟隨控制器包括橫向跟蹤控制器和縱向跟隨控制器，其結(jié)構(gòu)如圖4所示。由圖4可見：上閉環(huán)為橫向位置跟蹤環(huán)，下閉環(huán)為縱向跟隨環(huán)，兩者互為前提，相互依托，共同完成路面跟隨控制。

3.1 橫向跟蹤控制

在移動(dòng)機(jī)器人系統(tǒng)中，車體跟蹤控制是其主要部分，而橫向跟蹤控制又是車體跟蹤控制的主要部分。所以，設(shè)計(jì)簡單有效的橫向跟蹤控制策略很有必要。

由式（4）～（6）可見：該系統(tǒng)為非線性、時(shí)變系統(tǒng)。從其結(jié)構(gòu)來看，該系統(tǒng)為單輸入單輸出仿射非線性系統(tǒng)，所涉及的問題為非線性系統(tǒng)的輸出跟蹤問題。

許多學(xué)者采用微分幾何控制理論中的反饋線性化方法研究非線性控制系統(tǒng)的輸出跟蹤問題[10?11]。文獻(xiàn)[12]對該方法進(jìn)行了較詳細(xì)的論述，獲得了較好的控制效果。在此，采用文獻(xiàn)[12]中的方法，進(jìn)行橫向位置跟蹤控制。

3.2 縱向跟隨控制

多移動(dòng)機(jī)器人在進(jìn)行路徑跟隨時(shí)，在橫向上跟蹤路面軌跡，在縱向上還必須保證移動(dòng)機(jī)器人之間保持安全距離，即保證縱向跟隨[13?15]。

由于路面軌跡上各點(diǎn)斜率不同，各車輛在縱向跟隨時(shí)，各自的行駛狀態(tài)也不相同，嚴(yán)格保證車體縱向間等間距幾乎不可能。

這里將縱向跟隨方法進(jìn)行適當(dāng)改進(jìn)，以達(dá)到近似等間距跟隨，如圖5所示。

圖4 跟隨控制系統(tǒng)結(jié)構(gòu)圖Fig.4 Block diagram of following control system

圖5 縱向跟隨控制Fig.5 Longitudinal following control

通過安裝在后車前部的激光雷達(dá)等傳感器，測量后車前部中心點(diǎn)Mf距前車后側(cè)中心點(diǎn)Mr的距離L（t），以及該2點(diǎn)的連線與后車航向間的夾角γ（t），進(jìn)而計(jì)算出這2個(gè)中心點(diǎn)間的距離L（t）在后車航向上的投影Ld（t）。保持Ld（t）為常值，進(jìn)而實(shí)現(xiàn)在縱向上近似等間距控制。

可見：經(jīng)拉普拉斯變換后，該系統(tǒng)含有2個(gè)積分環(huán)節(jié)，系統(tǒng)不穩(wěn)定。可采用校正控制，校正環(huán)節(jié)為：（其中：K為增益常數(shù)，T1為一階微分時(shí)間常數(shù)；T2為一階慣性時(shí)間常數(shù)）。校正參數(shù)K=1 250,T1=1/20和T2=1/300，所得校正后的縱向系統(tǒng)開環(huán)波特圖見圖6。

圖6 縱向系統(tǒng)波特圖Fig.6 Corrected bode diagram of longitudinal system

4 系統(tǒng)仿真

移動(dòng)機(jī)器人車體長d=2.83 m，寬dwidth=1.5 m，車體前輪舵角限幅|θ|≤20o，在平面直角坐標(biāo)系內(nèi)所跟蹤的理想軌跡方程為：

前車后輪中心點(diǎn)的初始位置為：x=349.893 3 m，y=203.998 93 m；在自然坐標(biāo)系內(nèi)，初始航向角為初始前輪舵角后輪中點(diǎn)速度取為：

后車后輪中心點(diǎn)的初始位置為：x=350 m，y= 200 m；在自然坐標(biāo)系內(nèi)，初始航向角為ψ（0）=

縱向車間理想間距為Ld（t）=2 m，采樣周期為Ts=0.03 s。

其中：e（s）為后車與前車間的縱向跟隨誤差。所得仿真曲線見圖7～12（由于前、后兩車在橫向跟蹤上采用的控制方法相同，故后車前輪舵角曲線、后車航向角曲線、后車橫向跟蹤誤差曲線與前車的極其相似）。

從圖7～12可知：在前車車體縱向速度時(shí)變甚至為負(fù)值的情況下，前、后2車的車體在橫向上仍然能夠很好地跟蹤事先未知的路面軌線，動(dòng)態(tài)特性較好，穩(wěn)態(tài)跟蹤精度高；在縱向上雖然前車的縱向速度發(fā)生時(shí)變，所跟蹤路面軌線的每一點(diǎn)切線斜率各不相同，但后車仍然能夠很好地跟隨前車。可見：該跟隨控制方法非常適用于移動(dòng)機(jī)器人路面跟隨控制系統(tǒng)。

圖7 理想軌跡圖Fig.7 Desired path

圖8 前車縱向速度Fig.8 Longitude velocity of lead vehicle

圖9 前車前輪舵角Fig.9 Steering angle of front wheel of lead vehicle

圖10 前車航向角Fig.10 Course angle of lead vehicle

圖11 前車橫向跟蹤誤差Fig.11 Lateral tracking error of lead vehicle

圖12 后車縱向跟蹤誤差Fig.12 Longitudinal tracking error of follow vehicle

5 結(jié)論

（1） 兼顧車體運(yùn)動(dòng)的“柔和”性，增補(bǔ)了輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，將多機(jī)器人跟隨控制問題分解成2部分：在橫向上，依據(jù)車體增補(bǔ)后的運(yùn)動(dòng)學(xué)模型，采用輸入輸出反饋線性化方法，制定出車體橫向位置跟蹤的控制方案；縱向上，將航向角的變化當(dāng)成一種干擾，采用古典校正的控制方法制定了跟隨方案。這2種方案相互依托，互為前提，從整體上實(shí)現(xiàn)了路面跟隨控制。

（2） 該方案結(jié)構(gòu)簡單，即使在移動(dòng)機(jī)器人縱向速度發(fā)生時(shí)變的情況下，仍然能夠很好地實(shí)現(xiàn)多機(jī)器人路徑跟隨。

[1] PENG Ya-fu. Adaptive intelligent backstepping longitudinal control of vehicle platoons using output recurrent cerebellar model articulation controller[J]. Expert Systems with Applications, 2010, 37（3）： 2016?2027.

[2] Yi S Y, Chong K T. Impedance control for a vehicle platoon system Mechatronics[J]. Mechatronics, 2005, 15（5）： 627?638.

[3] Nouvelière L, Mammar S. Experimental vehicle longitudinal control using a second[J]. Control Engineering Practice, 2007, 15（8）： 943?954.

[4] Enache N M, Netto M, Mammar S, et al. Driver steering assistance for lane departure avoidance[J]. Control Engineering Practice, 2009, 17（6）： 642?651.

[5] 鄒小兵, 蔡自興. 非完整移動(dòng)機(jī)器人道路跟蹤控制器設(shè)計(jì)及應(yīng)用[J]. 控制與決策, 2004, 19（3）： 319?322.

ZOU Xiao-bing, CAI Zi-xing. Control and decision, design and application of path tracking controller for nonholonomic mobile robots[J]. Control and Decision, 2004, 19（3）： 319?322.

[6] PEI Xin-zhe, LIU Zhi-yuan, PEI Run. Robust trajectory tracking controller design for mobile robot swith bounded input[J]. Acta Automatica Sinica, 2003, 129（16）： 876?882.

[7] HUANG Ji-hua, Tomizuka M. LTV controller design for vehicle lateral control under fault in rear sensors[J]. IEEE/ASME Trans Mechatronics, 2005, 10（1）： 1?7.

[8] Haddad M, Chettibi T, Hanchi S, et al. Random-profile approach for trajectory planning of wheeled mobile robots[J]. European Journal of Mechanics： A/Solids, 2007, 26（3）： 519?540.

[9] Martins F N, Celeste W C, Carelli R, et al. Adaptive dynamic controller for autonomous mobile robot trajectory tracking[J]. Control Engineering Practice, 2008, 16（11）： 1354?1363.

[10] 楊軍宏, 尹自強(qiáng), 李圣怡. 閥控非對稱缸的非線性建模及其反饋線性化[J]. 機(jī)械工程學(xué)報(bào), 2006, 42（5）： 203?207.

YANG Jun-hong, YIN Zi-qiang, LI Sheng-yi. Nonlinear modelling and feedback linearization of valve-controlled asymmetrical cylinder[J]. Journal of Mechanical Engineering, 2006, 42（5）： 203?207.

[11] de Tuglie E, Iannone S M, Torelli F. Feedback-linearization and feedback-feedforward decentralized control for multimachine power system[J]. Electric Power Systems Research, 2008, 78（3）： 382?391.

[12] 劉子龍. 基于反饋線性化的無人駕駛車輛橫向位置跟蹤控制[J]. 系統(tǒng)工程與電子技術(shù), 2009, 31（1）： 165?169.

LIU Zi-long. Lateral position tracking control of unpiloted vehicle based on feedback-linearization[J]. Systems Engineering and Electronics, 2009, 31（1）： 165?169.

[13] Lee G D, Kim S W. A longitudinal control system for a platoon of vehicles using a fuzzy-sliding mode algorithm[J]. Mechatronics, 2002, 12（1）： 97?118.

[14] LIANG Hong, Chong K T, No T S, et al. Vehicle longitudinal brake control using variable parameter sliding control[J]. Control Engineering Practice, 2003, 11（4）： 403?411.

[15] HUANG Su-nan, REN Wei. Vehicle longitudinal control using throttles and brakes[J]. Robotics and Autonomous Systems, 1999, 26（4）： 241?253.

（編輯 陳燦華）

Following control of four-wheeled mobile robot

LIU Zi-long, DING Yu-jing, JIANG Yan-xia
（School of Optical-Electrical and Computer Engineering, University of Shanghai for Science and Technology, Shanghai 200093, China）

A following control law that is conceptually simple and straightforward for wheeled mobile robot was proposed. Firstly, the kinematical model for wheeled mobile robot was supplemented, then the kinematics model included feedback-linearization tracking control strategy on lateral side with classic correction control on longitudinal side was decoupled. These two controllers were interdependent and coordinated. The results show that this method exerts better control effect in wide range of speed, shorter time response, higher precision and better stability.

wheeled mobile robot; following control; feedback-linearization; classic correction control

TP242

A

1672?7207（2011）05?1348?06

2010?05?10；

2010?07?22

國家自然科學(xué)基金資助項(xiàng)目（60874002）

劉子龍（1972?），男，山東安邱人，博士，講師，從事機(jī)器人控制、過程控制研究；電話：15921927209；E-mail： liuzl0704@yahoo.com.cn