增維啟發(fā)式搜索路徑規(guī)劃算法

吳宏

摘要：在智能無人車路徑規(guī)劃研究中，路徑規(guī)劃算法的效率一直是重要的研究問題。搜索狀態(tài)空間過大、時間復(fù)雜度過高以及低效率一直是路徑規(guī)劃算法的瓶頸。本論文提出一種增維啟發(fā)式搜索算法來解決的這一問題。該方法通過多階段增加搜索空間維度，降低了搜索算法的狀態(tài)空間從而提高算法效率。仿真實(shí)驗(yàn)結(jié)果顯示，與一般的高維啟發(fā)式搜索算法相比，該方法減少了87%的搜索狀態(tài)，執(zhí)行效率提高了近10倍。實(shí)驗(yàn)結(jié)果表明，該算法在算法效率與生成軌跡質(zhì)量兩方面取得一個非常好的平衡。

關(guān)鍵詞： 增維啟發(fā)式搜索； 智能車； 路徑規(guī)劃； 高效率； 平衡

中圖分類號： TP311 文獻(xiàn)標(biāo)識碼：A 文章編號：1009-3044（2016）36-0188-04

Increment-dimensional Heuristic Search Motion Planning Algorithm

WU Hong

（Department of Computer Science and Technology， Tongji University， Shanghai 201804， China）

Abstract： For intelligent vehicle motion planning， effective enough is always an important issue. The huge statue-space， high time complexity of the high dimensional search approach is always the bottle-neck of the algorithm. To solve this problem， this paper proposes a new method， increment-dimensional heuristic search algorithm. This method is a stepped-up heuristic search to reduce the searching status and improve the search algorithm execution efficiency. In experiment， the result shows that this algorithm reduces 87% of searching status and executes time is nearly 1/10 of that of the traditional heuristic search method. It is a very good trade-off between execution efficiency and trajectory quality.

Key words： increment-dimensional heuristic search； intelligent vehicle； motion planning； effective； trade-off

1 引言

在智能無人車領(lǐng)域，智能車無人車的行駛安全以及駕駛舒適度一直是一個非常重要的研究問題。而智能無人車的路徑規(guī)劃是這一問題的核心。智能無人車路徑規(guī)劃算法需要在有限的時間內(nèi)，輸出高質(zhì)量高精度的路徑，傳輸給智能無人車的控制模塊、執(zhí)行模塊加以執(zhí)行。一般的移動機(jī)器人路勁規(guī)劃算法研究的是在高維度的空間里探索出一條路徑，相比之下，智能無人車的路徑規(guī)劃則需要考慮車輛動力學(xué)模型約束，通常我們需要考慮四維狀態(tài)。二維狀態(tài)（x， y），表示車輛的地理坐標(biāo)，車輛的航向角θ，以及行駛速度v。在四維狀態(tài)空間里搜素出一條可行路徑，是一個計(jì)算密集型的任務(wù)。與此同時，智能無人車的行駛速度可能很高，因此要求規(guī)劃算法能夠在一個非常有限的時間里給出搜索的結(jié)果。

為了解決這一問題，本文給出一種增維啟發(fā)式路徑規(guī)劃搜索算法。該算法采取一種分階段，逐步增加搜索維度的方法來生成路徑。在每一個階段，增維搜索算算法選擇離車輛當(dāng)前位置附近的一個區(qū)域，增加狀態(tài)空間維度，進(jìn)行啟發(fā)式搜索。因此該算法的輸出軌跡是多精度的軌跡。在車輛附近位置，輸出軌跡為高維度高精度，充分考慮車輛動力學(xué)模型，駕駛舒適度，能耗以及可靠性。而在遠(yuǎn)處，低維度低精度的軌跡依然可以引導(dǎo)智能無人車的行駛方向正確，充分考慮的地圖信息，障礙物信息。從人類正常的駕駛習(xí)慣上來說，駕駛員總是對近處的駕駛精度較高，而遠(yuǎn)處相對較低。該算法充分利用了這一點(diǎn)原理，犧牲了遠(yuǎn)處的軌跡精度，極大地提高了算法的運(yùn)行效率。在頻繁聯(lián)系的反復(fù)規(guī)劃中，車輛會一直執(zhí)行高精度部分軌跡。因此，該算法在運(yùn)行效率以及輸出軌跡質(zhì)量方面，取得一個非常好的均衡。

