張旭東，徐?？担u 淵，郭寧遠(yuǎn)，張 宇

（1.北京理工大學(xué)，北京 100081；2.陸軍裝甲兵學(xué)院，北京 100072）

前言

隨著汽車智能化的發(fā)展，自動(dòng)駕駛技術(shù)成為智能車輛領(lǐng)域研究的重要方向，而路徑規(guī)劃是其關(guān)鍵技術(shù)之一。智能車輛的路徑規(guī)劃是指在一定環(huán)境模型的基礎(chǔ)上，給定起點(diǎn)與目標(biāo)點(diǎn)后，按照性能指標(biāo)規(guī)劃出一條無(wú)碰撞、能安全到達(dá)目標(biāo)點(diǎn)的有效路徑［1］。

目前路徑規(guī)劃按不同算法可分為圖搜索類算法［2-3］、采樣類算法［4-5］、人工勢(shì)場(chǎng)類算法［6-8］和智能仿生類算法［9-10］等。圖搜索和采樣類算法最為常見(jiàn)，運(yùn)算速度較快，但需要路徑平滑處理以適應(yīng)車輛的運(yùn)動(dòng)特性，并常用于已知全局地圖的情況。人工勢(shì)場(chǎng)類算法對(duì)障礙物的適應(yīng)性較好，但容易陷入局部最優(yōu)解。智能仿生類算法參考了生物的行為模式和特性，相比其他算法可以得出更優(yōu)的結(jié)果，其中Lumelsky等［11］參考爬蟲（Bug）的行為模式，針對(duì)僅能獲取局部地圖和定位信息的情況，提出了具有完備性的Bug1和Bug2算法。

Bug1和Bug2面向可全向運(yùn)動(dòng)的機(jī)器人，解決其在二維平面上的局部路徑規(guī)劃問(wèn)題，要求機(jī)器人裝有接觸式傳感器以探測(cè)周圍障礙物，并有精確的實(shí)時(shí)定位。Bug1和Bug2均由朝目標(biāo)移動(dòng)和環(huán)繞障礙物兩種狀態(tài)組成，但Bug2在簡(jiǎn)單場(chǎng)景下規(guī)劃路徑的總長(zhǎng)度可能更短，而在復(fù)雜場(chǎng)景時(shí)Bug1更為穩(wěn)健。Sankaranarayanan等［12］修改了Bug2中環(huán)繞障礙物的規(guī)則，提出了在某些場(chǎng)景下規(guī)劃路徑總長(zhǎng)度更短的Alg1算法。Kamon等［13］使用測(cè)距傳感器解決了Bug類算法必須接觸障礙物的問(wèn)題，并提出了基于局部切線圖的TangentBug算法，其規(guī)劃路徑的總長(zhǎng)度隨測(cè)距傳感器探測(cè)距離的增加而減小。Kim等［14］在TangentBug的基礎(chǔ)上考慮了采用Ackermann轉(zhuǎn)向的車輛存在最小轉(zhuǎn)向半徑的運(yùn)動(dòng)約束，其規(guī)劃路徑為預(yù)瞄算法生成的圓弧而非直線段，但路徑的曲率變化較大，不利于路徑跟隨控制。Xu等［15］在小尺度未知環(huán)境范圍內(nèi)將Dubins曲線和Bug類算法思想結(jié)合，但由于中間位姿與目標(biāo)點(diǎn)的位姿相關(guān)，其仿真結(jié)果中的規(guī)劃路徑出現(xiàn)了扭結(jié)和圓環(huán)。為保證規(guī)劃路徑的安全性，文獻(xiàn)［14］和文獻(xiàn)［15］中基于局部切線圖對(duì)障礙物邊界點(diǎn)進(jìn)行擴(kuò)展的方法，在實(shí)際應(yīng)用中易受傳感器精度的制約和環(huán)境的干擾，擴(kuò)展距離往往過(guò)于保守。此外，上述算法均要求被控對(duì)象有精確的實(shí)時(shí)定位，而算法的收斂性易受定位誤差的影響。

