基于多傳感器信息融合的車輛高精度定位技術

師小波 趙丁選 孔志飛 倪 濤 趙小龍 郭慶賀

1.燕山大學電氣工程學院，秦皇島，0660042.燕山大學機械工程學院，秦皇島，0660043.燕山大學車輛與能源學院，秦皇島，0660044.河北建材職業技術學院，秦皇島，0660045.吉林大學機械與航空航天工程學院，長春，130022

0 引言

我國幅員遼闊、人口眾多，自然災害時有發生，高機動應急救援車輛作為救援裝備，需要在復雜的道路環境下保持良好的駕駛平順性和操縱穩定行，以確保車輛快速安全地到達災害現場。基于車前地形的主動懸掛控制方法通過車輛地形建構系統和組合導航系統來調節主動懸架作動力，以保證車輛在復雜的道路沖擊下仍能具有較高的平順性和穩定性。實現上述懸架控制方法需要車輛在復雜地形環境中的高精度定位技術。

近年來，全球定位系統(global positional system，GPS)以其定位精度高、速度快等優勢得到了廣泛應用，但其信號不穩定、頻率低，當處于有樓房遮擋或者信號反射嚴重的環境時很難精確定位。

目前，室外定位技術[1-4]已有諸多研究成果，依據獲取數據的傳感器的不同主要分為如下三類：基于航跡推算(dead reckoning，DR)的室外定位技術，基于衛星定位的室外定位技術，基于環境感知的室外定位技術。但在復雜的室外環境下，單一的定位方法難以滿足實際需求，隨著多傳感器信息融合技術的日益成熟和完善，人們考慮通過融合多個傳感器采集的數據，以實現一個更為精確和穩定的室外定位系統[5-8]。文獻[9]提出了一種基于全球導航衛星系統(glabal navigation satellite system，GNSS)、慣性測量單元(inertial measurement unit，IMU)、視覺多傳感器融合的組合定位系統，可同時克服單一傳感器的定位缺陷；文獻[10]提出的長距離GPS、北斗衛星導航系統(BeiDou navigation satellite system，BDS)網絡實時動態(real-time kinematic，RTK)定位方法可實現GPS/BDS單系統、雙系統的厘米級定位；文獻[11]提出的激光雷達測程和測圖(lidar odometry and mapping，LOAM)算法實現了IMU、視覺、激光三種傳感器數據的融合，是目前公認的精度最高的定位算法，但在缺少特征點的環境中難以實現車輛的準確定位。上述方法試圖通過融合多個傳感器構成組合定位系統來消除單一定位方法的局限性，雖然定位精度和穩定性有了很大提高，但也同時存在著某些缺點。

現有的高精度定位技術主要面向無人駕駛領域，使用場景大多在城市和高速公路等二維平面內，無法滿足應急救援車輛的高精度定位需求。此外，相比于城市場景下的無人駕駛乘用車輛，多軸應急救援車輛需要在復雜的道路環境下行駛，面臨著GPS信號頻繁丟失、受阻等問題，無法保證行駛過程中的穩定性。

基于上述分析，本文在研究現有室外定位技術的基礎上，通過融合GPS、IMU、激光雷達信號，實現實時、穩定、準確的信號輸出，滿足多軸應急救援車輛在復雜道路環境下高精度、高穩定性的定位需求。

1 總體方案設計

本文以三軸越野車輛為載體，GPS、IMU、VLP-16為硬件設備，實現車輛定位和環境感知功能。系統總體方案如圖1所示，選用GPS、IMU作為前端里程計[12]模型所使用的傳感器，通過卡爾曼(Kalman)濾波器[13-16]實現兩者的數據融合。融合后計算的車體位姿信息既能夠克服GPS信號不穩定、獲取數據效率低的問題，又能夠同時解決IMU定位帶來的累積誤差問題。后端優化模型[17]首先使用VLP-16激光雷達獲取車體周圍點云信息[18]，然后構建全局地圖以及局部高程地圖，通過局部和全局地圖特征點云匹配，實現對前端里程計模型的誤差修正。

圖1 總體方案示意圖Fig.1 Schematic diagram of the overall plan

2 前端里程計模型的構建

2.1 基于IMU的運動學預測模型

考慮到三軸車復雜的結構特點和動力學特性，本文首先建立了三軸車輛運動學模型[19]，如圖2所示。L1，2為第一、二軸軸距，L2，3為第二、三軸軸距，P為轉向中心，R為車輛轉向半徑，δf、δr分別為車輛前輪和后輪的轉向角，(Xf，Yf)、(X，Y)、(Xr，Yr)分別為車輛第一、二、三軸的軸心坐標，θ為車輛縱軸線與環境坐標系橫軸的夾角(即車輛的航向角)。

