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

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

典型的自動泊車規(guī)劃控制 Profile

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

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

本文優(yōu)化的泊車規(guī)劃算法

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

1、預測器

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

2、規(guī)劃器

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

自動泊車系統(tǒng)實現(xiàn)

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

A. 運動規(guī)劃定義

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

（2）

其中 Ts 是采樣時間。

B. 系統(tǒng)架構(gòu)

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

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

C. 混合環(huán)境預測器

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

1）級聯(lián)運動估計：

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

短期運動預測：

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

長期模式預測：

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

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

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

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

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

5）策略運動規(guī)劃器

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

6）軌跡修復規(guī)劃器：

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

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

總結(jié)

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

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

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

基于優(yōu)化的修復規(guī)劃器，用于在原始路徑失效時規(guī)劃新路徑。

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

審核編輯：湯梓紅