邱 潤(rùn)，黎敬濤，李孝疆，楊改娣，宋開(kāi)雨

（昆明理工大學(xué)，云南 昆明 650504）

0 引言

自從20 世紀(jì)70 年代美國(guó)國(guó)家航空航天局（NASA）運(yùn)用卡爾曼濾波解決阿波羅登月計(jì)劃的軌道預(yù)測(cè)問(wèn)題后，卡爾曼濾波突破了當(dāng)時(shí)工程領(lǐng)域維納濾波的局限，其原理是通過(guò)觀測(cè)值與理論實(shí)際數(shù)據(jù)融合得到當(dāng)前的最佳預(yù)估值[1]。觀測(cè)數(shù)據(jù)中包含噪聲，卡爾曼濾波就是對(duì)包含噪聲的數(shù)據(jù)進(jìn)行處理，卡爾曼最佳估計(jì)也可以看作是一個(gè)濾波過(guò)程[2]?？柭鼮V波有兩種建模方法：一種是基于控制理論通過(guò)運(yùn)動(dòng)模型根據(jù)前一時(shí)刻的狀態(tài)來(lái)估計(jì)得到當(dāng)前時(shí)刻的最佳估計(jì)值與協(xié)方差矩陣，以此類推進(jìn)行下一時(shí)刻的最佳估計(jì)，這個(gè)建模求解的重點(diǎn)在于卡爾曼增益；另一種是利用一些概率性質(zhì)進(jìn)行建模的概率模型。本文將卡爾曼濾波運(yùn)用到行車(chē)的軌跡預(yù)測(cè)中，通過(guò)運(yùn)動(dòng)系統(tǒng)方程、測(cè)量模型以及它們的誤差來(lái)求得卡爾曼增益，隨后用卡爾曼濾波后的值反過(guò)來(lái)修正誤差，從而在行車(chē)過(guò)程中獲得最佳的軌跡預(yù)測(cè)。

1 卡爾曼濾波理論

1.1 卡爾曼濾波的預(yù)測(cè)過(guò)程

（1）通過(guò)狀態(tài)估計(jì)公式得到此時(shí)刻的先驗(yàn)估計(jì)值，即：

式中：和ut-1分別為上一時(shí)刻的先驗(yàn)估計(jì)值和控制量，A為狀態(tài)轉(zhuǎn)移矩陣，B為控制矩陣。

（2）通過(guò)當(dāng)前時(shí)刻的先驗(yàn)誤差來(lái)表示系統(tǒng)包含的不確定性，即：

式中：Pt-1為上一時(shí)刻的誤差，Q為過(guò)程噪聲的方差。

在車(chē)輛的運(yùn)行過(guò)程中，受傳感器測(cè)量精度問(wèn)題的影響會(huì)存在方差為R的測(cè)量噪聲Vt。而建立的運(yùn)動(dòng)系統(tǒng)也不是百分百符合真實(shí)運(yùn)動(dòng)，即存在方差為Q的過(guò)程噪聲w。這兩個(gè)噪聲都服從期望為零的標(biāo)準(zhǔn)正態(tài)分布，即：

根據(jù)觀測(cè)矩陣H、先驗(yàn)誤差以及測(cè)量噪聲的方差R，計(jì)算卡爾曼增益系數(shù)為：

（3）通過(guò)先驗(yàn)估計(jì)值、真實(shí)值Zt與卡爾曼增益Kt，計(jì)算最佳估計(jì)值為：

隨著車(chē)輛運(yùn)動(dòng)的進(jìn)行，系統(tǒng)會(huì)不斷積累誤差，需要通過(guò)協(xié)方差矩陣來(lái)不斷更新誤差[3]，即：

式中：Pt為更新后的誤差，I為單位矩陣。

1.2 卡爾曼濾波的最新研究

