基于時滯補償?shù)募冏粉櫩刂祁A(yù)瞄距離優(yōu)化方法

湯新華，成宇慶，潘樹國，鮑亞川，黃 璐，蔚保國

（1.東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096：2.微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096：3.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點實驗室，石家莊 050081）

近年來，隨著交通迅速發(fā)展以及汽車數(shù)量的持續(xù)上升，道路擁堵以及疲勞駕駛等人為因素造成的交通事故日益增加，作為未來改善道路交通與駕駛安全最具潛力的技術(shù)，自動駕駛技術(shù)的研發(fā)備受矚目。自動駕駛技術(shù)主要包括導(dǎo)航定位、環(huán)境感知、決策規(guī)劃和控制執(zhí)行四大技術(shù)模塊[1]，如圖1 所示。車輛通過全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System,GNSS）/慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）等組合導(dǎo)航系統(tǒng)獲取自身車輛位姿信息[2]，并利用視覺、激光雷達等相關(guān)傳感器對環(huán)境信息進行采集與處理。綜合位置信息和環(huán)境信息，車輛根據(jù)目標(biāo)任務(wù)完成規(guī)劃、自主避障等決策，并下發(fā)至車輛執(zhí)行機構(gòu)[3]，其中規(guī)劃根據(jù)是否包含時間信息分為路徑規(guī)劃和軌跡規(guī)劃。控制執(zhí)行是在導(dǎo)航定位、環(huán)境感知、決策規(guī)劃的基礎(chǔ)之上，根據(jù)規(guī)劃決策的目標(biāo)軌跡，對車輛進行縱向控制和橫向控制，使車輛沿著規(guī)劃的目標(biāo)軌跡行駛，保證車輛行駛的安全性與操縱穩(wěn)定性[4]。

圖1 自動駕駛系統(tǒng)結(jié)構(gòu)圖Fig.1 Structure diagram of autopilot system

目前主流的控制算法包括純追蹤算法（Pure Pursuit,PP）、Stanley 算法、比例積分微分（Proportion Integration Differentiation,PID）算法、線性二次型調(diào)節(jié)器（Linear Quadratic Regulator,LQR）算法和模型預(yù)測控制（Model Predictive Control,MPC）算法等。純追蹤算法由于具有幾何模型簡單、魯棒性高且算法實現(xiàn)復(fù)雜度低的特點，被廣泛用于解決無人駕駛領(lǐng)域中路徑跟蹤問題，但在實際應(yīng)用中，純追蹤算法需要結(jié)合具體應(yīng)用場景進行進一步優(yōu)化以保證控制精度和可靠性。其中，預(yù)瞄距離的選擇會在很大程度上影響純追蹤算法的性能。預(yù)瞄距離越小，對目標(biāo)路徑的收斂速度就越快，從而能夠更快地到達目標(biāo)點，但容易引起振蕩現(xiàn)象，導(dǎo)致跟蹤精度低、車輛舒適度差。預(yù)瞄距離較大可以避免明顯的振蕩現(xiàn)象，但在初始跟蹤階段或轉(zhuǎn)彎階段存在較大的跟蹤誤差。因此，優(yōu)化預(yù)瞄距離是保證純追蹤算法在實際應(yīng)用中可靠運行的重要因素。目前已有大量文獻針對預(yù)瞄距離優(yōu)化做了相關(guān)研究。其中，Horváth E 等人提出了一種多目標(biāo)優(yōu)化純追蹤，利用車輛運動學(xué)約束計算離散曲率，從而選擇最佳轉(zhuǎn)向角[5]。Chen Y 等人提出了一種利用PID 控制器和低通濾波器對純追蹤控制器的轉(zhuǎn)向角進行擬合補償，弱化預(yù)瞄距離對跟蹤精度的影響[6]。Ibrahim A 等人將路徑曲率分為直線和圓弧部分，提取曲線信息自適應(yīng)修正預(yù)瞄距離[7]。唐小濤等人提出了模糊自適應(yīng)純追蹤，通過先驗路徑特性、速度和跟蹤誤差自適應(yīng)地調(diào)整預(yù)瞄距離[8]。另一方面，也有部分學(xué)者針對車輛模型時延優(yōu)化做出了相關(guān)研究，Tao H 等人通過更新卡爾曼濾波器改變GPS 接收信號，解決了延時反饋和慢反饋的問題[9]。趙建輝等人通過預(yù)測車輛延遲后的位置和方向，減小延遲對路徑跟蹤控制精度的影響[10]。Pornthiwa C 等人通過改變MPC 內(nèi)部結(jié)構(gòu)，輸出新的轉(zhuǎn)向角輸出[11]。Xu S 等人提出了一種考慮前向狀態(tài)預(yù)測器和延遲增強的優(yōu)化算法，利用預(yù)瞄控制理論解決時滯問題[12]。

