CN115342829A - 基于迭代扩展卡尔曼滤波器的里程计算方法及装置 - Google Patents

基于迭代扩展卡尔曼滤波器的里程计算方法及装置 Download PDF

Info

Publication number
CN115342829A
CN115342829A CN202210863221.6A CN202210863221A CN115342829A CN 115342829 A CN115342829 A CN 115342829A CN 202210863221 A CN202210863221 A CN 202210863221A CN 115342829 A CN115342829 A CN 115342829A
Authority
CN
China
Prior art keywords
imu
odometer
visual
initialization
state information
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210863221.6A
Other languages
English (en)
Inventor
唐付林
卫浩
徐泽文
吴毅红
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Automation of Chinese Academy of Science
Original Assignee
Institute of Automation of Chinese Academy of Science
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Automation of Chinese Academy of Science filed Critical Institute of Automation of Chinese Academy of Science
Priority to CN202210863221.6A priority Critical patent/CN115342829A/zh
Publication of CN115342829A publication Critical patent/CN115342829A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Image Analysis (AREA)

Abstract

本申请提供一种基于迭代扩展卡尔曼滤波器的里程计算方法及装置，应用于视觉惯性里程计，视觉惯性里程计包括相机和IMU，该方法包括：对视觉惯性里程计进行动态初始化处理，得到视觉惯性里程计的初始化状态信息，并基于初始化状态信息建立初始空间三维地图；根据初始化状态信息、相机实时采集的图像数据和IMU实时采集的IMU数据进行状态传播，得到视觉惯性里程计的线性化模型；基于迭代扩展卡尔曼滤波器方法对线性化模型进行多次迭代更新处理，得到视觉惯性里程计的当前系统状态向量，当前系统状态向量用于指示视觉惯性里程计在初始空间三维地图中的位置和姿态。提高了视觉惯性里程计的精度。

Description