卡爾曼濾波擁有廣泛的應(yīng)用領(lǐng)域，例如故障診斷[4]、系統(tǒng)監(jiān)測(cè)[5-6]、巡航制導(dǎo)等[7]。文獻(xiàn)[8]在精密工程中運(yùn)用擴(kuò)展卡爾曼濾波進(jìn)行故障診斷，對(duì)工程結(jié)構(gòu)進(jìn)行探傷，成功降低了測(cè)量噪聲的影響，準(zhǔn)確鎖定損傷位置并掌握損傷程度。文獻(xiàn)[9]將卡爾曼濾波運(yùn)用到建筑工程領(lǐng)域，通過(guò)建立時(shí)序預(yù)測(cè)模型對(duì)大壩的滲流、位移等檢測(cè)數(shù)據(jù)做出預(yù)測(cè)，從而掌握數(shù)據(jù)的變化趨勢(shì)，實(shí)現(xiàn)對(duì)大壩危險(xiǎn)情況的預(yù)測(cè)。袁等人[10]運(yùn)用低成本深度傳感器收集行人運(yùn)動(dòng)的數(shù)據(jù)，通過(guò)卡爾曼濾波獲取狀態(tài)的預(yù)估值，從而獲得行人的道路軌跡預(yù)測(cè)，彌補(bǔ)了傳統(tǒng)軌跡預(yù)測(cè)算法無(wú)法挖掘行走意圖的缺陷。

2 構(gòu)建小車(chē)運(yùn)動(dòng)模型

2.1 簡(jiǎn)化阿克曼運(yùn)動(dòng)模型

在全局坐標(biāo)系XOY下，連接車(chē)輛重心（A，B兩點(diǎn)），將車(chē)輛的正前方視為橫軸指向（OB）、車(chē)輛的左側(cè)面視為縱軸指向（OA），建立參考坐標(biāo)系xoy，如圖1 所示。嚴(yán)格來(lái)說(shuō)，需要根據(jù)阿克曼轉(zhuǎn)向理論建立車(chē)輛的運(yùn)動(dòng)模型[11]。但是在合理的假設(shè)條件下，可以將車(chē)輛的運(yùn)動(dòng)看作是質(zhì)點(diǎn)運(yùn)動(dòng)，將阿克曼轉(zhuǎn)向幾何模型簡(jiǎn)化成自行車(chē)運(yùn)動(dòng)模型。將車(chē)輛前后四輪簡(jiǎn)化為前后各一個(gè)輪，前后輪間用車(chē)軸進(jìn)行連接[12]。

圖1 阿克曼轉(zhuǎn)向理論簡(jiǎn)化模型

選取參考點(diǎn)C，ψ為車(chē)輛方向的橫擺角，δ為車(chē)輛轉(zhuǎn)向角度，β為車(chē)輛滑移角（車(chē)輛的航向與中心點(diǎn)速度Vc的夾角），L為小車(chē)的軸距。假設(shè)C 到前輪的距離為L(zhǎng)f，C 到后輪的距離為L(zhǎng)r，則Lf+Lr=L。對(duì)于自行車(chē)運(yùn)動(dòng)模型來(lái)說(shuō)，小車(chē)每時(shí)每刻的速度方向都與過(guò)車(chē)身質(zhì)點(diǎn)的旋轉(zhuǎn)半徑相垂直，即A 點(diǎn)的旋轉(zhuǎn)半徑RA（OA）垂直于Va，B 點(diǎn)的旋轉(zhuǎn)半徑RB（OB）垂直于Vb，C 點(diǎn)旋轉(zhuǎn)半徑R（OC）垂直于Vc。在相同的時(shí)間間隔內(nèi)，小車(chē)在X方向和Y方向上位移的增量可以通過(guò)速度V來(lái)表示。

根據(jù)橫擺角計(jì)算起始兩點(diǎn)在全局坐標(biāo)上的分速度，可以構(gòu)建微分方程：

將點(diǎn)C 選在車(chē)輛后方，此時(shí)Lr=0，則Lf=L，這樣就可以達(dá)到任意時(shí)刻車(chē)輛的滑移角β為0。根據(jù)角速度公式可以得到：

