全自動泊車仍然具有挑戰性，尤其是在具有多個獨立代理的動態環境中，不僅因為它涉 及在狹小空間內的運動規劃，而且因為自動駕駛車輛 (AV) 應該對周圍的障礙物車輛 (OV)

做出智能反應。與在公路或高速公路上駕駛相比，泊車區域的車輛運動沒有一套明確的規則 可循，很大程度上取決于駕駛員的意圖甚至技能水平。這些使得泊車環境預測具有較大的 挑戰性，因此，要想對整個自動泊車控制有一個好的控制水平，也需要有一個集成預測和規 劃的自主泊車系統。自動泊車過程中，運動預測至關重要，因為它決定了規劃模塊的安全約束，從而決定了 運動規劃的可行性和平滑性。這其中的規劃過程包括了短期規劃和長期規劃兩個方面。特別 是，準確的短期運動預測使智駕車輛能夠對障礙物做出安全的計劃和反應；短期預測的一個 主要挑戰是估計障礙物的轉向角。通常可以使用擴展卡爾曼濾波器 (EKF) 來重建障礙物的 速度，然后求助于自適應觀測器進行轉向估計。而長期計劃/模式預測可以確保智駕車輛在 運行過程中更有效、更順利地進行規劃處合理的行駛路徑。長期運動通常盡管很難預測準確的，但可以觀察到，由于駕駛習慣，司機通常會遵循一 些路線（例如，在中國汽車應該靠右行駛）。此外，車輛在整個停車/離開過程中的運動可 以通過幾種“模式”來捕捉（例如，機動進出狹窄空間和在過道上巡航）。基于這兩個先 驗，可以使用成本圖來捕獲這些路線，結合短期預測來確定障礙物的模式，并進行長期預測。本文針對自動泊車預測與規劃提出了一種基于模型的混合預測器，通過觀察障礙物的姿態來執行短期和長期的模式預測。

典型的自動泊車規劃控制 Profile

智駕車輛的自動泊車運動規劃是自動泊車場景中的一個主要挑戰。一般運動規劃算法 無法直接適用于存在障礙物的地方泊車，因為這需要對復雜的駕駛操作進行快速重新規劃。另一方面，專門從事自動泊車的運動規劃要么未能將短期規劃與長期規劃相結合，要么無法 將在線路徑修復納入動態環境中的新障礙。實施多種策略的場景感知規劃器在計算時間方面 可能是有效的，從而導致重新規劃率較高，才可能保證安全。總體來講，自動泊車的規控模塊主要包含 4 個部分：障礙物車輛軌跡預測，可行駛區域 選擇，局部軌跡規劃，車輛控制。

如上圖所示為一種典型的泊車路徑規劃的軟件分布模式圖。其中，預測可以使用交互多模型濾波器 Interacting multiple model (IMM) filter，輸入自車位置、障礙物車的 heading angle, velocity, acceleration, yaw-rate，輸出的 intention 分 2 種：直行，規避對向來車。“可行駛區域”選擇采用勢場法，障礙物預測軌跡、道路邊沿、靜態障礙物的位姿都可 以作為可行駛邊界的輸入，由此來構造勢能函數，以評估危險性以及是否可行駛。“局部路徑規劃”則可以采用 RRT 算法，采用了 2 個小技巧來提升速度：將可行駛區 域劃分為 seeding surfaces，在這些 surfaces 上隨機采樣；使用前述的勢能函數的倒數作為采 樣的概率密度函數，以減少采樣點數。在采樣點中尋找代價最小路徑作為動態規劃（DP） 方法。輸出的軌跡可以用 B-spline（piecewise polynomial）進行平滑。

本文優化的泊車規劃算法

本文算法以前序基礎的泊車算法為基礎進行了有效信息的優化。首先使用雙向 A 搜索 引導樹 (BIAGT) 生成長期運動參考。然后，基于混合環境預測器結果的戰略運動規劃器實 施三種策略：基于模型預測控制（MPC）的安全控制器，如果參考對環境仍然有效，則用于 軌跡跟蹤；基于搜索的避撞路徑規劃，可在緊急情況下快速找到避撞路徑；以及當參考失效 時基于優化的修復可用路徑規劃。基于以上操作我們可以實現基于模型的混合環境預測器對前面提到的預測短期運動和 長期運動模式進行規劃。由此，制定的一種策略型運動規劃器，可以實現在不同情況下進行 有效規劃。

