基于主動(dòng)安全的改進(jìn)人工勢場局部路徑規(guī)劃研究

張三川， 明 珠

(鄭州大學(xué) 機(jī)械與動(dòng)力工程學(xué)院，河南 鄭州 450001)

0 引言

局部路徑規(guī)劃是解決車輛主動(dòng)安全問題的重要途徑之一[1]，具有實(shí)時(shí)性強(qiáng)、計(jì)算效率高的顯著特點(diǎn)[2]。目前局部路徑規(guī)劃算法主要有A*算法[3]、遺傳算法[4]、蟻群算法[5]、粒子群算法[6]和人工勢場法[7]等，其中人工勢場算法具有結(jié)構(gòu)簡單、信息處理速度快、實(shí)時(shí)性好、路徑光滑等優(yōu)點(diǎn)。但傳統(tǒng)人工勢場法容易出現(xiàn)局部極小值和目標(biāo)不可達(dá)等問題[8]，國內(nèi)外學(xué)者對(duì)此提出了不同的方法加以改進(jìn)。Fedele等[9]引入了螺旋勢場，利用目標(biāo)引力場和螺旋勢場之間的切換策略，使實(shí)驗(yàn)小車能夠繞過障礙物。Rostami等[10]通過尋找成本函數(shù)，引入調(diào)節(jié)器對(duì)斥力函數(shù)加以修正，成功解決了局部極小值問題。Raheem等[11]應(yīng)用粒子群優(yōu)化算法尋找人工勢場因子的最佳值，提出了基于迭代優(yōu)化的改進(jìn)混合路徑規(guī)劃方法。郭梟鵬[12]引入廣義虛擬勢場概念，構(gòu)造了一種使實(shí)驗(yàn)車輛沿勢場函數(shù)下降方向運(yùn)動(dòng)的勢場模型。徐小強(qiáng)等[13]針對(duì)自行式機(jī)器人，基于安全距離概念，引入預(yù)測距離算法使機(jī)器人陷入局部極小值之前能做出反應(yīng)，并用設(shè)置虛擬的目標(biāo)點(diǎn)的方法來引導(dǎo)機(jī)器人脫離極小值。宣仁虎[14]提出了修正勢場，對(duì)人工勢場法存在的目標(biāo)不可達(dá)和局部極小值問題引入逃逸力進(jìn)行改進(jìn)。劉志強(qiáng)等[15]建立了道路邊界危險(xiǎn)斥力模型，并在障礙物斥力場中引入速度因素，通過求解平衡方程來獲取安全路徑。

鑒于相關(guān)文獻(xiàn)對(duì)傳統(tǒng)人工勢場理論改進(jìn)方式較為單一，本文擬引入調(diào)節(jié)因子與附加力，從兩個(gè)方面對(duì)傳統(tǒng)人工勢場加以改進(jìn)；同時(shí)，綜合各改進(jìn)算法的特點(diǎn)，建立以時(shí)效性與安全性為主要評(píng)價(jià)指標(biāo)的價(jià)值函數(shù)模型，以期為自動(dòng)駕駛技術(shù)開發(fā)提供基礎(chǔ)理論支持。

1 改進(jìn)人工勢場

1.1 傳統(tǒng)人工勢場

當(dāng)實(shí)驗(yàn)車位于路徑規(guī)劃區(qū)間時(shí)，會(huì)受到如圖1所示的障礙物斥力和目標(biāo)點(diǎn)引力，并始終在引力和斥力的耦合作用下實(shí)現(xiàn)避障前行。目標(biāo)點(diǎn)產(chǎn)生的引力勢場Uatt(M,Mg)用實(shí)驗(yàn)車與目標(biāo)點(diǎn)之間距離S(M,Mg)和引力增益系數(shù)α來表達(dá)：

(1)

圖1 傳統(tǒng)人工勢場Figure 1 Traditional artificial potential field

本文為純數(shù)學(xué)模型，除仿真時(shí)長外所有參數(shù)均無量綱。

目標(biāo)點(diǎn)對(duì)實(shí)驗(yàn)車所產(chǎn)生的引力方向始終指向目標(biāo)點(diǎn)，其大小為引力勢場(式(1))的負(fù)梯度：

Fatt(M)=-gradUatt(M,Mg)=-αS(M,Mg)。

(2)

