多路徑下航向預估的無人車路徑跟蹤算法研究

魯棒棒，皮大偉，謝伯元，王顯會

(南京理工大學，江蘇 南京 210094)

1 引言

作為當今汽車行業的熱門，無人駕駛車輛已廣泛應用于安保巡邏、物流配送等方面。對此，無人駕駛車輛在較低車速下的路徑跟蹤研究顯得尤為關鍵。在低速下的路徑跟蹤研究中最為關鍵的是設計精度高、穩定、適應性好的路徑跟蹤控制器[1-2]。目前，研究路徑跟蹤問題的控制算法有很多，主要為純點跟蹤算法、斯坦利算法、模型預測算法[3-6]等。國內外學者也對此做了大量研究[7-9]，這方面面臨的主要問題是縱向車速和方向的解耦降低了算法的精度和穩定性，目前的路徑跟蹤控制算法在設計控制器的時候大多忽略縱向車速的影響，以某一恒定的經驗來確定控制器的參數，導致縱向車速變化后控制精度和穩定性都有所降低。為解決這一問題，以一輛無人物流車為研究對象，以GPS采集的離散數表形式的數據建立多路徑下的預期道路模型，通過引入GPS獲取的車輛位置、慣性導航系統、前輪轉角、車速等位姿反饋量，建立二自由度車輛模型、航向預估模型及附加航向模型，計算橫向位置偏差、航向預估量及附加航向，以航向預估量、附加航向、車輛當前航向、前輪轉角為航向反饋量，設計多路徑下基于航向預估的模糊PID控制器，并通過SIMULINK仿真和對應的實車實對算法進行驗證。

控制架構如圖1。本文旨在設計多路徑下適應縱向車速的基于航向預估與模糊控制的PID控制器，安排如下：第二節著重于預期道路模型、航向預估模型、附加航向模型的建立。在第三節，設計了多路徑下適應縱向車速的基于航向預估的模糊PID控制器。第四節通過SINULINK仿真和對應的實車實驗對提出的路徑跟蹤算法進行驗證。最后，在第五節中給出簡短的結論。

圖1 控制架構

2 動力學與數學模型

2.1 道路模型

本文設計的控制器建立在預先獲取的理想道路模型的前提下。無人車希望跟隨的“道路”千變萬化，錯綜復雜[10]。考慮到本文的道路模型通過GPS采集的數據獲取，采用離散的數表形式描述道路，如圖2。根據道路曲率的大小確定該段道路離散點的數量，二者成正比，道路曲率越大，數量越多，點越密集。

圖2 預期道路的描述

2.2 二自由度車輛模型

無人駕駛車輛在進行路徑跟蹤的過程中，橫向位置偏差y隨縱向車速、橫擺角速度等車輛狀態參數改變而變化。無人駕駛車輛作為高度非線性的復雜系統，橫向車速、橫擺角速度等參數無法通過測量獲取。為更精確地計算由車輛當前位置與預期道路產生的橫向位置偏差y，引入只有側向和橫擺兩個自由度的車輛二自由度模型。

車輛二自由度模型微分方程如下：

(1)

(2)

其中：

m為無人駕駛車輛整車質量(kg)；

Iz為車輛繞z軸的轉動慣量(kg.m2)；

kf、kr分別為前后輪胎側偏系數(N/rad)；

u、ν分別為汽車縱向、橫向速度(m/s)；

a、b分別為質心到前后軸距離(m)；

ω為汽車橫擺角速度(rad/s)；

β為質心側偏角(rad)；

δ為前輪轉角(rad)；

考慮車輛在轉向時轉向機構具有一定的遲滯性，視為一階慣性環節，則有:

(3)

假設車輛在行駛過程中橫向車速v遠遠小于縱向車速u，即u?v。如圖3，有

(4)

聯立上式(1)(2)(3)(4)可獲得以橫擺角速度、橫向車速、橫向位置偏差、前輪轉角為狀態量的系統狀態空間表達式

(5)

2.3 附加航向模型

附加航向模型如圖3。在大地直角坐標系下，(X0，Y0)為GPS獲取的車輛在當前時刻質心位置的坐標，(Xk+1，Yk+1)為預瞄點rk+1的坐標，ψ為大地坐標系下通過慣性導航系統獲得的車輛的當前航向，ψd為預瞄點rk+1在大地直角坐標系下無人駕駛車輛希望跟隨的期望航向。任何對無人駕駛車輛的控制需要轉換至車輛坐標系下進行，對預瞄點rk+1進行坐標轉換

(6)

無人駕駛車輛當前時刻的實際航向與預期道路下期望航向的航向差值φ為

φ=ψd-ψ

(7)

車輛質心至預瞄點rk+1的距離l為

(8)