針對(duì)現(xiàn)有研究存在的不足，本文中面向采用Ackermann轉(zhuǎn)向的智能輪式車輛，提出了一種融合TangentBug與Dubins曲線的局部路徑規(guī)劃算法。首先通過(guò)采樣法構(gòu)建規(guī)劃參考點(diǎn)集合，然后在TangentBug算法框架下生成Dubins曲線作為規(guī)劃路徑，旨在滿足車輛最小轉(zhuǎn)向半徑和目標(biāo)點(diǎn)的航向要求，并加入了碰撞檢測(cè)和考慮定位誤差的狀態(tài)轉(zhuǎn)換規(guī)則，最后通過(guò)實(shí)車實(shí)驗(yàn)，驗(yàn)證了本文所提出算法的可行性。

1 車輛運(yùn)動(dòng)學(xué)建模

對(duì)于采用Ackermann轉(zhuǎn)向的車輛，其運(yùn)動(dòng)學(xué)模型如圖1所示［16］。在慣性坐標(biāo)系OXY下，車輛后軸中心的坐標(biāo)為（Xr，Yr），車輛的橫擺角為Ψ，點(diǎn)C為車輛在轉(zhuǎn)向過(guò)程中的瞬時(shí)轉(zhuǎn)動(dòng)中心，車輛后軸中心速度為v，前輪偏角為δ，車輛軸距為lw，車輛后輪的轉(zhuǎn)向半徑為r。以車輛坐標(biāo)和橫擺角為狀態(tài)量q=［Xr，Yr，Ψ］T，以車速和橫擺角速度為控制量u=［v，ω］T，可得車輛的運(yùn)動(dòng)學(xué)模型如式（1）所示。

圖1 車輛運(yùn)動(dòng)學(xué)模型

轉(zhuǎn)向半徑與前輪偏角的關(guān)系如式（2）所示。當(dāng)前輪偏角為最大值δmax時(shí)，有最小轉(zhuǎn)向半徑rmin。

2 局部路徑規(guī)劃算法設(shè)計(jì)

本文算法與自動(dòng)駕駛系統(tǒng)結(jié)合的框架如圖2所示。本文算法對(duì)應(yīng)“局部路徑規(guī)劃”模塊，其輸入為環(huán)境感知模塊的輸出信息，包括障礙物信息和車輛及目標(biāo)位姿，其輸出為參考路徑，并作為運(yùn)動(dòng)控制模塊的輸入。局部路徑規(guī)劃模塊包含環(huán)境建模和決策規(guī)劃兩層，環(huán)境建模層用于接收障礙物信息以構(gòu)建規(guī)劃參考點(diǎn)集合和局部地圖，之后決策規(guī)劃層利用上述信息結(jié)合車輛自身和目標(biāo)的位姿生成參考路徑。

圖2 自動(dòng)駕駛系統(tǒng)框架圖

2.1 基于采樣的規(guī)劃參考點(diǎn)集合構(gòu)建

假設(shè)在二維平面R2上有一點(diǎn)P，其狀態(tài)q=［X，Y，Ψ］T，以此構(gòu)建如圖3所示的車輛坐標(biāo)系oxy，其中點(diǎn)P與點(diǎn)o重合。對(duì)于可以在任意方向上獲取障礙物距離信息的測(cè)距傳感器，在狀態(tài)q和方位角φ上獲得的最近障礙物距離ρ（q，φ）可用式（3）表示。

圖3 車輛坐標(biāo)系與障礙物信息

式中:d（a，b）為點(diǎn)a與點(diǎn)b在二維平面上的歐氏距離；λ為比例系數(shù)，取值范圍在［0，∞］，λ[cosφ，sinφ]T表示模長(zhǎng)為λ、輻角為φ的向量；WOi表示第i個(gè)障礙物。

在實(shí)際應(yīng)用中，由于測(cè)距傳感器存在角度分辨率φr，故方位角φ是離散的而非連續(xù)的，離散的角度用φn表示，此外還受最大探測(cè)距離ρmax的限制。因此重新定義測(cè)距傳感器在不同方向上獲取的距離ρr（q，φn），如式（4）所示。

式中atan2（a，b）表示點(diǎn)a相對(duì)于點(diǎn)b的方位角。

對(duì)于沒(méi)有障礙物遮擋，位于測(cè)距傳感器探測(cè)邊界的點(diǎn)b，定義其構(gòu)成的集合B為