當(dāng)實(shí)驗(yàn)車輛進(jìn)入障礙物作用范圍內(nèi)時(shí)，障礙物會(huì)對(duì)其產(chǎn)生斥力，斥力場函數(shù)Urep為

(3)

式中：β為斥力增益系數(shù)；A為斥力作用的判斷參數(shù)，計(jì)算式為

(4)

式中：S(M,Mobs)為實(shí)驗(yàn)車與障礙物之間的距離；S0為斥力場作用半徑。

實(shí)驗(yàn)車所受斥力Frep(M)為斥力場的負(fù)梯度，大小為

(5)

若實(shí)驗(yàn)車受到多個(gè)障礙物的同時(shí)作用，其所受總斥力為各障礙斥力的矢量和：

(6)

顯然，傳統(tǒng)人工勢場下的實(shí)驗(yàn)車所受到的引力與斥力的合力為

(7)

如圖2(a)所示，當(dāng)障礙物與目標(biāo)點(diǎn)間的距離過于接近時(shí)，目標(biāo)點(diǎn)產(chǎn)生的引力可能會(huì)小于斥力，此時(shí)，合力方向與前進(jìn)目標(biāo)點(diǎn)方向相反，即出現(xiàn)實(shí)驗(yàn)車反而遠(yuǎn)離目標(biāo)點(diǎn)的不可達(dá)問題。如圖2(b)所示，當(dāng)實(shí)驗(yàn)車、障礙物和目標(biāo)點(diǎn)3者共線時(shí)，引力與斥力還可能相互抵消而出現(xiàn)合力為0的情況，出現(xiàn)局部極小值問題。

圖2 傳統(tǒng)人工勢場的2種特殊情形Figure 2 Two special cases of traditional artificial potential field

1.2 改進(jìn)人工勢場

為了解決上述問題，定義目標(biāo)點(diǎn)—實(shí)驗(yàn)車的連線與障礙物—實(shí)驗(yàn)車的連線之間夾角為θ0(可由車載毫米波雷達(dá)測得)，在障礙物的異側(cè)增設(shè)一個(gè)附加力Fadd，其與目標(biāo)點(diǎn)—實(shí)驗(yàn)車的連線間的夾角為θ，實(shí)驗(yàn)車在任一點(diǎn)位的受力如圖3所示。

圖3 改進(jìn)人工勢場Figure 3 Improved artificial potential field

(1)目標(biāo)不可達(dá)問題。障礙物斥力隨S(M,Mobs)的減小而增大是實(shí)驗(yàn)車目標(biāo)不可達(dá)的根本原因，由式(5)可知，實(shí)驗(yàn)車與障礙物的距離S(M,Mobs)越近，斥力越大，因此可以選用實(shí)驗(yàn)車到目標(biāo)點(diǎn)之間距離S(M,Mg)作為距離調(diào)節(jié)因子對(duì)斥力計(jì)算式加以數(shù)值大小上的修正，改進(jìn)后的斥力大小為

(8)

從式(8)可以看出，隨實(shí)驗(yàn)車與障礙物間的距離的減小而斥力增大的趨勢，因引入呈恒定減小的距離調(diào)節(jié)因子而得到抑制，可保證實(shí)驗(yàn)車所受的合力方向始終與前進(jìn)目標(biāo)方向大體一致，從而避免了局部路徑規(guī)劃中的目標(biāo)不可達(dá)問題的出現(xiàn)。

(2)局部極小值問題。目標(biāo)點(diǎn)引力與障礙物斥力的合力為0是局部極小值問題出現(xiàn)的關(guān)鍵。因此，為保證車輛不陷入局部路徑規(guī)劃中的極小值點(diǎn)，引入一個(gè)由θ角(限定θ>θ0)與S(M,Mg)共同控制的附加力Fadd(M)：

Fadd(M)=k·S(M,Mg)cosθ。

(9)

式中：k為大于0的附加力增益系數(shù)，其取值大小對(duì)規(guī)劃的局部路徑長短有影響。

顯然，由式(9)可見，即使實(shí)驗(yàn)車無限小距離接近目標(biāo)點(diǎn)，以及雷達(dá)探測角θ0為0時(shí)，附加力也將始終不為0，故既可保證有效避開障礙物，又可在到達(dá)目標(biāo)點(diǎn)處時(shí)衰減為0。

由此，實(shí)驗(yàn)車在改進(jìn)人工勢場中所受的合力可由式(7)改寫為

(10)

