江 洪,姜 民

（江蘇大學(xué) 機(jī)械工程學(xué)院,鎮(zhèn)江 212013）

無(wú)人地面車輛（Unmanned Ground Vehicle,UGV）,是指在各種復(fù)雜環(huán)境中可以自主完成行駛?cè)蝿?wù)的車輛,近年來(lái),隨著定位導(dǎo)航與控制技術(shù)的發(fā)展,UGV憑借可以代替人類完成諸多繁重工作的優(yōu)勢(shì),已被廣泛應(yīng)用于軍事、民用與科研等領(lǐng)域,其相關(guān)技術(shù)的研究也越來(lái)越受到人們的重視,尤其是UGV在復(fù)雜環(huán)境下的路徑規(guī)劃問(wèn)題一直是一個(gè)重要的熱點(diǎn)研究方向[1].

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

為此,有學(xué)者提出了一些改進(jìn)辦法:文獻(xiàn)[12]中提出了一種雙向A*算法,該方法通過(guò)從起點(diǎn)和目標(biāo)點(diǎn)同時(shí)運(yùn)行A*算法,達(dá)到縮短規(guī)劃時(shí)間的目的,但該方法容易受實(shí)際地圖環(huán)境影響,規(guī)劃時(shí)間有時(shí)可能更長(zhǎng),且生成的路徑往往不是最優(yōu),文獻(xiàn)[13]提出一種基于目標(biāo)偏向策略的P概率RRT算法,采樣時(shí)設(shè)定一個(gè)參數(shù)值Pa和一個(gè)（0,1）范圍內(nèi)的隨機(jī)值P,當(dāng)Pa

針對(duì)現(xiàn)有研究中存在的問(wèn)題,本文通過(guò)變步長(zhǎng)的方法對(duì)A*算法進(jìn)行改進(jìn),縮短規(guī)劃時(shí)間; 并在全局路徑的折點(diǎn)處基于車輛運(yùn)動(dòng)學(xué)模型進(jìn)行局部重規(guī)劃,使路徑滿足車輛運(yùn)動(dòng)學(xué)約束,易于跟蹤; 此外還引入了障礙物延伸策略,提高了路徑的安全性,最后通過(guò)仿真驗(yàn)證了本文改進(jìn)算法的有效性.

1 A*算法

1.1 A＊算法的原理

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

圖1 A*算法流程圖

在成本函數(shù)f（n）=g（n）+h（n）中,g（n）為過(guò)去成本函數(shù),表示當(dāng)前節(jié)點(diǎn)到起始點(diǎn)的成本,h（n）為啟發(fā)函數(shù),表示當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的成本,在二維柵格化環(huán)境中,通常用歐氏距離來(lái)表示兩個(gè)點(diǎn)之間的成本.

1.2 變步長(zhǎng)A＊算法

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

為此,本文提出一種變步長(zhǎng)A*算法,通過(guò)設(shè)置子目標(biāo)點(diǎn)的方式計(jì)算當(dāng)前合適的搜索步長(zhǎng),達(dá)到縮短規(guī)劃時(shí)間的效果,設(shè)置子目標(biāo)點(diǎn)的具體步驟如下:

（1）首先通過(guò)雷達(dá)檢測(cè)并獲取車輛與周圍障礙物之間的最短距離Dz,此時(shí)可保證以車輛為圓心,Dz為半徑的圓形區(qū)域是無(wú)障礙的安全區(qū)域.

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

圖2 子目標(biāo)點(diǎn)選取示意圖

如圖2所示,在確定了子目標(biāo)點(diǎn)Pe之后,將起點(diǎn)Ps與子目標(biāo)點(diǎn)Pe分別作為一組對(duì)角柵格的中心點(diǎn),便可得到一個(gè)分辨率為2×2的方形柵格地圖（如圖2中的點(diǎn)虛線區(qū)域所示）,此時(shí)的搜索步長(zhǎng)為:

