編者按：目前機(jī)器學(xué)習(xí)已經(jīng)廣泛的應(yīng)用在自動(dòng)駕駛規(guī)劃、控制算法的開(kāi)發(fā)當(dāng)中。但是車輛的工作環(huán)境復(fù)雜多變，具有較高的不確定性，加之建模的誤差、環(huán)境噪聲以及軟件的性能和邏輯漏洞等，這些都對(duì)車輛的通行能力、學(xué)習(xí)算法的效率等提出了更高的要求，車輛的實(shí)際表現(xiàn)非常有可能超出由建模過(guò)程中的假設(shè)條件推導(dǎo)出的車輛運(yùn)動(dòng)安全邊界，對(duì)行車安全造成很大的威脅。為了最大程度保證行車安全、提升系統(tǒng)的魯棒性，對(duì)車輛進(jìn)行異常監(jiān)測(cè)是十分必要的。本文提出了一種對(duì)系統(tǒng)的正常狀態(tài)進(jìn)行學(xué)習(xí)、異常狀態(tài)進(jìn)行學(xué)習(xí)的方法，可以對(duì)算法的適應(yīng)性、安全性等進(jìn)行不同程度的提升。

本文譯自：

《Enhancing Lattice-based Motion Planning With Introspective Learning and Reasoning 》

文章來(lái)源：

IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 6, NO. 3, JULY 2021

作者：

Mattias Tiger , David Bergstr?m , Andreas Norrstig, and Fredrik Heintz

原文鏈接：https://arxiv.org/abs/2005.07385

摘要：基于柵格的運(yùn)動(dòng)規(guī)劃是一種混合規(guī)劃方法，規(guī)劃由離散動(dòng)作組成，同時(shí)也是物理上可行的軌跡。該規(guī)劃同時(shí)考慮了離散和連續(xù)的方面，例如動(dòng)作前提條件和配置空間中的無(wú)碰撞動(dòng)作持續(xù)時(shí)間。安全運(yùn)動(dòng)規(guī)劃依賴于經(jīng)過(guò)良好校準(zhǔn)的安全裕度來(lái)進(jìn)行碰撞檢測(cè)。軌跡跟蹤控制器必須進(jìn)一步能夠在這個(gè)安全范圍內(nèi)可靠地執(zhí)行運(yùn)動(dòng)，以便保持安全。在這項(xiàng)工作中，我們關(guān)注的是內(nèi)省學(xué)習(xí)和推理對(duì)控制器的性能的影響。使用顯式不確定性量化的機(jī)器學(xué)習(xí)技術(shù)學(xué)習(xí)不同動(dòng)作的正?？刂破鲌?zhí)行，以便在安全關(guān)鍵應(yīng)用中安全使用。通過(guò)提高模型的精度，可以降低安全邊際，同時(shí)保持與以前相同的安全性。推理既可以驗(yàn)證學(xué)習(xí)到的模型保持安全的性能，也可以使用更準(zhǔn)確的執(zhí)行預(yù)測(cè)和更小的安全邊際來(lái)在運(yùn)動(dòng)規(guī)劃器中提高碰撞檢查的有效性。該方法允許在正常情況下明確感知控制器的性能，并在異常情況下檢測(cè)不正確的性能。利用仿真方法對(duì)四軸飛行器的三維非線性動(dòng)力學(xué)進(jìn)行了評(píng)價(jià)。

關(guān)鍵詞：運(yùn)動(dòng)和路徑規(guī)劃，避免碰撞

Ⅰ 引言 
安全的運(yùn)動(dòng)規(guī)劃是機(jī)器人在動(dòng)態(tài)、非結(jié)構(gòu)化和人為定制的環(huán)境中導(dǎo)航的必要條件，如在室內(nèi)或城市環(huán)境中。在真實(shí)的動(dòng)態(tài)環(huán)境中操作使內(nèi)省功能變得重要，因?yàn)榍闆r可以很容易地改變，超出合理的設(shè)計(jì)假設(shè)：硬件退化、建模錯(cuò)誤、軟件錯(cuò)誤以及罕見(jiàn)的外部干擾，如極端天氣或來(lái)自其他外界的意外對(duì)抗性攻擊。從機(jī)器人的角度了解正常的運(yùn)動(dòng)執(zhí)行是什么樣子，并及時(shí)檢測(cè)所執(zhí)行的行為何時(shí)變得異常是非常重要的?；跂鸥竦倪\(yùn)動(dòng)規(guī)劃是自動(dòng)化車輛實(shí)際實(shí)現(xiàn)中最常用的運(yùn)動(dòng)規(guī)劃技術(shù)之一。它的工作原理是將運(yùn)動(dòng)限制在有限數(shù)量的預(yù)先計(jì)算的運(yùn)動(dòng)基元上，這些運(yùn)動(dòng)基元在狀態(tài)格上的點(diǎn)之間移動(dòng)，并利用圖搜索找到一個(gè)物理上可行的軌跡作為兼容的運(yùn)動(dòng)基元序列。這是一種適合于動(dòng)態(tài)環(huán)境的技術(shù)和一種快速搜索方法來(lái)尋找可以實(shí)時(shí)執(zhí)行最優(yōu)軌跡，考慮舒適、安全和車輛約束?；跂鸥竦倪\(yùn)動(dòng)規(guī)劃已經(jīng)成功地在各種機(jī)器人上實(shí)現(xiàn)了。例如，最近的進(jìn)展是，在實(shí)時(shí)重新規(guī)劃新的具有多個(gè)動(dòng)態(tài)障礙的無(wú)碰撞軌跡的挑戰(zhàn)中。使用像度量時(shí)間邏輯(MTL)或者概率信號(hào)的時(shí)序邏輯(ProbSTL)這樣的正式語(yǔ)言來(lái)評(píng)價(jià)正常行為，用于現(xiàn)代運(yùn)行時(shí)驗(yàn)證框架，以執(zhí)行運(yùn)動(dòng)計(jì)劃的執(zhí)行監(jiān)控，是一個(gè)非常困難的挑戰(zhàn)。機(jī)器學(xué)習(xí)可以用來(lái)補(bǔ)充正式的安全要求，學(xué)習(xí)的正常動(dòng)作執(zhí)行模型，例如機(jī)器人操作任務(wù)。