在障礙物的邊界附近，距離ρr（q，φn）會(huì)發(fā)生較大的變化，因此通過(guò)對(duì)sn相鄰點(diǎn)的距離進(jìn)行分析，可得到不同類型的障礙物邊界點(diǎn)。對(duì)于障礙物邊界點(diǎn)的旋轉(zhuǎn)采樣，可分為順時(shí)針和逆時(shí)針采樣兩種情況。如圖4所示，點(diǎn)O1為順時(shí)針采樣的邊界點(diǎn)，點(diǎn)O2為逆時(shí)針采樣的邊界點(diǎn)。對(duì)于集合S中的任意一點(diǎn)sn，令sn-1，sn-2，...，sn-m表示該點(diǎn)順時(shí)針?lè)较蛏舷噜彽膍個(gè)點(diǎn)，令sn+1，sn+2，...，sn+m表示該點(diǎn)逆時(shí)針?lè)较蛏舷噜彽膍個(gè)點(diǎn)。設(shè)用于障礙物邊界點(diǎn)判斷的距離閾值為de，Ecw為順時(shí)針采樣的邊界點(diǎn)集合，Eccw為逆時(shí)針采樣的邊界點(diǎn)集合，那么Ecw和Eccw可分別用式（7）和式（8）表示。

圖4 障礙物邊界點(diǎn)的旋轉(zhuǎn)采樣

式中:ksafe為安全系數(shù)；rsafe為安全距離。

然后確定旋轉(zhuǎn)角度的上限θsample，這里令θsample=ksampleθsafe，ksample為采樣比例系數(shù)。采樣的步長(zhǎng)取為傳感器的角度分辨率φr，那么逆時(shí)針旋轉(zhuǎn)采樣點(diǎn)的集合E′ccw為

同理可得順時(shí)針旋轉(zhuǎn)采樣點(diǎn)的集合E′cw為

在本文的規(guī)劃算法中，作為規(guī)劃參考的點(diǎn)有3種:一是不受障礙物遮擋，位于測(cè)距傳感器探測(cè)邊界上的點(diǎn)；二是基于障礙物邊界點(diǎn)旋轉(zhuǎn)采樣后的點(diǎn)；三是目標(biāo)點(diǎn)T。對(duì)于集合T′，若T位于傳感器探測(cè)范圍內(nèi)，則T′=｛T｝，否則T′=?。當(dāng)考慮全部方向時(shí)，設(shè)規(guī)劃參考點(diǎn)的集合為F，則

若僅考慮自身前方信息，設(shè)規(guī)劃參考點(diǎn)的集合為F′，則

2.2 沿規(guī)劃路徑的碰撞檢測(cè)

本文算法通過(guò)沿規(guī)劃出的Dubins曲線進(jìn)行嚴(yán)格的碰撞檢測(cè)以保證規(guī)劃路徑的安全性。對(duì)存在最小轉(zhuǎn)向半徑且不能后退的車輛，在朝各個(gè)方向運(yùn)動(dòng)時(shí)的最短路徑可用Dubins曲線表示［17］。與圓弧路徑相比，Dubins曲線在滿足目標(biāo)點(diǎn)的航向要求的情況下，使規(guī)劃路徑最短。

如圖5所示，采用覆蓋圓將車輛包圍，其圓心分別位于車輛中心，前軸中心和后軸中心，覆蓋圓的半徑為rsafe。若任意覆蓋圓內(nèi)存在障礙物，則認(rèn)為存在碰撞的風(fēng)險(xiǎn)。從Dubins曲線起點(diǎn)S開(kāi)始，沿曲線以固定步長(zhǎng)選取車輛位置參考點(diǎn)，該參考點(diǎn)與車輛后軸中心點(diǎn)重合，參考點(diǎn)處切向量的方向?yàn)檐囕v的航向。若從規(guī)劃出的Dubins曲線的起點(diǎn)到終點(diǎn)均通過(guò)了碰撞檢測(cè)，那么認(rèn)為該規(guī)劃路徑是安全的，否則認(rèn)為有碰撞的風(fēng)險(xiǎn)。

圖5 沿規(guī)劃路徑進(jìn)行碰撞檢測(cè)

