基于可觀測度分析和增量因子圖的多源融合導航方法

左思琪，朱建良，沈 凱，劉庭欣

(1. 南京理工大學自動化學院，南京 210094； 2. 北京理工大學自動化學院，北京 100081)

0 引言

21世紀以來，人工智能(Artificial Intelligence, AI)和自主無人系統(Autonomous Unmanned System, AUS)技術發展日新月異，受到世界各國學術界和工業界的普遍關注，推動著智能交通、應急救援、智能安防和國防軍事等領域的發展。瞬息萬變的城市交通、野外和復雜的戰場等環境，要求自主無人系統具有高可靠性、精確的導航定位精度、靈活機動性、智能自主和動態決策能力等。單一的導航定位手段難以滿足上述要求，因此，自主無人系統必須搭載多源異構傳感器，例如全球導航衛星系統(Global Navigation Satellite System, GNSS)接收機、慣性導航系統(Inertial Navigation System, INS)、單目/雙目/全景相機、激光雷達等，以實現多源融合導航[1-2]，確保導航定位的高精度、強魯棒和可靠性。

當前，絕大多數自主無人系統只能適應相對友好的地面和空中環境，例如環境簡單的結構化道路等;但在動態障礙物頻出的城市環境、顛簸粗糙的野外環境、電磁干擾下的戰場環境和復雜惡劣的氣象環境下，則很難實現連續、準確、安全和靈活的機動。由于自主無人系統所處環境隨時間不斷改變，客觀上要求其組合導航模式也應隨之變化[1,3]。此外，由于多源異構傳感器通常按照不同的頻率工作，且信息估計和觀測模型有時是非線性的，因此，不可避免地增加了多源融合導航系統的設計難度。為了解決多傳感器輸出頻率不同、觀測模型非線性、各傳感器跨場景優選等問題[4-5]，提出了綜合利用Kalman濾波(Kalman Filter, KF)和因子圖(Fa-ctor Graph)，構建具有分布式混合架構的多源融合導航系統。

在多源異構導航信息處理方面，國內外專家學者從多個角度提出了基于優化理論、圖論和貝葉斯估計的導航定位算法。值得一提的是，Chiu等提出了基于因子圖的多源導航傳感器信息融合方法，因子圖將未知變量節點和已知量測值的關系進行編碼，能夠融合來自不同類型且可能不同步的傳感器信息，并且使系統具有即插即用的功能[6-7]。針對GNSS信號丟失的問題，Kuse等提出了一種基于邊界匹配的位姿估計算法[8]。在此基礎上，Qiu等利用擴展Kalman濾波(Extended Kalman Filter, EKF)融合視覺/慣導信息，驗證并實現了基于邊界匹配的全時段全局定位方法[9]。Sunderhauf等提出了基于因子圖的可切換約束的優化模型處理方法[10]，該方法首先被用于即時定位與地圖構建(Si-multaneous Localization and Mapping, SLAM)系統的后端中識別和剔除異常值,后來被用于處理GNSS的多徑效應[11]。針對不同的應用場景，通過對上述算法的深入研究，相關學者又提出了大量的改進算法。在改進和簡化算法計算過程的同時，也力求提高導航狀態估計精度、降低系統延時，進而保證導航定位的準確性、實時性和魯棒性。

復雜、動態、開放的大范圍場景下，自主無人系統面臨著多源傳感器環境適應性和異構導航信息可信性在線評價的問題。為了主動適應快速變化的復雜動態場景，本文提出了基于動態時變系統的可觀測度分析，在線度量慣性/衛星、慣性/視覺等組合導航因子的可信程度。可觀測性和可觀測度(Degree of Observability, DOO)[1]是控制論和估計理論中2個非常重要的概念。可觀測性是指能否通過系統在有限時間內的觀測值確定系統的初始狀態；而可觀測度反映了系統狀態變量可被觀測的程度。理論上，估計算法(例如因子圖、KF等)的工作性能也取決于系統狀態的可觀測性和可觀測度。因此，可觀測度能夠作為表征估計算法收斂速度和精度的重要指標[12]，并在線評估多源融合導航中各組合導航模式的工作性能。

針對自主無人系統復雜動態場景下高精度導航定位的需求，考慮多源融合導航系統的魯棒性、可靠性和實時性，本文綜合利用KF和因子圖，提出了一種具有分布式混合架構的多源融合導航系統。通過引入動態時變系統的可觀測度分析方法，在線度量慣性/衛星、慣性/視覺等組合導航因子的可信程度，進而根據各組合導航因子的可信程度，主動優化因子構建和增量平滑過程，實現了多傳感器自適應融合導航與可靠定位。

1 多源融合導航系統