綜上所述，針對預(yù)瞄距離優(yōu)化的研究缺少對實際車輛模型時延的考慮，本文結(jié)合實際車輛分析控制回路時延和轉(zhuǎn)向系統(tǒng)遲滯對車輛模型及其穩(wěn)定性的影響，提出了一種基于時滯補償?shù)募冏粉欘A(yù)瞄距離優(yōu)化方法，圖2 為控制算法的總流程圖。通過實際的車輛實驗，驗證了基于時滯補償?shù)募冏粉欘A(yù)瞄距離優(yōu)化算法與實際路徑跟蹤效果的一致性和可靠性。

圖2 控制總流程圖Fig.2 Flow chart of the autonomous control process

1 純追蹤及ABMSSA 算法

1.1 純追蹤算法

純追蹤算法及其變式是無人駕駛領(lǐng)域中路徑跟蹤問題最常用的方法，該方法在幾何層面上計算行駛路徑上車輛后軸與前方目標(biāo)點的圓弧曲率，根據(jù)后軸位置到所需路徑的預(yù)瞄距離dL確定目標(biāo)點，通過目標(biāo)點位置以及車輛航向矢量和超前矢量之間的角度α確定車輛的轉(zhuǎn)向角δ，達到路徑追蹤的目的。純追蹤的示意圖如圖3 所示。

圖3 純追蹤示意圖Fig.3 Diagram of the pure pursuit algorithm

圖3 中，R是車輛轉(zhuǎn)彎半徑，d是橫向誤差，(gx,gy)是目標(biāo)點。

應(yīng)用正弦定理可以得到α與δ的關(guān)系：

以γ表示圓弧的曲率，得到曲率表達式：

根據(jù)車輛幾何模型，轉(zhuǎn)向角可寫為：

其中，L為車輛軸距。

利用式(2)(3)，純追蹤的控制律可以寫作：

其中，δu為純追蹤期望轉(zhuǎn)向角。從式(4)可以看出，預(yù)瞄距離Ld影響純追蹤的控制精度。若預(yù)瞄距離過大，則車輛從某一偏離車道位置到達目標(biāo)點的響應(yīng)時間變長，路徑跟蹤偏差變大；若預(yù)瞄距離過小，會使車輛駕駛時發(fā)生振蕩，甚至?xí)管囕v失控。因此，在利用純追蹤進行路徑跟蹤時，預(yù)瞄距離決定其跟蹤精度和跟蹤效果，選取合適的預(yù)瞄距離尤為重要。

在一般工況下，使用純追蹤預(yù)瞄距離與速度一次線性關(guān)系的經(jīng)驗公式[5]：

其中，k為比例系數(shù)；v為車輛速度；Lf為初始預(yù)瞄距離或預(yù)瞄距離下限。

1.2 ABMSSA 算法

ABMSSA（Adaptive Brownian Motion Salp Swarm Algorithm）是一種改進的樽海鞘智能優(yōu)化算法，通過在SSA 的領(lǐng)導(dǎo)者位置更新公式中引入布朗運動，在SSA 跟隨者位置更新公式中增加兩個自適應(yīng)權(quán)重，使SSA 的全局優(yōu)化能力和局部優(yōu)化能力得到聯(lián)合調(diào)整，加快了ABMSSA 算法的收斂速度，同時滿足了路徑跟蹤精度和收斂速度的要求[13]。

布朗運動N(0,h) 是粒子的隨機運動機制，粒子運動的步長h是正態(tài)分布的。對于一維的布朗運動，假設(shè)Wt是概率空間上布朗運動實值過程，ΔWt是增量，t為當(dāng)前時刻。布朗運動實值更新公式為：