2.3 決策規(guī)劃算法

本文的局部路徑規(guī)劃算法基于以下假設(shè):車輛和障礙物均位于二維平面上；障礙物的數(shù)量和周長(zhǎng)有限；車輛配有測(cè)距傳感器，可比較準(zhǔn)確地獲取探測(cè)范圍內(nèi)各個(gè)方向上障礙物的距離；車輛配有定位系統(tǒng)，可比較準(zhǔn)確地定位；目標(biāo)的位姿已知；車輛只能向前移動(dòng)；車輛采用Ackermann轉(zhuǎn)向。

本文的局部路徑規(guī)劃以TangentBug算法為基本框架，同樣由朝目標(biāo)移動(dòng)和環(huán)繞障礙物兩種狀態(tài)組成，以保證本文算法的完備性。但本文算法在生成參考路徑時(shí)借助規(guī)劃參考點(diǎn)集合，生成了Dubins曲線以代替TangentBug中的直線并考慮了其安全性，在處理狀態(tài)轉(zhuǎn)換規(guī)則和判斷目標(biāo)點(diǎn)是否可到達(dá)時(shí)考慮了定位系統(tǒng)的誤差，且無(wú)須提前規(guī)定環(huán)繞障礙物的方向。局部路徑規(guī)劃算法的偽代碼如表1所示，其中:unreachable為判斷目標(biāo)點(diǎn)是否可以到達(dá)的旗標(biāo)，初始化時(shí)令unreachable=false；ld為生成的Dubins曲線長(zhǎng)度，dt為距離閾值，若規(guī)劃參考點(diǎn)為目標(biāo)點(diǎn)且ld＜dt，即認(rèn)為車輛到達(dá)目標(biāo)點(diǎn)附近，dt的取值可根據(jù)車輛的幾何尺寸或?qū)嶋H需要設(shè)定；d′（P，T）用于記錄車輛與目標(biāo)點(diǎn)的距離。

表1 局部路徑規(guī)劃算法

2.3.1 朝目標(biāo)移動(dòng)狀態(tài)

朝目標(biāo)移動(dòng)狀態(tài)算法的偽代碼如表2所示。其中state為狀態(tài)旗標(biāo)，若state=“motion toward the target”，車輛則處于朝目標(biāo)移動(dòng)的狀態(tài)，state初始化為此值。若state=“boundary following”，車輛則處于環(huán)繞障礙物的狀態(tài)。

表2 朝目標(biāo)移動(dòng)狀態(tài)算法

在朝目標(biāo)移動(dòng)狀態(tài)時(shí)，設(shè)目標(biāo)點(diǎn)T在車輛坐標(biāo)系下為點(diǎn)t，集合F中任意元素fi的代價(jià)cfi為車輛自身到該元素的距離加該元素到目標(biāo)點(diǎn)的歐式距離，即

按代價(jià)由小到大的順序，依次生成Dubins曲線并進(jìn)行碰撞檢測(cè)，一旦通過(guò)碰撞檢測(cè)，則以此Dubins曲線作為局部參考路徑，否則認(rèn)為沒(méi)有安全的路徑并停車。如圖6所示，在車輛坐標(biāo)系oxy下，Dubins曲線的起點(diǎn)在原點(diǎn)o處，其狀態(tài)可表示為:qo=［0，0，0］T。對(duì)于集合F中的元素fi，其在車輛坐標(biāo)系下的坐標(biāo)為（xfi，yfi），方位角φf(shuō)i=atan2（fi，o），故Dubins曲線的終點(diǎn)狀態(tài)為:qfi=［xfi，yfi，φf(shuō)i］T。

圖6 生成Dubins曲線

當(dāng)目標(biāo)點(diǎn)進(jìn)入傳感器探測(cè)范圍后，首先嘗試滿足目標(biāo)點(diǎn)處的航向角要求。設(shè)目標(biāo)點(diǎn)T在車輛坐標(biāo)系下的狀態(tài)為qt=［xt，yt，φt］T，那么首先嘗試以qo為起點(diǎn)，qt為終點(diǎn)生成Dubins曲線，若成功，則以此為參考路徑。否則，計(jì)算此時(shí)車輛坐標(biāo)系下的目標(biāo)點(diǎn)t的方位角φt′=atan2（t，o），并嘗試以qt′=［xt，yt，φt′］T為終點(diǎn)生成Dubins曲線。