1.1 多源融合導航系統架構

在城市峽谷、隧道、地下或室內停車場等場景下，衛星信號弱甚至丟失，嚴重影響了慣性/衛星/視覺等多源融合導航的精度和可靠性。面向復雜動態場景下自主無人系統高精度導航定位需求，針對環境的復雜性、時變性和不確定性，通過系統地分析信息估計、預測和融合方法，提出了構建慣導/衛星/里程計/視覺等多傳感器智能化融合的導航定位系統，如圖1所示。

多源融合導航系統采用分布式混合架構，以慣性導航系統為核心傳感器，衛星導航接收機、慣性里程計、視覺導航系統等為輔助傳感器。為保證慣性/衛星、慣性/視覺等濾波和融合的魯棒性、可靠性和實時性，基于自適應魯棒Kalman濾波算法，構建了慣性基組合導航因子(即子濾波器)，實現了衛星導航信號等異常時的穩健濾波，確保異常誤差的有效控制和濾波算法的快速收斂。為主動適應動態場景變化，基于動態時變系統的可觀測度分析方法，在線度量慣性/衛星、慣性/視覺等組合導航因子的可信程度，實現了多源異構信息融合過程的量化評價。考慮到自主無人系統上多傳感器輸出頻率不同和跨場景失效等因素，采用因子圖構建集中融合/濾波器，從而根據各組合導航因子的可信程度，主動優化因子構建和增量平滑過程，保證復雜動態場景下多傳感器即插即用、跨場景導航模式切換和連續可靠的導航定位。

圖1 多源融合導航系統分布式混合架構Fig.1 Distributed hybrid architecture of multi-sensor integrated navigation system

1.2 多源融合導航系統誤差模型

在多源融合導航系統中，選擇東北天(E-N-Up)地理坐標系作為導航解算坐標系。考慮到自主無人系統大多處于中低速(≤60km/h；一般情況下，自主無人系統運動速度在30km/h左右)運動狀態，因此，可以忽略載體運動參數對導航誤差的影響。此時，多源融合導航系統誤差[13]具有如下形式

(1)

為了方便在線解算，將式(1)表達成狀態空間方程的形式為

xk+1=Fkxk+Gkwk

(2)

式中，xk為系統狀態誤差矢量，其中包括位置、速度、姿態、陀螺儀漂移和加速度計零偏等誤差；wk為系統過程噪聲矢量；Gk為過程噪聲分布矩陣；Fk為系統狀態轉移矩陣，其中包含位置、速度、姿態和傳感器誤差模型。

多源融合導航系統量測方程為

zk+1=Hk+1xk+1+vk+1

(3)

式中，zk+1為量測矢量；Hk+1為量測(或觀測)矩陣；vk+1為量測噪聲矢量。

2 多源融合導航系統分析

由于自主無人系統的復雜性及觀測數據的不確定性，要求多源融合導航系統能夠對實時變化的環境具有更好的適應性及靈活配置多傳感器的能力。理論上，KF作為發展成熟且實時計算性能好的濾波算法，在組合導航和多源信息融合方面發揮了重要作用；實際工程中，常用KF算法中的估計誤差協方差來評價算法的性能。但是，在復雜環境的不確定性挑戰下，采用KF算法可能出現導航解算不可信或不可靠的情況。為了避免導航定位解算錯誤對可靠性和準確性的影響，有必要設計一種新的估計算法性能的評價方法。隨著可觀測性理論的發展，可觀測度概念的引入解決了可觀測性只能定性描述系統是否可觀的問題，實現了對系統狀態變量收斂速度及其精度的量化評價。

2.1 多源融合導航系統可觀測度

為了表征每一時刻下每個導航系統誤差的可被觀測能力，引入具有標量形式的系統量化分析方法。此時，基于式(2)和所描述的系統模型，可以得到如下的表達式[1,12]

(4)

式中

當系統完全可觀時，即可觀測性矩陣滿秩，通過利用可觀測性矩陣的偽逆，得到系統狀態誤差與量測之間的關系為

(5)

式中，+表示矩陣的偽逆。

(6)

(7)

此時，量測噪聲方差可以近似表示為

(8)

因此，基于動態時變系統的量化分析，系統狀態變量的可觀測度定義為

(9)

在實際工程中，為了計算簡便，可觀測度亦可定義為[13]

(10)

上述所定義的可觀測度計算準則，既結合了可觀測性判據矩陣，又充分考慮了估計過程中的動態誤差，以及量測噪聲異常對系統可靠性的影響。因此，保證了時變系統可觀測度與估計精度的一致性，為多源融合導航系統中的信息估計和融合算法設計提供了性能評價方法支撐。