領(lǐng)導(dǎo)者的更新公式為：

圖4 c1 變化曲線圖Fig.4 Plot ofthevariation of parameter c1

圖5 直線路徑時滯系統(tǒng)穩(wěn)定閾值Fig.5 Stability threshold of straight path time-delay systems

自適應(yīng)權(quán)重的更新公式為：

跟隨者的位置更新公式為：

為使預(yù)瞄距離最優(yōu)化，實現(xiàn)不同車速下預(yù)瞄距離自適應(yīng)地尋優(yōu)，將預(yù)瞄距離設(shè)置為樽海鞘種群。綜合考慮車輛的跟蹤精度和車輛舒適性，設(shè)計目標(biāo)函數(shù)為：

其中，ai(i=1,2)為權(quán)重系數(shù)；J1為車輛跟蹤誤差指標(biāo)；J2為車輛航向誤差指標(biāo)。J1、J2的計算公式為：

其中，xi和yi分別為車輛在第i時刻的橫向位置和縱向位置；和分別車輛在第i時刻的參考橫向位置和參考縱向位置；θi和θi-1分別車輛在第i時刻和第i-1時刻的車輛航向角。在本文中，a1和a2分別設(shè)置為0.7 和0.3。

通過ABMSSA 算法尋找使目標(biāo)函數(shù)JABMSSA達到最小的預(yù)瞄距離，即為該速度該工況下的最優(yōu)預(yù)瞄距離。

2 基于時滯補償?shù)念A(yù)瞄距離優(yōu)化

2.1 時滯車輛模型優(yōu)化

在實際車輛平臺運行過程中，一方面由于離散控制系統(tǒng)狀態(tài)觀測與閉環(huán)控制存在周期時延，導(dǎo)致車輛執(zhí)行機構(gòu)存在一定的動作滯后，即指令由決策層到執(zhí)行層不會被瞬間執(zhí)行，而是存在一定的控制時延；另一方面由于機械間隙、輪胎側(cè)向力等因素的存在，車輛轉(zhuǎn)向機構(gòu)系統(tǒng)接收到指令后，在采樣時間內(nèi)無法準(zhǔn)確輸出指令轉(zhuǎn)向角，具有一定的慣性遲滯。

本文引入控制回路時延和轉(zhuǎn)向系統(tǒng)遲滯，對車輛模型進行更新。車輛方向盤由步進電機驅(qū)動，整個轉(zhuǎn)向系統(tǒng)可以看作為一階系統(tǒng)[14]：

其中，T為轉(zhuǎn)向系統(tǒng)時間常數(shù)。

引入控制回路時延，將角度轉(zhuǎn)動視為微小轉(zhuǎn)動，則車輛模型更新為：

2.2 時滯補償模型的穩(wěn)定閾值選擇

在時滯補償模型下，選擇合適的ABMSSA 尋優(yōu)下界閾值lb，對模型的穩(wěn)定性尤為重要。本文對時滯補償模型直線路徑穩(wěn)定性（γ=0 時）進行了分析與選擇[14]。時滯補償模型在平衡位置(x=0，φ=)進行線性化，可以從線性系統(tǒng)的穩(wěn)定性中確定非線性系統(tǒng)在平衡時的局部穩(wěn)定性：

設(shè)G(s) 為時滯補償模型系統(tǒng)特征多項式[14]，則：

令sj=ω，整理非線性方程組得到：

3 實驗驗證

為了驗證基于時滯補償?shù)募冏粉櫩刂祁A(yù)瞄距離優(yōu)化的有效性，本節(jié)分為兩部分：1）不考慮時滯補償，使用傳統(tǒng)車輛模型對比分析ABMSSA 算法，得到預(yù)瞄距離的最優(yōu)性，并通過擬合得到傳統(tǒng)車輛模型預(yù)瞄距離與速度的線性關(guān)系；2）使用時滯補償模型，通過擬合得到優(yōu)化后的車輛模型預(yù)瞄距離與速度的線性關(guān)系，并進行實車驗證。設(shè)定車速為1 m/s，車輛參考路徑共計 200 個參考軌跡點，車輛初始位置為(0 m,0.15 m)，航向角為北偏東5°。仿真參考路徑設(shè)置為直線路徑，如圖6 所示。

