非結(jié)構(gòu)化環(huán)境下自主車輛軌跡規(guī)劃方法

陳清陽，張小波，孫振平，賀漢根

(國防科學(xué)技術(shù)大學(xué) 機(jī)電工程與自動化學(xué)院，湖南 長沙，410073)

在當(dāng)今信息化社會，作為一個國家自動化和智能化水平高低的典型代表，自主駕駛車輛的研究受到了前所未有的關(guān)注，許多發(fā)達(dá)國家如美國和德國等，都投入了大量的人力物力進(jìn)行研究[1-3]，但是到目前為止相關(guān)的研究仍不能很好地解決非結(jié)構(gòu)化環(huán)境下車輛的自主導(dǎo)航。而軌跡規(guī)劃作為其中的關(guān)鍵部分，更是存在著許多問題有待解決[4-6]。面向非結(jié)構(gòu)化環(huán)境，最主要的特點便是環(huán)境的復(fù)雜性及廣泛存在的不確定性。無論是感知系統(tǒng)，還是定位系統(tǒng)，都無法為軌跡規(guī)劃提供準(zhǔn)確的信息?？赡艽嬖诘膭討B(tài)障礙物，對軌跡規(guī)劃也提出了巨大的挑戰(zhàn)。另一方面，面向?qū)嶋H車輛自主駕駛的軌跡規(guī)劃方法，運(yùn)動動力學(xué)約束與實時性之間的矛盾更是制約著車輛智能駕駛系統(tǒng)的發(fā)展。盡管目前人們對這一難題進(jìn)行了很多研究，提出了許多有創(chuàng)意的想法，如在加速度空間對機(jī)器人運(yùn)動進(jìn)行避障與目標(biāo)跟蹤規(guī)劃[7]，Qu等[8-11]所提出的多項式等數(shù)值解析方法，改進(jìn)的 A*搜索算法[12]及 Bottasso等[13-14]所提出的軌跡原子方法，F(xiàn)lores等[15]所提出的微分平坦等，這些方法都對移動機(jī)器人的運(yùn)動規(guī)劃研究起了巨大的推動作用，但是面對實際車輛運(yùn)動過程，實時性仍是一個難以解決的問題。本文作者對非結(jié)構(gòu)化環(huán)境下的自主駕駛車輛軌跡規(guī)劃問題提出了一種融合車輛運(yùn)動特性的軌跡規(guī)劃方法。本文將實現(xiàn)一種兩點約束下的可行軌跡生成方法，仿真結(jié)果顯示該方法得到的軌跡具有光滑的曲率特性，符合車輛實際運(yùn)動特性；在此基礎(chǔ)上，本文作者將基于實際車輛自主駕駛中初始狀態(tài)、目標(biāo)狀態(tài)及障礙物約束，實現(xiàn)基本可行的初始軌跡系列；最后，基于本文所提出的優(yōu)化指標(biāo)對軌跡系列進(jìn)行評價，得到一條適合車輛運(yùn)動的期望軌跡。

1 兩點約束下可行軌跡生成方法

首先介紹本文主要采用的兩點約束下可行軌跡生成方法。對問題進(jìn)行建模，如圖1所示，在參考坐標(biāo)系XOY中，為簡化問題表示，假設(shè)初始狀態(tài)即位于O點，目標(biāo)狀態(tài)位于O′(否則可通過一定的平移與旋轉(zhuǎn)操作實現(xiàn))。針對本文要研究的自主車輛軌跡規(guī)劃問題，O點的狀態(tài)表示為O(0,0,90)，其中，90代表此參考坐標(biāo)系下車輛航向與橫軸的夾角，O′點的狀態(tài)表示為O′ (x,y,α)，α代表此坐標(biāo)系下目標(biāo)點方向與橫軸的夾角，算法將解決初始狀態(tài)O與目標(biāo)狀態(tài)O′約束下可行軌跡的實現(xiàn)。

圖1 兩點約束建模表示Fig.1 Model of two-point constraints