2 算法設(shè)計(jì)與評(píng)價(jià)

2.1 算法設(shè)計(jì)

改進(jìn)人工勢場法算法流程如圖4所示。當(dāng)實(shí)驗(yàn)車接收到來自前方障礙物信息后，開始計(jì)算每個(gè)障礙物與車輛初始位置連線和X軸夾角；然后計(jì)算出各個(gè)分力并正交分解于X軸、Y軸；最后矢量相加求得合力。通過合力來確定實(shí)驗(yàn)車輛的下一位置，并時(shí)刻監(jiān)控在規(guī)定時(shí)間內(nèi)能否到達(dá)目標(biāo)點(diǎn)并回到全局路徑中：若到達(dá)，則局部路徑規(guī)劃成功；若不能到達(dá)目標(biāo)點(diǎn)，則更新障礙物位置進(jìn)一步規(guī)劃。

圖4 改進(jìn)人工勢場算法流程Figure 4 Algorithm flow chart of improved artificial potential field

2.2 價(jià)值評(píng)價(jià)

為驗(yàn)證改進(jìn)人工勢場的優(yōu)劣性，可從時(shí)效性TS與安全性QS兩方面進(jìn)行評(píng)價(jià)。

(1)時(shí)效性。設(shè)單位步長為Δd，實(shí)驗(yàn)車到目標(biāo)點(diǎn)的局部路徑規(guī)劃長度為L，可算出總步數(shù)n(圓整)，則時(shí)效性為

(11)

式中：Ti為單步計(jì)算時(shí)長。

時(shí)效性由單步計(jì)算時(shí)長與局部規(guī)劃路徑長度所決定，規(guī)劃路徑越短，單步時(shí)長越短，即時(shí)效性越好。

(2)安全性。設(shè)局部規(guī)劃路徑至障礙物最小碰撞安全距離為S，規(guī)劃路徑中出現(xiàn)極小值點(diǎn)的次數(shù)為K，安全性指標(biāo)為

(12)

式中：h為安全系數(shù)，為大于或等于1的任意數(shù)。

對(duì)于某一局部規(guī)劃路徑，其安全性由極小值點(diǎn)出現(xiàn)的次數(shù)與最小碰撞安全距離決定，極小值點(diǎn)次數(shù)出現(xiàn)越少，則安全性越好。

3 仿真分析

3.1 附加力引入的影響

圖5為用附加力增益系數(shù)表達(dá)的不同大小的附加力作用下的規(guī)劃路徑結(jié)果，表1為時(shí)效性和安全性評(píng)價(jià)結(jié)果。從圖5可見，所有規(guī)劃路徑均能越過局部極小值點(diǎn)，但附加力的大小對(duì)最小碰撞安全距離大小的影響不同，從表1也可見引入附加力后的所有路徑規(guī)劃均未出現(xiàn)極小值問題。

圖5 不同附加力增益系數(shù)下的路徑Figure 5 Paths with different additional force gain coefficients

當(dāng)附加力增益系數(shù)較小時(shí)(如k=1)，規(guī)劃路徑幾乎緊貼障礙物邊緣而越過，其安全性指標(biāo)過低，即碰撞安全距離過小，實(shí)際行車過程中仍存在避障失敗的可能；當(dāng)附加力增益系數(shù)過大時(shí)(如k=10)，規(guī)劃路徑能可靠越過障礙物，但碰撞安全距離明顯過大(安全性指標(biāo)值最大)，且規(guī)劃路徑步數(shù)較附加力增益系數(shù)為5時(shí)增加4.3%，時(shí)效性也增加了8.3%。

從圖5(b)可見，當(dāng)附加力增益系數(shù)取值分別為5、6和7時(shí)的3條規(guī)劃路徑基本上處于相互重合的狀態(tài)，它們的碰撞安全距離和安全性指標(biāo)也都十分接近，表明在該區(qū)間內(nèi)規(guī)劃路徑具有較好的一致性，也說明采用附加力改進(jìn)的人工勢場規(guī)劃算法具有較好的結(jié)果穩(wěn)定性和可靠性。

3.2 距離調(diào)節(jié)因子引入的影響

圖6(a)為改進(jìn)人工勢場下障礙物對(duì)實(shí)驗(yàn)車產(chǎn)生的斥力變化仿真曲線，Z軸表示無量綱斥力。圖6(b)為將斥力峰值坐標(biāo)投影在XOZ平面內(nèi)的斥力峰值曲線圖。

