武汉大学周瑞:基于先验位姿与运动编排的相机惯导外参标定方法 | 《测绘学报》2026年第3期

本文内容来源于《测绘学报》2026年第3期（审图号 GS京(2026)0641号）

周瑞¹, 朱锋¹, 张小红²

1.武汉大学测绘学院，湖北　武汉　430079

2.武汉大学中国南极测绘研究中心，湖北　武汉　430079

多传感器融合利用异构传感器观测的互补性实现高精度导航定位，而精确的传感器外参是实现可靠融合的前提。本文针对现有相机惯导外参标定方法中对标定板依赖较强、标定结果易受数据质量影响、数据采集过程较为复杂等问题，提出一种基于先验位姿与运动编排的无标靶相机惯导外参标定方法：利用GNSS/SINS后处理平滑结果作为先验位姿，构建重投影误差方程，基于高斯牛顿优化框架实现外参的精确估计；通过运动编排优化数据采集轨迹，利用运动转台充分激励惯导，提高影像重叠度，保证标定数据的重复性和可靠性；提出一套完整的初始化与外参精优化流程，加速优化过程的收敛，实现外参的最优估计。仿真结果表明，该方法可实现优于0.05°的旋转外参估计精度和优于1 cm的平移外参估计精度；实测试验验证了该标定方法及流程的可行性与标定效果。

关键词：组合导航; 传感器标定; 惯性导航系统; 多源融合

导航定位服务在人类社会、经济和军事活动中发挥着支撑作用^[1]，被广泛应用于空间探索、地球科学、大地测量、智慧城市和智能交通等领域。随着智能化时代的到来，多源传感器融合技术已成为提升机器人、自动驾驶等智能系统导航精度和稳健性的关键^[2]。通过整合不同传感器的数据，克服单一传感器定位技术固有的局限性和脆弱性^[3]，能显著提高系统在各种复杂情境下的适应能力与稳定性，从而实现更准确、更可靠的导航和决策。在这些智能系统中，精确的定位与定姿是感知、决策与控制等上层应用的基础。多源异构传感器坐标系之间的刚体变换关系是进行数据融合的基础与前提^[4]。不同观测数据（如GNSS载波、相位、像素特征点、激光雷达点云、UWB距离等）被各个传感器独立获取并表达在自身的坐标系下，而最后的融合结果往往是载体或者惯导中心的位置速度与姿态，其中涉及坐标系的转换，即将各观测数据转换到统一的坐标框架下来处理^[5]。

目前，可见光相机凭借观测信息丰富、成本低、体积小、功耗低等优势，被广泛用来与惯导进行融合实现导航定位与建图等功能。利用相机获得丰富的纹理信息能纠正IMU零偏与噪声导致的误差积累并抑制发散^[6]，而IMU的高频观测能在快速运动中提供载体的运动增量，这种互补特性能够提高系统在复杂环境下的状态估计精度与稳定性^[7]。近年来，基于相机与惯导的同时定位与建图技术（visual-inertial simultaneous location and mapping，VI-SLAM）得到了广泛的研究，诞生出一系列优秀的开源项目（如ORB-SLAM3^[8]、VINS-Mono^[9]、OKVIS^[10]等）。然而，这些算法都需要较精确的相机惯导外参值作为输入，以保证观测数据的正确关联与转换^[11]，并提升系统初始化与状态估计的稳定性。

按照标定耗时来区分，目前的相机惯导外参标定方法主要包括在线标定与离线标定两类^[12]。前者是在系统初始化时进行外参的粗略估计，后续将其误差作为滤波或者优化参数在定位过程中做动态补偿。文献[13]使用扩展卡尔曼滤波（extended Kalman filter，EKF），将相机惯导外参作为滤波参数进行估计，但标定精度对于外参的初始状态较敏感。文献[14]重构了IMU的预积分计算，提出“相对预积分”方法（relative preintegration，RP），将IMU观测值从IMU坐标系转换到其他刚体坐标系下，从而实现更精更高效的IMU数据产出，在非线性优化框架下解决初始化与外参标定问题。文献[15]在移动设备上实现了同时导航与标定相机惯导的外参，该方法基于滤波框架，并在后续扩展到可以同时标定相机与IMU的内参。文献[16]在先前的VINS-Mono系统中引入了一种在线标定相机惯导数据的时间延迟的方法，通过联合优化时间延迟、相机与IMU的状态参量，以及特征点位置，实现了不亚于离线标定方法的高精度。文献[17]提出了一种基于单目VINS的实时状态估计系统，在未初始化时，会自动初始化速度、姿态并进行初步的相对位姿标定。初始化完成后，再实施高精度的状态估计，并非线性优化相机与惯导之间的位姿参数。此外，文献[18]推导了多传感器耦合时，导致单目相机与IMU的时间与空间标定失败的4种退化运动，对于单目相机与惯导的外参标定具有指导意义。