圖1 當(dāng)動(dòng)作執(zhí)行是安全至關(guān)重要的時(shí)候，對(duì)于像機(jī)器人這樣的自主系統(tǒng)來(lái)說(shuō)，機(jī)器人本身知道正常的執(zhí)行是什么樣子的（學(xué)習(xí)）以及當(dāng)前的動(dòng)作是正常的還是異常的（監(jiān)控）是很重要的。

基于格的運(yùn)動(dòng)規(guī)劃提供了一個(gè)有效學(xué)習(xí)和監(jiān)控運(yùn)動(dòng)動(dòng)作執(zhí)行的機(jī)會(huì)，因?yàn)橐呀?jīng)離散和定義良好的運(yùn)動(dòng)基元（動(dòng)作）。在這篇文章中，我們提出了一種用于增強(qiáng)基于柵格的運(yùn)動(dòng)規(guī)劃方法的一般方法：（1）正常的運(yùn)動(dòng)基元系統(tǒng)執(zhí)行的學(xué)習(xí)模型 ，（2）使用學(xué)習(xí)的模型，提高碰撞檢測(cè)的有效性 ，（3）有效的監(jiān)控運(yùn)動(dòng)基元執(zhí)行的異常情況。
由于碰撞檢查和異常檢測(cè)都是安全的關(guān)鍵，學(xué)習(xí)是使用機(jī)器學(xué)習(xí)技術(shù)（從貝葉斯機(jī)器學(xué)習(xí)的顯式不確定性量化）完成的。對(duì)異常的監(jiān)視并實(shí)時(shí)驗(yàn)證學(xué)習(xí)到的模型對(duì)于運(yùn)動(dòng)規(guī)劃器中使用的碰撞檢查仍然有效。第二節(jié)描述了給定動(dòng)態(tài)障礙的現(xiàn)代基于網(wǎng)格的運(yùn)動(dòng)規(guī)劃和控制方法。相關(guān)工作在第三小節(jié)中。我們方法中的學(xué)習(xí)、改進(jìn)碰撞檢查和監(jiān)測(cè)運(yùn)動(dòng)原始執(zhí)行的方法在第四節(jié)中提出，結(jié)果在第五節(jié)中，結(jié)論在第六節(jié)中。

Ⅱ 問(wèn)題構(gòu)造

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

考慮一個(gè)被建模為時(shí)常非線性系統(tǒng)的機(jī)器人：

其中，代表機(jī)器人的狀態(tài)； 是控制信號(hào)；機(jī)器人對(duì)其狀態(tài)及其控制信號(hào)施加了物理約束。該機(jī)器人在2D或3D世界中運(yùn)行 或者 。有一些區(qū)域被靜態(tài)和動(dòng)態(tài)的障礙所占據(jù) ；自由空間是機(jī)器人所占據(jù)的區(qū)域由狀態(tài)轉(zhuǎn)換，在時(shí)刻沒(méi)有與任何障礙物發(fā)生碰撞。

運(yùn)動(dòng)規(guī)劃器的目標(biāo)是產(chǎn)生一個(gè)可行的參考軌跡，使機(jī)器人從開(kāi)始狀態(tài)移動(dòng)到目標(biāo)狀態(tài)。同時(shí)優(yōu)化一個(gè)給定的性能度量，例如，在最小時(shí)間和平滑度之間進(jìn)行權(quán)衡。考慮到這些障礙，參考軌跡也必須是無(wú)碰撞的。這個(gè)問(wèn)題被稱為動(dòng)態(tài)運(yùn)動(dòng)規(guī)劃問(wèn)題(DMPP)：

B.基于柵格的運(yùn)動(dòng)規(guī)劃

