武汉大学朱锋 |《测绘学报》专栏:多传感器融合导航中的连续时间状态估计方法 |《测绘学报》2026年第5期

在智能时代背景下，多传感器融合技术已成为提升辅助驾驶汽车^[1]、机器人等移动系统导航精度与环境适应性的核心支撑。通过协同处理GNSS、惯导（inertial measurement unit，IMU）、视觉、LiDAR等异构传感器的互补数据，可以增强在卫星信号拒止、惯性器件累积误差、视觉特征退化或激光点云稀疏等单传感器失效场景下对动态复杂环境的整体感知能力，进而实现高精度、高可用、高可靠的导航定位^[2-3]。

现阶段，多源融合参数估计方法主要基于离散时间状态，分为滤波框架与优化框架两大类。基于卡尔曼滤波（Kalman filter，KF）衍生的方法，如扩展卡尔曼滤波（extended Kalman filter，EKF）、无迹卡尔曼滤波（unscented Kalman filter，uKF），采用递归贝叶斯估计，通过马尔可夫假设将状态转移约束到相邻历元，实现单历元高效求解；然而历史观测信息随递归过程呈指数衰减^[4]，难以保持一致性。优化框架基于最大后验估计，通过构建全局非线性最小二乘问题，关联所有观测的时空约束，获得更高精度的解算结果，但全批量优化的解算效率很低，难以满足实时需求。为兼顾估计精度与计算效率，滑窗滤波^[5]与滑窗优化^[6]逐渐成为多传感器融合中的主流解决方案。两者均可实现固定滞后平滑，当系统与观测噪声符合高斯分布时，数学模型均等价于窗口内数据的加权最小二乘估计^[7]。此外，滑动窗口能够平衡历史信息的利用与实时性解算的需求，已成为高精度实时系统的核心设计思路，并在典型系统中得到广泛应用，如VINS-Mono^[8]、LIO-SAM^[9]。然而，基于离散时间状态模型的估计方法在处理多传感器异步、异频与高频观测时仍存在挑战，其要求将观测数据强制对齐至固定时间节点而产生插值近似误差，引发状态不确定性传播，导致估计时延与精度退化^[10-11]。相反，连续时间状态估计方法通过轨迹参数化直接融合原始异步数据，避免时间同步与插值步骤，降低因时延带来的误差。

连续时间状态估计方法主要分为参数化与非参数化两类。其中，参数化方法的核心在于使用样条函数、多项式等具有解析表达式的函数形式，显式表达系统状态随时间演化的连续轨迹。文献[12]提出基于连续时间状态建模的最大似然估计方法，并通过相机与IMU外参标定问题验证了该框架的有效性。文献[13]在标定框架中增加了对相机与IMU间的时间偏移估计，提升标定精度的同时增大了计算开销。为缓解这一问题并提升整体优化效率，文献[14]提出采用矩阵表示法代替传统递归定义，通过结构化矩阵运算显著降低了B样条在状态估计中的计算复杂度。计算效率的提升与理论框架的完善，使基于B样条的连续时间状态估计在异步、异频与高频观测数据融合中已经取得了显著的研究进展^[15]。文献[16]将IMU原始观测数据和GNSS数据统一视为测量更新输入，直接以位置、速度与姿态作为状态量，采用三维刚体运动方程代替机械编排；在GNSS失锁期间，能将MEMS-IMU的定位漂移降低一个数量级。文献[17]提出一种创新性的惯导算法iNavFIter：从导航原理出发，基于切比雪夫多项式构建连续时间运动轨迹的全局参数化模型，从理论上规避了旋转不可交换性误差。文献[18]将该技术延伸至视觉-惯性导航系统，通过切比雪夫多项式表征连续导航状态，相较传统基于优化的VINS采用的预积分技术，能显著提升全局状态估计精度。文献[19]提出的Coco-LIC算法基于非均匀B样条实现视觉-LiDAR-惯性联合估计，在传感器退化场景和大规模场景中均表现出较高的精度与稳健性。尽管目前已有大量研究采用B样条进行连续时间状态建模，但在GNSS/SINS组合导航中的应用仍较少。

基于连续时间状态估计模型在多传感器异步、异频与高频数据融合中的优势，本文采用均匀B样条对载体轨迹进行建模，以样条控制点为状态量。该模型将IMU原始观测值作为约束融入优化框架，直接估计样条控制点参数，显著降低了系统状态维度。在此框架下融合GNSS位置观测，利用样条基函数内在的连续性关联控制点与所有观测数据，将传统的逐历元导航状态估计转换为对样条控制点的全局最优估计。通过仿真试验验证了基于均匀B样条建模的连续时间状态估计方法在多源异步传感器融合导航中的有效性，并在GNSS信号失锁与IMU数据缺失条件下进行了对比试验。实测结果进一步表明，该方法在复杂环境下仍具稳健性，为高精度导航定位提供了一种思路。