基本思想：由幾何知識可知，基于一段固定曲率的圓弧同時滿足初始狀態(tài)與目標(biāo)狀態(tài)約束，在大部分情況下是不可能實現(xiàn)的。因此，本文采用一種漸近逼近的算法，實現(xiàn)基本滿足這兩點約束的可行解。首先，本文將目標(biāo)狀態(tài)的約束弱化為O″(x,y)，由幾何知識可知，由一段光滑的圓弧可同時滿足初始狀態(tài)與弱化目標(biāo)狀態(tài)的約束條件，具體見式(1)～(3)(在這里只列舉目標(biāo)點位于第一象限的情況，其他狀態(tài)可類比得到)。

式中：β為OO′連線與橫軸的夾角；lOO′為OO′連線的距離；(xc,yc)為圓弧上的一個采樣點c的坐標(biāo)；θc為采樣點與橫軸的夾角。

同樣的，在目標(biāo)狀態(tài)與弱化的初始狀態(tài)約束下，依據(jù)上述方法可得到一段固定曲率的圓弧。在2段圓弧上均勻采樣，可得到相應(yīng)的點對(xc,yc)與(xc′,yc′)，基于式(4)可實現(xiàn)2段圓弧的漸近過渡。

實驗中取ε= 1-i/M，η=i/M，i為圓弧上的采樣點序號，M為采樣點個數(shù)，算法得到的仿真結(jié)果如圖2所示。圖2(a)中軌跡1為基于初始狀態(tài)與弱化目標(biāo)狀態(tài)所得到的可行軌跡，軌跡2為基于目標(biāo)狀態(tài)與弱化初始狀態(tài)所得到的可行軌跡，而圖中的合成軌跡則是基于式(4)所得到的漸近過渡軌跡。由圖2(b)中相應(yīng)的合成軌跡曲率可以看到，得到的合成軌跡具有光滑的曲率特性，符合實際車輛的運(yùn)動特性。

2 初始軌跡系列生成方法

在前一部分討論了兩點約束下車輛可行軌跡的生成方法。在此基礎(chǔ)上，將面向?qū)嶋H非結(jié)構(gòu)化環(huán)境下車輛自主導(dǎo)航的需求，提出滿足實時性要求的軌跡規(guī)劃方法。首先本文作者將在前述算法的基礎(chǔ)上，在初始狀態(tài)與目標(biāo)狀態(tài)及障礙物約束下實現(xiàn)基本可行的初始軌跡系列。

2.1 初始狀態(tài)與目標(biāo)狀態(tài)約束

基于初始狀態(tài)與目標(biāo)狀態(tài)對軌跡進(jìn)行約束，首先要實現(xiàn)的是規(guī)劃坐標(biāo)系的建立及車輛狀態(tài)與目標(biāo)狀態(tài)在規(guī)劃坐標(biāo)系中的投影。為了實現(xiàn)對障礙物的準(zhǔn)確表示，本文將建立圖3所示的規(guī)劃參考坐標(biāo)系XVY，圖3中車輛位于坐標(biāo)系原點V，而目標(biāo)狀態(tài)經(jīng)過一定的平移與旋轉(zhuǎn)操作位于Y軸上的某一點。如此的坐標(biāo)系表示方法，可更準(zhǔn)確地實現(xiàn)障礙物的膨脹表示與車輛在規(guī)劃坐標(biāo)下的質(zhì)點表示。在圖3中，規(guī)劃坐標(biāo)系下的車輛方位角如圖中V點處箭頭所示方向，目標(biāo)方向則為T點處箭頭所示方向，圖3中黑色塊狀區(qū)域表示的是車上所裝傳感器實際感知到的不可通行區(qū)域。下面將討論本文所介紹的方法在實際車輛導(dǎo)航中的應(yīng)用。在圖3(a)中，由車輛所在位置V及車輛方向角(即為圖3中V處箭頭所指方向)，結(jié)合弱化的目標(biāo)狀態(tài)約束T，即目標(biāo)點T的位置，可得到固定曲率的圓弧軌跡1，即；類似地，由完整的目標(biāo)狀態(tài)T及弱化初始狀態(tài)約束，即V的位置，可得到固定曲率的圓弧軌跡2，即。在此基礎(chǔ)上，基于前面所介紹的兩段圓弧漸近過渡的方法，可實現(xiàn)可行的初始軌跡，即為圖3中合成軌跡所代表的曲線。由圖3中合成軌跡的曲率圖可以看到，合成軌跡的曲率光滑過渡，適合于實際的車輛控制。

圖2 兩點約束下軌跡生成算法仿真結(jié)果Fig.2 Simulation results of proposed algorithm under two-point constraints