离线标定对于计算量与计算的复杂度限制较小，能实现更高的标定精度与可靠性，对于传感器相对位姿比较固定的集成设备，有较大实用意义。文献[19]提出了一种基于EKF的相机惯导空间位姿标定方法，其证明当给定随机运动时，外参是可观测的。该方法局限性在于当IMU数据存在较大的噪声时，非线性系统的线性化的过程会降低结果的稳健性，实际的应用场景受到限制。离线标定的经典工作是Kalibr^[20-22]，该方法使用连续时间基函数来建模传感器轨迹，得益于参数化表达能查询系统任意时刻的运动状态，该框架能方便地关联惯导原始观测与相机观测，实现相机惯导外参、时间延迟等参数的全局精确估计。文献[23]在Kalibr基础上，提出了无须标定板的相机惯导外参标定方法，在保证标定效果的同时，进一步拓展了该方法在弹性集成设备上的适用性，但标定效果依赖于数据采集质量。

作为对现有依赖原始视觉与IMU观测，且对运动激励和场景条件较为敏感的外参标定方法的改进，本文提出一种基于运动编排与先验位姿的相机-惯导外参标定的方法。该方法通过设计可有效激励IMU各自由度的运动轨迹，提升外参可观测性，并利用GNSS/SINS后处理获得的高精度先验位姿构建重投影误差模型，构建联合优化框架，形成一套无须标定板、适用于大尺度场景的自动化标定流程。此外，本文进一步提出一种多阶段外参精优化策略，实现平移与旋转外参的高精度求解。为验证方法的有效性与优势，本文通过仿真与实测数据对标定精度和实际应用可行性进行了系统评估。

1　相机惯导外参初始化

相机惯导外参标定需要一个粗略的初始值，以便于进行后续的迭代调整。本节将详细介绍相机的观测模型，以及相机惯导间旋转与平移外参的初始化方法。该方法无须预先给定传感器安装结构参数，仅根据相机观测值与GNSS/SINS后处理位姿即可完成外参的初始估计。

1.1　相机观测模型

本文采用针孔成像模型描述路标点与像素观测间的关系

(1)

式中，u为路标点对应的像素观测坐标；P^c为路标点在相机坐标系下的坐标，z为其z轴分量；P^e为路标点在地心地固坐标系（Earth centered Earth fixed，ECEF）下的坐标；为ECEF系到相机坐标系的旋转矩阵；为相机光心在ECEF系下的坐标；K为相机内参矩阵，表达为

(2)

式中，f_x、f_y为焦距分量；（u₀，v₀）为像主点坐标。

1.2　旋转外参初始化

相机与惯导间的旋转外参初值对于标定方法的精度与收敛性十分关键，过大的偏差会导致方法陷入局部最优、系统初始化失败。而与此同时，基于旋转外参对于精度敏感的特性，只要给予系统足够的旋转激励，该参数通常具有良好的可观测性并可被稳定估计。本文采用手眼标定法进行旋转外参初值的估计。

首先，对影像数据进行全局运动恢复结构（structure from motion，SfM）处理，获取相机在运动过程中的离散位置、姿态以及路标点的坐标初值，从而可以计算相邻影像帧间的相对旋转。其次，结合GNSS/SINS经过RTS（rauchtung-striebel）双向平滑后获得的惯导先验位姿，同理可以计算对应于相邻影像帧时刻的惯导相对旋转

(3)

式中，、为k时刻相机坐标系到世界坐标系的旋转四元数以及k时刻惯导坐标系到ECEF坐标系的旋转四元数；⊗为四元数乘法操作符；（·）^*为取四元数的逆。