1　连续时间状态估计模型

1.1　李群和李代数表示的非线性参数空间

李群和李代数能够精准刻画导航系统中的非线性运动特征，为构建连续时间状态模型提供了数学基础^[20]。三维特殊正交群SO（3）由行列式为1的3×3正交矩阵R组成，表示三维空间中的旋转，如式（1）所示

(1)

式中，R也称作旋转矩阵；使用B样条对载体运动轨迹进行建模时，通常采用R表示姿态。

李群在单位元附近的切空间元素构成其李代数。借助指数映射与对数映射，可以在李群与李代数之间转换，解决群上的非线性难题。

1.2　连续时间状态模型

1.2.1　B样条曲线理论基础

真实世界中的载体运动，在三维空间中的轨迹基本都是连续光滑的，可以描述为时间的连续函数。B样条曲线通过控制点拟合出一条连续光滑的运动轨迹，是描述和生成平滑运动轨迹的理想工具，常见的表达式如下

(2)

式中，t∈[t₀，t_m]表示时间序列；p_i+j表示控制点；p（t）表示在t时刻的插值结果；B_j，k（t）表示随时间变化的控制点系数。

根据节点的分布情况，B样条曲线可分为均匀B样条、准均匀B样条和非均匀B样条。均匀B样条的时间节点向量中各节点在时间轴上均匀等距分布，对应的基函数B_i，k（t）形状一致且周期重复，计算简单、实现高效，在轨迹表达中被广泛采用，具体的基函数表达式可以通过Cox-de Boor递推式^[21]得到

(3)

式（3）表明，一阶系数仅在其支撑区间上非零，高阶次系数由上一阶次系数的线性组合递归生成。

1.2.2　位置姿态的B样条函数表示方法

在导航系统的轨迹建模中，理论上均匀B样条因其节点均匀分布，可能难以灵活适应真实轨迹中变化剧烈的非均匀变化特征。然而，在绝大多数实际应用场景中，载体的运动都是连续且平滑的，不会出现极端突变，因此仍可以使用均匀B样条来进行轨迹建模。本系统采用四阶均匀B样条曲线对IMU在e系下的位姿轨迹进行建模。对于6-DoF的运动状态，位置与姿态可分别表示为ℝ³（三维实数空间）与SO（3）上的样条^[22]，前者用三维欧氏向量表示，后者用旋转矩阵表示；二者在插值过程中相互独立，能提高轨迹估计的灵活性与精度^[23]。