2.2 多源融合導航系統可信度

理論上，KF和因子圖等估計算法的工作性能在一定程度上取決于系統狀態的可觀測性和可觀測度。因此，可觀測度能夠作為表征估計算法收斂速度和精度的重要指標，并在線評估多源融合導航中各組合導航模式的工作性能。基于動態時變系統的可觀測度分析，可以在線度量慣性/衛星、慣性/視覺等組合導航因子的可信程度，從而提升復雜環境變化和跨場景情況下系統的魯棒性和自適應性。

(11)

此外，基于動態時變系統的可觀測度分析，能夠探究復雜場景下傳感器性能退化對系統的影響。為了防止由于可觀測性較差狀態的引入，對多源融合導航系統造成的不利影響，本文利用函數分段線性處理策略，合理設置可信度閾值，從而主動優化各組合導航因子的接入過程，保證充分利用可信的多源導航信息來實現融合，以達到更好的狀態估計效果[10]。

多源導航信息融合過程中，隨機接入的組合導航因子所占權值，即可信度(Degree of Confidence, DOC)為

(12)

式中，低可信度閾值設置為0.2<Λ—≤0.4；高可信度閾值設置為0.6<Λ+≤1。

3 多源融合導航算法

在復雜、動態、開放的大范圍場景下，自主無人系統需要充分利用多源融合導航算法，通過提取各導航信息源中的有用信息來更新導航參數，以實現準確有效的融合定位。因此，多源融合導航算法的設計及其性能直接決定了導航系統的工作效果和系統集成的可靠性和準確性。充分考慮到復雜動態場景下多源異構導航信息源自適應切換和靈活配置的需要，在利用KF算法完成慣性/衛星、慣性/視覺等局部濾波后，本文提出了分布式混合架構下的因子圖優化和集中融合算法，充分利用因子圖即插即用和協同增效的優點，以確保多源融合導航系統的高可靠性、精確定位精度、靈活機動性、智能自主和動態決策能力等。

3.1 多源融合導航因子圖算法

因子圖是一種概率圖模型，也可以看作是由變量和因子節點組合而成的雙邊圖模型。通過對狀態隨時間的后驗概率進行編碼，能夠以簡單直觀的方式，解決具有不同頻率的異構傳感器融合問題[14]。在因子圖中，最優系統狀態估計可以基于最大后驗概率(Maximum A Posteriori, MAP)準則得到

(13)

(14)

類似地，利用2個相鄰時刻變量節點之間的約束，可以定義二元狀態轉移因子為

(15)

式中，err(·)為狀態變量誤差函數。

3.2 基于可信度的自適應因子圖算法

在城市峽谷、隧道、地下或室內停車場等場景下，衛星信號弱甚至丟失，嚴重影響了慣性/衛星/視覺等多源融合導航的精度和可靠性。為主動適應動態場景變化，迫切需要設計一種適用于跨場景的多源融合導航系統和算法。因此，充分考慮場景變化對接入組合導航因子節點的影響，提出了如式(16)所示的自適應因子

(16)

式中，Φ(Λk)為自適應可信度調節函數，其值為[0,1]之間的標量；通過可觀測度量化傳感器組合的可信程度λk=Φ(Λk)∶R→[0,1]。

從因子圖拓撲網絡的角度來看，基于可信度的值，可以啟用或禁用與其關聯的約束邊，從而實現在拓撲中加入或刪除操作。

如圖2所示，構建基于可信度因子的自適應因子圖模型。圖2中，大的空心圓圈表示變量節點；小的實心圓圈表示因子節點；xk表示第k時刻的狀態變量；第m組傳感器的自適應因子fINS/Sm(藍色)連接狀態變量xk及可信度因子；通過先驗信息構建初始時刻的因子節點fPrior(黃色)；由狀態轉移因子fst[0,1](綠色)連接0到1時刻的狀態向量，對于后續時刻可以依此類推。

圖2 自適應因子圖模型Fig.2 Adaptive factor graph model

根據式(15)和式(16)，經過自適應改進后的、基于MAP準則的最小二乘解為

(17)

通過高斯牛頓法求解最小二乘問題，并利用QR分解實現增量平滑，可以求得最優估計解，并確保更新迭代直到收斂[15]。

4 實驗結果及分析

為了驗證所設計的具有分布式混合架構的多源融合導航系統和基于可信度的自適應因子圖算法的有效性，利用在城市道路采集的真實導航數據集，本文開展了仿真實驗研究。仿真中的組合導航系統由慣性/衛星、慣性/里程計、慣性/視覺這3個獨立的子系統組成。仿真實驗總時長為340s，其中包括3段衛星導航信號異常的區域，分別是輕度干擾A區(<10s)、短暫異常B區(約20s)以及長時間異常/拒止C區(約160s)。需要特別指出的是，C區域失效路段全長約為500m，定位誤差在20s內迅速達到約20m，嚴重影響了車輛行駛安全。