从而，在任意时刻k，相机与惯导之间的相对旋转均满足如下约束

(4)

叠加不同时刻的多次观测，可以获得如下超定方程

(5)

式中，和分别表示左右四元数乘积矩阵；β_k为旋转对的权重，用启发式算法确定以抑制离群值的影响，表达式如下

(6)

式中，δ为设定的阈值。

对于超定方程，基于最小二乘原则，最优的解可以通过奇异值分解法获得，具体来说，可以表示为矩阵Q的最小奇异值对应的右单位奇异向量。

1.3　平移外参初始化

得到相机惯导间的初始旋转参数后，平移外参初值可以通过轨迹对齐得到。假定在数据采集过程中，相机与惯导之间保持稳定的刚体连接，那么相机光心与惯导中心存在如下的杆臂固连关系

(7)

(10)

式中，L_IMU是惯导中心的运动航迹长度；L_camera是相机中心的运动轨迹长度；s₀是尺度因子初值；、为相邻时刻惯导的位置向量；、为相邻时刻相机的位置向量。

此时，，代入式（8），得

(11)

定义残差

    (12)

从而，基于列文伯格-马夸尔特（Levenberg-Marquardt）算法，迭代调整、与s，获得相机与惯导间的平移外参初值     (13)

2　相机惯导外参联合优化

在初始化阶段，基于SfM与GNSS/SINS后处理获得的位姿，相机与惯导间的平移与旋转外参被粗略的计算。接下来，以路标点的重投影误差为优化目标，进一步对外参进行精细估计。

首先，路标点通过三角化处理，计算其在ECEF系下的初值坐标P^e。

然后，根据路标点距离相机光心的远近，将其分为近点与远点

(14)

式中，d₀为设定的阈值，本文取4 m；d_i为相机光心与路标点的距离；S_near、S_far分别为近点与远点的集合；i为路标点的序号，i=1，2，…，n。

本文提出一种远近点策略，利用近距离路标点对平移扰动更敏感、远距离路标点对旋转扰动更敏感的几何特性，来依次调整平移与旋转外参，从而加快参数收敛过程，如图1所示。

图1

图1   外参优化策略

Fig. 1   Optimization strategy for sensor extrinsics

在处理近处点时，只迭代调整平移外参与路标点，将其余优化变量固定

(15)

在处理远处点时，只迭代调整旋转外参与路标点，而固定其余参数

(16)

式中，u_i，j为路标点P_i在第j帧影像中的像素观测值；f（·）为针孔投影函数。

最后，基于全局光束法平差，联合优化路标点坐标、平移旋转外参以及惯导先验位姿，获得全局最优的标定结果

(17)

对于上述非线性优化问题，将待估参数的真值与初值间的误差量作为优化参数

(18)

式中，、为与对应的“ϕ角模型”误差向量，满足

(19)

式中，、为旋转矩阵的真值；、为旋转矩阵的初值；×为将向量转换为对应的反对称矩阵的操作符。

通过将投影方程式（1）在参数初值处做一阶泰勒展开，得到参数增量与重投影误差间的关系

(20)

式中，δu_i，j为第i个路标点在第j张影像中的重投影误差；重投影误差关于待估参数的雅可比矩阵J_x通过链式法则计算

(21)

式中，为第i个路标点在第j帧影像的相机坐标系下的坐标。各偏导数表达式为

(22)

(23)

基于最小二乘原则，每次迭代中待估参数的增量Δx计算方式为

(24)

式中，W为像素观测权阵，本文取单位阵。

根据参数增量更新待估参数

(25)

式中，为广义加法运算符，其对于欧氏空间元素a与流形空间元素R的运算定义如下

(26)

通过不断地迭代优化，最终获得最优的外参标定结果

(27)

3　试验评估

为了验证本文方法，开展仿真与实测试验验证。仿真试验侧重于分析该方法的可行性与理论标定精度；实测试验旨在验证在实际场景下的标定效果。

3.1　仿真试验

3.1.1　试验设置

