基于模型預測控制的無人駕駛主動避障策略

崔玉定，熊豪杰，賀滿川，薛仁強

（430065 湖北省 武漢市 武漢科技大學 汽車與交通工程學院）

0 引言

無人駕駛是汽車技術發展的重要趨勢之一，可以降低交通事故的發生率，減少擁堵現象，同時能提高資源的利用率。其中主動避障技術作為無人駕駛的關鍵受到廣泛的研究。

主動避障技術主要涵蓋了局部路徑規劃和軌跡跟蹤。局部路徑規劃主要的控制目標是在已知環境模型中，從當前位置到目標點間尋找一條可行的無碰撞路徑[1]。目前常用的算法有人工勢場法、基于搜索的軌跡優化算法和圖窗搜索法等[2]。人工勢場法是一種虛擬力法，由Khatib[3]最早提出并應用于路徑規劃，規劃出的路徑相對平滑，算法簡單可靠，實時性較好。但傳統人工勢場法也存在容易陷入局部最優和目標不可達的問題。安林芳等[4]通過在斥力場中加入虛擬目標點，避免了目標點不可達問題的發生。修彩靖等[5]改變了傳統的引力場目標函數構建方式，建立高斯組合隸屬函數，并在其中加入了調節因子，消除了傳統人工勢場法容易出現局部最優的問題。在軌跡跟蹤控制方面，Tagne 等[6]采用滑模控制算法，以實際軌跡與參考軌跡的側向偏差為基礎設計滑動面，實現對目標軌跡的跟蹤。雖然該方法魯棒性較好，但會引起嚴重的系統抖振，系統穩定性較差。Fan 等[7]利用 LQR控制算法對線性化后的車輛系統進行閉環控制，最終設計的控制系統具有穩定性高和實時性好的優點，但跟蹤過程中出現了較大的偏差且LQR 算法無法對狀態量和控制量加以限制處理約束問題。

基于模型預測控制（MPC）設計了分層式主動轉向避障控制器。在上層避障軌跡規劃層，接收傳感器傳入的障礙物信息和全局期望路徑信息，為避開障礙物引入了避障懲罰函數并在目標函數中考慮了車輛運動學約束，規劃出一條無碰撞的局部路徑。下層軌跡跟蹤控制層接收上層規劃器傳來的局部參考軌跡信息，將車輛安全行駛包絡線約束與模型預測控制算法相結合，求解出前輪轉向角的最優解[8]。

1 局部路徑規劃控制器

1.1 車輛點質量模型

為減小計算量、提高計算速度同時兼顧控制性能，選用車輛點質量模型，該模型將車輛看作一個質量點，忽略車身尺寸、輪胎受力和載荷變化[9]，如圖1 所示。圖1 中：O——車輛質心；x——車輛縱向位移；——車輛縱向速度；y——車輛側向位移；——車輛側向速度；φ——車輛橫擺角。

圖1 車輛點質量模型Fig.1 Point mass model of autonomous vehicle

在大地參考坐標系下，車輛點質量模型表示為

1.2 規劃層預測模型

路徑重規劃層的實時性要求較低，且采用的點質量模型也有一定程度的簡化。采用非線性模型預測控制算法（NMPC）在滿足軌跡重規劃的要求外具有更高的求解精度，對于后續的二次規劃求解函數的設計也帶來了便利。

采用歐拉法對點質量模型進行離散化，得到車輛點質量模型的差分方程

在小學生的成長過程中，家庭教育不可或缺，家庭也應該對學校的教學起監督作用。但是在調查中發現，出于對學生成績的考慮，部分家長并不關心教學改革，也不關心多媒體在教學中的應用，只關心學生的成績是否優秀，如果學生的成績得到了提升，就會對教師和學校給予高度的評價，反之則評價較低。這在無形之中對于教師形成的壓力，也在一定程度上阻礙了教學改革、教學創新。

式中：k——當前采樣時刻；T——離散時間；y（k），x（k）——車輛在k 時刻橫、縱向位移；——車輛在k 時刻的橫、縱向速度；φ（k）——車輛在k 時刻的橫擺角。

為簡化計算，假設車輛縱向速度為一恒定常數，即縱向加速度ax=0,以汽車橫向加速度為控制變量即u=ay，以為狀態變量，將式（2）整理為

式中：i=1，2，…，Np，j=1，2，…，Nc；f（·）——點質量模型的差分方程函數表達式；Np——預測時域；Nc——控制時域；t——采樣時間。

式中：Jobs——具體的懲罰函數值；Sobs——權重系數；（xi，yi）——障礙物第i 個端點在大地坐標系下的位置坐標；（xo，yo）——車輛質心坐標；κ——一個極小的非0 正數，目的是防止懲罰函數的分母為0。

1.3 優化問題構建與求解

