基于EKF的变电站巡检机器人VIO/GPS融合定位算法

摘 要：随着电力事业的发展，变电站智能巡检系统被广泛应用以保障变电站的安全运行与供电质量。针对现有巡检机器人导航技术中定位精度不高的问题，提出了一种基于扩展卡尔曼滤波的视觉惯性里程计与GPS融合算法。该算法通过视觉惯性估计传播滤波器状态，并利用GPS测量进行更新，有效消除位置和航向累积漂移，同时对计算资源需求较小。理论和实验验证表明，该算法能够在室外变电站环境下实现全局一致且准确的六自由度姿态估计。

关键词：视觉惯性里程计；全球定位系统；扩展卡尔曼滤波；变电站巡检；机器人导航

DOI：10.15938/j.jhust.2024.05.006

中图分类号： TM63；TP242

文献标志码： A

文章编号： 1007-2683（2024）05-0048-08

VIO/GPS Fusion Positioning Algorithm of Substation Inspection Robot Based on EKF

ZHANG Guifeng1， ZHANG Zhongxuan2， ZHOU Chaorong3， YU Zhilong4

（1.China Southern Power Grid Research Institute Co.， Ltd.， Guangzhou 510000， China；

2.School of Instrument Science and Engineering， Harbin Institute of Technology， Harbin 150000， China；

3.Wenshan Power Supply Bureau of Yunnan Power Grid Co.， Ltd.， Wenshan 663000， China；

4.School of Automation， Harbin University of Science and Technology， Harbin 150080， China）

Abstract：With the development of the electric power industry， intelligent inspection systems for substations have been widely applied to ensure the safe operation of substations and maintain power supply quality. To address the issue of low localization accuracy in existing substation inspection robot navigation technologies， this paper proposes a fusion algorithm based on visual-inertial odometry and GPS using Extended Kalman Filtering （EKF）. The algorithm propagates the filter state through visual-inertial estimation and updates it using GPS measurements， effectively eliminating cumulative drift in position and heading， while requiring minimal computational resources. Theoretical and experimental results demonstrate that this algorithm can achieve globally consistent and accurate six-degree-of-freedom pose estimation in outdoor substation environments.

Keywords：visual-inertial odometry; global positioning system; extended kalman filter; substation inspection; robot navigation

0 引 言

随着国民经济的迅速发展，电力事业的发展愈发起到了至关重要的作用。电力建设近年来一直作为各地发展的主导力量，向着大容量、超高压的方向快速发展。变电站负责电压和电流交换及电能的分配工作，是电网系统中的重要场所。作为电能输送的纽带，保证变电站可靠、安全运行是保障电力网络供电质量的重要前提，故变电站巡检过程尤为重要。伴随着机器人技术和人工智能技术的迅速发展，越来越多的适应不同领域需求的机器人被深入研究和开发，代替人类完成一些重复性高、危险性大的工作^［1-5］。用于变电站巡检的变电站机器人智能巡检系统也得到了广泛的应用^［6-8］。利用机器人技术、多传感器技术、模式识别技术、无线通讯技术等实现对变电站电力设备全天候、全方位、全自主智能的巡检监控，解决了传统的人工巡检方式存在的巡检效率低、劳动强度大、人工成本高、安全风险高等问题，适应现代电力系统的发展需求^［9-12］。

为保障变电站巡检机器人自主运行，导航定位技术起到了至关重要的作用。现有的变电站巡检机器人技术存在性能指标差异较大、全局定位精度不高、鲁棒性较差等问题，自主导航能力受到制约，仍难以开展完全自主的巡检工作^［13-14］。变电站工作环境复杂，利用单一传感器的导航方式难以达到变电站巡检需求的导航定位精度。随着对巡检机器人定位能力要求的提高以及传感器成本、功耗的降低，组合导航方式被广泛应用于巡检机器人系统中^［15-16］。

