基于自適應預瞄路徑的非連續曲率軌跡跟蹤控制方法

李杭宇，劉冉冉，姜 宇，王文豪，李 峰，郭 威

（江蘇理工學院汽車與交通工程學院，江蘇 常州 213001）

0 引言

隨著人工智能的不斷發展，智能駕駛汽車成為現代科技的重要領域，其中智能駕駛的軌跡跟蹤性能決定了車輛在智能駕駛過程中的行駛穩定與安全［1］，因此，軌跡跟蹤控制層面逐漸成為當前的研究熱點［2］，軌跡跟蹤技術作為實現智能駕駛的基本要素之一，能夠使車輛在無人操縱的情況下，實現車輛按照規劃路徑行駛，因此，盡可能地提高智能駕駛過程中的跟蹤精度是該技術的重要目標［3］。

針對智能駕駛過程中存在的跟蹤精度不足問題，國內外研究人員對此進行了大量的探索，在現階段的研究中，大部分軌跡跟蹤控制算法基于預瞄理論或模型擬合理論［4］，其中主要有PID控制法［5］、最優預瞄控制法、純跟蹤控制法［6］、模型預測控制法（MPC）［7－8］及最優控制法（LQR）［9－11］等。其中最優預瞄控制法因計算簡單方便而廣泛應用于智能駕駛領域。

Hu等［12］根據整車運動響應特性的橫向偏差、道路邊界和道路邊界的優化函數來確定自適應預瞄時間，建立單點預瞄模型，實驗表明該方法對速度和黏附系數具有良好的效果和適應性，聶枝根等［13］模仿人類的駕駛習慣，采取預瞄前饋與車輛狀態反饋結合的方式設計控制器，實現以平穩且較小的控制量完成智能汽車動態軌跡的跟蹤動作；謝辰陽等［14］提出了一種預瞄距離自適應的分層式路徑跟蹤控制系統，根據車輛自身實時狀態進一步調整預瞄距離，提高了智能車輛的軌跡跟蹤精度；黃海洋等［15］以預瞄跟蹤誤差為目標函數，并結合LQR最優控制方法，提出了一種新型的基于多點預瞄的最優軌跡跟蹤控制策略，該方法進一步改進了道路模型的預瞄誤差和跟蹤算法的適應性及實時性；Liu等［16］根據迭代學習理論，設計了一種角度誤差的補償方法，對車輛前輪轉向不足的情況進行補償，提高了車輛側傾穩定性；邱少林等［17］根據預瞄控制理論，提出了一種最優預瞄軌跡跟蹤控制方法，得到了車輛最優轉向盤轉角，進一步提高了軌跡跟蹤精度；楊浩等［18］提出了一套路程預瞄曲率閾值理論，該理論可判斷目標路徑相對于預瞄點位置關系，建立了一種可根據路程預瞄的有效信息和當前車速改變預瞄距離的自適應跟蹤模型，研究結果顯示，建立的模型具有較高的路徑跟蹤精度和合理的速度控制策略；王全等［19］以智能車輛的轉向控制特點，設計了一種將模糊控制與預瞄控制相結合的軌跡跟蹤控制方法，試驗表明，該控制方法能夠實時調節預瞄點的個數，從而提高車輛在行駛時的跟蹤擬合性和魯棒性。

上述研究主要通過速度、預瞄時間等因素調節預瞄距離或預瞄誤差從而實現軌跡跟蹤控制，但忽略了真實工況下道路曲率、轉向穩定性對預瞄距離與行駛安全的影響，為此，根據最優預瞄控制，在考慮速度、曲率的綜合影響情況下，提出一種基于自適應預瞄路徑的非連續曲率軌跡跟蹤控制方法，采取自適應預瞄前饋和狀態反饋相結合的方式對方向盤轉角進行控制；首先根據車體信息建立車輛2自由度動力學模型，依據坐標轉換設計了一種改進的預瞄誤差模型，并依據轉向幾何原理設計了一種駕駛員轉向模型，同時引入橫擺角速度、前輪轉角約束，保證車輛運動過程中的穩定性與安全性；其次以預瞄-跟隨理論建立了自適應預瞄控制器，通過自適應預瞄距離模塊求解實時預瞄距離，并以五次多項式方程擬合方法得到預瞄路徑；最后以Matlab／Simulink設計期望軌跡模塊、軌跡跟蹤控制模塊，在Carsim中設定車輛2自由度動力學參數，并于Prescan中構建實時仿真工況、駕駛場景與傳感器模型，驗證控制器跟蹤效果。