式中,Step為搜索步長(zhǎng),LPsPe為起點(diǎn)Ps至子目標(biāo)點(diǎn)Pe的距離.另外由于起點(diǎn)Ps至子目標(biāo)點(diǎn)Pe之間為無(wú)障礙的安全區(qū)域,故可將此2×2分辨率的柵格地圖視作無(wú)障礙區(qū)域.

而當(dāng)Dz小于γDs時(shí),若仍采用低分辨率地圖模型,由前文可知容易帶來(lái)安全隱患,故此時(shí)回歸傳統(tǒng)A*算法尋路,搜索步長(zhǎng)設(shè)置為常數(shù)N.

綜上,UGV在不同環(huán)境中的搜索步長(zhǎng)設(shè)置方法可用式（2）表達(dá):

式（2）中,Ds為車輛當(dāng)前車速v下的最短剎車距離,γ為安全系數(shù),Ds的計(jì)算方法如式（3）所示.

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

安全系數(shù)γ與車速v的關(guān)系如式（4）所示.

2 局部重規(guī)劃

采用變步長(zhǎng)搜索方式對(duì)A*算法作出改進(jìn)后,可以提高規(guī)劃效率,但其并未改變A*算法的尋路原理,因此規(guī)劃出的路徑仍然存在諸多折點(diǎn),而UGV由于執(zhí)行機(jī)構(gòu)之間存在約束,導(dǎo)致其無(wú)法準(zhǔn)確跟蹤曲率不連續(xù)的路徑[15].針對(duì)這一問(wèn)題,本文基于UGV的穩(wěn)態(tài)轉(zhuǎn)向模型對(duì)變步長(zhǎng)A*算法規(guī)劃出的全局路徑中的折點(diǎn)位置進(jìn)行局部重規(guī)劃,使規(guī)劃出的路徑滿足UGV轉(zhuǎn)向運(yùn)動(dòng)學(xué)約束,易于跟蹤.

2.1 車輛轉(zhuǎn)向模型分析

考慮到UGV的實(shí)時(shí)性要求,需要減少計(jì)算量,因此對(duì)模型做出如下簡(jiǎn)化假設(shè):

① UGV在跟蹤路徑過(guò)程中,質(zhì)心側(cè)偏角保持不變;

② 內(nèi)外輪轉(zhuǎn)向角相同;

③ 忽略UGV的側(cè)傾與俯仰運(yùn)動(dòng);

④ 假設(shè)懸架為剛體,忽略垂向運(yùn)動(dòng).

在此基礎(chǔ)上,UGV的穩(wěn)態(tài)轉(zhuǎn)向運(yùn)動(dòng)學(xué)模型如圖3所示,圖中P為車輛轉(zhuǎn)向時(shí)的瞬時(shí)轉(zhuǎn)動(dòng)中心,ω為UGV的橫擺角速度,v為質(zhì)心的速度,vx與vy分別為v在車輛坐標(biāo)系oxy下x軸和y軸的分量,δ為前輪轉(zhuǎn)角,lo為質(zhì)心o的預(yù)測(cè)軌跡,a和b分別為前后軸到質(zhì)心的距離.

根據(jù)汽車?yán)碚揫16]與UGV的穩(wěn)態(tài)轉(zhuǎn)向模型可知,此時(shí)車輛的轉(zhuǎn)向靈敏度（又稱橫擺角速度增益）為:

故橫擺角速度ω為:

式中,K為穩(wěn)定性系數(shù),是反映車身穩(wěn)態(tài)響應(yīng)能力的重要參數(shù),具體的計(jì)算方法為:

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

穩(wěn)態(tài)時(shí)車輛的轉(zhuǎn)彎半徑R為:

假設(shè)車輛質(zhì)心在當(dāng)前位置o（Xo,Yo）時(shí)的橫擺角為φo（圖3中的φo=0°）,經(jīng)過(guò)時(shí)間 t到達(dá) o′（（Xo′,Yo′））處,則車身位置與橫擺角的變化量為:

圖3 UGV穩(wěn)態(tài)轉(zhuǎn)向模型

因此,由式（9）-式（11）可知,經(jīng)過(guò)t時(shí)刻后質(zhì)心o′的位置與橫擺角為:

