基于多路線追蹤的機器人局部路徑規劃與實驗

牛作碩 宮金良 張彥斐

1(山東理工大學機械工程學院 山東 淄博 255000) 2(山東理工大學農業工程與食品科學學院 山東 淄博 255000)

0 引 言

路徑規劃是智能行駛機器人研究的重要問題之一，較好的路徑規劃方式可以讓機器人平穩且快速地按照規劃路線進行移動，以執行其要完成的相關任務。智能行駛機器人的路徑規劃可分為全局路徑規劃和局部路徑規劃，其中全局路徑規劃為機器人制定最優預設路線，并通過局部路徑規劃控制機器人的實時動作，完成機器人行走軌跡和預設路徑的擬合工作。目前，國內外針對路徑跟蹤的方法已有大量研究，主要包括智能控制、最優控制、PID控制和純追蹤模型等方法。純追蹤模型作為其中較為簡便的方法已被廣泛研究使用。唐小濤等[1]針對自動導航插秧機路徑跟蹤系統，提出了一種利用模糊控制調整純追蹤模型前視距離的路徑跟蹤方法；汪小旵等[2]基于Kinect體感感應技術，結合純追蹤模型設計了一套溫室果蔬采摘運輸自動跟隨平臺；張攀等[3]采用偏差統計等方法對純追蹤算法進行改進，提高了飛機地面自動牽引滑行導航控制系統的精度。純追蹤模型算法的研究和改進可以提高機器人的控制精度，對智能機器人的發展具有重要的意義。

作為一種幾何模型算法，純追蹤模型在使用時的路徑擬合效果主要由前視距離決定。前視距離是指車輛在正常行駛過程中，車輛目前所在位置與要到達的目標點的直線距離。在建立數學模型時，前視距離需要根據車體與預設路徑的航向角偏差和橫向距離偏差選取合適的數值，受機器人啟動時車體位置和姿態不同的影響[4-5]，如果設定的前視距離過小，機器人會因與預設路徑航向角偏差較大而引起抖動現象，設定較大的前視距離雖然可以減小航向角偏差，但同時會引起機器人走捷徑現象，導致路線的擬合程度降低。針對上述問題，本文提出一種雙段式純追蹤模型，對已有的純追蹤模型的應用進行了改進，在機器人位于初始狀態橫向距離偏差和航向角偏差較大時，能夠減小前視距離的取值，從而提高純追蹤模型的路徑擬合程度，并以一款自行設計的智能送餐機器人為研究對象，結合嵌入式控制系統和MATLAB建模，對改進后的應用效果進行實地實驗和對比仿真。

1 傳統純追蹤模型原理

本文使用的智能送餐機器人采用后輪驅動、前輪轉向的工作方式，通過矩形化轉向梯形設計實現阿克曼轉向功能[6]，機器人結構簡化模型如圖1所示。其中：A點為車體后軸中心點；l為機器人軸距；U點為機器人在轉彎時后輪軸線延長線與前輪軸線延長線的交點，此時機器人以U點為圓心進行旋轉前進；r為旋轉圓弧半徑，有lUA=r。

圖1 智能送餐機器人簡化模型

如圖2所示，對符合阿克曼轉向的送餐機器人車輛模型進行二輪車輛模型簡化[7]。以車輛預設定路徑為Y軸建立送餐機器人坐標系，其中：β角為機器人在運行過程中整體車身轉向偏角；σ角為車體與預設路線走向的航向偏差角。以車身后軸中心點為車輛運動參考點M(x1,y1)，其橫坐標值x1等于機器人車體距離預設定路徑的橫向偏差值d。由前視距離p(p>x1)取合適的長度后得到車體在偏離路線狀態下要到達的預設路徑上的目標點N。σ值根據機器人沿預設路徑的航向逆時針旋轉時取正值，順時針旋轉時取負值。根據圖2中幾何關系有：

圖2 智能送餐機器人二輪車輛模型

(1)

如圖3所示,以M點為原點，機器人車身行駛方向為Y軸建立坐標系，根據純追蹤模型原理，機器人的轉彎半徑可以由機器人與目標點的位置關系求得，有：