在采用魯棒Kalman濾波算法的基礎上，計算出可觀測度及可信度作為各組合導航因子(子濾波器)工作性能的評價指標[16]。如圖3所示，慣性/衛星子系統在A、B和C這3個區域可觀測度和可信度的值明顯降低，嚴重影響了多源融合導航系統的可靠性和精度。在仿真實驗中，分別采用傳統因子圖算法和基于可信度的自適應因子圖算法，與真實軌跡及慣性/衛星子系統軌跡進行對比，得到了如圖4所示的多源融合導航定位結果。

圖3 慣性/衛星子系統可觀測度及可信度Fig.3 INS/GNSS subsystem DOO and DOC

圖4 因子圖融合導航結果Fig. 4 Factor graph fusion navigation results

如圖4所示，黃色實線為采用傳統KF算法時，慣性/衛星子系統的導航定位結果。在A區域，由于周圍建筑物遮擋，衛星導航信號受到輕度干擾，慣性/衛星子系統的可觀測度略有下降，但是，慣性/衛星子系統仍能正常工作且可信程度較高；在B區域，衛星導航信號異常時，慣性/衛星子系統的可觀測度和可信度迅速下降(如圖3所示)，相應地也會引起導航定位誤差增大，但慣性/衛星子系統的濾波融合過程還未發散且仍能工作；當車輛行駛進入C區域時，由于高架橋和高層建筑遮擋，慣性/衛星子系統長時間無法獲取衛星信號，此時，該子系統的可觀測度和可信度值為0，北向最大定位誤差超過200m，東向最大定位誤差超過20m，慣性/衛星子系統無法正常工作。

為了有效抑制衛星導航信號異常，除采用車載慣性導航系統與衛星導航系統接收機的組合導航策略外，有必要充分利用車載里程計和視覺等傳感器，通過多傳感器融合和優勢互補，以保證多源融合導航系統的高可靠性和高精度。如圖5和圖6所示，當采用傳統因子圖算法融合慣性/衛星/視覺等多傳感器信息時，系統的導航定位精度和可靠性有所增加，但是還不能保證車輛的安全行駛。為主動適應動態場景變化和抑制衛星導航信號異常，通過對多源融合導航系統的可觀測度分析，在線度量慣性/衛星、慣性/視覺等組合導航因子的可信程度，進而采用基于可信度的自適應因子圖算法，根據各組合導航因子的可信程度，實現了多源傳感器的智能化融合。多源融合導航定位誤差曲線如圖5和圖6所示。

圖5 東向定位誤差Fig.5 East positioning error

圖6 北向定位誤差Fig.6 North positioning error

如圖5和圖6所示，在衛星導航信號正常的情況下，衛星導航定位精度較高，能夠實時補償慣性導航系統誤差，此時，慣性/衛星組合導航因子(子系統)發揮主要作用；在衛星導航信號異常的情況下，傳統因子圖算法難以保證高精度的導航定位，而基于可信度的因子圖算法能夠靈活配置各導航信息源，通過發揮即插即用和協同增效作用，有效排除異常組合導航因子對系統性能的影響。尤其是，通過比較衛星拒止區域C的定位精度，不難看出：基于可信度的自適應因子圖算法，能夠自適應調整因子圖拓撲結構，從而保證了復雜動態場景下多源融合導航算法的魯棒性和環境自適應性，提升了自主無人系統跨場景導航模式切換和連續可靠的導航定位能力。

5 結論

針對復雜動態場景下高精度導航定位的需求，本文設計了一種具有分布式混合架構的多源融合導航系統，并基于動態時變系統的可觀測度分析，提出了基于可觀測度和可信度的自適應因子圖算法。實驗結果和分析表明：

1)綜合利用魯棒Kalman濾波和因子圖算法，構建具有分布式混合架構的多源融合導航系統，從而根據傳感器感知能力靈活配置和使用各傳感器，實現了多源異構傳感器的自適應融合與可信可靠的導航定位。

2)通過對慣性/衛星、慣性/視覺等組合導航子系統的可觀測度分析，在線評價了多傳感器不同組合方案的可信程度，以確定適合于當前場景的多傳感器優選組合方案。

3)根據各組合導航子系統的可信程度，主動優化因子構建和增量平滑過程，保證了衛星導航信號異常等復雜場景下的連續、可靠導航定位，顯著提升了多源融合導航系統的環境適應性和跨場景能力。