在自動駕駛的定位、感知、預測、決策規(guī)劃和控制等模塊中，感知模塊就像是人的眼睛和耳朵，負責對外部環(huán)境進行感知；控制模塊就像人的雙手和雙腳，負責最終的加減速、轉(zhuǎn)向等操作；而決策規(guī)劃模塊就像人的大腦，基于接收到的感知等信息進行行為決策和軌跡生成。

正如人的大腦又分為左腦和右腦一樣，決策規(guī)劃模塊又可以繼續(xù)分為行為決策層（Behavioral Layer）和運動規(guī)劃層（Motion Planning）。

其中，行為決策層在接收到全局路徑后，結(jié)合感知信息，進行具體的行為決策；運動規(guī)劃層根據(jù)具體的行為決策，規(guī)劃生成一條滿足特定約束條件的軌跡，該軌跡作為控制模塊的輸入決定車輛最終行駛路徑。

隨著自動駕駛等級的不斷提高，決策規(guī)劃層作為自動駕駛的大腦，其重要性也隨之提高。但與人腦相比，自動駕駛的這顆大腦還有太長的路需要追趕。本文將通過萬字詳述路徑規(guī)劃中的Motion Planning存在的問題與挑戰(zhàn)。

1. Motion Planning常用算法

Motion Planning算法是從機器人領域發(fā)展起來的，逐漸發(fā)展出適用于自動駕駛領域的各種算法。論文[1]對Motion Planning的軌跡生成方法做了綜述，介紹的方法如下圖所示。

• 基于采樣搜索的算法：Dijkstra、RRT、A*、hybird  A*和Lattice等；

• 基于曲線插值的算法：RS曲線、Dubins曲線、多項式曲線、貝塞爾曲線和樣條曲線等；

• 基于最優(yōu)化的算法：Apollo的piecewise-jerk等；

上述算法一般都是相互結(jié)合在一起使用的。比如多項式曲線需要對終端狀態(tài)進行采樣、貝塞爾曲線對控制點進行采樣、hybird A*中使用到了RS曲線或者Dubins曲線等。

論文[1]總結(jié)了各種軌跡生成算法的優(yōu)缺點，如下圖所示?？梢姏]有哪一種算法是完美的，需要結(jié)合具體的場景和工況選用合適的算法。目前行業(yè)內(nèi)應用比較多的是多項式曲線插值(高速場景)和最優(yōu)化的算法。

2. Motion Planning問題與挑戰(zhàn)

上述介紹的Motion Planning的算法，基本能解決大部分的自動駕駛場景軌跡生成問題，軌跡生成算法已經(jīng)不是主要瓶頸。但是在Motion Planning領域內(nèi)仍然存在許多挑戰(zhàn)需要去攻克，主要包括以下幾個方面：

• 最優(yōu)性問題；

• 認知推理問題；

• 不確定性問題(Uncertainty/Probability)；

• Single-Agent；

• Multiple-Agent；

• 工程化問題。

2.1 最優(yōu)性問題

全局最優(yōu)是NP-hard問題[3]，為了實時性，行業(yè)內(nèi)多數(shù)采用橫縱向解耦的規(guī)劃方法。但是這么做會犧牲最優(yōu)性，在一些工況下不能得到良好的車輛行為，比如超車[2]、對向來車、向心加速度約束處理、橫向規(guī)劃需要考慮縱向規(guī)劃能力等。例如，當自動駕駛主車(Autonomous Driving Car ，ADC)前方有一個減速行駛的車輛時，橫縱向解耦的方法一般只有當前方車輛車速降低到一定值時才會超車行駛。ADC的行為表現(xiàn)就是先減速甚至停車，然后再繞障行駛，這顯然不是最優(yōu)的行駛策略。如果采用時空一體化規(guī)劃方法，則可以避免減速或者停車行為。下圖中左圖是解耦方法的示例，在前方有減速停車車輛時，ADC會進行減速。右圖是時空規(guī)劃的示例，在前方車輛減速時ADC會進行超車。