由此可知,在給定車速v下,輸入一個(gè)前輪轉(zhuǎn)角δ（前輪轉(zhuǎn)角范圍受車輛機(jī)械結(jié)構(gòu)約束）,可推知經(jīng)過(guò)時(shí)間t后車輛的位置與橫擺角,因此可為行進(jìn)中的車輛建立如圖4所示的預(yù)測(cè)軌跡集合,圖中l(wèi)oi（i=1,2,3,…）為質(zhì)心o的預(yù)測(cè)軌跡.

圖4 車輛質(zhì)心預(yù)測(cè)軌跡集合

2.2 局部路徑生成

如圖5所示,在變步長(zhǎng)A*算法規(guī)劃出全局路徑Lc之后,在Lc的折點(diǎn)處根據(jù)車身穩(wěn)態(tài)轉(zhuǎn)向模型生成一組預(yù)測(cè)軌跡集合loi（i=1,2,3,…）,之后根據(jù)設(shè)計(jì)的評(píng)價(jià)函數(shù)從loi（i=1,2,3,…）中選擇一條評(píng)價(jià)值最高的路徑log作為局部重規(guī)劃路徑.

圖5 局部路徑平滑示意圖

為了使選擇的最優(yōu)軌跡log能夠安全避開障礙物的同時(shí),長(zhǎng)度盡可能短,且能較好貼合Lc,本文設(shè)計(jì)選擇局部重規(guī)劃路徑log的評(píng)價(jià)函數(shù)為:

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

權(quán)重系數(shù)α1,α2,α3對(duì)應(yīng)著不同的行駛要求,增大α1,UGV偏向走距離較短的路徑; 增大α2,UGV偏向走與全局路徑能較好貼合的路徑; 增大α3,UGV偏向走曲率較小的路徑,注重行駛穩(wěn)定性; 根據(jù)UGV行駛環(huán)境可以改變權(quán)重系數(shù)的大小,來(lái)滿足不同的行駛要求.

3 障礙物延伸策略

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

圖6 障礙物延伸示意圖

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

其中,Da與車速v的關(guān)系如式（15）所示:

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

4 仿真驗(yàn)證

為了驗(yàn)證本文改進(jìn)算法的有效性,用Matlab 2018b進(jìn)行仿真實(shí)驗(yàn),計(jì)算機(jī)配置為Intel（R）Core（TM）i5-4200U 2.30 GHz CPU、內(nèi)存為8.00 GB.

4.1 本文改進(jìn)算法與傳統(tǒng)A＊算法仿真分析

本文首先創(chuàng)建了一個(gè)包含障礙物的地圖模型,地圖分辨率為30×30,起點(diǎn)坐標(biāo)為（5,4）,目標(biāo)點(diǎn)坐標(biāo)為（27,26）,黑色部分為障礙物區(qū)域,白色部分為無(wú)障礙區(qū)域,在此地圖上分別運(yùn)用傳統(tǒng)A*算法與變步長(zhǎng)A*算法進(jìn)行路徑規(guī)劃,為便于對(duì)比分析,設(shè)置A*算法的搜索步長(zhǎng)N=1; 變步長(zhǎng)A*算法的相關(guān)參數(shù)設(shè)置如下:設(shè)定路徑跟蹤時(shí)的車速v=10 km/h,道路摩擦系數(shù)μ=0.9,重力加速度g=9.8 m/s2.仿真結(jié)果如圖7所示.

圖7 傳統(tǒng)A*與變步長(zhǎng)A*算法仿真結(jié)果

