城市峽谷環境多源導航信息彈性融合定位方法

孟 騫,姜穎穎,王立輝,胡祎雯,李圣英

（1.東南大學儀器科學與工程學院,南京 210096;2.東南大學微慣性儀表與先進導航技術教育部重點實驗室,南京 210096）

0 引言

現代都市環境下,道路交通周邊高樓林立且擁擠,形成類似自然峽谷的城市峽谷環境。在城市峽谷場景中,衛星導航信號（Global Navigation Satellite Systems,GNSS）易受到建筑物遮擋和折射,慣性導航系統（Inertial Navigation System,INS）的誤差容易隨時間積累,以視覺以及激光雷達LiDAR等新興傳感器為代表的SLAM（Simultaneous Localization and Mapping） 技術對環境和天氣較為敏感[1]。在城市峽谷環境下單一傳感器一方面暴露出脆弱性和局限性,另一方面突出表現為以故障、偏差為典型現象的模型失配,無法滿足生命安全應用需求對定位精度、完好性、連續性等的要求[2]。

彈性PNT（Resilient Positioning,Navigation and Timing,Resilient PNT）是近些年來受到較多關注的新興PNT 服務模式。彈性PNT 強調的是以多源PNT 傳感器優化集成為平臺,以函數模型彈性調整和隨機模型彈性優化為手段,融合生成適應多種復雜環境的PNT 信息,以提高海、陸、空、天等各領域關鍵任務應用的可靠性和安全性[3]。彈性導航理論的先進性從物理屬性上不僅表現為可以確保物體在多種外力作用下通過消解抵抗化解外力影響且保持自身完好,而且也表現為功能上可適應于多種外部環境的靈活適應能力[4]。2020年,美國國土安全部科學技術局發布《彈性PNT 合規性框架報告》,定義了彈性PNT 應具備的預防、響應和恢復三大功能以及面向最終用戶需求的四個級別及其對應要求[5]。早在2018年,楊元喜院士[6]已撰文闡述彈性PNT 的基本框架,在用戶終端層面提出實時或準實時地修改、選擇已有觀測模型,使其適應相應場景和相應傳感器的“彈性函數模型” 的概念以及隨觀測信息不確定度而彈性變化的“彈性隨機模型” 的優化策略。美國空軍理工學院的Jurado 等[7]研究了一套面向多傳感器全源導航的在線彈性自主管理系統（Autonomous and Resilient Management of All-source Sensors,ARMAS）,系統架構由監測、驗證、標定以及模型重構等多項功能閉環完成。美國加州大學歐文分校的Kassas 團隊[8]利用機會信號開發無線電SLAM 技術,實現了衛星拒止環境下的彈性導航。韓國船舶與海洋工程研究所[9]設計了基于增強羅蘭設備的船舶彈性PNT 系統以應對GNSS 信號弱易受干擾的問題。美國Geodetics 公司[10]基于小型化SWaP-C系統搭建了集成GPS/IMU/保持振蕩器/視覺多傳感器的終端,通過冗余IMU 和分布式網絡等提高了拒止場景下的定位精度和連續性。針對GNSS 和視覺應用均具有挑戰性的城市峽谷自動駕駛場景,2021年加拿大女王大學的Noureldin 團隊[11]將自適應巡航系統調頻連續波雷達、簡化慣性傳感器、磁強計與傳統GNSS 進行整合,建立了基于快速正交搜索非線性誤差模型的多級組合導航系統,同時在2022年進一步建立了慣導與GNSS 精密單點定位技術（Precise Point Positioning,PPP）的緊組合模型,可以在GNSS 信號中斷情況下實現車道級連續導航[12]。國內學者在彈性PNT 的架構設計方面也有探索性成果,武漢大學的劉萬科等[13]針對室內外切換定位挑戰提出了一種基于云-端協同增強的多類傳感器集成、多種先進技術融合的多體制協同精密定位架構,在智能手機平臺實現了云-端協同和多源協同?？哲姽こ檀髮W的張偉等[14]以定位精度為效用函數,研究了基于博弈論的PNT 手段選擇策略,屬于多傳感器彈性優選方面的探索。當前,針對彈性融合的終端架構模型構建方面,還處于需求量化、功能定義和新興技術探索階段,迫切需要提出和驗證更先進的架構解析模型和拓撲模型,以拓展彈性融合理論的科學范疇。

