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

王翔昌 吳訓成 張偉偉

（上海工程技術大學 上海 201620）

1 引言

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

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

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

2.1 參考坐標系

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

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

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

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

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

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

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

3.1 道路障礙物檢測

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

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

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

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

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

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

3.2 擴展卡爾曼濾波器

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

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

1）時間更新

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

2）測量更新

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

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

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

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

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

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

俯仰角和橫擺角可通過更新的測量模型計算：

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

4 驗結(jié)果與分析

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

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

圖3 測試道路軌跡

圖4 慣性測量單元和激光雷達的安裝位置

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

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

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

5 結(jié)語

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