圖6 仿真實驗參考路徑Fig.6 Reference path in the simulation experiment

3.1 不考慮時滯補償

按照式(7)和式(9)對樽海鞘智能優(yōu)化算法進行更新，比較適應(yīng)度值。利用ABMSSA 算法優(yōu)化得到最優(yōu)預(yù)瞄距離為Ld=0.3695 m，為驗證Ld為最優(yōu)預(yù)瞄距離，分別取Ld=0.3695 m、0.15 m、1 m 得到不同預(yù)瞄距離下路徑跟蹤和橫向誤差對比，如圖7 所示。

圖7 無時滯補償預(yù)瞄距離下路徑跟蹤和橫向誤差對比Fig.7 Comparison of path tracking and lateral errors using different look-ahead distance with no time delay compensation

1）圖7(a)中，當(dāng)所選預(yù)瞄距離在Ld=0.15m 時，瞬時收斂速度快，但整體路徑跟蹤控制表現(xiàn)出不穩(wěn)定的震蕩現(xiàn)象。

2）圖7(b)中，預(yù)瞄距離Ld=0.3695 m 與Ld=1 m相比，收斂速度更快，總體路徑誤差最小。

采用ABMSSA 算法對不同速度、不同路況下的預(yù)瞄距離進行最小二乘擬合，得到預(yù)瞄距離和速度的一次線性關(guān)系。為了得到合理的最優(yōu)預(yù)瞄曲線，本節(jié)速度設(shè)置為定速，即初始速度和目標(biāo)速度一致，不設(shè)置加速度變量。速度作為自變量分別取0.6 m/s、0.8 m/s、1.0 m/s、1.2 m/s、1.4 m/s，得到無時滯補償?shù)念A(yù)瞄距離擬合曲線如圖8 所示。

圖8 無時滯補償?shù)念A(yù)瞄距離擬合曲線Fig.8 Fitting curve of the relationship between look-ahead distance and velocity with no delay compensation

采用最小二乘法直線擬合得到無時滯補償?shù)念A(yù)瞄距離Ld與速度v的線性關(guān)系為：

3.2 考慮時滯補償

3.2.1 仿真實驗

在實驗車輛平臺中，轉(zhuǎn)向系統(tǒng)遲滯T約為0.25 s，控制回路延時τ約為0.4 s，在該系統(tǒng)下選取穩(wěn)定閾值預(yù)瞄下限lb=1.1511v，利用ABMSSA 算法優(yōu)化得到最優(yōu)預(yù)瞄距離為Ld=2.7176 m。為驗證Ld為最優(yōu)預(yù)瞄距離，分別取Ld=2.7176 m、2m、3 m 得到不同預(yù)瞄距離下的路徑跟蹤和橫向誤差對比，如圖9 所示。

圖9 時滯補償預(yù)瞄距離下路徑跟蹤和橫向誤差對比Fig.9 Comparison of path tracking and lateral errors using different look-ahead distance with time delay compensation

1）圖9(a)中，當(dāng)預(yù)瞄距離為Ld=2 m時，具有較快的上升速度，但收斂速度慢，穩(wěn)定性差。

2）圖 9(b)中，本文選擇的優(yōu)化預(yù)瞄距離Ld=2.7176 m在跟蹤誤差和穩(wěn)定性方面優(yōu)于Ld=2 m 和Ld=3 m 。

考慮時滯補償后得到各速度對應(yīng)的最優(yōu)預(yù)瞄距離，如表1 所示。

表1 考慮時滯補償?shù)母魉俣认伦顑?yōu)預(yù)瞄距離Tab.1 Optimal look-ahead distance under different velocity with time delay compensation

考慮時滯補償?shù)念A(yù)瞄距離擬合曲線如圖10 所示。

圖10 考慮時滯補償?shù)念A(yù)瞄距離擬合曲線Fig.10 Fitting curve of the relationship between look-ahead distance and velocity with time delay compensation

采用最小二乘法直線擬合得到時滯補償下預(yù)瞄距離Ld與速度v的線性關(guān)系為：

