自動泊車路徑純跟蹤算法應(yīng)用研究

齊志權(quán)，吳世南，鄒波，王浩，任宏斌

（1.北京理工大學(xué) 機(jī)械與車輛學(xué)院，北京 100081；2.重慶鐵馬工業(yè)集團(tuán)有限公司，重慶 400050）

智能化是汽車行業(yè)發(fā)展的一個(gè)重要方向，智能車輛典型體系結(jié)構(gòu)主要包括環(huán)境感知、行為決策兩個(gè)部分[1]，其中行為決策包含路徑規(guī)劃、軌跡規(guī)劃以及車輛控制等子系統(tǒng).智能車輛控制系統(tǒng)包括縱向控制和橫向控制，橫向控制指路徑跟蹤，即在保證舒適性和安全性的情況下，通過自動轉(zhuǎn)向控制車輛沿著參考軌跡行駛[2].路徑跟蹤方法有基于模型的控制方法[3-4]、基于幾何的方法以及基于機(jī)器學(xué)習(xí)[5]的方法等.幾何方法只利用參考路徑和車輛之間的幾何關(guān)系，例如純跟蹤法和Stanley 法.其中，純跟蹤算法自提出以來，因其良好的魯棒性、易實(shí)現(xiàn)且與人類駕駛員行為類似的特點(diǎn)得到了廣泛應(yīng)用.它通過計(jì)算車輛從當(dāng)前位置到目標(biāo)點(diǎn)的曲率來控制轉(zhuǎn)向，即使在存在較大的橫向偏差和參考路徑曲率不連續(xù)的情況下也能達(dá)到很好的跟蹤效果.以圓與直線，Hybrid A*為基礎(chǔ)的泊車路徑規(guī)劃算法得到的軌跡大多是曲率不連續(xù)的，因此純跟蹤算法在泊車軌跡跟蹤中應(yīng)用廣泛.算法難點(diǎn)在于選擇恰當(dāng)?shù)念A(yù)瞄距離[6]，過短的預(yù)瞄距離會導(dǎo)致路徑點(diǎn)跳變,控制量離散化進(jìn)而引起控制振蕩；過長的預(yù)瞄距離使得跟蹤精度下降，在大曲率工況下會出現(xiàn)切圓現(xiàn)象.

國內(nèi)外學(xué)者在改進(jìn)純跟蹤算法方面做了大量研究.PARK 等[7]將PI 控制應(yīng)用到側(cè)向運(yùn)動控制中，提出了自適應(yīng)純跟蹤算法，在輸出期望方向盤轉(zhuǎn)角時(shí)考慮了車輛與參考路徑間的橫向偏移量，該方法在曲線路徑的跟蹤上取得了良好的效果.WANG[8]在純跟蹤算法中提出了一種基于優(yōu)化前瞻距離的純跟蹤算法，在直線路徑、正弦路徑、拋物線路徑以及換道路徑中驗(yàn)證了算法有效性.YU 等[9]為了解決無人駕駛公交車的路徑跟蹤問題，提出了一種基于前軸參考的模糊純跟蹤控制的路徑跟蹤控制器，將純跟蹤算法的參考點(diǎn)選為前軸中心，改進(jìn)算法在實(shí)車實(shí)驗(yàn)中取得了良好效果.AHN 等[10]在選取預(yù)瞄點(diǎn)的過程中綜合考慮了車輛和參考路徑之間的相對位置及方向關(guān)系，使用該方法車輛可以快速收斂到參考路徑上并且減少了“切角”現(xiàn)象.WANG 等[11]采用兩個(gè)模糊控制器同時(shí)調(diào)整純跟蹤算法的車速和預(yù)瞄距離，以減少跟蹤誤差.SUKHI 等[12]提出了一種貪心算法，為參考軌跡上的路點(diǎn)分配最佳預(yù)瞄距離.WANG等[13]將GPS 數(shù)據(jù)與純跟蹤算法融合，提出基于二次多項(xiàng)式函數(shù)的動態(tài)預(yù)瞄距離純跟蹤算法，以減小橫向誤差.然而，上述改進(jìn)算法均沒有考慮大曲率路徑工況下，純跟蹤算法控制量輸出振蕩問題.自動泊車系統(tǒng)規(guī)劃模塊生成的參考路徑曲率較大，跟蹤精度要求較高，選擇較小預(yù)瞄距離時(shí)方向盤抖動問題愈發(fā)明顯，需要對純跟蹤算法進(jìn)行必要的改進(jìn).

為平衡跟蹤精度和控制平順性能，文中針對智能車輛自動泊車低速大曲率工況下的路徑跟蹤控制問題，對純跟蹤算法進(jìn)行了改進(jìn)，算法經(jīng)修正處理后可以最小程度地選取預(yù)瞄距離.通過軌跡跟蹤誤差以及方向盤轉(zhuǎn)角控制平順性兩個(gè)指標(biāo)與傳統(tǒng)純跟蹤算法進(jìn)行了對比.

