差速驅(qū)動(dòng)輪式移動(dòng)機(jī)器人路徑規(guī)劃新策略

史恩秀 黃玉美 朱從民 張亞旭

1.西安理工大學(xué)，西安，710048

2.中國(guó)人民解放軍總后勤部建筑工程研究所，西安，710032

0 引言

輪式移動(dòng)機(jī)器人（wheeled mobile robot，WMR）是自動(dòng)化制造系統(tǒng)中重要的物流工具之一，被廣泛應(yīng)用于 FMS（flexible manufacture system）、CIMS（computer intelligent manufacture system）等無(wú)人生產(chǎn)車(chē)間以完成物料的自動(dòng)搬運(yùn)。因此，對(duì)其在目標(biāo)點(diǎn)處的位置和姿態(tài)（兩者合稱(chēng)位姿）有嚴(yán)格的要求。為快速、安全、準(zhǔn)確地將物料運(yùn)輸?shù)侥康牡兀骔MR應(yīng)具有環(huán)境感知、路徑規(guī)劃和運(yùn)動(dòng)控制等功能。移動(dòng)機(jī)器人的工作效率在很大程度上取決于其運(yùn)動(dòng)路徑。根據(jù)WMR工作環(huán)境的已知程度，路徑規(guī)劃分為環(huán)境信息完全已知的全局路徑規(guī)劃和環(huán)境信息部分已知或完全未知的基于傳感器感知的局部路徑規(guī)劃。全局路徑規(guī)劃可離線進(jìn)行，規(guī)劃路徑的精確程度取決于獲取環(huán)境信息的準(zhǔn)確度。在相鄰兩目標(biāo)點(diǎn)之間，為WMR規(guī)劃一條路徑可采用的方法很多，如蟻群法［1］、人工勢(shì)場(chǎng)法［2］、神經(jīng)網(wǎng)絡(luò)法［3］。Dubins［4］對(duì)WMR進(jìn)行路徑規(guī)劃時(shí)，假設(shè)路徑由兩段圓弧和連接它們的切線段組成，此假設(shè)雖可獲得長(zhǎng)度最小的規(guī)劃路徑，但對(duì)于DDWMR，會(huì)因路徑曲率變化不連續(xù)而無(wú)法控制它準(zhǔn)確跟蹤路徑，最終影響它到達(dá)目標(biāo)點(diǎn)的精度。近年來(lái)，一些學(xué)者提出了基于高次參數(shù)曲線的路徑規(guī)劃方法［5］以解決路徑曲率不連續(xù)問(wèn)題，甚至將加速度的連續(xù)性也考慮在內(nèi)［6］。文獻(xiàn)［7］在二維空間和三維空間探討了Dubins路徑、回旋曲線等問(wèn)題，以尋求一種多移動(dòng)機(jī)器人場(chǎng)合下的等距離路徑的規(guī)劃方法。但這些方法實(shí)現(xiàn)起來(lái)較復(fù)雜，且對(duì)于WMR，沒(méi)有必要要求其加速度連續(xù)。

本文以差速驅(qū)動(dòng)型 WMR（即XAUT.AGV100）為研究對(duì)象，利用 Hermite多項(xiàng)式曲率變化連續(xù)的特點(diǎn)，提出了基于Hermite插值的移動(dòng)機(jī)器人路徑規(guī)劃方法，在WMR的兩目標(biāo)點(diǎn)間為其進(jìn)行路徑規(guī)劃，使差速驅(qū)動(dòng)型WMR跟蹤規(guī)劃路徑，可保證其運(yùn)動(dòng)的平穩(wěn)性和路徑跟蹤的準(zhǔn)確性，同時(shí)可保證WMR在目標(biāo)點(diǎn)的位姿要求。

1 差速驅(qū)動(dòng)型WMR運(yùn)動(dòng)方程