相机惯导外参真值设置如图2所示；仿真轨迹采用三轴正弦运动叠加，保证位置姿态变化足够丰富，路标点均匀分布在轨迹周围，相机前向视线整体保持朝向一致，以提高路标点的重复观测次数，如图3、图4所示；影像分辨率为1920像素×1080像素，采样率为10 Hz。通过向轨迹真值中添加一系列误差模拟GNSS/SINS后处理获得的先验位姿。

图2

图2   仿真试验外参

Fig. 2   Extrinsic parameters in simulation experiments

图3

图3   仿真轨迹与路标点分布

Fig. 3   Simulated trajectory and landmark points

图4

图4   仿真轨迹位置姿态

Fig. 4   The position and orientation of the simulated trajectory

3.1.2　结果分析

向各帧位姿真值中添加0.02 m的位置误差与0.1°的姿态误差，作为惯导先验位姿；基于各帧影像位姿真值与路标点真值，通过针孔成像公式获得路标点投影，向其中添加2个像素的随机误差作为视觉观测值。按上述设置进行500次蒙特卡罗仿真测试，标定结果的误差累积分布图、箱线图如图5所示，各标定参数的误差和2σ误差见表1。

图5

图5   误差累积分布和箱线图

Fig. 5   Cumulative error distribution plot and box plot of the simulation experiment

表1仿真试验外参标定误差统计

Tab. 1  Statistical analysis of extrinsic calibration errors in simulation experiments

试验结果表明，对于包含0.02 m/0.1°位姿误差的先验位姿，相机惯导外参标定精度可达到旋转外参优于0.02°、平移外参优于0.5 cm，即本文方法能实现高精度的外参标定。可以发现，对于旋转外参，三轴旋转角的标定精度接近，而平移外参中，z轴方向的分量精度明显差于x轴与y轴，这是因为z轴为相机视线方向，而x轴与y轴为像平面方向，后者对于重投影误差变化更加敏感，因而标定精度更高，而z轴平移外参受到的约束相对较弱，导致精度稍差。

进一步，考虑到在开阔环境下，不同等级与类型的惯导，其GNSS/SINS后处理获得的先验位姿精度也有所差别，下面分别将含有不同初始误差的先验位姿代入标定算法进行解算，统计结果见表2，误差累积分布如图6所示。

表2仿真试验不同位姿误差和像素误差下的外参标定结果误差统计

Tab. 2  Statistical analysis of extrinsic calibration error results under varying pose errors and pixel errors in simulation experiments

图6

图6   不同试验组别外参标定结果误差累积分布

Fig. 6   Cumulative error distribution plots of extrinsic calibration results for different experimental groups

结果表明，在相同的先验位姿误差下，外参标定结果精度受到像素误差影响较小，只要解算过程能收敛，那么标定算法就能提供高精度的外参结果。在5个像素误差下，旋转与平移外参精度仍优于0.02°与0.002 m；其次，可以看到，位置误差对于外参标定精度影响较为显著，随着位置误差的增大，旋转与平移参数的精度也明显下降，但即便是0.05 m的误差水平下，外参标定精度仍能维持在：旋转外参优于0.05°，平移外参优于0.01 m。与之类似，先验位姿的姿态误差也对标定精度有较大影响，在0.5°的误差水平下，旋转外参精度优于0.03°，平移外参精度优于0.005 m。此外，随着先验位姿的位置误差增大，解算成功率开始下降，这是因为在该优化问题中，惯导位置与平移外参及路标点坐标耦合在一起，在初值较差时，优化过程容易发散或者陷入局部最优，导致解算失败。

3.1.3　参数敏感性分析

由于本文方法采用远近点策略，通过分别使用远处与近处的路标点进行优化来提高外参估计精度，其中，远近点划分由预先设定的距离阈值d₀来界定，因此有必要对该参数进行敏感性分析来考察其对最终标定效果的影响，从而确定最优的阈值选择。

本文仍采用模拟试验的设置，先验位姿误差与像素误差分别控制在0.02 m/0.1°与2像素，在其余试验变量保持一致的情况下，分别设置不同的阈值d₀进行标定解算，统计结果精度。考虑到传感器实际的标定场景，阈值太小或太大则几乎可用路标点均被划分为了远点或近点，难以体现本文远近点策略的作用，于是这里的阈值范围控制在1~10 m，试验结果见表3。