城市峽谷環境是彈性PNT 技術的典型應用場景,彈性導航理論為解決城市峽谷環境下的自主導航問題提供了新的思路。本文面向城市峽谷環境下的多源信息融合導航應用,提出了一種基于觀測傳感器獨立更新的彈性融合方法。主要架構特點是各觀測傳感器保持獨立運行,對系統狀態的更新也獨立完成,從而降低不同傳感器之間的耦合度。同時在保證最優估計的前提下,盡可能將數據融合功能往終端狀態輸出層面下沉,并采用兩級交互式架構。首先得到各通道的后驗模型概率,進一步計算以后驗概率為權值,得到數據融合輸出,最后采用典型城市峽谷場景下的實際動態實驗數據對方法的優越性進行了驗證。

1 多傳感器彈性融合算法概述

本節將介紹基于傳感器獨立更新的交互式彈性融合算法的功能實現架構,其示意圖如圖1所示。在每一個導航歷元,導航系統在狀態預測與傳播后,一步預測的狀態分別送給各觀測導航源進行獨立更新,整個過程并行處理,各獨立導航源的信號通道之間沒有相關性和狀態耦合。同時引入觀測傳感器的先驗置信度,以馬爾科夫鏈的形式用于狀態交互式彈性融合以及各通道的狀態初始化。相關具體算法推導如下:

圖1 基于傳感器獨立更新的彈性融合框架Fig.1 Resilient fusion solution based on sensor-independent-update

1.1 觀測傳感器獨立濾波

以上標“（i）” 表示第i個獨立觀測傳感器更新通道,在k-1 時刻的狀態估計矩陣和誤差協方差矩陣為和;在k時刻,測量矩陣為,系統的轉移矩陣為Φk,系統的噪聲矩陣為Qk,觀測矩陣為,測量誤差的誤差矩陣為R（i）k。濾波流程如下:

（1）狀態估計

（2）協方差狀態估計

（3）卡爾曼濾波增益

（4）狀態更新

（5）協方差矩陣更新

1.2 交互式彈性融合

在狀態域對各個獨自并行濾波的傳感器進行數據融合,有

式（6） 中,Zk為總數為M的觀測傳感器集合,為模型的后驗估計概率。

當觀測量沒有故障時,可以獲得第i個傳感器的最小均方根估計

為了更新后驗估計權重,需要求得模型在k時刻的最大似然函數

傳感器先驗轉換概率以馬爾科夫鏈的形式存儲在轉換矩陣中,提供的是各傳感器先驗置信度,與模型似然參數一起根據貝葉斯原理通過交互式融合決定各通道的后驗概率

為基于k-1 時刻的先驗概率并且會被引入到接下來的狀態更新部分之中。

最終,輸出的狀態估計和狀態協方差為各個獨立更新狀態的加權結果

2 城市峽谷場景下的實驗驗證與分析

本節將通過實驗驗證交互式彈性融合算法在復雜挑戰環境下的可行性。針對城市峽谷環境下的動態車載實驗,分別用本文提出的彈性融合方法和傳統擴展卡爾曼濾波方法進行數據處理和定位解算,對比定位誤差以及算法對不同導航源信號的處理情況。

2.1 實驗設置與數據預處理

本實驗采用香港理工大學智能導航定位實驗室所提供的開源數據[15],數據集在香港尖東地區進行采集。實驗場景以及運動軌跡如圖2所示,該地區為集中的商業娛樂區,周圍的建筑物高大且密集,是一個典型的城市峽谷環境。數據為車載動態采集,時長超過450s。相關導航傳感器的信號信息如表1所示,包含慣性導航單元、衛星導航系統、激光雷達系統和視覺相機等典型導航傳感器。

圖2 香港尖東地區車載數據軌跡Fig.2 Kinematic trajectory in East Tsim Sha Tsui,Hong Kong

表1 導航傳感器類別及型號Table 1 Classifications and types of navigation sensors

衛星接收機的天線和激光雷達、相機攝像頭均安裝在車頂,不同坐標系提前進行標定,并采用Novatel SPAN-CPT 導航系統所獲得的導航定位數據作為參考軌跡。

2.2 實驗結果與數據分析

GNSS、激光雷達和視覺三種導航觀測傳感器的獨立定位效果和精度如圖3和表2所示。相關結果表明,在城市峽谷環境下,衛星信號受建筑物遮擋和信號折射非常嚴重,傳統GNSS-RTK 定位模式幾乎無法固定,只能采用單點定位模式,定位結果不僅平均誤差很大且跳變頻繁,無法滿足定位需求;視覺定位VINS 隨著時間的推移誤差累積情況非常嚴重;激光雷達的定位精度在三者中效果最好,但是相對定位的模式導致在部分地段累積誤差也較大。單一導航源都很難在復雜挑戰環境下提供可靠的定位導航結果。

