智能車輛動(dòng)態(tài)避障軌跡預(yù)測(cè)與跟蹤控制

趙海鵬，孫小松，段 敏

智能車輛動(dòng)態(tài)避障軌跡預(yù)測(cè)與跟蹤控制

趙海鵬1，孫小松*2，段 敏2

（1.遼寧理工職業(yè)大學(xué) 汽車學(xué)院，遼寧 錦州 121007；2.遼寧工業(yè)大學(xué) 汽車與交通工程學(xué)院，遼寧 錦州 121001）

為解決智能車輛動(dòng)態(tài)避障路徑預(yù)測(cè)及跟蹤問題，文章對(duì)動(dòng)態(tài)障礙進(jìn)行建模并基于三角形面積法引入避障約束，設(shè)計(jì)代價(jià)函數(shù)以滿足最優(yōu)預(yù)測(cè)軌跡輸出，同時(shí)設(shè)計(jì)引入側(cè)翻約束的非線性模型預(yù)測(cè)控制器進(jìn)行在線最優(yōu)路徑跟蹤。通過Matlab進(jìn)行基于結(jié)構(gòu)化道路下的避障軌跡預(yù)測(cè)與跟蹤控制仿真，仿真試驗(yàn)得出控制車輛在滿足避障約束等條件下，可以實(shí)現(xiàn)換道超車并到達(dá)指定目標(biāo)點(diǎn)，且加速度變化率（jerk）值小舒適性好。試驗(yàn)結(jié)果表明，預(yù)測(cè)軌跡理想、控制器準(zhǔn)確性高且具有一定的魯棒性。

智能車輛；非線性模型預(yù)測(cè)控制器；動(dòng)態(tài)避障；軌跡預(yù)測(cè)；跟蹤控制

到目前為止，對(duì)于預(yù)期應(yīng)用程序中的避障方法主要包括基于虛擬潛力和導(dǎo)航功能的方法[1]、基于圖搜索方法[2]、基于優(yōu)化的方法[3]。然而，前兩種方法在目前的公路應(yīng)用中無法達(dá)到動(dòng)力安全要求，第三種方法有無法承受計(jì)算負(fù)擔(dān)而引起非平滑導(dǎo)航命令的風(fēng)險(xiǎn)。而基于優(yōu)化的方法在依賴于嚴(yán)格的數(shù)學(xué)公式的同時(shí)對(duì)車輛動(dòng)態(tài)安全約束進(jìn)行系統(tǒng)處理，其既可以生成平滑的軌跡，又可以保證車輛的安全性。因此，優(yōu)化方法最適合當(dāng)下的應(yīng)用。文獻(xiàn)[4-5]成功的將模型預(yù)測(cè)控制（Model Predictive Control, MPC）技術(shù)應(yīng)用于車輛避障研究，但這些研究局限于遵循時(shí)刻的交通規(guī)則、無動(dòng)態(tài)障礙物進(jìn)入?yún)⒖悸窂降臈l件下。文獻(xiàn)[6]使用非線性模型預(yù)測(cè)控制（Nonlinear Model Predictive Control, NMPC）技術(shù)來避免非結(jié)構(gòu)環(huán)境中的靜止障礙物。然而，這些研究在問題的制定中將縱向速度始終考慮為恒定的，這樣不僅限制了運(yùn)動(dòng)性能，而且只能將性能限制在較小的障礙上。

本研究考慮了在結(jié)構(gòu)化環(huán)境下對(duì)動(dòng)態(tài)障礙物的處理并將動(dòng)態(tài)障礙信息融合到NMPC問題中實(shí)現(xiàn)實(shí)時(shí)避障，同時(shí)增加側(cè)翻約束保持車輛的安全性。

1 智能駕駛車輛與動(dòng)態(tài)障礙建模

1.1 汽車運(yùn)動(dòng)學(xué)模型

為確保基于結(jié)構(gòu)化道路下對(duì)車輛運(yùn)動(dòng)能力的準(zhǔn)確描述，假設(shè)路面平坦、車輪與路面保持良好滾動(dòng)接觸，忽略空氣阻力、地面?zhèn)认蚰Σ亮εc懸架結(jié)構(gòu)的影響，應(yīng)用雙自由度運(yùn)動(dòng)學(xué)模型對(duì)預(yù)測(cè)軌跡進(jìn)行優(yōu)化設(shè)計(jì)，如圖1所示。

圖1 運(yùn)動(dòng)學(xué)模型

慣性坐標(biāo)系下系統(tǒng)動(dòng)態(tài)方程約束為

式中，()為前輪偏轉(zhuǎn)角速度；()為沿軸的加速度；L為車寬；L為軸間距。