基于柵格的運(yùn)動(dòng)規(guī)劃是DMPP 問(wèn)題的一個(gè)可處理的近似，其中狀態(tài)空間被離散為一個(gè)狀態(tài)柵格，并構(gòu)造了有限數(shù)量的平移不變運(yùn)動(dòng)基元（動(dòng)作），以允許在柵格上的狀態(tài)（節(jié)點(diǎn)）之間的運(yùn)動(dòng)。圖搜索技術(shù)，如具有可接受啟發(fā)式的A?，可以用來(lái)尋找一個(gè)從到的有效的運(yùn)動(dòng)基元?jiǎng)幼餍蛄?。一個(gè)運(yùn)動(dòng)基元?jiǎng)幼魇窃跂鸥窬W(wǎng)格上從一個(gè)初始狀態(tài)到一個(gè)最終狀態(tài)的規(guī)劃。上述的規(guī)劃滿足如下的條件：

運(yùn)動(dòng)基元是利用使用相同的損失函數(shù)的數(shù)值最優(yōu)控制離線生成的，并被分配得到的目標(biāo)函數(shù)值。圖2顯示了一些運(yùn)動(dòng)基元。

對(duì)DMPP的柵格近似是：

是由狀態(tài)中的位置定義的平移矩陣，是一個(gè)在位置維數(shù)上的零對(duì)角線矩陣。 是規(guī)劃中前個(gè)運(yùn)動(dòng)基元的結(jié)果平移，是規(guī)劃中的前個(gè)運(yùn)動(dòng)基元的開(kāi)始時(shí)間。

所得到的規(guī)劃結(jié)果由一系列個(gè)運(yùn)動(dòng)基元?jiǎng)幼鹘M成，，。這個(gè)規(guī)劃的結(jié)束時(shí)間是。參考軌跡是由平面中運(yùn)動(dòng)基元序列的順序時(shí)空連接構(gòu)成的?；跂鸥竦倪\(yùn)動(dòng)規(guī)劃是完整的解決方案，他在分辨率極限下與DMPP等價(jià)。

圖2 在總共104個(gè)基元中，26個(gè)運(yùn)動(dòng)基元的初始速度和最終速度為零。所有開(kāi)始位置為

由運(yùn)動(dòng)規(guī)劃器找到的參考軌跡由軌跡跟蹤控制器執(zhí)行，例如使用非線性模型預(yù)測(cè)控制器（MPC）。軌跡跟蹤控制器的目標(biāo)是讓機(jī)器人以較小的跟蹤誤差跟蹤期望的參考軌跡，同時(shí)保持接近前饋控制信號(hào)。

對(duì)當(dāng)前時(shí)間點(diǎn)求解的連續(xù)時(shí)間非線性MPC問(wèn)題表示為：

其中，設(shè)計(jì)參數(shù)、、均為正定權(quán)值矩陣，為預(yù)測(cè)范圍。

C.碰撞檢測(cè)

機(jī)器人占據(jù)了一個(gè)區(qū)域，取決于其在時(shí)刻的狀態(tài)。我們想要找到一個(gè)參考軌跡，它在任何時(shí)間點(diǎn)是無(wú)碰撞的。

在應(yīng)用的運(yùn)動(dòng)規(guī)劃中存在著許多可能的不確定性來(lái)源。這些可能是由于例如建模誤差、傳感器噪聲、噪聲控制或其他因素的不可預(yù)測(cè)性。不確定性可以分為對(duì)狀態(tài)的感知和對(duì)未來(lái)狀態(tài)的可預(yù)測(cè)性，例如由于機(jī)器人的行動(dòng)，機(jī)器人和它的環(huán)境。緩解不確定性的一種常見(jiàn)方法是使用安全裕度來(lái)減少不確定性引起的碰撞。這種安全裕度邊界在本研究中考慮的是一個(gè)相對(duì)于機(jī)器人坐標(biāo)系的區(qū)域，它擴(kuò)展了機(jī)器人的空間占用率。

其中，是被機(jī)器人實(shí)際占據(jù)的區(qū)域；區(qū)域上的二進(jìn)制算符被定義為：

另一種方法是擴(kuò)展障礙區(qū)域：安全裕度可分為三個(gè)主要部分，

從機(jī)器人的角度、控制結(jié)果和在其他機(jī)器人的行為來(lái)反映了世界狀態(tài)的不確定性。在存在不確定性的安全關(guān)鍵應(yīng)用中，為了保證概率分布，必須保證概率安全性和概率可行性。一個(gè)安全裕度可以從這樣的表示來(lái)構(gòu)造約束碰撞的概率，例如每秒最多為0.01%，在文獻(xiàn)中經(jīng)常被稱為機(jī)會(huì)約束。

