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

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

主題詞：自動(dòng)泊車 位姿估計(jì) 改進(jìn)UKF濾波算法 常值噪聲統(tǒng)計(jì)估計(jì)器

1 前言

自動(dòng)泊車系統(tǒng)（Automatic Parking System，APS）是智能駕駛系統(tǒng)的典型功能，通過傳感器獲得輪速等信號(hào)并與算法相結(jié)合預(yù)測泊車過程中車輛的位姿狀態(tài)，對于構(gòu)建完整可靠的自動(dòng)泊車系統(tǒng)有一定的應(yīng)用價(jià)值。

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

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

2 車輛運(yùn)動(dòng)學(xué)模型的建立

阿克曼理論的前提為假設(shè)汽車在轉(zhuǎn)向過程中所有車輪均為滾動(dòng)狀態(tài)，此時(shí)車輛不受側(cè)向力作用，所有車輪轉(zhuǎn)向軸線交于一點(diǎn)，整車可以視為一個(gè)剛體。

由于車輛泊車過程中需要保持恒定低車速，可假設(shè)車輛運(yùn)動(dòng)不受側(cè)向力作用，車輪沒有發(fā)生滑移，因此可以基于阿克曼轉(zhuǎn)向原理建立運(yùn)動(dòng)學(xué)模型進(jìn)行研究。

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

圖1 車輛運(yùn)動(dòng)學(xué)模型示意

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

由圖1可知，前、后軸中心點(diǎn)坐標(biāo)的關(guān)系為：

對式（2）求導(dǎo)，可得其速度關(guān)系為：

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

前軸中心點(diǎn)在x、y方向上的速度分別為：

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

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

將式（7）積分可得后軸中心點(diǎn)運(yùn)動(dòng)軌跡方程為：

3 改進(jìn)UKF濾波器的搭建

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

考慮到泊車過程中車輛運(yùn)動(dòng)模型的狀態(tài)方程和觀測方程均含有非線性這一特點(diǎn)，且系統(tǒng)中含有先驗(yàn)統(tǒng)計(jì)未知的噪聲，假設(shè)均為常值噪聲，故采用加入常值噪聲統(tǒng)計(jì)估計(jì)器的改進(jìn)UKF算法分別實(shí)現(xiàn)聯(lián)邦濾波結(jié)構(gòu)中兩個(gè)子濾波器的位姿估計(jì)結(jié)果。

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

非線性離散系統(tǒng)模型為：

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

3.1 子濾波器的設(shè)計(jì)

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

其中，觀測變量為：

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

f（φ）通過試驗(yàn)標(biāo)定的方法確定，試驗(yàn)簡要過程如下：采集轉(zhuǎn)向盤轉(zhuǎn)角和該轉(zhuǎn)角下車輛后軸中點(diǎn)的轉(zhuǎn)彎半徑，然后得出離散數(shù)據(jù)表，實(shí)際應(yīng)用中對其進(jìn)行插值便可獲得后軸中點(diǎn)的轉(zhuǎn)彎半徑。

子系統(tǒng)1、2的狀態(tài)方程為：

由式（7）和式（8）可得狀態(tài)方程為：

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

3.2 子濾波器常值噪聲統(tǒng)計(jì)估計(jì)器的設(shè)計(jì)

在自動(dòng)泊車過程中，對于車輛位姿的估計(jì)不同于傳統(tǒng)UKF濾波過程，濾波過程中的狀態(tài)噪聲和測量噪聲處于先驗(yàn)統(tǒng)計(jì)未知狀態(tài)，均值均不為零，因此假設(shè)系統(tǒng)噪聲為常值噪聲，需設(shè)計(jì)常值噪聲估計(jì)器，對其進(jìn)行估計(jì)。

基于上述分析，對傳統(tǒng)UKF算法進(jìn)行改進(jìn)。首先提出2個(gè)假設(shè)[12]：

a.Wk和Vk為互不相關(guān)的高斯白噪聲，且具有如下常值統(tǒng)計(jì)特性：

b. 初始狀態(tài)X0與Wk、Vk互不相關(guān)，且服從正態(tài)分布，其均值和協(xié)方差矩陣為：

根據(jù)文獻(xiàn)[12]中的定理一、定理二可以得出次優(yōu)無偏MAP常值噪聲統(tǒng)計(jì)估計(jì)器q?k、Q?k、r?k、R?k。

3.3 子系統(tǒng)局部位姿估計(jì)過程

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

3.3.1 初始化

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

3.3.2 計(jì)算Sigma點(diǎn)

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

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

當(dāng)i＞500時(shí)，采用蒙特卡洛采樣策略生成Sigma點(diǎn)，其步驟為：建立滿足均值為-X，方差為D-X的高斯分布概率模型；對概率模型進(jìn)行隨機(jī)抽樣；由抽樣結(jié)果組成Sigma點(diǎn)集。

隨后，直接調(diào)用MATLAB軟件產(chǎn)生normrnd函數(shù)（A=normrnd（ave,sig,m）），返回m個(gè)均值為ave，標(biāo)準(zhǔn)差為sig的高斯分布的隨機(jī)數(shù)A。采用蒙特卡洛采樣策略，對均值為-Xk、方差為D-Xk的n維隨機(jī)變量X，生成的m個(gè)Sigma點(diǎn)集為：

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