表3仿真试验远近点阈值参数敏感性分析

Tab. 3  Sensitivity analysis of near-far point threshold parameters in simulation experiments

由表3可知，当远近点阈值过大或过小时，外参估计的精度都会有所下降，取折中的距离如4、5、6 m作为阈值时标定精度较高。这是因为，当阈值过小时，划分为近点的路标点过少，导致在精估计平移外参时可用观测值过少，路标点空间分布过于集中，限制了平移外参的估计精度。与此同时，划分为远点的路标点过多，对旋转外参精估计时，虽然观测充足，但由于有近处的点混在其中，实际上等同于拿所有的路标点一起估计旋转外参，无法有效利用旋转参数对远处点敏感的特性，因而会降低旋转外参的标定精度。同理，当阈值过大时，相当于所有的点都用于平移外参精估计，而用于旋转外参估计的远处点较少且分布较远，观测不足，导致标定精度下降。

3.2　实测试验

为了进一步验证该标定方法在实测环境下的可行性，在开阔环境下布设标定场景，进行原始数据采集与处理，并进行完整流程的标定测试。

3.2.1　试验场景与数据

试验环境与测试平台如图7所示。其中，测试场地为某校园内天台开阔场景，视觉纹理丰富，GNSS观测质量良好，事后RTK结果可实现厘米级精度；数据采集平台集成了单目工业相机、单天线GNSS接收机、MEMS级别惯性测量单元、工控机等设备，具体参数见表4。各传感器间通过硬件同步实现微秒级时间精度；运动转台可以带动传感器集成平台按照既定路线持续运动，实现丰富的姿态变化。

图7

图7   试验场景及数据采集平台

Fig. 7   Experimental setup and data acquisition platform

表4数据采集平台硬件信息

Tab. 4  Hardware information of the data acquisition platform

传感器类型	典型参数	参数值
MEMS-IMU（HGuide I300）	速度随机游走
角度随机游走
加表零偏不稳定性	20 μg
陀螺零偏不稳定性	3（°）/h
观测频率	200 Hz
单目相机（MER-131）	快门类型	全局快门
分辨率	1280×1024像素
观测频率	10 Hz
GNSS接收机（Mosaic-X5）	GNSS天线	HX-CH7609A
观测频率	10 Hz

为了使数据采集平台能充分运动，设计如图8所示的三轴正弦轨迹，设计好的轨迹数据按照约定格式传入运动转台，在起止阶段各静止3 min，中间阶段进行10 min充分的三轴姿态运动，进行自动化的数据采集。

图8

图8   转台运动轨迹编排及数据采集情况

Fig. 8   Planning of the turntable motion trajectory and an overview of the data acquisition process

3.2.2　结果分析

将采集到的GNSS与IMU原始观测进行RTK/INS紧组合解算，通过RTS双向平滑解生成惯导中心的先验位姿，截取其中500 s用于标定算法处理，这段数据的GNSS状态如图9所示，前后向互差如图10所示。可以看到，姿态前后向互差最大约为航向角1′，位置互差最大约为0.02 m，即开阔环境下RTK/INS后处理能提供精确可靠的惯导先验位姿。

图9

图9   实测数据GNSS状态

Fig. 9   GNSS status of the measured data

图10

图10   RTK/INS后处理紧组合前后向互差

Fig. 10   Forward-backward residuals of RTK/INS post-processed tight coupling from measured data

将影像、先验位姿带入标定算法进行解算，标定结果见表5。可以看到，本文方法得到的外参结果与传感器设计值及Kalibr标定结果较接近，其中旋转外参差异小于1°，平移外参差异小于1 cm，这表明本文的标定算法能实现相机惯导外参的精确标定。

表5实测数据外参标定结果

Tab. 5  Results of extrinsic parameter calibration on field data

随着设备长期使用过程中的震动、碰撞等影响，真实外参会逐渐发生变化，且对于相机而言，其光心位置从结构上难以确定，从而外参的设计值并不等价于真值。为了更好地评价外参精度，分别将本文标定结果与Kalibr标定结果代入开源VIO框架VINS-MONO中，根据实测数据定位精度侧面验证外参精度。测试场景与定位结果如图11所示。