第三個(gè)安全邊際的組成部分對(duì)于基于柵格的方法很重要，但到目前為止在很大程度上被忽視了。由于建模誤差、噪聲源和硬件的限制，實(shí)際上期望軌跡跟蹤控制器會(huì)跟蹤參考軌跡是不合理的。即使使用精確的模型和沒(méi)有噪聲，精度也取決于運(yùn)動(dòng)原元序列，以及設(shè)計(jì)參數(shù)、、、的選擇。這種跟蹤誤差往往同時(shí)有偏差和協(xié)方差，并直接影響。本文提出了一種系統(tǒng)地確定用于基于柵格的運(yùn)動(dòng)規(guī)劃的方法，并且還可以在運(yùn)行時(shí)驗(yàn)證確實(shí)是正確的概率。后者是為了確保在不斷變化的情況下保持安全保障。

Ⅲ  相關(guān)工作

雜亂、復(fù)雜、動(dòng)態(tài)環(huán)境中的實(shí)時(shí)運(yùn)動(dòng)規(guī)劃問(wèn)題在文獻(xiàn)中受到越來(lái)越多的關(guān)注。

多分辨率柵格等進(jìn)展使得基于柵格的方法適用于雜亂和復(fù)雜的環(huán)境?；跂鸥竦倪\(yùn)動(dòng)規(guī)劃實(shí)現(xiàn)了最先進(jìn)的（SOTA）性能，例如具有多個(gè)拖車的卡車，對(duì)于四軸飛行器的規(guī)劃和重新規(guī)劃，有動(dòng)態(tài)和時(shí)間，在復(fù)雜的環(huán)境中實(shí)時(shí)的動(dòng)態(tài)障礙。我們?cè)诘贗I-A節(jié)和第II-B節(jié)中概述了[3]的一般問(wèn)題設(shè)置。我們通過(guò)提供詳細(xì)的上下文來(lái)展示如何將我們提出的內(nèi)省能力集成到基于柵格的運(yùn)動(dòng)規(guī)劃中，并且我們的工作與[3]中的多分辨率柵格的方法完全兼容。[3]中的多分辨率柵格點(diǎn)方法又是基于[14]，[14]中的軟約束可以包含在損失函數(shù)中。

通常的做法是放松的組成，并選擇一個(gè)任意的大區(qū)域作為可能滿足期望的安全約束的。這種方法中最常見(jiàn)的簡(jiǎn)化是使用像[15]這樣的球形安全邊際，由單個(gè)半徑參數(shù)定義。這種做法雖然計(jì)算效率很高，但通常需要一個(gè)比實(shí)際需要的要大得多的，從而降低了碰撞檢查的有效性。這可能是由于不同方向上的位置不確定性和位置偏差造成的，這兩者都可能在強(qiáng)度和方向上隨著時(shí)間的推移而變化（如強(qiáng)風(fēng)）。先前的工作調(diào)查了的不同部分，并提供了概率基于和 的方法。

在[16]中，在不確定性下使用基于柵格的運(yùn)動(dòng)規(guī)劃，在參考軌跡中明確考慮了狀態(tài)不確定性。其中的一個(gè)結(jié)果是，在規(guī)劃期間的每個(gè)時(shí)間點(diǎn)上， 都可以作為概率的基礎(chǔ)。例如，它可以是時(shí)間點(diǎn)狀態(tài)的99%概率區(qū)域。就其本身而言，這對(duì)應(yīng)于一個(gè)安全邊際，即機(jī)器人所占據(jù)的區(qū)域在范圍內(nèi)的可能性至少為99%。

如果有一個(gè)真實(shí)的目標(biāo)域模擬器，它可以用來(lái)模擬其他個(gè)體的真實(shí)行為，以及這些個(gè)體與機(jī)器人之間的交互。這樣的模擬器可以用于確定的不確定性，并且邊際可以被調(diào)整以滿足例如99.9%的無(wú)碰撞的概率。由于是基于其他個(gè)體的預(yù)測(cè)模型的，因此它適用于各種運(yùn)動(dòng)規(guī)劃方法，包括基于柵格的方法。

在其他運(yùn)動(dòng)規(guī)劃方法中已經(jīng)考慮了安全裕度部分，比如那些基于快速擴(kuò)展的隨機(jī)樹(shù)（RRT），其中過(guò)程噪聲用于采樣可能的運(yùn)動(dòng)規(guī)劃執(zhí)行。在[16]中，過(guò)程噪聲影響參考軌跡周圍的協(xié)方差，但不確定性是無(wú)偏的。

我們的貢獻(xiàn)，除了提出的集成到基于柵格的運(yùn)動(dòng)規(guī)劃，在基于柵格的范式之外是有用的。學(xué)習(xí)模型和運(yùn)動(dòng)基元的異常檢測(cè)假設(shè)有一組固定的運(yùn)動(dòng)基元，我們可以觀察它們的執(zhí)行，如在SOTA中的四軸飛行器運(yùn)動(dòng)規(guī)劃[18]，[19]。碰撞檢查有效性的提高進(jìn)一步假設(shè)了模型是對(duì)執(zhí)行可變性的精確描述，這可能并不總是適用于搜索后的軌跡優(yōu)化[4]，[18]。這將導(dǎo)致我們提出的方法也包括在不同的優(yōu)化結(jié)果的變化。