2.2 障礙物約束

由前面所提到的方法獲得的基于初始狀態(tài)及目標(biāo)狀態(tài)約束的基本可行軌跡，在某些情況下并非是安全可行的。圖4所示為實車實驗過程中遇到的一幀規(guī)劃。此時圖中可行的合成軌跡明顯與黑色不可通行區(qū)域發(fā)生重疊，因此，所得到的合成軌跡實際上是不可行的。如何在前述方法的基礎(chǔ)上滿足障礙約束，這便是本節(jié)要解決的問題。

借鑒前面所介紹的兩點約束下可行軌跡生成算法，本文將采用一種多分辨率細(xì)分的方法來解決軌跡的避障問題。沿用圖4所示的場景，假設(shè)在當(dāng)前實現(xiàn)的基本可行軌跡上第k個路徑點與障礙物發(fā)生碰撞，則在第k點的基礎(chǔ)上按式(5)進(jìn)行相應(yīng)的平移變換操作，可獲得一系列不與障礙相碰的候選局部目標(biāo)點k1,k2,k3,…,kn。

在一系列候選目標(biāo)點的基礎(chǔ)上，同樣地，依據(jù)兩點約束下可行軌跡生成方法，可分別獲得由初始狀態(tài)及候選局部目標(biāo)狀態(tài)約束的可行軌跡及候選目標(biāo)狀態(tài)與目標(biāo)狀態(tài)約束的可行軌跡。由于在候選局部目標(biāo)點處同時考慮了該點的方向?qū)傩裕虼?，由與連接而成的曲線也是一條可行的規(guī)劃軌跡，具體實驗結(jié)果如圖5(a)中的點所代表的系列曲線。由圖5(b)可以看到：得到的合成軌跡曲率與圖4中合成軌跡曲率相比，盡管具有較大的跳變，但是仍是平滑過渡的，對于實際車輛的控制意味著以較低的速度行駛，這與原始障礙圖中的場景是相一致的。

可得到不與障礙物發(fā)生重疊的軌跡系列，因此，軌跡仍保持相對平滑的過渡特性。在得到的一系列基本可行軌跡的基礎(chǔ)上，基于一定的優(yōu)化目標(biāo)對軌跡系列進(jìn)行評價，得到此優(yōu)化指標(biāo)下的最優(yōu)可行軌跡。

圖3 本文所建立的規(guī)劃坐標(biāo)系及初始狀態(tài)與目標(biāo)狀態(tài)約束下得到的結(jié)果Fig.3 Representation of proposed planning frame and results under constraint of initial-final state

圖4 實車實驗中遇到的一幀規(guī)劃曲線Fig.4 Example in real vehicle experiments

圖5 障礙約束下系列軌跡生成結(jié)果Fig.5 Series of trajectories generation with obstacle constraints

3 最優(yōu)可行軌跡

在前文中詳細(xì)討論了基本可行軌跡系列的實現(xiàn)，在此軌跡系列的基礎(chǔ)上，將基于一定的優(yōu)化指標(biāo)對軌跡系列進(jìn)行評價，得到最優(yōu)的可行軌跡，保證車輛以較高的速度安全行駛。

首先，對于自主車輛的運(yùn)動，安全性是第一位的。對于軌跡規(guī)劃來說，這要求規(guī)劃的軌跡在保證可行性的基礎(chǔ)上必須離障礙物足夠遠(yuǎn)，以避免由于感知、規(guī)劃及控制等環(huán)節(jié)所存在的誤差帶來的潛在危險性。在前面介紹的軌跡系列的基礎(chǔ)上，對于某一軌跡上的采樣點p(x,y)，在感知地圖內(nèi)對其鄰近區(qū)域Q(p(x,y),D)(以p(x,y)為圓心，以D為半徑的圓區(qū)域)進(jìn)行障礙相關(guān)性判斷。若是在Q(p(x,y),D)區(qū)域內(nèi)與障礙在距離d處相交，則可得到該點的安全距離d(x,y)，否則，其安全距離便為D。在此基礎(chǔ)上，定義軌跡的安全指標(biāo)為：

