周 維，過學迅，裴曉飛，張 震，余嘉星

（1.武漢理工大學，現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室，武漢 430070；2.武漢理工大學，汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070）

前言

智能車輛近年來是國內(nèi)外汽車領(lǐng)域的研究熱點，而局部路徑規(guī)劃和跟蹤控制是其發(fā)展的關(guān)鍵技術(shù)之一［1］。其中，以RRT算法［2］為代表的采樣規(guī)劃算法，通過一個初始點作為根節(jié)點，采用隨機采樣的方式增加樹結(jié)構(gòu)的分支，當分支節(jié)點包含目標節(jié)點或進入目標區(qū)域，即可快速生成一條由樹節(jié)點生成的從初始點到目標點的路徑，具有概率完備性，在多障礙物的復雜場景應用較多。

針對傳統(tǒng)RRT規(guī)劃算法節(jié)點隨機分布導致的計算時間長、規(guī)劃路徑完備且非最優(yōu)、路徑不平滑的問題，國內(nèi)外學者提出了多種優(yōu)化方法。文獻［3］中提出了一種雙向隨機搜索樹（Bi-RRT），從起始點和目標點同時向外拓展樹結(jié)構(gòu)，提高計算效率。文獻［4］中指出合理的目標導向，可提高RRT求解速度。文獻［5］和文獻［6］中以Dijkstra算法和A*算法作為RRT的啟發(fā)式函數(shù)，在搜索路徑時以最小路徑為參考，使規(guī)劃結(jié)果更接近最優(yōu)解。文獻［7］中通過選擇合理的度量函數(shù)節(jié)點，開發(fā)了連續(xù)曲率RRT（continues-curvature RRT，CC-RRT）路徑規(guī)劃方法。文獻［8］中將RRT*與稀疏RRT（sparse-RRT）相結(jié)合，通過RRT*快速收斂于最優(yōu)解的同時，利用修剪無用節(jié)點帶來的稀疏特性減少計算量。文獻［9］中首先用卷積神經(jīng)網(wǎng)絡訓練預定場景下對應安全軌跡的期望加速度和轉(zhuǎn)向角，然后將生成的控制輸入加入RRT方法中完成狀態(tài)空間的節(jié)點連接。仿真表明該方法在保證規(guī)劃結(jié)果安全性的同時，提高了計算效率。

模型預測控制根據(jù)已知模型和當前狀態(tài)量可估算輸出，根據(jù)預測值與規(guī)劃的期望值偏差，施加控制量輸入，同時在控制過程中可施加多種約束，從而保證系統(tǒng)穩(wěn)定、可靠地運行，因此在跟蹤控制中得到廣泛應用［10］。文獻［11］中基于非線性MPC方案設計了智能車輛的主動轉(zhuǎn)向系統(tǒng)，使車輛在預定軌跡跟蹤時可保持穩(wěn)定性，但該算法在速度大于10 m／s的情況下控制效果不佳，算法的實時性需要進一步提升。文獻［12］中基于駕駛員意圖識別和狀態(tài)評估，設計了一種可完成避障路徑的時變模型預測控制器，既可幫助駕駛員完成避障也可在不同工況下獲得較好的車輛穩(wěn)定性。文獻［13］中通過實車試驗證明基于非線性模型預測控制的軌跡跟蹤能在高速結(jié)冰路面上成功避障。

集成合理且平滑的規(guī)劃路徑和魯棒性較好的跟蹤控制算法有助于提升智能車輛的綜合性能，本文中通過目標導向、節(jié)點修剪、曲線平滑和最優(yōu)路徑選擇的方法對傳統(tǒng)RRT進行改進，以滿足安全避障和運動學約束。針對車輛系統(tǒng)的強非線性和多模式約束，提出了集成LTV-MPC跟蹤控制算法和改進RRT規(guī)劃算法的智能車輛規(guī)劃與控制算法，并以智能車輛的安全性和實時性作為評價指標，通過硬件在環(huán)仿真驗證算法的合理性?？紤]路徑規(guī)劃與跟蹤控制算法的相互影響，分析了不同車速、規(guī)劃步長、預測步長和控制步長對路徑規(guī)劃與跟蹤控制效果的影響。

