全自動(dòng)泊車(chē)仍然具有挑戰(zhàn)性，尤其是在具有多個(gè)獨(dú)立代理的動(dòng)態(tài)環(huán)境中，不僅因?yàn)樗?及在狹小空間內(nèi)的運(yùn)動(dòng)規(guī)劃，而且因?yàn)?a href="http://www.1cnz.cn/tags/自動(dòng)駕駛/" target="_blank">自動(dòng)駕駛車(chē)輛 (AV) 應(yīng)該對(duì)周?chē)恼系K物車(chē)輛 (OV)

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

典型的自動(dòng)泊車(chē)規(guī)劃控制 Profile

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

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

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

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

1、預(yù)測(cè)器

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

2、規(guī)劃器

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

自動(dòng)泊車(chē)系統(tǒng)實(shí)現(xiàn)

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

A. 運(yùn)動(dòng)規(guī)劃定義

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

（2）

其中 Ts 是采樣時(shí)間。

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

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

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

C. 混合環(huán)境預(yù)測(cè)器

混合環(huán)境預(yù)測(cè)器包含三個(gè)主要部分：障礙物的運(yùn)動(dòng)估計(jì)、運(yùn)動(dòng)預(yù)測(cè)和模式估計(jì)。接下來(lái) 對(duì)其主要的幾個(gè)特性進(jìn)行說(shuō)明。

1）級(jí)聯(lián)運(yùn)動(dòng)估計(jì)：

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

短期運(yùn)動(dòng)預(yù)測(cè)：

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

長(zhǎng)期模式預(yù)測(cè)：

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

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

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

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

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

5）策略運(yùn)動(dòng)規(guī)劃器

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

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

障礙物車(chē)輛 OV 的動(dòng)作可能會(huì)使自車(chē)的參考軌跡無(wú)效。下圖說(shuō)明了這樣一種情況，其中 由藍(lán)色框表示的障礙物車(chē)輛 OV，停在自車(chē)的參考軌跡上，用淺藍(lán)色線(xiàn)表示。

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

總結(jié)

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

用于軌跡跟蹤的基于模型預(yù)測(cè)控制的安全控制器；

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

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

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

審核編輯：湯梓紅