3.2.2 實車實驗

為了進一步驗證基于時滯補償所得最優(yōu)預(yù)瞄結(jié)果的合理性，在實驗小車平臺上進行實驗。實車路徑跟蹤具體框架分為感知層、決策層和執(zhí)行層。感知層通過實時動態(tài)定位技術(shù)（Real Time Kinematic,RTK）和慣性測量單元（Inertial Measurement Unit,IMU）進行組合導(dǎo)航接受定位信息；與機器人操作系統(tǒng)（Robot Operating System,ROS）以10 HZ 的頻率通信，傳輸定位數(shù)據(jù)到?jīng)Q策層。決策層通過CAN 總線輸出控制命令，對執(zhí)行層進行實時控制。執(zhí)行層由自動轉(zhuǎn)向機構(gòu)、制動和加速模塊構(gòu)成。為了減少潛在的耦合誤差，本文只涉及轉(zhuǎn)向機構(gòu)的橫向控制，不對車輛進行縱向速度控制。實驗小車測試平臺的基本參數(shù)如表2 所示。

表2 車輛參數(shù)Tab.2 Vehicle parameters

圖11 為實際測試場地環(huán)境，本次測試主要進行了直線跟蹤測試(γ=0)，利用導(dǎo)航系統(tǒng)選取起點與終點，根據(jù)起點和終點作一條直線，將其設(shè)置為參考軌跡。測試距離設(shè)置為40 m 左右，驗證直線路況下純追蹤ABMSSA 離線模型預(yù)瞄距離尋優(yōu)算法得到最優(yōu)預(yù)瞄距離的合理性和準(zhǔn)確性。

在直線路徑(γ=0) 下，通過計算實時跟蹤路徑與參考路徑的誤差，選取最小的跟蹤誤差作為該速度對應(yīng)的最優(yōu)預(yù)瞄距離，實驗結(jié)果如表3 所示。

表3 實車各速度下最優(yōu)預(yù)瞄距離Tab.3 Optimal look-ahead distances under different velocity in the vehicle

表4 給出了不同模型各速度對應(yīng)的最優(yōu)預(yù)瞄距離。從表4 可以看出，在相同速度下，加入轉(zhuǎn)向系統(tǒng)遲滯和控制回路時延的車輛模型對車輛預(yù)瞄距離尋優(yōu)結(jié)果更貼合實際情況，不考慮時滯模型的預(yù)瞄距離平均誤差為83.49%，考慮時滯模型的預(yù)瞄距離平均誤差為15.94%。上述實驗結(jié)果表明，相比于傳統(tǒng)模型，加入時滯補償所得最優(yōu)預(yù)瞄準(zhǔn)確率提高67.55%，驗證了時滯車輛模型的合理性。

表4 不同模型各速度下的最優(yōu)預(yù)瞄距離對比Tab.4 Comparison of optimal look-ahead distances for the two different system models at different velocities

4 結(jié)論

本文對基于時滯補償?shù)念A(yù)瞄距離優(yōu)化進行了深入分析。首先，針對實際車輛轉(zhuǎn)向機構(gòu)遲滯和控制回路時延帶來的路徑跟蹤控制精度降低問題，引入了時滯補償。其次，從理論上分析時滯模型的穩(wěn)定性，對ABMSSA 算法的閾值下限進行優(yōu)化，并分別在無時滯和有時滯補償情況下，計算得到不同的最優(yōu)預(yù)瞄距離。最后，在實際車輛平臺上，對所提出的基于時滯補償模型的純追蹤預(yù)瞄距離進行實驗驗證。實驗結(jié)果表明，相比于傳統(tǒng)模型，加入時滯補償所得最優(yōu)預(yù)瞄與實驗測試所得最優(yōu)預(yù)瞄準(zhǔn)確率提高了67.55%。

本文對直線路徑工況下的時滯補償模型進行了討論，并證明時滯補償模型在純追蹤預(yù)瞄距離優(yōu)化的合理性和有效性。針對更復(fù)雜的實際路況，后續(xù)將進一步研究對常規(guī)定曲率和變曲率工況下的無人駕駛路徑跟蹤控制效果，提高模型優(yōu)化的泛化能力。