1 自適應預瞄跟蹤控制系統

自適應預瞄軌跡跟蹤控制系統由車輛模型、預瞄誤差模型、自適應預瞄控制器、駕駛員轉向模型組成，整車控制系統如圖1所示，預設期望路徑的參考坐標（xr，yr），把質心坐標作為車體坐標，以車輛實時位姿參數為輸入，通過自適應預瞄距離模型計算預瞄距離L，并采用五次多項式擬合模塊，得到車輛擬合后預瞄路徑坐標（x，y）及預瞄點位置道路曲率kd，以（xr，yr）、（x，y）、kd、預瞄點航向角θd及預瞄距離L作為預瞄誤差模型的輸入變量，計算參考坐標與預瞄路徑坐標的跟蹤誤差ey，同時以ey作為駕駛員轉向模型的輸入量，計算前輪轉角δf，并輸出給車輛模型，車輛通過轉動方向盤調整轉角從而實現路徑的跟蹤，最后，根據實時車輛位姿參數，反饋調節自適應預瞄控制器，再進一步作用于預瞄誤差模型，改善車輛跟蹤效果。

由于汽車運動過程中是一個復雜的非線性時變系統，涉及到車輛橫縱向及橫擺特性，在實現車聯跟蹤控制的安全與穩定性條件下，忽略以下因素：1）車輛載荷變化及懸架運動的影響；2）空氣動力及車輛的垂直方向上運動的影響。因此采用簡化的2自由度動力學模型，如圖2所示。

圖2 2自由度動力學模型

圖2中：O點為車輛質心；v為質心速度；vx、vy分別為車輛縱向、側向速度；δf為車輛前輪轉角；lr、lf分別為質心到前軸、后軸的距離；αf、αr分別為車輛前、后輪側偏角；β為質心側偏角；vf、vr分別為車輛前后輪運動速度；Fyf、Fyr分別為車輛前、后輪胎側偏力。假設被控車輛以前輪作為驅動輪，考慮輪胎側偏力，則2自由度模型的動力學方程可以表示為：

式中：m為整車質量；ay為側向加速度；φ為橫擺角；I為繞軸的轉動慣量；M為橫擺力矩。故根據車輛輪胎特性［20］有：

式中：Cf、Cr分別為前、后輪側偏剛度?？紤]到模型中前輪轉角較小，因此cosδf≈1，且根據剛體運動關系，αf與αr分別為：

式中：表示車輛橫擺角速度。聯立式（1）—式（6）可得：

式中，K為穩定性系數。

2 預瞄誤差模型

為了進一步分析車輛動力學特性，設計了一種改進的預瞄誤差模型，如圖3所示，以XOY坐標系為基礎，取道路中心紅色虛線作為期望軌跡，黑色實線為車輛實際運動軌跡；根據車輛位置矢量關系及位姿狀態設計預瞄誤差方程；其次，基于運動幾何原理構建駕駛員轉向模型得到前輪轉角，實現車輛跟蹤控制。假設車輛此時運動位置于A點處時，根據軌跡投影規則，此時車輛投影點為B點；設預瞄距離為L，則車輛預瞄點為圖3中C點，將點C投影至期望軌跡上得到點D。分別以點A、B和點D為原點，建立Frenet坐標系，記法向單位向量分別為Νa、Νb、Νd，記切向單位向量分別為Va、Vb、Vd；以Xa、Xb、Xd分別表示點A、B、D處向量。

圖3 預瞄誤差模型

圖3中：θa表示車輛航向角；θb表示B點切線和XOY坐標系橫軸夾角；ey表示點C與點D間的距離即橫向誤差；L表示點A與點C間的距離即預瞄距離；v表示車速；s（t）表示投影點B在期望軌跡上的位移。根據車速v與車體位置A點的位置關系有：

式（10）對時間t求導可得：

同時點A、B、C、D間的矢量關系有：

對式（12）兩邊同乘Νd，且有Νd·Νd＝1，可得：

進一步求導：

根據D點矢量關系可得：

將式（11）、式（15）代入式（14）可得：

由于Frenet坐標系下有：

式中，kd表示D點位置道路曲率。將式（13）、（17）代入式（16）同時可得：

由于VdΝd＝0則有：

由于θa＝β＋φ，可得：

