基于路徑預(yù)測人工勢場法的自動跟隨小車路徑規(guī)劃

,,

(1.中汽動力(沈陽)有限公司，沈陽 110153；2.遼寧省實驗中學(xué)，沈陽 110841)

0 引言

近年來，隨著人工智能技術(shù)的快速發(fā)展，自動跟隨車逐步進入人們的視野，具有自動跟隨功能的行李箱、高爾夫球包車等已進入商業(yè)化使用中。路徑規(guī)劃和避障是目前研究的重點，自動跟隨小車在未知環(huán)境中找到一條最優(yōu)的無碰路徑，相對其它路徑可以減少能量損耗、提高跟隨速度。

路徑規(guī)劃是指基于傳感器等設(shè)備感知行駛遇到的障礙物和動態(tài)目標(biāo)，在滿足動力學(xué)、運動學(xué)約束和穩(wěn)定性等條件下實時地為規(guī)劃出從出發(fā)點到目標(biāo)終點的最優(yōu)路徑。目前常用的路徑規(guī)劃方法包括神經(jīng)網(wǎng)絡(luò)、模糊控制、遺傳算法和人工勢場法等[1]。其中，人工勢場法屬于局部規(guī)劃方法，無需對全局進行搜索，計算量小，執(zhí)行效率高，適合實時性要求較高的路徑規(guī)劃，且用該算法規(guī)劃的路徑具有較平滑的特點。

傳統(tǒng)的人工勢場法具有目標(biāo)不可達和局部最小點的問題[2]，文獻[3]通過在斥力勢場函數(shù)中加入目標(biāo)點與被控對象之間的距離作為引入調(diào)節(jié)因子來解決目標(biāo)不可達問題；文獻[4]通過對人工勢場法的引力公式和斥力分力方向進行重新定義，在引力公式中加入速度差與加速度差因子，將斥力分力方向重新定義為與引力方向夾角不大于90°的方向解決局部最小點問題。

以上研究均針對靜態(tài)目標(biāo)和障礙進行，文獻[5]和文獻[6]雖然對動態(tài)目標(biāo)和障礙點進行了路徑規(guī)劃的研究，但研究內(nèi)容主要集中在提高避障效率和運動的平穩(wěn)性方面。本文提出的基于路徑預(yù)測的人工勢場法根據(jù)小車、障礙物和目標(biāo)點的運動軌跡預(yù)測未來三者之間可能的位置關(guān)系，并根據(jù)預(yù)測結(jié)果確定行走路徑的方法可提供優(yōu)化的跟隨路徑并提高了避障能力。本文組織如下：第一節(jié)介紹傳統(tǒng)人工勢場法的基本原理，提出了路徑預(yù)測人工勢場法的模型。第二節(jié)給出了小車與控制系統(tǒng)相關(guān)的部件參數(shù)，并運用MATLAB將本文提出的控制模型與傳統(tǒng)的人工勢場法進行仿真對比。第三節(jié)是結(jié)論部分。

1 基于路徑預(yù)測的人工勢場法模型

1.1 人工勢場法

人工勢場法路徑規(guī)劃最初是由khatib于1986年提出的一種虛擬力法并將其引入到路徑規(guī)劃領(lǐng)域，基本思想是將機器人運動空間環(huán)境視為一個虛擬的勢場模型，目標(biāo)點對機器人產(chǎn)生引力勢場，障礙物對機器人產(chǎn)生斥力勢場，機器人在引力勢場和斥力勢場的合力作用下向目標(biāo)點運動。機器人每行走到一個位置就去計算當(dāng)前位置機器人所受的合力大小以及合力的方向，通過邊行走邊去探測周圍的環(huán)境信息，避開全部障礙物并最終達到目標(biāo)點[7]。

圖1 人工勢場法受力模型

如圖1所示，將目標(biāo)點、障礙物和機器人簡化為質(zhì)點，機器人同時受到來自目標(biāo)點的引力Fatt和來自一個或多個障礙物的斥力Frep，其中引力場和斥力場分別為：

(1)

(2)

式中，katt為引力增益系數(shù)；ρg為機器人與目標(biāo)點距離；krep為斥力增益系數(shù)；ρob為機器人與障礙物的距離；ρ0為障礙物的斥力影響范圍。根據(jù)人工勢場法的定義，引力場隨著機器人與目標(biāo)點距離的增大而增大，斥力場隨機器人與障礙物距離的增大而減小，且當(dāng)障礙物與機器人距離大于ρ0后，斥力場消失。其主要目的是為了當(dāng)空間中存在多個障礙物時，去除遠距離障礙物的影響，從而簡化計算。機器人所受引力和斥力函數(shù)分別為：

