基于變步長A＊與車身穩態轉向模型的UGV路徑規劃①

江 洪,姜 民

（江蘇大學 機械工程學院,鎮江 212013）

無人地面車輛（Unmanned Ground Vehicle,UGV）,是指在各種復雜環境中可以自主完成行駛任務的車輛,近年來,隨著定位導航與控制技術的發展,UGV憑借可以代替人類完成諸多繁重工作的優勢,已被廣泛應用于軍事、民用與科研等領域,其相關技術的研究也越來越受到人們的重視,尤其是UGV在復雜環境下的路徑規劃問題一直是一個重要的熱點研究方向[1].

路徑規劃的本質是指在給定的環境場景中,將決策系統的宏觀指令轉換成一條連接起點與目標點的安全無碰撞的路徑[2,3],其核心是尋路算法的設計[4].目前比較常見的尋路算法有人工勢場法（ADF）[5],A*算法,快速隨機樹法（RRT）[6-8],遺傳算法[9]等.這些算法各有優缺點,比如人工勢場法具有較好的實時性,卻容易陷入局部最優解; RRT算法簡單實用,在搜索空間中的搜索速度也較快,但由于隨機性過大,導致搜索結果不穩定,不能保證規劃出的路徑為最優路徑,遺傳算法具備較好的全局求解能力和魯棒性,但收斂速度較慢、局部搜索能力弱; A*算法作為基于柵格地圖的啟發式搜索算法[10],具備最優性和完備性的特點,但其存在隨著柵格增多搜索效率下降的缺點[11],且生成的路徑曲率不連續,不利于車輛跟蹤.

為此,有學者提出了一些改進辦法:文獻[12]中提出了一種雙向A*算法,該方法通過從起點和目標點同時運行A*算法,達到縮短規劃時間的目的,但該方法容易受實際地圖環境影響,規劃時間有時可能更長,且生成的路徑往往不是最優,文獻[13]提出一種基于目標偏向策略的P概率RRT算法,采樣時設定一個參數值Pa和一個（0,1）范圍內的隨機值P,當Pa

針對現有研究中存在的問題,本文通過變步長的方法對A*算法進行改進,縮短規劃時間; 并在全局路徑的折點處基于車輛運動學模型進行局部重規劃,使路徑滿足車輛運動學約束,易于跟蹤; 此外還引入了障礙物延伸策略,提高了路徑的安全性,最后通過仿真驗證了本文改進算法的有效性.

1 A*算法

1.1 A＊算法的原理

A*算法是一種啟發式搜索算法,在人工智能領域,常被用于解決各種路徑規劃問題,其在尋路前需先將地圖柵格化,接著生成兩個列表,一個是存放即將搜索的節點的open列表,一個是存放當前open列表中代價最小節點的closed列表,在開始尋路時,首先將起點放入closed列表,再將起點周圍的8個臨近柵格放入open列表,之后根據成本函數f（n）=g（n）+h（n）,從open列表中選出成本最小的節點,并將其存入closed列表中,循環執行這一過程,直到目標點出現在open列表中為止.此時將目標點加入closed列表,然后在closed列表里從目標點依次返回到起點,便可獲得最小成本路徑.具體流程圖如圖1所示.

圖1 A*算法流程圖

在成本函數f（n）=g（n）+h（n）中,g（n）為過去成本函數,表示當前節點到起始點的成本,h（n）為啟發函數,表示當前節點到目標點的成本,在二維柵格化環境中,通常用歐氏距離來表示兩個點之間的成本.

1.2 變步長A＊算法

在用A*算法進行路徑規劃時,需要先對無人地面車輛周圍的環境進行柵格化處理,此時若搜索步長（指每個柵格的邊長）設置較小,則對車輛周圍環境的刻畫會更精確,但隨之計算量會變大,規劃的效率顯著降低;若搜索步長設置較大,可以縮短規劃時間,但此時對障礙物輪廓的刻畫不準確,導致在離障礙物較近時,可能會出現規劃不出路徑的情況,甚至有時會帶來安全隱患[14]; 當前在用A*算法進行路徑規劃時,出于安全考慮,搜索步長通常較小,導致規劃效率低.