圖2 三軸車運動學模型Fig.2 Three-axis vehicle kinematics model

車輛的運動學微分方程如下：

(1)

式中，st為車體的位置向量；vt為車體的速度向量；at為車體的加速度向量；qt為四元數；ωt為車體角速度向量；abt為加速度畸變；ωbt為角速度畸變；gt為重力加速度；aw、ωw分別為從感器獲取的加速度向量、角速度向量；下標t表示t時刻。

通過IMU獲取的數據存在誤差，所以需要將式(1)推導為包含IMU測量噪聲的運動學微分方程：

(2)

式中，s為IMU獲取的位置向量；v為IMU獲取的速度向量；q為通過IMU數據轉化的四元數；ωb為IMU獲取的角速度向量；ab為IMU獲取的加速度向量；g為重力加速度；δ為對應量的測量誤差。

車體運動速度與IMU取得的加速度存在如下約束關系：

(3)

式(1)構建的運動學微分方程難以求解，本文將時間離散化，在Δt時間內，根據車體由k-1到k時刻的狀態轉移構建系統的誤差狀態預測方程。

通過IMU數據構建的運動學差分方程如下：

(4)

式中，vk-1、sk-1、ab，k-1、ωb，k-1、qk-1分別為k-1時刻通過IMU獲取的速度、位置、加速度、角速度和四元數值；vk、sk、ab，k、ωb，k、qk分別為k時刻通過IMU獲取的速度、位置、加速度、角速度和四元數值。

IMU測量誤差構建的運動學差分方程如下：

(5)

式中，vi、θi分別為速度、姿態角的測量誤差；ab，i、ωb，i分別為加速度、角速度的測量畸變。

2.2 基于GPS的運動學預測模型

通過IMU獲取的車輛位姿信息存在累積誤差，本文選取GPS定位系統，通過GPS與IMU的信息融合算法去消除累計誤差，提高定位系統精度。

通過GPS獲取的全局坐標與IMU積分獲取的定位坐標換算關系如下：

gm=hx(xm)+λ

(6)

式中，gm為GPS觀測到的定位坐標；xm為IMU獲取的定位坐標；hx(·)為IMU與GPS數據之間的轉化函數；λ為GPS的位置測量誤差。

由式(6)可得到位置誤差狀態觀測方程和位置誤差狀態協方差矩陣：

δgm=HPδxm

(7)

(8)

式中，Hp為位置雅可比矩陣，通過hx(·)對δxm求導獲取；U為GPS測量誤差的協方差矩陣。

通過GPS獲取的偏航角與IMU積分獲取的偏航角存在如下的換算關系：

θm=hθ(xm)+θz

(9)

式中，θm為GPS獲取的偏航角；hθ(·)為IMU和GPS航偏角信息之間的換算函數；θz為GPS的測量誤差。

由式(9)可得到偏航角誤差狀態觀測方程和偏航角誤差狀態的協方差矩陣：

δθm=Hθδxm

(10)

(11)

式中，Hθ為偏航角雅可比矩陣，通過hθ(·)對δxm求導得到；Θz為協方差矩陣。

通過GPS獲取的車體速度與IMU積分獲取的速度關系如下：

vm=hv(xm)+vz

(12)

式中，vm為GPS獲取的速度；hv(·)為IMU與GPS速度信息的換算方程；vz為GPS的速度測量誤差。

由式(12)可得到速度誤差狀態觀測方程和速度誤差狀態的協方差矩陣：

δvm=Hvδxm

(13)

(14)

式中，Hv為速度雅可比矩陣，是hv(·)對δxm的導數；Vz為速度協方差矩陣。

2.3 狀態更新

本文以卡爾曼濾波器為模型框架，通過卡爾曼增益矩陣修正誤差狀態，實現IMU和GPS數據融合，更新系統狀態和系統協方差矩陣。

卡爾曼增益矩陣如下：

(15)

通過卡爾曼增益取修正誤差狀態，修正后的誤差狀態方程為

(16)

更新后的系統協方差矩陣為

(17)

式中，I為單位矩陣。

3 后端模型優化

3.1 方案設計

前端里程計可以輸出車體的位置信息，但其定位精度仍無法滿足實際場景下所需的要求，為解決這一問題，本文采用點云匹配算法對前端里程計獲得的車輛位姿信息進行優化，并構建后端優化模型，其算法流程如圖3所示。