表2 獨立導航源定位誤差Table 2 Positioning errors of single navigation sensor

圖3 獨立導航源傳感器觀測值定位結果Fig.3 Positioning results of single navigation sensor

本文采用INS/GNSS/LiDAR/VINS 的多源融合松組合模型,系統模型采用15 維的狀態矢量

式（14）中,δφ、δv、δp分別為三維運動姿態、速度和位置誤差向量,δba、δbg為加速度計和陀螺儀的偏差向量。

觀測向量為

式（15） 中,pGNSS、pLiDAR分別為GNSS、LiDAR定位坐標的三維輸出,ΔpVINS為VINS 的相對坐標輸出。

鑒于在城市峽谷環境下LiDAR 的定位精度最高、GNSS 和VINS 的定位精度均不太理想,本實驗中彈性融合方法的傳感器優先級馬爾科夫鏈設置為

經典擴展卡爾曼濾波與彈性融合算法的定位結果如圖4和表3所示。其中,平均誤差指的是在每個歷元的平均定位誤差,標準差為此次實驗中所有歷元的標準差,最大誤差為所有歷元定位誤差中的最大值。

圖4 數據融合定位水平面誤差對比圖Fig.4 Positioning errors comparison of information fusion in horizontal direction

表3 擴展卡爾曼濾波和交互式彈性融合算法的定位誤差對比Table 3 Positioning errors comparison between EKF and interactive resilient fusion algorithm

實驗結果表明,在均沒有增加故障檢測和隔離功能的情況下,彈性融合算法相比傳統EKF 算法,在平均誤差、標準差和最大誤差方面均展現出明顯的優越性。尤其在運動實驗的后半段,隨著環境變化以及LiDAR、VINS 的相對誤差累計,EKF 算法下的定位誤差明顯上升,并且出現接近15m 的最大定位誤差,而彈性融合算法除最開始濾波收斂的5 個歷元外,全程定位精度均明顯優于EKF,并且對粗差的魯棒穩定性更好。

為進一步分析彈性融合算法的權重更新以及故障隔離效果,對彈性融合過程中各個獨立導航源通道的后驗概率和似然函數結果展示如圖5所示,統計結果如表4所示。

圖5 彈性融合算法中觀測傳感器的后驗概率與似然估計Fig.5 Posterior probabilities and model likelihoods of observable sensors in resilient fusion

表4 各傳感器后驗概率與似然估計均值Table 4 Means of posterior probabilities and model likelihoods for observable sensors

上述結果表明,GNSS 的μk幾乎全程為0,即算法在交互式融合過程中幾乎全程將其排除在外,動態權重的調整起到了天然的故障檢測與隔離效果。LiDAR 和VINS 的μk分別大致在0.8 和0.2 左右浮動,可見算法認定LiDAR 和VINS 的定位精度更高,認為這兩者更可靠。這其中,LiDAR 的μk遠大于VINS 的,說明在城市峽谷環境中LiDAR 的定位是最可信的,這與單傳感器的定位結果分析是相吻合的。各傳感器模型似然函數的輸出結果也驗證了算法對觀測傳感器的動態優選過程,從各傳感器模型似然函數值中可看出,LiDAR 的模型似然函數值最大,其次是VINS。GNSS 的模型似然函數值幾乎為0,系統幾乎將其定位結果排除在外,這與對各傳感器的后驗概率分析所得出的結論一致。

4 結論

本文提出了一種基于觀測傳感器獨立更新的多源導航信息彈性融合架構。各觀測傳感器均在獨立通道中對系統狀態進行更新,可以有效降低不同觀測傳感器在信號融合層面的耦合度,并且各傳感器可以采用最個性化的方式進行濾波或優化。后端交互式濾波結合各傳感器先驗信息,只對定位域狀態信息進行融合輸出。傳感器的剔除或引入均保持通道切換模式,在保持對可用傳感器開放性的同時保證了信號處理環路的穩定性和彈性。在典型城市峽谷環境下的動態車載實驗充分驗證了方法的優越性,通道更新架構和權重動態更新可以起到很好的故障隔離和傳感器動態優選,定位精度和魯棒性均明顯提高。