為此,本文提出一種變步長A*算法,通過設置子目標點的方式計算當前合適的搜索步長,達到縮短規劃時間的效果,設置子目標點的具體步驟如下:

（1）首先通過雷達檢測并獲取車輛與周圍障礙物之間的最短距離Dz,此時可保證以車輛為圓心,Dz為半徑的圓形區域是無障礙的安全區域.

（2）當Dz大于γDs時（Ds為UGV當前車速的下最短剎車距離,γ為安全系數）,如圖2所示,此時先連接起點Ps與目標點Pg,得到虛擬最短路徑LPsPg,再將以車輛當前位置為圓心,Dz為半徑的圓弧與LPsPg的交點Pe設置為子目標點.

圖2 子目標點選取示意圖

如圖2所示,在確定了子目標點Pe之后,將起點Ps與子目標點Pe分別作為一組對角柵格的中心點,便可得到一個分辨率為2×2的方形柵格地圖（如圖2中的點虛線區域所示）,此時的搜索步長為:

式中,Step為搜索步長,LPsPe為起點Ps至子目標點Pe的距離.另外由于起點Ps至子目標點Pe之間為無障礙的安全區域,故可將此2×2分辨率的柵格地圖視作無障礙區域.

而當Dz小于γDs時,若仍采用低分辨率地圖模型,由前文可知容易帶來安全隱患,故此時回歸傳統A*算法尋路,搜索步長設置為常數N.

綜上,UGV在不同環境中的搜索步長設置方法可用式（2）表達:

式（2）中,Ds為車輛當前車速v下的最短剎車距離,γ為安全系數,Ds的計算方法如式（3）所示.

式（3）中,v為車速,μ為地面的摩擦系數,g為重力加速度.

安全系數γ與車速v的關系如式（4）所示.

2 局部重規劃

采用變步長搜索方式對A*算法作出改進后,可以提高規劃效率,但其并未改變A*算法的尋路原理,因此規劃出的路徑仍然存在諸多折點,而UGV由于執行機構之間存在約束,導致其無法準確跟蹤曲率不連續的路徑[15].針對這一問題,本文基于UGV的穩態轉向模型對變步長A*算法規劃出的全局路徑中的折點位置進行局部重規劃,使規劃出的路徑滿足UGV轉向運動學約束,易于跟蹤.

2.1 車輛轉向模型分析

考慮到UGV的實時性要求,需要減少計算量,因此對模型做出如下簡化假設:

① UGV在跟蹤路徑過程中,質心側偏角保持不變;

② 內外輪轉向角相同;

③ 忽略UGV的側傾與俯仰運動;

④ 假設懸架為剛體,忽略垂向運動.

在此基礎上,UGV的穩態轉向運動學模型如圖3所示,圖中P為車輛轉向時的瞬時轉動中心,ω為UGV的橫擺角速度,v為質心的速度,vx與vy分別為v在車輛坐標系oxy下x軸和y軸的分量,δ為前輪轉角,lo為質心o的預測軌跡,a和b分別為前后軸到質心的距離.

根據汽車理論[16]與UGV的穩態轉向模型可知,此時車輛的轉向靈敏度（又稱橫擺角速度增益）為:

故橫擺角速度ω為:

式中,K為穩定性系數,是反映車身穩態響應能力的重要參數,具體的計算方法為:

其中,m為車身質量,k1和k2分別為前后輪胎的側偏剛度.

穩態時車輛的轉彎半徑R為:

假設車輛質心在當前位置o（Xo,Yo）時的橫擺角為φo（圖3中的φo=0°）,經過時間 t到達 o′（（Xo′,Yo′））處,則車身位置與橫擺角的變化量為:

圖3 UGV穩態轉向模型

