基于改進UKF的自動泊車車輛位姿估計算法研究＊

謝有浩 趙林峰 張銳陳 張澄宇 辛鑫

（1.滁州學院，滁州239000；2.安徽獵豹汽車有限公司，滁州 239064；3.合肥工業大學，合肥 230009）

主題詞：自動泊車 位姿估計 改進UKF濾波算法 常值噪聲統計估計器

1 前言

自動泊車系統（Automatic Parking System，APS）是智能駕駛系統的典型功能，通過傳感器獲得輪速等信號并與算法相結合預測泊車過程中車輛的位姿狀態，對于構建完整可靠的自動泊車系統有一定的應用價值。

文獻[1]～文獻[3]分別搭建了低速行駛工況下泊車系統坐標方程和低速倒車工況車輛運動學模型，并提出了自動泊車的路徑規劃及跟蹤策略，但并未涉及對車輛位姿估計的研究。文獻[4]～文獻[6]搭建擴展卡爾曼濾波（Extended Kalman Filter，EKF）算法框架對泊車過程中的車輛位姿進行估計。EKF算法將非線性方程近似為線性方程再對目標進行卡爾曼濾波處理，得到的預測結果與實際狀態必然存在較大差異。為克服EKF的局限性，一些學者提出了無跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法。UKF算法的核心思想是采用無跡變換對非線性系統狀態的后驗概率密度函數進行近似。與EKF相比，UKF以二階以上精度近似高斯非線性系統狀態的后驗均值與方差，具有實現簡單、濾波精度高以及收斂性好等優點。為進一步提高UKF的濾波精度，文獻[7]提出了一種可以自適應調整各Sigma點比重的比例因子，通過采用比例最小偏差采樣提高UKF的計算精度。文獻[8]分析了蒙特卡洛采樣策略并驗證了該方法具有一定的精確性，但前提是采樣數量足夠多。文獻[9]和文獻[10]研究表明，聯邦卡爾曼濾波算法具有與集中濾波器相當的精度，且容錯性能更好。上述文獻在位姿估計過程中均假設系統噪聲為均值為零的常值白噪聲，但噪聲實際往往處于先驗統計未知的狀態。

本文首先基于阿克曼轉向幾何建立泊車過程中的車輛運動模型，并建立觀測方程，隨后搭建常值噪聲統計估計器，選用合適采樣策略構造自適應UKF算法，最后通過聯邦濾波結構求得下一時刻位姿狀態預測值及協方差矩陣。利用MATLAB/Simulink軟件進行仿真驗證，并在基于CarSim整車模型和LabVIEW數據采集處理的試驗臺進行了硬件在環試驗，驗證了算法的有效性。

2 車輛運動學模型的建立

阿克曼理論的前提為假設汽車在轉向過程中所有車輪均為滾動狀態，此時車輛不受側向力作用，所有車輪轉向軸線交于一點，整車可以視為一個剛體。

由于車輛泊車過程中需要保持恒定低車速，可假設車輛運動不受側向力作用，車輪沒有發生滑移，因此可以基于阿克曼轉向原理建立運動學模型進行研究。

圖 1 所示為泊車運動學模型示意，其中，（xf,yf）、（xr,yr）分別為前、后軸中心點的坐標，（xr1,yr1）、（xr2,yr2）分別為左、右后輪中心點的坐標，v、vr分別為前、后軸中心點的速度，θ為車輛中心對稱線與x軸方向的夾角，即車輛的偏航角，l為車輛軸距，?為前輪轉角。

圖1 車輛運動學模型示意

后輪軌跡在車輛橫向上的速度為0，可得：

由圖1可知，前、后軸中心點坐標的關系為：

對式（2）求導，可得其速度關系為：

將式（3）代入式（1）可得：

前軸中心點在x、y方向上的速度分別為：

將式（5）代入式（4），即可求得車輛回轉圓角速度為：

將上述兩式代入式（3）中，即可求得vr在x、y方向上的分速度：

將式（7）積分可得后軸中心點運動軌跡方程為：

3 改進UKF濾波器的搭建