對(duì)于假設(shè)車輛可全向移動(dòng)的Bug算法，在朝目標(biāo)移動(dòng)的狀態(tài)時(shí)，一旦出現(xiàn)距離d（P，T）增大的情況，則認(rèn)為存在局部最近點(diǎn)Mi，此時(shí)車輛的位置記作SWi，見(jiàn)圖7。但對(duì)于存在最小轉(zhuǎn)向半徑的車輛，即使不受障礙物的影響，也會(huì)因?yàn)樽陨頎顟B(tài)等原因遠(yuǎn)離目標(biāo)點(diǎn)，如圖8所示。此外，由于定位系統(tǒng)存在誤差，即使車輛實(shí)際沒(méi)有遠(yuǎn)離目標(biāo)，也會(huì)出現(xiàn)距離d（P，T）增大的情況。而根據(jù)文獻(xiàn)［13］可知:在TangentBug算法中，距離d（P，T）在朝目標(biāo)移動(dòng)狀態(tài)下是非增的。因此，對(duì)于在某一時(shí)刻i的距離d′i（P，T），在朝目標(biāo)移動(dòng)狀態(tài)時(shí)，除按目標(biāo)點(diǎn)航向規(guī)劃成功外，其取值由式（16）和式（17）確定，此時(shí)d′i（P，T）為當(dāng)前時(shí)刻i之前到目標(biāo)點(diǎn)距離的最小值。而其他情況下均為d′i（P，T）=di（P，T）。

圖7 車輛軌跡示意圖

圖8 車輛因最小轉(zhuǎn)向半徑而遠(yuǎn)離目標(biāo)

設(shè)一距離閾值dm，若在某一時(shí)刻i，有式（18）成立，那么認(rèn)為存在局部最近點(diǎn)Mi，隨即切換為環(huán)繞障礙物狀態(tài)。當(dāng)在任何情況下均令d′i(P，T)=di（P，T），且dm=0時(shí)，對(duì)應(yīng)定位系統(tǒng)無(wú)誤差的理想情況。dm應(yīng)結(jié)合定位系統(tǒng)的誤差取一較小的正值。

2.3.2 環(huán)繞障礙物狀態(tài)

環(huán)繞障礙物狀態(tài)算法的偽代碼如表3所示。當(dāng)處于環(huán)繞障礙物狀態(tài)時(shí)，首先更新局部最近點(diǎn)Mi。設(shè)點(diǎn)Mi到目標(biāo)點(diǎn)的距離為dmin，在初始化時(shí)令dmin=∞。若有式（19）成立，則更新dmin，并記錄局部最近點(diǎn)Mi和車輛此時(shí)的位置SWi。

表3 環(huán)繞障礙物狀態(tài)算法

設(shè)一集合L，有

若?li∈L，使得從qo到qli=［xli，yli，atan2（li，o）］T生成的Dubins曲線通過(guò)碰撞檢測(cè)，令dleave=d（li，t），那么有式（21）成立，即在集合F中存在一元素比局部最近點(diǎn)到目標(biāo)點(diǎn)的距離更小，且生成的Dubins曲線通過(guò)碰撞檢測(cè)。隨后車輛切換為朝目標(biāo)移動(dòng)狀態(tài)。記此時(shí)在慣性坐標(biāo)系下的車輛坐標(biāo)為點(diǎn)Li，li的坐標(biāo)為L(zhǎng)Ri。在環(huán)繞障礙物狀態(tài)下，dleave需要實(shí)時(shí)更新，在初始化時(shí)dleave=d（S，T）。

