編者按：自動(dòng)駕駛車(chē)輛的路徑規(guī)劃問(wèn)題存在著諸多的解決方法，包括主要應(yīng)用在全局規(guī)劃的的A*、RRT快速搜索算法和遺傳搜索算法等，以及主要應(yīng)用在局部規(guī)劃的DWA算法和APF算法等。路徑規(guī)劃的最終目標(biāo)是在短時(shí)間內(nèi)生成一條符合車(chē)輛/機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)要求的最佳路徑，確保被控對(duì)象在準(zhǔn)確快速的到達(dá)指定目標(biāo)位置的同時(shí)，盡可能地規(guī)避全局路徑上新出現(xiàn)地障礙物。近年來(lái)，隨著無(wú)人駕駛技術(shù)的不斷發(fā)展，結(jié)合了車(chē)輛運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型的路徑規(guī)劃算法也相繼出現(xiàn)，該類(lèi)型的解決方案以傳統(tǒng)規(guī)劃為基礎(chǔ)，在路徑生成的決策規(guī)劃層通過(guò)引入車(chē)輛自身的約束條件，保證得到的實(shí)際路徑的可執(zhí)行性。

本文譯自：

《A LATERAL MOTION PLANNING METHOD FOR AUTOMATED VEHICLES baseD ON SINUSOIDS》

文章來(lái)源：

Dynamic Systems and Control Conference

作者：

Wei Wang，Zejiang Wang，Xinbo Chen，Junmin Wang

原文鏈接：

http://asmedigitalcollection.asme.org

摘要：對(duì)于自動(dòng)駕駛應(yīng)用，特別是在復(fù)雜環(huán)境中，精確的運(yùn)動(dòng)規(guī)劃對(duì)避障和當(dāng)?shù)伛{駛至關(guān)重要。從微分幾何的角度處理這個(gè)問(wèn)題有助于簡(jiǎn)化控制和規(guī)劃過(guò)程。與經(jīng)典的微分幾何方法只能得到前后運(yùn)動(dòng)的分段解不同，本文探討了正弦曲線在連續(xù)速度要求和系統(tǒng)非完整約束下的精確運(yùn)動(dòng)規(guī)劃中的應(yīng)用。通過(guò)將車(chē)輛運(yùn)動(dòng)學(xué)模型轉(zhuǎn)換為類(lèi)似的鏈?zhǔn)侥Ｐ?，我們?yīng)用帶偏差的正弦輸入來(lái)生成參考軌跡，并討論了給定橫向位移的參數(shù)選擇原則。為了克服洛必達(dá)規(guī)則帶來(lái)的額外約束，我們引入了一對(duì)進(jìn)入/離開(kāi)過(guò)渡期，以確保可調(diào)的速度剖面和設(shè)計(jì)靈活性。由此產(chǎn)生的軌跡反映了對(duì)變道或躲避轉(zhuǎn)向場(chǎng)景的強(qiáng)適用性。由于所提出的方法優(yōu)于正/連續(xù)速度約束而不考慮小時(shí)間可控性，因此進(jìn)一步研究了可達(dá)性問(wèn)題。給出了可達(dá)區(qū)域邊緣的解析估計(jì)，并與數(shù)值模擬結(jié)果進(jìn)行了比較。最后，室內(nèi)現(xiàn)場(chǎng)試驗(yàn)驗(yàn)證了該方法的可行性。

關(guān)鍵詞：運(yùn)動(dòng)規(guī)劃，自動(dòng)化車(chē)輛

1 引言