Ⅳ 內(nèi)省基于柵格的運(yùn)動(dòng)規(guī)劃與控制

圖3 學(xué)習(xí)基元的正常執(zhí)行模型的說(shuō)明。左：所有有效的三個(gè)運(yùn)動(dòng)基元規(guī)劃，中間是（前一個(gè)，下一個(gè)）。中心：觀察每個(gè)三個(gè)運(yùn)動(dòng)基元規(guī)劃的執(zhí)行情況。右：?jiǎn)畏宸植嫉钠骄A(yù)測(cè)區(qū)域和95%概率區(qū)域（11），捕獲了正常執(zhí)行的預(yù)期可變性。

A.學(xué)習(xí)正常的基元執(zhí)行

運(yùn)動(dòng)基元的執(zhí)行，被機(jī)器人感知，是一個(gè)離散軌跡，帶有時(shí)間點(diǎn)。設(shè)和表示一個(gè)關(guān)于的有效的前一個(gè)和下一個(gè)運(yùn)動(dòng)基元，則是一個(gè)有效的規(guī)劃。對(duì)于每一個(gè)，讓表示被觀察到的原始的執(zhí)行，一個(gè)表示每個(gè)有效的三運(yùn)動(dòng)基元序列 。所有基元的執(zhí)行都具有相同的時(shí)間。請(qǐng)注意，這些時(shí)間點(diǎn)將不會(huì)被對(duì)齊，而數(shù)據(jù)點(diǎn)的數(shù)量可能在不同的執(zhí)行之間略有不同。模型學(xué)習(xí)步驟的說(shuō)明如圖3所示。

從恢復(fù)執(zhí)行基本的，我們假設(shè)一個(gè)具有對(duì)角協(xié)方差的非線性加性高斯噪聲回歸模型：

并在函數(shù)之前放一個(gè)高斯過(guò)程：

高斯過(guò)程[20]是一種在函數(shù)上的分布，在許多統(tǒng)計(jì)分析和回歸任務(wù)中都非常成功，例如運(yùn)動(dòng)模式識(shí)別[21]。它是一個(gè)貝葉斯非參數(shù)模型，適用于建模軌跡和基于軌跡的運(yùn)動(dòng)模式[22]。高斯過(guò)程由平均函數(shù)和協(xié)方差函數(shù)定義。通過(guò)將GP調(diào)節(jié)在觀察軌跡上，我們得到一個(gè)預(yù)測(cè)分布，對(duì)于每個(gè)時(shí)間點(diǎn)，

在所有狀態(tài)維度上

其中，使用一個(gè)零均值函數(shù)，因?yàn)槲覀兛梢院苋菀椎貜臄?shù)據(jù)中減去均值，

,，是一個(gè)恒等矩陣。因?yàn)槭且粋€(gè)標(biāo)量，那么是一個(gè)標(biāo)量，是一個(gè)列向量，是一個(gè)方陣。為了簡(jiǎn)單起見(jiàn)，每個(gè)輸出維度都被視為獨(dú)立的，這相當(dāng)于它們被一個(gè)單獨(dú)的函數(shù)建模，每個(gè)函數(shù)都有一個(gè)單獨(dú)的高斯過(guò)程先驗(yàn)。

使用(6)，現(xiàn)在可以在相同的時(shí)間間隔內(nèi)對(duì)齊所有的三聯(lián)基元執(zhí)行。這組執(zhí)行跨越了基元的多樣性，我們期望未來(lái)的正常執(zhí)行都與這個(gè)集合相似。

運(yùn)動(dòng)基元是通過(guò)由時(shí)間參數(shù)化的nD狀態(tài)空間的一維曲線。因此，我們認(rèn)為函數(shù)上的單峰分布（如單個(gè)GP）是一個(gè)合適的模型來(lái)表示[22]的執(zhí)行可變性。

我們將運(yùn)動(dòng)基元執(zhí)行模型表示為函數(shù)上的單峰分布：

其中，和由[22]給出。

這可以解釋為具有高斯分布噪聲的噪聲樣本的樣本均值和樣本方差。它是高斯過(guò)程混合物的高斯近似MoGPs。MoGPs是基于所有具有相同權(quán)重的GPs（每個(gè)三聯(lián)基元都被觀察到一次）。高斯近似允許MoGP在個(gè)別執(zhí)行之外進(jìn)行推廣。運(yùn)動(dòng)基元狀態(tài)執(zhí)行模型如圖3右邊的第三個(gè)圖所示，圖4中的三維位置以及運(yùn)動(dòng)基元狀態(tài)軌跡和觀察到的執(zhí)行。

圖4 學(xué)習(xí)了一個(gè)運(yùn)動(dòng)基元的單峰分布。圖中顯示了殘差w.r.t.減去運(yùn)動(dòng)基元的參考狀態(tài)軌跡，即。紅線是觀測(cè)結(jié)果的單個(gè)高斯過(guò)程。單峰分布由藍(lán)色區(qū)域定義，顯示其99%概率區(qū)域，藍(lán)線為其模態(tài)。