為使?fàn)顟B(tài)、控制變量始終處于解空間某流形限制內(nèi)，設(shè)置其允許作用區(qū)間為

式中，t≥0；max與max為線加速度、前輪轉(zhuǎn)角速度的最大幅值，max取值為1；max為低速場(chǎng)景下車速上限；max為最大前輪轉(zhuǎn)角。

1.2 車輛動(dòng)力學(xué)模型

假設(shè)輪胎工作在線性區(qū)域，且同軸左右輪胎側(cè)偏角相同，則

定義前后輪側(cè)偏角為

1.3 動(dòng)態(tài)障礙動(dòng)力學(xué)模型

建立動(dòng)態(tài)障礙動(dòng)力學(xué)模型為

式中，obs為行駛速度，obs為航向角。

將車輛模型與障礙模型整合為NMPC控制器系統(tǒng)模型并進(jìn)行歐拉離散積分得

式中，為二者狀態(tài)量集合；為控制量。

2 預(yù)測(cè)軌跡規(guī)劃

2.1 選擇參考點(diǎn)

取目標(biāo)點(diǎn)同一慣性坐標(biāo)系下同值不同值的點(diǎn)做為參考軌跡點(diǎn)。為避免車輛避障后出現(xiàn)回頭形式的情況發(fā)生，將目標(biāo)點(diǎn)納入?yún)⒖架壽E點(diǎn)尋找中。除此之外，取穿過車身模型后軸中心點(diǎn)平行于、軸交于參考軌跡且離目標(biāo)終點(diǎn)距離最近的點(diǎn)作為真正的起始參考軌跡點(diǎn)。

2.2 碰撞約束條件

由于二維平面中存在的控制主車與obs個(gè)矩形障礙物之間的碰撞必源于二者的頂點(diǎn)處。因此，可限制每一時(shí)刻下雙方頂點(diǎn)皆處于彼此的外部，則可避免碰撞。采用三角形面積法對(duì)約束建模，借此描述點(diǎn)=(,)處于某一矩形1～4外部。如圖2所示，若與矩形障礙物每相鄰兩個(gè)頂點(diǎn)間的三角形面積和大于矩形障礙物，則處于其外部，反之處于內(nèi)部，建立外部約束為

將式（10）記為（,1,2,3,4），據(jù)此建立第個(gè)障礙物每一時(shí)刻障礙物頂點(diǎn)V1～V4與車身頂點(diǎn)()～()的避碰約束條件

其中，∈[0,t], t為終止時(shí)刻；=1,...,obs。

圖2 描述點(diǎn)P在矩形外的三角形面積法

2.3 預(yù)測(cè)軌跡生成

為得出眾多硬約束條件下的最優(yōu)軌跡輸出，以積分與末值型性能指標(biāo)表征整個(gè)時(shí)域上指標(biāo)累計(jì)程度、終止時(shí)刻車輛運(yùn)動(dòng)狀態(tài)的最小化需求，設(shè)計(jì)如下代價(jià)函數(shù)：

（11）

代價(jià)函數(shù)中，1,2,3,4,5為各性能指標(biāo)對(duì)應(yīng)的權(quán)重系數(shù)；t為終止時(shí)刻；首項(xiàng)為車輛盡快抵近各航路點(diǎn)而設(shè)置；(x,y)為障礙物的幾何中心點(diǎn)；k＞0決定了車輛遠(yuǎn)離障礙物的期待程度；三、四項(xiàng)積分指標(biāo)決定了預(yù)測(cè)軌跡的平滑程度；末項(xiàng)為最終預(yù)測(cè)的車輛位置應(yīng)到達(dá)目標(biāo)點(diǎn)位置而設(shè)置，0為車輛當(dāng)前位置到目標(biāo)位置的距離，d為車輛最終位置到目標(biāo)位置的距離。

該最優(yōu)解問題可通過最優(yōu)控制策略搜索工具（Optimal Control Decision-searching Tool, OCDT）+積分概率度量（Integral Probability Metric, IPM）的數(shù)值優(yōu)化法求解。

3 路徑跟蹤控制

3.1 側(cè)翻安全約束

為防止車輪出現(xiàn)脫離地面的情況，對(duì)每個(gè)車輪所承受的垂直載荷施加始終大于等于1 100 N[9]的約束并對(duì)車輛的簧載、非簧載質(zhì)量進(jìn)行估算，得出前、后軸靜態(tài)垂直載荷

假設(shè)僅考慮車輛縱向的軸荷轉(zhuǎn)移，進(jìn)一步對(duì)各輪動(dòng)態(tài)下的垂直載荷約束為

