一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法技术

本发明专利技术涉及定位导航技术领域，具体涉及一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法；利用IMU中加速度计和陀螺仪间的互补特性构建互补卡尔曼滤波器，通过加速度计的测量值校正陀螺仪预测的旋转结果，使IMU输出更准确的估计姿态；在后端中将未知时间偏移量添加到VIO系统的状态向量中；IMU的估计姿态被用作预测信息，多个相机对同一特征点的重投影误差被用作约束信息；在VIO系统的运行期间，持续计算时间偏移并将其叠加在IMU时间戳上，以获得IMU和相机同步的数据；使用舒尔补模型将系统状态中携带较少信息的相机状态边缘化，避免丢失图像之间的先验信息，提高了相机姿态估计的准确性。相机姿态估计的准确性。相机姿态估计的准确性。

【技术实现步骤摘要】
一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法


[0001]本专利技术涉及定位导航
，具体涉及一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法。

技术介绍

[0002]单一传感器的精度与其成本呈正相关，在低成本的民用领域，不得不考虑采用多传感器融合的策略。采用多传感器融合的策略不仅节省了大量成本，还可为系统提供更多信息，提高系统的精度。因此研究多传感器融合的方法具有重要的现实意义。由于受到各种不同传感器触发和传输延迟的影响，各传感器之间的数据存在不可避免的时间偏移，这将严重影响多传感器融合的准确性。中国专利(CN108629793A)公开了一种使用在线时间标定的视觉惯性测程法与装备。首先，将相机和IMU的时间戳粗略对齐，根据相机和IMU数据计算出粗略的时间延迟量。然后，将这些参数作为输入，构建误差项。最后，通过非线性优化方法，计算出精确的时间延迟和相机位姿。该方法虽然能够计算得到时间偏移量，但由于其采用了迭代优化算法，导致系统所需计算成本较高，在低成本计算平台上，难以满足实时性的需求。于是，刘等人提出基于改进投影模型的视觉惯性里程计在线标定方法(Y.Liu,and Z.Meng.Online Temporal Calibration Based on Modified Projection Model for Visual
‑
Inertial Odometry[J].IEEE Trans Instrum Meas.,2021,69(07):5197
‑
5207.).该方法将未知的时间偏移添加在图像的时间戳上，并将特征点投影到相邻相机帧中，与IMU预测的相机位姿进行求差，最终通过卡尔曼滤波框架计算得到未知的时间偏移。但是该方法受前端特征点的影响较大，周围环境需具备良好的光照条件，同时该方法未考虑传感器融合过程中IMU误差对系统的影响，最终会导致系统的误差变大，甚至发散。

技术实现思路