為了展示該算法的性能，本文進(jìn)行了仿真實(shí)驗(yàn)。在實(shí)驗(yàn)中，智能無人車剛剛進(jìn)入一個停車場，需要在目標(biāo)停車位泊車。實(shí)驗(yàn)結(jié)果表明，相比傳統(tǒng)的高維度啟發(fā)式搜索算法，該算法減少了超過87%的搜索狀態(tài)，運(yùn)行性能提高了近10倍。

2 研究現(xiàn)狀及文獻(xiàn)綜述

從20世紀(jì)70年代開始，歐美的西方國家開始無人駕駛汽車方面的研究工作，并在智能無人車的控制和商用化方面取得一定進(jìn)展。在汽車工業(yè)非常發(fā)達(dá)的德國，各大汽車公司都資助或者聯(lián)合高等院校以開發(fā)可在普通道路上行駛的智能無人車。目前，歐盟已啟動一個名叫CyberCars的智能無人車項(xiàng)目，以推動智能無人車的研究和各國間智能無人車技術(shù)的信息共享。

在20世紀(jì)的80年代，我國部分大學(xué)開始智能無人車的研究工作，雖然起步較晚也取得一定成果。目前，從事這方面研究工作的 主要是國防科技大學(xué)、軍事交通學(xué)院及清華大學(xué)等科研機(jī)構(gòu)。[1-6]

在智能無人車決策模塊的相關(guān)研究中，最核心的部分是路徑規(guī)劃算法的研究。文獻(xiàn)[7]提出一種快速擴(kuò)展隨機(jī)樹生成算法—RRT （Rapid-Exploring Random Tree）算法。RRT是一種多維空間中有效的路徑規(guī)劃算法。它以一個初始點(diǎn)作為根節(jié)點(diǎn)，通過隨機(jī)采樣增加葉子節(jié)點(diǎn)的方式，生成一個隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)樹中的葉子節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或者進(jìn)入目標(biāo)區(qū)域，便可以在當(dāng)前隨機(jī)樹中找到一條從初始點(diǎn)到目標(biāo)點(diǎn)的路徑。文獻(xiàn)[8]在RRT算法在自動駕駛汽車以及宇宙空間探測器路徑規(guī)劃上的應(yīng)用。文獻(xiàn)[9]對RRT算法提出優(yōu)化方法并通過實(shí)驗(yàn)，解決了基本RRT算法存在的動態(tài)環(huán)境中規(guī)劃路徑不穩(wěn)定的問題，同時提出雙向RRT生成算法以及動態(tài)步長等優(yōu)化方法，提高了RRT算法生成初始點(diǎn)到目標(biāo)點(diǎn)路徑生成的速度。然而RRT算法在規(guī)劃路徑的過程中產(chǎn)生的是可行解，而非最優(yōu)解。文獻(xiàn)[10]提出了RRT*算法，RRT算法進(jìn)行了改進(jìn)，保證了RRT算法生成解是漸進(jìn)最優(yōu)解。然而RRT*算法在時間復(fù)雜度上遠(yuǎn)高于樸素的RRT算法。文獻(xiàn)[11]提出了一種RRT*算法加速的方法，通過使用預(yù)生成RRT隨機(jī)樹，在使用RRT*_S算法優(yōu)化當(dāng)前隨機(jī)樹，構(gòu)造出與RRT*算法生成隨機(jī)樹本質(zhì)相同的RRT*_S隨機(jī)樹，從而實(shí)現(xiàn)RRT*算法的加速。文獻(xiàn)[12]為麻省理工學(xué)院將RRT*算法運(yùn)用于叉車移動路徑規(guī)劃的一次應(yīng)用實(shí)踐，并對RRT算法與RRT*算法在實(shí)際應(yīng)用中的結(jié)果給出對比分析。

文獻(xiàn)[13][14][15][16]給出了2007年美國DARPA智能無人車比賽麻省理工學(xué)院（MIT）參賽智能無人車的整體架構(gòu)，MIT智能無人車的軌跡生成算法，主要是用RRT算法生成可行路徑，并對該路徑進(jìn)行平滑，以此為基礎(chǔ)生成智能無人車運(yùn)動軌跡。