根據車輛前、后輪分別搭載的兩組編碼器采集的信號建立兩組不同的測量方程，從而分別構造兩種濾波算法。聯邦濾波[11]作為一種分布式的兩級濾波結構，由1個主濾波器和若干子濾波器構成，可融合子濾波器的估計結果，得出精度更高、可靠性更強的泊車位姿估計結果。

考慮到泊車過程中車輛運動模型的狀態方程和觀測方程均含有非線性這一特點，且系統中含有先驗統計未知的噪聲，假設均為常值噪聲，故采用加入常值噪聲統計估計器的改進UKF算法分別實現聯邦濾波結構中兩個子濾波器的位姿估計結果。

根據圖1，將非線性汽車系統狀態向量定義為X=（xr,yr,θ,ω,vr）T，其中，ω 為橫擺角速度，將觀測向量定義為 Z=（ω,vr）T。

非線性離散系統模型為：

式中，Xk+1與Zk分別為系統的5維狀態向量和2維量測向量；Wk和Vk分別為狀態噪聲和測量噪聲；fk（）和hk（）分別為系統非線性狀態函數和量測函數。

3.1 子濾波器的設計

通過安裝在車輪上的編碼器采集輪速信號，并由此得到車速，同時，由轉向盤轉角傳感器采集得到轉向盤轉角φ。由此，可建立子濾波器n（n=1,2）的測量方程：

其中，觀測變量為：

式中，ωn,k和vn,k分別為子濾波器i觀測到的橫擺角速度和后軸中心點速度；s=f,r分別表示前、后車輪；當n=1時，s=r，τ1=τ2=0；當 n=2 時，s=f，τ1=δ1,τ2=δ2；vs1,k、vs2,k分別為前輪或后輪左、右編碼器采集得到的輪速信號計算所得車速；f（φ）為后軸中點轉彎半徑，是車輛轉向盤轉角φ的離散函數；δ1、δ2分別為左、右前輪轉角；e為后軸中心點至車輪的距離；R1、R2分別為左、右后輪轉彎半徑。

f（φ）通過試驗標定的方法確定，試驗簡要過程如下：采集轉向盤轉角和該轉角下車輛后軸中點的轉彎半徑，然后得出離散數據表，實際應用中對其進行插值便可獲得后軸中點的轉彎半徑。

子系統1、2的狀態方程為：

由式（7）和式（8）可得狀態方程為：

式中，a1=cotφsinφsin（t）+T0cos（θk）；控制變量 Un,k=（0,l cotφ,?,0,0,0）T；T0為采樣時間步長；θk為k時刻車輛中軸線指向車頭的方向與x軸的夾角。

3.2 子濾波器常值噪聲統計估計器的設計

在自動泊車過程中，對于車輛位姿的估計不同于傳統UKF濾波過程，濾波過程中的狀態噪聲和測量噪聲處于先驗統計未知狀態，均值均不為零，因此假設系統噪聲為常值噪聲，需設計常值噪聲估計器，對其進行估計。

基于上述分析，對傳統UKF算法進行改進。首先提出2個假設[12]：

a.Wk和Vk為互不相關的高斯白噪聲，且具有如下常值統計特性：

b. 初始狀態X0與Wk、Vk互不相關，且服從正態分布，其均值和協方差矩陣為：

根據文獻[12]中的定理一、定理二可以得出次優無偏MAP常值噪聲統計估計器q?k、Q?k、r?k、R?k。

3.3 子系統局部位姿估計過程

分別獲得狀態方程和觀測方程后，通過UKF算法對泊車過程中的車輛進行位置和航向更新。

3.3.1 初始化

車輛位置與航向的狀態初值及誤差方差矩陣初值分別為：

3.3.2 計算Sigma點

蒙特卡洛采樣策略是一種隨機性采樣方法，得到的Sigma點集符合輸入狀態分布特征，利用蒙特卡洛算法得出大量Sigma點逼近概率密度分布，從而得到更高階近似，以體現非線性函數本身的非線性。而當Sigma點數不超過500時，采用蒙特卡洛策略的UKF方法容易失效[13-14]，因此本文采用傳統確定性采樣方法和蒙特卡洛采樣策略相結合的方法采集Sigma點。當采集的Sigma點數i≤500時，采用對稱采樣策略。對于均值為-X，方差為D-X的n維隨機變量，產生一個矩陣χ，該矩陣由（2n+1）個列向量組成，表達式為：

