本文提出了一種基于A*算法的用于道路場景的軌跡規(guī)劃方法，該方法中，使用了圖形搜索近似，并且通過一種新的推理技術來考慮其他車輛的不確定預測，這種新的推理技術允許高效計算一組模擬場景類型的危險結果的概率。研究者相信本文提出的幾何推斷方法可以很好地適應模型傳感器范圍和遮擋限制。后臺回復【軌跡規(guī)劃】獲取論文原文。

什么是路徑規(guī)劃和軌跡規(guī)劃？ 

車輛自主駕駛系統(tǒng)從本質上講是一個智能控制機器，其研究內(nèi)容大致可分為信息感知、行為決策及操縱控制三個子系統(tǒng)。 

路徑規(guī)劃是智能車輛導航和控制的基礎，是從軌跡決策的角度考慮的，可分為局部路徑規(guī)劃和全局路徑規(guī)劃。全局路徑規(guī)劃的任務是根據(jù)全局地圖數(shù)據(jù)庫信息規(guī)劃出自起始點至目標點的一條無碰撞、可通過的路徑。由于全局路徑規(guī)劃所生成的路徑只能是從起始點到目標點的粗略路徑，并沒有考慮路徑的方向、寬度、曲率、道路交叉以及路障等細節(jié)信息，加之智能車輛在行駛過程中受局部環(huán)境和自身狀態(tài)的不確定性的影響，會遇到各種不可測的情況。 

因此，在智能車輛的行駛過程中，必須以局部環(huán)境信息和自身狀態(tài)信息為基礎，規(guī)劃出一段無碰撞的理想局部路徑，這就是局部路徑規(guī)劃。通常路徑規(guī)劃的方法有:空間搜索法、層次法、動作行為法、勢場域法、柵格法、模糊邏輯法和神經(jīng)網(wǎng)絡法等。 

汽車自動駕駛任務可以分為三層，如圖所示，每層執(zhí)行不同任務，包括上層路徑規(guī)劃，中層行駛行為規(guī)劃和下層軌跡規(guī)劃。

上層路徑規(guī)劃在已知電子地圖、路網(wǎng)以及宏觀交通信息等先驗信息下，根據(jù)某優(yōu)化目標得到兩點之間的最優(yōu)路徑，完成路徑規(guī)劃的傳感信息主要來自于GPS定位信息以及電子地圖。 

中層行駛行為規(guī)劃是指根據(jù)主車感興趣區(qū)域內(nèi)道路、交通車等環(huán)境信息，決策出當前時刻滿足交通法規(guī)、結構化道路約束的最優(yōu)行駛行為，動態(tài)規(guī)劃的行駛行為序列組成宏觀路徑。行為規(guī)劃的傳感信息主要來自車載傳感器如雷達、照相機等，用以識別道路障礙、車道線、道路標識信息和交通信號燈信息等。 

下層軌跡規(guī)劃是指在當前時刻，以完成當前行車行為為目標，考慮周圍交通環(huán)境并滿足不同約束條件，根據(jù)最優(yōu)目標動態(tài)規(guī)劃決策出的最優(yōu)軌跡。同時，車輛的動力學約束也會在下層得到體現(xiàn)，下層軌跡規(guī)劃除了必要的外部環(huán)境信息外，還需要對主車狀態(tài)信息進行測量或估計。 

車輛路徑規(guī)劃問題中的幾個關鍵點：路網(wǎng)模型、路徑規(guī)劃算法和交通信息的智能預測，涉及的方面較多。本文主要探討軌跡規(guī)劃。 

用于道路場景的軌跡規(guī)劃方法 

A概述

本文提出了一種基于A*算法的用于道路場景的軌跡規(guī)劃方法，該方法中，使用了圖形搜索近似，并且通過一種新的推理技術來考慮其他車輛的不確定預測，這種新的推理技術允許高效計算一組模擬場景類型的危險結果的概率。同時研究者還將這種軌跡規(guī)劃方法與基于蒙特卡羅方法的POMDP求解器進行了比較，發(fā)現(xiàn)雖然POMDP求解器可以規(guī)劃比該研究的方法更保守更復雜的行為，但是它遭受粒子剝奪，這可能會導致危險的行為。這是因為蒙特卡羅方法有時不會有足夠多的樣本用于罕見但危險的未來情況。相反，我們的方法使用了對未來信念狀態(tài)的參數(shù)化描述，并且沒有遭受這個問題的困擾，并且運行時間明顯更長。 