Fatt=-▽Uatt=-kattρg▽ρg

(3)

(4)

式中，▽Uatt為引力場負(fù)梯度；▽Urep為斥力場負(fù)梯度；▽ρg為機器人指向目標(biāo)點方向的單位向量，ρob為機器人指向障礙物方向的單位向量。

人工勢場法將機器人路徑規(guī)劃等效成目標(biāo)點和障礙物對機器人的引力和斥力，使路徑平滑，避免了機器人行進過程中速度和角度的劇烈變化，人工勢場法成為機器人路徑規(guī)劃的主要方法之一。

但傳統(tǒng)的人工勢場法存在陷阱區(qū)域、在密集多障礙物點中無法有效確定路徑、機器人在障礙點前振蕩等問題，甚至?xí)霈F(xiàn)在障礙物附近目標(biāo)不可達問題。經(jīng)過改進的人工勢場法解決了靜態(tài)環(huán)境下目標(biāo)不可達和局部極小值的問題[5]，但這些方法對動態(tài)目標(biāo)點和障礙物的路徑規(guī)劃還不完善。

1.2 移動目標(biāo)點和移動障礙物的影響

人工勢場法對靜態(tài)目標(biāo)點和障礙物的環(huán)境具有良好的路徑規(guī)劃效果，而自動跟隨小車具有目標(biāo)點和障礙物移動的特點，如圖2所示目標(biāo)點進行勻速直線運動時，采用傳統(tǒng)人工勢場法雖然可實現(xiàn)到達目標(biāo)點附近的功能，但其規(guī)劃的路徑為一段弧線，而最優(yōu)路徑應(yīng)為自動跟隨小車初始坐標(biāo)與相遇點坐標(biāo)的直線連接線，傳統(tǒng)人工勢場發(fā)規(guī)劃的路徑顯然長于最優(yōu)的路徑；而圖3所示障礙物進行運動時，自動跟隨小車由于受到障礙物斥力的作用，運動路徑發(fā)生了明顯的變化，在這種情況下采用傳統(tǒng)人工勢場法規(guī)劃的路徑并非最優(yōu)路徑，且存在目標(biāo)不可達的可能。

圖2 移動目標(biāo)下運動軌跡 圖3 移動障礙物下運動軌跡

圖2和圖3分別為目標(biāo)點水平移動和障礙物水平移動情況下采用傳統(tǒng)人工勢場法自動跟隨小車的運動軌跡，目標(biāo)點和障礙物運動速度取值為自動跟隨小車運動速度的1/2，katt=15，krep=5，ρ0=3.5。

1.3 基于路徑預(yù)測的人工勢場法

自動跟隨小車中目標(biāo)點為自動跟隨小車主人，移動障礙物一般為行人，很多關(guān)于行人路徑預(yù)測的研究表明當(dāng)行人在行走時，都有一個初步的目的地。他們在行走過程中，根據(jù)當(dāng)時的環(huán)境選擇不同路線到達目的區(qū)域[8]。雖然行走路線并非是一成不變，但一般情況下，一段時間內(nèi)行人速度和方向不會出現(xiàn)大的變化。

本文提出的基于路徑預(yù)測的路徑規(guī)劃方法基本思想是：根據(jù)目標(biāo)點、障礙點及自動跟隨小車之間的位置坐標(biāo)和相對運行速度，提前計算出抵達目標(biāo)點或進入障礙點斥力場范圍內(nèi)時三者之間的位置關(guān)系，并根據(jù)這一關(guān)系實時修正自動跟隨小車的路徑，達到以近似最優(yōu)路徑躲避障礙、快速抵達目標(biāo)點的控制目標(biāo)。

對于目標(biāo)點移動的情況，自動跟隨小車的路徑規(guī)劃較為簡單，只需根據(jù)目標(biāo)點和自動跟隨小車之間的距離、運動速度和方向預(yù)測出二者相遇時相遇點的坐標(biāo)，并以該坐標(biāo)為自動跟隨小車行進的目標(biāo)點，應(yīng)用傳統(tǒng)人工勢場法進行路徑規(guī)劃，如圖2(a)所示。

對于移動障礙物的路徑規(guī)劃較為復(fù)雜，文獻[6]提出增加以障礙點、機器人的移動速度變量，將機器人到目標(biāo)點的相對距離建模于排斥勢場中對斥力場進行修正的方法。這個方法在避免了局部極小值，解決了目標(biāo)點不可達的問題。但該方法計算較為復(fù)雜，難以實現(xiàn)實時的路徑規(guī)劃，且規(guī)劃路徑與最優(yōu)路徑相比差距較大。基于路徑預(yù)測的路徑規(guī)劃方法躲避障礙點的實現(xiàn)過程如下：