近年来，结合相机和惯性测量单元（inertial measurement unit， IMU）进行姿态估计以完成导航定位的视觉惯性里程计（visual-inertial odometry， VIO）在室内外环境中取得了较高的定位精度。被广泛应用于机器人导航系统中^［17-18］。但是，作为一种局部定位方法，VIO算法会随着时间累积位置和航向漂移，不足以进行长期的全局定位，定位导航精度会随时间降低，易受干扰，不满足变电站机器人智能巡检系统的导航定位需求，并且由于电力场景存在非可视区域，局部定位精度也会有所影响。为了解决这个问题，可以利用全球定位系统（global positioning system， GPS）来提供全局一致的位置估计，但GPS测量噪声大，频率低且易受障碍物遮挡影响卫星信号，使其无法可靠地进行精确定位。将VIO和GPS进行集成可以克服两种模式各自的缺点，以进行高速、全局准确、鲁棒性强的姿态估计。

融合视觉、惯性和GPS数据以进行机器人导航有两种主要方法：优化算法和滤波算法。在文［19］中，VIO估计在位姿图优化中与全局测量以松耦合的方式进行融合。文［20］与文［19］结构相似，添加了一个节点来表示局部框架来对绝对航向进行约束。文［21］则使用了一种紧耦合优化算法，考虑所有传感器之间的相关性来提高估计精度。

尽管优化算法定位精度较高，但计算开销也大幅增加，这限制了它们在嵌入式系统上的适用性。通过边缘化历史测量并仅更新最新状态，滤波算法可以实现有利于在线估计的高效递归计算。文［22］使用迭代扩展卡尔曼滤波器（iterated extended kalman filter， IEKF）将来自不同传感器的延迟测量与IMU驱动状态融合，从而实现亚毫秒级处理时间。文［23］构建了一个强大的无迹卡尔曼滤波器 （unscented kalman filter， UKF），它结合了视觉里程计、GPS、气压计和IMU测量，用于微型飞行器导航。文［24］基于文［25］的紧耦合多状态约束卡尔曼滤波器（multi-state constraint kalman filter， MSCKF）框架来执行在线的VIO/GPS时空校准。尽管一些研究取得了较好的定位精度，但大都缺乏航向评估结果，且大多数算法通过更新融合中的3个航向轴，会使GPS带有高噪声的高度测量影响融合后的横滚角和俯仰角估计，会对变电站巡检机器人系统正常工作产生不利的影响。

现有的变电站巡检机器人导航定位方法，主要依赖于单一传感器（如GPS或视觉惯性里程计），但在复杂的电力设施环境中，单一传感器方法容易受到遮挡、信号噪声等因素的影响，导致定位精度不足。为解决这一问题，本文提出了一种基于扩展卡尔曼滤波器（extended Kalman filter， EKF）的VIO/GPS融合定位算法。与传统的融合方法不同，本文算法通过结合VIO的局部精度与GPS的全局稳定性，并采用扩展卡尔曼滤波对传感器数据进行优化融合，从而提高了在复杂场景中的导航定位精度和稳定性。此外，本算法设计考虑了计算资源的限制，具备较低的计算复杂度，适合在资源有限的机器人系统中部署。

1 算法框架

1.1 坐标系

系统框架所使用的坐标系如图1所示。相机、IMU和GPS被安装在同一个地面载体上。VIO估计在世界坐标系FW和IMU坐标系FI之间计算，同时将IMU坐标系与载体坐标系重合。GPS测量由以东北天序列表示的全球坐标系FG和GPS天线坐标系FA间转换计算得到。FW和FG的Z轴均与地球的引力矢量对齐。

1.2 系统框架

本文提出的定位系统框架如图2所示。首先，由相机产生的双目图像Π^c1j和Π^c2j与IMU输出的线加速度aik和角速度ωik相耦合得到VIO的状态估计vk。然后，VIO状态估计将被GPS的测量zgl通过EKF进行校正，输出VIO/GPS状态估计ek。VIO和GPS的不确定项记为σvk和σgl，由两个子系统连续提供给滤波器以适应当前噪声条件。其中j、k、l分别为相机、IMU、GPS的时间序列标记。

2 基于EKF的VIO/GPS位姿估计算法