式中，θa－θd表示向量Va與Vd的夾角，且當車輛在道路上平穩行駛時β較小，即有航向偏差eφ＝θa－θd。根據曲率與航向角的關系可知：

式中，ka表示A點位置道路曲率。當智能車輛在通常的結構化道路上行駛時，可知φ－θd≈eφ且數值較小，可近似取eφ＝sineφ、coseφ＝1，并將式（23）代入式（22），可得預瞄誤差模型為：

式中，L為預瞄距離。為進一步分析ey與δf的關系，根據車輛幾何運動原理建立了一種駕駛員轉向模型，如圖4所示。

圖4 駕駛員轉向模型

圖4中：紅色XGY為Frenet坐標系；θ為轉彎圓心角；G點為車輛當前時刻的質心位置坐標點；Q點為車輛預瞄點；F點為Q點在期望軌跡上的投影點；E點為直線FG的中點。當車輛行駛時，假設被控的橫擺角速度在轉彎瞬間保持不變，即此刻汽車運動方向與期望路徑相切，經tp時間后，汽車將到達理想的質心位置即點F，即有：

由幾何關系對式（25）進一步推導可得：

聯立式（7）、（8）、（9）、（26）可得理想的前輪轉角方程為：

為了保證車輛的側向安全性，考慮約束的車輛橫擺角速度定義為［21］：

3 自適應預瞄控制器設計

3.1 自適應預瞄控制原理圖

自適應預瞄控制器的思想是以車輛實時位姿參數為輸入，通過自適應預瞄距離模塊實現預瞄距離L實時調整，并根據L進一步結合五次多項式路徑擬合模塊實現自適應預瞄路徑，并控制車輛跟隨預瞄路徑完成跟蹤過程?？紤]到不同路面工況下車輛運動的安全性，基于路面附著及道路曲率等因素對車速進行了限制，防止車輛因轉向導致車輛失穩。原理如圖5所示。

圖5 自適應預瞄控制原理

圖5中：vc表示車輛安全速度；λ表示駕駛風格影響系數；μ表示路面附著系數；ka表示當前位置道路曲率；vc＝F（λ，μ）為直道安全速度限制函數；vc＝F（λ，μ，ka）表示彎道安全速度限制函數；L＝F（vc，ka）表示自適應預瞄距離函數。

3.2 五次多項式路徑擬合

為模仿人類對運動路徑的預見性，以五次多項式方程［22］對未來路徑進行擬合。針對車輛路徑擬合場景，五次多項式求解問題通?？梢员硎緸榻涍^多個點（xi，yi）（i＝1，2，3，…）且xi處五次多項式斜率為wi。待擬合的五次多項式可用式（30）表示，其在任意x處的斜率可表示為式（31）所示，式中at（t＝0，1，…5）為五次多項式方程待求解系數。

進一步：

式（32）為五次多項式方程需要滿足的不等式約束，點（xz，yz）（z＝1，2，3，…）和（xj，yj）（j＝1，2，3，…）分別代表左右道路邊界線上的點；按位置偏差與斜率偏差平方和最小的原則，求解五次多項式路徑擬合問題，則五次多項式擬合問題轉化為如式（33）的優化問題。

式（33）取得極小值時滿足如下形式，即優化問題的導數為0，式（34）中（t＝0，1，…，5）為G對五次多項式系數的導數。

路徑擬合計算步驟如下：①確定當前車輛位置坐標，該坐標即為車輛質心坐標即P0（0，0），當前車輛處切線斜率為w0＝0；②根據預瞄距離L（L＝P0Pd）及此時質心坐標確定期望路徑上的預瞄點，即圖6中點Pd（xd，yd）所示，進一步確定預瞄點Pd位置處的斜率wd；③直接對式（34）進行求解，將P0、Pd、w0和wd代入式（34）可直接求解得到擬合路徑的五次多項式（30）的系數。經過上述步驟從而得到的擬合后的軌跡方程，確定預瞄路徑如圖6中紅色實線所示。則擬合軌跡在車輛預瞄點Pd的曲率及航向角為：

圖6 預瞄路徑擬合原理

3.3 自適應預瞄距離模型

