基于A*算法與人工勢場法無人車路徑規(guī)劃的改進(jìn)探索

張皞宇 楊劉一 陳旭淼

摘要：針對靜態(tài)地圖全局路徑規(guī)劃問題提出了一種改進(jìn)的A*算法。根據(jù)激光雷達(dá)掃描從而獲取地圖數(shù)據(jù)再建立柵格地圖環(huán)境模型，再使用A*算法進(jìn)行初步的路徑規(guī)劃。其次，針對A*算法規(guī)劃的路徑靠近障礙物以及轉(zhuǎn)向角度偏大的缺陷，提出將A*算法與人工勢場法相結(jié)合，在行駛階段中通過建立車輛與障礙物之間的斥力場使得行駛路徑遠(yuǎn)離障礙物。當(dāng)目標(biāo)車輛行駛到一定范圍內(nèi)，通過人工勢場法的引導(dǎo)將車輛準(zhǔn)確引導(dǎo)至目標(biāo)地點(diǎn)，同時(shí)解決轉(zhuǎn)折角度過大等問題，讓車輛的行駛路線更加平滑，經(jīng)仿真實(shí)驗(yàn)，測試和證實(shí)了本文算法的可行性。

關(guān)鍵詞：A*算法;人工勢場法;路徑規(guī)劃

中圖分類號：TP311? ? ? 文獻(xiàn)標(biāo)識碼：A

文章編號：1009-3044（2021）17-0233-03

開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）：

1 引言

避障路徑規(guī)劃和跟蹤控制是智能車輛研究領(lǐng)域的熱點(diǎn)之一，其中避障路徑規(guī)劃經(jīng)過多年的發(fā)展，已經(jīng)積累了許多的研究成果，包括A*、蟻群算法、人工勢場法，多項(xiàng)式規(guī)劃等[1]。

A*算法通過啟發(fā)函數(shù)能夠快速鎖定目標(biāo)方向進(jìn)行路徑規(guī)劃。能在已知地圖中有效地規(guī)劃最優(yōu)路徑，但實(shí)際導(dǎo)航中需與局部規(guī)劃算法結(jié)合[2]。

傳統(tǒng)的A*算法中有依舊存在如：路徑緊貼障礙物和轉(zhuǎn)向角度過大和不滿足車輛運(yùn)動(dòng)學(xué)等問題[3]。

針對上述問題，本文通過對傳統(tǒng)A*實(shí)施改進(jìn)，加入了人工勢場法并對車輛模型進(jìn)行簡化，通過人工勢場法的引力場的引力得出汽車的目標(biāo)姿態(tài)角，從而改進(jìn)傳統(tǒng)A*算法中的轉(zhuǎn)向角過大的問題。同樣對障礙物與目標(biāo)車輛附加了斥力場，通過人工勢場法中斥力場的作用解決了路徑緊貼障礙物的問題。

2 A*算法與人工勢場法

2.1 A*算法

A*算法是一種典型的啟發(fā)式搜索計(jì)算方式，其中可在一定程度上避免無效搜索路徑，有較好的搜索效率。在深度優(yōu)先、廣度優(yōu)先和最佳優(yōu)先3種啟發(fā)式搜索算法中，A*算法歸屬于最佳優(yōu)先類。在A*中通過評估函數(shù)進(jìn)行引導(dǎo)搜索與決定的方向。其公式表示為：

[f（n）=g（n）+h（n）]? ? ? ? ? ? ? ? ? ? ? ? ? （1）

[f（n）]表示從起始點(diǎn)到目標(biāo)點(diǎn)的估價(jià)函數(shù);[g（n）]是在從起始點(diǎn)到節(jié)點(diǎn)[n]的移動(dòng)代價(jià);[h（n）]是從節(jié)點(diǎn)[n]到目標(biāo)節(jié)點(diǎn)估計(jì)代價(jià)，通過上述函數(shù)可以得到每個(gè)函數(shù)的價(jià)值，通過選取價(jià)值最低的節(jié)點(diǎn)來進(jìn)行最優(yōu)節(jié)點(diǎn)的選取，從而直接決定了算法能否進(jìn)行最優(yōu)路徑的查找。

2.2 人工勢場法

人工勢場法是由Khatib提出，通過設(shè)置假想力的概念促使目標(biāo)進(jìn)行移動(dòng)[4]。該方法將控制對象、目標(biāo)點(diǎn)、障礙物看作質(zhì)點(diǎn)，目標(biāo)點(diǎn)對控制對象形成引力場，障礙物對控制對象形成斥力場。通過產(chǎn)生的引力場與斥力場的合力從而引導(dǎo)控制對象的行動(dòng)。此過程是將控制對象在實(shí)際環(huán)境中的運(yùn)動(dòng)過程虛擬為電荷在受力場中的運(yùn)動(dòng)。

