牛作碩 宮金良 張彥斐

1(山東理工大學(xué)機械工程學(xué)院 山東 淄博 255000) 2(山東理工大學(xué)農(nóng)業(yè)工程與食品科學(xué)學(xué)院 山東 淄博 255000)

0 引 言

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

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

1 傳統(tǒng)純追蹤模型原理

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

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

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

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

(1)

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

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

y2=f(x2)

(2)

(3)

(4)

(5)

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

σ=arctan(2lp·cosγ)

(6)

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

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

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

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

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

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

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

2.2 純追蹤模型的應(yīng)用改進

圖5 追蹤模型鄰接狀態(tài)示意圖

圖6 直線路段補償示意圖

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

3 實 驗

3.1 機器人實驗平臺

如圖7所示，本文設(shè)計的送餐機器人為一款自行設(shè)計的后驅(qū)四輪機器人，車體前后軸距為1.2 m，橫向?qū)挾葹?.7 m，最小轉(zhuǎn)彎半徑為1 m，采用前輪舵機轉(zhuǎn)向系統(tǒng)，并安裝角度傳感器實時測量前輪轉(zhuǎn)向角度。

圖7 智能送餐機器人

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

3.2 路徑追蹤實驗

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

圖8 全局送餐路徑

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

表1 實驗數(shù)據(jù)

圖9 實驗一

圖10 實驗二

圖11 實驗三

圖12 實驗四

通過重復(fù)實驗和采集整理數(shù)據(jù)后可以看出，由于機器人轉(zhuǎn)向結(jié)構(gòu)存在一定誤差和轉(zhuǎn)彎過程中會出現(xiàn)車體的側(cè)滑、動作延遲等現(xiàn)象，理想路徑和機器人實際路徑存在一定的滯后性和一定的偏差，在車體到達預(yù)設(shè)路徑時，航向角偏差在5°以內(nèi)，橫向偏差在0.1 m以內(nèi)，前向位移在6 m以內(nèi)，總體滿足實際應(yīng)用效果?？梢钥闯龈倪M后的純追蹤模型可以在機器人避障等原因偏離預(yù)設(shè)路線較大范圍時，在較短的前向位移內(nèi)使機器人迅速、準確地返回原路線繼續(xù)執(zhí)行任務(wù)。

4 結(jié) 語

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