基于自適應擴展Kalman濾波的路側多傳感器融合

武一民，鄭凱元,2，高博麟，陳 明,2，王義鋒

（1. 河北工業大學 機械工程學院，天津300130，中國；2. 清華大學 車輛與運載學院，北京100084，中國；3. 滴滴出行，北京100089，中國）

借著人工智能、云計算、大數據等新一輪的科技革命，自動駕駛技術不斷發展與完善，其中準確、全面的環境感知技術是自動駕駛汽車決策與控制的前提，是行駛安全的保障[1]。傳感器是感知外界環境的強有力手段，目前車載端傳感器感知視野有限、測量穩定性差、成本相對較高；而路側端傳感器探測范圍廣、安裝平臺穩定[2]。相機(cameras)、激光雷達(lidars)、毫米波雷達(radars)等單一傳感器，各有優缺點，不能獨自勝任自動駕駛環境感知任務；為了充分利用不同傳感器的特點，實現優勢互補，需要進行多傳感器信息融合[3]。路側多傳感器信息融合能準確提供動靜態障礙物信息，對提高自動駕駛汽車的行駛性能具有重要意義。

按照數據的抽象層次不同，多傳感器信息融合方法可分為4類[4]：數據級（像素級）融合、特征級融合、目標級融合[5]、決策級融合。其中數據級融合與特征級融合通常稱為前融合，目標級融合與決策級融合稱為后融合。

傳統估計理論為多傳感器信息融合技術奠定了良好的基礎，主要包括Bayes理論[6]、粒子濾波[7]、Kalman濾波[8]、證據理論等[9]。其中以Kalman濾波原理為基礎的各類算法在多傳感器信息融合領域得到了廣泛的應用。為了提高基于多傳感器融合的目標狀態估計的精度，研究者們提出了多種融合策略和濾波算法改進方案。黃文錦等人利用車載激光雷達和路側雙目攝像頭，提出了一種基于Kalman濾波的位姿信息融合算法[10]，通過時空同步、數據關聯、雙層信息融合完成了對自身位姿的估計。由于機動目標是一個非線性系統，而經典Kalman濾波只能處理線性系統[11]。黃彬等人利用車端的相機、毫米波雷達、激光雷達采集到的車輛前方目標物信息[12]，提出了基于擴展Kalman濾波的信息融合算法；為了獲取傳感器誤差特性，給Kalman濾波算法設置合理的量測噪聲，該研究利用動靜態實驗獲取了傳感器的動靜態感知性能，建立了動靜態下的傳感器感知誤差模型。但此種實驗手段僅適用于車載端傳感器，路側端傳感器的融合方法亟需探索突破。

由于目標遠近等客觀因素的影響，傳感器的感知穩定性差，量測噪聲是動態變化的。傳統擴展Kalman濾波（extended Kalman filter, EKF)與無跡Kalman濾波(unscented Kalman filter, UKF)算法中，量測噪聲協方差矩陣是恒定的，導致融合結果難以動態適應傳感器變化特性。針對此類問題，王向華等人提出了一種自適應Kalman濾波算法[13]，通過同一時刻2次不同步長的Kalman濾波結果，自適應的調整量測噪聲協方差矩陣，降低了由于量測噪聲不準確造成的精度損失。然而在不同的應用背景下，對量測噪聲進行調整仍是十分棘手的問題。

針對路側多傳感器融合過程中的傳感器誤差特性難以在線獲取且動態定量描述的問題，本文提出一種基于自適應擴展Kalman濾波器的路側多傳感器融合方法。通過多項式擬合的方式求取各傳感器測量值序列的殘差，通過方差函數生成初始化量測噪聲；通過對傳感器測量值穩定性進行檢測，生成量測噪聲的修正系數，對其進行自適應調整；采用擴展Kalman濾波算法對多傳感器信息進行融合。經實車試驗，以驗證所提方法的有效性。

1 基于自適應擴展Kalman濾波的多傳感器融合算法

1.1 路側多傳感器融合框架