3.3.3 時(shí)間更新

用Sigma點(diǎn)集矩陣代替狀態(tài)方程中的X，得到新的狀態(tài)方程：

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

對（2L+1）個(gè)狀態(tài)預(yù)測值進(jìn)行加權(quán)求和，可得到車輛位置和航向的狀態(tài)預(yù)測值為：

同時(shí)，可計(jì)算得到車輛位置和航向狀態(tài)方差為：

式中，Qk+1為（k+1）時(shí)刻車輛位置和航向狀態(tài)方程的噪聲方差矩陣；Pk+1|k為狀態(tài)估計(jì)方差矩陣。

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

同時(shí)可計(jì)算得到車輛位置和航向觀測方差為：

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

車輛定位過程中統(tǒng)計(jì)特性權(quán)系數(shù)為：

3.3.4 測量更新

在時(shí)間更新后，繼續(xù)對系統(tǒng)進(jìn)行測量更新。利用車輛位置和航向狀態(tài)值以及觀測值，計(jì)算車輛位置和航向狀態(tài)值和觀測值的協(xié)方差：

結(jié)果輸出：

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

3.4 自適應(yīng)UKF算法流程

根據(jù)所搭建的常值噪聲統(tǒng)計(jì)估計(jì)器以及UKF過程可以得到自適應(yīng)UKF計(jì)算過程：

3.5 主濾波器的設(shè)計(jì)

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

圖2 無反饋結(jié)構(gòu)的流程框圖

圖3 有反饋結(jié)構(gòu)的流程框圖

無反饋結(jié)構(gòu)的表達(dá)式為：

有反饋結(jié)構(gòu)在無反饋結(jié)構(gòu)基礎(chǔ)上對全局融合估計(jì)結(jié)果進(jìn)行信息分配，并反饋給各子濾波器用于下一工作循環(huán)的局部估計(jì)值。因此，有反饋結(jié)構(gòu)更復(fù)雜的同時(shí)，精度也有所提高。有反饋結(jié)構(gòu)表達(dá)式為：

式中，β1和β2為信息分配系數(shù)。

根據(jù)式（38）～（40），計(jì)算兩種主濾波器結(jié)構(gòu)得出的最后預(yù)測值。

4 仿真驗(yàn)證

為了驗(yàn)證本文所設(shè)計(jì)的自適應(yīng)UKF算法對泊車過程中車輛位姿的估計(jì)效果，對其進(jìn)行仿真驗(yàn)證。仿真車輛的相關(guān)參數(shù)如表1所示。

表1 自車相關(guān)參數(shù)

其次構(gòu)建基于聯(lián)邦-無損卡爾曼濾波器的位姿估計(jì)算法模塊。它接收CarSim車輛運(yùn)動(dòng)學(xué)模型的“虛擬”傳感器信號(hào)，包括車輪輪速信號(hào)和轉(zhuǎn)向盤轉(zhuǎn)角等，進(jìn)而算出車輛的位姿信息估計(jì)值。仿真時(shí)，人為地為輪速信號(hào)添加噪聲，模擬實(shí)際傳感器中信號(hào)噪聲的干擾。同時(shí)，CarSim車輛模型也能實(shí)時(shí)地輸出車輛的位姿信息作為參考值。將參考值與估計(jì)值進(jìn)行對比，便可對算法進(jìn)行評估。仿真流程如圖4所示，本次仿真另外加入標(biāo)準(zhǔn)UKF與EKF算法進(jìn)行對比，結(jié)果如圖5、圖6所示。

圖4 仿真分析流程框圖

圖5 3種結(jié)構(gòu)估計(jì)值橫向偏差對比

圖6 3種結(jié)構(gòu)估計(jì)值縱向偏差對比

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

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

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

5 硬件在環(huán)驗(yàn)證

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

圖7 硬件在環(huán)試驗(yàn)平臺(tái)

本文選用虛擬干燥路面進(jìn)行試驗(yàn)，并將車速設(shè)為恒定3.6 km/h。啟動(dòng)自動(dòng)泊車功能后，接口系統(tǒng)采集硬件系統(tǒng)中的傳感器信號(hào)并處理之后實(shí)現(xiàn)與軟件系統(tǒng)中運(yùn)行的整車模型及控制器進(jìn)行信息實(shí)時(shí)交換。

試驗(yàn)結(jié)果如圖8所示：EKF算法估計(jì)路徑與實(shí)際路徑差異始終較大，在相同橫坐標(biāo)下，在Y方向上的最大偏差為21.9 cm；標(biāo)準(zhǔn)UKF算法估計(jì)結(jié)果也出現(xiàn)局部精度下降的情況，在相同橫坐標(biāo)下，在Y方向上的最大偏差為13.1 cm；改進(jìn)UKF算法估計(jì)結(jié)果始終貼合實(shí)際路徑，在相同橫坐標(biāo)下，在Y方向上的最大偏差為7.2 cm。

圖8 位姿估計(jì)效果對比

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

6 結(jié)束語

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