式中，L=5為車輛狀態向量維數；λ=（α2-1）L為控制形成Sigma點散布程度的參數；α為比例縮放因子（正值），取值范圍為1×10-4≤α≤1，通常取較小的值。

當i＞500時，采用蒙特卡洛采樣策略生成Sigma點，其步驟為：建立滿足均值為-X，方差為D-X的高斯分布概率模型；對概率模型進行隨機抽樣；由抽樣結果組成Sigma點集。

隨后，直接調用MATLAB軟件產生normrnd函數（A=normrnd（ave,sig,m）），返回m個均值為ave，標準差為sig的高斯分布的隨機數A。采用蒙特卡洛采樣策略，對均值為-Xk、方差為D-Xk的n維隨機變量X，生成的m個Sigma點集為：

式中，（Xk）i為X 的第i個元素為D第i個對角線元素的平方根值。

3.3.3 時間更新

用Sigma點集矩陣代替狀態方程中的X，得到新的狀態方程：

式中，χ*i,k+1|k為車輛位置和航向的狀態預測值。

對（2L+1）個狀態預測值進行加權求和，可得到車輛位置和航向的狀態預測值為：

同時，可計算得到車輛位置和航向狀態方差為：

式中，Qk+1為（k+1）時刻車輛位置和航向狀態方程的噪聲方差矩陣；Pk+1|k為狀態估計方差矩陣。

用式（25）得到的車輛位置和航向的狀態預測值代替式（10）中的Xi,k，得到新的觀測方程：

同時可計算得到車輛位置和航向觀測方差為：

式中，Rk+1為（k+1）時刻車輛位置和航向觀測方程的噪聲方差矩陣。

車輛定位過程中統計特性權系數為：

3.3.4 測量更新

在時間更新后，繼續對系統進行測量更新。利用車輛位置和航向狀態值以及觀測值，計算車輛位置和航向狀態值和觀測值的協方差：

結果輸出：

因此-可得（k+1）時刻汽車位置和航向，及其誤差方差矩陣Xk+1=[]，擴展到任意時刻，得到車輛UKF子濾波器的定位軌跡。

3.4 自適應UKF算法流程

根據所搭建的常值噪聲統計估計器以及UKF過程可以得到自適應UKF計算過程：

3.5 主濾波器的設計

聯邦濾波器具有多種結構形式，從信息反饋的角度可分為無反饋結構（NR）和有反饋結構（FR）兩種。無反饋結構的優點在于信息傳遞更及時，且具有一定的容錯能力；有反饋結構具有更強的魯棒性，精度相比于無反饋結構更高。本文搭建兩種結構形式并進行比較分析，流程框圖分別如圖2、圖3所示。聯邦濾波器中，主濾波器的作用在于對來自子濾波器的局部估計結果進行信息整合，將k時刻2個子濾波器的局部估計值X1、X2及其估計誤差的協方差矩陣P1、P2進行融合，并最終得到全局融合估計結果，即狀態的預測結果Xg及其協方差Pg。

圖2 無反饋結構的流程框圖

圖3 有反饋結構的流程框圖

無反饋結構的表達式為：

有反饋結構在無反饋結構基礎上對全局融合估計結果進行信息分配，并反饋給各子濾波器用于下一工作循環的局部估計值。因此，有反饋結構更復雜的同時，精度也有所提高。有反饋結構表達式為：

式中，β1和β2為信息分配系數。

根據式（38）～（40），計算兩種主濾波器結構得出的最后預測值。

4 仿真驗證

為了驗證本文所設計的自適應UKF算法對泊車過程中車輛位姿的估計效果，對其進行仿真驗證。仿真車輛的相關參數如表1所示。

表1 自車相關參數

