差速驅動輪式移動機器人路徑規劃新策略

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

1.西安理工大學，西安，710048

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

0 引言

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

本文以差速驅動型 WMR（即XAUT.AGV100）為研究對象，利用 Hermite多項式曲率變化連續的特點，提出了基于Hermite插值的移動機器人路徑規劃方法，在WMR的兩目標點間為其進行路徑規劃，使差速驅動型WMR跟蹤規劃路徑，可保證其運動的平穩性和路徑跟蹤的準確性，同時可保證WMR在目標點的位姿要求。

1 差速驅動型WMR運動方程

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

圖1 DDWMR航位推算

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

或

其中，B為兩驅動輪間距。vc（i）與WMR的角速度ωc（i）滿足：

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

假設車體所在路面平整，運動過程中車輪與地面間為純滾動，車體參數如輪徑、輪距等保持不變，則在t（i＋1）時刻 WMR的位姿P（i＋1）為

式中，Tc為控制周期。

由式（4）知，根據WMR當前位姿和目標位姿，調節控制量Uc可使它以要求的位姿到達目標地。

由于對WMR在目標點的位姿有要求，若在兩點間所規劃的路徑具有曲率連續變化的特點，則可容易地控制WMR沿規劃的路徑運動，并以要求的姿態到達目標點。

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

Hermite插值方法是常用的插值方法，所得到的Hermite多項式可保證插值曲線通過起始點和目標點，還可保證曲線在這兩點處的切線斜率。若控制DDWMR跟蹤此多項式所表示的路徑，由于路徑段內曲率是連續變化的，這不僅有利于對WMR的運動控制，而且可保證WMR以要求的姿態到達目標點。

設在對WMR進行路徑規劃時，它在坐標系oxyz中的位姿為P1＝［x1y1θ1］T，根據其任務要求，到達目標點時的位姿為P2＝［x2y2θ2］T（圖2）。本文利用 Hermite多項式的特點，在滿足其速度、加速度限制的條件下，根據WMR的起點Ps和目標點Pe的位姿信息，采用三次Hermite插值法在路徑節點間為其規劃出一條可行的運動路徑。

圖2 WMR路徑點信息

2.1 Hermite插值曲線

則H2n-1（x）可表示為

Hermite插值多項式為

2.2 路徑曲率及運動控制量

根據WMR路徑規劃時的條件，已知信息為

由式（8）和式（9）得

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

式（13）所示的三次Hermite多項式要求在x方向為單調遞增，同時，應避免在插值點（x1，y1）和（x2，y2）處的切線斜率和為無窮大，在選擇規劃路徑的起點和終點時，應滿足｜θ1-θ2｜＜180°。為此，在進行路徑規劃時，可用路徑起點作為WMR路徑坐標系的原點，根據WMR的起始位姿P1和目標位姿P2，由式（11）和式（12）可得路徑坐標系下的路徑節點信息為

由式（13）和式（14）可得規劃路徑上的插值點（xP，yP）為

在t（i）時刻，WMR在路徑某點處的位姿PP（i）＝ ［xP（i）yP（i）θP（i）］T，由式（15）可得WMR的運動路徑曲率為

由式（16）可知，所規劃的路徑曲線曲率連續，因此，當控制WMR沿此路徑運動時，可保證其左右輪速度連續變化并精確跟蹤路徑。

對DDWMR路徑跟蹤時，根據其運動速度vc求出 WMR的Uc，即

2.3 規劃路徑跟蹤仿真

2.3.1 兩點間的路徑規劃

設 WMR的起始位姿P1＝ ［000°］，目標位姿P2＝［10 10 45°］，采用所設計的路徑規劃方法為其規劃路徑，并控制WMR跟蹤規劃的路徑，仿真結果如圖3所示。圖3a所示為WMR跟蹤規劃路徑時的運動軌跡曲線，圖3b所示為WMR跟蹤路徑時左右輪速度和其姿態角的變化曲線。

圖3 WMR跟蹤二點一段插值路徑

2.3.2 多點間的路徑規劃

WMR通常工作在有障礙物的環境中，因此，運動過程中難免要避障。進行路徑規劃時，在保證其運動平穩性的情況下，還應使其以最短路徑安全繞過障礙物，并準確到達目標點。不僅要考慮WMR的起始點和目標點的的位姿P1和P2，還必須考慮其繞過障礙物時的位姿。

設 WMR需從起始點P1＝［8 6 90°］運動到目標點P2＝ ［2 14 180°］，經中間點P3＝［8 8 90°］和P4＝ ［6 12 135°］，其路徑規劃及其速度、加速度的仿真結果如圖4所示。

圖4 WMR跟蹤四點三段插值路徑

仿真結果表明，采用基于三次Hermite插值的路徑規劃方法對WMR進行路徑規劃，當控制其跟蹤該路徑時，可確保WMR以要求的姿態到達目標點，提高了WMR的運動控制精度；同時，由于WMR在運動過程中左右輪速度無突變，所以提高了其運動穩定性。

3 實驗

為了檢驗本文設計的路徑規劃方法的實用性，在輪式移動機器人XAUT.AGV100上進行了跟蹤規劃路徑實驗。設AGV從S點以要求的姿態運動到E點，設計圖5所示的實驗現場。兩標識物間的位置關系已知。

圖5 實驗現場

為驗證AGV到達目標點的準確性，根據模擬量超聲波傳感器的檢測特性，在AGV一側的前后分裝一傳感器以檢測其距標識板的距離，在AGV的前方左右兩邊分裝一開關量超聲波傳感器（圖6）。由于開關量超聲波傳感器不具有測距功能，設置其遠距點dcmax＝1500mm，遠距點dcmin＝500mm。當傳感器的輸出信號發生第一次變化即AGV到達T點時減速；當發生第二次變化即AGV到達E點時停止。

圖6 實驗裝置

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

圖7 AGV跟蹤單段三次Hermite曲線軌跡實驗圖

由實驗曲線可知，AGV可平穩地運動至目標點。由表1可知，采用所設計的路徑規劃方法對DDWMR進行路徑規劃，當其沿該路徑路徑時，到達目標點的定位誤差為±3.63mm，姿態角定位誤差為±0.03°。定位精度可滿足工業生產中對AGV定位精度的要求（位置定位精度±25mm，姿態角定位誤差為±1°）。實驗結果表明，本文設計的基于Hermite插值的路徑規劃方法可應用于AGV的實際路徑規劃中。無論AGV的起始位姿如何，在AGV的運動能力范圍內，AGV能完成其規劃路徑的跟蹤，并能以要求的位姿平穩地運動到目標點。

4 結束語

本文以自主研制的物料搬運型輪式移動機器人XAUT.AGV100為研究對象，根據DDWMR的結構特點，針對其路徑規劃問題進行了研究，提出了基于三次Hermite插值的移動機器人局部路徑規劃方法。將所設計的路徑規劃方法用于XAUT.AGV100，對其進行了局部路徑規劃與跟蹤控制仿真實驗。仿真結果表明，所設計的局部路徑規劃方法能較好地滿足WMR速度連續變化的要求，最重要的是，能夠保證 WMR以要求的位姿到達目標點，無論其平滑性，還是移動機器人跟蹤路徑時運動的平穩性等均有明顯的優勢。

在XAUT.AGV100上進行了仿真實驗研究，驗證了所設計的路徑規劃方法的有效性。實驗結果驗證了仿真結果的正確性。研究結果表明，采用所設計的路徑規劃方法對WMR進行路徑規劃，在保證其以要求的姿態準確到達目標點的前提下，有效地提高了WMR在跟蹤路徑過程中的運動平穩性。

［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.