圖3 基于特征點云匹配的后端優化模型算法流程圖Fig.3 Algorithm flow chart of back-end optimization model based on feature point cloud matching

3.2 點云誤差補償

圖4 外參標定求解示意圖Fig.4 Schematic diagram of external parameter calibration solution

點P在世界坐標系下的坐標可以通過兩種方法求解：

一是通過前端里程計模型計算點P在k時刻的坐標WPk：

(18)

(19)

結合式(18)、式(19)構建優化方程：

(20)

3.3 構建特征點云匹配模型

為了提高車體定位過程中點云匹配算法的效率以及精確度，對點云數據集中的點進行了分類，如圖5所示。

圖5 特征點分類示意圖Fig.5 Feature point classification diagram

本文將點云分為平面點和邊線點兩類，并對采集到的每一個點求其特征值c，其計算公式如下：

(21)

按照曲率對特征點進行分類時會存在兩類無效特征點：一是由于相鄰點與激光雷達之間的夾角過小而形成的無效特征點；二是由于某些障礙物的遮擋而形成的無效特征點。為了保證選取特征點的有效性，本文將每根掃描線平均劃分為6個區間，在每個區間內，按照點云曲率的大小進行排列，選取曲率較小的4個點作為平面特征點，選取曲率較大的2個點作為邊角特征點。

(22)

(a)邊線特征點 (b)幾何關系圖6 邊線特征點求解示意圖Fig.6 Schematic diagram of solving edge feature points

平面特征點的求解如圖7所示，點i為局部地圖上的平面特征點，點j、l、m分別為全局地圖上距離點i最近的三個點，則點i到平面jlm的距離為

(23)

(a)平面特征點 (b)幾何關系圖7 平面特征點求解示意圖Fig.7 Schematic diagram of solving plane feature points

全局地圖與局部地圖坐標系下的平面特征點存在以下關系：

(24)

全局地圖與局部地圖坐標系下的直線特征點存在以下關系：

(25)

聯立式(21)～式(23)得

(26)

4 實驗及數據分析

本文在真實環境中對定位系統進行了精度和穩定性實驗，通過對比算法處理的數據和真實測量的數據來驗證系統定位的準確性和穩定性。實驗車輛參數如表1所示。

表1 車輛參數

實驗過程如下：車體在室外沿直線運行，在其運行軌跡上設置雙邊橋路障，如圖8所示。車體行進過程中選取12個點進行測量。車體在通過雙邊橋的過程中通過車載傳感器實時收集數據并繪制出高度(z)變化曲線和俯仰角的變化曲線(圖9)，通過對比真實測量的垂向高度數值和俯仰數值來驗證定位系統的精度。

圖8 雙邊橋路障Fig.8 Bilateral bridge obstecles

(a)高度變化曲線

系統輸出結果與真實測量值之間的均方根誤差即為系統的定位精度，通過圖9的數據計算可得系統的定位誤差為3.45 cm，角度誤差為0.10°。

通過屏蔽部分時間段的GPS信號來獲得GPS不穩定時段的定位數據，對比GPS信號穩定和GPS信號不穩定的實驗數據，以驗證定位系統的穩定性。同時，將本文提出的算法與LOAM算法進行對比，以驗證系統在缺少特征點環境中的穩定性，繪制數據對比曲線如圖10所示。

(a)x方向上軌跡對比

通過圖10曲線可以看出，當GPS信號不穩定時，車輛的定位精度會降低，GPS信號恢復后，車輛定位系統會迅速回到穩定狀態；通過對比LOAM算法可知，LOAM算法在缺少特征點的環境中難以進行精準的定位，而本文提出的算法可以有效地優化此類問題。

為了進一步驗證算法的實時性，使車輛以恒定速度多次通過障礙物，并實時記錄本文提出的算法與LOAM算法的程序運行周期，相關程序在2.5 GHz四核處理器和6 GB內存的Win10操作系統上運行，數據對比如表2所示。

通過表2可知，LOAM算法的平均運行周期為120.6 ms，而本文提出的算法平均運行周期為85.6 ms，所以在實時性方面可以滿足高機動救援車輛的需求。

表2 算法數據

5 結語

本文提出了一種基于多信息融合的車輛高精度定位方法，綜合利用了多種傳感器的優點，不僅消除了IMU的累計誤差，也解決了GPS在實際環境中信號不穩定、信號丟失等問題。實車測試結果表明，在GPS信號不穩定時，提出的車輛定位方法所得到的定位誤差為±3.45 cm,角度誤差為±0.10°。實驗結果表明該定位系統的準確性、穩定性均得到了有效改善。