基于迭代扩展卡尔曼滤波器的里程计算方法及装置
技术领域
本申请涉及计算机视觉技术领域，尤其涉及一种基于迭代扩展卡尔曼滤波器的里程计算方法及装置。
背景技术
近年来，由于低廉的微机电系统(Micro-Electro-Mechanical System,MEMS)惯性传感器技术飞速发展，并且低廉轻量的空间定位技术在增强现实(Augmented Reality，AR)/虚拟现实(Virtual Reality，VR)、机器人、无人机等领域有着极为重要的应用，越来越多的人研究融合相机数据和惯性测量单元(Inertial Measurement Unit，IMU)数据估计设备的6自由度空间位姿。
基于滤波的方法为视觉惯性里程计中的一种常用的方法，其具有速度快和鲁棒性高的优点，适合在计算资源有限的嵌入式平台运行。但目前的基于滤波的方法的精度比较低，线性误差比较大。
发明内容
本申请提供一种基于迭代扩展卡尔曼滤波器的里程计算方法及装置，用以提高目前视觉惯性里程计的精度。
第一方面，本申请提供一种基于迭代扩展卡尔曼滤波器的里程计算方法，应用于视觉惯性里程计，所述视觉惯性里程计包括相机和IMU，所述方法包括：
对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；
根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；
基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
在一种可能的实施方式中，所述对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，包括：
根据视差阈值在所述多张初始图像中构建滑动窗口，并将所述滑动窗口之间的匹配特征点确定为所述三维特征点；
根据所述滑动窗口中的各所述初始图像对应的初始IMU数据，获取各所述初始图像对应的陀螺仪偏置、IMU的速度、重力方向和尺度因子；
根据所述陀螺仪偏置、所述IMU的速度和所述重力方向，获取所述初始化状态信息；
根据所述初始化状态信息和所述尺度因子建立初始空间三维地图。
在一种可能的实施方式中，所述根据所述陀螺仪偏置、所述IMU的速度和所述重力方向，获取所述初始化状态信息，包括：
对所述重力方向进行修正，得到修正后的重力方向；
根据所述修正后的重力方向与方向向量之间的旋转角度，得到全局坐标系与相机坐标系之间的旋转角度；
根据全局坐标系与相机坐标系之间的旋转角度，将各所述初始图像对应的所述陀螺仪偏置和所述IMU的速度转换到所述全局坐标系下；
根据所述全局坐标系下的陀螺仪偏置和IMU的速度，获取所述初始化状态信息。
在一种可能的实施方式中，所述根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型，包括：
根据所述IMU数据获取IMU测量模型；
根据所述IMU测量模型和所述图像数据进行状态向量增广处理，得到所述线性化模型。
在一种可能的实施方式中，所述基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，包括：
设置初始系统状态向量估计值；
执行第i次迭代操作，所述第i次迭代操作包括：在第i-1次迭代时的系统状态下计算第i次迭代的雅克比矩阵、第i次迭代的卡尔曼增益；根据所述线性化模型、所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，计算所述视觉惯性里程计在第i+1次迭代时的系统状态；i初始为1，i依次为1,2,...,N-1，所述N为迭代次数，所述N为正整数，第0次迭代时的系统状态为所述初始系统状态向量估计值；
将所述视觉惯性里程计在第N次迭代时的系统状态确定为所述视觉惯性里程计的当前系统状态向量。
在一种可能的实施方式中，所述方法还包括：
根据所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，获取所述当前系统状态向量的协方差矩阵。
第二方面，本申请提供一种基于迭代扩展卡尔曼滤波器的里程计算装置，应用于视觉惯性里程计，所述视觉惯性里程计包括相机和IMU，所述装置包括：
初始化模块，用于对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；
第一处理模块，用于根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；
第二处理模块，用于基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
在一种可能的实施方式中，所述初始化模块具体用于：
根据视差阈值在所述多张初始图像中构建滑动窗口，并将所述滑动窗口之间的匹配特征点确定为所述三维特征点；
根据所述滑动窗口中的各所述初始图像对应的初始IMU数据，获取各所述初始图像对应的陀螺仪偏置、IMU的速度、重力方向和尺度因子；
根据所述陀螺仪偏置、所述IMU的速度和重力方向，获取所述初始化状态信息；
根据所述初始化状态信息和所述尺度因子建立初始空间三维地图。
在一种可能的实施方式中，所述初始化模块具体用于：
对所述重力方向进行修正，得到修正后的重力方向；
根据所述修正后的重力方向与方向向量之间的旋转角度，得到全局坐标系与相机坐标系之间的旋转角度；
根据全局坐标系与相机坐标系之间的旋转角度，将各所述初始图像对应的所述陀螺仪偏置和所述IMU的速度转换到所述全局坐标系下；
根据所述全局坐标系下的陀螺仪偏置和所述IMU的速度，获取所述初始化状态信息。
在一种可能的实施方式中，所述第一处理模块具体用于：
根据所述IMU数据获取IMU测量模型；
根据所述IMU测量模型和所述图像数据进行状态向量增广处理，得到所述线性化模型。
在一种可能的实施方式中，所述第二处理模块具体用于：
设置初始系统状态向量估计值；
执行第i次迭代操作，所述第i次迭代操作包括：在第i-1次迭代时的系统状态下计算第i次迭代的雅克比矩阵、第i次迭代的卡尔曼增益；根据所述线性化模型、所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，计算所述视觉惯性里程计在第i+1次迭代时的系统状态；i初始为1，i依次为1,2,...,N-1，所述N为迭代次数，所述N为正整数，第0次迭代时的系统状态为所述初始系统状态向量估计值；
将所述视觉惯性里程计在第N次迭代时的系统状态确定为所述视觉惯性里程计的当前系统状态向量。
在一种可能的实施方式中，所述第二处理模块还用于：
根据所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，获取所述当前系统状态向量的协方差矩阵。
第三方面，本申请提供一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如第一方面任一项所述的基于迭代扩展卡尔曼滤波器的里程计算方法。
第四方面，本申请提供一种非暂态计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面任一项所述的基于迭代扩展卡尔曼滤波器的里程计算方法。
第五方面，本申请提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如第一方面任一项所述的基于迭代扩展卡尔曼滤波器的里程计算方法。
本申请提供的基于迭代扩展卡尔曼滤波器的里程计算方法及装置，应用于视觉惯性里程计，视觉惯性里程计包括相机和IMU，首先对视觉惯性里程计进行动态初始化处理，得到视觉惯性里程计的初始化状态信息，并基于初始化状态信息建立初始空间三维地图，初始化状态信息包括多张相机拍摄的初始图像对应的相机位姿以及各初始图像上的三维特征点的空间位置；然后根据初始化状态信息、相机实时采集的图像数据和IMU实时采集的IMU数据进行状态传播，得到视觉惯性里程计的线性化模型；最后基于迭代扩展卡尔曼滤波器方法对线性化模型进行多次迭代更新处理，得到视觉惯性里程计的当前系统状态向量，当前系统状态向量用于指示视觉惯性里程计在初始空间三维地图中的位置和姿态。在基于滤波的VIO系统中，大多数系统采用静止初始化，本申请的方案采用动态初始化，使得方案能应用在运动的过程中初始化基于滤波的VIO系统，对使用者较为友好，同时针对目前的扩展卡尔曼滤波器系统在更新步骤中只进行一次迭代导致存在较大的线性化误差的问题，本申请采用迭代扩展卡尔曼滤波器方法，即在更新步骤中进行多次迭代，从而降低了VIO系统的非线性误差。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的基于迭代扩展卡尔曼滤波器的里程计算方法的流程示意图；
图2为本申请实施例提供的基于迭代扩展卡尔曼滤波器的视觉惯性里程计处理示意图；
图3为本申请实施例提供的动态初始化的流程示意图；
图4为本申请实施例提供的动态初始化示意图；
图5为本申请实施例提供的迭代扩展卡尔曼滤波器处理的流程示意图；
图6为本申请实施例提供的相机采集图像数据示意图；
图7为本申请实施例提供的3楼简单场景序列轨迹示意图；
图8为本申请实施例提供的楼梯间困难场景序列轨迹示意图；
图9为本申请实施例提供的基于迭代扩展卡尔曼滤波器的里程计算装置的结构示意图；
图10为本申请实施例提供的一种电子设备的实体结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请中的附图，对本申请中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
近年来，由于低廉的MEMS惯性传感器技术飞速发展，并且低廉轻量的空间定位技术在AR/VR、机器人、无人机等领域有着极为重要的应用，越来越多的人研究融合相机数据和IMU数据估计设备的6自由度空间位姿。许多优秀的视觉惯性里程计已经被提出了，主要有两大类：基于优化的方法和基于滤波的方法。在实际系统中，基于滤波的方法可以达到与基于优化的方法精度相当。基于滤波的方法还具有速度快和鲁棒性高的优势，更加适合在计算资源有限的嵌入式平台运行。然而，基于滤波的视觉惯性里程计(Visual-InertialOdometry，VIO)系统往往采用静止初始化，对使用者不友好。此外，基于滤波的VIO系统的精度还可以进一步提高。因此，本申请着重利用迭代扩展卡尔曼滤波器的方法提高基于滤波的VIO系统的精度，并且采用运动初始化。
图1为本申请实施例提供的基于迭代扩展卡尔曼滤波器的里程计算方法的流程示意图，应用于视觉惯性里程计，该视觉惯性里程计包括相机和IMU，如图1所示，该方法可以包括：
S11，对视觉惯性里程计进行动态初始化处理，得到视觉惯性里程计的初始化状态信息，并基于初始化状态信息建立初始空间三维地图，初始化状态信息包括多张相机拍摄的初始图像对应的相机位姿以及各初始图像上的三维特征点的空间位置。
在基于滤波的VIO系统中，大多数系统采用静止初始化。为了方便使用者在运动的过程中初始化基于滤波的VIO系统，本申请实施例采用运动初始化，即动态初始化。动态初始化需要一定的运动激励，不需要假设系统处于静止状态，从而完成初始化过程。在动态初始化阶段，分别完成以下四项操作：将IMU局部坐标系和重力方向对齐，恢复单目视觉的尺度信息，计算初始位置和运动速度，计算加速度偏置和陀螺仪偏置。
在对视觉惯性里程计进行动态初始化处理后，可以得到视觉惯性里程计的初始化状态信息，初始化状态信息为视觉惯性里程计还未进行里程计算时的初始状态，其中包括多张相机拍摄的初始图像对应的相机位姿，以及各初始图像上的三维特征点的空间位置，其中，三维特征点为各初始图像上具有一定特征的点，便于进行追踪的点，例如可以是图像边角、建筑物的顶点等等。在得到初始化状态信息后，可以基于初始化状态信息建立初始空间三维地图，则拍摄得到的各个物体在初始空间三维地图中均有相应的位置和坐标。
S12，根据初始化状态信息、相机实时采集的图像数据和IMU实时采集的IMU数据进行状态传播，得到视觉惯性里程计的线性化模型。
对于状态传播，主要包含IMU预测系统状态和状态向量增广。IMU预测系统状态指利用IMU测量值预测以及其协方差矩阵，状态向量增广指在状态向量中加入图像的位姿并且更新其协方差矩阵。本申请实施例中，在得到初始化状态信息后，根据初始化状态信息、相机实时采集的图像数据以及IMU实时采集的IMU数据进行状态传播，即可得到视觉惯性里程计的线性化模型，该线性化模型包括IMU测量值以及其协方差矩阵，以及图像数据的位姿和其协方差矩阵。
S13，基于迭代扩展卡尔曼滤波器方法对线性化模型进行多次迭代更新处理，得到视觉惯性里程计的当前系统状态向量，当前系统状态向量用于指示视觉惯性里程计在初始空间三维地图中的位置和姿态。
传统的扩展卡尔曼滤波器系统主要分为预测和更新两个步骤，在更新步骤中只进行一次迭代。然而，只进行一次迭代仍然存在较大的线性化误差。为此，本申请实施例采用迭代扩展卡尔曼滤波器对线性化模型进行多次迭代更新处理，得到视觉惯性里程计的当前系统状态向量，由于当前系统状态向量指示了视觉惯性里程计在初始空间三维地图中的位置和姿态，因此根据当前系统状态向量即可确定视觉惯性里程计当前的位姿。针对任意时刻，均可以采用上述方案得到视觉惯性里程计当前的位姿，然后根据时间先后顺序连接起来，即可得到视觉惯性里程计的里程、位姿等等信息。
本申请提供的基于迭代扩展卡尔曼滤波器的里程计算方法，应用于视觉惯性里程计，视觉惯性里程计包括相机和IMU，首先对视觉惯性里程计进行动态初始化处理，得到视觉惯性里程计的初始化状态信息，并基于初始化状态信息建立初始空间三维地图，初始化状态信息包括多张相机拍摄的初始图像对应的相机位姿以及各初始图像上的三维特征点的空间位置；然后根据初始化状态信息、相机实时采集的图像数据和IMU实时采集的IMU数据进行状态传播，得到视觉惯性里程计的线性化模型；最后基于迭代扩展卡尔曼滤波器方法对线性化模型进行多次迭代更新处理，得到视觉惯性里程计的当前系统状态向量，当前系统状态向量用于指示视觉惯性里程计在初始空间三维地图中的位置和姿态。在基于滤波的VIO系统中，大多数系统采用静止初始化，本申请的方案采用动态初始化，使得方案能应用在运动的过程中初始化基于滤波的VIO系统，对使用者较为友好，同时针对目前的扩展卡尔曼滤波器系统在更新步骤中只进行一次迭代导致存在较大的线性化误差的问题，本申请采用迭代扩展卡尔曼滤波器方法，即在更新步骤中进行多次迭代，从而降低了VIO系统的非线性误差。
下面结合附图对本申请的方案进行详细介绍。
本申请实施例提出了基于动态初始化和迭代扩展卡尔曼滤波器的视觉惯性里程计技术。该技术提高了基于滤波的视觉惯性里程计的精度并且利用动态初始化增加了基于滤波的视觉惯性里程计的易用性。
本申请实施例主要包含三个部分，分别是动态初始化、状态传播和基于图像的状态更新。图2为本申请实施例提供的基于迭代扩展卡尔曼滤波器的视觉惯性里程计处理示意图，如图2所示，本发明中的基于迭代扩展卡尔曼滤波器的VIO主要包含5个部分：
A、图像处理，对于图像处理，主要包括特征提取与跟踪。在特征提取时，在图像上均匀提取FAST特征点(是FAST算法定义的特征点，FAST特征点是指的图像上某个像素点和其周围领域足够多的像素点处于不同区域，那么这个像素点就是FAST特征点)。在特征跟踪时，利用光流对特征进行跟踪并且利用对极几何和随机抽样一致(Random SampleConsensus，RANSAC)剔除跟踪的外点。
B、状态传播，对于状态传播，主要包含IMU预测系统状态和状态向量增广。
C、混合特征点，对于混合特征点，主要包含多状态约束下的卡尔曼滤波器(Multi-State Constraint Kalman Filter，MSCKF)点和同步定位和制图(SimultaneousLocalization and Mapping，SLAM)点。MSCKF点指跟踪长度小于m帧图像的特征点，其不被加入状态向量。SLAM点指跟踪长度大于或等于m帧图像的特征点，其被加入状态向量。
D、更新SLAM点。
E、更新MSCKF点。
对于更新SLAM点和MSCKF点，其分别对应于用SLAM点更新系统状态和用MSCKF点更新系统状态。更新SLAM点和MSCKF点后，得到状态向量和协方差矩阵，即可进行迭代扩展卡尔曼滤波更新。
首先结合图3对初始化过程进行介绍。
图3为本申请实施例提供的动态初始化的流程示意图，如图3所示，包括：
S31，根据视差阈值在多张初始图像中构建滑动窗口，并将滑动窗口之间的匹配特征点确定为三维特征点。
在VIO系统中，本申请实施例采用动态初始化。动态初始化需要一定的运动激励，不需要假设系统处于静止状态，从而完成初始化过程。在动态初始化阶段，分别完成以下四项操作：将IMU局部坐标系和重力方向对齐，恢复单目视觉的尺度信息，计算初始位置和运动速度，计算加速度偏置和陀螺仪偏置。
假设在一段时间内相机处于运动状态，相机数据进行视觉运动恢复结构(Structure from Motion，SFM)，同时IMU数据进行预积分。对齐视觉SFM信息和IMU预积分信息，从而完成动态初始化。
从输入的多张初始图像中按照一定视差阈值选择关键帧并构建滑动窗口，寻找当前关键帧与之前所有关键帧之间的匹配特征点，作为三维特征点。然后，根据匹配的特征点进行三角化，计算特征点的空间位置。为了保证关键帧位姿和三角化特征点的一致性，对滑动窗口中所有的关键帧位姿和特征点空间位置进行全局捆绑调整。
S32，根据滑动窗口中的各初始图像对应的初始IMU数据，获取各初始图像对应的陀螺仪偏置、IMU的速度、重力方向和尺度因子。
可以基于下式(1)进行陀螺仪偏置bg估计：
Figure BDA0003756460460000111
其中，c0表示视觉SFM的参考坐标系，Ik和Ik+1分别表示k时刻和k+1时刻的IMU坐标系，
Figure BDA0003756460460000112
为k时刻IMU坐标在视觉SFM的参考坐标系下的旋转矩阵，
Figure BDA0003756460460000113
为k+1时刻IMU坐标在视觉SFM的参考坐标系下的旋转矩阵，
Figure BDA0003756460460000114
表示四元素乘法，
Figure BDA0003756460460000115
表示预积分的旋转测量值，
Figure BDA0003756460460000116
Figure BDA0003756460460000117
为雅克比矩阵，B为k的取值集合。
然后，对滑动窗口中的每一帧初始图像对应的IMU帧的速度、重力方向和尺度因子进行估计，如下式(2)所示：
Figure BDA0003756460460000118
其中，
Figure BDA0003756460460000119
Figure BDA00037564604600001110
表示n时刻IMU的速度，
Figure BDA00037564604600001111
表示视觉SFM参考坐标系下的重力加速度，s表示尺度因子；
Figure BDA00037564604600001112
Figure BDA00037564604600001113
表示预积分位置测量值，
Figure BDA00037564604600001114
表示预积分速度测量值，且：
Figure BDA0003756460460000121
Figure BDA0003756460460000122
Figure BDA0003756460460000123
其中，
Figure BDA0003756460460000124
表示从视觉SFM的参考坐标系c0到k时刻的IMU坐标系Ik的旋转矩阵，
Figure BDA0003756460460000125
表示从k+1时刻的IMU坐标系Ik+1到视觉SFM的参考坐标系c0的平移向量，
Figure BDA0003756460460000126
表示从k时刻的IMU坐标系Ik到视觉SFM的参考坐标系c0的平移向量，
Figure BDA0003756460460000127
表示从k时刻的IMU坐标系Ik到视觉SFM的参考坐标系c0的旋转矩阵，Δtk表示时间差，
Figure BDA0003756460460000128
表示k时刻IMU的速度，
Figure BDA0003756460460000129
表示从k+1时刻的IMU坐标系Ik+1到视觉SFM的参考坐标系c0的旋转矩阵，
Figure BDA00037564604600001210
表示k+1时刻IMU的速度，
Figure BDA00037564604600001211
为视觉SFM的参考坐标系c0下的重力加速度，I为单位矩阵，。
S33，根据陀螺仪偏置、IMU的速度和重力方向，获取初始化状态信息。
具体的，首先对重力方向进行修正，一般来说，重力大小是已知。因此，根据这个先验，对g进行进一步的修正。得到修正后的重力方向。
然后，根据修正后的重力方向与方向向量之间的旋转角度，得到全局坐标系与相机坐标系之间的旋转角度，并根据全局坐标系与相机坐标系之间的旋转角度，将各初始图像对应的陀螺仪偏置和IMU的速度转换到全局坐标系下。得到重力加速度g之后，即可根据其与方向向量[0,0,1]之间的旋转，得到全局坐标系与第一帧相机坐标系之间旋转，从而可以将所有图像帧的旋转转换到全局坐标系下。
根据全局坐标系下的陀螺仪偏置和IMU的速度，获取初始化状态信息。根据得到的尺度因子s，对地图点坐标和平移进行缩放。为了进一步提高滑动窗口中图像帧对应状态的精度，利用IMU残差项和图像重投影误差项建立目标函数，并且进行捆绑调整优化，即可得到初始化状态信息。
S34，根据初始化状态信息和尺度因子建立初始空间三维地图。
最后，将动态初始化的结果与VIO系统进行无缝衔接。动态初始化计算得到滑动窗口中所有图像帧对应的相机位姿以及三维特征点。在动态初始化成功完成之后，利用动态初始化估计的相机位姿，对跟踪的三维特征点进行重新三角化，并将通过马氏距离检验和跟踪帧数判断的特征点加入到状态向量中。
图4为本申请实施例提供的动态初始化示意图，如图4所示，通过将视觉SFM信息与IMU预积分信息对齐，获得VIO系统初始化的状态。将获得的VIO系统初始化的状态给到基于迭代扩展卡尔曼滤波器的视觉惯性里程计，基于迭代扩展卡尔曼滤波器的视觉惯性里程计就可以成功地启动了。
初始化的SLAM特征点加入到状态向量中的好处：首先，SLAM点为长期观测的特征，这些三维特征点可以认为是稳定的特征，加入到状态向量中不断估计系统状态，系统状态逐渐精确，系统精度越高。其次，SLAM点的存在能够部分解决纯旋转或者静止等三维特征点无法三角化的情况，SLAM点在这种情况下仍然能够对系统形成一个约束。因此，加入SLAM特征之后能够显著提高系统的鲁棒性，减少初始化成功后系统轨迹的抖动。
在动态初始化后，即可进行状态传播。具体的，首先根据IMU数据获取IMU测量模型，然后根据IMU测量模型和图像数据进行状态向量增广处理，得到线性化模型。
对于状态传播，主要包含IMU预测系统状态和状态向量增广。IMU预测系统状态指利用IMU测量值预测XI以及其协方差矩阵，状态向量增广指在状态向量中加入图像的位姿Xc并且更新协方差矩阵。
IMU的测量模型如下：
ωm(t)＝Iω(t)+bg(t)+nr(t) (3)
Figure BDA0003756460460000141
其中，ωm(t)和am(t)分别是IMU陀螺仪和加速度计的测量值，Iω(t)为IMU角速度，bg(t)为IMU角速度的偏置，nr(t)为IMU角速度的噪声。
Figure BDA0003756460460000142
表示从全局坐标系到IMU坐标系的旋转矩阵，Ga(t)为IMU在全局坐标系下的加速度，Gg为全局坐标系下的重力加速度，ba(t)为IMU加速度的偏置，na(t)为IMU加速度的噪声。
因此，根据上述IMU测量模型，假设时间从tk到tk+1，IMU的预测方程如下：
Figure BDA0003756460460000143
其中，
Figure BDA0003756460460000144
表示从全局坐标系到k+1时刻的IMU坐标系Ik+1的旋转四元数，
Figure BDA0003756460460000145
表示从全局坐标系到k时刻的IMU坐标系Ik的旋转四元数，
Figure BDA0003756460460000146
Ω表示反对称矩阵运算符号，ωm(tk)表示k时刻陀螺仪的测量值，
Figure BDA0003756460460000147
表示k时刻IMU角速度的偏置。
Figure BDA0003756460460000148
Figure BDA0003756460460000149
其中，
Figure BDA00037564604600001410
表示k+1时刻IMU在全局坐标系G下的速度，
Figure BDA00037564604600001411
表示k时刻IMU在全局坐标系G下的速度，
Figure BDA00037564604600001412
为IMU坐标系到全局坐标系的旋转矩阵，Gg为全局坐标系下的重力加速度，
Figure BDA00037564604600001413
为k+1时刻IMU在全局坐标系下的位置，
Figure BDA00037564604600001414
为k时刻IMU在全局坐标系下的位置，Δt＝tk+1-tk，并且
Figure BDA00037564604600001415
和
Figure BDA00037564604600001416
的计算公式如下：
Figure BDA00037564604600001417
Figure BDA0003756460460000151
其中，
Figure BDA0003756460460000152
坐标系到k时刻的IMU坐标系的旋转矩阵，am(tk)为k时刻的加速度计的测量值，
Figure BDA0003756460460000153
为k时刻IMU加速度的偏置。进一步，根据这些方程计算误差状态转移方程，得到线性化的模型如下式(8)所示：
Figure BDA0003756460460000154
Figure BDA0003756460460000155
为k+1时刻的误差状态向量，Φ(tk+1,tk)表示从k时刻到k+1时刻的状态转移矩阵，
Figure BDA0003756460460000156
为k时刻的误差状态向量，Gk为IMU噪声的雅克比矩阵。根据这个线性化的模型，协方差矩阵P的更新如下：
P(tk+1)＝Φ(tk+1,tk)P(tk)Φ(tk+1,tk)T+Qd (9)
其中，P(tk+1)为k+1时刻的协方差矩阵，P(tk)为k时刻的协方差矩阵，Qd表示噪声的协方差矩阵。
当收到新的一帧图像时，该时刻图像的位姿应当加入到状态向量中，即状态向量增广。在状态向量增广的过程中，既要更新状态向量，也要更新协方差矩阵。
在VIO系统中，本申请实施例基于图像的状态更新包含MSCKF点更新和SLAM点更新。MSCKF点指短期特征信息，不被加入到状态向量中。SLAM点指长期特征信息，被加入到状态向量中。二者的更新过程是相似的，建立基于图像的重投影误差观测方程，利用链式法则计算雅可比矩阵，然后将其线性化，最后利用迭代扩展卡尔曼滤波器进行更新。
在构建线性化观测方程之前，首先利用链式法则计算雅可比矩阵。对于某一特征，假设该特征在世界坐标系的空间点为Gpf，在相机坐标系的空间点为Cpf，归一化平面的坐标为zn，像素坐标系坐标为zm。根据相机成像模型可得：
Figure BDA0003756460460000161
其中，xm和ym为图像像素坐标测量值(即分别为横坐标和纵坐标)，fx和fy为相机焦距，cx和cy分别为相机主点的横纵坐标。相机坐标系下的3D位置Cpf到归一化平面为zn：
Figure BDA0003756460460000162
Figure BDA0003756460460000163
和
Figure BDA0003756460460000164
分别为相机坐标系下的空间点的横纵坐标。世界坐标系空间点到相机坐标系空间点的变换：
Figure BDA0003756460460000165
Figure BDA0003756460460000166
为相机与IMU之间的旋转矩阵，
Figure BDA0003756460460000167
为全局坐标系到IMU坐标系的旋转矩阵，Gpf为IMU在相机坐标系下的位置，GpI为IMU在全局坐标系下的位置。
计算对相机位姿的雅可比矩阵[Hx]2×6为：
Figure BDA0003756460460000168
其中，
Figure BDA0003756460460000169
计算对特征点位置的雅可比矩阵[Hf]2×3为：
Figure BDA00037564604600001610
因此，对于SLAM点的更新，其线性化的观测方程可以写成：
Figure BDA00037564604600001611
Figure BDA00037564604600001612
为误差状态向量，Hf为残差对空间点的雅可比矩阵，
Figure BDA00037564604600001613
为空间点的误差量。SLAM点被加入到状态向量中，则其线性化的观测方程又可以写成：
Figure BDA00037564604600001614
Figure BDA0003756460460000171
为整个误差状态向量。假设所有特征的观测帧数总和为N，则残差r的维数为2N×1，H的总维数为2N×M，M表示系统状态向量的总维数，
Figure BDA0003756460460000172
的总维数为2N×6C，C表示状态向量中相机位姿的个数(滑动窗口的大小)，Hf的总维数为2N×3P，P表示状态向量中SLAM点的数量，
Figure BDA0003756460460000173
表示当前IMU误差状态量，
Figure BDA0003756460460000174
表示滑动窗口中所有相机位姿的误差状态量，
Figure BDA0003756460460000175
表示所有SLAM点的误差状态量，n表示噪声。
Figure BDA0003756460460000176
是所有特征观测对所有相机姿态的雅可比矩阵(拼起来的矩阵)。对于MSCKF点的更新，由于其不被加入到状态向量中，所以其线性化的观测方程可以写成：
Figure BDA0003756460460000177
得到线性化的观测方程之后，利用迭代扩展卡尔曼滤波器更新系统状态向量。传统的扩展卡尔曼滤波器系统主要分为预测和更新两个步骤，在更新步骤中只进行一次迭代。然而，只进行一次迭代仍然存在较大的线性化误差。为此，本发明考虑引入迭代扩展卡尔曼滤波器，即在更新步骤中进行多次迭代，降低系统的非线性误差。下面结合图5对迭代扩展卡尔曼滤波器的具体步骤进行介绍。
图5为本申请实施例提供的迭代扩展卡尔曼滤波器处理的流程示意图，如图5所示，包括：
S51，设置初始系统状态向量估计值。
首先设置迭代次数i初始为1，设置初始系统状态向量估计值为
Figure BDA0003756460460000178
S52，在当前状态计算雅克比矩阵，卡尔曼增益和下一步迭代时的系统状态。
针对第i次迭代，执行第i次迭代操作，第i次迭代操作包括：计算第i次迭代的雅克比矩阵、第i次迭代的卡尔曼增益；根据线性化模型、第i次迭代的雅克比矩阵和第i次迭代的卡尔曼增益，计算视觉惯性里程计在第i+1次迭代时的系统状态，i初始为1，i依次为1,2,...,N-1，N为迭代次数，N为正整数。其中：
Figure BDA0003756460460000181
hk(s)为残差函数，
Figure BDA0003756460460000182
为第i次迭代时k时刻的状态向量。
Figure BDA0003756460460000183
Pk|k-1为协方差矩阵，
Figure BDA0003756460460000184
为噪声协方差矩阵。
Figure BDA0003756460460000185
S53，判断i是否等于N-1，若是，则执行S55，若否，则执行S54。
S54，更新i为i+1，并执行S52。
在未达到迭代停止条件时，重复执行S52。
S55，将视觉惯性里程计在第N次迭代时的系统状态确定为视觉惯性里程计的当前系统状态向量。
保存最后迭代结果，更新系统的状态和对应的协方差矩阵。
Figure BDA0003756460460000186
Figure BDA0003756460460000187
其中
Figure BDA0003756460460000188
为当前系统状态向量，Pk|k为
Figure BDA0003756460460000189
对应的协方差矩阵，I为单位矩阵，
Figure BDA00037564604600001810
为卡尔曼增益。
图6为本申请实施例提供的相机采集图像数据示意图，图7为本申请实施例提供的3楼简单场景序列轨迹示意图，图8为本申请实施例提供的楼梯间困难场景序列轨迹示意图，其中图7和图8的序列轨迹是基于图6提供的图像数据得到的，图7和图8中分别标示了起点和终点，起点和终点越靠近表示轨迹的精度越高。
表1为本方案与VINS-Mono在EuRoC数据集上位姿精度的对比，数字越小表示位姿精度越高。
表1
Figure BDA00037564604600001811
Figure BDA0003756460460000191
表2为本方案VINS-Mono在EuRoC数据集上执行时间的对比，数字越小表示速度越快。如表2所示，本方案相比于VINS-Mono算法相比，在各序列上的执行时间均明显较低，表示本方案的执行速度较快，时间开销较小。
表2
序列/算法 VINS-Mono(毫秒) 本方案(毫秒)
MH_01_简单 47.6 19.3
MH_02_简单 49.9 19.9
MH_03_中等 49.7 18.0
MH_04_困难 50.7 17.3
MH_05_困难 51.4 20.3
V1_01_简单 49.0 21.7
V1_02_中等 49.9 16.9
V1_03_困难 46.2 15.1
V2_01_简单 52.5 22.0
V2_02_中等 49.9 17.0
V2_03_困难 46.1 14.9
平均 49.4 18.4
表3为本方案与VINS-Mono在图6提供的图像数据上位姿精度的对比，数字越小表示位姿精度越高。如表3所示，相比于VINS-Mono算法，本方案的位姿精度明显较高，其定位效果较好。
表3
Figure BDA0003756460460000201
表4为本方案与VINS-Mono在图8提供的图像数据上执行时间的对比，数字越小表示速度越快。如表4所示，本方案相比于VINS-Mono算法相比，在各序列上的执行时间均明显较低，表示本方案的执行速度较快，时间开销较小。
表4
Figure BDA0003756460460000202
如图6至图8、以及表1至表4所示，本方案在业界公认的EuRoC数据集上进行了详细测试，速度可达到50～60每秒传输帧数(Frames Per Second，FPS)，定位精度在多个不同的场景下均达到误差在0.1m和2.0度以下，速度和精度均显著优于现有的最优的方法VINS-Mono。更进一步地，本申请实施例通过图6示例的更具挑战性的实际场景下采集了新的数据，验证本方案在较为极端的困难场景(弱纹理、重复结构、反光等)下的表现。在该数据集上，本方案平均定位精度能达到0.43m，相比VINS-Mono的1.64m，误差降低了75％，同时速度也保持在了50FPS以上。实验结果证明了本方案的有效性，以及其在速度和精度上的显著优势。
综上所述，本申请提供的基于迭代扩展卡尔曼滤波器的里程计算方法，采用动态初始化，使得方案能应用在运动的过程中初始化基于滤波的VIO系统，对使用者较为友好，同时针对目前的扩展卡尔曼滤波器系统在更新步骤中只进行一次迭代导致存在较大的线性化误差的问题，本申请采用迭代扩展卡尔曼滤波器方法，即在更新步骤中进行多次迭代，从而降低了VIO系统的非线性误差。
图9为本申请实施例提供的基于迭代扩展卡尔曼滤波器的里程计算装置的结构示意图，该装置应用于视觉惯性里程计，该视觉惯性里程计包括相机和IMU，如图9所示，该装置包括：
初始化模块91，用于对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；
第一处理模块92，用于根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；
第二处理模块93，用于基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
在一种可能的实施方式中，所述初始化模块91具体用于：
根据视差阈值在所述多张初始图像中构建滑动窗口，并将所述滑动窗口之间的匹配特征点确定为所述三维特征点；
根据所述滑动窗口中的各所述初始图像对应的初始IMU数据，获取各所述初始图像对应的陀螺仪偏置、IMU的速度、重力方向和尺度因子；
根据所述陀螺仪偏置、所述IMU的速度和重力方向，获取所述初始化状态信息；
根据所述初始化状态信息和所述尺度因子建立初始空间三维地图。
在一种可能的实施方式中，所述初始化模块91具体用于：
对所述重力方向进行修正，得到修正后的重力方向；
根据所述修正后的重力方向与方向向量之间的旋转角度，得到全局坐标系与相机坐标系之间的旋转角度；
根据全局坐标系与相机坐标系之间的旋转角度，将各所述初始图像对应的所述陀螺仪偏置和所述IMU的速度转换到所述全局坐标系下；
根据所述全局坐标系下的陀螺仪偏置和所述IMU的速度，获取所述初始化状态信息。
在一种可能的实施方式中，所述第一处理模块92具体用于：
根据所述IMU数据获取IMU测量模型；
根据所述IMU测量模型和所述图像数据进行状态向量增广处理，得到所述线性化模型。
在一种可能的实施方式中，所述第二处理模块93具体用于：
设置初始系统状态向量估计值；
执行第i次迭代操作，所述第i次迭代操作包括：在第i-1次迭代时的系统状态下计算第i次迭代的雅克比矩阵、第i次迭代的卡尔曼增益；根据所述线性化模型、所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，计算所述视觉惯性里程计在第i+1次迭代时的系统状态；i初始为1，i依次为1,2,...,N-1，所述N为迭代次数，所述N为正整数，第0次迭代时的系统状态为所述初始系统状态向量估计值；
将所述视觉惯性里程计在第N次迭代时的系统状态确定为所述视觉惯性里程计的当前系统状态向量。
在一种可能的实施方式中，所述第二处理模块93还用于：
根据所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，获取所述当前系统状态向量的协方差矩阵。
本申请实施例提供的基于迭代扩展卡尔曼滤波器的里程计算装置，用于执行上述方法实施例，其实现原理和技术效果类似，本实施例此处不再赘述。
图10示例了一种电子设备的实体结构示意图，如图10所示，该电子设备可以包括：处理器(processor)1010、通信接口(Communications Interface)1020、存储器(memory)1030和通信总线1040，其中，处理器1010，通信接口1020，存储器1030通过通信总线1040完成相互间的通信。处理器1010可以调用存储器1030中的逻辑指令，以执行基于迭代扩展卡尔曼滤波器的里程计算方法，该方法包括：对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
此外，上述的存储器1030中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的基于迭代扩展卡尔曼滤波器的里程计算方法，该方法包括：对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的基于迭代扩展卡尔曼滤波器的里程计算方法，该方法包括：对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于迭代扩展卡尔曼滤波器的里程计算方法，其特征在于，应用于视觉惯性里程计，所述视觉惯性里程计包括相机和惯性测量单元IMU，所述方法包括：
对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；
根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；
基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
2.根据权利要求1所述的方法，其特征在于，所述对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，包括：
根据视差阈值在所述多张初始图像中构建滑动窗口，并将所述滑动窗口之间的匹配特征点确定为所述三维特征点；
根据所述滑动窗口中的各所述初始图像对应的初始IMU数据，获取各所述初始图像对应的陀螺仪偏置、IMU的速度、重力方向和尺度因子；
根据所述陀螺仪偏置、所述IMU的速度和所述重力方向，获取所述初始化状态信息；
根据所述初始化状态信息和所述尺度因子建立初始空间三维地图。
3.根据权利要求2所述的方法，其特征在于，所述根据所述陀螺仪偏置、所述IMU的速度和所述重力方向，获取所述初始化状态信息，包括：
对所述重力方向进行修正，得到修正后的重力方向；
根据所述修正后的重力方向与方向向量之间的旋转角度，得到全局坐标系与相机坐标系之间的旋转角度；
根据全局坐标系与相机坐标系之间的旋转角度，将各所述初始图像对应的所述陀螺仪偏置和所述IMU的速度转换到所述全局坐标系下；
根据所述全局坐标系下的陀螺仪偏置和IMU的速度，获取所述初始化状态信息。
4.根据权利要求1-3任一项所述的方法，其特征在于，所述根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型，包括：
根据所述IMU数据获取IMU测量模型；
根据所述IMU测量模型和所述图像数据进行状态向量增广处理，得到所述线性化模型。
5.根据权利要求4所述的方法，其特征在于，所述基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，包括：
设置初始系统状态向量估计值；
执行第i次迭代操作，所述第i次迭代操作包括：在第i-1次迭代时的系统状态下计算第i次迭代的雅克比矩阵、第i次迭代的卡尔曼增益；根据所述线性化模型、所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，计算所述视觉惯性里程计在第i+1次迭代时的系统状态；i初始为1，i依次为1,2,...,N-1，所述N为迭代次数，所述N为正整数，第0次迭代时的系统状态为所述初始系统状态向量估计值；
将所述视觉惯性里程计在第N次迭代时的系统状态确定为所述视觉惯性里程计的当前系统状态向量。
6.根据权利要求5所述的方法，其特征在于，所述方法还包括：
根据所述第i次迭代的雅克比矩阵和所述第i次迭代的卡尔曼增益，获取所述当前系统状态向量的协方差矩阵。
7.一种基于迭代扩展卡尔曼滤波器的里程计算装置，其特征在于，应用于视觉惯性里程计，所述视觉惯性里程计包括相机和IMU，所述装置包括：
初始化模块，用于对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；
第一处理模块，用于根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；
第二处理模块，用于基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态。
8.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至6任一项所述的基于迭代扩展卡尔曼滤波器的里程计算方法。
9.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述的基于迭代扩展卡尔曼滤波器的里程计算方法。
10.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述的基于迭代扩展卡尔曼滤波器的里程计算方法。
CN202210863221.6A 2022-07-20 2022-07-20 基于迭代扩展卡尔曼滤波器的里程计算方法及装置 Pending CN115342829A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210863221.6A CN115342829A (zh) 2022-07-20 2022-07-20 基于迭代扩展卡尔曼滤波器的里程计算方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210863221.6A CN115342829A (zh) 2022-07-20 2022-07-20 基于迭代扩展卡尔曼滤波器的里程计算方法及装置