文獻(xiàn)[17][18][19][20][21][22]主要闡述了狀態(tài)空間搜索算法，通過估價函數(shù)進(jìn)行啟發(fā)式搜索以及狀態(tài)空間搜索剪枝。文獻(xiàn)[23]提出了ARA*（Anytime A*）算法，對短時間間隔內(nèi)連續(xù)反復(fù)用A*搜素算法進(jìn)行空間狀態(tài)搜索這一類狀態(tài)空間搜索應(yīng)用場景進(jìn)行優(yōu)化。

3 增維啟發(fā)式搜索算法

增維啟發(fā)式搜索是一種兩階段的啟發(fā)式搜索算法。在算法的第一階段，搜索出一條從車輛當(dāng)前位置到目標(biāo)位置的幾何最短路的軌跡。在第一階段的搜索，我們只考慮二維的搜索狀態(tài)空間（x， y），即車輛的地理坐標(biāo)。第二階段，選取第一階段的路徑中的一個點(diǎn)作為本階段目標(biāo)點(diǎn)，搜索狀態(tài)加入車輛的航向角θ，以及行駛速度v，總體狀態(tài)空間提升到四維，并且考慮車輛動力學(xué)模型，在此狀態(tài)空間下，搜索出一條高精度可執(zhí)行的車輛行駛軌跡。

3.1 第一階段搜索

在這一階段，因?yàn)槲覀冎豢紤]二維狀態(tài)空間（x， y），即車輛的地理坐標(biāo)。如果將狀態(tài)空間離散化，這一搜索問題會退化成一個圖論的最短路問題。雖然圖論的最短路問題有很多經(jīng)典成熟的算法。但是在這里還是有一些值得討論的問題。

3.1.1 柵格隨機(jī)化

一般地，在執(zhí)行最短路算法之前，會把狀態(tài)空間離散化成柵格，然后對柵格做4聯(lián)通或者8聯(lián)通處理，但是這種離散化方法會使最短路失去最優(yōu)解，如圖1a、1c所示。

圖1 a. 離散化使得幾何最短路失解；b. 隨機(jī)化18聯(lián)通柵格法；c. 8聯(lián)通柵格法幾何最短路（黑），隨機(jī)化18聯(lián)通柵格法幾何最短路（紅）。

a b c

為了解決這一問題。如圖1b所示，算法使用一種隨機(jī)化18聯(lián)通的柵格法來離散化空間。即在柵格之間連邊的時候，每個柵格除了相鄰向相鄰8個柵格聯(lián)通，同時隨機(jī)向其他10個柵格聯(lián)通。選取的10個柵格滿足與該柵格曼哈頓距離小于7，滿足條件的格子約為100個，足以隨機(jī)化，同時連邊長度小于兩個柵格長度，也方便計(jì)算是否與障礙物碰撞。

3.1.2 最短路算法

在離散化為柵格之后，采用單源最短路算法來計(jì)算車輛當(dāng)前位置到其他位置的幾何最短路，雖然單源最短路算法非常的經(jīng)典成熟，但依舊有值得討論的地方。

最短路的經(jīng)典算法是堆優(yōu)化的Dijkstra算法，該算法時間復(fù)雜度為 [O（eloge）]，其中[e]代表離散化后邊的數(shù)量，然而在稀疏圖中，SPFA算法的實(shí)際時間復(fù)雜度約為[O（e）]，在18隨機(jī)聯(lián)通結(jié)構(gòu)的圖中效率比價高，因而在本階段中，我們采用SPFA算法計(jì)算單源最短路。

3.2 第二階段搜索

在第二階段的搜索中，我們選取第一階段結(jié)果，幾何最短路上的一個點(diǎn)來作為目標(biāo)點(diǎn)，在搜索狀態(tài)加入車輛的航向角θ，以及行駛速度v，在搜索中充分考慮車輛動力學(xué)模型，搜索出一條高精度可執(zhí)行的車輛行駛軌跡。

3.2.1 啟發(fā)函數(shù)

