針對(duì)小車狀態(tài)感知的卡爾曼濾波多傳感器融合算法

任其亮，程昊東

(重慶交通大學(xué) 交通運(yùn)輸學(xué)院， 重慶 400074)

0 引言

隨著無人駕駛的蓬勃發(fā)展，定位技術(shù)一直是當(dāng)前的研究熱點(diǎn)。但是，現(xiàn)存的室內(nèi)定位系統(tǒng)主要搭載的是大型家用車，移動(dòng)小車的定位始終沒有突破性的進(jìn)展。當(dāng)下移動(dòng)小車定位研究最大瓶頸是：與大型小車相比，小車所處的環(huán)境更加復(fù)雜，無法精確標(biāo)定小車位置[1]，并且小車定位精度與定位成本難以兼顧。因此，探索一種高精度、低成本的小車室外定位系統(tǒng)很有必要。

在室外定位系統(tǒng)中，標(biāo)定自主移動(dòng)小車相對(duì)于車道線的參考位置是實(shí)現(xiàn)小車在未知環(huán)境里自主導(dǎo)航、狀態(tài)感知等功能的重要基礎(chǔ)。針對(duì)標(biāo)定過程普遍存在的定位精度差這一問題，大多學(xué)者提出了多個(gè)傳感器相融合的方法來解決，成悅等[2]提出基于置信度的加權(quán)特征融合相關(guān)濾波跟蹤方法，將互補(bǔ)的梯度和顏色特征進(jìn)行融合；付廷強(qiáng)[3]提出了基于GNSS、慣性導(dǎo)航系統(tǒng)、視覺傳感器相融合的定位方法；Zhang等[4]提出了IMU、視覺、激光3種傳感器相融合的方法。上述方法有效提升了定位精度和穩(wěn)定性，改善了單一傳感器的測(cè)量缺陷，但是存在著安裝繁瑣、計(jì)算復(fù)雜、成本較高并且缺乏位姿預(yù)測(cè)等問題。基于以上分析，本文選取精確度較高的視覺傳感器和雷達(dá)作為小車在實(shí)際坐標(biāo)系下相對(duì)于車道線位置的測(cè)量工具，并對(duì)多種傳感器獲得的原始數(shù)據(jù)進(jìn)行處理，盡可能消除傳感器自身存在的漂移特性及其他因素對(duì)測(cè)量結(jié)果精度的影響。數(shù)據(jù)濾波方法有很多，其中應(yīng)用最廣的是卡爾曼濾波。卡爾曼濾波是一種利用線性系統(tǒng)的狀態(tài)方程，通過系統(tǒng)的觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法，Zong等[5]提出了用2個(gè)擴(kuò)展卡爾曼濾波同時(shí)處理的方法估計(jì)小車狀態(tài)；李剛等[6]提出了一種基于改進(jìn)的Sage-Husa自適應(yīng)擴(kuò)展卡爾曼濾波的車輛行駛狀態(tài)估計(jì)算法。傳統(tǒng)卡爾曼濾波在非線性的算法下無法達(dá)到精度要求，而擴(kuò)展卡爾曼濾波實(shí)時(shí)性較強(qiáng)，能夠?qū)Σ杉降臄?shù)據(jù)進(jìn)行更新和處理，適合用于多傳感器的融合定位[7]。在本文中，由于小車運(yùn)動(dòng)呈非線性狀態(tài)，且短時(shí)間內(nèi)很難發(fā)生狀態(tài)突變，故采用改進(jìn)擴(kuò)展卡爾曼濾波在測(cè)量方差已知的情況下，從一系列夾雜噪聲的數(shù)據(jù)中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)[8]。

1 相關(guān)模型的建立

1.1 小車運(yùn)動(dòng)模型

根據(jù)復(fù)雜程度可以將常用的運(yùn)動(dòng)模型分為一次運(yùn)動(dòng)模型(也別稱為線性運(yùn)動(dòng)模型)和二次運(yùn)動(dòng)模型，其中二次運(yùn)動(dòng)模型可分為恒定轉(zhuǎn)率和加速度模型(constant turn rate and acceleration，CTRA)和恒定轉(zhuǎn)率和速度模型(constant turn rate and velocity，CTRV)。考慮到非全向四輪小車的運(yùn)動(dòng)特性以及傳感器測(cè)量間隔，本文中默認(rèn)小車的運(yùn)動(dòng)模型為恒定轉(zhuǎn)率和速度模型(CTRV)[7]。當(dāng)該小車運(yùn)動(dòng)時(shí)，其運(yùn)動(dòng)實(shí)時(shí)狀態(tài)量為:

X(t)=(x,y,v,θ,ω)

(1)

式中：θ為偏航角，取值范圍是[0,2π]；ω為偏航角速度。CTRV的狀態(tài)轉(zhuǎn)移函數(shù)為：

(2)

同時(shí)還存在一個(gè)問題，那就是ω=0的情況，此時(shí)的狀態(tài)轉(zhuǎn)移函數(shù)公式中的(x,y)將變成無窮大。因此需要將ω=0(即直線行駛)時(shí)的情況拆解開來單獨(dú)分析：此時(shí)運(yùn)動(dòng)模型為恒定速度模型(constant velocity)，所以(x,y)的計(jì)算公式為：

x(t+Δt)=vcos(θ)Δt+x(t)

(3)

y(t+Δt)=vsin(θ)Δt+y(t)

(4)

1.2 擴(kuò)展卡爾曼濾波模型

考慮到小車的運(yùn)動(dòng)是非線性的，需要引入擴(kuò)展卡爾曼濾波來處理，則原卡爾曼濾波器預(yù)測(cè)的第一步變成了如下非線性函數(shù)：

xk+1=g(xk,u)

(5)

式中：g為狀態(tài)轉(zhuǎn)移函數(shù)；u為噪聲。

由于該狀態(tài)轉(zhuǎn)移函數(shù)是非線性，因此需要對(duì)其進(jìn)行線性化，也就是求解相應(yīng)的雅可比矩陣[9]。在CTRV模型中，對(duì)各個(gè)元素求偏導(dǎo)數(shù)就可以得到相應(yīng)狀態(tài)雅可比矩陣。J1為ω≠0時(shí)的狀態(tài)轉(zhuǎn)移雅可比矩陣，J2為ω=0時(shí)的狀態(tài)轉(zhuǎn)移雅可比矩陣。

(6)

(7)

1.3 傳感器測(cè)量模型

假設(shè)有雷達(dá)和視覺傳感器，分別以一定的頻率來測(cè)量[10]。

1) 視覺傳感器：在里程計(jì)坐標(biāo)系(ODOM)上測(cè)量目標(biāo)車輛的坐標(biāo)(x,y,v,θ,ω)。這里的x,y是相對(duì)于車輛坐標(biāo)系的，即車輛為坐標(biāo)系的原點(diǎn)，車頭為X軸，車的左側(cè)為Y軸，車頭偏角為θ。其測(cè)量模型是線性的，對(duì)應(yīng)測(cè)量矩陣為：

(8)

2) 雷達(dá)：通過雷達(dá)坐標(biāo)系反饋出當(dāng)前運(yùn)動(dòng)車輛和中心線的相對(duì)位置關(guān)系(d,φ)，反映的是車道線與車輛的距離和角度關(guān)系，轉(zhuǎn)化為雷達(dá)坐標(biāo)系下的標(biāo)準(zhǔn)形式，其測(cè)量矩陣為：

(9)

2 整體融合方法

由于2個(gè)傳感器測(cè)量對(duì)象并非完全相同，因此最佳融合過程為交叉融合，在載體運(yùn)動(dòng)的短時(shí)間內(nèi)，雷達(dá)測(cè)量的數(shù)據(jù)精度高，但由于白噪聲以及長(zhǎng)時(shí)間的工作會(huì)累計(jì)誤差，精度降低；視覺傳感器提供車輛實(shí)時(shí)的狀態(tài)信息。因此采用信息融合的方法，將彼此的優(yōu)缺點(diǎn)進(jìn)行互補(bǔ)，計(jì)算出更為準(zhǔn)確的小車與中心線的距離和角度[11]。整體實(shí)現(xiàn)方法如圖1所示，對(duì)視覺傳感器測(cè)得的小車實(shí)時(shí)運(yùn)動(dòng)數(shù)據(jù)進(jìn)行誤差補(bǔ)償和濾波，并將其視為預(yù)測(cè)值，和雷達(dá)所得到的觀測(cè)值進(jìn)行融合，可以得到融合后的結(jié)果。

圖1 傳感器融合算法流程示意圖

視覺傳感器和雷達(dá)在測(cè)量數(shù)據(jù)是一個(gè)線性離散隨機(jī)的過程，各狀態(tài)參量之間的轉(zhuǎn)換關(guān)系由圖2所示。

圖2 雷達(dá)與視覺傳感器數(shù)據(jù)轉(zhuǎn)換示意圖