其次構建基于聯邦-無損卡爾曼濾波器的位姿估計算法模塊。它接收CarSim車輛運動學模型的“虛擬”傳感器信號，包括車輪輪速信號和轉向盤轉角等，進而算出車輛的位姿信息估計值。仿真時，人為地為輪速信號添加噪聲，模擬實際傳感器中信號噪聲的干擾。同時，CarSim車輛模型也能實時地輸出車輛的位姿信息作為參考值。將參考值與估計值進行對比，便可對算法進行評估。仿真流程如圖4所示，本次仿真另外加入標準UKF與EKF算法進行對比，結果如圖5、圖6所示。

圖4 仿真分析流程框圖

圖5 3種結構估計值橫向偏差對比

圖6 3種結構估計值縱向偏差對比

由圖5可知，在X方向上，改進UKF算法、標準UKF算法、EKF算法所得到的估計值與理論值的偏差范圍分別為[-1.13,2.97]cm、[-1.51,3.28]cm、[1.32,3.87]cm。改進UKF算法估計值與理論值偏差的絕對值均小于另外兩種算法的偏差絕對值。

由圖6可知，在Y方向上，改進UKF算法、標準UKF算法、EKF算法所得到的估計值與理論值的偏差范圍分別為[0.40,4.45]cm、[1.04,5.33]cm、[1.81,5.63]cm，Y方向上3種結構的估計值均大于理想路徑值。改進UKF算法估計值與理論值的偏差絕對值均小于另外兩種算法的偏差絕對值。

綜上，改進UKF算法估計車輛位姿的精度更高。

5 硬件在環驗證

為了驗證基于改進UKF算法設計的泊車位姿估計算法的有效性，采用CarSim汽車模型的仿真環境，進行硬件在環試驗驗證。根據某款車型的參數在CarSim中設置整車模型、汽車起始點位置、車速和泊車路面工況，并在計算機中實時運行；在LabVIEW中處理和顯示各路傳感器信號，由PXI主機執行；同時，利用數據采集卡和CAN通訊接口系統采集轉向盤轉角信號，并將信號實時發送給EPS控制器和PXI主機。試驗平臺主要設備包括：校企聯合研發的APS控制板及EPS控制器、雷達控制器、轉向管柱以及各類傳感器等。為了真實模擬輪胎與地面的作用力矩，實現對阻力的真實模擬，臺架還包括伺服電機組成的阻力模擬總成，通過CarSim輸出齒條受力信息給伺服電機，由LabVIEW控制伺服電機輸出力矩給齒輪齒條轉向器。試驗平臺如圖7所示。

圖7 硬件在環試驗平臺

本文選用虛擬干燥路面進行試驗，并將車速設為恒定3.6 km/h。啟動自動泊車功能后，接口系統采集硬件系統中的傳感器信號并處理之后實現與軟件系統中運行的整車模型及控制器進行信息實時交換。

試驗結果如圖8所示：EKF算法估計路徑與實際路徑差異始終較大，在相同橫坐標下，在Y方向上的最大偏差為21.9 cm；標準UKF算法估計結果也出現局部精度下降的情況，在相同橫坐標下，在Y方向上的最大偏差為13.1 cm；改進UKF算法估計結果始終貼合實際路徑，在相同橫坐標下，在Y方向上的最大偏差為7.2 cm。

圖8 位姿估計效果對比

綜上，本文采用的改進UKF位姿估計算法具有更高的精度和更好的收斂性。

6 結束語

本文從估計泊車位姿的角度出發，在傳統UKF方法的基礎上添加常值噪聲估計器，并采用精確度更高的綜合性Sigma點集采樣策略構成改進UKF算法。相比于傳統位姿估計算法，該算法對UKF的穩定性沒有影響，更適用于泊車過程中的車輛位姿估計，且不要求精確已知噪聲的先驗統計。利用傳感器采集得出的信號作為觀測變量，結合由阿克曼轉向幾何得出的運動學狀態方程進行時間更新以及測量更新，搭建兩種聯邦卡爾曼濾波結構，由主濾波器輸出估計的最優值。在MATLAB/Simulink軟件中進行了仿真驗證，并在基于CarSim整車模型和LabVIEW數據采集處理的試驗臺進行了硬件在環試驗，驗證了算法的有效性，為自動泊車系統實時估計預測車輛位姿提供支持，同時也增加了泊車過程的安全性與可控性。