基于傳感器融合的戶外道路下無人駕駛卡車姿態(tài)估計(jì)方法研究?

王翔昌 吳訓(xùn)成 張偉偉

（上海工程技術(shù)大學(xué) 上海 201620）

1 引言

近年來，車輛主動(dòng)安全系統(tǒng)已經(jīng)迅速發(fā)展，例如，輔助停車［1］，車輛穩(wěn)定性（包括偏航穩(wěn)定性［2］，側(cè)傾穩(wěn)定性［3］和懸架［4］）控制系統(tǒng)和主動(dòng)轉(zhuǎn)向［5］。對(duì)于這些主動(dòng)系統(tǒng)，獲得自身的姿態(tài)是首要任務(wù)，包括車輛姿態(tài)角，速度和加速度。然而，傳統(tǒng)傳感器的傳感精度很容易受到無人駕駛卡車質(zhì)量和體積過大的影響。因此，無人駕駛卡車在車輛姿態(tài)估計(jì)［6］、環(huán)境特征檢測(cè)［7］、障礙物檢測(cè)［8］等方面還有很長(zhǎng)的路要走。目前，慣性測(cè)量單元是獲取車輛姿態(tài)的典型手段［9］。然而由于傳感器的漂移問題，單純依靠慣性測(cè)量單元無法準(zhǔn)確估計(jì)車輛的姿態(tài)。微機(jī)電系統(tǒng)慣性測(cè)量單元包含三軸陀螺儀和三軸加速度是可選的解決方案。Wu［10］利用包含陀螺儀的低成本慣性測(cè)量單元來獲取車身姿態(tài)信息，然而，陀螺儀易受到漂移問題的影響，使得測(cè)量結(jié)果出現(xiàn)誤差。Ahmed［11］將陀螺儀和加速度計(jì)結(jié)合起來克服陀螺儀的漂移問題，但是該方案會(huì)隨著時(shí)間累積噪聲和偏差。對(duì)于道路環(huán)境的感知，有許多采用激光雷達(dá)進(jìn)行道路障礙物檢測(cè)的研究［12～15］。例如，Peng［16］采用激光雷達(dá)進(jìn)行動(dòng)態(tài)障礙物檢測(cè)、跟蹤與識(shí)別。Duan［17］采用多層激光雷達(dá)進(jìn)行道路與障礙物信息提取。然而這些方法僅是獨(dú)立使用激光雷達(dá)提取道路特征，并未與慣性測(cè)量單元進(jìn)行融合，難以解決慣性測(cè)量單元出現(xiàn)的滑移現(xiàn)象。

本文針對(duì)無人駕駛卡車，考慮到車輛行駛在戶外環(huán)境的不平整道路上，車輛在經(jīng)過障礙物時(shí)車身姿態(tài)會(huì)在短時(shí)間內(nèi)急劇變化。增加激光雷達(dá)來獲取車身周圍環(huán)境特征，包括障礙物的距離和尺寸信息，來幫助車輛獲取更精確的姿態(tài)信息。不同于以往方法，本文設(shè)計(jì)了基于卡爾曼濾波的無加速度模型，來克服微機(jī)電慣性測(cè)量單元隨著時(shí)間累積噪聲和偏差的問題。融合激光雷達(dá)和微機(jī)電慣性測(cè)量單元來有效獲取車身姿態(tài)信息，為后續(xù)的無人駕駛路徑規(guī)劃等應(yīng)用提供基礎(chǔ)姿態(tài)信息。

2 參考坐標(biāo)系與車身姿態(tài)表示

2.1 參考坐標(biāo)系

參考坐標(biāo)系包括地面參考系和用于安裝慣性測(cè)量單元和激光雷達(dá)的車身坐標(biāo)系。參考坐標(biāo)系的示意圖如圖1所示，地面坐標(biāo)系和車身坐標(biāo)系分別用PI和PV來表示。

圖1 參考坐標(biāo)系示意圖（地面坐標(biāo)系用PI來表示，車身坐標(biāo)系用PV來表示）

地面坐標(biāo)系選取東、北和上作為坐標(biāo)軸朝向，具體可有式（1）來表示：

車身坐標(biāo)系與車身縱向、橫向和垂向?qū)R，作為安裝慣性測(cè)量單元和激光雷達(dá)的坐標(biāo)系。具體可由式（2）表示：

2.2 車身姿態(tài)表示

其中，c和s分別表示cos和sin，θ，?和ψ分別表示俯仰角、橫擺角和航向角。至此我們便可以利用轉(zhuǎn)移矩陣進(jìn)行俯仰角和橫擺角的表示：

3 道路特征獲取與姿態(tài)估計(jì)

3.1 道路障礙物檢測(cè)

在本節(jié)，我們基于激光雷達(dá)的單線掃描模型，通過分析激光掃描線在凹、凸障礙物上的距離特征，建立多線掃描幾何模型，獲取道路特征。激光掃描線掃描凹、凸障礙物的幾何特征如圖2所示，雷達(dá)掃描線與障礙物分別相交于P1，P2，P3和P4?？梢钥吹郊す鈷呙杈€在不同的障礙物上呈現(xiàn)不同的特征，例如在圖2（a）中，要比小，而在圖2（b）中，要比大。

圖2 凹、凸障礙物的幾何模型

結(jié)合激光雷達(dá)的參數(shù)，激光掃描線在障礙物的分布可由式（7）計(jì)算得到：

其中，M為掃描線掃描一周的掃描點(diǎn)數(shù)，D為激光雷達(dá)與障礙物之間的距離，L為雷達(dá)相對(duì)于地面的安裝高度。