XAUT.AGV100是一種在平面上運(yùn)動(dòng)的差速轉(zhuǎn)向式WMR，為便于討論，分別建立WMR坐標(biāo)系oaxayaza及其工作空間坐標(biāo)系oxyz，如圖1所示。WMR的位置用坐標(biāo)系oaxayaza原點(diǎn)oa在坐標(biāo)系oxyz中的位置（xa，ya）表示，姿態(tài)用兩坐標(biāo)軸xa和x間的夾角θ表示，即WMR位姿可表示為P＝［x yθ］T。欲控制 WMR以設(shè)定的速度vc沿規(guī)劃路徑f（x，y）＝0運(yùn)動(dòng)，控制其左右驅(qū)動(dòng)輪的運(yùn)動(dòng)速度Uc＝［vlvr］T即可。

圖1 DDWMR航位推算

設(shè)t（i）時(shí)刻，WMR在路徑f（x，y）＝0上，位姿為P（i）＝ ［x（i）y（i）z（i）θ（i）］T，且姿態(tài)與所在軌跡的切線方向一致，為控制WMR跟蹤規(guī)劃路 徑，要 求Uc（i）與 速 度va.c＝ ［vcωc］T滿(mǎn)足：

或

其中，B為兩驅(qū)動(dòng)輪間距。vc（i）與WMR的角速度ωc（i）滿(mǎn)足：

式中，R（i）為 WMR 在路徑f（x，y）＝0上的點(diǎn)（x（i），y（i））處的曲率半徑。

假設(shè)車(chē)體所在路面平整，運(yùn)動(dòng)過(guò)程中車(chē)輪與地面間為純滾動(dòng)，車(chē)體參數(shù)如輪徑、輪距等保持不變，則在t（i＋1）時(shí)刻 WMR的位姿P（i＋1）為

式中，Tc為控制周期。

由式（4）知，根據(jù)WMR當(dāng)前位姿和目標(biāo)位姿，調(diào)節(jié)控制量Uc可使它以要求的位姿到達(dá)目標(biāo)地。

由于對(duì)WMR在目標(biāo)點(diǎn)的位姿有要求，若在兩點(diǎn)間所規(guī)劃的路徑具有曲率連續(xù)變化的特點(diǎn)，則可容易地控制WMR沿規(guī)劃的路徑運(yùn)動(dòng)，并以要求的姿態(tài)到達(dá)目標(biāo)點(diǎn)。

2 基于三次Hermite插值的WMR路徑規(guī)劃方法

Hermite插值方法是常用的插值方法，所得到的Hermite多項(xiàng)式可保證插值曲線通過(guò)起始點(diǎn)和目標(biāo)點(diǎn)，還可保證曲線在這兩點(diǎn)處的切線斜率。若控制DDWMR跟蹤此多項(xiàng)式所表示的路徑，由于路徑段內(nèi)曲率是連續(xù)變化的，這不僅有利于對(duì)WMR的運(yùn)動(dòng)控制，而且可保證WMR以要求的姿態(tài)到達(dá)目標(biāo)點(diǎn)。

設(shè)在對(duì)WMR進(jìn)行路徑規(guī)劃時(shí)，它在坐標(biāo)系oxyz中的位姿為P1＝［x1y1θ1］T，根據(jù)其任務(wù)要求，到達(dá)目標(biāo)點(diǎn)時(shí)的位姿為P2＝［x2y2θ2］T（圖2）。本文利用 Hermite多項(xiàng)式的特點(diǎn)，在滿(mǎn)足其速度、加速度限制的條件下，根據(jù)WMR的起點(diǎn)Ps和目標(biāo)點(diǎn)Pe的位姿信息，采用三次Hermite插值法在路徑節(jié)點(diǎn)間為其規(guī)劃出一條可行的運(yùn)動(dòng)路徑。

圖2 WMR路徑點(diǎn)信息

2.1 Hermite插值曲線

則H2n-1（x）可表示為

Hermite插值多項(xiàng)式為

