基于約束優(yōu)化的多傳感器車載定位方法

張子騰，盛傳貞，蔚保國，王星星，易卿武

(1.中國電子科技集團公司第五十四研究所，河北 石家莊 050081；2.衛(wèi)星導航系統(tǒng)與裝備技術國家重點實驗室，河北 石家莊 050081)

0 引言

實時連續(xù)、泛在無縫的高精度導航定位是保障各類無人平臺安全行進、智能作業(yè)的重要基礎，坍塌、滑坡和泥石流等城市或山區(qū)災害救援現場，復雜遮蔽地形導致應急救援平臺行進過程定位信號失聯，精度與穩(wěn)定性下降?，F階段，能夠應用于應急救援的小型化智能作業(yè)機器人僅限于其配屬的合作空間，例如商場、超市[1]，其導航定位有效范圍難以支持大型應急救援裝備在突發(fā)城市火災、地質災害等重大自然災害現場處置[2]。

視覺特征跟蹤受限于大尺度、光照等空曠自然環(huán)境；激光雷達場景辨識能力差，難以閉環(huán)全局修正；低成本慣性導航累積誤差時間漂移；GNSS信號抗干擾能力弱，易受遮擋等[3-5]。發(fā)展聯合多傳感器松/緊耦合定位方法是解決受限環(huán)境下高精度導航定位難題的主流手段，另外，在主動式定位方面，相較于視覺相機，激光雷達在精度上具有較大優(yōu)勢。通過激光雷達NDT配準，融合IMU的高精度定位方法雖然具有較高精度，但其計算復雜度較高，應用空間場景受限[6-8]；細化激光雷達點云特征可以提高算法的場景適用能力，然而IMU僅提供畸變矯正并不參與算法的優(yōu)化過程，松耦合的算法模型難以保持較高的定位精度[9]；緊耦合則通過聯合其他傳感器進行誤差同步優(yōu)化獲得更高的精度，本文多傳感器融合方法以GNSS/IMU/激光雷達緊耦合的算法模型為基礎[10-12]，可保持較高的車載定位精度。

本文約束優(yōu)化在于:① 進一步分析車輛通過連續(xù)起伏區(qū)域受力過程，給出受力方程，約束低成本MEMS高程震蕩；② 以RTK解算狀態(tài)監(jiān)測篩選有效的GNSS全局解；③ 構建3類傳感器全局的聯合誤差約束，通過全局最小誤差獲得最終解。

1 傳感器量測

1.1 GNSS量測

GNSS量測通過單基準站RTK(Real Time Kinematic)[5]進行解算無人平臺位置rG，并手動測量天線桿臂值進行補償，GNSS天線桿臂是由IMU中心指向GNSS天線相位中心的向量，衛(wèi)星天線相對于IMU處在前側、右側和上側時，桿臂值l以該向量投影在IMU坐標系下的3個分量l=(lx,ly,lz)T來表示：

rG=ra-l，

(1)

式中，ra為天線桿測量值。已知rS為系統(tǒng)位置測量值，GNSS量測誤差約束fG：

fG=min{rS-rG}。

(2)

1.2 IMU預積分

IMU量測計算無人平臺t+Δt時刻運動狀態(tài)速度vt+Δt、位置rt+Δt以及旋轉qt+Δt的估計模型為：

(3)

IMU預積分誤差約束fB為：

(4)

fB=argmin{δv,δr,δq},

(5)

式中，Δtij表示連續(xù)時刻i,j的時間間隔;vi,vj表示間隔Δtij的對應時刻i,j的速度;ri,rj表示間隔Δtij的對應時刻i,j的位置;qi,qj表示間隔Δtij的對應時刻i,j的位姿;R為慣性本體坐標系到世界坐標系的旋轉;δv,δr,δq分別表示預積分速度偏差、預積分位置偏差和預積分位姿偏差。

1.3 激光雷達里程計

激光雷達里程計通過計算曲率提取邊緣點、平面點特征[13]，執(zhí)行點到邊緣特征、點到平面特征的連續(xù)掃描匹配估計傳感器運動，de,dh分別表示點到邊緣特征、點到平面特征的距離，其中點到邊緣的特征約束為距離argmin{de}，點到平面的特征約束為距離argmin{dh}。

(6)

將上述狀態(tài)轉移過程使用6自由度[x,y,z,roll,pitch,yaw]表示，進行歐拉角到旋轉矩陣的變換，使用羅德里格斯公式進行變換：

(7)

式中，歐拉角

為旋轉方向單位向量，T=[x,y,z]為轉動幅度。

激光雷達匹配距離約束fL為：

fL=argmin{∑de+∑dh}，

(8)

式中，∑de,∑dh分別表示當前激光幀所有de,dh的累積和。

2 狀態(tài)約束

2.1 慣導高程約束

應急救援車輛在高速通過減速帶、坑洼和坎坡等起伏地段時，低成本MEMS震蕩抖動引起測量數據與車輛實際狀態(tài)偏離，造成高程方向誤差累積快速漂移，通過分析車輛連續(xù)起伏過程，給出受力方程，約束MEMS高程震蕩，實現對高程量測誤差的抑制；系統(tǒng)重力加速度為G，MEMS行進方向加速度為ay，俯仰角為pitch，則垂直方向加速度az：