1、預測器

車輛運動預測的研究對于自動駕駛系統的有效性控制有比較大的貢獻，其中涉及短期運 動預測方法和長期規劃/模式預測方法。可以說，這一領域的大多數研究都與道路駕駛有關。相比之下，泊車系統中車輛運動預測研究較少。交互多模型 (IMM) 濾波器可以用于預測泊 車中的短期軌跡。如果著眼于長期預測，首先需要訓練軌跡聚類分類器，然后獲取分類聚類 的均值軌跡。分類器識別的駕駛員意圖和車輛歷史姿態可以用于通過長期記憶網絡，從而生 成長期運動預測。對于這類泊車軌跡規劃而言純數據驅動的方法不可取，原因有二：1) 缺乏性能保證。它們的性能在很大程度上取決于訓練數據集，如果數據集選擇不當， 它們可能會出現較大的預測誤差。2) 預測網絡過度擬合也可能是一個問題。目前還沒有對融合短期和長期停車預測的預 測因子進行廣泛的研究。

2、規劃器

流行的運動規劃方法分為三類：基于搜索的、基于采樣和基于優化的方法。基于抽樣的 規劃器由于其不確定性，可能會引起對風險敏感任務的過度關注，而基于優化的規劃器只是 局部最優的，并且經常需要與全局規劃器一起工作。自動駕駛汽車廣泛采用各種基于搜索的 運動規劃器，因為它們具有精心選擇的運動原語和啟發式算法來提升計算效率。如果泊車環境發生變化，初始的長期軌跡可能需要修復。實時軌跡修復方法包括在線啟 發式更新，基于采樣的搜索結構進行修剪和重新連接，以及基于樣條的運動動力學搜索。啟 發式更新方法不能直接應用于在 BIAGT 中基于樹的搜索結構，并且修剪對于停車場景效率 較低。而基于樣條的運動動力學搜索相對有效，但未利用原始軌跡。另一方面，我們觀察到修復場景中的備選可行解通常與原始軌跡屬于同類型。因此，基于對備選路徑的可行解進行 有效優化會生成修復現有路徑的合適候選路徑。

自動泊車系統實現

對于自動泊車的系統級策略來說，當前已有在靜態環境中的自動泊車系統中采用交互多 模型 IMM 進行預測，同時采用基于抽樣的方法進行規劃。該方法首先預測障礙物信息，然后 選擇自動駕駛汽車的導航策略。對于智駕車輛而言，需要穿過停車場的道路，但不執行停車 操作。相反，在采用更完整的綜合泊車系統方法后，可以實現對環境進行短期和長期預測， 便可將其用于自主泊車戰略規劃。

A. 運動規劃定義

考慮車輛動力學的規劃問題可表示為：X’= f(X) + g(X; u); (1) 其中 X = [x;y;θ] T 表示二維坐標和車輛航向，u = [δ; v] T 是包括縱向速度和轉向 角的控制輸入。無碰撞配置空間 Cfree ? R nc 是車輛與障礙物沒有交叉點的配置集。本文考慮的運動規劃問題可以定義為給定初始配置 X0 ?free、目標配置 Xf ?Cfree 和公 式(1)，找到可行軌跡 Pt，其 (1) 起始于 0 到 Xf 結束，同時滿足（1）(2) 在于無碰撞配 置空間 Cfree。使用常見的自行車模型來表示車輛運動。通過歐拉離散化得到離散時間模型如下：

（2）

其中 Ts 是采樣時間。

B. 系統架構

下圖顯示了所提出自動泊車系統架構。混合環境預測器和戰略運動規劃器是兩個主要組 成部分。在運行時，中央控制首先處理停車場地圖 Mmap 并生成初始長期軌跡 Pref。這里使用 搜索引導樹 BIAGT，規劃生成一條軌跡，使本自動駕駛車輛準確地到達目標，這是在狹窄空 間停車的一個重要特征。