同樣，在駕駛環(huán)境中，不可通過的部分設(shè)置為與控制對象相同的電荷，即可產(chǎn)生斥力場，可以在目標(biāo)地點(diǎn)設(shè)置相反電荷來形成引力場。由此來控制目標(biāo)的行走路徑。有著結(jié)構(gòu)簡單，實(shí)時(shí)性強(qiáng)，規(guī)劃路徑簡單且路徑平滑等優(yōu)點(diǎn)。

3 本文算法

3.1.環(huán)境模型

在小車機(jī)器人工作中，對地圖的構(gòu)建是關(guān)鍵。在通過URG-04LX-UG01激光雷達(dá)掃描得到數(shù)據(jù)，規(guī)劃由本算法過柵格地圖的方式進(jìn)行表現(xiàn)，通過柵格法將小車可通過的區(qū)域和不可通過的區(qū)域在柵格地圖中做出呈現(xiàn)。

在算法中每個(gè)柵格的狀態(tài)位兩種分別為可通過與不可通過。在算法中分別由0與1進(jìn)行表示。在柵格地圖中分別由白色（可通過）和黑色（不可通過表示）。

其中在柵格地圖中的柵格大小會(huì)影響后續(xù)對于路徑規(guī)劃的效果。當(dāng)柵格偏大時(shí)，能夠減小運(yùn)算量與規(guī)劃時(shí)間，但會(huì)導(dǎo)致規(guī)劃的路線過于粗糙使得行走的路線增加;當(dāng)柵格偏小時(shí)，使得行走的路程準(zhǔn)確率增高，讓規(guī)劃時(shí)間大幅度上升。由此，柵格地圖的大小需要更具實(shí)際情況進(jìn)行設(shè)定，包含場地與實(shí)驗(yàn)對象的考量。

3.2算法實(shí)現(xiàn)

3.2.1 A*的路徑選取

將實(shí)驗(yàn)對象的場地用柵格地圖進(jìn)行表示后，可以根據(jù)柵格地圖的信息得到實(shí)驗(yàn)對象的地圖環(huán)境：

[M={Mi，i=0，1，2，3}]? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（2）

其中[M0]為實(shí)驗(yàn)對象起始點(diǎn)，[M1]為可通過區(qū)域，[M2]為不可通過區(qū)域，[M3]為目標(biāo)區(qū)域。

A*算法在靜態(tài)環(huán)境中進(jìn)行路徑規(guī)劃實(shí)驗(yàn)時(shí)具有較好的實(shí)時(shí)性，從而可迅速高效地求解最優(yōu)路徑[5]。在A*算法設(shè)計(jì)中，本文采用歐式距離作為啟發(fā)式函數(shù)：

[h（n）=（xi-xn）2-（yi-yn）2]? ? ? ? ? ? ? ? ? ? ? （3）

其中起始點(diǎn)在柵格地圖中橫坐標(biāo)為[xi]，起始點(diǎn)在柵格地圖中縱坐標(biāo)[yi]。終點(diǎn)在柵格地圖中橫坐標(biāo)[xn]，終點(diǎn)在柵格地圖中縱坐標(biāo)[yn]。

其中圖1為A*算法的實(shí)例：

3.2.2 行駛階段

在行駛階段中，為解決傳統(tǒng)A*算法中出現(xiàn)的規(guī)劃路徑貼近障礙物邊緣導(dǎo)致不符合正常汽車行駛情況。本文算法中將人工勢場法加入，通過給障礙物添加負(fù)電荷，形成斥力場在行駛過程中，目標(biāo)車輛受到障礙物的斥力隨著距離減小使得斥力越大。因此，勢能和距離成反比。公式如下：

[F=-grad（kl），l≤lmax0，其他]? ? ? ? ? ? ? ? ? ? ? （4）

其中[F]代表目標(biāo)車輛受到的斥力，[k]代表相應(yīng)系數(shù)，[l]代表目標(biāo)車輛與障礙物的距離，[lmax]為勢場作用的最大范圍。

同樣通過斥力場的產(chǎn)生的斥力作用，解決在路徑緊貼障礙物這一不符合汽車行駛的情況。

由于本文在地圖掃描階段中采用URG-04LX-UG01激光雷達(dá)，此雷達(dá)掃描范圍為240°（圖2）。故本文提出在目標(biāo)汽車后方120°人工添加假想障礙物（圖3），從而在人工勢場法斥力場計(jì)算給予目標(biāo)汽車一個(gè)斥力使得汽車在引力場和斥力場計(jì)算中能夠獲得一個(gè)向前行駛的斥力。

3.2.3局部階段

因在環(huán)境模型中將地圖通過柵格的表達(dá)方式，故當(dāng)實(shí)驗(yàn)對象進(jìn)入目的地柵格時(shí)，則進(jìn)入局部規(guī)劃階段。在局部規(guī)劃階段中，本算法通過人工勢場法中的應(yīng)力場對小車機(jī)器人進(jìn)行引導(dǎo)。

在目標(biāo)汽車的引導(dǎo)階段中，會(huì)出現(xiàn)需要使目標(biāo)階段轉(zhuǎn)向的情況，此時(shí)則需要對目標(biāo)的轉(zhuǎn)向角與姿態(tài)角進(jìn)行計(jì)算。在本文算法中為將目標(biāo)四輪車輛簡化為兩輪車輛。從而進(jìn)行相應(yīng)的計(jì)算。

通過引力場的牽引力，在計(jì)算出小車每時(shí)刻的姿態(tài)角，其中計(jì)算轉(zhuǎn)向角[u]的公式為：

[u=tan-1[Lv（λ（θr-θ）+θr）]]? ? ? ? ? ? ? ? ? （5）

其中[L]為車輛軸距，[v]為車輛速度，[θ]為車輛姿態(tài)角，[θr]為目標(biāo)姿態(tài)角，[λ]是轉(zhuǎn)向系數(shù)。

其中在確定轉(zhuǎn)向角之前，需要確定目標(biāo)姿態(tài)角，該姿態(tài)角等于人工勢場法中合力的角度。

基于應(yīng)力場的特性，可以讓小車的轉(zhuǎn)向曲線更加平滑，符合實(shí)際的汽車運(yùn)動(dòng)情況。

4實(shí)驗(yàn)結(jié)果

為驗(yàn)證算法的可行性和有效性，進(jìn)行了計(jì)算機(jī)仿真實(shí)驗(yàn)，使用Matlab軟件對汽車目標(biāo)做出規(guī)劃與到達(dá)地點(diǎn)的行駛，并對轉(zhuǎn)向角度進(jìn)行測試。其中地圖的掃描構(gòu)建采用URG-04LX-UG01激光雷達(dá)。所用小車如圖4所示：

由于在仿真中，車輛的行駛默認(rèn)為勻速駕駛，在到達(dá)目標(biāo)地點(diǎn)時(shí)，由于出現(xiàn)了轉(zhuǎn)向，勻變行駛使得在到達(dá)指定地點(diǎn)時(shí)出現(xiàn)些許誤差。通過多次測試，與設(shè)定的目標(biāo)點(diǎn)偏差如圖5在10mm之內(nèi)：

在行駛過程中目標(biāo)汽車的轉(zhuǎn)向角和姿態(tài)角變化如圖6、圖7所示。

5 結(jié)論

通過將A*算法和人工勢場法相結(jié)合，能夠較好地對汽車目的地進(jìn)行路徑規(guī)劃。同時(shí)由于人工勢場法的引入，能更好地調(diào)整汽車目標(biāo)轉(zhuǎn)向角，使得在轉(zhuǎn)向操作時(shí)角度更加合理，從而達(dá)到汽車行駛為平滑路徑的效果。在與目標(biāo)汽車障礙物建立斥力場，規(guī)避了路線緊貼障礙物，使其更符合實(shí)際行駛需要。通過一系列的仿真實(shí)驗(yàn)，證明了本算法的可行性。

參考文獻(xiàn)：

[1] 楊博，張緩緩，江忠順.智能車輛的避障路徑規(guī)劃與跟蹤控制仿真分析[J/OL].中國測試：1-8[2020-11-11].http：//kns.cnki.net/kcms/detail/51.1714.TB.20201016.1753.015.html.

[2] 孫煒，呂云峰，唐宏偉，等.基于一種改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].湖南大學(xué)學(xué)報(bào)（自然科學(xué)版），2017，44（4）：94-101.

[3] 馬飛，楊皞屾，顧青，等.基于改進(jìn)A*算法的地下無人鏟運(yùn)機(jī)導(dǎo)航路徑規(guī)劃[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2015，46（7）：303-309.

[4] KHATIB O.Real-time obstacle avoidance for manipulators and? and mobile robots[J].The International Journal of Robotics Research，1986，5（1）：90-98.

[5] 王維，裴東，馮璋.改進(jìn)A*算法的移動(dòng)機(jī)器人最短路徑規(guī)劃[J].計(jì)算機(jī)應(yīng)用，2018，38（5）：1523-1526.

【通聯(lián)編輯：光文玲】