1 純跟蹤算法

1.1 以后軸中心為原點(diǎn)的車輛運(yùn)動學(xué)模型

實(shí)現(xiàn)智能車輛路徑的準(zhǔn)確跟蹤控制，首先要對車輛的運(yùn)動建立準(zhǔn)確的數(shù)學(xué)模型，模型對車輛運(yùn)動的描述越準(zhǔn)確，則對車輛的跟蹤控制效果就越好.同時(shí)，建立的數(shù)學(xué)模型除了真實(shí)反映車輛特性外，也應(yīng)該盡可能的簡單易用.在自動泊車低速大曲率路徑工況下，由于車輪側(cè)偏特性影響較小，自行車車輛運(yùn)動學(xué)模型可以滿足算法設(shè)計(jì)需要.

自行車運(yùn)動學(xué)模型的建立基于如下假設(shè)：

①不考慮車輛在垂直方向(Z軸方向)的運(yùn)動，即假設(shè)車輛的運(yùn)動是一個(gè)二維平面上的運(yùn)動；②假設(shè)車輛同軸左右側(cè)輪胎在任意時(shí)刻都擁有相同的轉(zhuǎn)角和轉(zhuǎn)速，車輛的左右輪胎運(yùn)動可以合并描述；③忽略前后軸載荷轉(zhuǎn)移；④假設(shè)車身和懸架系統(tǒng)是剛性系統(tǒng)；⑤假設(shè)車輛前輪轉(zhuǎn)向；⑥車輛在低速行駛狀態(tài)下，側(cè)向加速度小，此時(shí)可認(rèn)為輪胎側(cè)偏角近似為0，即車輪沒有側(cè)向滑移.

如圖1 所示為自行車運(yùn)動學(xué)模型示意圖.圖中，O′點(diǎn) 是車輛的瞬時(shí)轉(zhuǎn)動中心；β為 質(zhì)心側(cè)偏角；ψ為車身橫擺角；δ為前輪轉(zhuǎn)角；v為車輛質(zhì)心處速度；R為轉(zhuǎn)彎半徑；a,b分別為質(zhì)心到前軸以及后軸的距離.

分析圖中幾何關(guān)系，在三角形O′GA和O′GB中，由正弦定理可得到如下關(guān)系：

1.2 純跟蹤算法原理

如圖2 所示為純跟蹤算法的幾何描述.以車輛后軸位置為圓心，預(yù)瞄距離ld為半徑做圓，與前方規(guī)劃路徑的交點(diǎn)為預(yù)瞄點(diǎn) (gx,gy).根據(jù)自行車模型，以車輛后軸中心為切點(diǎn)，車輛縱向車身為切線，通過控制前輪轉(zhuǎn)角，使車輛可以沿著一條經(jīng)過預(yù)瞄點(diǎn)的圓弧行駛，實(shí)現(xiàn)路徑跟蹤控制.

圖2 純跟蹤算法幾何描述Fig.2 Geometric description of pure pursuit algorithm

根據(jù)幾何關(guān)系可以計(jì)算得到預(yù)期路徑的曲率大小，具體計(jì)算式為

式中：α為車輛當(dāng)前航向與預(yù)期路徑圓弧之間的弦切角大小；R為預(yù)期路徑圓弧半徑.

整理可得：

可以看出，純跟蹤算法實(shí)際上是基于橫向偏差的比例控制器，增益受預(yù)瞄距離影響很大，預(yù)瞄距離越大，增益越小.當(dāng)選取小的預(yù)瞄距離時(shí)，因存在超調(diào)量且路徑曲率變化，車輛的頻繁轉(zhuǎn)向會引起控制振蕩，舒適性變差.當(dāng)選取大的預(yù)瞄距離時(shí)，車輛的振蕩減小，但是控制系統(tǒng)忽略了一部分參考路徑的信息，導(dǎo)致跟蹤精度下降.

2 純跟蹤算法在自動泊車路徑跟蹤中的應(yīng)用

本節(jié)針對智能車輛泊車工況應(yīng)用場景，分析純跟蹤算法在應(yīng)用過程中需要注意的問題.算法實(shí)現(xiàn)如圖3 所示.

圖3 純跟蹤算法實(shí)現(xiàn)流程圖Fig.3 Flow chart of pure pursuit algorithm

