自動駕駛汽車狀態(tài)估計(jì)的矩陣加權(quán)自適應(yīng)擴(kuò)展卡爾曼濾波方法

初宏偉 張穎

（1.長春汽車工業(yè)高等?？茖W(xué)校，長春 130013；2.一汽-大眾汽車有限公司，長春 130013）

主題詞：自動駕駛汽車 多傳感器 狀態(tài)估計(jì) 執(zhí)行機(jī)構(gòu)故障 卡爾曼濾波

1 前言

自動駕駛感知系統(tǒng)的性能多依賴于其所配置的傳感器硬件方案及相應(yīng)的汽車狀態(tài)估計(jì)方法?，F(xiàn)有自動駕駛汽車所配置的傳感器包括慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）、全球定位系統(tǒng)（Global Positioning System，GPS）、雷達(dá)、光學(xué)傳感器等。

以輪胎側(cè)向力傳感器為數(shù)據(jù)源，文獻(xiàn)[5]、文獻(xiàn)[6]采用遞歸最小二乘方法估計(jì)側(cè)向車速，但因?qū)Νh(huán)境噪聲存在高斯噪聲假設(shè)，其跟蹤存在一定時延。文獻(xiàn)[7]通過車速傳感器的信號，采用龍伯格（Luenberger）觀測器法實(shí)現(xiàn)了道路坡度的估計(jì)。文獻(xiàn)[8]采用擴(kuò)展卡爾曼濾波器實(shí)現(xiàn)了車輪側(cè)偏角的估計(jì)，但噪聲信息未知導(dǎo)致其狀態(tài)估計(jì)存在一定偏差。文獻(xiàn)[9]采用無跡卡爾曼濾波器實(shí)現(xiàn)了汽車速度估計(jì)，并在BMW5上進(jìn)行了實(shí)車試驗(yàn)，縱、側(cè)向速度跟蹤較好。

單一傳感器量測的可靠性相對較差，而采用不同傳感器能夠?qū)崿F(xiàn)信息融合，從而獲得更為精準(zhǔn)的目標(biāo)信息。目前，歐盟已開始研究雷達(dá)與光學(xué)傳感器的融合技術(shù)。在國內(nèi)，潘璞利用全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）/INS 組合導(dǎo)航系統(tǒng)設(shè)計(jì)了自適應(yīng)卡爾曼濾波方法實(shí)現(xiàn)自動駕駛汽車的姿態(tài)解算。沈崢楠融合超聲波雷達(dá)和可見光攝像頭實(shí)現(xiàn)了車位幾何信息識別。彭文正等采用迭代擴(kuò)展卡爾曼濾波算法融合了慣性測量單元（Inertial Measurement Unit，IMU）、GPS、相機(jī)等信息，實(shí)現(xiàn)了車輛的定位和速度觀測。劉志在深度學(xué)習(xí)框架下實(shí)現(xiàn)了多源信息的深度融合。綜上所述，多傳感器信息融合技術(shù)已然成為當(dāng)前自動駕駛汽車狀態(tài)估計(jì)領(lǐng)域的研究熱點(diǎn)。

現(xiàn)有方法鮮有討論汽車故障條件下的狀態(tài)和故障聯(lián)合估計(jì)問題。為此，本文針對自動駕駛汽車提出了一種多傳感器加權(quán)融合自適應(yīng)擴(kuò)展卡爾曼濾波（Adaptive Extended Kalman Filtering，AEKF）狀態(tài)估計(jì)方法，通過設(shè)計(jì)矩陣加權(quán)融合方法實(shí)現(xiàn)多傳感器信息融合，從而提升感知系統(tǒng)量測精度，以期為自動駕駛汽車的自動泊車、自動避障等應(yīng)用場景提供參考。

2 自動駕駛汽車運(yùn)動學(xué)模型