2.2 認知推理問題

2.2.1 地圖拓撲推理

以Apollo為例，PNC Map模塊從HD Map模塊提取數(shù)據(jù)形成參考線，并且通過HD Map模塊的API接口查詢道路元素。但Motion Planning模塊會忽略了一些道路的拓撲關系，例如匯入?yún)R出路口，而這些特殊的道路拓撲是會影響到車輛的行為。

此外，在沒有HD Map模塊而單純依靠視覺車道線的情況下，此時感知車道線會發(fā)生異常。在匯入?yún)R出道路和十字路口道路中，其道路拓撲問題尤為凸顯。

2.2.2 障礙物統(tǒng)一建模

交通場景的參與者有車輛、摩托車、自行車、行人、錐桶等。廣義上來講還包括人行橫道、紅綠燈、道路限速等地圖靜態(tài)元素，Motion Planning需要針對不同的元素做出不同的決策。障礙物統(tǒng)一建?？梢院喕瘑栴}，并且提升計算效率。

• Aopllo將所有交通參與者抽象為Static Obstacle，Dynamic Obstacle和Virtual Obstacle，Obstacle就是box， Static Obstacle和Dynamic Obstacle為車輛、行人等， Virtual Obstacle為人行橫道、禁停區(qū)等。路徑規(guī)劃時不考慮Virtual Obstacle。

• 使用能量場相關的方法，將交通參與者使用能量函數(shù)表示。上圖中間圖就是清華[4]提出的行車安全場，由靜止物體的勢能場、運動物體的動能場和駕駛員的行為場構(gòu)成。最優(yōu)軌跡就是尋找一條能量和最小的軌跡。

• 論文[5]將交通參與者分為obstacle-like和constraint-like。obstacle-like是動靜態(tài)車輛、紅燈等，將其映射到slt的3D柵格中。constraint-like是限速、停車標志等，作為semantic boundary。根據(jù)決策序列動作在slt配置空間內(nèi)生成若干cube邊界供軌跡生成使用。

2.2.3 場景認知推理

由于現(xiàn)實中環(huán)境的復雜性，一種決策策略或者規(guī)劃方法難以處理不同的工況。因此對行駛環(huán)境進行分類，在不同的場景下選擇不同的策略可以提升Motion Planning的性能。那么怎么進行場景分類和場景識別，在不同的場景Motion Planning又該有哪些不同？這些問題都是需要解決的。

• Aopllo中場景分類為LANE_FOLLOW、SIDE_PASS、STOP_SIGN_UNPROTECTED等。有兩種場景識別方式，一是通過規(guī)則的方法，一是通過機器學習的方法。不同的scenario有不同的stage，stage中依次執(zhí)行task。即使是相同的task在不同scenario中參數(shù)配置也可能不同。

• 毫末基于城市場景路口多、擁堵多和變道多的特點，將行駛場景分為十類，顯然是和Apollo中的scenario分類是不同的，然而毫末的場景識別方法卻不得而知。毫末此外還提出了行駛環(huán)境熵的概念來描述行駛環(huán)境的擁堵狀態(tài)。

2.3 不確定性

2.3.1 定位不確定性

在多數(shù)的Motion Planning中都是認為定位是足夠準確的，但是實際場景中由于遮擋、多徑干涉等問題，定位往往是不準確的。以論文[6]中的左下圖所示，由于定位誤差導致從HD Map模塊查詢到的道路邊界產(chǎn)生誤差，從而使規(guī)劃和車輛行駛軌跡在道路邊界上。

論文將定位不確定性假設為高斯分布，并且定位模塊可以計算出概率分布的期望與方差。論文將車輛坐標系轉(zhuǎn)換到了UTM坐標系下，根據(jù)定位的高速分布情況和坐標變換公式，就可以計算出車輛周圍環(huán)境在定位影響下的不確定性，如上右圖所示，其中顏色越深表示不確定性越大，其不確定性計算公式主要由下式得到。