其中，權(quán)重轉(zhuǎn)移系數(shù)λ,x,λ,yr,λ,yf可由式（2）計(jì)算推導(dǎo)。

3.2 目標(biāo)函數(shù)設(shè)計(jì)

令狀態(tài)量跟蹤優(yōu)化后預(yù)測(cè)軌跡的線路點(diǎn)，基于NMPC對(duì)移動(dòng)避障跟蹤控制器進(jìn)行設(shè)計(jì)，目標(biāo)函數(shù)為

式中，為較小正數(shù)；,為權(quán)重矩陣；前兩項(xiàng)表征懲罰車輛預(yù)測(cè)與參考下的狀態(tài)誤差、控制變化率以降低控制工作量；當(dāng)控制車輛各輪胎垂向載荷接近約束量時(shí)，第三項(xiàng)最小化預(yù)測(cè)軌跡橫向誤差與懲罰成本函數(shù)的同時(shí)避免了不必要的安全約束操作；最后一項(xiàng)表征避碰道路邊界，令其約束始終不小于1；,的取值計(jì)算源于文獻(xiàn)[9]。

使用多重打靶法將NMPC問題轉(zhuǎn)換為神經(jīng)語(yǔ)言程序?qū)W（Neuro-Linguistic Programming, NLP）問題后由MATLAB環(huán)境下的CasADI求解、開發(fā)。

4 仿真試驗(yàn)

4.1 仿真工況與主要參數(shù)

給無人車初始狀態(tài)（未處于參考軌跡上），使其在一段長(zhǎng)為120 m的結(jié)構(gòu)化道路下躲避前方三個(gè)不同車速行駛的障礙車輛后以1.5 m/s的車速到達(dá)目標(biāo)點(diǎn)，以試驗(yàn)控制效果。主要控制參數(shù)、整車主要參數(shù)如表1、表2所示。

表1 主要控制參數(shù)

表2 整車主要參數(shù)IZ/(kg·m2)

4.2 仿真結(jié)果分析

圖3為不同時(shí)刻的避障效果示意圖，可清晰的看到控制車輛狀態(tài)預(yù)測(cè)與所有移動(dòng)障礙狀態(tài)預(yù)測(cè)保持最小指定距離，即在滿足避障約束等條件下可以實(shí)現(xiàn)換道超車并到達(dá)指定目標(biāo)點(diǎn)。

圖3(a) 躲避一障礙

圖3 (c) 到達(dá)目標(biāo)點(diǎn)

圖3 (d) 圖例解析

圖4—圖5反映了車輛跟蹤預(yù)測(cè)規(guī)劃軌跡過程中橫擺角與質(zhì)心側(cè)偏角度的變化，可以看出，橫擺角波動(dòng)始終控制在30°以內(nèi)，較為穩(wěn)定，質(zhì)心側(cè)偏角最值在15°左右，體現(xiàn)了該控制過程中的橫擺穩(wěn)性。

圖4 橫擺角

圖5 質(zhì)心側(cè)偏角

圖6 縱向車速

圖6—圖7反映了車輛為應(yīng)對(duì)避障超車做出的縱向速度與加速度的變化。仿真車輛在不同位置接近障礙物區(qū)域時(shí)車速皆有所降低，其中車速最低降到了1.3 m/s，在找到無障礙物區(qū)域時(shí)加速，以最優(yōu)預(yù)測(cè)軌跡到達(dá)目標(biāo)點(diǎn)。除此之外，為使車輛在加速超過障礙物后以1.1 m/s的速度抵達(dá)目標(biāo)點(diǎn)，縱向加速度在110 m左右出現(xiàn)了大幅度的負(fù)增長(zhǎng)。這也導(dǎo)致了圖8中在100 m前舒適性保持良好的情況后，Jerk值波動(dòng)較大。

圖7 縱向加速度

圖8 Jerk變化

圖9 前輪轉(zhuǎn)角

圖10 轉(zhuǎn)向速率

圖9—圖10反映了車輛在跟蹤過程中前輪轉(zhuǎn)角與控制量（轉(zhuǎn)向速率）的變化，其中前輪轉(zhuǎn)角的波動(dòng)始終控制在30o的約束范圍內(nèi)。

5 結(jié)論

本文以汽車運(yùn)動(dòng)學(xué)模型與雙自由度模型為路徑預(yù)測(cè)與跟蹤模型對(duì)控制器進(jìn)行設(shè)計(jì)，基于結(jié)構(gòu)化道路下引入動(dòng)態(tài)障礙與側(cè)翻約束的同時(shí)通過設(shè)計(jì)最優(yōu)預(yù)測(cè)路徑的代價(jià)函數(shù)與NMPC跟蹤控制器。隨后通過Matlab仿真試驗(yàn)結(jié)果揭示設(shè)計(jì)的可行性。得出如下結(jié)論：