自動駕駛汽車運(yùn)動學(xué)模型如圖1所示。根據(jù)汽車運(yùn)動學(xué)規(guī)律，將上述關(guān)系在慣性系下建模為以下運(yùn)動學(xué)方程：

圖1 自動駕駛汽車運(yùn)動學(xué)模型

式中，()為汽車質(zhì)心在慣性系下的坐標(biāo)；為車速；為汽車加速度；為汽車行駛航向角；、分別為車輛質(zhì)心與前、后軸的距離；為汽車行駛方向與車身縱軸的夾角；為汽車前輪轉(zhuǎn)角。

為便于數(shù)字化工程設(shè)計(jì)，在假設(shè)轉(zhuǎn)向角很小的情況下（≈），將式（1）轉(zhuǎn)換為離散形式：

式中，、、、、、、分別為系統(tǒng)時刻、、、、、、的采樣值；Δ為數(shù)字測控系統(tǒng)的采樣時間。

本文的目標(biāo)是針對離散系統(tǒng)式（2），設(shè)計(jì)多傳感器矩陣加權(quán)融合AEKF 估計(jì)器，以降低整車成本、提升測控系統(tǒng)的量測精度，實(shí)現(xiàn)自動駕駛汽車行駛狀態(tài)和執(zhí)行機(jī)構(gòu)故障的同步估計(jì)。

3 汽車行駛狀態(tài)估計(jì)方法

3.1 自適應(yīng)擴(kuò)展卡爾曼濾波

為便于自適應(yīng)擴(kuò)展卡爾曼濾波器設(shè)計(jì)，同時考慮執(zhí)行機(jī)構(gòu)故障情形，將離散系統(tǒng)式（2）轉(zhuǎn)化為常規(guī)系統(tǒng)描述形式：

非線性動態(tài)方程(x,y)∈R的表達(dá)式為：

由于轉(zhuǎn)換后的系統(tǒng)式（3）存在非線性方程式（4），故采用擴(kuò)展卡爾曼濾波器設(shè)計(jì)思路對其進(jìn)行線性化處理。線性化后的系統(tǒng)為：

至此即完成了系統(tǒng)模型的轉(zhuǎn)換。本文自適應(yīng)擴(kuò)展卡爾曼濾波器設(shè)計(jì)如下：

a.給定迭代估計(jì)的初始條件：

式中，、為系統(tǒng)狀態(tài)量初始值；()0、為執(zhí)行機(jī)構(gòu)故障參數(shù)初始值；、為協(xié)方差矩陣的初始值；(0)、(0)均為用于求解最優(yōu)增益矩陣K的輔助矩陣。

b.協(xié)方差矩陣一步預(yù)測：

c.狀態(tài)估計(jì)增益矩陣更新：

式中，Σ=CP+R；R為當(dāng)前時刻噪聲方差矩陣。

d.故障估計(jì)增益矩陣Γ更新：

e.式（9）中對應(yīng)輔助矩陣H、S、Ω、Λ更新：

式中，為待設(shè)計(jì)遺忘因子。

通過式（6）～式（13）的迭代計(jì)算，即可通過單個傳感器信息估計(jì)得到自動駕駛汽車的狀態(tài)和故障信息。關(guān)于自適應(yīng)擴(kuò)展卡爾曼濾波收斂特性的探討參見文獻(xiàn)[17]。

3.2 多傳感器矩陣加權(quán)融合估計(jì)方法設(shè)計(jì)

為解決單一傳感器高精度測控與高成本的矛盾，有效提高自動駕駛汽車整車制造的費(fèi)效比，通過配置制造成本較低的傳感器，并利用傳感器信息融合技術(shù)對汽車狀態(tài)進(jìn)行在線估計(jì)成為了工業(yè)界的熱點(diǎn)研究方向之一。

設(shè)傳感器數(shù)量為，通過各傳感器的信號濾波得到的無偏估計(jì)x：

式中，表示數(shù)學(xué)期望。