以垂直車位自動泊車路徑跟蹤為例，泊車路徑由路徑規(guī)劃模塊給出，車輛實(shí)際行駛軌跡通過GPS設(shè)備采集.實(shí)驗(yàn)設(shè)備及場景如圖4 所示，圖4(a)為GPS基站及垂直車位圖，圖4(b)為實(shí)驗(yàn)車輛及GNSS 天線安裝位置，圖4(c)為GPS 移動站.實(shí)驗(yàn)用車具體參數(shù)如下：整車長4.59 m，整車寬1.88 m，軸距2.75 m，轉(zhuǎn)彎半徑5 m，方向盤傳動比18.06.

圖4 實(shí)驗(yàn)設(shè)備及場景Fig.4 Experimental equipment and scenes

路徑規(guī)劃模塊輸出的參考路徑如圖5 所示，最小半徑為6 m，生成的一系列參考路點(diǎn)集合為Pref={p0,p1,p2···}，其中pi={xi,yi}，相鄰參考路點(diǎn)之間的歐氏距離約為0.2 m.根據(jù)公式(10)曲率 κ 與半徑R的關(guān)系, 結(jié)合車輛最小轉(zhuǎn)彎半徑限制，半徑6 m 是在保證泊車任務(wù)順利完成的前提下，實(shí)驗(yàn)車輛可以達(dá)到的最大曲率.

圖5 垂直泊車路徑規(guī)劃（三步泊車）Fig.5 Path planning for vertical parking (three steps)

圖6 垂直泊車路徑純跟蹤控制結(jié)果Fig.6 Path tracking with pure pursuit for vertical parking

圖7 純跟蹤最大橫向誤差Fig.7 Pure pursuit maximum lateral error

圖8 垂直泊車路徑純跟蹤方向盤控制輸出Fig.8 Steering wheel angle output of pure pursuit for vertical parking

3 帶記憶參數(shù)的純跟蹤方向盤轉(zhuǎn)角修正

純跟蹤算法的每一個(gè)控制周期都會輸出前輪的期望轉(zhuǎn)角，通過車輛線控轉(zhuǎn)向或者電動助力轉(zhuǎn)向系統(tǒng)控制對應(yīng)的方向盤轉(zhuǎn)角，在自動泊車大曲率路徑的跟蹤控制中，方向盤的高頻抖動不僅會影響到駕駛體驗(yàn)，對車輛線控底盤系統(tǒng)也會帶來不利影響.文中在保證跟蹤精度的前提下，為消除純跟蹤算法帶來的方向盤抖動，基于自動泊車低速、方向盤轉(zhuǎn)角變化率小等特性，引入控制記憶參數(shù)，對方向盤轉(zhuǎn)角控制指令進(jìn)行修正.具體算法的處理過程主要有3 個(gè)步驟:

①記憶低通濾波.除第一個(gè)控制周期的方向盤轉(zhuǎn)角直接輸出外，此后的每一個(gè)控制周期的方向盤轉(zhuǎn)角控制值都與上一控制周期實(shí)際控制值進(jìn)行加權(quán)，得到有效低通濾波值，具體計(jì)算公式為

式中：θoutput(t-1)為 上一控制周期輸出的方向盤轉(zhuǎn)角，θe(t)為當(dāng)前控制周期根據(jù)式(7)及方向盤傳動比計(jì)算得出的期望方向盤轉(zhuǎn)角，θpro(t)為當(dāng)前控制周期待處理的方向盤轉(zhuǎn)角，β為根據(jù)實(shí)際工況選取的權(quán)重系數(shù).β越大，方向盤轉(zhuǎn)角指令輸出越平穩(wěn)，但是會造成靈敏度降低.

②期望方向盤轉(zhuǎn)角變化記憶參數(shù)計(jì)算.通過計(jì)算 θpro(t)和 θoutput(t-1)的 差 值 θdif，并 與 設(shè) 定 的 閾 值 進(jìn) 行 比較，當(dāng) θdif超過閾值時(shí)，轉(zhuǎn)角變化記憶參數(shù)增加，具體更新過程為

式中：Mleft，Mright分別為左(右)轉(zhuǎn)信號記憶參數(shù)，初值為0；steer_space 為設(shè)定的轉(zhuǎn)向閾值.

③更新期望方向盤轉(zhuǎn)角控制輸出.為了避免傳統(tǒng)純跟蹤算法在路徑跟蹤時(shí)頻繁轉(zhuǎn)向的情況，只有當(dāng)期望方向盤轉(zhuǎn)角變化記憶參數(shù)超過一定范圍，且期望方向盤轉(zhuǎn)角與上一控制周期的變化差值大于一定步長時(shí)，才會更新轉(zhuǎn)向控制輸出，具體更新方法如下:

當(dāng)Mleft，Mright小于等于設(shè)定的數(shù)量控制閾值control_space 時(shí)，仍將上一個(gè)控制周期輸出的方向盤轉(zhuǎn)角 θoutput(t-1)作為當(dāng)前控制輸出.

