,,

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

0 引言

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

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

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

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

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

1.1 人工勢場法

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

圖1 人工勢場法受力模型

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

(1)

(2)

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

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

(3)

(4)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(5)

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

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

圖4 避障軌跡

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

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

2 仿真驗證

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

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

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

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

2.2 仿真對比

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

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

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

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

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

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

圖5 兩種方法路徑比較

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

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

3 結(jié)論

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

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

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