式中，M∈R為加權(quán)矩陣。

在線性最小方差意義下，選擇M來極小化估計(jì)誤差分量均方和：

式（16）等價于：

式中，為加權(quán)估計(jì)的協(xié)方差矩陣；()為矩陣的跡。

因局部估計(jì)和融合估計(jì)均具有無偏性，則以下約束條件成立：

根據(jù)式（18）可知：

為了求取各加權(quán)矩陣M，設(shè)加權(quán)矩陣的合成陣為：

定義未知方差矩陣合成矩陣為：

根據(jù)上述定義，性能指標(biāo)式（17）可寫為：

而約束條件式（18）可轉(zhuǎn)換為：

至此，求解矩陣的問題轉(zhuǎn)換為以式（22）為性能指標(biāo)的矩陣等式約束極小化問題。采用拉格朗日乘數(shù)法，引入輔助函數(shù)：

式中，γ=[γ… γ]為拉格朗日乘子向量；e=(0,…,0,1,0,…,0)，其第列元素為1，其他為0。

為極小化性能指標(biāo)式（24），對輔助函數(shù)求的偏導(dǎo)數(shù)并令其等于0，即?/?=0，可得：

上式第2項(xiàng)可以寫成如下形式：

式中，=(,…,γ)=(γ)，該矩陣的第行第列為γ。

根據(jù)式（25）和式（26），可得：

輔助函數(shù)對求偏導(dǎo)并令其等于0，即?/?=0，則有：

根據(jù)上述推導(dǎo)可知：

求解線性方程組式（29），可得：

由式（30）可推知=I，進(jìn)而可知：

推知：

上述推導(dǎo)存在如下關(guān)系：

根據(jù)協(xié)方差矩陣定義可知：

綜合上述推導(dǎo)過程，得到多傳感器矩陣加權(quán)融合估計(jì)：

本文所設(shè)計(jì)方法的整體應(yīng)用框架如圖2所示。

圖2 本文方法框架

4 仿真分析

4.1 仿真條件設(shè)置

為驗(yàn)證本文所提出的方法的有效性，將本文所提出的AEFK（矩陣加權(quán)）方法與單個傳感器的AEKF方法進(jìn)行對比。驗(yàn)證方案中，采用了2種不同精度的傳感器對汽車狀態(tài)信息進(jìn)行感知，并設(shè)置如表1所示的仿真參數(shù)進(jìn)行方法驗(yàn)證。

表1 仿真參數(shù)

4.2 仿真結(jié)果及分析

利用表1 中所設(shè)計(jì)的仿真參數(shù)，以MATLAB 2018b為仿真平臺搭建仿真模型，設(shè)置了3種仿真情形：

a.采用低精度傳感器1（=0.8）進(jìn)行狀態(tài)信息采集，并利用3.1 節(jié)的AEKF 方法進(jìn)行狀態(tài)和故障系數(shù)估計(jì)，記為AEKF（傳感器1）；

b.采用高精度傳感器2（=0.2）進(jìn)行狀態(tài)信息采集，并利用3.1 節(jié)的AEKF 方法進(jìn)行狀態(tài)和故障系數(shù)估計(jì)，記為AEKF（傳感器2）；

c.基于情形a 和情形b 得到的估計(jì)結(jié)果，利用3.2節(jié)的多傳感器矩陣加權(quán)融合方法進(jìn)行狀態(tài)和故障估計(jì)，記為AEKF（矩陣加權(quán)）。

根據(jù)上述仿真情形和表1的仿真參數(shù)設(shè)置，得到仿真結(jié)果如圖3～圖7所示。

圖3 汽車位置估計(jì)結(jié)果

圖4 汽車行駛航向角估計(jì)結(jié)果

圖5 汽車速度估計(jì)結(jié)果

圖6 故障系數(shù)估計(jì)結(jié)果

圖7 方差矩陣的跡