路側多傳感器融合框架如圖1所示。傳感器包含相機、毫米波雷達、激光雷達3種。各傳感器通過自帶的檢測跟蹤算法，輸出交通參與者的目標級數據，其中包含：時間戳、目標類型、空間位置、速度、加速度、角速度等。由于各傳感器的起始時間和數據更新頻率不同，在進行融合之前，需對傳感器信息進行時間同步，假設一個周期內目標運動屬性的變化是線性的，采用線性插值[14]的方式進行時間同步，即利用傳感器連續2個時刻的數據，通過插值計算出某一時刻的等效數據。在空間上，所有傳感器的目標級數據均已轉換到了通用橫Mercator格網系統（universal transverse Mercator grid system，UTM）坐標系下。經過時空同步的目標級數據傳遞到邊緣云信息融合平臺，再通過本文提出的自適應擴展Kalman濾波算法，對多傳感器信息進行融合。

1.2 狀態量定義

目標車輛行駛在開放性道路上，需對車輛的狀態進行觀測，而車輛、行人、非機動車等交通參與者的加速度和角速度在短時間（例如不超過一個采樣周期）內近似不變[15]，故采用常角速度常加速度（constant turn rate and acceleration，CTRA)模型來描述目標的運動變化。

令目標的狀態向量為

其中：x和y分別表示目標車輛位于UTM坐標系下的絕對橫向距離和縱向距離；vx和vy分別表示目標車輛位于UTM坐標系下的絕對橫向速度和縱向速度，由合速度v分解得到；ax和ay分別表示目標車輛位于UTM坐標系下的絕對橫向加速度和縱向加速度；ω為目標車輛的角速度。

1.3 AEKF融合算法流程

由于傳感器的量測噪聲會隨著運動目標的位置、狀態的不同而動態變化，采用定噪聲值，易造成濾波不穩定甚至發散，故以擴展Kalman濾波為基礎，利用修正系數自適應調整量測噪聲協方差矩陣，對EKF算法進行優化改進。AEKF融合算法流程如下：

1） 采用CTRA模型，建立系統狀態轉移方程。若令T為時間步長，則各狀態向量分量的轉移公式為：

根據上述狀態轉移方程，求解狀態轉移矩陣的Jacobi矩陣Fk。

2） 時間更新階段。由第一步計算得到的Jacobi矩陣Fk對目標狀態xk及狀態協方差矩陣Pk進行預測。

3） 確定觀測矩陣。采用量測融合的方式對多傳感器信息進行融合，如圖2所示，量測融合[16]即將所有傳感器的測量值以向量的形式組合起來，然后進行濾波。

令量測向量z= [zc,zr,zl]T,其中zc= [xc,yc,vcx,vcy,acx,acy,ωc]T為路側相機的量測向量，zr、zl同理，分別為路側毫米波雷達和激光雷達的量測向量，角標c、r、l分別代表相機(camera)、毫米波雷達(millimetre wave radar，或者radar)、激光雷達(laser radar，或者lidar)。根據狀態向量x和量測向量z確定觀測矩陣H=E7，其中H是狀態向量x和量測向量z的映射矩陣，本文為7階單位陣E7。

3）求解量測噪聲。本文通過處理一定時間序列的傳感器的測量值，進而求解量測噪聲。對傳感器一定時間序列的測量值進行擬合，得到不同傳感器各種測量值的擬合項：

其中：zik為量測向量z的第i個分量在時間k內的測量值序列，?ik為量測向量z的第i個分量在時間k內的測量值序列的擬合項，fitf(·)為擬合函數，可為多項式、樣條函數、三角函數等。

量測向量z的第i個分量在時間k內的測量值序列的殘差為

通過方差函數，計算傳感器的量測噪聲。

k時刻第i個分量的初始化量測噪聲為k時刻的初始化量測噪聲矩陣為

R0k會隨著時間的增加不斷收斂穩定。

4） 量測噪聲的自適應調整。考慮到目標運動狀態的不確定性以及傳感器的動態感知特性，系統中的量測噪聲是動態變化的，為保證濾波穩定，通過定義修正系數u，對量測噪聲進行動態調整。首先通過對傳感器測量值的穩定性進行檢測，生成修正系數。k時刻第i個量測分量的修正系數為

其中：zik為k時刻第i個量測分量的測量值，?ki為k時刻第i個量測分量的擬合值，δi為第i個量測分量的閾值。反應測量值的波動性，可以是常量或時變函數，本文將傳感器測量值的波動性約束在一個固定值內，故采用常量δi，δi的數值選取用Δzik絕對值的百分位數來確定，將一段時間內的Δzik的絕對值從小到大排列，選擇其合適的百分位數作為δi，將傳感器測量值的波動性約束在一個較小的范圍內。若k時刻第i個測量值的殘差zik-?ki小于等于δi，表明傳感器的測量值穩定，則設定第i個量測分量的修正系數uki等于1；若k時刻測量值的殘差zki-?ik大于δi，表明傳感器的測量值不穩定，則設定uki為大于1的值，用以放大該量測分量的噪聲方差。