無(wú)碰撞運(yùn)動(dòng)規(guī)劃(MP，Motion Planning)是機(jī)器人技術(shù)的核心問(wèn)題之一，有著悠久的歷史。隨著路線圖方法的發(fā)展和對(duì)經(jīng)典的“鋼琴搬運(yùn)工問(wèn)題”的研究，在幾何環(huán)境中的MP問(wèn)題被充分理解并解決了。然而，對(duì)于類(lèi)車(chē)機(jī)器人或車(chē)輛系統(tǒng)的開(kāi)環(huán)MP問(wèn)題，應(yīng)在規(guī)劃算法的同時(shí)考慮系統(tǒng)的非完整性，特別是在狹窄空間和復(fù)雜避障場(chǎng)景下，如傳統(tǒng)完整方法無(wú)法解決的停車(chē)問(wèn)題[1]。事實(shí)上，直接找到一個(gè)曲率上界車(chē)輛系統(tǒng)的可行路徑并非易事。Fortune和Wolfgang證明了約束路徑的決定性問(wèn)題在EXPTIME(確定性圖靈機(jī)可在指數(shù)時(shí)間內(nèi)解決的所有決策問(wèn)題的集合)[2]上。目前解決系統(tǒng)非完整性的方法主要是基于抽樣方法和碰撞檢查等。使用一組固定機(jī)動(dòng)(離散集合)[3]生成一個(gè)格圖，它不僅表示位置，而且表示其他狀態(tài)?？焖偬剿麟S機(jī)樹(shù)(RRT)方法是一種基于系統(tǒng)微分方程和隨機(jī)選擇輸入的樹(shù)狀結(jié)構(gòu)展開(kāi)方法，適合于解決系統(tǒng)非完整性和高自由度(包括Kino-dynamic)問(wèn)題。 在基于采樣的路線圖構(gòu)建過(guò)程中，設(shè)計(jì)一種精確的轉(zhuǎn)向方法，將車(chē)輛從初始配置引導(dǎo)到目標(biāo)配置是非完整車(chē)輛系統(tǒng)的一個(gè)關(guān)鍵問(wèn)題。文獻(xiàn)中提出了幾種轉(zhuǎn)向方法。對(duì)于對(duì)稱(chēng)可控系統(tǒng)，轉(zhuǎn)向方法的設(shè)計(jì)通常采用微分幾何的方法。Lafferriere和Sussmann通過(guò)從P. Hall族中選擇控制李氏代數(shù)的一組基，提出了冪零或可零系統(tǒng)的一般策略。對(duì)于一個(gè)類(lèi)車(chē)機(jī)器人MP，得到的精確路徑包括26塊[4]。通過(guò)將系統(tǒng)轉(zhuǎn)換為鏈?zhǔn)叫问?，Murray和Sastry提出了多級(jí)正弦輸入控制鏈?zhǔn)较到y(tǒng)的應(yīng)用。產(chǎn)生的路徑仍然是離散的[5]。在[6]中，利用微分平面度特性將問(wèn)題簡(jiǎn)化為平面C2曲線設(shè)計(jì)，建立了路徑曲率與轉(zhuǎn)向機(jī)動(dòng)之間的關(guān)系。精確的MP問(wèn)題也可以用最優(yōu)控制理論來(lái)處理，其中最短長(zhǎng)度問(wèn)題是研究最多的。盡管大多數(shù)最優(yōu)路徑的求解依賴(lài)于數(shù)值方法例如直接或間接射擊等，但對(duì)于簡(jiǎn)單的車(chē)輛運(yùn)動(dòng)學(xué)，經(jīng)典結(jié)果如杜賓路徑[7]和RS路徑[8]已通過(guò)龐特里亞金極大值原理[9]被證明是長(zhǎng)度最優(yōu)的。此外，非完整轉(zhuǎn)向方法的拓?fù)湫再|(zhì)決定了遵循完整路徑的可行性。通過(guò)將完整路徑離散到足夠小，考慮非完整無(wú)碰撞路徑的轉(zhuǎn)向方法，可以到達(dá)附近的每一個(gè)構(gòu)型[10,11]。這個(gè)特性是基于系統(tǒng)是可控的假設(shè)。 在更多細(xì)節(jié)和需求被考慮后，大多數(shù)為類(lèi)車(chē)機(jī)器人設(shè)計(jì)的MP方法都適用于自動(dòng)駕駛汽車(chē)[12-14]。由于路徑變化和轉(zhuǎn)向機(jī)動(dòng)之間的差異已經(jīng)被很好地理解，在幾何上用不同的曲線(如線和圓、回旋線和多項(xiàng)式)插值已知的路徑點(diǎn)是自然和直接的。但其明顯的缺點(diǎn)是：路徑曲率變化復(fù)雜，與典型轉(zhuǎn)向機(jī)動(dòng)不對(duì)應(yīng)，給在線執(zhí)行帶來(lái)了困難。此外，在解決連接條件和曲率限制等設(shè)計(jì)約束時(shí)，采用4小時(shí)或更高階插值曲線，使得路徑點(diǎn)方法計(jì)算量十分大。為了簡(jiǎn)化精確轉(zhuǎn)向方法和MP過(guò)程中的機(jī)動(dòng)，我們從微分幾何角度來(lái)處理自動(dòng)車(chē)輛MP，并應(yīng)用帶偏差的正弦輸入來(lái)避免離散/尖點(diǎn)機(jī)動(dòng)(對(duì)應(yīng)于倒車(chē)換擋)。該方法對(duì)變道和躲避轉(zhuǎn)向具有較好的適用性。然而，由于正/連續(xù)速度約束，這種方法不再考慮局部可控性。因此，我們也進(jìn)行了可達(dá)性分析。本文的其余部分組織如下。第二節(jié)介紹了基于鏈?zhǔn)杰?chē)輛系統(tǒng)和正弦輸入的MP方法，以及參數(shù)選擇原則。對(duì)于橫向運(yùn)動(dòng)機(jī)動(dòng)，第三節(jié)研究了該方法的可達(dá)性問(wèn)題。在第四節(jié)中，我們對(duì)基于規(guī)模汽車(chē)平臺(tái)的規(guī)劃算法進(jìn)行了測(cè)試，驗(yàn)證了所提方法的可行性。第五節(jié)對(duì)這項(xiàng)工作做出總結(jié)。