1 智能車分層構(gòu)架

自動駕駛汽車主要由環(huán)境感知層、任務決策層、路徑規(guī)劃層和跟蹤控制層構(gòu)成，如圖1所示。由環(huán)境感知層獲知車輛的定位信息并構(gòu)建可行駛區(qū)域，經(jīng)由決策層形成全局路徑，以全局路徑為參考，路徑規(guī)劃完成局部環(huán)境下起始點到目標點最優(yōu)路徑的選擇，該路徑應連續(xù)、平滑、可避障并滿足車輛運動學約束，以實現(xiàn)車輛的避障和連續(xù)平滑行駛的功能，跟蹤控制模塊接收路徑規(guī)劃的信號，自動調(diào)節(jié)油門、制動和轉(zhuǎn)向等底層執(zhí)行器，實現(xiàn)軌跡的平穩(wěn)跟蹤。

圖1 智能車分層構(gòu)架

本文中的主要工作集中于底層的路徑規(guī)劃和跟蹤控制上，改進RRT規(guī)劃算法根據(jù)全局路徑和障礙物信息規(guī)劃出一條平滑的局部路徑后，跟蹤控制模塊接收來自規(guī)劃層提供的參考軌跡點，通過LTV-MPC控制器計算出控制輸入量，實現(xiàn)軌跡跟蹤。

2 改進RRT路徑規(guī)劃算法

由于RRT算法的遍歷特征，會導致生成的軌跡點具有不確定性、規(guī)劃時間長、軌跡振蕩劇烈和平滑性較差等不足。因此，提出了改進的RRT算法框架，如表1所示。表中：第6行，是采用目標導向的方法，改善基礎(chǔ)RRT算法的隨機性，減小單次規(guī)劃所需的時間；第11行，是針對車輛行駛過程中應滿足運動學和執(zhí)行器不宜頻繁操作的目標提出的節(jié)點修剪；第12行，為解決因節(jié)點修剪發(fā)生的振蕩，引入路徑平滑的方法；第13行，結(jié)合智能車輛的避障、穩(wěn)定性和路徑距離的要求，對基礎(chǔ)RRT的規(guī)劃算法制定了權(quán)重系數(shù)，引入最優(yōu)路徑選擇的方法。

表1 改進快速搜索隨機樹算法框架

2.1 目標導向

基礎(chǔ)RRT因概率完備，使得從起始點到目標點的收斂速度較慢，依據(jù)文獻［4］，本文中通過概率偏置和目標連接的方法進行引導隨機樹以提高計算效率。如表1中第4行，采樣節(jié)點Sample-Point在概率小于一定值時，以目標點為隨機點，可保證隨機點的分布趨向于目標點。通過隨機點分析出最接近點后，直接判斷最接近點與目標點間的避障情況，減少規(guī)劃次數(shù)，加速收斂。

2.2 最優(yōu)路徑選擇

在目標點附近按照一定規(guī)律設置目標函數(shù)，可同時生成多條從起始點到目標點范圍內(nèi)的規(guī)劃路徑。以距離障礙物的位置、側(cè)向加速度和路徑長度作為評價指標，對可行軌跡進行選擇，其判定方程為

式中：Lpath和Lmin分別為可供選擇的路徑長度和最短路徑長度；dobstacle和dmin分別為各路徑距離障礙物的距離和備選路徑中距離障礙物最近距離；ay和amin分別為路徑的側(cè)向加速度和可行路徑的最小側(cè)向加速度；w1、w2和w3分別為距離障礙物位置、側(cè)向加速度和路徑長度的權(quán)重系數(shù)。為突出規(guī)劃過程中對車輛避障和安全的考慮，在代價函數(shù)中提高車輛距離障礙物位置的權(quán)重，參照文獻［14］的取值，w1、w2和w3分別取0.5、0.25和0.25。

2.3 節(jié)點修剪