圖1:道路行駛功能系統(tǒng)架構

圖1顯示了用于自主道路操作的模塊的高級視圖。全局規(guī)劃模塊處理任務，為軌跡規(guī)劃模塊提供路線，這也需要定位、附近車道模型和描述附近物體狀態(tài)和行為的模型。軌跡規(guī)劃模塊應該為自主車輛(稱為ego車輛)找到合適的軌跡，以便遵循公式： 

其中xe是自我車輛的狀態(tài)，Th是預測范圍的長度。這是通過優(yōu)化成本函數(shù)來實現(xiàn)的，其中高風險的軌跡具有高成本。通過分析由 i 指數(shù)的附近其他車輛的不確定預測來估計風險。在不確定性下的這種計劃將在該方法中以固定間隔重復，而當前最佳計劃由下級控制模塊執(zhí)行。 

這里，我們通過借用[13]中的符號和定義簡要概述不確定條件下的軌跡規(guī)劃問題。我們假設環(huán)境狀態(tài)x =（xe，xi），i = 0，..，K在計劃范圍的離散時間步長中演變：

其中xik是車輛 i 在時刻tnow +kΔtp的狀態(tài)，uk是對于自我車輛的控制動作，ωk是擾動，并且N = Th /Δtp其中Δtp是時間離散化。我們每次都會對環(huán)境進行嘈雜的測量：

其中νk模擬測量噪聲。一般來說，我們不能直接觀察xk，而是總結信息狀態(tài)Ik中的所有可用信息。目標是為計劃范圍找到最優(yōu)策略Π* = {π*0（I0），...，π*N-1（IN-1）}，給出我們的控制動作，在給定信息狀態(tài)的情況下最小化未來預期成本，對于附加成本函數(shù)：

為了找到最優(yōu)政策，我們計劃在信念空間中，并對未來的測量做出不同的假設，以獲得易處理的問題。在信念狀態(tài)bk被定義為bk = p（xk | Ik）并且Ik是歷史I狀態(tài)的情況下：Ik =（I0，u0：k-1，y0：k-1）。這個問題可以重新表述為隨機動態(tài)規(guī)劃（SDP）問題。 

我們可以使用部分可觀察的決策過程（POMDP）方法來逼近SDP問題的解決方案，或者我們可以以后退的方式解決一系列控制動作，忽略未來的測量。我們在本文中使用的后一種方法稱為開環(huán)后退水平控制（OLRHC），這里信念狀態(tài)是“對象的開環(huán)預測分布”[13]，反饋是通過重新規(guī)劃實現(xiàn)的。 

如[5] [14]中所述，我們的軌跡規(guī)劃算法描述了自主車輛的狀態(tài)，在道路對齊的坐標系中，橫坐標對應于沿參考路徑的弧長s，縱坐標表示有符號的橫向距離。路徑d。我們假設我們可以精確地測量與參考路徑（se0，de0）相關的當前狀態(tài)，并且可以獲得其他車輛的開環(huán)預測列表。設xik是慣性參考系中車輛 i 的姿態(tài)和速度，xik =（x，y，θ，v），xik是與參考路徑 r 相關的狀態(tài)，xik,r =（sik,r , dik,r , sik,r）和g（xik,r）= xik是在它們之間進行映射的函數(shù)。 對每個其他車輛的預測表示為高斯混合序列，其中每個高斯模型表示車輛狀態(tài)相對于特定路線的分布：

其中αj是高斯prj（xik,r(j)）=N（μir(j),k ，Σir(j),k）的混合分量權重，描述了假設 j 的路徑 r（j）的車輛 i 的狀態(tài)分布。該表示允許我們表達單個車輛的多個運動假設，例如，我們模擬車輛是否將使用不同的αj值轉向兩個假設的不確定性，每個假設屬于不同的路線。分布N（μir(j),k ，Σir(j),k）可以用不同的方法估計，例如[15]，或者諸如執(zhí)行卡爾曼濾波器的預測步驟的簡單方法。我們還假設我們知道每條車輛沿其路線的縱向范圍。 