[0003]为了解决传感器融合过程中时间偏移和IMU噪声导致系统姿态估计误差较大的问题，本专利技术提出一种基于双级扩展卡尔曼滤波(Extended Kalman Filter,EKF)的视觉惯性里程计在线时间校准方法。
[0004]本专利技术所提的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，包括以下步骤：
[0005]S1.定义基于双级扩展卡尔曼滤波的视觉惯性里程计的状态向量X，定义基于双级扩展卡尔曼滤波的视觉惯性里程计的误差状态向量
[0006]S2.相机按照相机的工作频率采集双目视觉图像；IMU的加速度计和陀螺仪分别按照各自的工作频率测量加速度和角速度；
[0007]S3.当IMU的加速度计和陀螺仪分别采集到一次加速度和角速度，计算互补卡尔曼滤波器的最优估计并传播；
[0008]S4.当相机采集到一次新的双目视觉图像，对其进行特征提取与特征匹配；
[0009]S5.状态向量X增广一个相机状态，得到增广后的状态向量X，并计算增广后的协方差矩阵；
[0010]S6.通过多约束特征点的重投影误差对增广后的状态向量X进行更新，并更新增广后的协方差矩阵；
[0011]S7.采用舒尔补模型对步骤S6获取的状态向量X进行边缘化处理。
[0012]本专利技术的有益效果：
[0013]本专利技术由互补卡尔曼滤波器和多状态约束卡尔曼滤波器(Multi
‑
State Constrained Kalman filter,MSCKF)组成。互补卡尔曼滤波器将加速度计测量值作为观测向量，校正陀螺仪偏差得到最优估计，即最终为MSCKF输出更准确的初始姿态。然后，在后端中将未知时间偏移添加到MSCKF的状态向量中，并计算特征点重投影误差，该误差用于更新时间偏移、IMU状态向量和相机状态。最后，使用舒尔补模型来边缘化旧相机状态，保持较小的计算规模，保留先验约束信息，并进一步提高VIO精度。通过该技术，可有效解决当IMU受到噪声影响时相机和IMU的时间不同步问题。
附图说明
[0014]图1为本专利技术的基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法流程图；
[0015]图2为本专利技术的基于双级扩展卡尔曼滤波的视觉惯性里程计系统；
[0016]图3为本专利技术的边缘化示意图；
[0017]图4为本专利技术的延迟时间仿真图；
[0018]图5为本专利技术的轨迹精度仿真图。
具体实施方式
[0019]下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。
[0020]本专利技术提出了一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，如图1所示，包括以下步骤：
[0021]S1.定义基于双级扩展卡尔曼滤波的视觉惯性里程计的状态向量X，定义基于双级扩展卡尔曼滤波的视觉惯性里程计的误差状态向量
[0022]具体地，步骤S1包括：
[0023]S11.定义增广惯性测量单元IMU状态向量为
[0024][0025]其中，上标T表示向量转置，为互补卡尔曼滤波器的最优估计；表示从惯性坐标系到IMU坐标系的单位旋转四元数；表示陀螺仪的偏置，被建模为行走速率为n
wg
的随机游走过程；
G
v
I
和
G
p
I
分别表示IMU坐标系在惯性
坐标系下的速度和位置；表示加速度计的线性加速度偏差；和
I
p
C
分别表示相机坐标系与IMU坐标系之间的相对旋转变换和相对位置变换；t
d
表示相机与IMU之间的时间偏移；
[0026]S12.定义增广惯性测量单元IMU误差状态向量为
[0027][0028]其中，表示从惯性坐标系到IMU坐标系的小角度旋转，表示b
g
的误差状态，表示
G
v
I
的误差状态，表示b
a
的误差状态，表示
G
p
I
是误差状态，表示的误差状态，表示
I
p
C
的误差状态，表示t
d
的误差状态；
[0029]S13.定义视觉惯性里程计VIO的状态向量为
[0030][0031]其中，表示t
i
,i＝1,2,...,N时刻的相机状态，表示t
i
时刻相机坐标系到惯性坐标系的相对旋转，表示t
i
时刻相机坐标系到惯性坐标系的相对平移；
[0032]S13.定义视觉惯性里程计VIO的误差状态向量为
[0033][0034]其中，表示时刻相机状态t<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，包括以下步骤：S1.定义基于双级扩展卡尔曼滤波的视觉惯性里程计的状态向量X，定义基于双级扩展卡尔曼滤波的视觉惯性里程计的误差状态向量S2.相机按照相机的工作频率采集双目视觉图像；IMU的加速度计和陀螺仪分别按照各自的工作频率测量加速度和角速度；S3.当IMU的加速度计和陀螺仪分别采集到一次加速度和角速度，计算互补卡尔曼滤波器的最优估计并传播；S4.当相机采集到一次新的双目视觉图像，对其进行特征提取与特征匹配；S5.状态向量X增广一个相机状态，得到增广后的状态向量X，并计算增广后的协方差矩阵；S6.通过多约束特征点的重投影误差对增广后的状态向量X进行更新，并更新增广后的协方差矩阵；S7.采用舒尔补模型对步骤S6获取的状态向量X进行边缘化处理。2.根据权利要求1所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤S1包括：S11.定义增广惯性测量单元IMU状态向量为其中，上标T表示向量转置，为互补卡尔曼滤波器的最优估计；表示从惯性坐标系到IMU坐标系的单位旋转四元数；表示陀螺仪的偏置，被建模为行走速率为n
wg
的随机游走过程；
GvI
和
G
p
I
分别表示IMU坐标系在惯性坐标系下的速度和位置；表示加速度计的线性加速度偏差；和
I
p
C
分别表示相机坐标系与IMU坐标系之间的相对旋转变换和相对位置变换；t
d
表示相机与IMU之间的时间偏移；S12.定义增广惯性测量单元IMU误差状态向量为其中，表示从惯性坐标系到IMU坐标系的小角度旋转，表示b
g
的误差状态，表示
G
v
I
的误差状态，表示b
a
的误差状态，表示
G
p
I
是误差状态，表示的误差状态，表示
I
p
C
的误差状态，表示t
d
的误差状态；S13.定义视觉惯性里程计VIO的状态向量为其中，表示t
i
,i＝1,2,...,N时刻的相机状态，表示t
i
时刻相机坐标系到惯性坐标系的相对旋转，表示t
i
时刻相机坐标系到惯性坐标系的相对平移；S13.定义视觉惯性里程计VIO的误差状态向量为
其中，表示t
i
,i＝1,2,...,N时刻的相机状态的误差向量。3.根据权利要求1所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤S3通过互补卡尔曼滤波器更新方程计算互补卡尔曼滤波器的最优估计，互补卡尔曼滤波器更新方程的求取过程包括：S301.定义互补卡尔曼滤波器状态向量为S301.定义互补卡尔曼滤波器状态向量为表示从惯性坐标系到IMU坐标系的单位旋转四元数；表示陀螺仪的偏置，被建模为行走速率为n
wg
的随机游走过程；S302.按照互补卡尔曼滤波器算法，列写连续时刻下的IMU中陀螺仪测量近似状态方程为为其中，表示的估计向量的导数，表示b
g
(t)的估计向量的导数；表示估计角速度，其中w
m
(t)＝[w
x
(t) w
y
(t) w
z
(t)]
T
表示测量角速度，Ω()表示将向量转化为反对称矩阵的函数，03×1表示3
×
1维零向量；S303.基于IMU中陀螺仪测量近似状态方程，定义离散时间状态转移矩阵为S303.基于IMU中陀螺仪测量近似状态方程，定义离散时间状态转移矩阵为其中，为过程矩阵，Δt
k
表示两帧IMU数据间的时间间隔，I表示单位矩阵，Ω表示估计角速度被函数Ω()转化后的结果，03×3表示3
×
3维零向量；S304.根据离散时间状态转移矩阵得到预测状态向量，表示为：其中，H
p
表示陀螺仪状态的状态转移矩阵，w(k)表示服从均值为0，协方差矩阵为Q
k
的高斯分布；S305.基于步骤S301
‑
S304列写的方程，结合加速度残差得到互补卡尔曼滤波器更新方程，表示为：<...

【专利技术属性】
技术研发人员：李帅永，聂嘉炜，
申请(专利权)人：重庆邮电大学，
类型：发明
国别省市：