2.1 视觉惯性里程计子系统

为获得VIO状态估计，首先需进行相机与IMU之间的时间同步与校准。在校准方面，校准数据集在理想的光照和运动条件下收集，并由开发完善的Kalibr工具箱^［25］处理，以生成相机内部和外部校准参数，以及相机和IMU之间的空间和时间转换矩阵；在时间同步方面，通过连续读取并补偿时变相机的快门周期，以确保图像和惯性测量之间的紧密同步。

在准备工作之后，我们利用最先进的基于开放关键帧的视觉惯性SLAM（OKVIS）方法进行VIO估计^［26］。该算法通过最小化视觉界标的重投影误差和有界滑动窗口中的IMU误差的联合损失函数来求解最优载体状态。它通过前端检测、描述与匹配图像关键点来关联相机帧并初始化3D地标。在滑动窗口中保留了许多重要的相机帧（称为关键帧），以减少地标的观测不确定性。历史帧和部分非关键帧被边缘化，以提高计算效率。

2.2 过程模型

扩展卡尔曼滤波器的状态向量定义如下：

xe=［pT^WIvT^WIψ^WI］T∈R7（1）

其中：p^WI和v^WI为世界坐标系中载体的位置和速度。由于VIO子系统已经提供了全局横滚角和俯仰角的可观测性^［27］，因此只有偏航角ψ^WI在融合模型中被校正。

载体连续时间状态运动学模型为

^WI=R^WIIvv+np

^R·WI=R^nψR·v^WI（2）

其中：n=［nTp，nψ，nλ］T为包含具有协方差Q=^E{nnT}=（σv）2I4的VIO估计的高斯白噪声；R为偏航角噪声的旋转矩阵。VIO线速度vv和角速度^R·v^WI用来防止将累积漂移添加回滤波器状态中。

为实现EKF，我们应用式（2）的一阶近似来获得离散时间过程模型如式（3）所示：

xek=f（xe^k－1，uvk，nk）

p^WIkv^WIkψ^WIk=

p^WIk－1+R^WIk－1pv^Ik－1Ik+n_p^kΔtv

R^WIk－1v^Ik－1Ik+n_p^k

ψ（R^WIk－1Rv^Ik－1Ik）+n^ψkΔtv（3）

式中：Δtv为VIO系统的采样周期；ψ（R）=atan2（R²¹，R¹¹）计算旋转矩阵所提供的偏航角。输入向量uv由两个连续帧之间的VIO位置、速度、航向的相对偏差组成，如式（4）所示：

uvk=pv^Ik－1Ik

vv^Ik－1Ik

Rv^Ik－1Ik=

（Rv^WIk－1）T（pv^WIk－pv^WIk－1）

（Rv^WIk－1）Tvv^WIk

（Rv^WIk－1）TRv^WIk（4）

围绕最终状态e^k－1将式（3）线性化，状态转移和过程噪声的雅克比矩阵为：

Fk=I30［ez］×^{R^WIk－1}pv^Ik－1Ik

00［ez］×^{R^WIk－1}vv^Ik－1Ik

001（5）

Lk=I3Δtv00

I300

0Δtv0（6）

其中ez=［001］T为z轴的单位向量；［ez］×为ez的反对称阵。

EKF状态估计和误差协方差传播方程如下：

^e－k=f（e^k－1，uvk，0）（7）

P－k=FkP^k－1FTk+LkQkLTk（8）

2.3 测量模型

在tl时刻，GPS天线位置、速度的滤波测量模型pg^GAl和vg^GAl如下式所示：

zgl=h（xel，ml）

pg^GAlvg^GAl=R^GW（R^WIlp^IA+p^WIl）+p^GW+m^pl

R^GW（R^{WIl［ωvl］}×p^IA+v^WIl）+m^vl（9）

其中：测量噪声矢量m=［mTp，mTv］T包含协方差R=E{mmT}=（σg）2I6的高斯白噪声过程。局部和全局参考帧之间的变换由p^GW和R^GW表示，通过使用文［28］中的posyaw方法对准VIO和GPS的初始轨迹获得。位置偏移量p^IA在真实传感器设置时手动标定。角速度ωv由VIO系统连续提供。

