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

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

背景技术：

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

技术实现思路

1、为了解决传感器融合过程中时间偏移和imu噪声导致系统姿态估计误差较大的问题，本发明提出一种基于双级扩展卡尔曼滤波(extended kalman filter,ekf)的视觉惯性里程计在线时间校准方法。

2、本发明所提的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，包括以下步骤：

3、s1.定义基于双级扩展卡尔曼滤波的视觉惯性里程计的状态向量x，定义基于双级扩展卡尔曼滤波的视觉惯性里程计的误差状态向量

4、s2.相机按照相机的工作频率采集双目视觉图像；imu的加速度计和陀螺仪分别按照各自的工作频率测量加速度和角速度；

5、s3.当imu的加速度计和陀螺仪分别采集到一次加速度和角速度，计算互补卡尔曼滤波器的最优估计并传播；

6、s4.当相机采集到一次新的双目视觉图像，对其进行特征提取与特征匹配；

7、s5.状态向量x增广一个相机状态，得到增广后的状态向量x，并计算增广后的协方差矩阵；

8、s6.通过多约束特征点的重投影误差对增广后的状态向量x进行更新，并更新增广后的协方差矩阵；

9、s7.采用舒尔补模型对步骤s6获取的状态向量x进行边缘化处理。

10、本发明的有益效果：

11、本发明由互补卡尔曼滤波器和多状态约束卡尔曼滤波器(multi-stateconstrained kalman filter,msckf)组成。互补卡尔曼滤波器将加速度计测量值作为观测向量，校正陀螺仪偏差得到最优估计，即最终为msckf输出更准确的初始姿态。然后，在后端中将未知时间偏移添加到msckf的状态向量中，并计算特征点重投影误差，该误差用于更新时间偏移、imu状态向量和相机状态。最后，使用舒尔补模型来边缘化旧相机状态，保持较小的计算规模，保留先验约束信息，并进一步提高vio精度。通过该技术，可有效解决当imu受到噪声影响时相机和imu的时间不同步问题。

技术特征：

1.一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤s1包括：

3.根据权利要求1所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤s3通过互补卡尔曼滤波器更新方程计算互补卡尔曼滤波器的最优估计，互补卡尔曼滤波器更新方程的求取过程包括：

4.根据权利要求3所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，加速度残差的计算公式为：

5.根据权利要求1所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤s3传播最优估计，包括：

6.根据权利要求5所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤s5包括：

7.根据权利要求1所述的一种基于双级扩展卡尔曼滤波的视觉惯性里程计在线时间校准方法，其特征在于，步骤s6通过多约束特征点的重投影误差对增广后的状态向量x进行更新，包括：

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

技术研发人员：李帅永,聂嘉炜
受保护的技术使用者：重庆邮电大学
技术研发日：
技术公布日：2024/1/14