（1）本文所設(shè)計(jì)的控制器滿足車輛在換道超車過程中的約束條件。

（2）基于最優(yōu)軌跡預(yù)測(cè)過程中，預(yù)測(cè)軌跡較為精確，可滿足車輛安全到達(dá)目標(biāo)點(diǎn)。

（3）基于側(cè)翻約束下的NMPC跟蹤控制器具有較好的穩(wěn)定性，滿足橫擺穩(wěn)定性要求，且在盡可能保證乘客舒適性的前提下完成跟蹤任務(wù)。

[1] SHIMODA S,KURODA Y,IAGNEMMA K.High-speed Navigation of Unmanned Ground Vehicles on Uneven terrain Using Potential Fields[J].Robotica,2007,25(4): 84-92.

[2] KARAMAN S.Emilio Frazzoli.Sampling-based Motion Planning with Deterministicμ-Calculus Specifications [C].//Proceedings of Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference(6).北京:中國(guó)自動(dòng)化學(xué)會(huì),2009:8.

[3] A Potential Field-Based Model Predictive Path-Planning Controller for Autonomous Road Vehicles[J].Journal of Robotics & Machine Learning,2017:158-162.

[4] NILSSON J, FALCONE P, MOHAMMAD A, et al. Receding Horizon Maneuver Generation for Auto- mated Highway Driving[J].Control Engineering Practice, 2015:41.

[5] Shared Steering Control Using Safe Envelopes for Obstacle Avoidance and Vehicle Stability[J].Journal of Robotics & Machine Learning,2016:134-151.

[6] Combined Speed and Steering Control in High-Speed Autonomous Ground Vehicles for Obstacle Avoidance Using Model Predictive Control[J].Journal of Trans- portation,2017: 54-65.

[7] Sprung.Unsprung Mass Properties Determination wi- thout Vehicle Disassembly[J].SAE Transactions,1996: 105.

[8] KLOMP M,BRUZELIUS F,NIELSEN J,et al. Advan- ces in Dynamics of Vehicles on Roads and Tracks[M]. Springer:2020.

[9] LIU J C ,JAYAKUMAR P,JEFFREY L,et al. A Non- linear Model Predictive Control Formulation for Obstacle Avoidance in High-speed Autonomous Gro- und Vehicles in Unstructured Environments[J].Vehi- cle System Dynamics,2018,56(6):75-78.

[10] WANG H R,WANG Q D,CHEN W W,et al.Path Tracking Based on Model Predictive Control with Variable Predictive Horizon[J].Transactions of the Institute of Measurement and Control,2021,43(12): 34-39.

[11] LI M Y,ZHOU P,HE X M,et,al.Parallel Parking Path Planning and Tracking Control Based on Adaptive Algorithms[J].International Journal of Automotive Technology,2021,22(4):64-69.

Intelligent Vehicle Dynamic Obstacle Avoidance Trajectory Prediction and Tracking Control

ZHAO Haipeng1, SUN Xiaosong*2, DUAN Min2

( 1.College of Automotive Engineering, Liaoning Vocational University of Technology,Jinzhou 121007, China; 2.College of Automotive Engineering and Transportation Engineering,Liaoning University of Technology, Jinzhou 121001, China )

To solve the problem of intelligent vehicle dynamic obstacle avoidance path prediction and tracking, dynamic obstacles are modeled and obstacle avoidance constraints are introduced based on the triangle area method, cost functions are designed to meet the optimal predicted trajectory output, and a nonlinear model predictive controller (NMPC) that introduces rollover constraints is designed to track the online optimal path.The Matlab performs the obstacle avoidance trajectory prediction and tracking control simulation under the structured road. The simulation test shows that the control vehicle can change lanes and reach the designated target point when meeting the obstacle avoidance constraint, and the jerk value is small and has good comfort. The test results show that the predicted trajectory is ideal, the controller has high accuracy and certain robustness.

Intelligent vehicle; NMPC; Dynamic obstacle avoidance; Trajectory prediction; Tracking control

U461

A

1671-7988(2022)23-39-06

U461

A

1671-7988(2022)23-39-06

10.16638/j.cnki.1671-7988.2022.023.008

趙海鵬（1987—），男，碩士，講師，研究方向?yàn)樾履茉雌?，E-mail:2990534806@qq.com。

孫小松（1998—），男，碩士研究生，研究方向?yàn)檐囕v系統(tǒng)動(dòng)力學(xué)及控制，E-mail:1443270358@qq.com。

遼寧省教育廳科技大平臺(tái)項(xiàng)目（JP2017006）。