围绕传播状态估计^e－l将式（9）线性化，得到测量和测量噪声的雅克比矩阵如下式所示：

Hl=R^GW0R^GW［ez］×^{R^}－^WIlp^IA0

0R^GWR^GW［ez］×^{R^}－^WIl［ωvl］×p^IA0（10）

Ml=I6（11）

卡尔曼增益以及状态估计和误差协方差校正如下式所示：

Kl=P－lHTl（HlP－lHTl+MlRlMTl）^－1

el=^e－l+Kl（zgl－h（el，0））

Pl=（I－KlHl）P－l（12）

3 实验和分析

3.1 实验设备

为了对所提算法进行评估，我们配置了基线为12cm的Point Grey Bumblebee2相机、ADIS16488 MEMS IMU、U-blox M8N GPS接收机。相机频率为20Hz，图像分辨率为640×480。IMU频率为500Hz，角度随机游走为0.3°/（hr）^1/2，速度随机游走为0.029（m/s）/（hr）^1/2。GPS接收机频率为5Hz，水平位置精度为2.5m，速度精度为0.05m/s。相机和IMU以硬件触发方式进行同步。

为了记录评估数据以及输出结果，我们以无人车为地面载体，为减少相机视图中特征点较少的天空区域，将顶部向下倾斜10°。GPS天线距离IMU为1m，以确保偏航角良好的可观测性。真值由South S82 GPS-RTK系统以及Novatel SPAN-CPT GPS/INS系统提供。GPS-RTK系统3D定位精度为1cm，GPS/INS系统由光纤陀螺仪制成，横滚角和俯仰角精度为0.02°，偏航角精度为0.06°。

3.2 算法评估步骤

为了正确评估定位算法的误差，首先需要将输出坐标系与真值坐标系相统一。以FR和FS分别表示RTK和SPAN传感器输出所在的坐标系，位置和航向的真值由p^GR和R^GS表示。VIO/GPS系统的位姿估计分别由^WI和^{R^WI}表示，可使用局部全局对准和传感器偏移将其转换到真值坐标系，如下式所示：

^GR=R^GW（^{R^WI}p^IR+^WI）+p^GW

^{R^GS}=R^{GWR^GI}R^IS（13）

在实验设置中，由于天线之间的安装存在偏移，经过实际测量修正，设定IMU和RTK间的位置偏移为p^IR=［0.018，－0.001，0.247］T，由于IMU和SPAN传感器被安装到相同的方向，二者之间的旋转漂移为R^IS=I。

GPS测量值转换方程如下：

pg^GR=R^GAp^AR+pg^GA（14）

通过将R^SA设置为单位针，通过实际测量将偏移p^AR设置为p^AR=［－0.975，0.152，－0.005］T使R^GA与R^GS相等以简化计算。

经过上述转化，每种定位方法的位置和航向与真值的相对误差计算如下式：

Δp=^GR－p^GR

ΔR=^{R^GS}RT^GS（15）

其中：ΔR可被分解为横滚、俯仰以及偏航角误差。为了评估算法的总体误差，计算各轴的均方根误差为

RMSE（）=∑N^k=1（Δxk）2N（16）

3.3 实验环境

实验所用载体为轮式移动机器人，搭载传感器在变电站室外场地进行移动采集各传感器数据。

为验证算法的局部精度与全局精度是否满足变电站巡检机器人系统要求，实验数据以两种不同的路线采集，如图3、4所示。在圆形路线中，车辆以0.5m/s的速度平稳地运动5个直径为20m的圆形环路。每次循环后，将车辆沿直线拉回起点，以检查位置和偏航估计值的重复性。在方形路线中，车辆以0.7m/s的速度平稳的运动6个方形环路，方形4个直线边均为20m，两边之间角度均为90°。

在两次数据采集过程中，在每次循环结束之后，围绕起点进行小幅度的运动，来对局部定位精度进行评估。