az=(G-ay·sin(pitch))·cos(pitch)。

(9)

2.2 GNSS狀態(tài)解約束

GNSS量測通過單基準站RTK獲取緯度、經度和高程，如圖1所示。

(a) RTK解狀態(tài)

受遮蔽環(huán)境影響，GNSS量測難以保持固定解，利用多系統(tǒng)GPS/BDS信號解狀態(tài)監(jiān)測對GNSS量測可用性進行評估，相較于水平方向，RTK在高程方向的漂移更為明顯，單獨對高程的可用性進行分析，進一步約束全局高程跳變：

① 未解析：解算過程中檢測周跳而未解析最終定位結果，GNSS不可用；

② 浮點解：當前接收衛(wèi)星信號質量可用，已成功解算定位結果，但整周模糊度未固定，此時定位水平精度滿足車道級可用，但垂直精度不可用；

③ 固定解：當前接受衛(wèi)星信號質量良好，已成功解算定位結果，且整周模糊度固定，此時定位精度滿足車道級可用。

3 多源融合系統(tǒng)

3.1 系統(tǒng)描述

本文多傳感器融合系統(tǒng)構建GNSS/IMU/激光雷達緊耦合的算法模型，如圖2所示，并進一步分析車輛通過連續(xù)起伏區(qū)域受力過程，給出受力方程，約束低成本MEMS高程震蕩；以RTK解算狀態(tài)監(jiān)測篩選有效的GNSS全局解；構建3類傳感器全局的聯合誤差約束，通過全局最小誤差獲得最終解。

圖2 多傳感器融合框架Fig.2 Multi-sensor fusion framework

3.2 坐標轉換

首先將IMU坐標系轉入雷達坐標系[14]，然后將原始GNSS坐標系(緯度、經度、高程)轉為UTM(Universal Transverse Mercator)網格坐標系(xUTM,yUTM,zUTM)，采用擴展卡爾曼濾波估計器(Extended Kalman Filter,EKF)[15-16]融合原始IMU將GNSS結果再次轉入世界坐標系(xW,yW,zW),實現3類傳感器坐標統(tǒng)一。

(10)

(11)

(12)

(13)

(14)

式中，φ,θ,φ分別表示車載系統(tǒng)的滾轉角、俯仰角和偏航角，滾轉角、俯仰角可通過IMU獲取，偏航角可結合GPS全局信息，獲取真實NEU(北-東-天)坐標。

3.3 系統(tǒng)優(yōu)化

利用集束調整[17-18]進行全局約束優(yōu)化，建立最小誤差多因子概率權重模型。已知3類傳感器誤差約束包含IMU預積分誤差約束、GNSS量測定位誤差約束、激光雷達匹配距離誤差約束。全局約束優(yōu)化是將單一信源置信的后驗概率作為誤差約束的權重進行賦值，累計聯合形成全局約束，利用LM(Levenberg-Marquarelt)算法進行非線性參數求解，優(yōu)化各信源運動估計模型，獲得多源融合最佳置信定位解：

min{∑P(A)f(A)},A=G,B,L，

(15)

式中，P(A)為傳感器可用性度量概率值。

4 實驗結果

4.1 平臺搭建

多傳感器車載平臺配備Velodyne VLP-16激光雷達，INS-D-E1組合導航系統(tǒng)，內置GNSS板卡和MEMS慣性器件，通過接入測量天線，可輸出多系統(tǒng)GNSS以及慣性導航原始數據，系統(tǒng)平臺如圖3所示，各傳感器部分性能參數如表1所示，中央處理器為4核心i7-8550U；室內測試平臺搭載輪式小車，室外測試平臺搭載商務汽車。

圖3 多傳感器車載平臺Fig.3 Multi-sensor vehicle platform

表1 傳感器性能參數Tab.1 Sensor parameters

4.2 室內環(huán)境定位測試

室內定位測試場地為某實驗室導航試驗場，試驗場大小約為24 m×27 m，室內測試所選路徑繞行試驗場外側及內側，并盡可能回歸起點區(qū)域，驗證室內運動軌跡全程累積誤差，其中外側穿行大門走廊，通過震蕩區(qū)域(過地板門檻0.01 m)，系統(tǒng)測試結果如圖4所示。圖4(a)表示室內場景構建圖；圖4(b)，圖4(c)分別表示優(yōu)化前后水平運動軌跡圖，藍色線表示運動軌跡，黃色線表示檢測回環(huán)，紅色橢圓表示振蕩區(qū)域，綠色橢圓表示起點和終點區(qū)域；圖4(d)，圖4(e)分別表示圖4(b)，圖4(c)垂直方向局部放大；圖4(f)分別表示X，Y，Z方向軌跡定位解，紅色線為優(yōu)化前解，藍色線為本文算法解；算法優(yōu)化前如圖4(b),圖4(d)所示，小車經過震蕩點(紅色橢圓)時軌跡存在明顯偏移，高程跳變嚴重，小車回歸起點區(qū)域(綠色橢圓區(qū)域)后，高程方向存在明顯偏移；算法經過約束優(yōu)化后如圖4(c),圖4(e)所示，運動軌跡跳變頻率減少，運動軌跡平滑，起點至終點高程累計誤差顯著降低；圖4(f)對優(yōu)化前后定位結果進行標定點統(tǒng)計，震蕩后優(yōu)化前水平誤差為1.93 m，垂直誤差為0.81 m，震蕩后優(yōu)化后水平誤差為0.31 m，垂直誤差為0.09 m，震蕩發(fā)生前2算法軌跡基本一致。