需要指出的是：式(6)中的D為由于感知、規(guī)劃、控制等各方面的誤差所期望的最小安全距離，min(d(x,y))為某一規(guī)劃軌跡上所有采樣點的最小安全距離，ε為大于0的較小實數(shù)，代表在期望安全距離之外的規(guī)劃軌跡，行車安全代價為可以接受的較小值。對于該安全指標(biāo)，最優(yōu)軌跡為軌跡系列中對應(yīng)于minJ1的可行軌跡。

在保證安全可行的基礎(chǔ)上，基于規(guī)劃軌跡實現(xiàn)車輛控制自然是期望車輛能以較高的速度行駛。對于實際車輛來說，由于受到各種極限物理條件的限制，制約了車輛沿著規(guī)劃軌跡行駛所允許的最高速度。由式(7)可知：在固定的轉(zhuǎn)彎半徑，即曲線曲率的基礎(chǔ)上，車輛的行駛速度必須滿足最大側(cè)向加速度約束，否則車輛的側(cè)向摩擦力不足以提供車輛轉(zhuǎn)彎所必需的向心力，可能產(chǎn)生打滑等現(xiàn)象，這對于車輛的運(yùn)動控制來說是非常不利的。

式中：v為車輛速度；r為軌跡的相應(yīng)曲率；amax為實際車輛運(yùn)動允許的最大側(cè)向加速度；rmin為由于車輛的物理條件所允許的最小轉(zhuǎn)彎半徑。

因此，為了提高車輛的行駛速度，本文定義軌跡的效能指標(biāo)為：

式中：為某一規(guī)劃軌跡上的曲率均值；kσ為規(guī)劃軌跡的曲率方差；w3為協(xié)調(diào)兩者之間關(guān)系的權(quán)值。該指標(biāo)在考慮規(guī)劃軌跡的平均曲率的基礎(chǔ)上，同時對曲率方差進(jìn)行約束，保證完整軌跡的光滑特性，較好地體現(xiàn)了車輛行駛快速性對規(guī)劃軌跡的約束?；谠撝笜?biāo)的最優(yōu)軌跡即為軌跡系列中實現(xiàn) m inJ2的相應(yīng)軌跡。在該指標(biāo)的基礎(chǔ)上，為了滿足實際車輛的物理限制，必須滿足式(9)中的條件。

式中：φ為車輛前輪擺角；φ為前輪擺角轉(zhuǎn)速。

結(jié)合前面的討論，面向?qū)嶋H自主車輛的運(yùn)動，本文給出了如下的優(yōu)化指標(biāo)：

其中：w1和w2為對應(yīng)安全性與效能指標(biāo)的權(quán)值系數(shù)，實現(xiàn)兩者之間較好地折中，保證在各種情況下都能得到最優(yōu)的軌跡。

4 實驗結(jié)果

基于本文所提出的融合車輛運(yùn)動特性的軌跡規(guī)劃方法，作者進(jìn)行了大量的實車實驗，實驗面向的主要是復(fù)雜非結(jié)構(gòu)化環(huán)境，部分實驗場地與實驗結(jié)果如圖6所示。在實驗結(jié)果中白色的塊狀區(qū)域表示檢測到的障礙物，白色點所代表的曲線則為實際規(guī)劃的軌跡。在該規(guī)劃結(jié)果的指引下，實驗車輛可較好地完成軌跡跟蹤與安全避障任務(wù)，峰值速度可接近 30 km/h。更主要的是，在實際測試過程中，軌跡規(guī)劃周期在 100 ms以內(nèi)，滿足實際車輛自主駕駛的需求，對于智能駕駛技術(shù)的實際應(yīng)用具有巨大的推動作用。

圖6 實車實驗環(huán)境與實驗結(jié)果Fig.6 Experimental environments and results

5 結(jié)論

(1) 針對非結(jié)構(gòu)化環(huán)境下自主車輛軌跡規(guī)劃問題，首先提出一種兩點約束下可行軌跡生成算法。該算法得到的軌跡具有光滑的曲率特性，符合車輛實際運(yùn)動特性。在此基礎(chǔ)上，綜合考慮車輛初始狀態(tài)、目標(biāo)狀態(tài)及障礙物約束，實現(xiàn)基本可行的初始軌跡系列。最后為了保證自主車輛安全與快速地運(yùn)動，提出合理的優(yōu)化指標(biāo)，對軌跡系列進(jìn)行評價，從而得到期望的理想軌跡用于車輛控制。