軌跡規(guī)劃算法可以訪問道路模型，在該道路模型中，軌道規(guī)劃算法可以快速確定兩條路線是否相鄰，例如，檢查車輛是否在我們的左側或右側，如果一輛車與另一輛車合并或者路線是否交叉。本文描述風險評估方法主要基于兩條車輛在路線上占據(jù)相同縱向位置的距離和車輛的相對速度。 

B圖形搜索近似 

決定什么時候改變車道，或者當駕駛到兩個車道合并的道路的一部分時是一個非凸的問題，特別是我們經(jīng)?？梢赃x擇在另一輛車前面或后面，每個車輛都有一個局部最佳值選擇。 通過允許為我們的解決方案用一組采樣狀態(tài)值，我們將軌跡優(yōu)化轉換為圖搜索問題：從有限集中找出狀態(tài)值的序列，其具有最低成本，滿足我們的約束并達到狀態(tài)預測范圍的結束。我們通過對和時間進行采樣來構造一個格子超過允許狀態(tài)，并定義模擬我們的控制動作的運動原語u從時間k的狀態(tài)（s，d，v）轉換到時間k的另一個狀態(tài) k+1，k后的Δtp秒，從而形成可能的狀態(tài)序列的圖形表示。我們使用了一個簡化的模型來說明車輛在道路對齊坐標系中的移動方式： 

其中運動原語由不同的預定義加速度和橫向速度組成。速度v被限制為在上面被vmax限制為非負，并且d被縮放到1和1之間，其中1是參考路徑右邊的一個車道寬度和左邊一個車道寬度。允許速度的集合是從v = 0開始可達到的任何速度，其是非負的并且在規(guī)劃步驟的整個時間期間使用A中的任何加速度小于vmax，假設步驟之間的加速度瞬時變化。允許縱向位置的集合是時間步長k = 0，...，N 從 s = 0 開始以初始速度進入并且使用A中的加速度行進的距離的集合。在我們的簡化模型中，我們允許加速度的瞬時變化這個誤差假設最多是dmin的縱向位置誤差，它是從執(zhí)行模塊轉換為非完整運動的距離中減去的。 

對于我們在A *算法中的啟發(fā)式，我們使用進入結束狀態(tài)（k = N）的最優(yōu)成本，假設如果dk不在車道的中心并且在時間步長k，k + 1之間將發(fā)生橫向移動，并且沒有其他車輛。 這些成本是針對每個s，v，k組合離線計算的并存儲在查找表中。 這低估了達到目標狀態(tài)的成本，因為cl和cd總是大于或等于零，啟發(fā)式是可以接受的。 

總結

自動駕駛車輛的軌跡規(guī)劃應試圖在傳感器數(shù)據(jù)噪聲和不確定預測的情況下最小化預期風險。本文提出了一種基于圖搜索近似的道路場景軌跡規(guī)劃方法。其他車輛的不確定預測是由一種新的推理技術來解釋的，這種推理技術可以有效地計算建模情況類型集的危險結果的概率。對于高速公路合并場景，這里使用簡單的恒速預測，研究者并不考慮傳感器范圍和遮擋。未來的工作將考慮更復雜的預測模型，研究者相信本文提出的幾何推斷方法可以很好地適應模型傳感器范圍和遮擋限制。例如，為了對超出傳感器范圍的可能車輛所構成的風險進行建模，研究者可以在給定未知車輛速度和位置的統(tǒng)一概率的情況下，整合約束違反概率。 

參考資料 

[1] D. Gonz´alez, J. P´erez, V. Milan´es, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 4, pp. 1135–1145, 2016.

[2] C. Hubmann, M. Aeberhard, and C. Stiller, “A generic driving strategy for urban environments,” in Intelligent Transportation Systems (ITSC), 2016 IEEE 19th International Conference on. IEEE, 2016, pp. 1010–1016.