如表1中第12行，采用貪心算法對最優(yōu)路徑上的節(jié)點進行修剪，以保證路徑朝著最優(yōu)路徑逼近。改進RRT算法示意圖如圖2所示。由基礎(chǔ)RRT可知從起始點到目標點會依次生成一系列點集，當其連線與障礙物有交集時，判定該點為碰撞點，進行規(guī)避，并重新規(guī)劃，直到起始點與目標點進行連接，如圖中實線所示?？紤]實際車輛前輪轉(zhuǎn)角約束，規(guī)劃路徑之間的夾角應盡可能平緩，仿真分析中設定路徑的最小轉(zhuǎn)角為90°［7］，以a0點為起點，依次連接各規(guī)劃點，以碰撞安全和最小轉(zhuǎn)角為約束，修剪規(guī)劃路徑。對于夾角較小的規(guī)劃路徑，如圖2中點a4所示，插入可平緩規(guī)劃夾角的新點，直到抵達目標點。

圖2 改進RRT算法示意圖

2.4 曲線平滑

由于車輛存在運動學約束，規(guī)劃路徑應滿足曲線的2階連續(xù)?？紤]避障因素，節(jié)點的修改只影響局部曲線的改變，如圖2中雙點劃線所示。綜合規(guī)劃路徑的要求和計算成本，選擇三次B樣條曲線對路徑進行平滑處理，三次B樣條的矩陣表達式為

式中：Bi為第i段弧的B樣條線解析式；k的取值范圍為［0，1］；ai、ai＋1為內(nèi)點。為了使擬合的曲線經(jīng)過起始點和目標點，在a0的前面和an的后面分別增加兩個節(jié)點a-2與a-1和an＋1與an＋2，以保證起始點和目標點處的運動學約束。

此外，改進RRT的規(guī)劃周期和規(guī)劃步長決定了路徑規(guī)劃的效果，規(guī)劃步長過大，曲線擬合和避障效果變差，過小則對硬件的計算能力要求較高。

3 LTV-MPC軌跡跟蹤控制算法

3.1 非線性軌跡跟蹤模型

考慮單軌車輛的動力學模型作為MPC控制器的預測模型，采用線性輪胎模型，建立如下車輛動力學方程：

軌跡跟蹤模型的輸出為航向角φ和車輛質(zhì)心的位置Y（或者包括質(zhì)心的位置X），最終式（3）系統(tǒng)方程可表示為

3.2 LTV-MPC算法

利用Taylor線性化法建立基于LTV-MPC的軌跡跟蹤控制器，相對于非線性模型預測控制中的用非線性規(guī)劃求解優(yōu)化問題，線性模型預測控制中采用的QP算法效率更高［9］。

3.2.1 線性時變狀態(tài)方程

結(jié)合1階Taylor展開式和雅可比矩陣，可對式（4）非線性狀態(tài)方程在工作點附近進行線性化和離散化，為便于在約束條件中添加控制量增量約束，將狀態(tài)方程寫成增量形式，可推導出LTV-MPC的輸出表達式：

式中：A（t）、B（t）、C（t）分別為動力學方程對狀態(tài)量與控制量的導數(shù)和輸出量對狀態(tài)量的導數(shù)；Ak，t＝T A（t）＋I，Bk，t＝T B（t），Ck，t＝［C（t）0］，T為離散時間間隔；Np、Nc分別為預測時域和控制時域；I為單位矩陣；dk，t為偏差項。

3.2.2 LTV-MPC代價函數(shù)及動力學約束

根據(jù)式（5）設計LTV-MPC代價函數(shù)，以保證自動駕駛汽車穩(wěn)定、快速地跟蹤期望軌跡，為了確保函數(shù)具有可行解，在目標方程中添加松弛因子，即LTV-MPC的代價函數(shù)為

式中：ρ為權(quán)重系數(shù)；ε為松弛因子。

由式（6）可知，LTV-MPC代價函數(shù)的解為控制增量，約束以控制增量或通過轉(zhuǎn)換矩陣與控制增量乘積的形式出現(xiàn)。在車輛實際運行中，前輪轉(zhuǎn)角和轉(zhuǎn)角轉(zhuǎn)速有一定限值，故控制量約束表達式為