環(huán)繞障礙物的方向由狀態(tài)轉(zhuǎn)換時(shí)記錄的目標(biāo)方位確定，若此時(shí)atan2（t，o）＜0，則環(huán)繞方向?yàn)轫槙r(shí)針，否則為逆時(shí)針。以逆時(shí)針為例，如圖7所示，車輛從狀態(tài)轉(zhuǎn)換點(diǎn)SWi處開(kāi)始逆時(shí)針環(huán)繞障礙物，直到點(diǎn)Li處駛離障礙物。為防止在環(huán)繞障礙物狀態(tài)下出現(xiàn)規(guī)劃路徑的局部振蕩，以僅考慮自身前方信息的集合F′作為規(guī)劃參考點(diǎn)集合。逆時(shí)針環(huán)繞時(shí)，按方位角由大到小的順序?qū)螰′中的元素進(jìn)行排序，而順時(shí)針時(shí)，按由小到大的順序排序。依次生成Dubins曲線并進(jìn)行碰撞檢測(cè)，一旦通過(guò)則以此作為參考路徑，否則認(rèn)為沒(méi)有安全的路徑并停車。

對(duì)于環(huán)繞障礙物一周的情況，由以下規(guī)則判斷。

（1）若存在一車輛位置o，滿足d（swi，o）＞dsw且|atan2（swi，o）|＞π/2，則認(rèn)為車輛駛離狀態(tài)轉(zhuǎn)換點(diǎn)SWi，其中點(diǎn)swi為點(diǎn)SWi在車輛坐標(biāo)系下的位置，dsw為距離閾值，可根據(jù)定位系統(tǒng)的誤差取一較小的正值。

（2）在滿足條件（1）的情況下，若存在一車輛位置o，滿足d（swi，o）＜dsw且|atan2（swi，o）|＜π/2，則認(rèn)為車輛再次回到點(diǎn)SWi。

當(dāng)條件（1）和（2）均滿足時(shí)，認(rèn)為車輛繞障礙物一周回到了狀態(tài)轉(zhuǎn)換點(diǎn)SWi，即得出目標(biāo)不可到達(dá)的結(jié)論并停車。

2.3.3 算法收斂性證明

車輛存在最小轉(zhuǎn)向半徑的運(yùn)動(dòng)約束不會(huì)影響本文算法的收斂性。如圖8所示，若目標(biāo)點(diǎn)位于測(cè)距傳感器探測(cè)范圍內(nèi)，則車輛保持朝目標(biāo)移動(dòng)狀態(tài)前進(jìn)至目標(biāo)點(diǎn)。若目標(biāo)點(diǎn)位于測(cè)距傳感器探測(cè)范圍外，由于受最小轉(zhuǎn)向半徑的運(yùn)動(dòng)約束，車輛以朝目標(biāo)移動(dòng)狀態(tài)遠(yuǎn)離目標(biāo)，當(dāng)滿足式（18）后，切換為環(huán)繞障礙物狀態(tài)。若在測(cè)距傳感器探測(cè)范圍內(nèi)不存在障礙物，則距離dmin=∞；或雖然存在障礙物但未阻礙車輛運(yùn)動(dòng)，此時(shí)存在使規(guī)劃路徑通過(guò)碰撞檢測(cè)的點(diǎn)LR，隨后式（21）均成立。因此車輛的狀態(tài)將在朝目標(biāo)移動(dòng)和環(huán)繞障礙物之間切換，直到車輛在朝目標(biāo)移動(dòng)的狀態(tài)下不再滿足式（18）。而在文獻(xiàn)［13］中已經(jīng)證明上述兩種狀態(tài)的組合不會(huì)導(dǎo)致TangentBug發(fā)散，故在有限次狀態(tài)轉(zhuǎn)換后車輛將保持在朝目標(biāo)移動(dòng)的狀態(tài)到達(dá)目標(biāo)點(diǎn)。

3 實(shí)車實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)搭建

為驗(yàn)證本文算法在真實(shí)環(huán)境中的表現(xiàn)，通過(guò)如圖9所示的北京理工大學(xué)智能輪式車輛平臺(tái)進(jìn)行了實(shí)驗(yàn)。該平臺(tái)為線控電動(dòng)底盤，上位機(jī)可通過(guò)以太網(wǎng)轉(zhuǎn)CAN設(shè)備（CANet）對(duì)底盤進(jìn)行控制。傳感器包括作為測(cè)距傳感器的激光雷達(dá)和毫米波雷達(dá)，作為定位系統(tǒng)的慣性組合導(dǎo)航系統(tǒng)。軟件算法在上位機(jī)中運(yùn)行，程序指令通過(guò)CANet下發(fā)到CAN總線上，從而控制驅(qū)動(dòng)電機(jī)和轉(zhuǎn)向、制動(dòng)執(zhí)行器等設(shè)備，以實(shí)現(xiàn)車輛控制。