可以發(fā)現(xiàn)距離ADC越遠其不確定越高，隨著車輛的前進，其不確定性會被更新。路徑規(guī)劃方法采用了Lattice（五次多項式曲線）的方法，在cost計算時，增加了兩個項目。一個是硬約束：規(guī)劃路徑上點的最大不確定性不能大于某一個閾值；二是在cost function中增加了不確定性的權重和。

2.3.2 感知不確定性

由于傳感器噪聲、車輛震動、行駛環(huán)境和不完善的算法，感知得到結(jié)果具有不確定性，甚至是錯誤的。由于感知的不確定性會造成Motion Planning結(jié)果的不安全性。一種簡單的處理方式是加buffer，但是粗暴的處理方式會減小Motion Planning的可行域，可能造成過于激進或者過于保守的行駛策略。

論文[7]以裝備了Around View Monitoring(AVM)的泊車應用為例，由于感知誤差會使路徑規(guī)劃在實際超車位置停車，可能會發(fā)生碰撞，如下左圖所示。論文將感知的不確定性建模為高斯分布，感知效果距離ADC越遠不確定性越高，如下右圖所示。

論文中整體架構(gòu)如下左圖所示論文，采用此算法后的效果如下右圖所示。

• Parking space sampling：對距離ADC最近的兩個角點進行采樣，將采樣點看作是正態(tài)分布的，根據(jù)采樣角點和設定的?？臻g的長度，計算ADC后軸中心的停車點；

• Path candidate generation：采用ocp理論對每個采樣點進行路徑規(guī)劃，其中將時域問題轉(zhuǎn)化為Ferent坐標系下，并使用SQP求解非線性問題；

• Optimal Path Selection：使用utility theory進行最優(yōu)路徑的選擇。Utility function為：EU(s) = P(s) x Uideal(s) +(1-P(s)) x Ureal(s)，其中P(s) 為路徑對應采樣點的概率，Uideal為路徑到目標點(當前時刻感知檢測到的，并非采樣得到的)的偏差效用函數(shù)值， Ureal為路徑上到ADC當前位置的效用函數(shù)值。

2.3.3 預測不確定性

預測是實現(xiàn)L4以上高級別自動駕駛的重要環(huán)節(jié)。然而截至目前，預測對整個行業(yè)來說仍是一個非常難的問題。因此預測的準確性很差，在不確定性預測結(jié)果下做Motion Planning是非常重要的。

論文[8]提出了一個基于高斯分布的規(guī)劃架構(gòu)，處理預測和控制不確定性帶來的規(guī)劃軌跡不安全的問題。

• 候選軌跡生成：通過多階段橫縱向采樣生成?？梢岳斫鉃锳opllo Lattice方法。

• 預測軌跡生成：對于某一個車輛的軌跡進行預測(進行規(guī)劃)時，認為其他車輛是勻速行駛的，并且其狀態(tài)都是確定的，則通過對候選軌跡的cost計算，得到最優(yōu)的預測軌跡。之后通過卡爾曼濾波計算預測軌跡的概率分布，并假設其遵從正態(tài)分布。

• ADC軌跡生成：此時需要考慮其他交通參與者的預測的不確定性。針對每一條候選軌跡，通過LQR算法計算出控制誤差，然后再通過卡爾曼濾波計算出軌跡的概率分布，在軌跡評價進行cos計算時，碰撞檢測是基于預測和ADC規(guī)劃軌跡的概率分布的，即在所有概率分布內(nèi)都不能發(fā)生碰撞。

作者認為此方法相當于給box加上一個自適應的buffer，而常規(guī)的固定大小的buffer會導致保守或者激進的駕駛行為。

論文[9]論文提出了一種可以嵌入現(xiàn)有Motion Planning框架的fail-safe機制，分為三部分：