3.4 实验结果

由表1可见，VIO/GPS融合算法在圆形路线上的整体表现优于单独使用VIO或GPS算法。具体而言，融合算法在X轴、Y轴和Z轴上的误差相较于单独使用VIO算法分别减少了58%、75%和81.6%；相较于单独使用GPS算法分别减少了29.7%、72.5%、90.6%。综合3轴的RMSE，融合算法的总误差为0.36m，相较单独使用VIO算法减少了70.7%；相较单独使用GPS算法减少了75.3%，显示了融合算法在复杂场景下更强的定位精度。

由表2可见，在GPS不提供方位测量信息的情况下。融合算法在偏航角上的误差相较于单独使用VIO算法依旧减少了82.9%，综合3轴航向角的RMSE，融合算法的总误差为0.92°，相较单独使用VIO算法减少了82.2%，显示出融合算法在复杂场景中更高的航向角估计精度。

由表3可见，VIO/GPS融合算法在方形路线上的位置精度同样优于单独使用VIO或GPS算法。融合算法在X轴、Y轴和Z轴上的误差相较于单独使用VIO算法分别减少了61.4%、76.2%和66.7%；相较于单独使用GPS算法分别减少了28.9%、64.7%和85.9%。综合3轴的RMSE，融合算法的总误差为0.42m，相较单独使用VIO算法减少了65.8%，相较单独使用GPS算法减少了62.5%，显示了融合算法在复杂场景中的优越性。

由表4可见，VIO/GPS融合算法在方形路线上的航向角精度显著提高。融合算法在偏航角上的误差相较于单独使用VIO算法减少了77.9%。综合三轴航向角的RMSE，融合算法的总误差为1.11°，相较单独使用VIO算法减少了76.9%，进一步展示了融合算法在航向角估计上的显著精度提升。

圆形和方形路线下算法的定位结果如图5和图6所示。在这两种情况下，VIO估计最初都很精确，但由于漂移的累积，算法的全局定位精度逐渐受到影响。

GPS测量具有有界误差，但由于变电站场景下卫星信号可能受到遮挡，定位结果与真值相比波动较大。本文提出的VIO/GPS算法克服了上述缺点，产生了全局精确且无漂移的姿态估计。

特别地，虽然GPS不提供方位测量，但我们的算法仍然成功地消除了偏航角的漂移，这对于机器人航向控制至关重要。

4 结 论

本文针对变电站巡检机器人导航定位技术存在的性能指标差异大、全局或局部定位精度不高等问题，提出了一种基于扩展卡尔曼滤波的高效的VIO/GPS融合定位算法，采用VIO姿态估计进行状态预测，利用GPS位置测量进行状态更新，有效地将VIO的相对估计与GPS的绝对测量相结合。融合算法在圆形路线实验中将位置偏差降低了约60%，在方形路线实验中位置偏差减少了约55%。同时，融合算法在两种路线实验中均显著降低了航向角误差，相较于单独使用VIO算法，偏航角误差减少了约77.9%。实验结果表明，该算法有效解决了在变电站环境下GPS卫星信号易受遮挡、VIO估计累计漂移的问题，全局或局部定位精度均满足变电站巡检机器人系统导航定位需求。

参 考 文 献：

［1］ 张欣，李春智，薛明，等.基于阵列式发射模组的群体机器人动态无线供电路径规划研究［J］.电测与仪表，2023，60（7）：168.

ZHANG Xin，LI Chunzhi，XUE Ming，et al.Research on Path Planning for Dynamic Wireless Charging of Swarm Robots Based on Array Transmitting Module［J］.Electrical Measurement amp; Instrumentation，2023，60（7）：168.

［2］ 周开河，杨跃平，叶夏明，等.架空线路绝缘喷涂异物检测识别及机器人运动轨迹规划［J］.电测与仪表，2023，60（6）：32.

ZHOU Kaihe，YANG Yueping，YE Xiaming，et al.Detection and Identification of Foreign Matters in Overhead Line Insulation Spraying and Robot Trajectory Planning［J］.Electrical Measurement amp; Instrumentation，2023，60（6）：32.