因此,由式（9）-式（11）可知,經過t時刻后質心o′的位置與橫擺角為:

由此可知,在給定車速v下,輸入一個前輪轉角δ（前輪轉角范圍受車輛機械結構約束）,可推知經過時間t后車輛的位置與橫擺角,因此可為行進中的車輛建立如圖4所示的預測軌跡集合,圖中loi（i=1,2,3,…）為質心o的預測軌跡.

圖4 車輛質心預測軌跡集合

2.2 局部路徑生成

如圖5所示,在變步長A*算法規劃出全局路徑Lc之后,在Lc的折點處根據車身穩態轉向模型生成一組預測軌跡集合loi（i=1,2,3,…）,之后根據設計的評價函數從loi（i=1,2,3,…）中選擇一條評價值最高的路徑log作為局部重規劃路徑.

圖5 局部路徑平滑示意圖

為了使選擇的最優軌跡log能夠安全避開障礙物的同時,長度盡可能短,且能較好貼合Lc,本文設計選擇局部重規劃路徑log的評價函數為:

式（13）中,Di為車輛質心o到Pri（i=1,2,3…）之間的路徑長度,θ為弧線路徑Li上點Pri處的切線Lq與初始路徑Lc的夾角,Ki為每條弧線路徑loi的曲率,Ob為障礙物,α1,α2,α3為每項的權重系數.

權重系數α1,α2,α3對應著不同的行駛要求,增大α1,UGV偏向走距離較短的路徑; 增大α2,UGV偏向走與全局路徑能較好貼合的路徑; 增大α3,UGV偏向走曲率較小的路徑,注重行駛穩定性; 根據UGV行駛環境可以改變權重系數的大小,來滿足不同的行駛要求.

3 障礙物延伸策略

在UGV的路徑規劃仿真實驗中,通常不考慮車身的實際寬度,這就導致規劃出的路徑可能會緊貼障礙物的邊緣,在實際工程應用中容易帶來安全隱患.為此,本文提出一種障礙物延伸策略,如圖6所示,在障礙物的外邊緣處,沿障礙物外邊緣曲線的法線方向延伸一段距離Le,考慮到車輛質心與車身兩側的距離為車寬的一半,且實際行駛時UGV與障礙物需保持一定的安全邊距Da,故本文將延伸距離Le設置為:

圖6 障礙物延伸示意圖

式中,w為車輛的寬度,單位為m;Da為安全邊距,單位為m.

其中,Da與車速v的關系如式（15）所示:

式中,v為車輛速度,單位為km/h.

4 仿真驗證

為了驗證本文改進算法的有效性,用Matlab 2018b進行仿真實驗,計算機配置為Intel（R）Core（TM）i5-4200U 2.30 GHz CPU、內存為8.00 GB.

4.1 本文改進算法與傳統A＊算法仿真分析

本文首先創建了一個包含障礙物的地圖模型,地圖分辨率為30×30,起點坐標為（5,4）,目標點坐標為（27,26）,黑色部分為障礙物區域,白色部分為無障礙區域,在此地圖上分別運用傳統A*算法與變步長A*算法進行路徑規劃,為便于對比分析,設置A*算法的搜索步長N=1; 變步長A*算法的相關參數設置如下:設定路徑跟蹤時的車速v=10 km/h,道路摩擦系數μ=0.9,重力加速度g=9.8 m/s2.仿真結果如圖7所示.

圖7 傳統A*與變步長A*算法仿真結果

由圖7可知,傳統A*算法在進行路徑規劃時,即便在距離障礙物較遠的空曠區域,仍會生成許多冗余的路徑節點,導致搜索效率下降,而在經過變步長的優化后,可以發現起點至A點、B點至目標點之間的區域,路徑節點明顯減少,由此達到縮短規劃時間的效果,在A點至B點之間的區域,路徑節點數量沒有減少,這是因為此段區間UGV距離障礙物較近,應將搜索步長重新設置為N,即搜索步長為1.