• Set-based prediction：根據(jù)制定的交通參與者的駕駛策略和車輛運動學模型，將原有的交通參與者單一的預測軌跡，改為多預測軌跡；

• Fail-safe trajectory：根據(jù)預測的結(jié)果，計算原planning trajectory有碰撞風險的第一個軌跡點，然后再根據(jù)最優(yōu)化理論生成軌跡；

• online verification：將ADC在第二步生成的軌跡上進行投影，判斷其是否和第一步的預測車輛軌跡是否有碰撞。

感覺此方法是又重新做了一遍Motion Planning，由于論文中沒有描述fail-safe trajectory是否考慮decision的結(jié)果，可能會造成safe trajectory不滿足decision結(jié)果，并且此論文只是仿真，并沒有實際應用。

2.3.4 Partially Observable Environments

由于傳感器自身的感知范圍受限和感知結(jié)果的不確定性，在不良光照或者惡劣天氣中會進一步放大。而在城市工況中，建筑物的遮擋會造成不完全感知，如下圖所示。此外，大型車輛也會造成感知遮擋問題，而多數(shù)的Motion Planning都是以完全感知進行處理的，規(guī)劃結(jié)果具有很大的不安全性。

論文[10]提出了一種處理不完全感知的安全的Motion Planning，使規(guī)劃軌跡在最危險情況下可以在車輛最大制動能力下安全停車而不發(fā)生碰撞。分為兩種情況：一是在直道上行駛考慮感知的不確定性和感知距離范圍，二是在城市十字路口考慮不完全感知情況。并且容易嵌入其他的Motion Planning架構(gòu)中，作者在其之前提出基于最優(yōu)化方法的軌跡規(guī)劃中進行了仿真驗證(綜述中的圖(b))。

作者為其理論設計了幾個假設：

• 定位的縱向位置和速度信息遵從高斯分布；

• 感知的有效范圍是已知的，并且感知的結(jié)果遵從高斯分布；

• 地圖信息中包含建筑物位置，且為凸多邊形；

• 使用Intelligent Driver Model(IDM)進行車輛加速度預測。

由于論文分直道和十字路口兩種情況處理，因此需要進行場景識別，論文采用了基于規(guī)則的方式進行場景識別。

• 上左圖：紅色虛線是感知觀測到的環(huán)境的時間，黑色虛線是進行Motion Planning的時間，可見Motion Planning使用的感知信息是tp時刻前的。此外由于Motion Planning要保證連續(xù)性，在Motion Planning計算周期tpin時間內(nèi)的規(guī)劃軌跡要保證一致。更重要的是由于執(zhí)行器的延遲，在tsafe時間內(nèi)要保證軌跡的安全性。論文中tsafe= 2tpin；

• 上中圖：在直道行駛分為感知范圍內(nèi)沒有車輛或者感知范圍內(nèi)有車輛兩種情況：一是感知范圍內(nèi)沒有車輛，假設駕駛感知范圍外有一個靜止車輛，將其設為虛擬靜止障礙物，通過其高斯分布特性可以計算得到tsafe時刻內(nèi)，滿足以最大制動能力剎車的縱向位移和速度約束；二是感知范圍內(nèi)有車輛，考慮感知不確定性情況下的最危險情況，即前車以最大制動能力剎車，通過其高斯分布特性可以計算得到tsafe時刻內(nèi)，滿足以最大制動能力剎車的縱向位移和速度約束；

• 上右圖：在十字路口行駛，根據(jù)IDM模型計算ADC是需要讓行還是有路權需要明確表明表明自己優(yōu)先通過的意圖。最后轉(zhuǎn)換為直道行駛的兩種類型的約束。

2.4 Single Agent

Single Agent認為是單智能體問題，即ADC會對周圍環(huán)境做出決策，而不考慮ADC行為決策對其他交通參與者的影響，顯然這種假設是不對的，但是卻簡化了Motion Planning問題。

