MPC與Lyapunov的軌跡重規劃與跟蹤控制

孫小松，段 敏，漢紅彪，楊 岐

(遼寧工業大學 汽車與交通工程學院， 遼寧 錦州 121001)

自從無人駕駛汽車被公認為將對未來汽車設計產生重大影響以來，其軌跡規劃與跟蹤就一直是研究熱點。目前，國內外大部分研究僅考慮了原規劃軌跡無影響下的跟蹤控制，對軌跡重規劃的跟蹤控制問題的研究比較有限。汪佳興等[1]利用B樣條曲線局部性和凸包性對重規劃算法規劃出的離散點進行擬合以提高路徑平滑性。王銀等[2]采用自適應模型預測控制提高軌跡的跟蹤與穩定性。潘公宇等[3]將魔術公式輪胎與單軌車輛模型結合，提出一種基于預測輪廓的控制策略。周維等[4]采用目標導向、節點修剪、曲線擬合和最優路徑選擇方法對基礎RRT規劃算法改進的同時聯合MPC(模型預測控制)完成軌跡的規劃與控制。姜立標等[5]以3自由度模型進行線性時變模型預測跟蹤控制。周龍輝等[6]在模型預測控制中加入了LQR方法以保證跟蹤橫擺穩定性。宋曉華等[7]采用機械特性極限等數據對跟蹤偏差進行了優化。楊博等[8]、鄧濤等[9]對軌跡的規劃與跟蹤皆采用模型預測控制，雖此控制方式的精確性有所提高，但對執行機構的可靠性有一定負面影響。Wang Huiran等[10]采用可變預測視域的模型預測控制的路徑跟蹤方法。鄒啟杰等[11]通過對RRT算法進行強化學習(RT)改進來實現路徑重規劃。蘇凱[12]在軌跡跟蹤控制階段采用模糊PID控制，雖然提高了控制的可靠性，但忽略了對橫擺穩定性的控制。

因此，本文在引入障礙物干擾原軌跡的跟蹤下進行研究，采用MPC與Lyapunov(李雅普諾夫)第二法設計控制器以實現軌跡重規劃跟蹤的目的，并通過仿真試驗其性能、可行性，防止由環境因素引起不必要的事故發生。

1 無人駕駛汽車建模

1.1 汽車動力學模型

為滿足控制性能與計算速度的要求，忽略懸架作用與空氣動力學的影響，不考慮車輛的翻滾、俯仰運動，建立線性雙自由度動力學模型(如圖1所示)，轉化為微分方程：

(1)

將式(1)轉化為矩陣形式：

(2)

考慮車輛動力性能、穩定性與小角度模型，假設汽車縱向速度不變[13]，基于干燥、潮濕路面立建如下約束：

(3)

圖1 雙自由度動力學模型

1.2 汽車點質量模型

軌跡重規劃(避障)模塊中忽略車輛尺寸與加速運動中的軸荷轉移應用點質量模型，如圖2所示。

圖2 點質量模型

其加入約束的數學模型為：

(4)

s.t.δg>ay|

(5)

2 軌跡重規劃與跟蹤控制器設計

本文控制結構采用雙模塊控制器協同控制，如圖3所示，軌跡重規劃模塊通過接收障礙信息與原規劃路徑的Yref、Xref、φref實現軌跡重規劃調整，將重規劃后當下理想的跟蹤參數輸入自適應跟蹤模塊進行跟蹤，跟蹤過程該模塊可調節前、反饋控制器參數值Ka、Kb，使理想跟蹤參數的偏差e盡量小后完成無人駕駛汽車模型輸入的精確取值δw。

圖3 雙模塊控制器結構

2.1 MPC軌跡重規劃控制器

2.1.1參考點與障礙物處理

在慣性坐標系XOY下取車身模型后軸中心點，做穿過此點平行于X、Y軸分別交原期望軌跡于點g1、g2。為使期望軌跡與重規劃軌跡間偏移量Δη小，將取離目標終點距離最近的點為初始參考點gi(i=1,2,…)。

對障礙物的處理可將障礙點連結為一個密封點整體，防止重規劃軌跡出現穿過兩障礙點時車身尺寸不能通過障礙間隙的情況。在車身坐標系xoy做i個障礙點，則任意一點的橫、縱坐標為：

(6)

式中：Yγ、Xγ分別為慣性坐標系下障礙點的橫縱坐標與車輛質心點橫縱坐標之間的差值；γ為x軸與障礙點間夾角。

2.1.2評價函數與控制器形式

車輛通過讀取傳感器得到的障礙信息，分別根據障礙物之間、障礙物與目標點的距離差進行聚類膨脹與評價函數值的調節，同時介于車速影響，設計任意采樣時刻ti下的避障功能函數：

(7)

式中：xi,0、yi,0為車身坐標系下障礙物與質心坐標之間的差平方；Qobs為權重系數;z取極接近于0。

評價函數與障礙點坐標的關系如圖4所示。

以避障為優先條件，降低原軌跡?與實際重規劃軌跡?ref之間的偏移量設計MPC控制器形式為：

(8)

式中：Q為Y誤差的評價矩陣；R為控制序列的評價矩陣；Np為預測時域；Ui為控制量合集。

圖4 評價函數示意圖