B.碰撞檢測(cè)

學(xué)習(xí)到的運(yùn)動(dòng)基元模型表示機(jī)器人在執(zhí)行運(yùn)動(dòng)基元時(shí)的狀態(tài)。在高斯過(guò)程噪聲假設(shè)下，其模態(tài)平均與運(yùn)動(dòng)原始參考狀態(tài)軌跡相同或更準(zhǔn)確。執(zhí)行可變性被明確地表示為狀態(tài)上隨時(shí)間變化的概率密度。

對(duì)于根據(jù)多元高斯分布分布的隨機(jī)變量，將概率為P的中心概率區(qū)域（PR）定義為[23]

是概率的具有個(gè)自由度的卡方分位數(shù)函數(shù)。對(duì)于一個(gè)對(duì)角線矩陣簡(jiǎn)化為一個(gè)軸對(duì)準(zhǔn)的橢球體，

每個(gè)軸的軸長(zhǎng)由給出。設(shè)是運(yùn)動(dòng)規(guī)劃(4)中的一個(gè)運(yùn)動(dòng)基元，對(duì)應(yīng)于運(yùn)動(dòng)基元和開(kāi)始時(shí)間 。設(shè) ，它限制在0和之間。運(yùn)動(dòng)基元的模型是一個(gè)維的多元分布，在每個(gè)時(shí)間點(diǎn)處具有對(duì)角協(xié)方差矩陣。安全裕度現(xiàn)在可以基于概率給定概率，

C.異常檢測(cè)

一旦我們學(xué)習(xí)了運(yùn)動(dòng)基元模型，我們就想使用它們來(lái)檢測(cè)運(yùn)動(dòng)基元的異常執(zhí)行。基元的執(zhí)行可變性模型上的概率區(qū)域描述了我們期望機(jī)器人在給定時(shí)間點(diǎn)處于的狀態(tài)。例如，[7]在執(zhí)行監(jiān)控中使用概率間隔來(lái)實(shí)現(xiàn)機(jī)器人的安全。

有幾種可能的方法來(lái)使用概率區(qū)域來(lái)定義什么是正常的行為。一種簡(jiǎn)單的方法是檢查機(jī)器人是否離開(kāi)該區(qū)域，如果離開(kāi)，將該執(zhí)行定義為異常。然而，由于我們選擇的區(qū)域不包含所有的概率，我們?nèi)匀黄谕麢C(jī)器人偶爾離開(kāi)概率區(qū)域。

更準(zhǔn)確地說(shuō)，如果我們選擇一個(gè)99%的PR，機(jī)器人預(yù)計(jì)將在1%的時(shí)間點(diǎn)上離開(kāi)這個(gè)間隔。如果機(jī)器人開(kāi)始更頻繁地離開(kāi)概率區(qū)域，這可能是一個(gè)錯(cuò)誤的指示。我們將離開(kāi)概率區(qū)域的速率定義為失敗率，并將其建模為一個(gè)隨機(jī)變量。我們將考慮以下參數(shù)：

我們將我們之前關(guān)于的假設(shè)編碼為一個(gè)分布，將模式設(shè)置為，例如同時(shí)仍然允許一些不確定性：

與和一起使用，其中為先驗(yàn)強(qiáng)度。

鑒于我們知道，觀察到的失敗的數(shù)量遵循一個(gè)二項(xiàng)分布：

因?yàn)樨愃植际嵌?xiàng)分布的一個(gè)共軛先驗(yàn)，的后驗(yàn)也是：

使用的后驗(yàn)值，有可能評(píng)估大于預(yù)期的概率：

其中在大多數(shù)統(tǒng)計(jì)庫(kù)中都是一個(gè)固定時(shí)間的標(biāo)準(zhǔn)函數(shù)，

這個(gè)故障率高于的概率，然后可以被監(jiān)測(cè)和閾值。在這項(xiàng)工作中，我們使用了的概率閾值和的先驗(yàn)強(qiáng)度，但這些值可以進(jìn)行微調(diào)，以選擇精度和召回率之間的權(quán)衡。

Ⅴ 實(shí)驗(yàn)和結(jié)果

為了評(píng)估我們提出的方法，我們考慮了一個(gè)模擬的DJI Matrice 100四軸飛行器（圖5），一個(gè)常用的商業(yè)研究平臺(tái)。

圖5 模擬了DJI Matrice 100四軸飛行器。

A.運(yùn)動(dòng)規(guī)劃和控制

使用了與[3]中相同的DJI Matrice 100中相同的非線性模型。我們使用一個(gè)基于[24]中的工作的非線性MPC控制器，并使用ACADO [25]來(lái)生成一個(gè)有效的實(shí)現(xiàn)來(lái)求解(5)。使用ACADO（圖2），使用與非線性MPC控制器相同的目標(biāo)，總共生成了104個(gè)運(yùn)動(dòng)基元。使得運(yùn)動(dòng)基元具有初始狀態(tài)和最終狀態(tài)：

