廣告
結構化道路智能車輛的時域可預測軌跡規(guī)劃和分層運動控制
2019-09-18 06:40:45· 來源：同濟智能汽車研究所
編者按：基于增量采樣的快速擴展隨機樹（RRT），能很好處理考慮車輛kinodynamics約束的規(guī)劃問題。然而基礎RRT算法存在采樣效率低、生成軌跡抖動等等問題。運動控
編者按：基于增量采樣的快速擴展隨機樹（RRT），能很好處理考慮車輛kinodynamics約束的規(guī)劃問題。然而基礎RRT算法存在采樣效率低、生成軌跡抖動等等問題。運動控制是自動駕駛車輛另外一個重要技術，被廣泛應用的以PID 控制為基礎的算法認為車輛相對于軌跡的運動學關系假設為線性的，在動力學上，缺少對車輛橫擺角速度上限和對底層執(zhí)行器飽和約束的考慮?；谏鲜龃嬖诘膯栴}，本文基于優(yōu)化RRT的路徑規(guī)劃方法和解耦的速度規(guī)劃方法，結合閉環(huán)仿真的預測，將路徑-速度解耦的軌跡實現(xiàn)時間-空間的配準，準確預估實際跟蹤軌跡。
本文譯自：
IEEE IV 2019
原文標題：
" Predictable Trajectory Planner in Time-domain and Hierarchical Motion Controller for Intelligent Vehicles in Structured Road "
原作者為來自同濟智能汽車研究所的熊璐教授、李志強碩士等。
摘要：路徑規(guī)劃及其跟蹤作為智能車輛的基本模塊，近來得以迅速發(fā)展。但路徑-速度解耦的規(guī)劃方法生成的路徑在時域內并不可行。在本文中，提出了一種基于改進RRT的路徑規(guī)劃方法，實現(xiàn)高效規(guī)劃與路徑平滑。文章采用閉環(huán)預測跟蹤路徑的方法，實現(xiàn)軌跡點在空間與時間上的匹配，更準確地得知控制系統(tǒng)跟蹤路徑。為適應車輛橫/縱向動力學的非線性約束和執(zhí)行器飽和，文章設計了統(tǒng)一的條件積分控制律，該方法可以保證跟蹤誤差全局漸近穩(wěn)定，同時避免因為控制量約束使積分運算發(fā)散所造成的控制器性能退化。經仿真和試驗證明，本文提出的規(guī)劃方法更為高效，控制算法能夠有效跟蹤規(guī)劃軌跡，預測方法能準確預測實際軌跡，這對碰撞檢測非常重要。
1、前言
運動規(guī)劃模塊是自動駕駛車輛的基本技術之一，同樣受到重視而發(fā)展迅速。運動規(guī)劃問題最早在移動機器人領域提出和研究，其中一些著名的規(guī)劃方法包括Dijkstra、A*和它們的變形（D*，混合A*等）[1-4]，基于網(wǎng)格化的規(guī)劃環(huán)境搜索路徑，不能容易地考慮車輛復雜的動力學和微分約束。
相比之下，基于增量采樣的快速擴展隨機樹[5]（RRT），則更能應對這種考慮車輛動態(tài)約束的規(guī)劃問題?；ARRT算法同樣存在采樣效率低、生成軌跡抖動等不足。為提高規(guī)劃效率，雙向RRT[6]和RRT-connect[7]相繼被提出。目標偏向等加速手段同樣被應用。為適應特殊場景，基于障礙物邊界的RRT算法[8]和關注狹窄通道的算法[9]相繼提出?？紤]車輛的非完整約束，文獻[10]在構造樹的過程中考慮擴展的相鄰邊夾角符合車輛約束。為減少RRT軌跡的抖動，文獻[9]設計了后處理方法，對成功子樹進行修剪。獲得成功子樹后，采用各類曲線平滑，如Dubins路徑、回旋曲線和貝賽爾曲線等[11]。
通過這種方式，速度規(guī)劃器實現(xiàn)路徑-速度解耦[12-13]，也就是說——在生成軌跡之后，在每個軌跡點上規(guī)劃速度。這種方式先在空間域內得到符合曲率約束的軌跡形狀，并滿足如橫向加速度等動態(tài)約束。然而，它的主要缺點在于，由于速度的變化，軌跡點間時間差不固定。而實際上，車輛的控制器和執(zhí)行器有固定的計算頻率，控制動作間時間差固定。顯然地，上述規(guī)劃方法獲得的軌跡在現(xiàn)實中是不可行的。
在軌跡預測方面，MIT團隊[14-15]在參加DARPA比賽時應用閉環(huán)軌跡預測的方法平滑路徑形狀，但是其速度規(guī)劃策略不適用于對起/終點速度有約束的局部路徑規(guī)劃場景。后續(xù)的工作[16-17]多用此方法消除定位導致的位置或航向偏差，而忽略了對軌跡跟蹤預測的作用。
運動控制是自動駕駛車輛另外一個重要技術?；谲囕v動力學模型的閉環(huán)反饋控制算法,根據(jù)車輛相對路徑的側向位移誤差,對其進行閉環(huán)反饋控制，其中PID [18]控制被應用得較為廣泛。Chaib S [19]通過PID和H∞的控制方法相結合對車輛側向運動進行控制，有較好的魯棒性。在一般PID控制基礎上，Marino R [20]設計了嵌套PID控制，對側向位移誤差和橫擺角速度分層進行反饋控制，并對系統(tǒng)的穩(wěn)定性進行了分析。總的來說,這些控制算法認為車輛相對于軌跡的運動學關系假設為線性的，在動力學上，缺少對車輛橫擺角速度上限和對底層執(zhí)行器飽和約束的考慮，會導致車輛失穩(wěn)[21]。
基于上述問題，提出了一種基于優(yōu)化RRT的路徑規(guī)劃方法和解耦的速度規(guī)劃方法來進行軌跡平滑。在獲得軌跡之后，結合閉環(huán)仿真的預測，實現(xiàn)時間-空間的配準，準確預估實際跟蹤軌跡。基于縱側解耦的運動控制框架，分別設計了側向運動的轉向控制律和縱向運動的驅/制動扭矩/壓力控制律，并統(tǒng)一使用條件積分控制律。本文剩下部分組織如下：第二節(jié)主要介紹基于優(yōu)化RRT的軌跡規(guī)劃和解耦的速度規(guī)劃，并利用閉環(huán)預測方法獲取可行的路徑。第三節(jié)介紹了用于規(guī)劃路徑的跟蹤的統(tǒng)一控制算法。第四節(jié)分析仿真和試驗結果，第五節(jié)是結論。
2、軌跡規(guī)劃與預測
由于基礎的RRT軌跡計算方法存在前述的問題，我們依據(jù)結構化道路的特點優(yōu)化基礎的RRT算法（稱為PB-RRT，prunedbias-sampled RRT）。選擇帶有偏向性的采樣方法來加速擴展和適應動態(tài)需求，例如適用于車輛的最臨近點判斷策略和后處理方法。然后可以規(guī)劃每一個軌跡點滿足車輛動力學約束的速度。由于位置和速度是已知的，因此可以預測智能車輛如何跟蹤規(guī)劃的軌跡?？偟囊?guī)劃和預測過程如下所示：
A. 采樣方法
在結構化道路上，在整個空間內完全隨機采樣會浪費大量時間。選擇偏向目標的高斯采樣以提高效率。每個采樣點生成為
其中(x0,y0)表示采樣參考點的坐標，(sx,sy)表示采樣點坐標，(r0,θ0)是相對于(x0,y0)的偏置參數(shù)，即高斯分布的均值，(σr,σθ)是徑向和圓周方向的高斯分布標準差，(rrand,θrand)是符合標準高斯分布的隨機變量。在這種情況下，采樣點將在環(huán)形或扇形區(qū)域中隨機生成，如圖1所示。
圖1 高斯采樣區(qū)域
圖2 結構化道路上不同場景的采樣區(qū)域
根據(jù)駕駛模式和目標點位置，可以設置合適的(r0, θ0)和(σr, σθ)，引導搜索向著目標點的方向進行。換道工況下，將采樣區(qū)域固定在一個扇形區(qū)域內，如圖2所示，該區(qū)域的中心線是以起點和終點為端點的線段。相應的，在轉向工況，根據(jù)目標點位置和車道中心線分別構造兩個偏向采樣區(qū)域。兼顧采樣效率和概率完備性，在整個空間內完全隨機采樣并以較小頻率選擇目標點。
B. 最臨近點搜索策略
如何選擇最鄰近點的方式對隨機樹的形狀和生長方向很重要，這決定了路徑的長度和抖動程度。傳統(tǒng)的RRT方法直接計算節(jié)點之間的歐幾里德距離。在針對車輛進行規(guī)劃時，應考慮節(jié)點間的夾角關系，使得軌跡趨于平滑。由于距離和角度的量綱不同，使用簡單的線性歸一化方法對兩個變量進行處理。
其中，N1(•) and N2(•)分別是距離和角度的歸一化方法。
C. 考慮最大曲率約束的剪枝方法
RRT采樣過程具有隨機性，直接利用采樣點獲得的路徑進行平滑會導致生成的軌跡非常曲折。因此有必要對有效路徑子樹基于最大曲率約束進行處理，刪除不必要的節(jié)點、插入必要的節(jié)點。
如圖3所示，從起始節(jié)點開始依次連接后續(xù)的路徑節(jié)點，如果連線與空間內障礙物無交集，那么它們之間的路徑點刪去，將兩節(jié)點直接相連，依次類推；否則，將保留初始節(jié)點和最后一個節(jié)點的父節(jié)點之間的線路，并將最后一個節(jié)點設置為下一個起始節(jié)點。繼續(xù)執(zhí)行上述過程，直到達到目標節(jié)點。在修剪后獲得新的子樹時，為了滿足曲率最大約束，可能需要將另一個無碰撞節(jié)點插入當前子樹以確保相鄰邊緣之間的每個角度不小于αmin。
圖3 成功子樹的Prunimg算法
在獲得修剪后的子樹后，所有節(jié)點及其中點都用作控制點，用于構建B樣條曲線以平滑路徑。本文選擇了B樣條，因為它的最大路徑曲率很容易根據(jù)約束進行限制。相鄰頂點的長度L和它們之間的角度α需要滿足如下條件[22]：
假設兩條邊的長度不相等，則較小的長度將被視為L。
D. 碰撞檢測
根據(jù)環(huán)境地圖和車輛結構的碰撞檢測方法被廣泛使用。如圖4所示，車輛結構被構造成幾個圓圈以檢測在網(wǎng)格圖中是否將發(fā)生碰撞。尺寸參數(shù)設計為
每個圓圈占用的每個網(wǎng)格都需要進行安全性判斷。只要有一個不安全的圓圈，這個采樣點就不安全。
E. 速度規(guī)劃
根據(jù)解耦軌跡-速度規(guī)劃思想，在生成與靜態(tài)障礙物無碰撞的平滑路徑后，可以根據(jù)動態(tài)障礙物的動態(tài)約束和避障要求來規(guī)劃速度[12-13]。由于已經獲得路徑，因此可以根據(jù)它們的位置計算從起點開始的每個路徑點的長度s。另外，還可以計算航向角和曲率κ，并用s表示為自變量。類似地，其他參數(shù)也可以設計為s作為自變量，例如速度v，加速度a和橫擺率ω。
速度規(guī)劃需要考慮邊界條件和動態(tài)約束。決策模塊給出的邊界條件是初始位置的速度vi，初始位置的加速度ai和目標位置的速度vg。速度限制的動態(tài)約束主要包括：
對于路徑上的每個點，滿足上述約束的允許最大速度可以計算為：
其中，Δv為安全閾值。此外加速度也應限制在[amin，amax]內。因此，我們的方法是基于具有最大曲率（絕對值）和速度邊界的點處的允許速度來設計加速度曲線。然后，每個點的速度可以計算為。在本文中，首先確定曲率峰值處的最大速度（絕對值）。如果起始位置和目標位置的速度較小，則速度不會改變。否則，規(guī)劃減速或加速線性變化的加速度（以s作為自變量）。例如，我們在26m的縱向距離內構建了車道變換的情景，其中橫向距離為3.5m，vi=40km / h，vg=36km / h，ai =0。規(guī)劃的軌跡和曲率、速度曲線如圖5所示。
圖5 基于路徑-速度解耦方法的速度規(guī)劃實例
由于已經確定了路徑長度和速度，因此還可以計算路徑點之間的時間間隔。這種解耦的規(guī)劃方法清晰且易于計算。但是，也存在明顯的缺點。在改變速度的情況下，路徑點之間的時間間隔是不確定的和不穩(wěn)定的，這與實際情況不一致。實際上，當智能車輛跟蹤規(guī)劃路徑時，控制模塊的執(zhí)行器具有固定的計算頻率（通常為10Hz-50Hz）。這會導致基于空間約束的軌跡與存在時域約束的控制跟蹤的失配。因此，存儲在路徑點中的大量信息（航向，曲率，速度等）將被忽略，因為在跟蹤規(guī)劃軌跡時控制器會跳過大量的點。實際行駛的軌跡與規(guī)劃的理想軌跡將有較大偏差。
F. 閉環(huán)預測軌跡
針對上述問題，本文采用閉環(huán)預測的方法解決這種失配。閉環(huán)預測的子系統(tǒng)由車輛模型和底層控制器組成，如圖6所示。獲取運動規(guī)劃的路徑之后，選擇某一路徑點作為控制器輸入，控制器結合車輛當前狀態(tài)計算控制輸入，車輛模型用控制輸入估計下一時刻車輛狀態(tài)。出于計算簡便性速度的需求，本文選擇車輛運動學模型、Stanley轉向控制器和PI速度控制器，這些在閉環(huán)預測相關工作中廣泛應用[23-24]。
車輛運動學自行車模型被描述為
其中，x和y指后軸位置，是車輛航向角，v是車輛的前行速度，δ是前輪轉向角，L是車輛軸距。Stanley方法選取距離前軸中心最近點跟蹤，轉向和速度控制為
其中θe是當前車輛航向和跟蹤點航向間的誤差，vcur和vcmd分別指車輛當前實際速度和期望速度。ks，kp和ki是增益參數(shù)。
圖6 閉環(huán)預測流程圖
圖7 魯棒分層運動控制器
從起點開始，不斷向前模擬更新車輛狀態(tài)，直至到達路徑終點，生成可跟蹤的路徑和控制序列。由于所用的控制器執(zhí)行頻率為50Hz，設置向前仿真的時間間隔為0.02s。
3、分層運動控制器
為了實現(xiàn)智能車輛的規(guī)劃軌跡的執(zhí)行，我們設計了一個魯棒的分層運動控制器，負責產生期望的驅動扭矩/制動壓力和轉向角。如圖7所示，我們的分層運動控制器由運動控制器（頂層）和動態(tài)控制器（底層）組成。由于在跟蹤智能車輛的規(guī)劃軌跡時路徑跟蹤比速度控制更重要，因此選擇縱向和橫向解耦車輛模型來減弱縱向運動的影響。由于頂層采用解耦橫向和縱向運動控制算法，底層由縱向動力學控制器和橫向動力學控制器組成，其中前者產生驅動電機系統(tǒng)和電液制動系統(tǒng)的指令，后者產生電動轉向系統(tǒng)的指令。所有控制器都采用統(tǒng)一的條件積分算法，其穩(wěn)定性可以通過Lyapunov穩(wěn)定性理論驗證[25]。
A. 運動車輛解耦模型
A. 運動車輛解耦模型
如圖8所示，X-E0-Y是大地坐標系，而x-Eb-y是車輛坐標系。當跟蹤參考軌跡時，選擇某個點作為跟蹤點（P0），并且Pk是軌跡上的跟蹤點。跟蹤點的速度（vd）方向與參考軌跡相切。車輛坐標系中的跟蹤點的坐標是（ls，lb）。根據(jù)車輛的運動學關系，可以將車輛運動學的狀態(tài)方程描述為
其中，β是車輛側滑角，φ是履帶點的航向角，vx和vy是縱向和橫向速度。
圖8 軌跡跟蹤模型原理圖
圖9 解耦運動學模型原理圖
如圖9所示，假設車輛的初始縱向位移誤差為零，只要P軸在x軸方向上的速度總是等于vkx，則x軸的速度為P0，其中可以確?？v向位移誤差xL_steady在任何縱向速度和P0的要求速度下都可以為零。
以這種方式，實現(xiàn)了在任何期望速度下的車輛軌跡跟蹤，并且可以解耦縱向和橫向運動控制。解耦的橫向運動學表達式可以描述為
B.車輛橫向動力學模型
如在智能車輛控制領域中普遍使用的那樣，動態(tài)單車車輛模型包括力平衡方程和力矩平衡方程
其中，kf和kr是單個前輪或后輪的剛度，lf和lr分別是從車輛質心到前輪軸和后輪軸的距離。
圖10 橫向動態(tài)自行車車輛模型
這兩個方程可以寫成狀態(tài)空間方程
C.縱向動力學模型
根據(jù)圖11的動力學模型圖，可以得到驅動軸的縱向動力學方程
其中，JRi是轉動慣量，wRi是車輪轉速，TRi是驅動轉矩，F(xiàn)x是縱向摩擦阻力，R是車輪半徑。系統(tǒng)的輸入是由執(zhí)行器（驅動電機或電液制動系統(tǒng)）提供的正向或反向扭矩。單個輪胎的縱向力可以通過基于magic公式的輪胎縱向力的公式表示：
在行駛或制動期間，車輪的滑移率λ可以計算為
D.統(tǒng)一條件集成控制器
總的來說，上述橫向和縱向系統(tǒng)可表示為
圖11 縱向二自由度單輪模型
其中，x是系統(tǒng)狀態(tài)，u是系統(tǒng)輸入命令，y是系統(tǒng)輸出，b是系統(tǒng)參數(shù)，t0是初始時間，x0是狀態(tài)變量的初始值，f（t，x）是非線性連續(xù)系統(tǒng)函數(shù)。由于可以以相同的形式描述不同的系統(tǒng)，因此設計了統(tǒng)一的控制算法以應用于每個系統(tǒng)，其也可以滿足致動器約束。
在（t0，x0）的某個鄰域中，對于所有（t，x），（t，y）和f（t，x），可以滿足不等式，如下所示：
其中，Lf是Lipschitz常數(shù)[26]。
狀態(tài)的跟蹤誤差可以定義為：
誤差控制系統(tǒng)是
輸入u的控制規(guī)則設計如下：
其中，sat（•）是飽和函數(shù)，sgn（•）是符號函數(shù)：
4、仿真和實驗結果
為了驗證上述規(guī)劃和控制算法的有效性，分別設計了仿真和實驗測試。在仿真中，將PB-RRT算法與基本RRT算法進行了比較，解釋了第二節(jié)中各步驟的優(yōu)化過程，驗證了算法的優(yōu)越性。然后，我們在交叉和車道變換等城市場景下，基于改進的電動智能車輛平臺進行了多次實驗，驗證了算法的實用性。
A.仿真結果
仿真在連續(xù)的車道變換場景中進行，以避免靜態(tài)障礙。兩個車道的寬度為7米，該區(qū)域的長度為50米，如圖12所示。
紅色矩形表示車輛的初始位置和最后位置，綠色點是RRT延伸過程的起點和目標點。紅色折線是搜索的成功子樹，藍色折線是修剪的子樹，綠色曲線是具有B樣條曲線的平滑路徑。
比較圖12（a）和（b），可以看出偏向性采樣可以加速擴展，考慮角度的距離測量方法可以成功減少子樹的抖動。圖12（c）展示了基于PB-RRT修剪原始子樹的過程。圖12（d）和（e）顯示了基于B樣條及其曲率曲線的平滑路徑，從中可以證明基于B樣條構造的曲線具有連續(xù)曲率和有限曲率最大值（絕對值）。
圖12 路徑規(guī)劃器仿真結果
圖13 改裝電動智能測試車
B.實驗結果
我們的實驗測試是在改裝的電動智能汽車上進行的，該汽車配備了集中驅動電機，轉向電機和電控液壓制動系統(tǒng)。通過激光雷達和單目相機獲得環(huán)境信息，用于決策和軌跡規(guī)劃。通過RTK和IMU獲得車輛的確切位置。實驗車允許的最大曲率為0.2m-1。如圖14和圖15所示，在交叉路口進行左右轉彎的實驗。黑線表示車道標記。道路寬度為3.5米。由于在轉彎過程中曲率較大，因此選擇了相對較低的均勻速度（10km/ h）。由于前瞻距離的存在，由車輛實際跟蹤的軌跡和由閉環(huán)模擬預測的軌跡沿采樣軌跡向內偏移。如圖14（a）和圖15（a）所示，我們的控制器可以實現(xiàn)規(guī)劃路徑的穩(wěn)定跟隨。如圖14（b）和圖15（b）所示，在轉彎過程中，規(guī)劃軌跡與跟蹤軌跡之間的橫向誤差達到60cm，而預測軌跡與實際軌跡之間的橫向誤差只有10厘米，這對于碰撞檢測非常重要。
圖14 十字路口左轉的軌跡
圖15 十字路口右轉的軌跡
圖16 變道軌跡和速度跟蹤
如圖16（a）所示，跟蹤軌跡與規(guī)劃軌跡或預測軌跡之間的橫向誤差均在20cm以內，這也證明了我們的控制準則在不同情景下的實用性。在這種情況下，當這兩個障礙物限制車道變換的縱向距離時，規(guī)劃軌跡的曲率最大值相對較大。并且初始速度很高（25 km / h），超過了根據(jù)動力學約束允許的最大速度。此時，規(guī)劃模塊選擇減速。如圖16（b）所示，跟蹤點的選擇不連續(xù)，這意味著跳過路徑上的某些點。證明了第二節(jié)的分析，這也可以解釋閉環(huán)預測方法的效果。
5、實驗
本文的主要貢獻是提出一種改進的RRT軌跡規(guī)劃器，稱為PB-RRT，它具有更高的效率，可以為智能車輛獲得平滑合適的軌跡。由于通過路徑速度解耦方法獲得的軌跡實際上是不可行的，因此采用閉環(huán)預測來估計車輛將如何跟蹤規(guī)劃路徑。通過實驗驗證預測的準確性，這在檢測碰撞時是有用的。為了適應車輛的非線性動力學約束和執(zhí)行器的飽和，設計了縱向和橫向控制的統(tǒng)一條件積分控制方法，可以保證跟蹤誤差的全局漸近穩(wěn)定性。通過仿真和實驗測試驗證了上述所有規(guī)劃方法，軌跡預測方法和控制方法的有效性。 聯(lián)系人：李老師
電話：021-69589225
郵箱：11666104@#edu.cn
廣告 編輯推薦
最新資訊
-
“汽車爬坡試驗方法”將有國家標準
2026-03-03 12:44
-
十年耐久監(jiān)管時代：電池系統(tǒng)開發(fā)策略將如何
2026-03-03 12:44
-
聯(lián)合國法規(guī)R59對機動車備用消聲系統(tǒng)的工程
2026-03-03 12:08
-
聯(lián)合國法規(guī)R58對后下部防護裝置的工程化約
2026-03-03 12:07
-
聯(lián)合國法規(guī)R57對摩托車前照燈配光性能的工
2026-03-03 12:07
