基于超聲波與IMU融合的倒車測距算法

馬強，王云，陳文淼，楊新達，武玉臣，劉曉林

(1.內燃機可靠性國家重點實驗室，山東濰坊 261061；2.濰柴動力股份有限公司發動機研究院，山東濰坊 261040)

0 引言

隨著經濟的發展和人們對物質文化需求的增長，汽車倒車安全的要求越來越高，采用精確倒車測距技術已成為車輛發展的必由之路[1-2]。

目前倒車預警系統主要應用超聲波測距報警技術，一般為區段報警，無法提供精確距離信息，需要結合駕駛員的判斷力，這在一定程度上增加了倒車的難度，影響車輛的控制精度、安全性及駕駛體驗[3-6]。

倒車過程中駕駛員最關心的是車輛與障礙物之間的距離，目前直接用于探測距離的傳感器主要有紅外線、激光雷達、毫米波雷達及超聲波傳感器等。紅外線受環境影響較大，而激光雷達價格昂貴，2種傳感器目前尚不適于大規模應用和商業化開發；毫米波雷達不適于短距測量，在應用方面也受到限制；超聲波傳感器具有環境適用性強、結構簡單、成本低廉、短距測量可靠性高等特點，越來越受到市場的歡迎。本研究中采用超聲波傳感器作為距離感知的主要部件[7-11]。

任何類型的測距傳感器都有內在的缺陷和不足，一個傳感器無法滿足系統魯棒性的要求。目前國內外倒車精確測距應用中一般采用多個或多種類型的傳感器，通過對各種類型傳感器的信息進行融合實現對環境較準確的感知[12-15]。目前倒車精確測距系統多采用超聲波和攝像頭的融合方案[16]，但攝像頭受環境影響較大，易受光線等因素制約。慣性測量單元(inertial measurement unit，IMU)不受光線影響，提供的車輛加速度、車速等信息同樣可參與到超聲波的測距融合中，本文中提出一種基于超聲波和IMU融合的測距方案，并結合方案對具體的融合算法進行設計。

1 感知融合算法

超聲波與IMU融合測距算法主要結合超聲波和IMU各自的優點，使得探測到的距離信息更加可靠，精確度更高。首先，超聲波傳感器探測到的距離信息要經過“同類型多傳感器融合算法”進行數據預處理，然后，結合IMU感知的車輛加速度及當前車速信息，利用卡爾曼濾波對障礙物的距離進行最優估計，從而實現IMU與超聲波傳感器數據的最終融合[17-20]。

1.1 同類型多傳感器融合算法

同類型傳感器信息的融合采用基于最大似然法的多傳感器數據融合方法，該方法可以改善單個傳感器可靠性對最終測量結果的影響[21]。設xi和xj分別表示在一次測量中第i個和第j個傳感器的輸出數據，有：

式中：dij為xi對xj的置信距離；dji為xj對xi的置信距離；Si和Sj的幾何意義分別為概率密度曲線pi(x│xi)和pj(x│xj) 與區間(xi，xj)或(xj，xi)圍成的曲邊形面積，pi(x│xi)和pj(x│xj)分別為當xi和xj為均值時xi和xj的概率密度函數

式中σi、σj為均值方差。

置信距離反映了傳感器輸出數據之間的相互支持關系，如dij反映了傳感器輸出數據對傳感器j輸出數據的支持程度。置信距離越小，2個傳感器的觀測值越相近，否則2個傳感器偏差就大。

由此方法可以得到n(本系統n=4)個傳感器中任意2個傳感器輸出數據之間的置信距離，將這些數據用矩陣形式表示，即為n個傳感器輸出數據的置信距離矩陣

得到置信距離矩陣后需要選擇一個臨界值βij對置信距離進行劃分，用以判斷2個傳感器輸出數據是否互相支持。βij值根據實際情況選取，本系統選取βij=0.5， 當dij≤βij時，則認為第i個傳感器的輸出數據與第j個傳感器的輸出數據互相支持，互相支持度rij為1，否則為0,即

由此可得到一個關系矩陣

關系矩陣表示任意2個傳感器輸出數據之間是否支持，由此可以判斷每一個傳感器輸出數據是否有效。這樣需要臨界值m，即對于一個傳感器輸出數據，當它跟多于或等于m個(包括本身傳感器，本系統m=3)傳感器的輸出數據互相支持時則認為其輸出數據有效。由此方法依據關系矩陣對n個傳感器的輸出結果進行選擇，得到若干個有效數據參與融合計算，這些有效數據為最佳融合。

基于最大似然法的似然函數求導，最終得到融合結果：

式中:μ為均值;σi為均值方差。

1.2 基于卡爾曼濾波的IMU與超聲波傳感器數據融合算法

卡爾曼濾波器是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法，是多傳感器信息融合系統的主要手段之一，也是進行距離估計的有效方法。本系統采用距離(同類型傳感器融合算法處理后的距離)、速度融合的標準卡爾曼濾波方式，這是現代最優估計——卡爾曼濾波技術最簡單融合方式[22]。直接使用標準卡爾曼濾波算法對距離信息做最優估計，濾波器的狀態方程和觀測方程都是線性的。對于本系統，擋墻距離的狀態方程可表示為：

定義pt為系統在t時刻狀態預測距離，vt為t時刻的車輛速度，at為IMU計算的t時刻的車輛加速度，可得到：

本系統的狀態方程推導如下：

設zt為觀測的信息，本文中為超聲波傳感器探測的距離信息，則t時刻的系統狀態到觀測狀態之間的關系可表示為：

式中H為觀測矩陣。

測量余量協方差

最優卡爾曼增益

更新系統的估計狀態為：

更新估計誤差協方差

如此，通過卡爾曼濾波不斷的迭代，從而估算出最優的障礙物距離值。

2 試驗驗證及分析

試驗驗證如圖1所示，采用4個超聲波傳感器作為主要測距部件， IMU作為車輛加速度的感知部件。等間隔(4 cm)選取15個測量點，每個測量點重復測量10次取均值，由人工分別實時采集4個超聲波傳感器原始測量數據及融合后的距離，對比分析融合前后的數據。

圖1 試驗驗證示意圖

根據試驗數據得到各超聲波傳感器原始測量距離及經融合算法計算后的距離與實際距離的關系，如圖2所示。計算單個傳感器及經融合算法后數據的隨機測量方差，以數據的最大誤差及方差作為評判指標，測量結果表明單傳感器的測量數據較為發散(方差為7.58～10.26)且誤差較大，與實際值的最大偏差為±5 cm，經融合算法處理后的數據較為集中(方差為4.66)且誤差較小，最大偏差為±3 cm，經融合算法處理后的數據明顯優于單個傳感器的測量結果，單傳感器與數據融合后測量方差對比如圖3所示。

圖2 單傳感器與數據融合后的實際距離與測量距離 圖3 單傳感器與數據融合后測量方差對比

3 結論

設計的測距算法以基于最大似然法的同類傳感器融合為前提，在此基礎上通過卡爾曼濾波算法進一步完成了2種傳感器之間的信息互補，綜合利用了2種傳感器的優勢，最終實現了超聲波與IMU的信息融合，使得融合后的距離信息具有比單一傳感器更高的可靠性及精確度，結果表明該融合算法優于任意單一數據的評判方法，對于精確倒車測距技術的開發具有實際的應用價值。