圖9 實(shí)驗(yàn)車輛及關(guān)鍵零部件

實(shí)驗(yàn)的軟件平臺(tái)為Ubuntu 16.04和ROSKinet?ic，編程語(yǔ)言為C++。局部路徑規(guī)劃程序接收來(lái)自環(huán)境感知程序發(fā)布的障礙物信息和定位信息，并發(fā)布一條參考路徑給運(yùn)動(dòng)控制程序。本文橫向運(yùn)動(dòng)控制采用了預(yù)瞄控制算法，縱向運(yùn)動(dòng)控制采用了PID算法。本文局部路徑規(guī)劃算法主要參數(shù)的取值見(jiàn)表4。

表4 算法主要參數(shù)取值

3.2 實(shí)車實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)環(huán)境為工業(yè)園區(qū)，障礙物主要包括建筑物、花壇、貨物和其他停放的車輛等。通過(guò)環(huán)境建模得到的規(guī)劃參考點(diǎn)集合和局部柵格地圖如圖10所示，其中黑色的點(diǎn)為障礙物邊界；藍(lán)色的箭頭為規(guī)劃參考點(diǎn)集合，外圍圓環(huán)為測(cè)距傳感器探測(cè)邊界上的點(diǎn)，其余為障礙物邊界旋轉(zhuǎn)采樣后的點(diǎn)；紅色的Dubins曲線為生成的參考路徑。

圖10 實(shí)驗(yàn)場(chǎng)景下算法實(shí)時(shí)運(yùn)行結(jié)果

實(shí)驗(yàn)測(cè)試場(chǎng)景有兩種，場(chǎng)景1只包含朝目標(biāo)移動(dòng)的狀態(tài)，場(chǎng)景2包含兩種狀態(tài)組合的情況，兩種測(cè)試場(chǎng)景均通過(guò)了測(cè)試。場(chǎng)景1中車輛軌跡長(zhǎng)度為113.248 m，場(chǎng)景2的軌跡長(zhǎng)度為140.492 m。實(shí)驗(yàn)平臺(tái)使用的上位機(jī)的配置為CPU i5?3450，8G內(nèi)存。規(guī)劃程序的控制周期固定在了0.05 s，每次循環(huán)實(shí)際計(jì)算時(shí)間如圖11所示，其中場(chǎng)景1計(jì)算時(shí)間平均值為0.004 8 s，最大值為0.030 3 s，場(chǎng)景2計(jì)算時(shí)間平均值為0.006 6 s，最大值為0.030 1 s，由此可得該算法可滿足實(shí)時(shí)運(yùn)行的要求。

圖11 每次循環(huán)算法實(shí)際計(jì)算時(shí)間

通過(guò)記錄實(shí)驗(yàn)數(shù)據(jù)，離線運(yùn)行LeGO?LOAM［18］分別得到場(chǎng)景1、2的全局地圖和車輛軌跡如圖12和圖13所示?？梢钥闯鲕囕v在行駛的過(guò)程中避開(kāi)了附近的障礙物，并在目標(biāo)處按規(guī)定航向停止。在圖12中，車輛以朝目標(biāo)移動(dòng)的狀態(tài)避開(kāi)了周圍的障礙物，最終順利到達(dá)目標(biāo)點(diǎn)處。在圖13中，車輛由于存在最小轉(zhuǎn)向半徑，從起點(diǎn)S開(kāi)始遠(yuǎn)離目標(biāo)點(diǎn)T，直到狀態(tài)轉(zhuǎn)換點(diǎn)SW1處，滿足式（18）所述條件，記錄下局部最近點(diǎn)M1后開(kāi)始逆時(shí)針環(huán)繞障礙物。直到點(diǎn)L1處，由于存在點(diǎn)LR1比點(diǎn)M1離目標(biāo)更近且生成的Dubins曲線通過(guò)了碰撞檢測(cè)，故結(jié)束環(huán)繞障礙物的狀態(tài)，并以朝目標(biāo)移動(dòng)的狀態(tài)前進(jìn)直到目標(biāo)點(diǎn)處。

