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

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

0 引言

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

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

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

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

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

1 多源融合導(dǎo)航系統(tǒng)

1.1 多源融合導(dǎo)航系統(tǒng)架構(gòu)

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

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

圖1 多源融合導(dǎo)航系統(tǒng)分布式混合架構(gòu)Fig.1 Distributed hybrid architecture of multi-sensor integrated navigation system

1.2 多源融合導(dǎo)航系統(tǒng)誤差模型

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

(1)

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

xk+1=Fkxk+Gkwk

(2)

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

多源融合導(dǎo)航系統(tǒng)量測方程為

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

(3)

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

2 多源融合導(dǎo)航系統(tǒng)分析

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

2.1 多源融合導(dǎo)航系統(tǒng)可觀測度

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

(4)

式中

當(dāng)系統(tǒng)完全可觀時，即可觀測性矩陣滿秩，通過利用可觀測性矩陣的偽逆，得到系統(tǒng)狀態(tài)誤差與量測之間的關(guān)系為

(5)

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

(6)

(7)

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

(8)

因此，基于動態(tài)時變系統(tǒng)的量化分析，系統(tǒng)狀態(tài)變量的可觀測度定義為

(9)

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

(10)

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

2.2 多源融合導(dǎo)航系統(tǒng)可信度

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

(11)

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

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

(12)

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

3 多源融合導(dǎo)航算法

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

3.1 多源融合導(dǎo)航因子圖算法

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

(13)

(14)

類似地，利用2個相鄰時刻變量節(jié)點(diǎn)之間的約束，可以定義二元狀態(tài)轉(zhuǎn)移因子為

(15)

式中，err(·)為狀態(tài)變量誤差函數(shù)。

3.2 基于可信度的自適應(yīng)因子圖算法

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

(16)

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

從因子圖拓?fù)渚W(wǎng)絡(luò)的角度來看，基于可信度的值，可以啟用或禁用與其關(guān)聯(lián)的約束邊，從而實(shí)現(xiàn)在拓?fù)渲屑尤牖騽h除操作。

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

圖2 自適應(yīng)因子圖模型Fig.2 Adaptive factor graph model

根據(jù)式(15)和式(16)，經(jīng)過自適應(yīng)改進(jìn)后的、基于MAP準(zhǔn)則的最小二乘解為

(17)

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

4 實(shí)驗(yàn)結(jié)果及分析

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

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

圖3 慣性/衛(wèi)星子系統(tǒng)可觀測度及可信度Fig.3 INS/GNSS subsystem DOO and DOC

圖4 因子圖融合導(dǎo)航結(jié)果Fig. 4 Factor graph fusion navigation results

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

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

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

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

如圖5和圖6所示，在衛(wèi)星導(dǎo)航信號正常的情況下，衛(wèi)星導(dǎo)航定位精度較高，能夠?qū)崟r補(bǔ)償慣性導(dǎo)航系統(tǒng)誤差，此時，慣性/衛(wèi)星組合導(dǎo)航因子(子系統(tǒng))發(fā)揮主要作用；在衛(wèi)星導(dǎo)航信號異常的情況下，傳統(tǒng)因子圖算法難以保證高精度的導(dǎo)航定位，而基于可信度的因子圖算法能夠靈活配置各導(dǎo)航信息源，通過發(fā)揮即插即用和協(xié)同增效作用，有效排除異常組合導(dǎo)航因子對系統(tǒng)性能的影響。尤其是，通過比較衛(wèi)星拒止區(qū)域C的定位精度，不難看出：基于可信度的自適應(yīng)因子圖算法，能夠自適應(yīng)調(diào)整因子圖拓?fù)浣Y(jié)構(gòu)，從而保證了復(fù)雜動態(tài)場景下多源融合導(dǎo)航算法的魯棒性和環(huán)境自適應(yīng)性，提升了自主無人系統(tǒng)跨場景導(dǎo)航模式切換和連續(xù)可靠的導(dǎo)航定位能力。

5 結(jié)論

針對復(fù)雜動態(tài)場景下高精度導(dǎo)航定位的需求，本文設(shè)計了一種具有分布式混合架構(gòu)的多源融合導(dǎo)航系統(tǒng)，并基于動態(tài)時變系統(tǒng)的可觀測度分析，提出了基于可觀測度和可信度的自適應(yīng)因子圖算法。實(shí)驗(yàn)結(jié)果和分析表明：

1)綜合利用魯棒Kalman濾波和因子圖算法，構(gòu)建具有分布式混合架構(gòu)的多源融合導(dǎo)航系統(tǒng)，從而根據(jù)傳感器感知能力靈活配置和使用各傳感器，實(shí)現(xiàn)了多源異構(gòu)傳感器的自適應(yīng)融合與可信可靠的導(dǎo)航定位。

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

3)根據(jù)各組合導(dǎo)航子系統(tǒng)的可信程度，主動優(yōu)化因子構(gòu)建和增量平滑過程，保證了衛(wèi)星導(dǎo)航信號異常等復(fù)雜場景下的連續(xù)、可靠導(dǎo)航定位，顯著提升了多源融合導(dǎo)航系統(tǒng)的環(huán)境適應(yīng)性和跨場景能力。