［3］ 白万荣，张驯，朱小琴，等.基于E-FCNN的电力巡检图像增强［J］.中国电力，2021，54（5）：179.

BAI Wanrong， ZHANG Xun， ZHU Xiaoqin， et al.E-FCNN Based Electric Power Inspection Image Enhancement ［J］. Electric Power，2021，54（5）： 179.

［4］ 柯清派，史训涛，袁智勇，等.基于改进遗传算法的变电站巡检机器人路径规划［J］.电测与仪表，2023，60（8）：144.

KE Qingpai，SHI Xuntao，YUAN Zhiyong，et al.Route Planning for Substation Patrol Robot Based on Improved Genetic Algorithms［J］.Electrical Measurement amp; Instrumentation，2023，60（8）：144.

［5］ 梅远腾，李旻.基于平面螺旋发射线圈的胶囊机器人无线供能系统的设计与优化［J］.电测与仪表，2023，60（6）：19.

MEI Yuanteng，LI Min.Design and Optimization of Wireless Power Transfer System for Capsule Robots Based on Planar Spiral Transmitting Coil［J］.Electrical Measurement amp; Instrumentation，2023，60（6）：19.

［6］ 蔡志全，张晶，邢婷婷.巡检机器人在变电站运维中的应用［J］.冶金动力，2022（3）：59.

BAI Wanrong， ZHANG Xun， ZHU Xiaoqin， et al.E-FCNN Based Electric Power Inspection Image Enhancement ［J］. Electric Power，2021，54（5）： 179.

［7］ 马锋，高明亮，许崇耀，等.基于仿真优化智能变电站巡检机器人路径研究［J］.电测与仪表，2023，60（9）：14.

MA Feng，GAO Mingliang，XU Chongyao，et al.Research on Path Optimization of Intelligent Substation Inspection Robot Based on Simulation［J］.Electrical Measurement amp; Instrumentation，2023，60（9）：14.

［8］ 喻洋，董明知.光伏变电站智能巡检机器人功能设计与实现［J］.电测与仪表，2023，60（6）：40.

YU Yang，DONG Mingzhi.Design and Implementation of Intelligent Inspection Robot in Photovoltaic Substation［J］.Electrical Measurement amp; Instrumentation，2023，60（6）：40.

［9］ ZHAO Wenchao， GONG Maofa， CHEN Yujie， et al. Research on Navigation Method of Substation Inspection Car Based on Image Recognition［C］//Chinese Automation Congress. 2017 Chinese Automation Congress （CAC）， Jinan， China. Piscataway： IEEE， 2017： 3236.

［10］DAVID L， COLIN K， JOHN O. GNSS Time Signal Spoofing Detector for Electrical Substations［C］//2022 IEEE Power amp; Energy Society General Meeting （PESGM）， Denver， CO， USA. Piscataway： IEEE， 2022： 1.

［11］WANG Yuechao， MAI Ying， WEN Weijian. Visual Composite Navigation Technology of Electric Inspection Robot［C］//2021 3rd International Academic Exchange Conference on Science and Technology Innovation （IAECST）， Guangzhou， China. Piscataway： IEEE， 2021： 327.

［12］HAN Yuankai， LI Jianxiang， ZHOU Dazhou， et al. Wireless Charging System for Substation Inspection Robot［C］//2020 IEEE 9th Joint International Information Technology and Artificial Intelligence Conference （ITAIC）， Chongqing， China. Piscataway： IEEE， 2020： 1201.

［13］于志浩，张艳荣，张安迪，等.基于改进RBPF的变电站巡检机器人建图方法研究［J］.电测与仪表，2023，60（6）：26.

YU Zhihao，ZHANG Yanrong，ZHANG Andi，et al.Research on Mapping Method of Substation Inspection Robot Based on Improved RBPF［J］.Electrical Measurement amp; Instrumentation，2023，60（6）：26.

