基于約束優化的多傳感器車載定位方法

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

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

0 引言

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

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

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

1 傳感器量測

1.1 GNSS量測

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

rG=ra-l，

(1)

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

fG=min{rS-rG}。

(2)

1.2 IMU預積分

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

(3)

IMU預積分誤差約束fB為：

(4)

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

(5)

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

1.3 激光雷達里程計

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

(6)

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

(7)

式中，歐拉角

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

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

fL=argmin{∑de+∑dh}，

(8)

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

2 狀態約束

2.1 慣導高程約束

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

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

(9)

2.2 GNSS狀態解約束

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

(a) RTK解狀態

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

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

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

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

3 多源融合系統

3.1 系統描述

本文多傳感器融合系統構建GNSS/IMU/激光雷達緊耦合的算法模型，如圖2所示，并進一步分析車輛通過連續起伏區域受力過程，給出受力方程，約束低成本MEMS高程震蕩；以RTK解算狀態監測篩選有效的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類傳感器坐標統一。

(10)

(11)

(12)

(13)

(14)

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

3.3 系統優化

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

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

(15)

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

4 實驗結果

4.1 平臺搭建

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

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

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

4.2 室內環境定位測試

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

(a) 室內場景構建圖

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

4.3 城市環境定位測試

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

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

(a) 衛星圖

4.4 山區環境定位測試

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

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

4.5 定位測試結果

3類環境定位測試結果如表2所示。

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

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

5 結束語

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