為使智能車輛準確高效跟蹤上層控制器規劃的局部軌跡，需考慮車輛縱向、側向和橫擺3 個維度的運動，建立更加精確的車輛動力學模型。如圖2 所示，圖2 中：m——整車質量；Iz——車輛繞Z軸轉動慣量；Fxf，Fyr——車輛前后輪胎所受到的側偏力；Fxf，Fxr——前后輪胎受到的縱向力。

小學科學的最終目標應當是促使學生能夠主動將科學技能與生活實踐聯系起來，進而實現科學教學內容的生活化以及學生課外活動的生活化。達到這一最終目標，除了需要科學教師自身擁有超高的專業素養，還需要其主動將教學內容與學生生活實際有機結合起來，運用科學的引導作用，促使學生將科學知識運用到課外的實踐活動中，幫助學生理解科學與生活的密切聯系性，為生活服務。

為躲避行駛路徑上的障礙物，需要在上層控制器的目標函數中加入避障懲罰函數。龔建偉等[10]提出根據障礙物點與目標點之間的距離偏差和車輛行駛速度設置障礙物懲罰系數，當車輛與障礙物越近、車速越高，懲罰值越大。具體懲罰功能函數為

式中，f(x,y)為回波信號函數, Xc為脈沖波形形心的時間位置.用式(1)對脈沖激光波形建模，激光雷達形心算法基于式(3)計算形心，為了降低信號中噪聲對形心計算的影響，在對采樣點進行濾波預處理后，通過滑窗找到波形位置，根據形心計算理論分別計算發射波的形心位置xf和回波的形心位置xr，距離信息的計算公式為

根據式（3）可得NMPC 控制時域內的預測表達式為

（x）通過加權集結算子將子組Ey（y=1，2，…，10）群決策矩陣y=1，2，…，10）轉化為組E群決策矩陣D=（dij）m×n（i1，2，...，10，j=1，2，3，4，y=1，2，…，10）。

1.3.2 軌跡重規劃層目標函數設計

將線性化后的Pacejka 輪胎模型引入車輛動力學模型中得到

利用式（16）和式（17）進行迭代運算，可以得到預測時域內系統輸出量表達式為

式中：Yref——預先給定的全局參考軌跡；Q——上層控制器中的輸出權重系數矩陣；R——控制輸入的權重系數矩陣；Jobs——避障懲罰函數值。

1.4 重規劃軌跡擬合

在軌跡重規劃算法中，重規劃后的軌跡是離散點的形式。如果直接以離散點的形式傳輸給下層控制器，會占用太多的控制器輸出與輸入接口，控制器的設計會變得更加復雜[11]。此外由于多個控制周期的軌跡可以包含在規劃控制器的一次求解范圍內，因此上層規劃周期與下層控制周期并不相同。為了確保上層局部規劃控制器與下層跟蹤控制器之間的順利對接，采用5 次多項式擬合重規劃后的離散軌跡點，形式為

式中：ai，bi（i=0，1，2，3，4，5）——待求軌跡擬合和航向角擬合參數。

2 軌跡跟蹤控制器

2.1 車輛動力學模型

1.3.1 避障懲罰函數設計

圖2 車輛動力學模型Fig.2 Dynamic model of autonomous vehicle

本文只涉及轉向避障控制，車輛縱向速度被設定為常數，只考慮側向和橫擺2 個維度的運動，根據車輛受力平衡和力矩平衡可以得到：

第Ⅲ含礦帶賦存于石閆背斜南翼，位于第Ⅱ含礦帶以南，以東石門斷層為界可分為東西兩個區段，東段為石門鐵礦黃牛嶺鐵礦區，向東至白水牛石斷層，控制長度約2.4km，礦帶核部被剝蝕，礦帶傾向SW，S，傾角45°～65°。礦帶發育2個礦層，礦層總厚度在13.45～14.94m之間，厚度穩定；西段為中鋼山東礦業小閆莊鐵礦區南翼，為隱伏礦床，發育核部，與第Ⅱ含礦帶在核部相連，礦帶傾角由上至下由緩變陡，控制長度約2.6km，向西尚無工程控制，礦帶發育3個體，礦體總厚度23.50m，厚度穩定。

基于小角度假設，選用線性化的魔術輪胎模型[12]。具體計算公式為

式中：Cα——車輪等效側偏剛度；Cs——車輪縱向剛度；α——輪胎側偏角；S——輪胎滑移率。

重規劃后的軌跡應滿足：（1）能避開障礙物，滿足安全避障要求；（2）車輛的運動學及性能約束；（3）與全局參考路徑之間的偏差足夠小。因此設計的軌跡重規劃層的目標函數為

可改寫為如式（12）、式（13）的形式

