初宏偉 張穎

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

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

1 前言

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

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

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

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

2 自動駕駛汽車運動學模型

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

圖1 自動駕駛汽車運動學模型

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

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

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

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

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

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

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

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

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

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

a.給定迭代估計的初始條件：

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

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

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

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

d.故障估計增益矩陣Γ更新：

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

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

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

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

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

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

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

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

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

式（16）等價于：

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

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

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

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

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

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

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

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

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

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

上式第2項可以寫成如下形式：

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

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

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

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

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

由式（30）可推知=I，進而可知：

推知：

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

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

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

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

圖2 本文方法框架

4 仿真分析

4.1 仿真條件設(shè)置

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

表1 仿真參數(shù)

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

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

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

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

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

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

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

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

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

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

圖7 方差矩陣的跡

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

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

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

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

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

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

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

5 結(jié)束語

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

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