通過(guò)車(chē)輛轉(zhuǎn)向角度δ可以計(jì)算出車(chē)輛的轉(zhuǎn)彎半徑R，根據(jù)速度進(jìn)而可以得知橫擺角ψ。根據(jù)橫擺角度ψ，可以在單位時(shí)刻求出車(chē)輛在全局坐標(biāo)系X方向的位移ΔX和在Y方向的位移ΔY，則小車(chē)的位移模型為：

在全局坐標(biāo)系中，當(dāng)車(chē)輛平行于X軸運(yùn)動(dòng)時(shí)，初始t=0，擺角ψ0=0。當(dāng)小車(chē)轉(zhuǎn)向δ0角度行駛到下一時(shí)刻（t=1）時(shí)，擺角更新為ψ1=ψ0+δ0=δ0；當(dāng)t=2、轉(zhuǎn)向角為δ1時(shí)，擺角更新為ψ2=ψ1+δ1=δ0+δ1。由此可知，車(chē)輛擺角與轉(zhuǎn)向角的關(guān)系為ψt=ψt-1+δt-1。

2.2 卡爾曼濾波軌跡預(yù)測(cè)建模

將模型預(yù)測(cè)控制理論與狀態(tài)參數(shù)估計(jì)相結(jié)合，以此來(lái)預(yù)測(cè)車(chē)輛的運(yùn)動(dòng)軌跡[13]。根據(jù)小車(chē)在建模過(guò)程中的運(yùn)動(dòng)系統(tǒng)是否符合線性系統(tǒng)特征，可以將其運(yùn)動(dòng)具體區(qū)分為兩種狀態(tài)。一種是無(wú)轉(zhuǎn)向角時(shí)的直線運(yùn)動(dòng)，屬于線性運(yùn)動(dòng)系統(tǒng)；另一種是存在轉(zhuǎn)向角而不滿足線性要求的轉(zhuǎn)向運(yùn)動(dòng)，屬于非線性運(yùn)動(dòng)系統(tǒng)。在具體應(yīng)用時(shí)，必須先將非線性系統(tǒng)線性化。

直線運(yùn)動(dòng)模型的輸出只取xt和yt的值，則根據(jù)觀測(cè)方程得到：

將小車(chē)的運(yùn)動(dòng)看作是簡(jiǎn)單的自行車(chē)運(yùn)動(dòng)模型，當(dāng)小車(chē)轉(zhuǎn)向時(shí)，車(chē)輛的轉(zhuǎn)向角度等于車(chē)輛前輪的偏角，如圖2 所示。

圖2 全局坐標(biāo)系下的車(chē)輛運(yùn)動(dòng)

在全局坐標(biāo)系中，小車(chē)通過(guò)單位時(shí)間由上一位置St-1行駛到當(dāng)前位置St，空間狀態(tài)方程方程為：

由于轉(zhuǎn)向運(yùn)動(dòng)系統(tǒng)的輸入方程包含三角函數(shù)，屬于非線性系統(tǒng)方程，因此需要通過(guò)泰勒級(jí)數(shù)將其線性化。通過(guò)狀態(tài)量X=[xy]T與控制量u=[vψ]T得到相應(yīng)的雅克比矩陣A與B，即：

同直線運(yùn)動(dòng)一樣，這里輸出也只取xt和yt的值，根據(jù)觀測(cè)方程得到觀測(cè)矩陣H為單位矩陣。

3 卡爾曼濾波預(yù)測(cè)模型仿真分析

在Simulink 中進(jìn)行實(shí)驗(yàn)，建模主要分為車(chē)輛的物理建模部分和卡爾曼濾波部分。自行車(chē)縱向運(yùn)動(dòng)物理模型如圖3 所示，其中WhlAngF 為前輪轉(zhuǎn)向角，xdotin 為橫軸方向的速度，xdot 為車(chē)輛的橫向速度，ydot 為車(chē)輛的縱向速度，psi 為偏航角，r為偏航率，F(xiàn)z-F 與Fz-R 為前后輪的法向力。此外，Info 端口包含該模型的所有信息，可以通過(guò)連接一個(gè)總線選擇器來(lái)選擇實(shí)驗(yàn)所需的信息。