图11

图11   VIO测试结果

Fig. 11   Experimental results of visual-inertial odometry

结果表明，使用本文方法标定得到的外参用于VIO测试时，其定位精度与Kalibr标定结果相当，甚至在部分场景中表现出更高精度，进一步验证了本文方法在实际应用中的有效性。

实际上，本文方法对试验环境的要求较为宽松，仅需具备足够丰富的视觉纹理信息，以保证特征点提取与匹配的稳定性。试验所选天台场景虽为开阔空间，但并未进行任何结构化布设，也未添加人工辅助特征，其视觉特征主要来自自然纹理分布，能够反映典型的真实应用条件。

需要强调的是，这类满足GNSS信号可用且视觉纹理丰富的环境在实际应用中十分普遍，如道路、厂区、场站、开放场地等。因此，本文试验环境在条件上具备代表性，能够充分验证本文方法在一般非结构化场景下的适用性和稳健性。换言之，试验场景虽然为开阔环境，但试验结果并不局限于特定场所，因而本文试验结果在同类开阔且具有一定自然纹理的非结构化场景中具有较好的参考意义。

3.2.3　效率分析

考虑到本文标定方法需要使用GNSS/SINS后处理结果，且在实际步骤上与传统离线标定方法相比也有区别，为进一步分析本文方法的实际操作复杂度与标定效率，进行了如下定性辅以定量的讨论。在操作层面，开始数据采集前需要选择一处开阔且视觉纹理较明显的区域，不需要刻意搭人工场景，能满足有一定的空间层次与稳定充足的视觉特征点即可，这在多数环境下是容易实现的。到了数据采集阶段，与常见的离线标定方法类似，都需要传感器做充分的运动，本文方法的不同之处在于，采用运动转台带动传感器集成平台完成预设姿态运动，这个改变使得标定数据采集更加便捷。由于运动转台能进行充分的姿态运动，且其运动可编程的特性使得研究人员可以重复标定效果最优的轨迹，这两点能帮助得到比人工标定数据采集更好的数据质量。在数据处理阶段，需借助GNSS/SINS后处理软件先获得高精度先验位姿，再代入标定算法中进行外参的解算，前者由于发展已经十分成熟，自动化程度高，耗时短，后者由于有前者提供的可靠的位姿数据，也能加速收敛，因而本文方法在数据处理阶段也具备不错的效率与简单的操作流程。

以实测试验为参考，统计本文方法与Kalibr在整个标定流程中的平均时间消耗。该耗时数据测试平台为i7 9700 CPU与GTX 1650 GPU的x86架构笔记本电脑。由表6可知，虽然本文方法多了GNSS/SINS解算的步骤，但由于标定阶段运算量以及收敛效率的优势，总用时与Kalibr接近，即引入GNSS辅助观测不会影响整体的标定效率。

表6实测数据标定耗时分析

Tab. 6  Time consumption analysis of measured data calibration

4　结束语

本文着眼于相机惯导外参标定问题，提出了一种基于先验位姿与运动编排的标定方法。该方法利用开阔场景下GNSS/SINS紧组合获取的高精度先验位姿构建视觉观测模型，并以环境中路标点为视觉特征，通过最小化重投影误差完成外参优化。通过设计合理的采集轨迹并结合转台姿态运动，实现了高质量标定数据的自动化获取；再配合有效的外参初始化与远近点联合优化策略，加速了参数收敛并提升了标定精度。模拟试验表明，该方法可实现旋转外参优于0.05°、平移外参优于1 cm的理论精度。进一步的实测结果验证了其在真实环境中的可行性，标定精度与Kalibr的结果相当，可在无人工标志物条件下实现可靠的相机惯导外参标定。相较于依赖人工标志物、结构化场景或严格运动约束的现有方法，本文方法具有无须特定场景、可自动采集高质量数据等特点，可为外参标定提供更具工程实用性的解决方案。尽管如此，本文方法仍存在一定局限性。如在GNSS可用度较低或视觉特征稀疏的环境中，先验位姿质量及路标点分布可能影响最终标定精度；对运动轨迹的设计仍具有一定依赖性。未来的研究将扩展至更复杂场景与更大规模试验集，以全面验证方法的泛化能力。