圖12 場(chǎng)景1地圖和車輛軌跡

圖13 場(chǎng)景2地圖和車輛軌跡

為驗(yàn)證本文算法對(duì)定位系統(tǒng)誤差的魯棒性，將式（17）改為:di′（P，T）=di（P，T），并令式（18）中的距離閾值dm=0，即不考慮定位系統(tǒng)的誤差。重復(fù)場(chǎng)景1下的實(shí)驗(yàn)，得到的實(shí)驗(yàn)結(jié)果如圖14所示。由于實(shí)際定位誤差導(dǎo)致車輛與目標(biāo)點(diǎn)間的距離增加，使得車輛由朝目標(biāo)移動(dòng)狀態(tài)轉(zhuǎn)換為環(huán)繞障礙物狀態(tài)。由此可得，實(shí)際定位誤差會(huì)對(duì)Bug類算法的穩(wěn)定性產(chǎn)生影響，通過(guò)式（17）和對(duì)式（18）中距離閾值dm合理地取值，可避免因定位誤差造成的狀態(tài)轉(zhuǎn)換問(wèn)題，使得本文算法對(duì)定位誤差的魯棒性提高。

圖14 定位系統(tǒng)誤差對(duì)本文算法的影響

由于規(guī)劃路徑的曲率變化會(huì)直接影響運(yùn)動(dòng)控制程序的控制效果，故對(duì)本文算法生成的參考路徑的平均曲率與文獻(xiàn)［14］中用的圓弧作為參考路徑的算法進(jìn)行了對(duì)比分析。在場(chǎng)景1下兩種算法生成的參考路徑的平均曲率變化如圖15所示，正值表示左轉(zhuǎn)，負(fù)值表示右轉(zhuǎn)。對(duì)兩組數(shù)據(jù)進(jìn)行分析，分別得到統(tǒng)計(jì)特征值如表5所示，對(duì)于參考路徑平均曲率的標(biāo)準(zhǔn)差，融合圓弧曲線的算法為0.045 2，本文融合Dubins曲線的算法為0.025 3，提升了44%。對(duì)于參考路徑平均曲率的最大值，融合圓弧曲線的算法為0.128 4，本文融合Dubins曲線的算法為0.068 0，提升了47%。由此可得，本文算法更有利于路徑跟隨控制。

圖15 參考路徑平均曲率對(duì)比

表5 參考路徑平均曲率統(tǒng)計(jì)特征值

4 結(jié)論

本文中面向采用Ackermann轉(zhuǎn)向的智能輪式車輛，針對(duì)僅能獲取局部地圖和定位信息的情況，提出了一種融合TangentBug和Dubins曲線的局部路徑規(guī)劃算法。首先通過(guò)采樣的方法構(gòu)建了規(guī)劃參考點(diǎn)集合，無(wú)須構(gòu)建局部切線圖。然后以Dubins曲線作為規(guī)劃路徑，滿足了車輛最小轉(zhuǎn)向半徑的運(yùn)動(dòng)約束和目標(biāo)點(diǎn)處的航向要求，并通過(guò)加入沿規(guī)劃路徑的碰撞檢測(cè)，提升了算法的安全性。結(jié)合TangentBug的證明結(jié)果，提出了適應(yīng)定位系統(tǒng)誤差的狀態(tài)轉(zhuǎn)換規(guī)則，并證明了車輛最小轉(zhuǎn)向半徑的運(yùn)動(dòng)約束不會(huì)影響本文算法的收斂性。最后通過(guò)實(shí)車實(shí)驗(yàn)證明了本文算法在真實(shí)環(huán)境下的可行性，結(jié)果表明:本文算法可規(guī)劃出安全的局部參考路徑，使車輛按規(guī)定位姿到達(dá)目標(biāo)點(diǎn)，并可保證算法的實(shí)時(shí)性；本文提出的狀態(tài)轉(zhuǎn)換規(guī)則可有效避免定位誤差的影響，提高了本文算法對(duì)定位誤差的魯棒性；相對(duì)于使用圓弧曲線，本文通過(guò)融合Dubins曲線規(guī)劃出的路徑更有利于路徑跟隨控制。