在生活質量評分方面，觀察組顯著優于對照組（P＜0.05）；在肢體功能評分情況方面，觀察組較對照組優勢顯著（P＜0.05），充分表明其臨床優勢。

2.2 軌跡跟蹤層預測模型

由于用于軌跡跟蹤層的車輛三自由度模型比較復雜，且采樣周期短，非線性模型預測控制不能滿足實時性的要求。基于狀態軌跡線性化方法對整車模型進行近似線性化，以獲得線性時變模型預測控制[13]。狀態軌跡線性化的基本原理是通過對車輛系統連續施加恒定的控制量來獲得車輛狀態軌跡，然后通過車輛狀態軌跡與實際狀態量之間的偏差來線性化整車模型。

假設車輛狀態方程有一工作點為[χ0，u0]，在[χ0，u0]以泰勒公式展開，為提高計算速度，忽略1階以上的高階項，可得

得到在每一采樣時間t 有

為保證車輛輸入量變化平穩，所以在模型預測控制中對控制量采用增量方式，以線性時變模型當前狀態量和上一采樣時刻的控制量為新的狀態變量，即ξ(k)=[χ(k);u(k-1)]。將線性時變模型控制量的變化量即Δu(k)作為控制變量,得到系統狀態方程和系統輸出方程

甘薯又名地瓜、山芋、番薯等，是旋花科一年生或多年生草本塊根植物。我國是世界上最大的甘薯生產國，占世界總產量的75.3%。我們日常食用的部分，就是甘薯的塊根。

目前辣椒辣度的測定方法中，國標(GB 10783-2008)法[2，3]測定辣椒堿含量，存在使用巨毒藥品、重復性差等問題；Scoville感官評定法[4，5]存在測試環境限制較多、主觀成分較大的問題；高效液相色譜法(High Performance Liquid Chromatography，HPLC)[6-8]的操作環境難以摸索，體系難以建立，并且儀器維護費用較高，且不便于田間應用。因此，需要建立一種便攜式的快速、準確、穩定的檢測技術。

2.3 軌跡跟蹤層優化問題構建與求解

2.3.1 車輛安全行駛包絡線約束構建

當智能車輛進行軌跡跟蹤時，勢必會產生橫向運動，車輛容易發生失穩現象。所以一個完整的軌跡跟蹤控制器，除了要滿足軌跡跟蹤精度要求，還要保證車輛系統的穩定。Bobier 等[14]提出車輛質心側偏角和橫擺角速度對于車輛穩定性起著非常重要的作用。車輛穩定包絡線如圖3 所示，只要車輛質心側偏角和橫擺角速度處于車輛橫向穩定安全區域即包絡線之內，就可以保證車輛穩定行駛。將車輛安全行駛包絡線與模型預測控制相結合，設計出具有車輛穩定控制的軌跡跟蹤控制器。

3.7 中醫耳壓療法 減少或消除ICU綜合征，除了上述治療及護理外，傳統的中醫治療也有一定效果。曾影紅等［29］報道，對63例ICU綜合征患者進行中醫耳壓療法，總有效率約68.3%，且此法具有較好的療效，無損傷、無不良反應、操作簡便和經濟等優點。

式中：vx——車輛縱向速度；——橫擺角速度上限值；αlim——輪胎側偏角上限值。

圖3 車輛安全行駛包絡線約束Fig.3 Constraints of envelope for safe driving of vehicles

需要聲明的是，該模型不考慮變電站、線路的短時過載能力和轉供條件下的N-1安全校驗。通過該模型的應用，可以為變電站全停進行升級改造提供參考依據。

由于車輛行駛時前后軸距受荷載轉移、懸架運動等影響，后輪載荷相對于前輪載荷較大，后輪的側偏力更容易達到飽和狀態，且當車輛發生橫擺失穩時，后軸側滑造成的危害更大[15]，所以安全包絡線邊界受后輪側偏角決定。由魔術輪胎公式可知，后輪的最大側偏角為

式中：μ——路面摩擦系數；l——車輛軸距；Cαr——后輪等效側偏剛度。

為將安全包絡線約束與模型預測控制相結合，可以使用一個線性不等式約束實現。同時當車輛輪胎進入非線性區域時，并不一定導致車輛側向失穩，可以在線性不等式中加入松弛因子，對約束值進行適當的放寬，具體形式為

2.3.2 目標函數設計

在跟蹤過程中，應使實際軌跡與參考軌跡偏差盡可能小，轉角及轉角增量應在一定的范圍內，同時考慮安全行駛包絡線約束，構建出優化形式的線性模型預測控制的目標函數為

式中：Yref——二次規劃后的局部參考軌跡；umin——轉角上下邊界；Δumax，Δumin——轉角增量的上下邊界；ymax，ymin——系統輸出的約束。