在智能車輛的預瞄控制中，預瞄距離直接影響著智能駕駛過程中車輛的跟蹤精度及穩定性。通過對比不同預瞄距離對控制器的影響，發現預瞄距離過短會導致車輛轉角頻繁變化，導致車輛的行駛穩定性［23］變差，預瞄距離過長會使系統執行與現實情況下不匹配的控制指令，使控制器性能變差［24］，因此合理的預瞄距離是實現較高軌跡跟蹤精度的關鍵，而現如今大多數研究中只考慮到車輛自身姿態對預瞄距離的影響，并未考慮安全速度范圍及道路曲率因素，因此，引入安全車速與道路曲率相結合的方法實現自適應預瞄距離。當道路曲率較小時，預瞄距離受安全車速、道路曲率的影響，且隨安全車速的增大而增大、道路曲率增大而減??；當道路曲率較大時，此時預瞄距離與安全制動距離相關。各階段具體過程如式（36）所示。

式中：vc表示安全車速；ka表示當前道路曲率；ε表示經驗系數；amax表示智能汽車在保證駕駛舒適性條件下的最大制動加速度；τ表示車輛系統反應時間，取0.1 s。當車輛在路面上行駛時，為了保證車輛運動的安全性，需根據不同的路面工況，設定安全車速限制，如式（37）、（42）所示。

3.3.1 直道安全車速限制

當被控車輛在直道上運動時即ka＝0，路面附著系數μ對車速的影響較大，因此，根據μ的高中低程度，并考慮不同駕駛風格對車速的影響，直道安全車速表達式為：

式中：vh為期望高速；vm為期望中速；vs為期望低速；λ為駕駛風格系數。

3.3.2 彎道安全車速限制

車輛于彎曲道路上運動時，即ka＞0，彎道內車速達到最大值則車輛離心力等于最大地面附著力。即有：

式中：g為重力常數，取10 N／kg，縱向力Fx與橫向力Fy可分別表示為：

將式（38）—式（40）聯立可得最高車速表達式為：

考慮不同的駕駛風格，并保證一定的安全系數，彎道安全車速表達式為：

式中：kmin表示最小道路曲率；kmax表示最大道路曲率；ksafe表示為安全系數。

4 仿真實驗與結果分析

為了充分驗證基于自適應預瞄路徑的非連續曲率軌跡跟蹤控制策略的有效性，選擇小型轎車作為被控汽車，車輛模型參數見表1所示。

表1 車輛模型參數

其中Carsim用于實現整車動力學模型和虛擬場景設定，Prescan用于搭載傳感器模型，Matlab／Simulink軌跡跟蹤模塊作為控制中樞；引入駕駛員風格系數λ對行車的影響，并分別選取雙移線工況、非連續曲率工況、連續換道工況為期望軌跡，對所設計控制器即自適應-曲率-速度控制器進行仿真研究。在上述工況下進行自適應曲率-速度控制器下的3種速度對比實驗及所設計控制器、自適應-時間控制器［25］、自適應-速度控制器［26］對比實驗。觀測4種控制器下的跟蹤誤差、前輪轉角、橫擺角速度變化情況。通過這3個工況，進一步驗證汽車在復雜道路環境下的跟蹤能力、穩定性及適應性。

4.1 雙移線工況-多速度對比

為了驗證自適應曲率-速度控制器在雙移線軌跡下對不同行駛速度的適應性，取λ＝0.8即駕駛員在采取平穩型駕駛規則，同時以行駛速度25、35、45 km／h進行對比實驗，3種速度下軌跡對比如圖7（a）所示。

圖7 雙移線多速度仿真對比

由圖7（b）可知，在25 km／h速度下，當t＝17 s時，跟蹤誤差最大值為0.183 m，在35 km／h速度下，當t＝14 s時，跟蹤誤差最大值為0.224 m，在45 km／h速度下，當t＝6 s時，跟蹤誤差最大值為0.288 m，由此可知，當車速越低時系統跟蹤精度越高，由圖7（c）與圖7（d）可知，25 km／h時車輛橫擺角速度與前輪轉角最大值分別為13（°）／s、5.5°，35 km／h時車輛橫擺角速度與前輪轉角最大值為14（°）／s、4°，45 km／h時車輛橫擺角速度與前輪轉角最大值分別為16（°）／s、4°，均處于在合理范圍內，驗證了所設計控制器在不同車速條件下均有著良好的適應性。

4.2 雙移線工況-多控制器對比

為了驗證自適應-曲率-速度控制器下車輛在雙移線工況的車輛跟蹤能力，將所設計控制器與自適應-時間控制器、自適應-速度控制器進行對比實驗，取車速為45 km／h、λ＝0.8。車輛在笛卡爾坐標系下的實際軌跡如圖8（a）所示。