式中：T1為Nc×Nc維的轉(zhuǎn)換矩陣；Im為維度為m的單位矩陣；?表示克羅內(nèi)克積；umin和umax分別為控制時域內(nèi)的控制量最小值和最大值集合；ysc，min、ysc，max為軟約束的極值。

為保證車輛處于線性區(qū)，參照文獻［15］增加汽車質(zhì)心側(cè)偏角、車輛側(cè)向加速度約束和輪胎側(cè)偏角約束，即

式中：β為質(zhì)心側(cè)偏角；ay為車輛側(cè)向加速度；μ為地面附著系數(shù)；af，t為輪胎側(cè)偏角。

綜上由式（5）～式（9）可知，影響跟蹤控制效果的因素較多，如車速v、Np、Nc和權(quán)重系數(shù)Q與R等。

4 路徑規(guī)劃與跟蹤的硬件在環(huán)驗證

4.1 主要車型參數(shù)

在Simulink／CarSim聯(lián)合仿真平臺中，如圖3所示，采用dSPACE RT1005模塊進行實時仿真。首先，搭建智能車輛軌跡規(guī)劃與跟蹤控制系統(tǒng)的Simulink模型，然后通過自動代碼生成模塊加載相應的C代碼，通過dSPACE／ControlDesk實現(xiàn)控制參數(shù)的實時調(diào)整，并完成車輛狀態(tài)參數(shù)的在線觀測和記錄。圖1中智能車的主要參數(shù)如表2所示，并在CarSim RT中進行相應設置。

圖3 試驗臺架

表2 智能車主要結(jié)構(gòu)參數(shù)

4.2 離散點處理

在改進RRT的規(guī)劃算法中，以全局路徑為參考，結(jié)合環(huán)境信息規(guī)劃出一條平滑的軌跡，并以離散點的形式傳遞給跟蹤控制層。由于規(guī)劃層和控制層的周期不一致，跟蹤控制部分無法直接利用規(guī)劃層提供的離散點數(shù)據(jù)，采用五次多項式擬合方法先將規(guī)劃層得到的局部路徑擬合成曲線，在控制層基于該曲線進行離散化，從而由時間域轉(zhuǎn)化成幾何域，消除周期不一致，實現(xiàn)規(guī)劃層和控制層的順利對接［15］。圖4為規(guī)劃路徑與跟蹤控制的聯(lián)立圖，其中虛線表示規(guī)劃層離散點擬合曲線，控制層根據(jù)質(zhì)心到曲線的最短路徑計算車輛的輸入控制增量，并進行跟蹤控制。

圖4 路徑規(guī)劃與路徑跟蹤的聯(lián)立

4.3 仿真試驗驗證

基于ISO3888—1雙移線試驗工況，以道路中間線為規(guī)劃與跟蹤控制的啟發(fā)路徑。實時仿真采用的規(guī)劃與控制器主要參數(shù)如表3所示，圖5為智能車實時仿真結(jié)果。

由圖5可知，基于啟發(fā)式的引導路徑，通過改進的RRT規(guī)劃算法和LTV-MPC控制算法，可實現(xiàn)智能車的平穩(wěn)行駛，圖5（a）顯示在移線過程中，智能車會出現(xiàn)偏離引導線，但最大橫向誤差小于0.7 m，最大橫向偏差發(fā)生在拐角處，且在直線工況逐漸趨于穩(wěn)定，在道路的邊界線內(nèi)可正常通過。圖5（b）表示在智能車跟蹤過程中，其最大的側(cè)向加速度小于0.2g，航向角小于0.2 rad，滿足穩(wěn)定性約束要求。多次硬件在環(huán)仿真時長誤差平均值為0.07 s，小于0.1 s，即小于單個的規(guī)劃周期，說明提出的算法可實現(xiàn)路徑的實時規(guī)劃和穩(wěn)定性跟蹤。

表3 智能車路徑規(guī)劃與跟蹤算法主要參數(shù)

圖5 智能車實時仿真結(jié)果

5 路徑規(guī)劃和跟蹤效果的主要影響因素分析