［14］MING Yi. Research on Simulation of Intelligent Robot System for Substation Inspection［C］//2021 5th Asian Conference on Artificial Intelligence Technology （ACAIT）， Haikou， China. Piscataway： IEEE， 2021： 469.

［15］郭川，徐文虎，韩舒.基于实时多任务流系统的高空自动化接线机器人研究［J］.电测与仪表，2023，60（6）：46.

GUO Chuan，XU Wenhu，HAN shu.Research on High-altitude Automatic Wiring Robot Based on Real-time Multi-tasking Flow System［J］.Electrical Measurement amp; Instrumentation，2023，60（6）：46.

［16］LIU Weidong， ZHANG Shaohai， FAN Shaosheng. A Visual Navigation Method of Substation Inspection Robot［C］//2016 International Conference on Progress in Informatics and Computing （PIC）， Shanghai， China. Piscataway： IEEE， 2016： 148.

［17］JEFFREY D， DAVIDE S. A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots［C］//2018 IEEE International Conference on Robotics and Automation （ICRA）， Brisbane， QLD， Australia. Piscataway： IEEE， 2018： 2502.

［18］SHERIF A. S. M， MOHAMMADHASHEM H， TOMI W， et al. A Survey on Odometry for Autonomous Navigation Systems［J］. IEEE Access， 2019， 7： 97466.

［19］RUBEN M， LUCAS T， TIMO H， et al. GOMSF： Graph-Optimization Based Multi-Sensor Fusion for Robust UAV Pose Estimation［C］//2018 IEEE International Conference on Robotics and Automation （ICRA）， Brisbane， QLD， Australia. Piscataway： IEEE， 2018： 1421.

［20］PAN J， SHEN S，CAO S， et al. A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors， 2019.

［21］GIOVANNI C， DAVIDE S. Tightly-coupled Fusion of Global Positional Measurements in Optimization-based Visual-Inertial Odometry［C］//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）， Las Vegas， NV， USA. Piscataway： IEEE， 2020： 5089.

［22］SIMON L， MARKUS W. A， STEPHAN W， et al. A Robust and Modular Multi-sensor Fusion Approach Applied to MAV Navigation［C］//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems， Tokyo， Japan. Piscataway： IEEE， 2013： 3923.

［23］ANDREW C， SEBASTIAN S， LUKE Y， et al. Robust Multi-sensor Fusion for Micro Aerial Vehicle Navigation in GPS-degraded/denied Environments［C］//2014 American Control Conference， Portland， OR， USA. Piscataway： IEEE， 2014：1892.

［24］WOOSIK L， KEVIN E， PATRICK G， et al. Intermittent GPS-aided VIO： Online Initialization and Calibration［C］//2020 IEEE International Conference on Robotics and Automation （ICRA）， Paris， France. Piscataway： IEEE， 2020：5724.

［25］NGUYEN H. K. T， VINHHAO N. A Tightly-Coupled Visual-Inertial System with Synchronized Time for Indoor Localization［J］. International Journal of Mechanical Engineering and Robotics Research， 2021， 10（4）： 202.

［26］PAUL F， JOERN R， ROLAND S. Unified Temporal and Spatial Calibration for Multi-sensor Systems［C］//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems， Tokyo， Japan. Piscataway： IEEE， 2013： 1280.

［27］STEPHAN W， MARKUS W. A， SIMON L， et al. Real-time Onboard Visual-Inertial State Estimation and Self Calibration of MAVs in Unknown Environments［C］//2012 IEEE International Conference on Robotics and Automation， Saint Paul， MN， USA. Piscataway： IEEE， 2012：957.

［28］ZHANG Zichao， DAVIDE S. A Tutorial on Quantitative Trajectory Evaluation for Visual（-Inertial） Odometry［C］//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）， Madrid， Spain. Piscataway： IEEE， 2018： 7244.

（编辑：温泽宇）

基金项目： 国家自然科学基金（61673128，61573117）.

作者简介：张贵峰（1986—），男，高级工程师；

张忠轩（1997—），男，硕士研究生.

通信作者：于智龙（1979—），男，副教授，E-mail：zlyu@hrbust.edu.cn.