由圖3 可知，高精度傳感器（傳感器2）能夠得到更好的估計(jì)結(jié)果。雖然3 種情形均能保證汽車行駛位置的有效跟蹤，但本文所提出方法的融合估計(jì)結(jié)果顯然優(yōu)于單個傳感器的估計(jì)結(jié)果，精度更高。

由圖4、圖5 可知，本文所提出的方法能夠有效、較好地實(shí)現(xiàn)汽車行駛航向角的高精度估計(jì)。結(jié)合圖3～圖5 可知，本文方法能夠?qū)崿F(xiàn)自動駕駛汽車位置、航向角和速度的高效估計(jì)。

由圖6可知，本文方法還具備執(zhí)行機(jī)構(gòu)故障的估計(jì)能力，且較單個傳感器估計(jì)情形具有一定優(yōu)勢。綜合圖3～圖6可知，本文方法不僅能夠保證自動駕駛汽車在正常狀態(tài)下的狀態(tài)感知，還可實(shí)現(xiàn)故障情形下執(zhí)行機(jī)構(gòu)故障系數(shù)的有效估計(jì)，保證了自動駕駛汽車行車全過程的安全、可靠。

由圖7可知，所提出方法的方差矩陣的跡隨著時間推移收斂更快，意味著參數(shù)收斂更快。這也是該方法估計(jì)結(jié)果快速高效的內(nèi)在原因。

為進(jìn)一步量化對比所提出方法的優(yōu)勢，以故障估計(jì)誤差的統(tǒng)計(jì)學(xué)均方誤差（Mean Squared Error，MSE）和誤差標(biāo)準(zhǔn)差（STandard Deviation，STD）為量化指標(biāo)，將本文方法與對比方法（單個傳感器）統(tǒng)計(jì)學(xué)信息進(jìn)行對比，如表2所示。

表2 仿真結(jié)果統(tǒng)計(jì)學(xué)分析

對比表2 中的均方誤差結(jié)果可知，矩陣加權(quán)AEKF方法的誤差均值最小，則狀態(tài)估計(jì)效果最佳。進(jìn)一步對比誤差標(biāo)準(zhǔn)差結(jié)果可知，矩陣加權(quán)AEKF的跟蹤誤差標(biāo)準(zhǔn)差最小，說明所提出的估計(jì)方法與汽車實(shí)際狀態(tài)的偏差較小，能夠保證誤差估計(jì)的快速、平順，從而保證自動駕駛汽車控制系統(tǒng)的穩(wěn)定運(yùn)行和行車安全。

5 結(jié)束語

針對自動駕駛汽車狀態(tài)和故障估計(jì)問題，本文在自適應(yīng)擴(kuò)展卡爾曼濾波理論基礎(chǔ)上提出了一種矩陣加權(quán)多傳感器信息融合估計(jì)方法。仿真驗(yàn)證結(jié)果表明，較之單個傳感器的狀態(tài)和故障估計(jì)方法，所提出的方法能夠較好地實(shí)現(xiàn)汽車行駛位置、航向角、速度以及執(zhí)行機(jī)構(gòu)故障的高效估計(jì)，為汽車自動避障、自動泊車等提供一定技術(shù)參考。

本文方法雖能實(shí)現(xiàn)汽車狀態(tài)和故障的有效估計(jì)，但由于EKF方法需已知外部擾動和傳感器量測的噪聲統(tǒng)計(jì)特性，實(shí)際應(yīng)用中較難準(zhǔn)確獲取上述信息，下一步工作將致力于噪聲特性的在線建模，同時嘗試將工程應(yīng)用中的不同真實(shí)傳感器數(shù)據(jù)源引入本文算法進(jìn)行實(shí)測數(shù)據(jù)驗(yàn)證，以期得到工程實(shí)用性更強(qiáng)的自動駕駛汽車狀態(tài)融合估計(jì)方法。