運(yùn)行一組三聯(lián)基元執(zhí)行可能會(huì)導(dǎo)致一個(gè)三聯(lián)基元執(zhí)行干擾下一個(gè)執(zhí)行。為了避免瞬態(tài)噪聲在三聯(lián)體之間傳播，每個(gè)三聯(lián)基元規(guī)劃都在三聯(lián)基元之前和之后添加動(dòng)作，這樣整個(gè)規(guī)劃總是從靜止?fàn)顟B(tài)開(kāi)始。

B.學(xué)習(xí)

對(duì)于運(yùn)動(dòng)基元模型，采用具有平方指數(shù)協(xié)方差函數(shù)的高斯過(guò)程先驗(yàn)，

其中為每個(gè)輸入維具有長(zhǎng)度尺度的對(duì)角矩陣，為信號(hào)方差。這些，連同對(duì)角線噪聲協(xié)方差，是這個(gè)層次貝葉斯模型的超參數(shù)。

我們通過(guò)最大化邊際對(duì)數(shù)似然值（經(jīng)驗(yàn)貝葉斯）來(lái)從數(shù)據(jù)中估計(jì)超參數(shù)，

其中，被定義為（9-10）中，是一個(gè)常數(shù)。

我們研究了學(xué)習(xí)到的運(yùn)動(dòng)基元執(zhí)行模型的預(yù)測(cè)均值與觀測(cè)軌跡的接近程度，并將其與運(yùn)動(dòng)基元參考狀態(tài)軌跡與觀測(cè)軌跡的接近程度進(jìn)行了比較。在表I中，第一行顯示了觀察到的執(zhí)行和參考狀態(tài)軌跡或已執(zhí)行的運(yùn)動(dòng)基元之間的RMSE。

該表顯示了幾個(gè)均方根誤差（RMSE）。第1行：運(yùn)動(dòng)基元參考狀態(tài)軌跡與觀察到的同一運(yùn)動(dòng)基元執(zhí)行之間的誤差。第2行：運(yùn)動(dòng)基元執(zhí)行模型的平均預(yù)測(cè)與觀察到的同一運(yùn)動(dòng)基元執(zhí)行之間的誤差。第3行：原始參考軌跡與執(zhí)行該運(yùn)動(dòng)的模型的平均預(yù)測(cè)之間的誤差。

作為執(zhí)行軌跡的點(diǎn)預(yù)測(cè)，平均預(yù)測(cè)明顯比參考狀態(tài)軌跡更準(zhǔn)確，如下行所示。這樣做的原因是，與學(xué)習(xí)到的模型相比，單個(gè)基元的參考狀態(tài)軌跡對(duì)執(zhí)行軌跡（跨狀態(tài)，而不是時(shí)間）的平均值有更大的偏差。在圖4中可以看到一個(gè)示例。表一第三行比較了學(xué)習(xí)到的運(yùn)動(dòng)基元模型和基元的參考狀態(tài)軌跡的平均預(yù)測(cè)。表一第三行比較了學(xué)習(xí)到的運(yùn)動(dòng)基元模型和基元的參考狀態(tài)軌跡的平均預(yù)測(cè)。這提供了額外的證據(jù)，表明與運(yùn)動(dòng)原模型本身相比，學(xué)習(xí)運(yùn)動(dòng)原模型模型是一個(gè)系統(tǒng)地更好的執(zhí)行平均預(yù)測(cè)器。

C.碰撞檢查

文獻(xiàn)中的標(biāo)準(zhǔn)方法是使用一個(gè)球體，捕獲 ，,的聚合，單個(gè)半徑參數(shù)對(duì)時(shí)間不變。我們使用這個(gè)基線，并構(gòu)造了兩個(gè)更緊密的邊界，（每個(gè)基元不同）和（也隨著時(shí)間而變化）。為了與我們提出的變異性模型進(jìn)行有意義的比較，我們通過(guò)假設(shè)以原始參考軌跡為中心的觀測(cè)值具有高斯似然性來(lái)概率地建立基線。對(duì)于、中的，

其中對(duì)于各自的基線是：

平均歸一化面積（平均，相當(dāng)于），相對(duì)于進(jìn)行歸一化（對(duì)所有維度和所有運(yùn)動(dòng)基元使用單一的安全邊際半徑）。

將每個(gè)維度和IV-B中定義的分開(kāi)。安全裕度 ，給定的概率為

在表Ⅱ中，將基線與學(xué)習(xí)到的運(yùn)動(dòng)基元模型進(jìn)行了比較。是所有基元中最大的99%概率區(qū)域，每個(gè)維度，以參考軌跡為中心。是特定于單個(gè)基元的，這允許它更小。是時(shí)變的，允許在觀測(cè)結(jié)果允許的情況下進(jìn)一步減小半徑。