由于橫向位置偏差y而產生的附加航向φΔ為

(9)

圖3 附加航向模型

2.3 航向預估模型

航向預估模型就是基于縱向車速、前輪轉角預測出車輛的航向變化趨勢，以此為航向反饋并計入控制偏差來影響控制器輸出的前輪轉角，減小因航向偏差滯后帶來的控制效果變差的影響[11]。本文提出的航向與估算法基于簡化的車輛二自由度模型，預估模型如圖4。O為無人駕駛車輛轉向時的瞬時轉向中心，r為無人駕駛車輛轉向時繞O的轉向半徑，G為無人駕駛車輛的質心，δ為前輪轉過的角度，a、b分別為G到車輛前軸與后軸的距離。

圖4 航向預估模型

設控制器的采樣周期為T，無人駕駛車輛的前輪車速近似等于縱向車速u，車輛在T內的航向變化量即航向預估量Δθ近似為

Δθ=uT/r

(10)

前輪繞轉向中心O的轉向半徑r近似為

r=(a+b)/sinδ

(11)

聯立上面兩式可得

Δθ=uTsinδ/(a+b)

(12)

3 控制器設計

本文的控制策略建立在航向跟蹤的基礎上，在無人駕駛車輛的控制過程中，產生的橫向位置偏差和航向偏差全部轉化為航向偏差進行控制。為解決直道下車輛縱向車速變化時控制算法精度和安全性下降、彎道的航向偏差大、駛入彎道時轉向不足、駛出彎道時過度轉向等問題，考慮車速和前輪轉角和橫向位置對航向變化的影響，提出多路徑下基于航向預估的模糊PID控制算法。控制器分為兩層，上層為多路徑控制器，計算直道和彎道下對應的航向偏差量，確保車輛在多路徑、縱向車速變化下行駛時具有良好的跟蹤性能，下層為模糊PID控制器，計算下發至車輛的期望前輪轉角，控制器架構如圖5。

圖5 控制器架構

3.1 多路徑控制器

設計多路徑控制器的目的是解決因路徑(直道和彎道)不同和縱向車速變化而導致的跟蹤算法精度和穩定性下降的問題。為此引入路徑標志變量signal、0-1損失函數和代價變量kψ、kψd、kΔθ、kφΔ、kδ，控制器結構如圖6。

變量signal為衡量車輛處于直道或者彎道的標志，若處于直道，signal≥0；處于彎道時，signal<0。

各代價變量為

(13)

控制器輸出量航向偏差E為

E=kψdψd-kψψ-kΔθΔθ-kφΔφΔ-kδδ

(14)

圖6 多路徑控制器結構

3.2 模糊PID控制器

考慮到車輛縱向和橫擺運動之間的耦合關系，下層控制器器是基于模糊和PID控制理論設計的，控制框圖如圖7所示。模糊PID控制器可以計算下發至車輛的前輪期望轉角。

圖7 模糊PID控制框圖

模糊PID控制器由模糊控制和PID控制兩部分組成。模糊控制器的輸出量為PID控制器的三個參數。然后PID控制器計算下發至車輛的期望轉角。通過模糊控制對PID控制器的Kp、Ki、Kd三個參數自整定，可提高系統的魯棒性、穩健性和適應性。

上層多路徑控制器的輸出量為航向偏差E，因此模糊控制器的輸入量為航向偏差E及航向偏差變化率EC，模糊控制器的輸出為PID控制器的三個可調參數調整量ΔKp、ΔKi、ΔKd。

在模糊化過程中，輸入變量E、EC輸出模糊集合分別為NB(負大)，NM(負中)，NS(負小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)，E及EC的模糊論域為[-3，3]。隸屬度函數采用較為簡單的三角函數，量化因子為1.1和2.0。ΔKp、ΔKi、ΔKd的輸出論域分別為[-3，3]，[-1，1]，[-0.2，0.2]。

將車輛行駛工況分成下述三個典型工況進行分析，以確定Kp，Ki，Kd的選取規則。

規則1：if E is PB and EC is PB，then ΔKpis NB，ΔKiis PB，ΔKdis PS

在這種工況下，車輛當前實際航向遠遠小于希望跟隨的期望航向，車輛處于嚴重轉向不足狀態。為了使車輛能夠迅速跟隨上期望航向，應取較大的Kp，增大前輪轉角，同時為避免轉向機構超過極限位置，應取較小的Kd，Ki應取為0，防止車輛實際航向超過期望航向。

規則2：if E is NS and EC is NB，then ΔKpis PM，ΔKiis NB，ΔKdis Z0

在這種工況下，車輛當前實際航向略大于希望跟隨的期望航向。為提高跟蹤精度，減小車輛在穩定后與期望航向的差值，應取較大的Kp，Ki，同時，為減少車輛進入穩定狀態的調節時間，應取較小的Kd。