混合環境預測器監視環境并預測障礙物的移動。根據預測，戰略運動規劃器檢查自動駕 駛車輛是否違反了安全邊際。如果不是，它會檢查初始長期預測軌跡 Pref 是否由于障礙物 的運動需要被修復。如果發生任何這些情況，Pref 將被更新。最后，基于模型預測控制器 MPC 規劃一個無碰撞運動，以跟蹤動態環境中的最新的長期運動軌跡 Pref。如果修復規劃器無法 成功優化可行駛的自車運動軌跡，則會請求中央域控更新地圖并重新生成參考軌跡。

C. 混合環境預測器

混合環境預測器包含三個主要部分：障礙物的運動估計、運動預測和模式估計。接下來 對其主要的幾個特性進行說明。

1）級聯運動估計：

運動估計在基于獨輪車模型從 (x; y) 的測量中重建狀態 X。這樣的處理對于涉及頻繁 改變移動方向和轉向動作的泊車過程是不夠的。為了準確預測障礙物的短期運動，重構控制 輸入 u 是有利的。可以將其視為未知輸入估計問題或使用控制輸入增加障礙物車輛 OV 的 系統狀態并解決狀態估計問題。假設障礙物運動按照模型 (2) 演化。通過假設控制輸入 (δ; v) 是分段常數來獲得 OV 的增廣模型，并估計增廣狀態 [x;y;θ;δ;v] T。給定非線性增強模型，很自然地應用完善的非線性狀態估計器（例如擴展的卡爾曼濾波 EKF 或粒子濾波器）進行狀態估計。通過調整 EKF 來準確估計轉向角其實并不簡單，這部 分歸因于涉及未測量狀態乘法的項 v tan(δ)的存在多帶來的。粒子濾波器雖好，但是其繁 重的計算也可能阻礙實際的運算。

短期運動預測：

短期運動預測更加關注計算效率和實時性，我們假設障礙物車輛 OV 的短期運動完全由 狀態 Xcc 的平均值及其協方差捕獲。通過向前傳播估計狀態為 Xcc，k 并獲得短期預測 XH ;k = [X1 ,kT ,......,XH ,kT ] T 用于時間范圍的未來 H 個步驟。同理，根據擴展卡爾曼濾波器 EKF 的前向預測公式進行前向傳播得到協方差矩陣 PmH，k ={Pmk+1 ,......,Pmk+H }，這些信息將有助于 長期預測，并用于確定每個未來時間步長的安全裕度。

長期模式預測：

長期運動所關注的障礙物車輛 OV 在一段時間內的運動依賴于它的歷史狀態、動態模型 及其相對于環境的運動。其中前兩個因素在某種程度上被短期運動預測捕獲。為了利用相對 環境運動，可以引入成本圖（如下圖所示混合預測器預測短期 OV 軌跡（綠線）并將其與模 式預測結合使用以生成 h = 1 和 h = H 的安全裕度和安全邊界。），以此來捕捉障礙物車 輛 OV 可能的長期運動。

同時，也可以使用路線規劃器構建成本圖路徑 Mroute，其中成本圖包含障礙物車輛 OV 所 采用的可能路線。

其中 Xmk;i 是模式中路線的第 i 個航路點；?，W1 和 W2 為權重矩陣。函數 f(mk ; Xcc;k ) 與 OV 轉向角的大小以及 OV 航向角與路線最終航向角的 偏差成正比。最后，對進行歸一化，得到 b(mk )，取最大置信度的 mk的值為 m^k。此外，停車場中的車輛通常以兩種模式運行，“手動模式”和“巡航模式”。處于手動 模式的車輛有可能會頻繁改變轉向并偏離成本圖中的路線（上圖中的黑色虛線）以停放或離 開狹窄的停車位。處于巡航模式的車輛具有較小或穩定的轉向角，并且通常遵循其中一條路 線。車輛首次進入停車場并接近停車位時或車輛離開停車位并離開停車場時處于該模式。

4）安全裕度和安全界限：