當(dāng)Mleft，Mright超過設(shè)定的閾值control_space 時(shí)，若θdif的絕對值超過設(shè)定的步長控制閾值step_space，則將 θpro(t)作為當(dāng)前時(shí)刻轉(zhuǎn)向控制輸出；否則，仍將上一控制周期的方向盤控制轉(zhuǎn)角 θoutput(t-1)作為當(dāng)前時(shí)刻轉(zhuǎn)向控制輸出.算法流程如圖9 所示.

圖9 純跟蹤方向盤轉(zhuǎn)角修正算法流程圖Fig.9 Flow chart of corrected pure pursuit algorithm for steering wheel angle

4 算法實(shí)車實(shí)驗(yàn)驗(yàn)證

4.1 參數(shù)選擇

以上述垂直泊車工況為例進(jìn)行實(shí)車驗(yàn)證，如圖5所示，車輛初始位置為(-0.08，4)，垂直泊車位位置為(-5.6，0)，整個(gè)泊車規(guī)劃過程分為三步，泊車過程中最高車速限定為2.5 km/h.經(jīng)實(shí)車實(shí)驗(yàn)標(biāo)定，主要車輛參數(shù)和控制參數(shù)如表1 所示.同樣分別選取預(yù)瞄距離ld為1 m，2 m，3 m，將傳統(tǒng)純跟蹤算法與文中修正后的純跟蹤算法在跟蹤精度以及方向盤轉(zhuǎn)角控制輸出穩(wěn)定性進(jìn)行對比分析.

表1 參數(shù)選擇Tab.1 Parameters value used

4.2 跟蹤精度分析

圖10 為修正純跟蹤算法軌跡圖，從圖中可以看出，隨著預(yù)瞄距離減小，跟蹤精度能得到提高.圖11為修正純跟蹤算法最大橫向誤差圖，當(dāng)預(yù)瞄距離為1 米時(shí)，軌跡的橫向誤差最大為0.054 7 m，與傳統(tǒng)純跟蹤算法相同；當(dāng)預(yù)瞄距離為2 m 時(shí)，修正算法的軌跡最大橫向誤差與傳統(tǒng)純跟蹤算法幾乎相同；當(dāng)預(yù)瞄距離為3 m 時(shí)，軌跡的最大橫向誤差有所減少，可以達(dá)到0.156 2 m.由此可見，文中提出的修正純跟蹤算法在低速大曲率路徑跟蹤工況下，可以保證甚至提高跟蹤精度.

圖10 垂直泊車路徑修正純跟蹤控制結(jié)果Fig.10 Path tracking with corrected pure pursuit for vertical parking

圖11 修正算法最大橫向誤差Fig.11 Correction algorithm maximum lateral error

4.3 控制輸出穩(wěn)定性分析

圖12 為修正純跟蹤算法方向盤轉(zhuǎn)角控制輸出結(jié)果，與圖8 對比可以看出，在預(yù)瞄距離分別為1 m，2 m，3 m 時(shí)，采用修正純跟蹤算法，方向盤轉(zhuǎn)角的抖動問題均得到很好控制，同時(shí)跟蹤精度沒有降低.原因在于改進(jìn)算法在輸出方向盤轉(zhuǎn)角時(shí)通過記憶低通濾波和設(shè)定記憶參數(shù)使輸出慢于輸入，產(chǎn)生遲滯，通過記憶因子的衰減作用濾掉了較大的高頻波動.圖13 為預(yù)瞄距離為1 m 時(shí)純跟蹤算法修正前后輸出方向盤轉(zhuǎn)角的對比結(jié)果，可以看出修正的純跟蹤算法可以大幅度提高小預(yù)瞄距離下純跟蹤算法方向盤轉(zhuǎn)角輸出的穩(wěn)定性，使控制更為平順，避免控制振蕩問題.

圖12 修正純跟蹤算法方向盤控制輸出Fig.12 Steering wheel angle of corrected pure pursuit.

圖13 方向盤轉(zhuǎn)角輸出對比Fig.13 Comparation of steering wheel angle

5 結(jié) 論

針對傳統(tǒng)純跟蹤算法在自動泊車大曲率路徑跟蹤過程中，跟蹤精度和方向盤轉(zhuǎn)角控制輸出平順性相互制約的問題，文中在原始純跟蹤算法的基礎(chǔ)上，利用自動泊車低速、方向盤轉(zhuǎn)角變化率小的特性，通過引入控制記憶參數(shù)，對方向盤轉(zhuǎn)角控制輸出進(jìn)行修正.實(shí)驗(yàn)驗(yàn)證表明，該方法可以應(yīng)用在自動泊車軌跡跟蹤系統(tǒng)中，在保證跟蹤精度的同時(shí)，大幅度減少方向盤抖動，提高跟蹤控制的平順性.值得指出的是，該方法同樣適用于其他低速大曲率路徑跟蹤工況.