在啟發(fā)式搜索過程中，一個強(qiáng)力有效的啟發(fā)式函數(shù)對搜索效率來說至關(guān)重要。啟發(fā)式函數(shù)不僅為搜索的實(shí)際代價提供了一個下界，同時也是實(shí)際代價的一個良好估算，可以引導(dǎo)搜索往正確的方向擴(kuò)展，并且實(shí)現(xiàn)搜索剪枝，在第二階段的搜索中，使用以下啟發(fā)式函數(shù)。

動力學(xué)約束無障礙啟發(fā)函數(shù)，[hnh（x，y，θ，v）]，該函數(shù)忽略障礙物信息，考慮車輛動力學(xué)模型，在此條件下求出最優(yōu)的路徑。這一啟發(fā)式函數(shù)因?yàn)楹雎粤苏系K物信息，只考慮動力學(xué)模型，所以可以離線計(jì)算、存儲，在真實(shí)路徑規(guī)劃的過程中查詢，計(jì)算速度極快。該函數(shù)極大的消除接近目標(biāo)點(diǎn)航向角錯誤的搜索分支。

地圖信息非動力學(xué)模型啟發(fā)函數(shù)，[hh（x，y）]，該啟發(fā)函數(shù)是上一啟發(fā)函數(shù)的對偶函數(shù)，忽略車輛動力學(xué)模型，以幾何最短路作為啟發(fā)函數(shù)。該啟發(fā)函數(shù)充分考慮的地理信息，消除了錯誤行駛方向的搜索分支。

結(jié)合二者，選取啟發(fā)函數(shù)[h（x， y，θ，v）] = max（[hnhx，y，θ，v， hh（x，y））]，

fxyv） = g（x， y， ，v） + h（x， y， ，v） （1）

fxyv） Fxyv） （2）

f 為狀態(tài)[（x， y， θ， v）]的估價函數(shù)， g為當(dāng)前搜索狀態(tài)[（x， y， θ， v）]的實(shí)際代價， [F]為實(shí)際搜索代價。

在該啟發(fā)函數(shù)的引導(dǎo)下，第二階段啟發(fā)式搜索可以高效地計(jì)算出四維高精度路徑。

4 仿真實(shí)驗(yàn)以及實(shí)驗(yàn)結(jié)果

為了展示該算法的性能，本文進(jìn)行了仿真實(shí)驗(yàn)。在實(shí)驗(yàn)中，智能無人車剛剛進(jìn)入一個停車場，需要在目標(biāo)停車位泊車。實(shí)驗(yàn)環(huán)境為Ubuntu 12.04 Linux系統(tǒng)，Intel i5處理器， 8GB內(nèi)存。停車場大小為長80米寬50米，柵格離散化精度為10厘米，車輛采用72個不同的航向角，同時采用兩個速度，最大的前向速度以及最大的后向速度。

圖2 a樸素四維啟發(fā)式搜索；b增維啟發(fā)式搜索；c樸素四維啟發(fā)式搜索輸出路徑；d增維啟發(fā)式搜索輸出路徑

a

b

c

d

表1 算法性能比較

[ 階段 樸素四維啟發(fā)式搜索 增維啟發(fā)式搜索 搜索狀態(tài)數(shù)量 第一階段 400000 第二階段 10808634 408773 共計(jì) 10808634 808773 算法運(yùn)行時間

（毫秒） 第一階段 142 第二階段 2844 141 共計(jì) 2844 283 ]

如圖1b，對于每一次路徑規(guī)劃，增維啟發(fā)式搜索算法可以有效地減少搜索狀態(tài)的數(shù)量，因?yàn)楦呔S度高精度部分的搜索集中在離車輛較近的區(qū)域，而從全局的角度，二維的幾何最短路依舊引導(dǎo)著軌跡往正確的方向。相比之下樸素的四維啟發(fā)式搜索搜索量極大（圖2b）。從輸出軌跡上看，兩者的輸出軌跡質(zhì)量幾乎相同（圖2c、2d）。

5 結(jié)論

本文展示了增維啟發(fā)式搜索路徑規(guī)劃算法。該算法分為兩階段。第一階段在全局考慮二維的搜索狀態(tài)空間，得出起始點(diǎn)到目標(biāo)位置的幾何最短路。在第一階段幾何最短路基礎(chǔ)上選取一個目標(biāo)點(diǎn)作為第二階段目標(biāo)狀態(tài)空間，進(jìn)而得到考慮了車輛動力學(xué)模型、四維的高精度可執(zhí)行軌跡。仿真實(shí)驗(yàn)結(jié)果表明，在現(xiàn)實(shí)場景下，該算法極大地減少了搜索狀態(tài)數(shù)量，提高了算法執(zhí)行效率，同時輸出高質(zhì)量的智能無人車行駛軌跡。