2.1 構(gòu)建EKF方程

轉(zhuǎn)換完成后，可以開始濾波，注意實(shí)驗(yàn)?zāi)康臑榈玫叫≤嚺c中心線的距離和角度，因此小車實(shí)時(shí)狀態(tài)可表現(xiàn)為:

(9)

則姿態(tài)角相應(yīng)狀態(tài)方程和測(cè)量方程分別為：

X(k)=Ax(k-1)+BU(k)+ω(k)

(10)

Z(k)=HX(k)+v(k)

(11)

式中：視覺傳感器的角度為預(yù)測(cè)角度，ω(k)為噪聲誤差；雷達(dá)的角度為實(shí)時(shí)測(cè)量角度，v(k)為噪聲誤差；A、B為系統(tǒng)矩陣；U(k)為控制矩陣；H為測(cè)量矩陣。

根據(jù)卡爾曼濾波系統(tǒng)，k-1時(shí)刻的狀態(tài)用于預(yù)測(cè)k時(shí)刻的先驗(yàn)估計(jì),構(gòu)建濾波方程。

1) 狀態(tài)先驗(yàn)方程。

X(k/(k-1))=AX((k-1)/(k-1))+BU(k)

(12)

2) 先驗(yàn)均方誤差方程。

P(k/k)=AP(k/(k-1))AT+Q

(13)

式中：Q為視覺傳感器噪聲協(xié)方差。

3) 狀態(tài)估計(jì)方程。

X(k/k)=X(k/(k-1))+

Kg(k)[Z(k)-HX(k/(k-1))]

(14)

式中：Kg(k)為k時(shí)刻的卡爾曼增益，其表達(dá)式為：

Kg(k)=P(k/(k-1))HT/[HP(k/(k-1))HT+R]

(15)

式中：R為雷達(dá)噪聲協(xié)方差。

4) 估計(jì)均方誤差方程。

P(k/k)=[I-Kg(k)H]P(k/(k-1))

(16)

式中，X(k/k)為當(dāng)前時(shí)刻狀態(tài)的濾波值，為得到最優(yōu)濾波效果，協(xié)方差矩陣Q,R需要時(shí)刻更新。

2.2 Q,R的更新求解

在本模型中，狀態(tài)的影響表達(dá)矩陣為：

(17)

式中：ua為直線加速度誤差；uw為偏航角加速度誤差。

Q是處理噪聲的協(xié)方差矩陣，根據(jù)協(xié)方差矩陣定義可推導(dǎo)出其表達(dá)式為：

G*E[u*uT]*GT

(18)

視覺傳感器，雷達(dá)的測(cè)量誤差為獨(dú)立的零均值白噪聲序列，由上面狀態(tài)影響表達(dá)矩陣可得，在某一時(shí)刻k，視覺傳感器和雷達(dá)的狀態(tài)估計(jì)誤差可分別表示為:Δtkuo，Δtkul。

(19)

相應(yīng)的協(xié)方差矩陣為：

cov(f,f)=E[(f-Ef)2]=

(20)

由于傳感器具有單一獨(dú)立性，其測(cè)量誤差與上一時(shí)刻相比相互獨(dú)立，因此應(yīng)有：

則：

(21)

對(duì)于本小車而言，其搭載的雷達(dá)誤差遠(yuǎn)小于視覺傳感器，且前后相鄰時(shí)刻傳感器誤差相差極小，因此則有如下合理假設(shè):

(22)

(23)

將式(22)—(23)代入式(12)中，可得：

(24)

對(duì)于該隨機(jī)變量，其2階中心矩為其方差，因此，R為：

(25)

將濾波融合后的Q和R用于數(shù)據(jù)更新，即可得到最優(yōu)的狀態(tài)。

3 實(shí)驗(yàn)及其誤差分析

3.1 實(shí)驗(yàn)小車模型介紹

本小車在車頂搭載了ZED雙目攝像頭，并在相鄰位置安裝了雷達(dá)和視覺傳感器，圖3為模型原理圖，表1為視覺傳感器參數(shù)圖，表2為雷達(dá)傳感器參數(shù)圖。

表1 視覺傳感器參數(shù)

表2 雷達(dá)傳感器參數(shù)

圖3 實(shí)驗(yàn)平臺(tái)