由于障礙物車輛 OV 在機動模式下的運動很難預測，因此預測器需要生成一個自車運動 的安全邊界（OV 歷史姿態的凸形邊界，上圖中的橙色線）導致規劃模型將更加保守，確保 使自車遠離難以預測的障礙物車輛 OV。請注意，考慮到運動學模型和路線信息，安全裕度 和安全邊界也可以應用于其他移動障礙物，例如行人或摩托車。

5）策略運動規劃器

使用參考軌跡 Pref，運動策略規劃器運行的主要模塊包括基于 MPC 的安全控制器和兩 個支持模塊：避撞軌跡規劃器和修復軌跡規劃器。如果自車當前位置和參考軌跡分別因自車 的運動而無效，則如上兩個規劃器都會被激活。A、基于 MPC 的安全控制器：安全控制器在給定安全余量和安全界限的情況下跟蹤參考軌跡 Pref。借助 Pref，使用優 化基礎規劃器的方式來計算 MPC 框架中的跟蹤軌跡。假設 Xref;k 為 Pref 的片段，設置時間 步長 k 進行跟蹤。外部參照 k 將被選擇和修剪，確保不會違反在所有模式要求的安全裕度 或者在“機動”模式下安全邊界。整個軌跡跟蹤問題可以表述為非凸優化問題的解決方式， 這里可以使用非線性規劃求解器（例如 IPOPT）進行求解，以參考路徑作為熱啟動，平均求 解時間約為 0:06 秒。B、避障規劃：當自車停留在原始參考軌跡上可能會被認為不安全，此時需要啟動避障規劃程序。這種 情況可能發生在障礙物 OV 駛向自車時，其運動與之前的預測有很大不同——可能違反安全 裕度并造成安全威脅。因此，自車需要尋路避險。通常不建議采取后退運動，因為自車沒有 安全目標。相反，探索前向可行駛環境來找到最佳目標才是可選的，因此我們提出了一種基 于搜索的避障規劃器，它可以探索空間并快速找到避障軌跡。

6）軌跡修復規劃器：

障礙物車輛 OV 的動作可能會使自車的參考軌跡無效。下圖說明了這樣一種情況，其中 由藍色框表示的障礙物車輛 OV，停在自車的參考軌跡上，用淺藍色線表示。

當前方區域不可行時，安全控制器將命令自車停在參考軌跡上。除非接收到新的參考軌 跡，否則安全控制器將控制自車停止運動并等待障礙物 OV 消失 - 如果 OV 長時間停止則認 為此次軌跡規劃不合理或效率較低。此時應該更新參考軌跡 Pref，這樣安全規劃器就可以指 導自車繞過 OV 并合并回原來的路徑。我們注意到修復后的軌跡通常與原始軌跡位于同一類 軌跡集合，這樣可以直接通過基于優化當前軌跡來進行安全隱患修復生成可執行的軌跡。為 了快速獲得修復路徑，就需要在二維空間上進行軌跡規劃的修復，即設置軌跡為 Xrepair = [x; y] T ,并相應地修改約束。由此產生的路徑盡管沒有碰撞，但不能確保循跡始終準確，從而可能引起自動駕駛汽車撞上障礙物。因此，只有當它通過運動學可行性檢查時，才會被認可為 已驗證的路徑并接受為修復后的軌跡。如果修復失敗，則通知中控接管修復任務。

總結

本文介紹了一種用于在存在其他移動車輛的情況下自動駕駛車輛 (AV) 泊車集成運動 規劃系統。這一自動泊車系統包括預測周圍車輛運動的混合環境預測器和對預測做出反應的 戰略運動規劃器。戰略運動規劃器包括三種：

用于軌跡跟蹤的基于模型預測控制的安全控制器；

基于搜索的避撞規劃器，用于在緊急情況下尋找避撞路徑；

基于優化的修復規劃器，用于在原始路徑失效時規劃新路徑。

基于此，混合環境預測器通過擴展卡爾曼濾波器和自適應觀察器執行短期預測。它還將短期預測與駕駛員行為成本圖相結合，以做出長期預測。

審核編輯：湯梓紅