由圖7可知,傳統(tǒng)A*算法在進(jìn)行路徑規(guī)劃時(shí),即便在距離障礙物較遠(yuǎn)的空曠區(qū)域,仍會(huì)生成許多冗余的路徑節(jié)點(diǎn),導(dǎo)致搜索效率下降,而在經(jīng)過(guò)變步長(zhǎng)的優(yōu)化后,可以發(fā)現(xiàn)起點(diǎn)至A點(diǎn)、B點(diǎn)至目標(biāo)點(diǎn)之間的區(qū)域,路徑節(jié)點(diǎn)明顯減少,由此達(dá)到縮短規(guī)劃時(shí)間的效果,在A點(diǎn)至B點(diǎn)之間的區(qū)域,路徑節(jié)點(diǎn)數(shù)量沒有減少,這是因?yàn)榇硕螀^(qū)間UGV距離障礙物較近,應(yīng)將搜索步長(zhǎng)重新設(shè)置為N,即搜索步長(zhǎng)為1.

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

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

圖8 加入局部重規(guī)劃

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

圖9 引入障礙物延伸策略

傳統(tǒng)A*算法與本文改進(jìn)算法的尋路時(shí)間和路徑長(zhǎng)度數(shù)據(jù)如圖10所示,從圖中可以發(fā)現(xiàn),本文改進(jìn)的算法相比傳統(tǒng)A*算法,尋路時(shí)間縮短了50.6%,而路徑長(zhǎng)度略長(zhǎng)于傳統(tǒng)A*算法,這是因?yàn)楸疚母倪M(jìn)算法加入了障礙物延伸策略,生成的路徑與障礙物保持了安全間距,導(dǎo)致路徑長(zhǎng)度也隨之增加.

圖10 本文改進(jìn)算法與傳統(tǒng)A*算法路徑長(zhǎng)度與尋路時(shí)間對(duì)比

4.2 本文改進(jìn)算法與其他算法仿真對(duì)比

之后本文又搭建了3組相對(duì)復(fù)雜的環(huán)境場(chǎng)景,并分別與文獻(xiàn)[12]中提出的雙向A*算法、文獻(xiàn)[13]中提出的P概率RRT算法進(jìn)行對(duì)比.其中,3組仿真場(chǎng)景中的起點(diǎn)坐標(biāo)分別設(shè)置為（5,25）,（3,15）,（4,7）; 目標(biāo)點(diǎn)坐標(biāo)分別設(shè)置為（25,5）,（25,17）,（26,22）; 仿真結(jié)果如圖11和圖12所示.

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

圖11 不同算法尋路效果對(duì)比

由圖12可知,在3組不同的仿真場(chǎng)景中,本文改進(jìn)算法的規(guī)劃時(shí)間均為最短,其中在圖12（a）場(chǎng)景中,本文改進(jìn)算法的規(guī)劃時(shí)間相比雙向A*算法減少了8.3%,相比P概率RRT算法減少了24.6%; 在圖12（b）場(chǎng)景中,本文改進(jìn)算法的規(guī)劃時(shí)間相比雙向A*算法減少了11.8%,相比P概率RRT算法減少了31.3%; 在圖12（c）場(chǎng)景中,本文改進(jìn)算法的規(guī)劃時(shí)間相比雙向A*算法減少了14.2%,相比P概率RRT算法減少了18.7%; 而在3組仿真場(chǎng)景中,本文改進(jìn)算法規(guī)劃出的路徑長(zhǎng)度略長(zhǎng)于雙向A*算法,這是因?yàn)楸疚母倪M(jìn)算法加入了障礙物延伸策略,導(dǎo)致生成的路徑無(wú)法緊貼障礙物邊緣,從而路徑長(zhǎng)度也隨之增加; 與P概率RRT算法相比,本文改進(jìn)算法的路徑長(zhǎng)度優(yōu)勢(shì)明顯,這是因?yàn)镻概率RRT算法的隨機(jī)樹在擴(kuò)展節(jié)點(diǎn)時(shí),仍然存在一定概率往隨機(jī)方向擴(kuò)展,缺乏完備的目標(biāo)導(dǎo)向性.

圖12 不同算法規(guī)劃路徑長(zhǎng)度與尋路時(shí)間

5 結(jié)論與展望

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

目前的研究成果對(duì)接下來(lái)的實(shí)車試驗(yàn)具有理論指導(dǎo)作用,下一步將搭建UGV實(shí)車平臺(tái)對(duì)提出的改進(jìn)算法進(jìn)行驗(yàn)證.