(a) 室內場景構建圖

(a) 社區(qū)衛(wèi)星圖、實時建圖及運動軌跡(左->右)

4.3 城市環(huán)境定位測試

測試地點選取石家莊市某居民社區(qū)，駕駛測試車輛通過社區(qū)居民小區(qū)，并繞行社區(qū)內主干道，全程約2 km，道路兩側為小區(qū)居民樓、商超和樹木等，多傳感器車載平臺接入衛(wèi)星導航測量型天線，并通過單基準站RTK差分解算獲取GNSS定位結果，系統(tǒng)測試結果如圖5所示。

圖5(a)表示城市社區(qū)衛(wèi)星圖、建圖及運動軌跡(左->右)；圖5(b)表示GNSS周跳監(jiān)測；圖5(c)，圖5(d)分別表示優(yōu)化前、后RTK重新固定建圖連續(xù)性；圖5(f)分別表示X，Y，Z方向軌跡定位解，綠色線為IMU/Lidar耦合定位解，紫色線為EKF定位解，紅色線為緊耦合優(yōu)化前解，藍色線為本文算法解。算法經過優(yōu)化前如圖5(c)所示，由于存在浮點解和周跳影響，導致建圖偏移，GNSS信號重新固定后，建圖連接斷裂，運動軌跡急劇抖動；算法經過約束優(yōu)化后，全程檢測約束周跳6次(參見圖1RTK解狀態(tài)統(tǒng)計，例如：圖5(a)右紅色橢圓,圖5(b))，降低了GNSS信號周跳等引起的運動軌跡和場景建圖的影響，如圖5(d)所示，信號中斷重新固定后，建圖結構連續(xù)，避免結構斷裂現象；圖5(a)右中綠色橢圓，滿足回環(huán)檢測，表明全程軌跡誤差較低，具備車道級定位能力；圖5(e)對優(yōu)化前后定位結果進行統(tǒng)計，本文算法平滑性及準確性均優(yōu)于其他算法，樓層遮擋環(huán)境下，最大定位誤差小于3.5 m。

(a) 衛(wèi)星圖

4.4 山區(qū)環(huán)境定位測試

為進一步驗證算法在復雜山區(qū)環(huán)境的實用性和魯棒性，選取西山森林公園進行山區(qū)環(huán)境定位測試，起點為游客停車場，駕駛測試車輛穿過山間道路，途徑轉盤、服務區(qū)、林蔭道路以及上下坡等，全程約2 km，道路兩側為山壁、樹木以及景觀建筑等，多傳感器車載平臺通過單基準站RTK差分解算獲取GNSS定位結果，系統(tǒng)測試結果如圖6所示。

圖6(a)表示西山森林公園衛(wèi)星圖；圖6(b)表示定位系統(tǒng)實時建圖；圖6(c)分別表示X，Y，Z方向軌跡定位解，綠色線為IMU+Lidar耦合定位解，紫色線為EKF定位解，紅色線為緊耦合優(yōu)化前解(在橢圓處漂移)，藍色線為本文算法解。多傳感器車載平臺實時定位系統(tǒng)在道路崎嶇、山壁、林木遮擋的山區(qū)自然環(huán)境下，運動軌跡跳變較少，軌跡平滑，實時建圖完整，表明算法具有良好的實用性和魯棒性。圖6(c)對優(yōu)化前后定位結果進行統(tǒng)計，本文算法平滑性及準確性均優(yōu)于其他算法，平均誤差為1.56 m。

4.5 定位測試結果

3類環(huán)境定位測試結果如表2所示。

表2 定位結果(RMS)Tab.2 Positioning results (RMS) 單位：m

由表2可以看出，算法具備亞米級室內定位和車道級室外定位性能，在穩(wěn)定性和精確性上均優(yōu)于現有算法。

5 結束語

復雜場景下多傳感器融合定位的魯棒性、準確性是導航定位的研究熱點與難點，本文給出一種基于約束優(yōu)化的多傳感器車載定位方法，通過設計搭建一套GNSS/IMU/激光雷達定位系統(tǒng)，進行實時跑車測試，結果表明本文算法可實現亞米級室內定位和車道級室外定位性能。該方法有助于應急救援類車載平臺在室內、城市峽谷、山區(qū)自然環(huán)境等廣域室內外定位能力提升，后續(xù)將通過引入環(huán)境信息檢測與識別，以實現多信源概率自適應調整和廣域無縫定位切換。