圖6 改進(jìn)人工勢場斥力仿真曲線Figure 6 Repulsion change simulation curve of improved artificial potential field

定義實(shí)驗(yàn)車初始位置與目標(biāo)點(diǎn)連線為中軸線，從圖6可見，中軸線上不同位置障礙物產(chǎn)生的斥力峰值隨其與實(shí)驗(yàn)車初始位置間的距離變化而變化：當(dāng)障礙物距實(shí)驗(yàn)車較近時(shí)(如在實(shí)驗(yàn)車起始位置附近)，斥力峰值很大；而當(dāng)障礙物逐漸遠(yuǎn)離實(shí)驗(yàn)車時(shí)，斥力峰值則呈迅速下降的趨勢，并在目標(biāo)點(diǎn)附近所受斥力衰減為0，確保了實(shí)驗(yàn)車目標(biāo)可達(dá)。

3.3 路徑規(guī)劃的價(jià)值評(píng)價(jià)

圖7為引力增益系數(shù)為12，斥力增益系數(shù)為3，附加力增益系數(shù)為6，單位步長為0.2，障礙物數(shù)目為7，安全系數(shù)為1.5的仿真參數(shù)條件下，傳統(tǒng)人工勢場法與改進(jìn)人工勢場法的仿真規(guī)劃路徑算例。為保證傳統(tǒng)人工勢場可以獲得完整路徑，在出現(xiàn)局部極小值點(diǎn)時(shí)采用自動(dòng)延續(xù)步長處理。

圖7 局部路徑規(guī)劃Figure 7 Local path planning

從圖7可見，傳統(tǒng)人工勢場規(guī)劃算法在出現(xiàn)局部極小值點(diǎn)或障礙物距離目標(biāo)點(diǎn)過近時(shí)，不能避障，并可能會(huì)出現(xiàn)局部震蕩，而改進(jìn)人工勢場規(guī)劃的路徑，能成功越過障礙物，并可完全規(guī)避路徑規(guī)劃中極小值點(diǎn)的出現(xiàn)。

表2為傳統(tǒng)及改進(jìn)人工勢場兩種規(guī)劃算法的價(jià)值評(píng)價(jià)結(jié)果。由表2可見，改進(jìn)算法的單步計(jì)算時(shí)長略有增加，但由于所規(guī)劃的路徑不存在震蕩區(qū)間，其計(jì)算時(shí)效性與傳統(tǒng)人工算法的基本一致。同時(shí)，改進(jìn)人工勢場規(guī)劃路徑的最小碰撞安全距離遠(yuǎn)大于傳統(tǒng)人工勢場規(guī)劃，因此其安全性得以大大提高。

表2 時(shí)效性與安全性價(jià)值評(píng)價(jià)表Table 2 Valuation table of timeliness and security

4 結(jié)論

引入斥力調(diào)節(jié)因子和附加力對(duì)傳統(tǒng)人工勢場加以改進(jìn)的局部路徑規(guī)劃算法，解決了存在的局部極小值和目標(biāo)不可達(dá)兩個(gè)問題。

(1)引入S(M,Mg)調(diào)節(jié)因子的改進(jìn)人工勢場規(guī)劃算法可以顯著降低斥力在目標(biāo)點(diǎn)附近的影響，改進(jìn)后的斥力峰值具有隨障礙物與局部路徑規(guī)劃起始點(diǎn)的增加而呈現(xiàn)出指數(shù)級(jí)減小的規(guī)律，斥力在目標(biāo)點(diǎn)附近衰減為0，確保實(shí)驗(yàn)車實(shí)現(xiàn)目標(biāo)可達(dá)。

(2)引入k·S(M,Mg)增益量的附加力可改變局部極小值點(diǎn)處的合力大小與方向，能完全避免出現(xiàn)局部極小值點(diǎn)，使實(shí)驗(yàn)車按規(guī)劃路徑行駛越障過程中能與障礙物間保持合適的碰撞安全距離。

(3)采用距離調(diào)節(jié)因子與附加力耦合改進(jìn)的人工勢場算法的局部路徑與傳統(tǒng)路徑時(shí)效性均為0.26 s，安全指標(biāo)由傳統(tǒng)人工勢場法的0.018 8增大至0.305 0，可顯著提高局部路徑規(guī)劃的安全性。