2.2 路徑曲率及運(yùn)動(dòng)控制量

根據(jù)WMR路徑規(guī)劃時(shí)的條件，已知信息為

由式（8）和式（9）得

由式（7）可得規(guī)劃路徑的曲線方程

式（13）所示的三次Hermite多項(xiàng)式要求在x方向?yàn)閱握{(diào)遞增，同時(shí)，應(yīng)避免在插值點(diǎn)（x1，y1）和（x2，y2）處的切線斜率和為無(wú)窮大，在選擇規(guī)劃路徑的起點(diǎn)和終點(diǎn)時(shí)，應(yīng)滿(mǎn)足｜θ1-θ2｜＜180°。為此，在進(jìn)行路徑規(guī)劃時(shí)，可用路徑起點(diǎn)作為WMR路徑坐標(biāo)系的原點(diǎn)，根據(jù)WMR的起始位姿P1和目標(biāo)位姿P2，由式（11）和式（12）可得路徑坐標(biāo)系下的路徑節(jié)點(diǎn)信息為

由式（13）和式（14）可得規(guī)劃路徑上的插值點(diǎn)（xP，yP）為

在t（i）時(shí)刻，WMR在路徑某點(diǎn)處的位姿PP（i）＝ ［xP（i）yP（i）θP（i）］T，由式（15）可得WMR的運(yùn)動(dòng)路徑曲率為

由式（16）可知，所規(guī)劃的路徑曲線曲率連續(xù)，因此，當(dāng)控制WMR沿此路徑運(yùn)動(dòng)時(shí)，可保證其左右輪速度連續(xù)變化并精確跟蹤路徑。

對(duì)DDWMR路徑跟蹤時(shí)，根據(jù)其運(yùn)動(dòng)速度vc求出 WMR的Uc，即

2.3 規(guī)劃路徑跟蹤仿真

2.3.1 兩點(diǎn)間的路徑規(guī)劃

設(shè) WMR的起始位姿P1＝ ［000°］，目標(biāo)位姿P2＝［10 10 45°］，采用所設(shè)計(jì)的路徑規(guī)劃方法為其規(guī)劃路徑，并控制WMR跟蹤規(guī)劃的路徑，仿真結(jié)果如圖3所示。圖3a所示為WMR跟蹤規(guī)劃路徑時(shí)的運(yùn)動(dòng)軌跡曲線，圖3b所示為WMR跟蹤路徑時(shí)左右輪速度和其姿態(tài)角的變化曲線。

圖3 WMR跟蹤二點(diǎn)一段插值路徑

2.3.2 多點(diǎn)間的路徑規(guī)劃

WMR通常工作在有障礙物的環(huán)境中，因此，運(yùn)動(dòng)過(guò)程中難免要避障。進(jìn)行路徑規(guī)劃時(shí)，在保證其運(yùn)動(dòng)平穩(wěn)性的情況下，還應(yīng)使其以最短路徑安全繞過(guò)障礙物，并準(zhǔn)確到達(dá)目標(biāo)點(diǎn)。不僅要考慮WMR的起始點(diǎn)和目標(biāo)點(diǎn)的的位姿P1和P2，還必須考慮其繞過(guò)障礙物時(shí)的位姿。

設(shè) WMR需從起始點(diǎn)P1＝［8 6 90°］運(yùn)動(dòng)到目標(biāo)點(diǎn)P2＝ ［2 14 180°］，經(jīng)中間點(diǎn)P3＝［8 8 90°］和P4＝ ［6 12 135°］，其路徑規(guī)劃及其速度、加速度的仿真結(jié)果如圖4所示。

圖4 WMR跟蹤四點(diǎn)三段插值路徑

仿真結(jié)果表明，采用基于三次Hermite插值的路徑規(guī)劃方法對(duì)WMR進(jìn)行路徑規(guī)劃，當(dāng)控制其跟蹤該路徑時(shí)，可確保WMR以要求的姿態(tài)到達(dá)目標(biāo)點(diǎn)，提高了WMR的運(yùn)動(dòng)控制精度；同時(shí)，由于WMR在運(yùn)動(dòng)過(guò)程中左右輪速度無(wú)突變，所以提高了其運(yùn)動(dòng)穩(wěn)定性。

3 實(shí)驗(yàn)

為了檢驗(yàn)本文設(shè)計(jì)的路徑規(guī)劃方法的實(shí)用性，在輪式移動(dòng)機(jī)器人XAUT.AGV100上進(jìn)行了跟蹤規(guī)劃路徑實(shí)驗(yàn)。設(shè)AGV從S點(diǎn)以要求的姿態(tài)運(yùn)動(dòng)到E點(diǎn)，設(shè)計(jì)圖5所示的實(shí)驗(yàn)現(xiàn)場(chǎng)。兩標(biāo)識(shí)物間的位置關(guān)系已知。

圖5 實(shí)驗(yàn)現(xiàn)場(chǎng)

為驗(yàn)證AGV到達(dá)目標(biāo)點(diǎn)的準(zhǔn)確性，根據(jù)模擬量超聲波傳感器的檢測(cè)特性，在AGV一側(cè)的前后分裝一傳感器以檢測(cè)其距標(biāo)識(shí)板的距離，在AGV的前方左右兩邊分裝一開(kāi)關(guān)量超聲波傳感器（圖6）。由于開(kāi)關(guān)量超聲波傳感器不具有測(cè)距功能，設(shè)置其遠(yuǎn)距點(diǎn)dcmax＝1500mm，遠(yuǎn)距點(diǎn)dcmin＝500mm。當(dāng)傳感器的輸出信號(hào)發(fā)生第一次變化即AGV到達(dá)T點(diǎn)時(shí)減速；當(dāng)發(fā)生第二次變化即AGV到達(dá)E點(diǎn)時(shí)停止。

圖6 實(shí)驗(yàn)裝置

根據(jù)模擬量超聲波傳感器的測(cè)量值（分別為298mm和302mm）和手工測(cè)量值知，AGV的初始位姿為P1＝［-8-500 0.27°］。AGV在目標(biāo)點(diǎn)E 的位姿為P1＝［5866-1700-30°］。在運(yùn)動(dòng)程序中控制AGV的運(yùn)動(dòng)速度為0.5m／s，加速度為0.5m／s2。采用基于三次Hermite插值的路徑規(guī)劃方法在S點(diǎn)與E點(diǎn)間對(duì)AGV進(jìn)行路徑規(guī)劃并控制其跟蹤該路徑運(yùn)動(dòng)到E點(diǎn)。AGV的運(yùn)動(dòng)軌跡曲線如圖7所示。運(yùn)動(dòng)結(jié)束后，根據(jù)模擬量超聲波傳感器的檢測(cè)值可得AGV中心距離2號(hào)標(biāo)識(shí)物的距離ds和相對(duì)其姿態(tài)角θ；手工測(cè)得AGV車(chē)體前端距離3號(hào)標(biāo)識(shí)物的距離df。實(shí)驗(yàn)結(jié)果如表1所示。

圖7 AGV跟蹤單段三次Hermite曲線軌跡實(shí)驗(yàn)圖

由實(shí)驗(yàn)曲線可知，AGV可平穩(wěn)地運(yùn)動(dòng)至目標(biāo)點(diǎn)。由表1可知，采用所設(shè)計(jì)的路徑規(guī)劃方法對(duì)DDWMR進(jìn)行路徑規(guī)劃，當(dāng)其沿該路徑路徑時(shí)，到達(dá)目標(biāo)點(diǎn)的定位誤差為±3.63mm，姿態(tài)角定位誤差為±0.03°。定位精度可滿(mǎn)足工業(yè)生產(chǎn)中對(duì)AGV定位精度的要求（位置定位精度±25mm，姿態(tài)角定位誤差為±1°）。實(shí)驗(yàn)結(jié)果表明，本文設(shè)計(jì)的基于Hermite插值的路徑規(guī)劃方法可應(yīng)用于AGV的實(shí)際路徑規(guī)劃中。無(wú)論AGV的起始位姿如何，在AGV的運(yùn)動(dòng)能力范圍內(nèi)，AGV能完成其規(guī)劃路徑的跟蹤，并能以要求的位姿平穩(wěn)地運(yùn)動(dòng)到目標(biāo)點(diǎn)。

4 結(jié)束語(yǔ)

本文以自主研制的物料搬運(yùn)型輪式移動(dòng)機(jī)器人XAUT.AGV100為研究對(duì)象，根據(jù)DDWMR的結(jié)構(gòu)特點(diǎn)，針對(duì)其路徑規(guī)劃問(wèn)題進(jìn)行了研究，提出了基于三次Hermite插值的移動(dòng)機(jī)器人局部路徑規(guī)劃方法。將所設(shè)計(jì)的路徑規(guī)劃方法用于XAUT.AGV100，對(duì)其進(jìn)行了局部路徑規(guī)劃與跟蹤控制仿真實(shí)驗(yàn)。仿真結(jié)果表明，所設(shè)計(jì)的局部路徑規(guī)劃方法能較好地滿(mǎn)足WMR速度連續(xù)變化的要求，最重要的是，能夠保證 WMR以要求的位姿到達(dá)目標(biāo)點(diǎn)，無(wú)論其平滑性，還是移動(dòng)機(jī)器人跟蹤路徑時(shí)運(yùn)動(dòng)的平穩(wěn)性等均有明顯的優(yōu)勢(shì)。

在XAUT.AGV100上進(jìn)行了仿真實(shí)驗(yàn)研究，驗(yàn)證了所設(shè)計(jì)的路徑規(guī)劃方法的有效性。實(shí)驗(yàn)結(jié)果驗(yàn)證了仿真結(jié)果的正確性。研究結(jié)果表明，采用所設(shè)計(jì)的路徑規(guī)劃方法對(duì)WMR進(jìn)行路徑規(guī)劃，在保證其以要求的姿態(tài)準(zhǔn)確到達(dá)目標(biāo)點(diǎn)的前提下，有效地提高了WMR在跟蹤路徑過(guò)程中的運(yùn)動(dòng)平穩(wěn)性。

［1］Tan Guanzheng，He Huan，Sloman A.Ant Colony System Algorithm for Real-time Globally Optimal Path Planning of Mobile Robots［J］.Acta Sutomatica Sinica，2007，33（3）：279-285.

［2］Shi Enxiu，Cai Tao，He Changlin.Study of the New Method for Improving Artifical Potential Field in Mobile Robot Obstacle Avoidance［C］／／Proceedings of the IEEE International Conference on Automation and Logistics，Jinan，2007：282-286.

［3］Lebedev D.Neural Network Model for Robot Path Planning in Dynamically Changing Environment［J］.Modeling and Analysis of Information Systems，2001，18（1）：12-18.

［4］Dubins L E.On Curves of Minimal Length with a Constraint on Average Curvature and with Prescribed Initial and Terminal Positions and Tangents［J］.Amer.J.Math.，1957，79（3）：497-517.

［5］Piazzi A，Romano M，Bianco C G L.G3-splines for the Path Planning of Wheeled Mobile Robots［C］／／Proc.2003Eur.Control Conf..Cambridge，2003.

［6］Bianco C G L，Piazzi A，Romano M.Smooth Motion Generation for Unicycle Mobile Robots Via Dynamic Path Inversion［J］.IEEE Transactions on Robotics，2004，20（5）：884-891.

［7］Madhavan S.Path Planning of Multiple Autonomous Vehicles［D］.Crarcfield：Cranfield University，2007.