基于可观测度分析和增量因子图的多源融合导航方法

左思琪，朱建良，沈 凯，刘庭欣

(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)根据各组合导航子系统的可信程度，主动优化因子构建和增量平滑过程，保证了卫星导航信号异常等复杂场景下的连续、可靠导航定位，显著提升了多源融合导航系统的环境适应性和跨场景能力。