在直角坐標(biāo)系第一象限中設(shè)定目標(biāo)點坐標(biāo)(xd,yd)和自動跟隨小車初始坐標(biāo)(xr，yr),且yr

若滿足公式(5),則自動跟隨小車自點R(t0)開始沿障礙點斥力場邊界右側(cè)切線方向運動；否則，小車沿障礙點斥力場邊界左側(cè)切線方向運動，直至小車所受全部斥力(其他障礙點)與引力的合力方向與x軸夾角大于障礙點運動方向與x軸夾角后，再沿用傳統(tǒng)人工勢場法。

(5)

式中，θ為自動跟隨小車運行至斥力場邊界時，障礙點運行軌跡與自動跟隨小車和障礙點連線之間的夾角，Vr為小車運行速度，Va為障礙點運行的平均速度。

由于實時求取全部障礙點斥力場邊界切線方程的計算量較大，公式(5)通過障礙點運行速度、自動跟隨小車運行速度及二者運行方向的夾角近似計算左側(cè)路徑和右側(cè)路徑的長度，從而進行路徑的選擇，如圖4(a)所示，當(dāng)自動跟隨小車運行至R(t0)時所選擇的路徑示意，R(t0)-L為左側(cè)路徑，R(t0)-R為右側(cè)路徑。圖4(b)所示為左側(cè)繞過路徑，其中圓C0、C1、C2、C3的圓心為相鄰的4個時刻障礙點位置，圓周代表斥力場的邊界，C0-Cn為障礙點運行的方向。t0時刻，自動跟隨小車首先以R(t0)為起點，沿C0左側(cè)切線方向行進至R(t1)；t1時刻，自動跟隨小車以R(t1)為起點，沿C1左側(cè)切線方向行進至R(t2)；t2時刻，自動跟隨小車以R(t2)為起點，沿C2左側(cè)切線方向行進至R(t3)；t3時刻，自動跟隨小車以R(t3)為起點，沿C3左側(cè)切線方向行進至R(t4)，此時點R(t4)滿足切換為傳統(tǒng)人工勢場法的條件，采用傳統(tǒng)人工勢場法繼續(xù)向目標(biāo)點D行進。圖4(c)所示為自動跟隨小車右側(cè)繞行的路徑示意，運行軌跡的規(guī)劃方法參照自動跟隨小車左側(cè)繞行的路徑規(guī)劃。

圖4 避障軌跡

自動跟隨小車對于動態(tài)障礙點的避讓，小車始終處于斥力場邊界外側(cè)，保證較短路徑的前提下不發(fā)生碰撞。

在大障礙物的情況下，只需以障礙物邊界點為障礙點，以全部障礙點斥力場邊界的包絡(luò)線為障礙點斥力場邊界進行計算即可。在多障礙點的情況下，可認(rèn)為稀疏多障礙點對自動跟隨小車路徑規(guī)劃的影響為單個障礙點對自動跟隨小車路徑規(guī)劃的簡單疊加；密集多障礙點會對路徑預(yù)測產(chǎn)生較大的影響，實時預(yù)測每一個障礙點在每一時刻的坐標(biāo)的計算量非常大，對于一般的嵌入式系統(tǒng)難以滿足計算的要求，因此，在密集多障礙點的情況下，必須對多障礙點的計算進行簡化，簡化方法為：各有效障礙點對自動跟隨小車與目標(biāo)點連線做垂線，選取垂足距離自動跟隨小車最近且垂線長度滿足可能發(fā)生碰撞的條件時，以該障礙點為唯一障礙點，沿用上文所述的單個障礙點避障方法，如不滿足碰撞條件，則定義該障礙點為無效障礙點，并沿自動跟隨小車與目標(biāo)點連線選取下一個障礙點進行計算，直至找到滿足條件的障礙點為止。

2 仿真驗證

2.1 自動跟隨小車主要參數(shù)及性能指標(biāo)

表1 小車主要參數(shù)

以上參數(shù)的選取可滿足的性能指標(biāo)指標(biāo)如表2。

表2 整車性能指標(biāo)

2.2 仿真對比

根據(jù)自動跟隨小車參數(shù)及目標(biāo)點、障礙點的特征，仿真參數(shù)設(shè)定如表3所示。

表3 仿真設(shè)定參數(shù)