變步長A*算法可以縮短規劃時間,但是生成的路徑仍然存在折點,不滿足UGV的轉向運動學約束,且路徑緊貼障礙物邊緣,不滿足工程實際,為此,在變步長的基礎上再依次進行局部重規劃和引入障礙物延伸策略,其中相關參數設置如下:α1=1,α2=2,α3=1; 車身寬度w=1.6 m,v=10 km/h.仿真結果如圖8、圖9所示.

由圖8可知,在進行局部重規劃后,全局路徑的折點處得到了較好的平滑,有利于UGV跟蹤,但在CD區間段的路徑與障礙物距離較近.

圖8 加入局部重規劃

如圖9所示,引入障礙物延伸策略之后得到的路徑,即為本文改進算法所規劃出的最終路徑,從圖中可以發現路徑與障礙物之間保持了一定的邊距,提升了路徑的安全性,更符合工程實際.

圖9 引入障礙物延伸策略

傳統A*算法與本文改進算法的尋路時間和路徑長度數據如圖10所示,從圖中可以發現,本文改進的算法相比傳統A*算法,尋路時間縮短了50.6%,而路徑長度略長于傳統A*算法,這是因為本文改進算法加入了障礙物延伸策略,生成的路徑與障礙物保持了安全間距,導致路徑長度也隨之增加.

圖10 本文改進算法與傳統A*算法路徑長度與尋路時間對比

4.2 本文改進算法與其他算法仿真對比

之后本文又搭建了3組相對復雜的環境場景,并分別與文獻[12]中提出的雙向A*算法、文獻[13]中提出的P概率RRT算法進行對比.其中,3組仿真場景中的起點坐標分別設置為（5,25）,（3,15）,（4,7）; 目標點坐標分別設置為（25,5）,（25,17）,（26,22）; 仿真結果如圖11和圖12所示.

由圖11可知,在3組不同的仿真場景下,相比雙向A*算法與P概率RRT算法,本文改進的算法規劃出的路徑更平滑,且與障礙物保持了安全邊距.

圖11 不同算法尋路效果對比

由圖12可知,在3組不同的仿真場景中,本文改進算法的規劃時間均為最短,其中在圖12（a）場景中,本文改進算法的規劃時間相比雙向A*算法減少了8.3%,相比P概率RRT算法減少了24.6%; 在圖12（b）場景中,本文改進算法的規劃時間相比雙向A*算法減少了11.8%,相比P概率RRT算法減少了31.3%; 在圖12（c）場景中,本文改進算法的規劃時間相比雙向A*算法減少了14.2%,相比P概率RRT算法減少了18.7%; 而在3組仿真場景中,本文改進算法規劃出的路徑長度略長于雙向A*算法,這是因為本文改進算法加入了障礙物延伸策略,導致生成的路徑無法緊貼障礙物邊緣,從而路徑長度也隨之增加; 與P概率RRT算法相比,本文改進算法的路徑長度優勢明顯,這是因為P概率RRT算法的隨機樹在擴展節點時,仍然存在一定概率往隨機方向擴展,缺乏完備的目標導向性.

圖12 不同算法規劃路徑長度與尋路時間

5 結論與展望

針對A*算法規劃效率低,且生成的路徑存在較多折點的問題,本文提出一種基于車身穩態轉向模型的變步長A*算法,首先通過設置子目標點的方式調節搜索步長,減少尋路時間; 然后在變步長A*算法生成的全局路徑的折點處,基于車輛轉向運動學約束進行局部重規劃,從而得到一條平滑路徑,更利于UGV跟蹤;改進后的算法還引入了障礙物延伸策略,提升了路徑的安全性; 最后搭建了兩組不同的環境場景進行仿真實驗,結果表明,本文改進的算法相比其他3種主流尋路算法,規劃時間更短,生成的路徑更平滑,且與障礙物保持了安全邊距.

目前的研究成果對接下來的實車試驗具有理論指導作用,下一步將搭建UGV實車平臺對提出的改進算法進行驗證.