圖3 智能送餐機器人純追蹤模型

y2=f(x2)

(2)

(3)

(4)

(5)

式(2)中函數f為預設定路徑的幾何函數，聯立式(2)和式(3)可求得在前視距離為p時的N點坐標(x2,y2)，代入式(1)和式(4)，可得機器人此時需要的轉向偏角為：

σ=arctan(2lp·cosγ)

(6)

即送餐機器人按照此偏角進行轉向即可達到預定路徑上的目標點。

2 純追蹤模型的基本原理與改進2.1 傳統追蹤模型的局限性分析

機器人在初始狀態未處于預設路徑的情況下，通過建立純追蹤模型，選取合適的前視距離并為機器人設定合適的轉向角，機器人即可圍繞固定圓心前進至目標點[8]。本文所設計的智能送餐機器人在送餐過程中，會涉及到躲避障礙物等多種任務執行情況，在執行完避障等任務后，機器人會重新置于路徑追蹤初始狀態，并與預設路徑會有一定的航向角偏差和橫向距離偏差。

如圖4所示，在常規情況下，機器人與預設路線航線角偏差和橫向距離偏差較小或航向角偏差較大，但機器人當前航向呈靠近預設路線趨勢時，使用傳統純追蹤模型規劃路線可快速擬合預設路徑。

(a)常規情況 (b)初始航向角偏差過大 (c)初始橫向偏差過大圖4 不同初始狀態追蹤模型對比

當機器人與預設路線橫向偏差距離大于兩個車體轉彎半徑距離或車體呈偏離預設路線趨勢且航向角介于0°～45°之間時，若前視距離取值較小，會出現到達目標點時機器人車體航向偏差角過大現象，繼而導致機器人路線異常抖動現象；選取前視距離較大，航向角偏差減小的同時，卻會出現機器人走捷徑現象，路徑擬合程度下降，此時傳統純追蹤模型無法滿足機器人能夠良好追蹤路徑的要求，不再適用。當車體呈偏離預設路線趨勢且航向角大于45°時，車體通過傳統路徑追蹤到達預設路徑時，航向角偏差將大于90°，與基準航向呈相反趨勢，無法繼續追蹤路徑。

以機器人到達目標路徑點時航向與預設路徑航向相同為機器人進行路徑追蹤的最優規劃結果[9-10]，針對上述現象，本文提出一種兩段式純追蹤模型，通過兩次追蹤實現減小前視距離的取值和減小到達目標點時的航向角偏差，從而提高路徑追蹤的準確性。

2.2 純追蹤模型的應用改進

圖5 追蹤模型鄰接狀態示意圖

圖6 直線路段補償示意圖

由圖5與圖6中幾何關系知：s2=p2-d2，當d固定時，機器人前向位移與前視距離正相關,即s為最小值時，機器人前視距離p達到最小。為便于對比和計算，使用前向位移p代替前視距離作為路徑擬合程度參考對改進追蹤模型進行分析。由圖5中幾何關系可知，s=r1+r2·(1+sinσ)，在滿足車輛最小轉彎要求情況下，令r1=r2=r，可得機器人當前初始姿態下s最小值為r·(2+sinσ)，若機器人處于臨界位置，全局最小值為2r；當機器人初始航向與X軸同向時，可得s全局最大值為3r。

3 實 驗

3.1 機器人實驗平臺

如圖7所示，本文設計的送餐機器人為一款自行設計的后驅四輪機器人，車體前后軸距為1.2 m，橫向寬度為0.7 m，最小轉彎半徑為1 m，采用前輪舵機轉向系統，并安裝角度傳感器實時測量前輪轉向角度。

圖7 智能送餐機器人