由此我們可以得到激光雷達(dá)多線掃描幾何模型由式（8）表示：

其中，t表示掃描點(diǎn)的個(gè)數(shù)，Num表示障礙物上的掃描點(diǎn)的個(gè)數(shù)。

3.2 擴(kuò)展卡爾曼濾波器

擴(kuò)展卡爾曼濾波器被設(shè)計(jì)用于非線性系統(tǒng)的信息融合［18］，本文用于對(duì)傳感器融合系統(tǒng)進(jìn)行時(shí)間和測(cè)量更新，以避免慣性測(cè)量單元隨時(shí)間累積噪聲和誤差。其原理可由式（9）、（10）來表示：

其中，xt為狀態(tài)向量，包含加速度、速度、俯仰角、橫擺角，ut和yt分別表示非線性系統(tǒng)的輸入和輸出，vt和ωt分別表示過程和測(cè)量噪聲。擴(kuò)展卡爾曼濾波的整體流程可由下式表示。

1）時(shí)間更新

其中，xt|t-1為預(yù)測(cè)的狀態(tài)估計(jì)，Pt|t-1為預(yù)測(cè)的協(xié)方差估計(jì)。

2）測(cè)量更新

其中，Zt為測(cè)量冗余，St為冗余協(xié)方差，Kt為濾波器增益，xtt為更新的狀態(tài)估計(jì)，Ptt為更新的協(xié)方差估計(jì)。

3.3 無加速度模型設(shè)計(jì)與姿態(tài)估計(jì)

在車輛運(yùn)動(dòng)的短時(shí)間內(nèi)，陀螺儀具有較高的測(cè)量精度，但是長(zhǎng)時(shí)間會(huì)產(chǎn)生累積誤差影響測(cè)量精度，而加速度計(jì)可以獲得更精確的靜態(tài)角度，因此加速度計(jì)獲取的加速度可用于測(cè)量更新。然而加速度計(jì)的測(cè)量易受車輛運(yùn)動(dòng)的干擾，因此我們?cè)O(shè)計(jì)基于卡爾曼濾波器的無加速度模型，在消除車輛運(yùn)動(dòng)影響加速度計(jì)測(cè)量結(jié)果的同時(shí)，減少陀螺儀隨時(shí)間累積的噪聲和誤差。

加速度計(jì)的測(cè)量結(jié)果、重力加速度以及外部加速度分別用At、gt和at來表示，包含橫向、縱向以及垂向加速度。將加速度計(jì)測(cè)量結(jié)果減去外部加速度即可得到不受外部運(yùn)動(dòng)影響的加速度值，具體可由式（18）、（19）、（20）表示：

然后利用3.2節(jié)所推導(dǎo)的擴(kuò)展卡爾曼濾波器進(jìn)行外部加速度的估計(jì)與跟蹤，完成無加速度模型的設(shè)計(jì)。

其中，at為外部加速度，αt為外部加速度的線性組成部分。

俯仰角和橫擺角可通過更新的測(cè)量模型計(jì)算：

至此完成道路特征的獲取以及基于卡爾曼濾波器的無加速度模型的建立并完成了姿態(tài)的估計(jì)與更新。

4 驗(yàn)結(jié)果與分析

本節(jié)中，我們進(jìn)行實(shí)驗(yàn)驗(yàn)證和結(jié)果分析，將所提的方法與現(xiàn)有的方法進(jìn)行對(duì)比比較，驗(yàn)證所提方法的優(yōu)越性。

對(duì)比實(shí)驗(yàn)選取分布障礙物的測(cè)試道路，選取的對(duì)比方法一僅使用加速度計(jì)進(jìn)行姿態(tài)估計(jì)，方法二使用加速度計(jì)和陀螺儀進(jìn)行姿態(tài)估計(jì)。測(cè)試道路的軌跡圖如圖3所示，慣性測(cè)量單元和雷達(dá)的安裝位置如圖4所示。

圖3 測(cè)試道路軌跡

圖4 慣性測(cè)量單元和激光雷達(dá)的安裝位置

激光雷達(dá)對(duì)道路特征的提取如圖5所示，姿態(tài)估計(jì)結(jié)果如圖6所示，我們可以看到，在有了道路障礙物的距離和尺寸信息，所提的加速度模型能夠準(zhǔn)確地移除車輛形式過障礙物時(shí)導(dǎo)致的外部姿態(tài)的變化，進(jìn)行準(zhǔn)確的自身姿態(tài)估計(jì)。而相比之下，方法一僅使用加速度計(jì)進(jìn)行姿態(tài)估計(jì)容易受到傳感器滑移問題的影響，方法二未采用擴(kuò)展卡爾曼濾波器進(jìn)行噪聲的移除以及外部加速度的干擾，導(dǎo)致結(jié)果較真實(shí)值有很大的偏差。

圖5 激光雷達(dá)道路特征提取結(jié)果

圖6 姿態(tài)估計(jì)結(jié)果

5 結(jié)語

本文針對(duì)行駛在戶外道路上的無人駕駛卡車，提出了一種用于精確姿態(tài)估計(jì)的方法，基于卡爾曼濾波的無加速度模型結(jié)合激光雷達(dá)提供的道路特征，能夠有效地移除道路障礙物帶來的外部加速度干擾，較現(xiàn)有的方法，極大地提高了姿態(tài)估計(jì)準(zhǔn)確率，能為后續(xù)的路徑規(guī)劃等應(yīng)用提供了精確的自身姿態(tài)信息。