圖3(a)則為斜45°方向上小車在SOLIDWORKS中的實(shí)體模型。圖3(b)說明了視覺傳感器設(shè)置位置及其感知區(qū)域的側(cè)視圖和俯視圖，其測(cè)量坐標(biāo)系通過ROS系統(tǒng)轉(zhuǎn)化為ODOM的中心坐標(biāo)系。圖3(c)說明了雷達(dá)設(shè)置位置及其俯視圖下的測(cè)量原理，通過ODOM的雷達(dá)坐標(biāo)系，直接測(cè)量小車與中心線的距離和角度。

3.2 實(shí)驗(yàn)及數(shù)據(jù)分析

讓實(shí)體小車運(yùn)動(dòng)起來，并獲取視覺傳感器和雷達(dá)測(cè)量的相應(yīng)數(shù)據(jù)，利用所獲原始數(shù)據(jù)計(jì)算出小車的實(shí)時(shí)狀態(tài)，并按照?qǐng)D1的流程圖對(duì)多傳感器數(shù)據(jù)進(jìn)行擴(kuò)展卡爾曼濾波融合，得到優(yōu)化后的小車狀態(tài)曲線。并將直接采集到的雷達(dá)數(shù)據(jù)與多傳感器融合值進(jìn)行比對(duì)，圖4顯示了對(duì)比結(jié)果。

圖4 雷達(dá)與中心線狀態(tài)濾波前后對(duì)比

由圖4可知，經(jīng)過濾波后的小車狀態(tài)曲線更加平滑，數(shù)據(jù)更為精確，幾乎不存在零點(diǎn)漂移。

此外，為了驗(yàn)證此方法的合理性，本車在lgsvl模擬器中進(jìn)行了測(cè)試，建立的模擬地圖如圖5所示，其中白點(diǎn)為小車運(yùn)動(dòng)的起始點(diǎn)和終止點(diǎn)，紅點(diǎn)為避障樁，小車會(huì)根據(jù)嵌入的Apollo避障算法進(jìn)行避障。

在建立的地圖上，使用卡爾曼濾波器處理后的結(jié)果如圖6所示，可以看出使用卡爾曼濾波器處理后，雷達(dá)的觀測(cè)噪聲被消除了，卡爾曼濾波處理后的結(jié)果比雷達(dá)反饋的結(jié)果更加貼近真實(shí)值和運(yùn)動(dòng)學(xué)理論。

圖6 lgsvl測(cè)試結(jié)果圖

在相同實(shí)驗(yàn)數(shù)據(jù)下，將傳統(tǒng)擴(kuò)展卡爾曼濾波與所提改進(jìn)方法進(jìn)行比較，表3為2種方法各運(yùn)行1 000次后與小車實(shí)驗(yàn)數(shù)據(jù)的最大誤差、平均方差和均方誤差。

表3 傳統(tǒng)擴(kuò)展卡爾曼濾波和改進(jìn)擴(kuò)展卡爾曼濾波對(duì)比

由表3可以看出，2種方法都能有效地提高位置的精度。但是傳統(tǒng)擴(kuò)展卡爾曼濾波波動(dòng)較大，相較于改進(jìn)擴(kuò)展卡爾曼濾波，傳統(tǒng)擴(kuò)展卡爾曼濾波的最大誤差增大了55%，平均誤差增大了44%，均方誤差增大了17%，且運(yùn)行處理數(shù)據(jù)時(shí)間更長(zhǎng)。改進(jìn)擴(kuò)展卡爾曼濾波由于具有交叉融合的特性，具有精度高、波動(dòng)小、效率快的優(yōu)點(diǎn)，在該小車融合定位中有更強(qiáng)的適用性。

4 結(jié)論

針對(duì)車周邊環(huán)境信息時(shí)視覺傳感器和雷達(dá)獲取的數(shù)據(jù)漂移問題，采用的改進(jìn)擴(kuò)展卡爾曼濾波，對(duì)視覺傳感器和雷達(dá)采集到的非全向小車關(guān)于道路中心線的狀態(tài)數(shù)據(jù)進(jìn)行交叉融合，得到小車的狀態(tài)參數(shù)。根據(jù)小車的特點(diǎn)，將視覺傳感器作為預(yù)測(cè)值，根據(jù)預(yù)測(cè)k時(shí)刻的先驗(yàn)狀態(tài)得到該時(shí)刻的先驗(yàn)協(xié)方差矩陣，然后通過改進(jìn)擴(kuò)展卡爾曼濾波算法不斷更新遞歸，最終估算出最佳的狀態(tài)。實(shí)驗(yàn)結(jié)果表明該算法具有較強(qiáng)的實(shí)用性和魯棒性。