行為決策是影響自動駕駛發(fā)展的另一個重要方面，隨著自動駕駛的等級越高，行為決策的重要性越高。行為決策的難點是如何體現(xiàn)自動駕駛車輛的智能性，如何使自動駕駛車輛可以像人類駕駛員一樣處理高維度、多約束的復雜場景，甚至要比人類駕駛員的表現(xiàn)更好。

目前多數(shù)方法是基于規(guī)則的方法，其能力有限。以基于規(guī)則方法的行為決策來說，在下匝道工況，一般會設計一個距離匝道口的距離閾值。當ADC到匝道口的距離在閾值內(nèi)時，就開始向最右側(cè)車道變道。

假設這個閾值是2km，如果ADC在匝道口2.1m處位于中間車道行駛，此時前方剛好有輛車且速度較低，基于規(guī)則的行為決策一般會選擇向左側(cè)車道變道(左側(cè)車道限速高，超車遵從左側(cè)超車，從小鵬NGP等可以看出也是左側(cè)車道優(yōu)先)。但是變道后距離匝道口的距離閾值小于2km，此時需要向最右側(cè)車道變道，需要連續(xù)進行二次變道，會顯得不夠智能。

再比如在匝道前500m最右側(cè)車道行駛，前方由于施工或者事故不能行駛，此時只能由駕駛員接管。由此可見，由于現(xiàn)實工況的復雜性，基于規(guī)則的行為決策方式很難做到良好的駕乘體驗。

港科大關于OPMDP[11]的行為決策工作，相比于基于規(guī)則的方法，性能有了一定提升，其對ADC和其他交通參與者的行為進行了剪枝，降低了OPMDP的耗時。但是其考慮了其他交通參與者會對ADC的行為進行規(guī)避等，可以看出是一個Multiple Agent問題來處理。

2.5 Multiple Agent

上述的Single Agent中認為交通參與者不會對ADC的行為做出相應的決策.但實際中，當ADC做出決策后，其行為會影響到其他交通參與者的行為，而使原有的預測結(jié)果的可信性降低，尤其是有些簡單基于規(guī)則的prediction不依賴于Motion Planning結(jié)果，或者使用上一幀Motion Planning的結(jié)果(Apollo)。

例如在下圖左圖中，當ADC L沿著trajectory1行駛時，A2可能會減速避讓。當ADC L沿著trajectory2行駛時，A2可能會加速通過路口。但是當ADC L沿著trajectory2行駛時，預測A2可能會加速通過路口，但是A2可能會理解錯ADC L的意圖進行減速，會造成兩輛車鎖死。因此ADC怎么理解其他交通參與者的意圖和怎么讓其他交通車理解ADC的意圖至關重要[12]。

2.6 工程化問題

在Motion Planning中還面臨一些工程化問題，主要包括如下幾個方面。

• 實時性：在第一個問題中提到的了最優(yōu)性問題，如果要解決，由于在三維空間搜索計算的復雜性，其實時性很能保證，這也是限制時空聯(lián)合規(guī)劃應用的一個原因。此外最優(yōu)化算法中的大規(guī)模約束和非線性也面臨實時性的挑戰(zhàn)。

• 完備性：插值、Lattice等算法是概率完備的，尤其在復雜多障礙物環(huán)境中，有限的采樣很難獲得無碰撞的軌跡。而最優(yōu)化方法由于數(shù)值求解，也不能達到完備性，常用的osqp求解器甚至會給出一個錯誤的解。

• 難量化性：Motion Planning中的評價指標多是主觀性的，比如舒適性和通過性等，很難量化評價。不同工程師調(diào)參得到體感不同，又與乘客的主觀感受不同。因此提出了機器學習的方法來學習Motion Planning中的參數(shù)或者變道策略。

3. 行業(yè)解決方案

針對上述問題與挑戰(zhàn)，行業(yè)內(nèi)公司也在積極探索并提出了一些解決方案，下面列舉一二。