(2) 實驗表明，該方法滿足車輛自主導(dǎo)航的實時性需求，同時考慮了車輛運(yùn)動特性，在非結(jié)構(gòu)化環(huán)境下能較好地實現(xiàn)軌跡跟蹤與避障等功能，適合復(fù)雜環(huán)境下智能車輛自主導(dǎo)航應(yīng)用。

[1]Montemerlo M,Becker J,Shat S,et al. Junior: The stanford entry in the urban challenge[J]. Journal of Field Robotics,2008,25(9): 569-597.

[2]Kammel S,Ziegler J,Pitzer B,et al. Team AnnieWAY’s autonomous system for the 2007 DARPA Urban Challenge[J].Journal of Field Robotics,2008,25(9): 615-639.

[3]Bacha A,Bauman C,Faruque R,et al. Odin: Team VictorTango’s entry in the DARPA Urban Challenge[J]. Journal of Field Robotics,2008,25(8): 467-492.

[4]蔡自興,賀漢根,陳虹. 未知環(huán)境中移動機(jī)器人導(dǎo)航控制研究的若干問題[J]. 控制與決策,2002,17(4): 385-390.CAI Zi-xing,HE Han-gen,CHEN Hong. Some issues for mobile robots navigation under unknown environments[J]. Control and Decision,2002,17(4): 385-390.

[5]John J L. Challenges for autonomous mobile robots[C]//Proceedings of International Machine Vision and Image Processing Conference. Maynooth: Inst of Elec and Elec Eng Computer Society,2007: 4.

[6]蔡自興,賀漢根,陳虹. 未知環(huán)境中移動機(jī)器人導(dǎo)航控制理論與方法[M]. 北京: 科學(xué)出版社,2009: 1-7.CAI Zi-xing,HE Han-gen,CHEN Hong. Theory and methods for mobile robots navigation under unknown environments[M].Beijing: Science Press,2009: 1-7.

[7]ZU Di,HAN Jian-da,TAN Da-long. Acceleration space LP for the path planning of dynamic target pursuit and obstacle avoidance[C]//Proceedings of the World Congress on Intelligent Control and Automation (WCICA). Dalian: Institute of Electrical and Electronics Engineers Inc,2006: 9084-9088.

[8]QU Zhi-hua,WANG Jing,Plaisted C E. A new analytical solution to mobile robot trajectory generation in the presence of moving obstacles[J]. IEEE Transactions on Robotics,2004,20(6): 978-993.

[9]Park S H,Lee B H. A new analytical representation to robot path generation with collision avoidance through the use of the collision map[J]. International Journal of Control,Automation and Systems,2006,4(1): 77-86.

[10]Nagy B,Kelly A. Trajectory Generation for Car-like Robots using Cubic Curvature Polynomials[C]//Proceedings of International Conference on Field and Service Robotics. Helsinki,2001: 731-736.

[11]Thompson S,Kagami S. Continuous curvature trajectory generation with obstacle avoidance for car-like robots[C]//Proceedings of International Conference on Computational Intelligence for Modelling,Control and Automation,and International Conference on Intelligent Agents,Web Technologies and Internet. Vienna: Inst of Elec and Elec Eng Computer Society,2005: 863-870.

[12]Likhachev M,Ferguson D. Planning long dynamically feasible maneuvers for autonomous vehicles[J]. International Journal of Robotics Research,2009,28(8): 933-945.

[13]Bottasso C L,Leonello D,Savini B. Path planning for autonomous vehicles by trajectory smoothing using motion primitives[J]. IEEE Transactions on Control Systems Technology,2008,16(6): 1152-1168.

[14]陳楊. 基于軌跡片段的運(yùn)動規(guī)劃方法及其在自主車輛中的應(yīng)用[D]. 長沙: 國防科學(xué)技術(shù)大學(xué)研究生院,2006: 11-24.CHEN Yang. Motion planning method based on trajectory primitives and its applications in autonomous land vehicles[D]. Changsha: National University of Defense Technology. Graduate School,2006: 11-24.

[15]Flores M E,Milam M B. Trajectory generation for differentially flat systems via NURBS basis functions with obstacle avoidance[C]//Proceedings of the American Control Conference.Minneapolis: Institute of Electrical and Electronics Engineers Inc,2006: 5769-5775.