圖6顯示了一個(gè)比較一個(gè)運(yùn)動(dòng)基元的，和學(xué)習(xí)模型 的例子。圖7顯示了圖6在綠色垂直線上的解剖圖。為了仍然確保安全，例如，觀察到的執(zhí)行情況應(yīng)該仍然在以內(nèi)，區(qū)域必須更大，并有更大的偏差。

當(dāng)以平均值為中心時(shí)，對(duì)稱的安全裕度達(dá)到最小值。表Ⅱ顯示，與基線相比，使用學(xué)習(xí)到的變異性模型的面積減少較大。即使與時(shí)變半徑相比，當(dāng)使用學(xué)習(xí)模型時(shí)，面積也減少了3倍以上。

圖6 （淺灰色包絡(luò)）、（深灰色包絡(luò)）、（藍(lán)線和藍(lán)色包絡(luò)）和觀察到的單一運(yùn)動(dòng)原體i (c.f的執(zhí)行（紅色）的比較。圖4）。和與它們的平均值（參考狀態(tài)軌跡）具有相同的黑線。沒(méi)有顯示，以便使其他細(xì)節(jié)更精細(xì)的可見(jiàn)。

圖7 圖6在處的殘余圖（用垂直的綠線表示）。（黑點(diǎn)和深灰色包絡(luò)）和（藍(lán)點(diǎn)和藍(lán)色包絡(luò)）與觀察到的單一運(yùn)動(dòng)基元i的執(zhí)行（紅點(diǎn)）一起描述。和沒(méi)有顯示出來(lái)，以使其他元素的更精細(xì)的細(xì)節(jié)可見(jiàn)。

D.異常檢測(cè)

該實(shí)驗(yàn)共使用了104個(gè)運(yùn)動(dòng)基元。除了圖2中的26個(gè)基元外，其他基元的初始速度和/或最終速度均為非零。對(duì)于每個(gè)基元，執(zhí)行20個(gè)隨機(jī)但不同的有效三聯(lián)基元組合并進(jìn)行記錄。每個(gè)基元的前10個(gè)三聯(lián)基元組合用于學(xué)習(xí)每個(gè)基元的模型，即一個(gè)訓(xùn)練集。以下10個(gè)被用于檢測(cè)異常檢測(cè)，即測(cè)試集。

將異常行為定義為離開(kāi)概率區(qū)域的樸素方法與所提出的使用后驗(yàn)的方法進(jìn)行了比較，使用。

對(duì)于方法，先驗(yàn)參數(shù)被設(shè)置為和。后驗(yàn)值的計(jì)算包括了最后一秒的觀測(cè)結(jié)果，這意味著我們?cè)O(shè)置了時(shí)間窗秒。對(duì)于每個(gè)時(shí)間點(diǎn)，我們計(jì)算故障率大于的概率。如果在任何時(shí)間點(diǎn)的概率大于的閾值，則整個(gè)執(zhí)行被歸類為異常，否則為正常。

兩種方法都使用從訓(xùn)練集學(xué)習(xí)到的運(yùn)動(dòng)基元模型給出的概率區(qū)域，然后在測(cè)試集上進(jìn)行測(cè)試。圖8顯示了每種方法和數(shù)據(jù)集組合的混淆矩陣。方法的性能略優(yōu)于樸素方法，同時(shí)仍然保持著完美的精度。這表明，我們可以通過(guò)調(diào)整的閾值和時(shí)間窗的大小來(lái)提高召回率，而不會(huì)損失精度。

圖8 使用中心99.9%-probability體積的所有基元的異常檢測(cè)的混淆矩陣。要求所有的觀察結(jié)果都在這個(gè)體積內(nèi)，或者使用后驗(yàn)，要求在1s窗口內(nèi)個(gè)體異常小于99.9%的比例可能高于0.01%。第一行：在模型訓(xùn)練集上的性能。第二行：對(duì)以前看不見(jiàn)的執(zhí)行的性能，來(lái)自相同的基元，但不同的三聯(lián)基元組合。

Ⅵ 結(jié)論和未來(lái)工作

在本文中，我們提出了一種用于自動(dòng)駕駛的在線概率、多模態(tài)、多目標(biāo)跟蹤算法。我們的模型學(xué)習(xí)融合2D相機(jī)圖像和3D激光雷達(dá)點(diǎn)云特征。然后，這些融合的特征被用于學(xué)習(xí)有效的權(quán)重，以將深度特征距離與Mahalanobis距離相結(jié)合，從而獲得更好的數(shù)據(jù)關(guān)聯(lián)。我們的模型還學(xué)習(xí)以數(shù)據(jù)驅(qū)動(dòng)的方式管理跟蹤循環(huán)周期。我們?cè)贜uScenes [9]和KITTI [10]數(shù)據(jù)集上驗(yàn)證了我們提出的方法，我們的方法在定量和定性上都優(yōu)于使用相同目標(biāo)檢測(cè)器的最先進(jìn)的基準(zhǔn)方法。

參考文獻(xiàn)