[3] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for berthaa local, continuous method,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 450–457.

[4] J. Nilsson, J. Silvlin, M. Brannstrom, E. Coelingh, and J. Fredriksson,“If, when, and how to perform lane change maneuvers on highways,” IEEE Intelligent Transportation Systems Magazine, vol. 8, no. 4, pp. 68–78, 2016.

[5] M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion planning for autonomous driving with a conformal spatiotemporal lattice,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 4889–4895.

[6] M. Bahram, A. Wolf, M. Aeberhard, and D. Wollherr, “A predictionbased reactive driving strategy for highly automated driving function on freeways,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 400–406.

[7] S. Ulbrich and M. Maurer, “Towards tactical lane change behavior planning for automated vehicles,” in 2015 IEEE 18th International Conference on Intelligent Transportation Systems. IEEE, 2015, pp.989–995.

[8] M. Althoff, O. Stursberg, and M. Buss, “Model-based probabilistic collision detection in autonomous driving,” IEEE Transactions on Intelligent Transportation Systems, vol. 10, no. 2, pp. 299–310, 2009.

[9] W. Zhan, J. Chen, C.-Y. Chan, C. Liu, and M. Tomizuka, “Spatiallypartitioned environmental representation and planning architecture for on-road autonomous driving,” in Intelligent Vehicles Symposium (IV), 2017 IEEE. IEEE, 2017, pp. 632–639.

[10] C. Hubmann, M. Becker, D. Althoff, D. Lenz, and C. Stiller, “Decision making for autonomous driving considering interaction and uncertain prediction of surrounding vehicles,” in Intelligent Vehicles Symposium (IV), 2017 IEEE. IEEE, 2017, pp. 1671–1678.

[11] W. Liu, S.-W. Kim, S. Pendleton, and M. H. Ang, “Situation-aware decision making for autonomous driving on urban road using online pomdp,” in Intelligent Vehicles Symposium (IV), 2015 IEEE. IEEE, 2015, pp. 1126–1133.

[12] D. Klimenko, J. Song, and H. Kurniawati, “Tapir: A software toolkit for approximating and adapting pomdp solutions online,” in Proceedings of the Australasian Conference on Robotics and Automation, Melbourne, Australia, vol. 24, 2014.

[13] N. E. Du Toit and J. W. Burdick, “Robot motion planning in dynamic, uncertain environments,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 101–115, 2012.

[14] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a frenet frame,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010, pp. 987–993.

[15] Q. Tran and J. Firl, “online maneuver recognition and multimodal trajectory prediction for intersection assistance using non-parametric regression,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 918–923.

[16] S. Ulbrich and M. Maurer, “Situation assessment in lane change behavior planning for automated vehicles,” in Proceedings of the 18th International IEEE Annual Conference on Intelligent Transportation Systems (ITSC 2015), 2015.

[17] M. Althoff and R. L¨osch, “Can automated road vehicles harmonize with traffic flow while guaranteeing a safe distance?” in Intelligent Transportation Systems (ITSC), 2016 IEEE 19th International Conference on. IEEE, 2016, pp. 485–491.

[18] O. Amidi and C. E. Thorpe, “Integrated mobile robot control,” in Fibers’ 91, Boston, MA. International Society for Optics and Photonics, 1991, pp. 504–523.

[19] E. Ward, N. Evestedt, D. Axehill, and J. Folkesson, “Probabilistic model for interaction aware planning in merge scenarios,” IEEE Transactions on Intelligent Vehicles, vol. 2, no. 2, pp. 133–146, 2017.

[20] P. F. Lima, M. Trincavelli, J. M°artensson, and B. Wahlberg,“Clothoidbased speed profiler and control for autonomous driving,” in Intelligent Transportation Systems (ITSC), 2015 IEEE 18th International Conference on. IEEE, 2015, pp. 2194–2199.

作者：Erik Ward, John Folkesson

The research leading to these results has been carried out within the iQMatic project funded by FFI/VINNOVA.

1Robotics Perception and Learning Lab, KTH, Stockholm, Sweden

(e-mail: eward@kth.se, johnf@kth.se)