機器人使用北斗/GNSS高精度差分定位系統與導航控制器對車輛整體進行位置信息采集。機載北斗/GNSS定位系統由上海思南衛星導航技術有限公司提供，組成部分包括基站和移動站兩部分，其中基站由一個M300接收機、一個定位天線、一套電臺發送設備組成，架設在校園空曠處；移動站由一臺M600接收機、兩個定位天線、一個電臺信號接收天線組成，安裝在移動機器人本體。在正常工作時，基站通過定位天線獲取實時站點坐標，與預設固定參考坐標做差后獲得實時差分信號，并通過電臺設備廣播發送，差分信號由移動站接收后與機器人坐標做誤差補償可得到機器人精準坐標，定位精度可達2 cm，從而求得機器人與預設路徑的橫向偏差；通過移動站兩個定位天線接收信號對比即可獲取機器人車體航向角,精度可達0.1°，數據傳輸頻率設置為5 Hz，繼而求得機器人與預設路徑航向角偏差。

3.2 路徑追蹤實驗

實驗選取山東理工大學為測試環境進行路徑規劃，考慮到校園內路況比較平坦，忽略車體在運行過程中的水平落差。如圖8所示，首先需要對實驗路段進行全局路徑規劃，并采集路段關鍵節點坐標，制定預設路徑，得到世界坐標系中預設路徑位置以及預設路徑的航向角，并建立數據庫，機器人運行時將以數據庫中路徑信息為基準進行純追蹤模型路徑追蹤，通過北斗/GNSS高精度差分定位系統得到機器人實時位置與航向角后對比數據庫數據，得到實時橫向偏差與航向偏差角。為得到理論追蹤軌跡與機器人實際運行路線偏差，將帶有標號的紙質標簽固定在道路理論路線特征點位置，同時使用墨盒漏斗裝載在機器人后軸中心位置進行運行路徑記錄。

圖8 全局送餐路徑

根據校園交通狀況設定機器人無障礙運行時速度為恒速v=1.5 m/s，當車體橫向偏差穩定在0.2 m內時，機器人達到誤差穩定狀態。選取NVIDIA JETSON NANO B為實驗的主控硬件，正點原子STM32單片機做底層控制，在Linux操作系統環境下，使用C++圖形用戶界面開發軟件QT進行程序編寫，通過機器人操作系統ROS(Robot Operating System)建立車體與工作環境模型，實時計算車體運行偏差狀態并加以顯示記錄，以具有感知能力的人群為校園送餐的主要避障對象，設計機器人執行避障任務后的四種運行狀態，路徑追蹤實驗和實驗結果如表1和圖9-圖12所示。

表1 實驗數據

圖9 實驗一

圖10 實驗二

圖11 實驗三

圖12 實驗四

通過重復實驗和采集整理數據后可以看出，由于機器人轉向結構存在一定誤差和轉彎過程中會出現車體的側滑、動作延遲等現象，理想路徑和機器人實際路徑存在一定的滯后性和一定的偏差，在車體到達預設路徑時，航向角偏差在5°以內，橫向偏差在0.1 m以內，前向位移在6 m以內，總體滿足實際應用效果。可以看出改進后的純追蹤模型可以在機器人避障等原因偏離預設路線較大范圍時，在較短的前向位移內使機器人迅速、準確地返回原路線繼續執行任務。

4 結 語

為了解決智能送餐機器人使用純追蹤模型規劃路徑時，由于初始位置航向偏差角和橫向偏差距離較大導致機器無法良好擬合預設路線的問題，本文對智能機器人進行了簡化建模和路徑規劃分析，提出使用兩次純追蹤模型的方式，對傳統純追蹤模型進行了改進。改進后的追蹤模型在使用先后兩次相切路徑追蹤后，在保證機器人到達目標點后與預設路徑航向相近的同時，減小了前視距離的取值，提高了路徑追蹤的擬合程度。使用自行設計的四輪智能送餐機器人在校園內進行實地實驗，設計四種初始運行狀態并進行多次實驗，通過記錄機器人運行路徑和理論路徑進行對比，得到機器人在使用改進純追蹤模型到達目標點航向角偏差在5°以內，橫向偏差小于0.1 m，完全滿足路徑規劃需要，從而證明了本文算法能夠為機器人局部路徑規劃提供一種有效的追蹤方法，提高了其路徑追蹤的穩定性和有效性。