運用Matlab中opti工具箱式(8)進行非線性求解。

2.1.3軌跡擬合

為使軌跡跟蹤控制模塊有良好的跟蹤效果，有必要設計不同的控制、預測時域應用于2個控制器模塊。但為避免控制器不規范化的設計，滿足車輛位置連續、橫擺角連續等運動學約束，采用5次多項式法對重規劃軌跡離散點進行擬合：

(9)

2.2 基于Lyapunov第二法的自適應軌跡跟蹤控制器

采用車輛雙自由度動力學模型建立實際軌跡與重規劃軌跡之間誤差狀態方程，用矩陣形式表示：

(10)

為確保通過Lyapunov第二法設計的能量函數Ve(t)有解且滿足控制系統穩定性，引入前、反饋矩陣，建立方程：

(11)

由上可得出新的Lyapunov方程：

(D1Ka1+D)J-1+J-1(D1Ka1+D)T=-J-1GJ-1

(12)

式中：Ka1+Ka2=Ka；J-1、G皆為正定矩陣。

通過D、D1矩陣的調控的同時利用Matlab控制系統工具箱中的dlyap函數可對J-1、Ka1進行直接求解、設計，并借此對能量函數Ve(t)進行詳細構造：

(13)

因為：

(14)

(15)

式中：η1、η2為權重系數；J1=JD1Ka2,J2=JD1Kw。

顯然此條件下：

(16)

式中，二者初值為零。

3 Carsim/simulink聯合仿真

3.1 仿真工況與主要參數

原軌跡采用雙移線工況，使車輛分別在附著系數為0.5的潮濕平坦路面與附著系數為0.85的干燥平坦路面進行基于速度38、58、78 km/h的仿真試驗，驗證控制器性能。主要控制器參數、整車主要參數如表1、2所示。

表1 主要控制器參數

表2 整車主要參數

3.2 仿真結果分析

3.2.1軌跡重規劃性能仿真

試驗場景為以躲避在原期望軌跡(預定行駛路徑)處出現與其交匯且經膨脹處理后的障礙物為目的構造的，以增加障礙物對原路徑的“干擾”程度，坐標如圖5所示，取車速為38、58、78 km/h時控制器所規劃的多數路徑并決定新的滿足約束條件且最終收斂于原期望軌跡的跟蹤軌跡，與原軌跡對比，體現出二者間偏差與車速成正增長。

3.2.2軌跡跟蹤性能仿真

圖6為基于附著系數為0.85的干燥平坦路面下的軌跡跟蹤仿真試驗。圖6(a)仿真結果顯示，在試驗開始階段，車速在38、58、73 km/h轉向時，側向加速度的數值出現了0.005～0.3可控范圍內的起伏且為提高路面可利用附著力使車輛轉彎順利，隨速度增加其轉向所需的側向加速度越大，在其余階段側向加速度趨于平緩，體現了控制器在保證跟蹤精度的同時滿足了橫擺穩定性。

圖6(b)～(d)對不同車速下跟蹤過程的縱向、側向位移與全局位置變化進行導出，可以看出，其滿足了對重規軌跡的精確跟蹤且峰值偏差控制在0.25以內。隨著車速的提高，跟蹤偏差有微小的上升趨勢，但最終的狀態誤差皆趨于0，這是由于控制器在時間無窮大時誤差趨于0的設計原理決定的。

圖6(c)～(f)顯示，在各個車速下，橫擺角無突出的波動，趨于平滑，體現車輛在重規劃軌跡跟蹤過程中處于穩定。輪胎非線性影響橫擺角速度圖像出現毛刺，但不影響車輛對行駛穩定性、控制器對控制精度的要求。

圖5 不同車速雙移線工況下軌跡重規劃仿真結果

圖6 u=0.85時不同車速雙移線工況下軌跡跟蹤仿真結果

圖7為基于附著系數為0.5的潮濕平坦路面下的軌跡跟蹤仿真試驗。圖7(a)(c)所示不同車速跟蹤位置變化、橫擺角速度與干燥路面下的仿真結果對比無明顯變化，軌跡跟蹤的性能度依舊保持高的精確度。圖7(b)顯示與對應干燥路面下的仿真結果相比，橫擺角在采樣時間為0.28 s左右時的穩定程度反而增加，這是由于控制結構中前、反饋控制器調節參數值Ka、Kb所致。

圖7 u=0.5時不同車速雙移線工況下軌跡跟蹤仿真結果

4 結論

以車輛點質量模型與雙自由度模型為基礎設計雙模塊控制器，基于MPC算法與Lyapunov第二法確立了控制量與狀態變量的同時借由實際軌跡與重規劃軌跡之間誤差狀態方程設計能量函數，并引入避障評價函數和前、反饋矩陣進行軌跡的重規劃與跟蹤控制。通過聯合仿真試驗結果揭示此控制器具有較強的魯棒性，能模擬多數軌跡的重規劃，并盡可能降低軌跡偏移量，實現精確的軌跡跟蹤，滿足橫擺穩定性要求。在軌跡跟蹤階段，本文為簡化模型采用僅考慮橫擺穩定性的雙自由度模型，為進一步實現實用多元化，未來研究將引入側傾、俯仰對重規劃軌跡跟蹤的影響。