構造量測噪聲修正矩陣為

利用Nk對初始化量測噪聲矩陣R0k進行自適應調整，得到新的量測噪聲矩陣為

2 試驗驗證

為驗證所提路側多傳感器融合方法的有效性，在某丁字路口部署了3個傳感器：相機、毫米波雷達、激光雷達。相機采用的是 200 萬像素，分辨率為 1 920× 1 080 的工業用途專用相機。激光雷達采用的是Velodyne HDL-64E激光雷達。毫米波雷達采用的是77 GHz的交通檢測雷達。各傳感器的精度如表1所示。

表1 大地坐標下各傳感器精度

各傳感器通過各自的檢測算法獲取目標車輛的狀態信息：位置、速度、加速度、角速度等。各傳感器的部署情況如圖3所示。相機、毫米波雷達覆蓋范圍近似為梯形，激光雷達覆蓋范圍近似為圓環形，覆蓋整個丁字路口。3個傳感器架設高度為12.4 m。本實驗采用一輛裝有差分高精度GPS（global positioning system，全球定位系統）的實驗車輛，在3個傳感器的覆蓋范圍內行駛，車載GPS用以獲取實驗車輛位置及速度的參考值。

圖4 —圖6所示為橫向距離Dx、縱向距離Dy、合速度v這3個參數的不同傳感器感知結果、傳統EKF算法融合結果以及本文AEKF算法融合結果。相機、激光雷達對目標車輛位置的識別較精準，毫米波雷達的位置識別差異性較大；毫米波雷達、激光雷達對目標車輛合速度的識別較精準，相機對合速度的識別波動性大。本文AEKF算法，相較于單傳感器的測量值而言，融合后的結果精度得到了提高，此外相較于傳統EKF算法；因此，本文AEKF算法能提高估計的精度，降低噪聲干擾。

統計計算出各傳感器測量值以及融合算法估計值相對于GPS參考值的均方根誤差,對其進行定量比較。表2列出了各傳感器的測量值、傳統EKF算法以及所提AEKF算法針對橫向距離Dx、縱向距離Dy、合速度v的均方根誤差RMSE指標。

表2 傳感器測量值、EKF以及AEKF算法的RMSE指標

據表2可以看出：本文的AEKF算法提高了估計精度；相較于單傳感器的測量值而言，橫向距離估計精度提高了9.7%，縱向距離估計精度提高了5.4%，速度估計精度提高了26.6%，相較于傳統EKF而言，橫向距離估計精度提高了44.9%，縱向距離提高了21.3%，速度估計精度提高了64.4%；本文方法使目標速度的估計精度明顯提高。

本文AEKF算法和傳統EKF算法的相關參數如表3所示（見下頁），AEKF算法和傳統EKF算法的相關參數初值相同。

表3 AEKF算法和EKF算法相關參數

3 結 論

針對路側多傳感器融合過程中的傳感器誤差特性難以在線獲取、且動態定量描述困難等制約，本文作者:

1） 提出了一種量測噪聲在線獲取方法，利用一定時間序列的不同傳感器的測量值，通過多項式擬合的方式，求取測量值序列的殘差，進而利用方差函數生成初始化量測噪聲。

2） 提出了基于AEKF的路側多傳感器融合方法，對多源異質信息進行融合。通過對傳感器測量值的穩定性進行檢測，利用修正系數自適應的調整量測噪聲矩陣，降低了量測噪聲對系統估計的影響。實車試驗結果表明：相較于單傳感器的測量值而言，基于量測噪聲自適應調整融合后橫向距離估計精度提高了9.7%，縱向距離估計精度提高了5.4%，速度估計精度提高了26.6%。對比于傳統的EKF算法，本文AEKF算法提高了估計精度，降低了噪聲干擾。

本文研究僅考慮量測噪聲對系統估計的影響，由于動態系統噪聲是由過程噪聲和量測噪聲共同組成，同時考慮過程噪聲和量測噪聲影響條件下，路側多傳感器對交通目標的融合感知方法有待進一步研究。