当时间t∈[t_i，t_i+1），区间上的曲线主要受4个控制点（p_i，p_i+1，p_i+2，p_i+3）影响

(4)

通过样条插值公式，能计算任意时刻t对应的位姿状态；对时间系数归一化，令u=（t－t_i）/（t_i+1－t_i），式（4）可以转换为式（5）

(5)

基函数B_j，3（u）与u间的关系，如式（6）所示

(6)

进一步写作矩阵表达式

(7)

式中，P表示控制点矩阵；U表示归一化时间系数矩阵；M表示融合矩阵。三者计算公式如下

(8)

位置属于三维欧氏向量空间，满足向量加法和数乘的封闭性，可直接进行线性插值，使用式（7）即可。旋转矩阵位于SO（3），插值需要借助指数映射与对数映射等李群工具实现。流形结构的特殊性，使位置与姿态对应的B样条曲线表达式有所差异，使用与式（5）等价的累积B样条形式来表达姿态轨迹，如式（9）所示

(9)

式中，p_i、p_i+1－p_i、p_i+2－p_i+1、p_i+3－p_i+2均表示支撑区间上的控制点，可将其依次表示为、、、；表示累积基函数系数。将式（9）写作矩阵表达式，如下

(10)

式中，表示累积控制点矩阵；表示累积融合矩阵。如式（11）所示

(11)

最后将控制点更替成旋转矩阵R的形式，得到姿态样条表达式，如下所示

(12)

式中，exp（•）与ln（•）分别表示指数映射与对数映射；表示矩阵的第j+1行。

2　多传感器融合的连续时间状态估计框架

基于样条函数构建的多传感器融合连续时间状态估计框架如图1所示。

图1

图1   多传感器融合的连续时间状态估计框架

Fig. 1   A continuous-time state estimation framework for multi-sensor fusion

整个框架分为3层结构：底层为待估状态层，包括位姿与零偏的样条控制点，分别描述载体的运动轨迹及IMU零偏的变化规律；中间为中间变量层，通过位置与姿态的样条函数，将底层的控制点与顶层的观测数据关联起来；顶部为观测数据层，包含位置、速度、姿态等导航状态量，以及IMU提供的加速度与角速度观测值。在该框架中，待估计的样条控制点作为优化变量，利用位置样条和姿态样条函数将各类观测数据与待估状态相关联，通过构建并最小化观测值与由样条函数计算得到的预测值之间的残差，即可实现位姿和零偏控制点的最优估计，进而获得高精度的位姿轨迹。

2.1　惯导观测方程

IMU输出载体在运动过程中的加速度与角速度信息，通过机械编排，可逐步估计载体的位置、速度与姿态。基于B样条构建的连续时间状态模型，通过计算位置与姿态样条函数的导数，建立IMU原始观测与待估状态参数之间的直接关联。

受惯性传感器误差的影响，IMU测量值与真实值间存在偏差，误差模型如式（13）所示，省略比例因子、轴交叉耦合误差

(13)

式中，上标b表示在载体系下；、表示加速度计与陀螺仪的测量值；f^b、表示真实值；b_a、b_g表示零偏；η_a、η_g表示高斯白噪声。

(14)

式中，、表示方差。

IMU的加速度、角速度测量值在b系与e系间的转换关系分别如式（15）、式（16）所示

(15)

(16)

式中，a^e表示e系下的加速度；表示由b系转换至e系的旋转矩阵；表示地球自转角速度；（•）^∧表示向量的上三角运算；v^e表示载体运动速度；g^e表示重力加速度；表示在b系下的角速度。将IMU的原始观测值与分别代入式（15）、式（16），即可得到加速度与角速度相应的测量值，如式（17）所示

(17)

根据运动学原理，位置对时间的一阶、二阶导数分别是速度与加速度。同理，对位置样条曲线求导可以得到速度与加速度样条曲线，如式（18）所示

(18)

式中，Δt为均匀样条时间节点间隔；（•）^（1）、（•）^（2）表示变量的一阶导数、二阶导数；a（u）等价于a^e（t），u=（t－t_i）/（t_i+1－t_i）。

在三维空间中，物体姿态用旋转矩阵表示，对式（12）进行改写，得到式（19）

(19)

姿态样条对时间的一阶导数与物体的瞬时角速度存在联系，采用式（20）进行递归求导^[24]

(20)

式中，表示的伴随矩阵。对于四阶（k=4）B样条，ω^（4）即为待求的角速度向量。进而得到关于加速度、角速度的残差表达式，分别如式（21）、式（22）所示

(21)

(22)

IMU的加速度计与陀螺仪零偏通常被建模为常数，但在实际应用中，零偏会受温度、硬件设备等因素的影响，随时间发生漂移。因此，为提高解算结果的精度，将动态变化的零偏作为状态量添加至系统。构建关于零偏的残差表达式，如式（23）所示

(23)

式中，b_a0、b_g0表示初始零偏；、表示第i组零偏；将零偏建模为分段常数，每隔dt_Bia秒设置一个常值零偏，初始值设置为0。

2.2　外部约束观测方程

在IMU的位姿轨迹模型基础上，可进一步融合GNSS、视觉及激光雷达等外部传感器的观测数据，通过多源信息克服单一传感器的局限性，提高参数估计的准确度。以GNSS-RTK的固定解^[25]为例，高精度的厘米级位置观测可提供全局绝对位置基准。

GNSS天线与IMU通过刚性结构固连，两者间存在一个固定的三维空间向量——杆臂，将惯导中心、GNSS天线相位中心的位置分别表示为p^SINS，e、p^GNSS，e，则两者间的关系如式（24）所示

(24)

式中，l^b表示IMU测量中心到GNSS天线相位中心在b系下的杆臂。由于B样条曲线直接以IMU测量中心的运动轨迹进行建模，故通过式（24），可将待估控制点参数化的IMU位置转换为对应的天线相位中心位置，设RTK定位输出的天线相位中心位置为，则可建立位置残差向量，如式（25）所示

(25)

2.3　连续时间状态系统设计

对由IMU构建的观测值、零偏残差表达式与由GNSS构建的残差表达式进行汇总，可以得到式（26）

(26)

对于以上各个残差表达式，W_a、W_g、W_ba、W_bg、W_p分别为其对应的权重矩阵。

基于四阶均匀B样条的连续时间状态系统设计如图2所示。其中，P_i表示控制点，Z_i区域表示各控制点的作用范围，每个控制点在四阶B样条框架下具有4个有效作用区间；待优化因子为IMU原始观测、GNSS位置结果，待优化参数包括载体的位置、姿态控制点及相关的零偏控制点。

图2

图2   连续时间状态系统设计

Fig. 2   Continuous-time state system design

最后构建非线性最小二乘优化问题来求解待估参数，如式（27）所示

(27)

位姿轨迹的求解过程基于Ceres Solver非线性优化库实现。此过程以待优化的位置与姿态样条控制点为变量，通过解析计算残差方程关于控制点的导数，进而高效、精确地求解最小二乘问题。

3　试验验证

3.1　试验描述

仿真与实测数据的载体运动轨迹如图3所示，本文分别采用基于离散时间状态估计（discrete-time state estimation，DSE）的EKF松组合，融合RTS（rauch-tung-striebel）平滑器以提升后验估计精度，与基于连续时间状态估计（continuous-time state estimation，CSE）的B样条基函数拟合进行高精度定位测试，通过计算位姿误差的RMS与最大误差，量化评估两者在不同观测条件下的精度。

图3

图3   载体运动轨迹

Fig. 3   Vehicle motion trajectory

仿真数据时长为800 s，IMU采样率设置为200 Hz，加速度计与陀螺仪的零偏分别为1 mg、0.75（°）/h，二者均按照一阶马尔可夫模型建模，相关时间为3600 s，相应的白噪声为、，对应的惯导型号为NovAtel SPAN-FSAS；RTK采样率设置为1 Hz，标准差为0.05 m。实测数据为城市复杂场景下的车载数据，惯导型号为Honeywell HG4930，其间车体处于快速运动状态且GNSS信号存在遮挡，为提升对真实环境复杂变化的适应性，将节点间隔设置为0.1 s，最后通过对比两者的位姿误差来评估本文方法在实测数据中的综合表现。

3.2　结果分析

3.2.1　模拟数据正常

在GNSS信号良好且IMU数据稳定的观测条件下，分别采用DSE与CSE方法解算惯导运动轨迹，并绘制位姿误差时间序列图，如图4所示。与DSE相比，CSE的位置误差曲线波动明显更小，且航向角误差明显降低；结果一致表明，本文方法能有效抑制高频噪声的影响，具有更优的平滑性。

图4

图4   数据正常时位姿误差时间序列

Fig. 4   Pose error time series during normal data operation

位姿误差RMS的统计见表1，表明CSE在定位与定姿精度上均全面优于DSE。位置误差在东向、北向、垂向3个分量的降幅分别为47.4%、33.3%、43.5%；姿态误差在航向角、俯仰角与横滚角的降幅分别为50.0%、25.0%与30.0%；整体上，定位精度平均提升了41.4%，定姿精度提升了35.0%。

表1数据正常时位姿误差RMS

Tab. 1  RMS of pose errors during normal data operation

3.2.2　模拟GNSS信号失锁

在IMU数据稳定的前提下，于300~400 s时段内模拟GNSS信号失锁，以评估方法在无外部观测时的位姿精度维持能力。结果如图5所示，在失锁期间，CSE的位置误差显著低于DSE；结合了RTS平滑的DSE，其误差在失锁初期先发散后收敛，定位精度因此有所改善；在姿态方面，CSE的航向角误差更小，且整体上CSE的误差序列更加平滑，验证了B样条控制点间的高阶连续性约束可有效抑制惯导的误差发散。

图5

图5   GNSS信号失锁时段位姿误差时间序列

Fig. 5   Pose error time series during GNSS signal loss

GNSS信号失锁时段的位姿最大误差统计见表2。关于位置误差，CSE在东向、北向、垂向相较DSE分别降低了72.2%、55.9%、77.1%；关于姿态误差，CSE的航向角、俯仰角与横滚角分别降低了50.9%、4.0%与60.5%；水平定位最大误差由DSE的17.4 cm显著降低到CSE的7.29 cm。以上统计充分表明，在GNSS失锁情况下，本文方法的误差发散程度得到了有效的抑制。

表2GNSS信号失锁时段位姿的最大误差

Tab. 2  The maximum pose errors during GNSS signal loss

为进一步验证本文方法在GNSS信号失锁情况下维持定位精度的能力，本文增加了不同失锁时长下的测试。在其他条件保持一致的前提下，分别模拟GNSS信号在300 s的时刻失锁10、30、60、120与180 s共5种情况，并在此基础上分别进行解算。定位结果的三维最大误差如图6所示。

图6

图6   GNSS信号不同失锁时长下的最大三维定位误差对比

Fig. 6   Comparison of maximum 3D positioning errors under different GNSS signal outage durations

由图6可知，随着GNSS失锁的时间增长，CSE与DSE的解算结果都在相应变差。但在不同的失锁时长下，CES的对于定位精度的维持能力都要明显优于DES，且三维最大误差都有近50%的提升。原因在于，连续时间状态估计方法不同于传统方法依赖位置更新去修正状态及抑制误差累计，缺失绝对位置观测的时段，位姿状态会受到前后样条控制点的约束，从而不会快速发散。此外，B样条曲线的平滑特性能进一步抑制较大的定位误差，同时其局部支撑性保证其余数据段不会因局部的约束缺失而损失精度。综合这些因素，本文方法在应对GNSS信号失锁时较传统方法能有更好的定位表现，这与试验结果也相符合。

3.2.3　模拟惯导数据缺失

在GNSS信号持续良好条件下，600 s与700 s处分别模拟0.5 s的IMU数据缺失，以评估传感器部分失效时的系统稳健性。两种方法的位姿误差时间序列对比如图7所示。在数据缺失时段，CSE的位置误差曲线保持平滑，未出现明显漂移，而DSE则产生剧烈波动；同时，CSE的姿态误差仍保持稳定，显著优于DSE，验证了连续旋转参数对瞬时数据缺失具有良好的容错能力。

图7

图7   惯导数据缺失时段位姿误差时间序列

Fig. 7   Pose error time series during IMU data outage

IMU数据缺失时段的位姿误差RMS统计见表3，关于位置误差，CSE在东向、北向、垂向上相较于DSE分别降低了37.5%、57.1%、50.0%；关于姿态误差，航向角、俯仰角与横滚角的降幅分别为76.2%、78.0%与80.0%。表明连续时间状态估计模型在数据缺失时段无须进行数据内插，即可有效维持待估状态的精度。

表3INS数据缺失时段位姿误差RMS

Tab. 3  RMS of pose errors during INS data outage

3.2.4　实测数据解算

实测数据解算结果如图8所示，可观测到CSE较DSE在位置上的误差曲线波动更小；在姿态误差上，CSE解算的航向角误差相较于DSE明显更小，且变化平缓，表明B样条对轨迹的整体约束性更强，受噪声影响小，平滑性更好。统计位姿误差RMS：对于位置误差，CSE较DSE在东向、北向、垂向上分别降低了45.9%、3.6%、40.7%；对于姿态误差，CSE较DSE在航向角、俯仰角与横滚角分别降低了78.9%、77.5%与52.5%；整体上，定位精度平均提升30.0%，定姿精度提升69.6%。

图8

图8   实测数据位姿误差时间序列

Fig. 8   Pose error time series from actual measurement data

4　结语

本文探索性地采用均匀B样条构建连续时间状态模型，建立融合IMU原始观测与外部位置信息的优化框架。仿真试验表明，在GNSS信号中断及IMU数据缺失的场景下，本文方法在位置与姿态估计精度以及稳健性方面均优于基于离散时间状态模型的EKF方法；实测数据进一步验证了本文方法的有效性，并表明基于B样条的轨迹约束在复杂真实环境中仍具有良好的稳健性。上述结果验证了连续时间状态模型在处理多传感器异步、异频与高频数据融合中的潜力。未来的研究工作可从以下方面展开：一是探索非均匀B样条以提升轨迹拟合能力与计算效率；二是引入滑动窗口机制，进一步提高系统的实时性能；三是融合视觉、LiDAR等多源传感器，构建更紧密的耦合系统；四是面向多样、复杂的实际应用场景，开展系统性测试，以全面评估该系统的有效性与稳定性。

初审：侯   琳

复审：余易举

终审：宋启凡

往期推荐

资讯

祝贺！自然资源领域这些重大科技成果荣获2025年度国家科学技术奖

国务院关于印发《“十五五”碳达峰行动方案》的通知

中国科学院空天信息创新研究院-尚华哲副研究员 |《测绘学报(英文版)》“大气云和气溶胶遥感”专栏推荐

武汉大学耿江辉|《测绘学报》专栏：多源传感器“盲插即用”的组件化PNT融合框架、原理及试验验证 |《测绘学报》2026年第5期

会议预告 | 第二届全国遥感地理大会（三号通知）