在實際應(yīng)用中，受到自動跟隨小車、驅(qū)動電機、動力電池等特性的限制，自動跟隨小車車速存在上限，因此表3中增加定義了引力場作用半徑，具體含義為：當(dāng)目標(biāo)點與自動跟隨小車距離大于引力場作用半徑時，自動跟隨小車所受引力為一定值，運行速度為自動跟隨小車最大速度VRmax，當(dāng)目標(biāo)點與自動跟隨小車距離小于引力場作用半徑時，自動跟隨小車運行速度符合傳統(tǒng)人工勢場法定義。

動態(tài)目標(biāo)點情況下自動跟隨小車運行參照圖2所示，自動跟隨小車每運動一步后，根據(jù)自動跟隨小車與目標(biāo)點之間的距離和速度對相遇點坐標(biāo)進行一次計算，并根據(jù)相遇點坐標(biāo)和自動跟隨小車運行速度確定下一步的運行方向。

動態(tài)障礙點下，自動跟隨小車的運行軌跡規(guī)劃較為復(fù)雜，本文的仿真工作主要圍繞動態(tài)障礙點進行。

圖5表示障礙點移動過程中采取路徑預(yù)測的人工勢場法與傳統(tǒng)人工勢場法自動跟隨小車路徑的比較，仿真中設(shè)定障礙點運行速度為1～2 m/s間的隨機數(shù)據(jù)，障礙點運行方向與X軸夾角為0～π/3間的隨機數(shù)據(jù)。圖5(a)為障礙點初始坐標(biāo)(4.2，3)時自動跟隨小車運行軌跡，圖5(b)所示為障礙點初始坐標(biāo)為(4.1，3)時自動跟隨小車運行軌跡。采用本文提出的路徑預(yù)測人工勢場法時，由于提前預(yù)測障礙點運行軌跡及自動跟隨小車與障礙點發(fā)生碰撞的坐標(biāo)，自動跟隨小車采取提前避讓的方式，并根據(jù)公式(5)和本文提到的避讓方法確定避讓路徑。圖5中可以看到，采取路徑預(yù)測人工勢場法的自動跟隨小車明顯提前于傳統(tǒng)人工勢場法的自動跟隨小車到達目標(biāo)點。

圖5 兩種方法路徑比較

為了能夠更加清晰的表明路徑預(yù)測人工勢場法的工作過程，圖6將整個避讓和接近目標(biāo)點的過程分為六個關(guān)鍵點，圖6(1)為采用路徑預(yù)測人工勢場法的的自動跟隨小車預(yù)測到發(fā)生碰撞后，開始向左前方沿障礙點斥力場邊界切線運動；圖6(2)為傳統(tǒng)人工勢場法進入斥力場后，受斥力場和引力場的合力作用，路徑開始向右側(cè)偏移；圖6(3)為路徑預(yù)測人工勢場法的自動跟隨小車從障礙點左側(cè)繞過障礙點后，只受目標(biāo)點作用力，開始向目標(biāo)點直線運動，而此時采用傳統(tǒng)人工勢場法的自動跟隨小車仍未繞過障礙點；圖6(4)為采用傳統(tǒng)人工勢場法的自動跟隨小車?yán)@過障礙點時障礙點及自動跟隨小車的位置；圖6(5)為自動跟隨小車遠離障礙點，向目標(biāo)點移動；圖6(6)為采用路徑預(yù)測人工勢場法的自動跟隨小車抵達目標(biāo)點附近而采用傳統(tǒng)人工勢場法的自動跟隨小車與目標(biāo)點仍有一定距離。

圖6 左側(cè)避障軌跡分時示意圖

3 結(jié)論

針對傳統(tǒng)人工勢場法在動態(tài)目標(biāo)點和動態(tài)障礙點環(huán)境下路徑規(guī)劃不完善的問題，提出了基于路徑預(yù)測的人工勢場法，根據(jù)預(yù)先預(yù)測的目標(biāo)點和障礙物的運行軌跡確定跟隨路徑，尤其是在動態(tài)障礙點的環(huán)境下，提出了采用沿斥力場左右邊界運動的方法，并給出了選擇左側(cè)或右側(cè)切線運動的公式。

結(jié)合某自動跟隨小車的實際參數(shù)，與傳統(tǒng)人工勢場法進行仿真對比，結(jié)果顯示采用基于路徑預(yù)測的人工勢場法的自動跟隨小車行走路徑在具備傳統(tǒng)人工勢場法計算簡單、路徑平滑等特點的基礎(chǔ)上，跟隨目標(biāo)點及避障能力顯著提高，避讓路徑長度比傳統(tǒng)人工勢場法減少10%以上。

下一步主要還需完成自動跟隨小車跟隨試驗、密集多障礙點及大障礙物避讓策略的仿真驗證及試驗等。