Publications (1)

Publication Number Publication Date
CN115342829A true CN115342829A (zh) 2022-11-15

Family

ID=83949647

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210863221.6A Pending CN115342829A (zh) 2022-07-20 2022-07-20 基于迭代扩展卡尔曼滤波器的里程计算方法及装置

Country Status (1)

Country Link
CN (1) CN115342829A (zh)

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN108648215B (zh) 基于imu的slam运动模糊位姿跟踪算法
CN111882607B (zh) 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN114018274B (zh) 车辆定位方法、装置及电子设备
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
US20220051031A1 (en) Moving object tracking method and apparatus
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN110660098A (zh) 基于单目视觉的定位方法和装置
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与系统
CN104848861A (zh) 一种基于图像消失点识别技术的移动设备姿态测量方法
CN111665512A (zh) 基于3d激光雷达和惯性测量单元的融合的测距和绘图
CN114022556A (zh) 定位初始化方法、装置和计算机可读存储介质
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN114812601A (zh) 视觉惯性里程计的状态估计方法、装置、电子设备
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN115342829A (zh) 基于迭代扩展卡尔曼滤波器的里程计算方法及装置
CN112037261A (zh) 一种图像动态特征去除方法及装置
CN116448105B (zh) 位姿更新方法、装置、电子设备和存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination