基于勢場化改進A*算法的履帶式車輛路徑規(guī)劃算法

王 濤，黎玉康，劉文學

（陸軍炮兵防空兵學院，合肥 230000）

0 引言

履帶式車輛由于與地面有更大的接觸面積，比輪式車輛更能適應環(huán)境復雜惡劣的路面，因此，其工作路面大多條件較為惡劣。工作環(huán)境的惡劣性使得履帶車更需要無人化改造。復雜的環(huán)境、崎嶇的路面給無人履帶車的路徑規(guī)劃帶來了更大的挑戰(zhàn)。文獻［1-3］提出了針對于履帶式車輛的路徑規(guī)劃方法，保證履帶式車輛行駛的平穩(wěn)性、安全性；文獻［4-6］通過增加路徑的平滑性，減少轉(zhuǎn)彎次數(shù)的方式減緩履帶式車輛擬合路徑的難度，縮短路徑長度；文獻［7］引入新增節(jié)點與刪除節(jié)點準則，改善了路徑質(zhì)量，并加入了動態(tài)窗口思想使之能夠?qū)崟r局部規(guī)劃避障，但該方法參數(shù)選擇困難；文獻［8］改變傳統(tǒng)A* 算法子節(jié)點拓展方式，其子節(jié)點位置根據(jù)拓展步長、車輛最小轉(zhuǎn)彎半徑以及前輪轉(zhuǎn)角離散數(shù)量確定，保證了路徑符合輪式車輛運動學特點；文獻［9］提出的關(guān)鍵點提取法大大減少了復雜環(huán)境下A*算法內(nèi)存的開銷，但不具備局部路徑規(guī)劃能力。

履帶式車輛通過兩側(cè)履帶差速運動時擬合連續(xù)曲線時，車身易產(chǎn)生滑轉(zhuǎn)滑移，以至于產(chǎn)生較大的橫向偏差。針對以上問題，提出一種基于改進A*算法的履帶式車輛路徑規(guī)劃算法。將人工勢場引入環(huán)境中，生成局部勢能最小值為子目標，為A*算法規(guī)劃局部路徑指引方向；提出刪除冗余節(jié)點的方法，減少了路徑轉(zhuǎn)折，縮短了路徑長度；引入了Reeds-Shepp 曲線，約束了路徑的曲率使之適用于履帶車輛的行駛。

1 改進A*算法

由于履帶車輛的轉(zhuǎn)向機構(gòu)操作相對較為困難，難以連續(xù)調(diào)節(jié)轉(zhuǎn)向半徑，使得履帶車輛運動軌跡顯示出曲率變化少、曲率易突變的特點。而傳統(tǒng)方法規(guī)劃的路徑曲率連續(xù)變化且變化多，不僅導致跟蹤效果差，還會使得的拉動操縱桿動作過于頻繁，大大縮短轉(zhuǎn)向機構(gòu)的壽命。為了符合履帶車運動規(guī)律，且易于控制，減少操縱桿的動作，履帶式車輛的運動路徑應有著較大的直線占比，轉(zhuǎn)彎部分以固定曲率半徑的圓弧連接。

傳統(tǒng)A*算法為全局啟發(fā)式算法，有著良好的搜索最短路徑的能力、但只能作為靜態(tài)全局算法，且路徑轉(zhuǎn)折點過多，計算量過大。為了使A*算法所規(guī)劃路徑更滿足履帶式車輛的實際運動路徑需要，本文將對A*算法作出一定的改進。

1.1 A＊算法勢場化

A*算法屬于全局靜態(tài)規(guī)劃算法，在把握全局環(huán)境的基礎上，擁有出色的最短路徑搜索能力，但是復雜的環(huán)境使得A*算法的計算效率急劇下降，且A*算法不具備未知環(huán)境下的局部路徑搜索能力，而人工勢場算法作為典型的局部路徑搜索算法，具有結(jié)構(gòu)簡單，響應迅速的特點。

假設T 時刻時，履帶式車輛前進方向上有半徑為r，角度為90°的扇形觀察域，此觀察域隨著履帶車輛的前進方向改變而改變，人工勢場初步感知出觀察域內(nèi)的障礙與非障礙區(qū)域，非障礙區(qū)域每個點相對終點的勢能為：

相對障礙的勢能為：

其中，k 為引力系數(shù)，m 為斥力系數(shù)，d1，d2分別為非障礙區(qū)域點與終點、障礙的距離，P0為斥力影響距離。

觀察域內(nèi)每個非障礙點的勢能為：

將T 時刻時人工勢場計算所得的勢能最低點設為當前A*算法當前的子目標點。如圖1 所示。

圖1 某時刻觀察域與勢能分布Fig.1 The distribution of observation domain and potential energy at a certain time

引入人工勢場后，復雜的環(huán)境相當于被分割為若干簡單環(huán)境，提高了A*算法規(guī)劃效率。人工勢場為A*算法指出了當前的子目標，使得A*算法具備了未知環(huán)境的路徑搜索能力。傳統(tǒng)人工勢場所規(guī)劃路徑質(zhì)量差，易于陷入局部極值。A*算法縮短了路徑長度，且可作為陷入極值點時的代替算法，跳出局部極值點。

1.2 優(yōu)化節(jié)點

傳統(tǒng)A*算法為求最短路徑，在復雜環(huán)境障礙的影響下，會得到多折線組成的路徑。其原由是路徑中冗余節(jié)點的存在，這些節(jié)點還造成了一些不必要的轉(zhuǎn)折。轉(zhuǎn)折過多不利于履帶車輛的行駛，不僅會造成車輛轉(zhuǎn)向機構(gòu)的磨損，還會大大減緩行進速度，消耗更多的時間。因此，需要對A*算法得到的路徑進行優(yōu)化以適應履帶車輛的正常行駛。

由于文獻［7］中刪除冗余節(jié)點的策略需對參數(shù)α 進行調(diào)整，地圖大小不同，復雜度不同，參數(shù)也不同，實際運用中限制較大。文獻［10］中通過簡化算子來減少拐點的方式簡化路徑。本文基于這兩種策略，作出調(diào)整，提出了自己的刪除冗余節(jié)點的策略。

將某次局部規(guī)劃觀察域內(nèi)障礙物所處坐標定義為危險區(qū)，危險區(qū)不可通行。在某次局部規(guī)劃完一條路徑后，假設該規(guī)劃包括起點終點在內(nèi)一共有X 個節(jié)點，對所得路徑進行二次規(guī)劃：從起點開始依次與第2 節(jié)點，第3 節(jié)點直線連接，直到與第N+1 點連接時，該直線經(jīng)過危險區(qū)，則停止，并刪除起點到第N 點之間的點，起點與第N 點的連線即為新的路徑。再從第N 點開始繼續(xù)上述步驟。如圖2 所示。

圖2 刪除冗余節(jié)點流程圖Fig.2 Flow chart of deleting redundant nodes

經(jīng)過該方法優(yōu)化路徑后，路徑長度將大大縮短，減少了不必要的轉(zhuǎn)彎與曲線路徑。優(yōu)化效果如圖3 所示。

圖3 路徑優(yōu)化效果Fig.3 Optimization effects of paths

1.3 Reeds-Shepp 曲線引入

傳統(tǒng)路徑規(guī)劃算法所得曲線路徑質(zhì)量較差，除了拐角較多以外，還存在拐角角度大，難以滿足履帶車輛的運動特點的現(xiàn)象。拐角角度大意味著履帶車輛運動方向變化大，在實際運動中，大角度的變向運動會引起較大路徑跟蹤誤差，極大影響了跟蹤精度。文獻［2-7］采用了B 樣條曲線以及刪除、增加節(jié)點的方法使得曲線變得平滑，易于跟蹤。但是在復雜的環(huán)境中，過于擁擠的障礙環(huán)境導致以上兩種方法平滑后的曲線依然難以滿足履帶車輛的極限轉(zhuǎn)彎半徑所限的運動軌跡。履帶式車輛的轉(zhuǎn)向運動往往會產(chǎn)生突變性的曲率變化，且由于機械結(jié)構(gòu)的限制，難以實現(xiàn)多次調(diào)節(jié)轉(zhuǎn)向半徑用于擬合曲率連續(xù)變化的曲線。

Reeds-Shepp 曲線由圓弧和直線組成，曲線的曲率可根據(jù)實際環(huán)境駕駛中的常用轉(zhuǎn)彎半徑而定。該曲線的特點正符合了車輛運動的理想路徑的特點。將算法所得路徑的轉(zhuǎn)角利用Reeds-Shepp 曲線平滑化，所得曲線不僅符合履帶車輛的運動規(guī)律，且為眾多由該圓弧與直線段組成的路徑中最短的一條。圓弧與直線連接的路徑，既保證了轉(zhuǎn)向開始時曲率的突變，又滿足了轉(zhuǎn)向過程中曲率保持不變的特點。

2 實驗分析

為驗證本文算法優(yōu)越性，在不同環(huán)境下將傳統(tǒng)算法與本文算法進行實驗仿真比較。實驗在CPU 為I7-7700HQ，RAM 為8 G 的計算機上運行，算法通過MATLAB 編程仿真實現(xiàn)。

2.1 與傳統(tǒng)A＊算法對比

履帶車輛的運動特性使得其跟蹤曲線路徑時會產(chǎn)生較大的誤差，跟蹤精度不理想。理想情況下的路徑應長度相對短，直線路徑占比高，轉(zhuǎn)彎曲線曲率滿足其轉(zhuǎn)彎半徑極限，如此保證了其運動的快速性、平穩(wěn)性。

傳統(tǒng)A*算法具有高效性，且尋路能力強。但傳統(tǒng)A*算法因其固有原因，所得路徑往往有著過多的轉(zhuǎn)角，不適合履帶式車輛的行駛。本文算法以A*算法為核心，為了避免上述缺陷，經(jīng)過改進，加入了刪除冗余節(jié)點的策略，大大減少了轉(zhuǎn)彎次數(shù)，減少了履帶式車輛在行駛中因過多轉(zhuǎn)彎導致的轉(zhuǎn)向機構(gòu)磨損以及減速。如下頁圖4 所示。

圖4 A*算法與本文算法路徑對比Fig.4 Path comparison of A*algorithm with the proposed algorithmalgorithm

圖5 不同障礙占比地圖Fig.5 Maps of different obstacle proportions

由表1 中數(shù)據(jù)可看出，優(yōu)化后的路徑相對于原路徑，縮短了19.6%，且大大減少了轉(zhuǎn)彎次數(shù)，僅為原本轉(zhuǎn)彎次數(shù)的1/6，平均轉(zhuǎn)彎角度也大大減小。相比于傳統(tǒng)A*算法，本算法所得路徑通過刪除冗余節(jié)點二次規(guī)劃后，得到了更適應履帶車輛行駛的路徑。

表1 傳統(tǒng)A*算法與本文算法數(shù)據(jù)對比Table 1 Data comparison of traditional A*algorithm and the proposed algorithm

2.2 與人工勢場算法對比

人工勢場算法作為一種典型的未知環(huán)境路徑搜索算法，具有反應迅速，結(jié)構(gòu)簡單等特點。但往往路徑質(zhì)量較差，不適合履帶車輛的行駛。實際的行駛路徑中，直線路徑占比越高，平均直線路徑長度越長，越有利于車輛的快速行駛。本文算法運用了人工勢場，并結(jié)合了改進A*算法令路徑得到了優(yōu)化。將傳統(tǒng)人工勢場算法與本文算法進行算法效率和路徑質(zhì)量上的對比。

為了消除其他復雜因素的干擾，現(xiàn)運用人工勢場算法與本文算法在同樣的計算機上，規(guī)劃出4 種環(huán)境復雜度的可行路徑。該地圖為100 m×100 m 大小，每個柵格為1 m×1 m 障礙占比依據(jù)環(huán)境復雜度的增加而逐漸增加為5.84%、11.85%、15.70%和21.32%。為了防止因車輛體積原因?qū)е屡c障礙相碰，對全地圖進行膨化處理，膨脹比為1.2。履帶車輛車體為3.3 m×6 m，無障礙分辨率為1 m。起點、終點均設為（3，3），（97，97）。

如圖6 所示，以障礙占比21.32%的環(huán)境為例，從直觀的路徑形貌來看，本文所提出的算法顯然較之傳統(tǒng)人工勢場算法所得路徑質(zhì)量更好，路徑轉(zhuǎn)彎次數(shù)更少，解決了傳統(tǒng)路徑轉(zhuǎn)彎半徑過小且路徑不夠平滑的問題。

圖6 人工勢場算法與本文算法路徑對比Fig.6 Path comparison of artificial potential field algorithm with the proposed algorithm

由表2 以及圖7～圖10 實驗數(shù)據(jù)可明顯表明本文算法的優(yōu)越性：1）論路徑長度，本文算法所得路徑更短，且隨著環(huán)境障礙占比的增大，本文算法所得路徑長度優(yōu)勢更加明顯；2）論算法效率，由于本文算法采用先探索勢能分布，再規(guī)劃局部路徑的方式，其運算時間必然較之傳統(tǒng)的單一算法較長。但本文算法在環(huán)境大小未變，復雜度增大的情況下，得到全部未知環(huán)境的路徑的時間較人工勢場法相比差距不大；且由于觀察域范圍有限，其中，環(huán)境較之完整地圖環(huán)境較為簡單，改進A*算法的運算速度極快，對算法的實時反應速度影響較??；3）論直線路徑占比，本算法所得路徑擁有較大的直線路徑長度占比率，相比于傳統(tǒng)人工勢場算法，本算法所得直線路徑占比率具有顯著優(yōu)勢，且平均直線路徑長度較長。環(huán)境復雜度越高，優(yōu)勢越明顯。4）論路徑轉(zhuǎn)彎次數(shù)，本文算法轉(zhuǎn)折數(shù)相對較少。隨著環(huán)境障礙占比率的增大，本文算法所得路徑轉(zhuǎn)彎次數(shù)的增加并不明顯，而人工勢場算法所得路徑的轉(zhuǎn)彎次數(shù)明顯大大增加。

表2 本文算法與人工勢場算法在不同復雜度環(huán)境下所得路徑對比Table 2 The obtained path comparison of the proposed algorithm and artificial potential field algorithm in differently complex environment

圖7 路徑長度隨障礙占比變化趨勢Fig.7 Path length with obstacle proportions

圖8 計算時間隨障礙占比變化趨勢Fig.8 Variation trend of time calculation with obstacle proportions

圖9 直線路徑長度占比隨障礙占比變化趨勢Fig.9 Variation trend of proportion of straight path length with obstacle proportions

圖10 路徑轉(zhuǎn)折數(shù)隨障礙占比變化趨勢圖Fig.10 Variation trend of path turning number with obstacle proportions

3 結(jié)論

傳統(tǒng)的關(guān)于履帶式車輛的路徑規(guī)劃方法多關(guān)注于得到平滑的曲線路徑，而忽略了履帶式車輛跟蹤多曲率變化路徑時不易提升跟蹤精度的問題。本文提出了適合履帶車輛行駛的路徑規(guī)劃方法。1）針對履帶車輛轉(zhuǎn)向操作不易，行駛路徑曲率變化較少的特點，運用刪除冗余節(jié)點策略，減少了轉(zhuǎn)向，引入了Reeds-Shepp 曲線，約束了路徑的曲率，再加上人工勢場對未知環(huán)境的探索，提出了基于勢場化改進A* 算法的履帶式車輛路徑規(guī)劃算法。2）根據(jù)履帶式車輛的行駛路徑，提出了評價路徑優(yōu)劣性的路徑直線占比、平均直線路徑長度以及轉(zhuǎn)向次數(shù)這3 個性能指標。高的直線占比率與平均直線路徑長度降低了控制難度，有利于提高跟蹤精度；較少的轉(zhuǎn)彎次數(shù)使得在履帶車輛跟蹤路徑的過程中，操縱桿的操作頻次大大減少，有利于減少轉(zhuǎn)向機構(gòu)不穩(wěn)定性帶來的跟蹤誤差。3）最終本文算法所得路徑總長度短，平均直線路徑長度，直線路徑占比高，在障礙占比20%的復雜環(huán)境下也可使得直線路徑占比高達75%，路徑相對傳統(tǒng)A*算法縮短了19.6%，轉(zhuǎn)彎次數(shù)相對于傳統(tǒng)人工勢場減少了63%。