規則3：if E is PM and EC is NM，then ΔKpis Z0，ΔKiis Z0，ΔKdis NS

在這種工況下，車輛當前實際航向相比于希望跟隨的期望航向小一些。為避免車輛在跟蹤過程中產生航向超調，應取較小的Kp，同時，為減少車輛進入穩定狀態的調節時間，Ki應取適中的值，應取較小的Kd并保持不變。由此構成的模糊規則如表2所示。

表1 模糊控制規則表

PID控制器的輸入變量為航向偏差值E，輸出變量為期望轉角δd，δd為

(15)

模糊PID控制器的參數如表2。

表2 模糊PID參數

4 仿真與實車實驗

為驗證本文提出的算法的有效性，本文進行了SIMULINK仿真和實車實驗，為表現所提出的多路徑下基于航向預估的模糊PID控制器(以下簡稱航向預估算法)的跟蹤性能，與只以實際航向和附加航向為反饋量的增量式PID算法控制器進行比較。

4.1 仿真研究

仿真在MATLAB/SIMULINK環境下進行，實驗車輛前輪轉角的最大值為45°，為與其一致，將仿真中控制器輸出中加入最大值為0.785rad。

考慮到GPS等傳感器的采樣周期，將控制器采樣周期設為100ms，實驗車速為2-4m/s，仿真時系統參數如表4所示。

表3 車輛參數

以實驗場地中的一段直道加彎道的實際路徑作為仿真路徑，仿真初始位置偏差為1.1m，航向偏差為1°，直道時車速為4m/s，進入彎道時減速為2m/s，過彎后再加速至4m/s，仿真結果見圖8、圖9、圖10。

圖8 路徑跟蹤曲線

圖9 航向跟蹤曲線

圖10 橫向位置誤差曲線

圖11 縱向車速曲線

圖8表明，航向預估算法的跟蹤效果比常規PID控制算更好法。從圖9可以看出，在直道時，縱向車速由0.5增加至4m/s時，航向預估算法對應的航向偏差變化較為平緩，在第16.5s時航向偏差趨于0，車輛進入穩定狀態，常規PID控制算法航向變化較為劇烈，在第23.3s時航向偏差趨于0，調整時間較長；在彎道時，航向預估算法航向跟蹤效果比較理想，始終與理想航向保持一定的偏差，而常規控制算法在第47-52s內時航向超調，在第62s時，駛出彎道重新駛入直道，車輛出現航向超調現象。圖10表明，當縱向車速變化時，航向預估算法的橫向位置誤差較小，保持在0.1m以內，常規控制算法出現較為劇烈的震蕩現象；綜合比較圖8、圖9、圖10和圖11，航向預估算法的控制效果、穩定性以及對車速的適應性優于常規算法。

4.2 實車實驗

實驗平臺為無人駕駛車輛，車輛位置由差分GPS系統給出，車輛航向由慣性導航系統給出，實驗路徑由一段直道加彎道和過彎后的一段直道組成，路徑初始位置偏差為0.5m，航向偏差為0°，直道時車速為4m/s，進入彎道時減速為2m/s，過彎后再加速至4m/s，實驗結果見圖12、圖13、圖14。

圖12 路徑跟蹤曲線

圖13 橫向位置偏差曲線

圖14 縱向車速曲線

從圖12可以看出，航向預估算法能夠較好地跟隨上路徑，而常規控制算法出現明顯的震蕩，控制的路徑沿理想路徑左右搖擺；圖13表明，航向預估算法的橫向位置偏差超調量較小，最大為0.1m，車輛進入穩定狀態的調節時間為13s，在駛入和駛出彎道時橫向位置偏差保持在0.1m以內，而常規PID控制算法的橫向位置偏差最大達到了0.2m，車輛進入穩定狀態的調節時間為18s，且出現明顯的震蕩，相比于常規控制算法，橫向位置偏差減少了0.1m，調節時間縮短了27.7%。

5 結論

針對低速下無人車的路徑跟蹤問題，設計了多路徑下適應縱向車速的航向預估模糊PID控制器。通過引入航向預估量、前輪轉角、附加航向、實際航向等反饋量，解決多路徑下、縱向車速變化等因素下引起的跟蹤精度與穩定性下降的問題。在未增加計算復雜性的前提下，提出的算法與常規算法相比，提高了路徑跟蹤的精度(橫向位置偏差減少至0.1m以內)，算法的穩定有所提升(調節時間減少27.7%)。

提出的算法只在低速下對縱向車速等變化因素只具有一定的自適應能力，應用于全車速下的自適應路徑跟蹤算法仍待研究。