由圖5可知，雖然提出的改進RRT結(jié)合LTVMPC可實現(xiàn)智能車的實時規(guī)劃和穩(wěn)定性跟蹤，但在移線拐點部分誤差較大，須對其主要影響因素進行進一步分析?；谟绊懜倪MRRT和LTV-MPC算法的主要因素，文中以單移線試驗工況為例，分析了不同車速、規(guī)劃步長、規(guī)劃周期對規(guī)劃與跟蹤效果的影響。

5.1 不同車速下的規(guī)劃與跟蹤效果

選取車速為18、24和36 km／h進行仿真，結(jié)果如圖6所示。由圖可見：在直線路徑為啟發(fā)路徑時，不同車速的跟蹤效果均較好；以轉(zhuǎn)彎換道路徑為啟發(fā)路徑，在一定速度范圍內(nèi)，速度越小，其跟蹤的軌跡越接近目標路徑。表4為不同車速下的最大側(cè)向誤差和最大航向角誤差。

圖6 不同車速下的仿真結(jié)果

表4 不同車速下的最大側(cè)向誤差和航向角誤差

5.2 不同規(guī)劃步長下的規(guī)劃與跟蹤效果

規(guī)劃步長為1 500、2 000和2 500 mm時的仿真結(jié)果如圖7所示。由圖可見，在換道轉(zhuǎn)彎時，當速度達36 km／h時，規(guī)劃步長為2 500 mm，其實際規(guī)劃與跟蹤的軌跡與目標軌跡的偏差較小。由文獻［16］可知在U-turn工況，規(guī)劃步長過長會對跟蹤控制效果產(chǎn)生影響。同時由圖2可知，RRT算法在規(guī)劃算法較長時，容易出現(xiàn)擬合曲線和障礙物相交，故不宜選取過大的規(guī)劃步長。表5列出了不同規(guī)劃步長下最大側(cè)向誤差和最大航向角誤差結(jié)果。

圖7 不同規(guī)劃步長下的仿真結(jié)果

5.3 不同規(guī)劃周期下的規(guī)劃與跟蹤效果

考慮到車輛執(zhí)行器的控制精度需求，選擇控制周期為0.01 s，對于不同規(guī)劃周期的跟蹤和規(guī)劃的效果如圖8所示。由圖可見，規(guī)劃周期越長，其規(guī)劃與跟蹤的路徑與目標路徑越接近。因規(guī)劃周期在一定范圍內(nèi)跟蹤效果相近，且對于更短的規(guī)劃周期，需要硬件提供更好的計算能力，因人類駕駛員規(guī)劃周期為0.2～0.3 s，故一般選擇稍快于人類駕駛員的0.2 s為規(guī)劃周期。表6為不同規(guī)劃周期下的最大側(cè)向誤差和最大航向角誤差。

圖8 不同規(guī)劃周期下的仿真結(jié)果

表6 不同規(guī)劃周期最大側(cè)向誤差和航向角誤差

6 結(jié)論

（1）本文中提出了一種基于改進RRT與LTVMPC的智能車輛路徑規(guī)劃與跟蹤控制算法的基礎(chǔ)構(gòu)架，基于啟發(fā)式路徑，結(jié)合五次多項式對規(guī)劃出的局部路徑進行擬合，實現(xiàn)路徑規(guī)劃層和跟蹤控制層的有效對接，硬件在環(huán)仿真驗證了該智能車路徑規(guī)劃和跟蹤算法的安全性和實時性。

（2）基于單移線試驗工況，分析了不同車速、規(guī)劃步長、預測步長和控制步長對規(guī)劃和跟蹤效果的影響。在一定的范圍內(nèi)，車速越小，規(guī)劃步長越大，規(guī)劃周期越大，規(guī)劃和控制的軌跡與目標軌跡越接近。

（3）除了本文所討論的規(guī)劃與跟蹤效果影響因素，執(zhí)行器響應速率和控制器的計算能力等需進一步研究。此外，本文中僅通過硬件在環(huán)驗證了算法的可行性，規(guī)劃與跟蹤的效果和實時性與穩(wěn)定性也需進一步的實車試驗進行驗證。