2 橫向運(yùn)動(dòng)規(guī)劃法

2.1 車(chē)輛運(yùn)動(dòng)學(xué)模型

實(shí)際地面車(chē)輛的運(yùn)動(dòng)約束通常是基于輪胎防滑假設(shè)來(lái)考慮的，這在正常駕駛條件下大致正確。在此假設(shè)下，自行車(chē)模型(圖1)被廣泛應(yīng)用于車(chē)輛系統(tǒng)的MP和動(dòng)態(tài)控制中，其中具有以下非完整約束。

當(dāng)我們將運(yùn)動(dòng)學(xué)關(guān)系轉(zhuǎn)化為具有指定輸入的微分方程時(shí)，約束仍然存在。從駕駛員的角度來(lái)看，汽車(chē)系統(tǒng)有兩個(gè)輸入，占兩個(gè)自由度，即加速器(油門(mén))和方向盤(pán)。為簡(jiǎn)單起見(jiàn)，我們將后軸的中點(diǎn)作為參考點(diǎn)。在不喪失一般性的前提下，我們假設(shè)后軸和前軸l之間的距離為1。我們用v表示前輪的平均速度。前輪方向和車(chē)輛方向之間的角度用φ表示，φ完全由駕駛員的輸入或執(zhí)行器控制。在指定的全局參考框架下，我們用θ定義車(chē)輛方向。簡(jiǎn)單的論證將我們引向以下控制系統(tǒng)，其中

和

分別代表油門(mén)和轉(zhuǎn)向輸入。

由于我們所關(guān)心的是車(chē)輛系統(tǒng)的運(yùn)動(dòng)學(xué)，前輪的速度與運(yùn)動(dòng)學(xué)模型和MP問(wèn)題無(wú)關(guān)。此外，對(duì)于前轉(zhuǎn)向車(chē)輛，后輪總是與汽車(chē)方向?qū)R。因此，選擇車(chē)頭方向的車(chē)速作為控制輸入比選擇前輪速度更好。前輪速度的另一個(gè)因素允許車(chē)輛以上界曲率1為參考點(diǎn)定向。以上的討論可以簡(jiǎn)化如下：

其中u1=vcosφ成為新的速度輸入。注意，通過(guò)考慮由機(jī)械結(jié)構(gòu)決定的系統(tǒng)動(dòng)力學(xué)，我們總是可以從輸入u1(t)和u2(t)轉(zhuǎn)移到真正的加速器和方向盤(pán)輸入。在運(yùn)動(dòng)學(xué)方面，系統(tǒng)(3)就是類(lèi)車(chē)機(jī)器人的運(yùn)動(dòng)學(xué)模型。參考經(jīng)典研究結(jié)果，非完整系統(tǒng)(3)在前輪轉(zhuǎn)向角物理上為[6]的情況下，仍然是小時(shí)間可控的。然而，在這項(xiàng)工作中對(duì)于平滑MP問(wèn)題的討論，要求速度控制是積極的，因?yàn)槿绻緳C(jī)需要停止和換到倒擋，就不會(huì)有一個(gè)平穩(wěn)的機(jī)動(dòng)。有了這個(gè)要求，系統(tǒng)(3)就不再是局部可控的了。

圖1 自行車(chē)運(yùn)動(dòng)學(xué)模型

基于系統(tǒng)(3)和相應(yīng)的輸入約束，我們將MP問(wèn)題描述為尋找一種開(kāi)環(huán)轉(zhuǎn)向方法，通過(guò)連續(xù)控制將車(chē)輛狀態(tài)從初始配置X0精確或近似地轉(zhuǎn)向目標(biāo)X1。利用正弦曲線的良好特性，我們可以解決正速度約束并獲得平穩(wěn)機(jī)動(dòng)。首先，我們引入系統(tǒng)(3)的鏈?zhǔn)叫问饺缦?

我們應(yīng)該注意到，最后一個(gè)方程是基于這樣的假設(shè)：在計(jì)劃過(guò)程中sin(θ)>0，狀態(tài)θ位于區(qū)間[0，π]內(nèi)。當(dāng)sin(θ)=0時(shí)，

是隱式確定的。

2.2側(cè)向運(yùn)動(dòng)的一步的非完整過(guò)程

在介紹一步過(guò)程之前，我們首先介紹了基于正弦信號(hào)輸入的多級(jí)轉(zhuǎn)向過(guò)程的一些經(jīng)典結(jié)果。標(biāo)準(zhǔn)雙鏈系統(tǒng)是一個(gè)雙輸入系統(tǒng)，如下所示