參考文獻(xiàn)：

[1] 楊明.無人駕駛車輛研究綜述與展望[J]..哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2006，38（增刊）：1259-1262.

[2] 孫振平.自主駕駛汽車智能控制系統(tǒng)[D].國防科技大學(xué)，2004.

[3] 楊森森.基于GPS/INS/激光雷達(dá)的無人車組合導(dǎo)航[D].上海交通大學(xué)碩士學(xué)位論文，2013.

[4] 錢鈞，楊汝清，王晨，等，基于路標(biāo)的智能車輛定位[J].上海交通大學(xué)學(xué)報(bào)，2007，41（6）：894-898.

[5] 王曦鳴.軍用無人車的路徑規(guī)劃與仿真研究[D].北京交通大學(xué)碩士學(xué)位論文，2010.

[6] 曹玉芬，張國斌.美國無人地面車輛計(jì)劃[J].國外坦克，2004（5）：25-27.

[7] Rapidly-exploring random trees： A new tool for path planning. S. M. LaValle. TR 98-11， Computer Science Dept.， Iowa State University， October 1998

[8] RRT-based trajectory design for autonomous automobiles and spacecraft. P. Cheng， Z. Shen， and S. M. LaValle. Archives of Control Sciences， 11（3-4）：167--194， 2001.

[9] Rapidly-exploring random trees： Progress and prospects. S. M. LaValle and J. J. Kuffner. In Proceedings Workshop on the Algorithmic Foundations of Robotics， 2000.

[10] S. Karaman and E. Frazzoli， Sampling-based algorithms for optimal motion planning，I. Robotic Res.， vol. 30， no. 7， pp. 846C894， 2011.

[11] Yun-xiao Shan Bi-jun Li Jian-Zhou Yue-Zhang，An Approach to Speed Up RRT* ，2014 IEEE Inteligent Vehicles Symposium （IV） June 8-11

[12] Karaman S， Walter M R， Perez A， et al. Anytime motion planning using the RRT*[C]//Robotics and Automation （ICRA）， 2011 IEEE International Conference on. IEEE， 2011： 1478-1483.

[13] Leonard J， How J， Teller S， et al. A perception-driven autonomous urban vehicle[J]. Journal of Field Robotics， 2008， 25（10）： 727-774.

[14] Kuwata Y， Fiore G， Teo J， et al. Motion planning for urban driving using RRT[C]//Intelligent Robots and Systems， 2008. IROS 2008. IEEE/RSJ International Conference on. IEEE， 2008： 1681-1686.

[15] Leonard J， Barrett D， How J， et al. Team MIT urban challenge technical report[J]. 2007.

[16] Kuwata Y， Karaman S， Teo J， et al. Real-time motion planning with applications to autonomous urban driving[J]. Control Systems Technology， IEEE Transactions on， 2009， 17（5）： 1105-1118.

[17] Barbehenn， M. and Hutchinson， S. （1995）. Efficient search and hierarchical motion planning by dynamically maintaining single-source shortest path trees. IEEE Transactions on Robotics and Automation， 11（2）： 198–214.

[18] Gaschnig， J. G. （1979）. Performance measurement and analsis of certain search algorithms. Ph.D. Dissertation， Carnegie Mellon University.

[19] Stentz， A. （1995）. The Focussed D* Algorithm for real-time replanning. Proceedings of the International Joint Conference on Artificial Intelligence （IJCAI）， pp. 1652–1659.

[20] Pearl， J. （1984）. Heuristics： Intelligent Search Strategies for Computer Problem Solving. Boston， MA： Addison-Wesley

[21] Hart， P. E.， Nilsson， N. J. and Raphael， B. （1968）. A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics， 4（2）： 100–107.

[22] Hart， P. E.， Nilsson， N. J. and Raphael， B. （1972）. Correction to “A formal basis for the heuristic determination of minimum cost paths”. ACM SIGART Bulletin， 37： 28–29.