圖3 自行車(chē)運(yùn)動(dòng)模型

通過(guò)設(shè)置初始的橫向速度和車(chē)輛轉(zhuǎn)向角，得到車(chē)輛在某仿真時(shí)刻的橫向速度和縱向速度，進(jìn)而可以得到某時(shí)刻的車(chē)輛狀態(tài)。設(shè)定橫軸方向速度為2 m/s、縱軸速度為0 m/s、仿真總時(shí)長(zhǎng)為20 s、過(guò)程誤差為0.001 且測(cè)量誤差為0.1，則小車(chē)無(wú)轉(zhuǎn)向角的直線運(yùn)動(dòng)軌跡仿真如圖4（a）所示，小車(chē)轉(zhuǎn)向角45°下做轉(zhuǎn)向圓周運(yùn)動(dòng)的軌跡仿真如圖4（b）所示。

圖4 軌跡仿真結(jié)果

實(shí)驗(yàn)表明，卡爾曼濾波后的預(yù)測(cè)軌跡相比于觀測(cè)軌跡更趨近于真實(shí)軌跡。

車(chē)輛在直線運(yùn)動(dòng)中真實(shí)軌跡值與測(cè)量軌跡值間的誤差如圖5（a）所示，真實(shí)軌跡值與卡爾曼濾波預(yù)測(cè)軌跡值間的誤差如圖5（b）所示。

根據(jù)圖5，在直線運(yùn)動(dòng)t=6.9 s 時(shí)，測(cè)量誤差約為2.2 m。經(jīng)過(guò)卡爾曼濾波后，誤差約為1.2 m，與測(cè)量誤差相比減小了約1.0 m，證明卡爾曼濾波有效降低了誤差。

圖5 誤差對(duì)比

小車(chē)直線運(yùn)動(dòng)位移仿真結(jié)果如圖6（a）所示，轉(zhuǎn)向運(yùn)動(dòng)位移仿真結(jié)果如圖6（b）所示。

圖6 運(yùn)動(dòng)位移仿真

由圖6（a）可知，由于轉(zhuǎn)向角為0 且過(guò)程誤差較小，因此小車(chē)實(shí)際的位移呈線性增加，而且濾波后的估計(jì)值更趨近于真實(shí)值。由圖6（b）可知，由于測(cè)量值與真實(shí)值間的誤差較大，因此波動(dòng)幅度也較大，通過(guò)卡爾曼濾波后的值相較于測(cè)量值更貼近于真實(shí)值。與此同時(shí)，由于小車(chē)從原點(diǎn)開(kāi)始做圓周運(yùn)動(dòng)，因此從t=0 時(shí)刻開(kāi)始，小車(chē)的位移先增加再減少，此后循環(huán)增加再減少。

4 結(jié)語(yǔ)

本文根據(jù)小車(chē)運(yùn)動(dòng)方程建立小車(chē)運(yùn)動(dòng)物理模型，通過(guò)Simulink 進(jìn)行實(shí)驗(yàn)仿真，驗(yàn)證了卡爾曼濾波的軌跡預(yù)測(cè)的可行性。實(shí)驗(yàn)表明，卡爾曼濾波后的軌跡比觀測(cè)出來(lái)的測(cè)量軌跡更加貼近于真實(shí)值，在測(cè)量誤差的基礎(chǔ)上降低了估計(jì)誤差。但是運(yùn)用自行車(chē)運(yùn)動(dòng)模型對(duì)車(chē)輛建模進(jìn)行了簡(jiǎn)化，未來(lái)將基于詳細(xì)具體的阿克曼轉(zhuǎn)向原理進(jìn)行建模分析。