• 輕舟智航采用了時空聯(lián)合規(guī)劃解決最優(yōu)性問題，提高規(guī)劃性能，并且自研了非線性規(guī)劃器高效求解[2]。

• 圖森未來新一代框架中，感知模塊在提供障礙物位置、速度等信息時，同時提供不確定性或者概率信息，以保證決策規(guī)劃可以提前做出安全舒適的決策[13]。

• 特斯拉將planner用于交通參與者的其他車輛。但與其他車輛交互時，不能只為ADC規(guī)劃，而是要為所有交通參與者共同規(guī)劃，針對整體場景的交通流進行優(yōu)化。為了做到這一點，會為場景中的每個參與對象都運行autopilot規(guī)劃器。除此之外，針對停車場景，采用A*搜索算法和神經(jīng)網(wǎng)絡結(jié)合策略，大大減少了A*算法的節(jié)點探索[15]。

• 小鵬和特斯拉針對車道線缺失，道路拓撲變化問題做了優(yōu)化[14]。

• Waymo提出了ChauffeurNet用于提升決策性能[16]，Apollo借鑒ChauffeurNet提出了自己的強化學習架構(gòu)[17]。

參考文獻

[1] D. González, J. Pérez, V. Milanés and F. Nashashibi, "A Review of Motion Planning Techniques for Automated Vehicles," in IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 4, pp. 1135-1145, April 2016, doi: 10.1109/TITS.2015.2498841.

[2] 揚奇直播課堂：《自動駕駛中的決策規(guī)劃技術》輕舟智航

[3] 自動駕駛中軌跡規(guī)劃的探索和挑戰(zhàn)

[4] 基于動態(tài)行車安全場的智能網(wǎng)聯(lián)汽車決策規(guī)劃方法研究 

[5] W. Ding, L. Zhang, J. Chen and S. Shen, "Safe Trajectory Generation for Complex Urban Environments Using Spatio-Temporal Semantic Corridor," in IEEE Robotics and Automation Letters, vol. 4, no. 3, pp. 2997-3004, July 2019, doi: 10.1109/LRA.2019.2923954. 

[6] Artunedo, Antonio, et al. "Motion planning approach considering localization uncertainty." IEEE Transactions on Vehicular Technology 69.6 (2020): 5983-5994. 

[7] Lee, Seongjin, Wonteak Lim, and Myoungho Sunwoo. "Robust parking path planning with error-adaptive sampling under perception uncertainty." Sensors 20.12 (2020): 3560. 

[8] W. Xu, J. Pan, J. Wei and J. M. Dolan, "Motion planning under uncertainty for on-road autonomous driving," 2014 IEEE International Conference on Robotics and Automation (ICRA), 2014, pp. 2507-2512, doi: 10.1109/ICRA.2014.6907209. 

[9] Pek, Markus Koschi Christian and Matthias Althoff. “An online Verification framework for Motion Planning of Self-driving Vehicles with Safety Guarantees.” (2019). 

[10] ?. ?. Ta? and C. Stiller, "Limited Visibility and Uncertainty Aware Motion Planning for Automated Driving," 2018 IEEE Intelligent Vehicles Symposium (IV), 2018, pp. 1171-1178, doi: 10.1109/IVS.2018.8500369. 

[11] L. Zhang, W. Ding, J. Chen and S. Shen, "Efficient Uncertainty-aware Decision-making for Automated Driving Using Guided Branching," 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 3291-3297, doi: 10.1109/ICRA40945.2020.9197302. 

[12] Autonomous vehicles' intended cooperative motion planning for unprotected turning at intersections

[13] 圖森未來在重卡自動駕駛的最新落地與實踐 | 王乃巖

[14] 重新定義 LCC？城市道路小鵬 P5 輔助駕駛VS 特斯拉 Model 3

[15] Tesla AI Day 2021完整視頻

[16] ChauffeurNet Learning to Drive by Imitating the Best and Synthesizing the Worst

[17] Apollo決策規(guī)劃強化學習