國際數學教育大會（ICME）是代表數學教育界最高水平的學術會議，也是世界各國數學教育最新進展和成果的展示、交流平臺，迄今為止已召開了13屆．經過中國數學教育工作者的不懈努力，華東師范大學獲得2020年ICME-14的舉辦權，這是國際數學教育大會首次在中國舉辦．以此為契機，西南大學將承辦ICME-14少數民族數學教育衛星會議，旨在讓世界各國研究者相互交流本國民族數學教育問題，分享民族數學教育中的特色、經驗，探討民族數學教育發展的路徑，目前會議正在緊鑼密鼓的籌備中．

3 仿真驗證

為驗證提出的分層式MPC 規劃控制器的可靠性，基于汽車動力學仿真軟件MATLAB/Simulink和CarSim 建立聯合仿真模型，設置路面附著系數為0.85，對應車輛行駛路面；設置雙移線全局參考軌跡，并在雙移線路段上30 m 處設一個5.0 m×0.5 m 的靜止障礙物，驗證車輛在低、中、高速行駛時避障的有效性和穩定性。

仿真結果表明，當智能車輛以不同車速行駛時，為躲避可能碰撞的障礙物，提出的控制器犧牲了規劃局部軌跡與預先設定的全局參考軌跡之間的跟蹤精度。由圖4 可知，車輛以30、60、90 km/h 速度行駛時，2 種規劃控制器都使車輛順利避開障礙物且在最后都跟蹤上了全局參考軌跡，由圖5 可得，實際行駛軌跡與全局參考軌跡之間的最大偏差分別為0.50、0.69、0.92 m。結果表明提出的控制器在低、中、高速工況下都可以有效避開障礙物，且實際行駛軌跡與全局參考軌跡之間的橫向誤差也足夠小。

圖4 不同行駛速度下的車輛軌跡Fig.4 Moving trajectories of vehicle at various velocities

圖5 不同行駛速度下的橫向偏差Fig.5 Lateral deviation of vehicle at various velocities

在30、60、90 km/h 車速下，汽車避障行駛的質心側偏角變化情況如圖6 所示。結果顯示，車輛以不同車速進行避障跟蹤時，質心側偏角峰值分別為0.53°、0.02°和0.88°，均不超過1°，控制在一個較小的范圍內。

“王九，不要著急，慢慢的會有人來的，你瞧，這蓮蓬，不是大爺們的路數？咱們呆一會兒，就來玩個什么給爺們看看，玩得好，還愁爺們不賞三枚五枚？玩得好，大爺們回家去還會同家中學生說：嗨，王九趙四摔跤多扎實，六月天大日頭下扭著蹩著摟著，還不出汗！（他又輕輕的說）可不是，你就從不出汗，天那么熱，你不出汗也不累，好漢子！”

圖6 不同行駛速度下的質心側偏角Fig.6 Side-slip angles of vehicle at various velocities

圖7 顯示，隨著車速的提高，車輛在進行主動避障時，橫擺角速度值會相應提高。在汽車附著系數為0.85 的路面上，汽車以30 km/h 速度行駛時，橫擺角速度峰值為3.94 (°)/s；以60 km/h和90 km/h 速度行駛時，橫擺角速度的峰值分別為6.98 (°)/s 和10.45 (°)/s。上述橫擺角速度峰值均在15 (°)/s 的行業經驗限制范圍內[16]。實驗結果表明，改進后的控制器能夠保證車輛在避障行駛時的穩定性，避免車輛失穩的發生。

圖7 不同行駛速度下的橫擺角速度Fig.7 Yaw velocity of vehicle at various velocities

圖8 顯示，當汽車以30 km/h 和60 km/h 速度行駛時，最大側向加速度均不超過0.4g 的行業經驗限制[16]，但當車輛90 km/h 高速行駛時，側向加速度在躲避障礙物時會短暫超過0.4g，之后車輛又恢復穩定行駛 。

圖8 不同速度下的側向加速度Fig.8 Lateral acceleration of vehicle at various velocities

4 結語

針對車輛主動避障技術，提出了基于車輛點質量模型和車輛三自由度動力學模型建立的分層控制架構的規劃控制器。在上層局部軌跡規劃器中提出了改進了的避障懲罰函數，同時在下層軌跡跟蹤控制器中融入了車輛安全行駛包絡線約束，保證車輛避障行駛的穩定性。

通過MATLAB/Simulink 和CarSim 聯合仿真，得出主要結論：（1）提出的雙層MPC 規劃控制器可以成功避開障礙物，保證實際行駛軌跡與全局參考軌跡之間偏差較小，并且在規避障礙物后可以重新跟蹤全局參考軌跡；（2）車輛以低中高速進行避障行駛時，控制器可以確保車輛穩定參數均在允許范圍內，說明提出的規劃控制器可以有效保證車輛行駛的安全穩定性。