圖8 雙移線多控制器仿真對比

由圖8（b）可知，當t＝12 s時，設計控制器達到最大跟蹤誤差0.288 m，可知該軌跡下所設計控制器跟蹤性能明顯優于其他控制器，而其他控制器最大跟蹤誤差達0.648 m，跟蹤精度提升55.34%，由圖8（c）、圖8（d）可知，所設計控制器橫擺角速度最大值為16（°）／s，前輪轉角最大值為4°，均處于合理范圍內，保證了被控車輛的行駛穩定性與安全性，進一步驗證了控制器的優越性。

4.3 非連續曲率工況-多速度對比

為驗證所設計的控制器在非連續曲率道路工況下對不同行駛速度的適應性，該工況下選取激進型駕駛規則即取駕駛風格影響系數λ＝1.25，分別以行駛速度25、35、45 km／h進行對比實驗，3種速度軌跡如圖9（a）所示，由圖9（b）可知，25 km／h條件下車輛最大跟蹤誤差為0.272 m，35 km／h條件下車輛最大跟蹤誤差為0.242 m，45 km／h條件下車輛最大跟蹤誤差為0.325 m，由圖9（c）可知，該工況下道路曲率呈階躍性變化，由圖9（d）、圖9（e）可知，所設計控制器3種速度下的最大橫擺角速度為38（°）／s，前輪轉角最大值為13°。由上述分析知，不同速度條件下，智能汽車依然保持著良好的跟蹤精度，且被控車輛橫擺角速度與前輪轉角均保持安全范圍內，進一步驗證了曲率-速度控制器在非連續曲率道路工況下對不同車速下的適應性。

圖9 非連續曲率多速度仿真對比

4.4 非連續曲率工況-多控制器對比

為了進一步研究控制策略應對道路曲率不連續惡劣路況的能力，設計如圖10（a）軌跡工況，此時，車速為45 km／h，且取駕駛風格影響系數λ＝1.25。由圖10（b）可知，當t＝17 s時，自適應-曲率-速度控制器最大跟蹤誤差為0.325 m，其他控制器最大誤差達到0.952 m，跟蹤精度提升65.86%。

圖10 非連續曲率多控制器仿真對比

由圖10（c）、圖10（d）可知，整個軌跡跟蹤過程中，自適應-曲率-速度控制器橫擺角速度最大值為38（°）／s，前轉角最大值為12°，均處于合理范圍內，保證了車輛行駛過程中較高的跟蹤精度與穩定性、安全性。

4.5 連續換道工況對比

為充分探究控制器在真實道路環境下的跟蹤能力，取駕駛風格影響系數λ＝1.25，在連續換道工況下，將3個控制器跟蹤效果進行對比，車輛運動軌跡如圖11（a）所示，可知自適應-曲率-速度控制器跟蹤軌跡最貼近期望軌跡，該工況下道路曲率大小在0～0.1劇烈變化如圖11（b）所示。由圖11（c）可知當t＝9 s時所設計控制器的跟蹤誤差達到最大值0.15 m，其他控制器最大跟蹤誤差達0.28 m，跟蹤精度提升46.00%。由圖11（d）、（e）可知，車輛運動過程中，橫擺角速度及前輪轉角最大值分別為5（°）／s、8°，均為較小量且在安全范圍內。

圖11 連續換道對比

以上3個工況的試驗結果表明，所設計的控制策略能根據外界道路信息迅速調整控制量，應對外界工況激烈變化，同時以較高精度跟蹤期望軌跡，且保證了跟蹤過程中汽車的行駛穩定性、安全性。

5 結論

1）基于自適應預瞄路徑的控制方法考慮了安全速度約束的條件下道路曲率與速度的綜合影響，提高了車輛跟蹤過程中的跟蹤精度；同時引入了真實條件下的車輛橫擺角速度、前輪轉角約束，進一步保證了運動穩定性與安全性。

2）以雙移線、非連續曲率、連續換道軌跡為實驗工況，Carsim／Simulink／Prescan聯合仿真平臺研究結果表明，自適應-曲率-速度控制器對跟蹤精度分別提升55.34%、65.86%、46.00%；并在不同的行駛速度下驗證了所設計的控制器的適應性。

3）自適應預瞄控制器在設計時未考慮車輛的機械轉向系統的轉向不足特性及系統穩態誤差對系統的影響，下一步將建立相應的反饋模型法，以提高控制系統的魯棒性。