CN110702107A - 一种单目视觉惯性组合的定位导航方法 - Google Patents

一种单目视觉惯性组合的定位导航方法 Download PDF

Info

Publication number
CN110702107A
CN110702107A CN201911003161.5A CN201911003161A CN110702107A CN 110702107 A CN110702107 A CN 110702107A CN 201911003161 A CN201911003161 A CN 201911003161A CN 110702107 A CN110702107 A CN 110702107A
Authority
CN
China
Prior art keywords
imu
visual
coordinate system
pose
state vector
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
CN201911003161.5A
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.)
BEIJING VEKETEC TECHNOLOGY Co Ltd
Original Assignee
BEIJING VEKETEC TECHNOLOGY Co Ltd
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 BEIJING VEKETEC TECHNOLOGY Co Ltd filed Critical BEIJING VEKETEC TECHNOLOGY Co Ltd
Priority to CN201911003161.5A priority Critical patent/CN110702107A/zh
Publication of CN110702107A publication Critical patent/CN110702107A/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种单目视觉惯性组合的定位导航方法，包括：采集视频流和IMU数据流，并进行打包对齐；进行初始化，其包括视觉初始化、IMU初始化、确定视觉坐标系与IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI‑EKF对尺度进行求精估计；获取IMU数据流中的惯导数据，通过用互补滤波结合预积分的方法得到IMU位姿，同时跟踪视频流中的图像特征并参考IMU位姿变化量得到视觉位姿；判断视觉跟踪是否丢失，若丢失，则用IMU位姿进行运动跟踪，若未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终的相机位姿。本发明针对现有技术的不足，提高了尺度的精度，达到了在定位过程中保持高精度，高鲁棒性的效果。

Description

一种单目视觉惯性组合的定位导航方法
技术领域
本发明涉及定位导航技术领域，特别是涉及一种单目视觉惯性组合的定位导航方法。
背景技术
近年来，随着自动驾驶汽车、增强现实、虚拟现实、机器人和无人机导航等新兴技术的发展，组合导航逐步成为定位导航领域的热门研究方向。在不同的导航传感器组合模式中，单目视觉惯性组合提供了一种廉价且极具潜力的解决方案。单目相机价格低，功耗小，且能提供丰富的环境信息，但是纯单目视觉导航方法存在尺度不确定，易受环境条件影响的缺点；惯性传感器提供自运动信息，不易受遮挡和光照的影响，但是纯惯性导航方法受其本身的漂移和积累误差的影响，只在初始化时刻附近有较好的精度，随着时间的增加，其精度会逐渐发散。通过将单目相机和惯性传感器组合起来可以使其对各自局限性进行相互补偿。
发明内容
本发明针对现有技术的不足，本发明在初始化中采用非线性优化和λI-EKF(Lamdba Iterated Extended Kalman Filter)算法对尺度进行求精估计，并采用IDSF(Double Sensor Iterated Extended Kalman Filter)算法实现了单目相机与惯性传感器的融合，提供了一种尺度精度更高，在定位过程中保持更高精度、更高鲁棒性的一种单目视觉惯性组合的定位导航方法。
本发明解决其技术问题所采用的技术方案是：
步骤一、通过单目相机采集视频流；通过IMU采集IMU数据流；
步骤二、将采集的视频流和IMU数据流进行对齐，再将对齐后的视频流和IMU数据流打包在一起；
步骤三、系统数据初始化；若初始化成功，则顺序执行步骤四和步骤五；若初始化失败，则重新进行初始化；初始化过程包括视觉初始化、IMU初始化、确定视觉坐标系和IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI-EKF对尺度进行求精估计；
步骤四、获取IMU数据流中的惯导数据，对惯导数据进行互补滤波得到姿态变化量，再结合预积分计算得到IMU位姿，并将IMU位姿变化量给步骤五作为计算当前视觉帧位姿的参考变化量；
步骤五、逐帧提取视频流中的图像特征，利用提取的图像特征进行视觉跟踪和匹配，得到视觉位姿；
步骤六、判断步骤五中的视觉跟踪是否丢失，若视觉跟踪丢失并且不能重定位，则用IMU位姿进行运动定位；若视觉跟踪未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终用于定位导航的相机位姿。
所述步骤二中的对齐包括时间戳同步；打包包括数据打包。
所述步骤三中初始化的步骤具体包括：
3.1)视觉初始化：选取特征点数目大于设定值的连续图像帧进行匹配，当前后帧匹配点对大于设定值时，进行视觉初始化；如果匹配点对小于设定值时，重新开始选择图像帧；如果可以进行视觉初始化，运用RANSAC 8点法同时计算单应矩阵和基础矩阵，择优选择计算得到相机的位姿，之后用视觉里程计持续计算相机位姿，同时对相机位姿计算得到视觉位姿；
3.2)输出视觉初始化得到的视觉位姿；
3.3)IMU初始化：配置相机与IMU外参，此时IMU外参为标定后的初始值，
Figure BDA0002241937180000031
表示相机与IMU之间的平移，
Figure BDA0002241937180000032
表示相机与IMU之间的旋转四元数；配置初始重力向量g，计算IMU位姿；
3.4)输出IMU初始化得到的IMU位姿以及重力向量g；
3.5)配置步骤3.3)中初始重力向量：通过视觉初始化得到某一静止图像帧的相机位姿，将与此静止图像帧对应的IMU帧的加速度计读数作为重力向量g；
3.6)确定视觉坐标系与IMU的世界坐标系之间转换关系：静止图像帧的位置即为系统的视觉坐标系的原点，将该静止图像帧的位置与姿态作为视觉坐标系，该静止图像帧也是此系统的视觉第一帧，用vicon表示视觉坐标系，将与此静止图像帧对应的IMU帧的位置与姿态作为IMU的世界坐标系，同时该IMU帧也是此系统的IMU第一帧，用world表示IMU的世界坐标系，此时，相机与IMU外参为视觉坐标系与IMU的世界坐标系的转换关系，用
Figure BDA0002241937180000033
表示视觉坐标系与IMU的世界坐标系之间的平移，用
Figure BDA0002241937180000034
表示视觉坐标系与IMU的世界坐标系之间的旋转四元数，其中转换关系中涉及到的尺度λ为初始配置值；
3.7)非线性优化确定尺度初始值：根据相同时间内相机的运动与IMU的运动实际是一致的，利用视觉坐标系与IMU的世界坐标系之间转换关系、IMU运动模型以及已知的重力向量，得到IMU位姿和视觉位姿之间的转换关系，从而通过非线性优化求解最小二乘解，可得到尺度的初始估计；
3.8)用λI-EKF对尺度进行求精估计：通过λI-EKF(Lamdba Iterated ExtendedKalman Filter)算法进一步精确尺度λ的值，并更新状态向量X,
Figure BDA0002241937180000035
Figure BDA0002241937180000036
其中bw、ba分别为IMU中陀螺仪的零漂、IMU中加速度计的零漂。
其中，所述步骤3.1)中计算视觉位姿方法具体包括：按照步骤3.6)确定的视觉坐标系，将相机位姿转换为此视觉坐标系下的视觉位姿，用
Figure BDA0002241937180000041
表示在视觉坐标系下的相机平移，用
Figure BDA0002241937180000042
表示在视觉坐标系下的相机旋转四元数。
其中，所述步骤3.3)中计算IMU位姿方法具体包括：按照上述步骤3.6)中所述确定视觉坐标系与IMU的世界坐标系之间转换关系后，通过对多个加速度积分得到速度，对多个速度积分得到位置，对惯导数据进行互补滤波得到姿态变化量，综合以上得到IMU位姿，
Figure BDA0002241937180000043
表示在IMU的世界坐标系下的IMU平移，
Figure BDA0002241937180000044
表示在IMU的世界坐标系下的IMU旋转四元数，
Figure BDA0002241937180000045
表示在IMU的世界坐标系下的IMU速度。
其中，所述步骤3.8)具体包括：
4.1)输入IMU初始化得到的IMU位姿以及重力向量g，输入步骤3.7)得到的尺度λ的初始值；
4.2)构建状态向量X，为方便起见，令
Figure BDA0002241937180000046
Figure BDA0002241937180000047
因此状态向量
Figure BDA0002241937180000048
Figure BDA0002241937180000049
4.3)由状态向量X以及IMU的运动模型、噪声模型，可以得出：状态向量的误差为此
Figure BDA00022419371800000411
表示状态向量的估计值，误差向量
Figure BDA00022419371800000412
为25维系统状态误差项，由扩展卡尔曼滤波的预测过程，可得误差状态的转移矩阵：
Figure BDA0002241937180000051
其中：
Figure BDA0002241937180000052
Figure BDA0002241937180000053
Figure BDA0002241937180000054
E＝-A
Figure BDA0002241937180000058
Figure BDA0002241937180000059
Figure BDA00022419371800000514
表示角速度ω的反对称矩阵，am表示IMU的加速度测量值，wm表示IMU的角速度测量值，
Figure BDA00022419371800000510
表示加速度计零漂的估计值，
Figure BDA00022419371800000511
表示陀螺仪零漂的估计值，
Figure BDA00022419371800000512
表示加速度的估计值，
Figure BDA00022419371800000513
表示角速度的估计值，Δt表示两个状态向量的时间间隔，
Figure BDA0002241937180000061
为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，由上述推导出的矩阵，根据扩展卡尔曼滤波可以得到预测误差状态及其协方差矩阵，至此，对状态向量进行了预测，并得到相应预测误差协方差矩阵；
4.4)输入视觉初始化的得到的视觉位姿；
4.5)通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，可得观测误差模型，对观测误差模型进行离散处理，可得观测矩阵Hv如下：
Figure BDA0002241937180000062
Figure BDA0002241937180000063
为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，通过上述推导，根据扩展卡尔曼滤波，可进行系统状态向量的更新，通过卡尔曼增益，可得状态误差项，从而可得更新后的状态向量以及状态误差协方差矩阵；
4.6)判断更新前后状态向量X中尺度λ的变化量是否小于阈值μ，若是，则输出更新后的尺度λ，并将更新后的状态向量输出，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用预测得到的状态向量，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续对尺度λ变化量进行判断，查看是否可以输出所得量。
所述步骤四中惯导数据为：三轴加速度和三轴角速度。预积分指的是把关键帧之间的IMU测量值积分成相对运动的约束。
所述步骤四中对惯导数据进行互补滤波得到姿态变化量的步骤具体包括：
6.1)对加速度进行归一化得ab；
6.2)将标准重力加速度gn＝[0 0 1]T转换为IMU坐标系下得gb；
6.3)对gb和ab做向量叉乘得到角速度的校正补偿值e；
6.4)利用PI控制的方法，将校正补偿值e补偿到滤波后的角速度上，得到校正后的角速度；
6.5)将校正后的角速度通过四元数微分方程转换为四元数，并将得到的四元数归一化后得到姿态矩阵，计算出姿态变化量。
所述步骤五中的图像特征为：图像的局部描述符。
所述步骤六中通过IDSF(Double Sensor Iterated Extended Kalman Filter)的方法对视觉位姿和IMU位姿进行融合的步骤具体包括：
8.1)输入多个IMU位姿；
8.2)构建状态向量X，状态向量
Figure BDA0002241937180000071
8.3)根据扩展卡尔曼滤波，可以由状态向量X以及IMU的运动模型、噪声模型计算得到预测误差状态及其协方差矩阵，增加系统噪声协方差矩阵，补偿状态预测中的误差，同时对预测误差状态协方差矩阵进行加权扩大，至此，对系统状态向量进行了预测，并得到相应调整过的预测误差协方差矩阵；
8.4)输入视觉位姿；
8.5)通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，根据扩展卡尔曼滤波，进行系统状态向量的更新；
8.6)判断更新前后状态误差协方差矩阵的迹的变化量是否小于阈值δ，若是，则输出更新后的状态向量，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用调整过的预测误差协方差矩阵，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续判断状态误差协方差矩阵的迹的变化量是否收敛至设定阈值δ以下，从而查看是否可以输出所得量。
本发明与现有技术相比，具有如下优点和有益效果：
本发明在初始化中使用了非线性优化和λI-EKF来对系统的尺度进行精确估计，相比于现有的单目视觉惯性组合系统，本发明的尺度精确度更高；同时用IDSF对单目相机和惯性传感器进行了数据融合，使得本系统达到了在定位过程中保持更高精度、更高鲁棒性的效果。
附图说明
图1是一种单目视觉惯性组合的定位导航方法的流程图；
图2是一种单目视觉惯性组合的定位导航方法中初始化的流程图；
图3是一种单目视觉惯性组合的定位导航方法中λI-EKF的流程图；
图4是一种单目视觉惯性组合的定位导航方法中IDSF的流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明一具体实施方式，如图1-4所示，本发明提供了一种单目视觉惯性组合的定位导航方法，包括步骤一至步骤六。
步骤一、通过单目相机采集视频流；通过IMU采集IMU数据流。
步骤二、将采集的视频流和IMU数据流进行对齐，再将对齐后的视频流和IMU数据流打包在一起。
其中，所述打包对齐包括时间戳同步和数据打包。
步骤三、系统数据初始化，若初始化成功，则顺序执行步骤四和步骤五；若初始化失败，则重新进行初始化。初始化过程包括视觉初始化、IMU初始化、确定视觉坐标系和IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI-EKF对尺度进行求精估计。
所述步骤三具体包括：
3.1)视觉初始化：选取特征点数目大于设定值的连续图像帧进行匹配，当前后帧匹配点对大于设定值时，进行视觉初始化；如果匹配点对小于设定值时，重新开始选择图像帧；如果可以进行视觉初始化，运用RANSAC 8点法同时计算单应矩阵和基础矩阵，择优选择计算得到相机的位姿，之后用视觉里程计持续计算相机位姿，同时相机位姿用步骤3.8)的方法计算得到视觉位姿；
3.2)输出视觉初始化得到的视觉位姿；
3.3)IMU初始化：配置相机与IMU外参，此时外参为标定后的初始值，表示相机与IMU之间的平移，
Figure BDA0002241937180000102
表示相机与IMU之间的旋转四元数；配置初始协方差矩阵P，配置初始重力向量g，并用3.7)的方法计算IMU位姿；
3.4)输出IMU初始化得到的IMU位姿以及重力向量g；
3.5)上述步骤3.3)中所述配置初始重力向量的方法有很多种，在此举例其中一种：通过视觉初始化得到某一静止图像帧的相机位姿，将与此静止图像帧对应的IMU帧的加速度计读数作为重力向量g；
3.6)确定视觉坐标系与IMU的世界坐标系之间转换关系的计算方法为：若对于重力向量g按照上述步骤3.5)中所述方法进行计算，则该静止图像帧的位置即为系统的视觉坐标系的原点，将该静止图像帧的位置与姿态作为视觉坐标系，该静止图像帧也是此系统的视觉第一帧，用vicon表示视觉坐标系，将与此静止图像帧对应的IMU帧的位置与姿态作为IMU的世界坐标系，同时该IMU帧也是此系统的IMU第一帧，用world表示IMU的世界坐标系，此时，相机与IMU外参为视觉坐标系与IMU的世界坐标系的转换关系，用
Figure BDA0002241937180000103
表示视觉坐标系与IMU的世界坐标系之间的平移，用
Figure BDA0002241937180000104
表示视觉坐标系与IMU的世界坐标系之间的旋转四元数，其中转换关系中涉及到的尺度λ为初始配置值；
3.7)上述步骤3.3)中所述计算IMU位姿方法具体包括：按照上述步骤3.6)中所述确定视觉坐标系与IMU的世界坐标系之间转换关系后，通过对多个加速度积分得到速度，对多个速度积分得到位置，对惯导数据进行互补滤波得到姿态变化量，综合以上得到IMU位姿，
Figure BDA0002241937180000105
表示在IMU的世界坐标系下的IMU平移，
Figure BDA0002241937180000106
表示在IMU的世界坐标系下的IMU旋转四元数，
Figure BDA0002241937180000107
表示在IMU的世界坐标系下的IMU速度；
3.8)上述步骤3.1)中所述计算视觉位姿方法具体包括：按照步骤3.6)确定的视觉坐标系，将相机位姿转换为此视觉坐标系下的视觉位姿，用
Figure BDA0002241937180000111
表示在视觉坐标系下的相机平移，用
Figure BDA0002241937180000112
表示在视觉坐标系下的相机旋转四元数；
3.9)尺度λ初始化：根据相同时间内相机的运动与IMU的运动实际是一致的，利用视觉坐标系与IMU的世界坐标系之间转换关系、IMU运动模型以及已知的重力向量，得到IMU位姿和视觉位姿之间的转换关系，从而通过非线性优化求解最小二乘解，可得到尺度的初始估计；
3.10)为使坐标系间转换以及位姿计算更加准确，对尺度进行优化。通过λI-EKF(Lamdba Iterated Extended Kalman Filter)算法进一步精确尺度λ的值，并更新状态向量X,
Figure BDA0002241937180000113
Figure BDA0002241937180000114
其中bw、ba分别为IMU中陀螺仪的零漂、IMU中加速度计的零漂。
其中，所述步骤3.10)具体包括：
3.10.1)输入IMU初始化得到的IMU位姿以及重力向量g，输入步骤3.9)得到的尺度λ的初始值；
3.10.2)构建状态向量X，为方便起见，令
Figure BDA0002241937180000116
因此状态向量
Figure BDA0002241937180000117
Figure BDA0002241937180000118
3.10.3)由状态向量X以及IMU的运动模型、噪声模型，可以得出：状态向量的误差为
Figure BDA0002241937180000119
此
Figure BDA00022419371800001110
表示状态向量的估计值，误差向量
Figure BDA00022419371800001111
为25维系统状态误差项，由扩展卡尔曼滤波的预测过程，可得误差状态的转移矩阵：
Figure BDA0002241937180000121
其中：
Figure BDA0002241937180000122
Figure BDA0002241937180000123
Figure BDA0002241937180000124
Figure BDA0002241937180000125
E＝-A
Figure BDA0002241937180000126
Figure BDA0002241937180000127
Figure BDA0002241937180000128
Figure BDA0002241937180000129
Figure BDA00022419371800001210
表示角速度ω的反对称矩阵，am表示IMU的加速度测量值，wm表示IMU的角速度测量值，
Figure BDA00022419371800001211
表示加速度计零漂的估计值，表示陀螺仪零漂的估计值，
Figure BDA00022419371800001213
表示加速度的估计值，
Figure BDA00022419371800001214
表示角速度的估计值，Δt表示两个状态向量的时间间隔，为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，由上述推导出的矩阵，根据扩展卡尔曼滤波可以得到预测误差状态及其协方差矩阵，至此，对状态向量进行了预测，并得到相应预测误差协方差矩阵；
3.10.4)输入视觉初始化的得到的视觉位姿；
3.10.5)通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，可得观测误差模型，对观测误差模型进行离散处理，可得观测矩阵Hv如下：
Figure BDA0002241937180000132
Figure BDA0002241937180000133
为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，通过上述推导，根据扩展卡尔曼滤波，可进行系统状态向量的更新，通过卡尔曼增益，可得状态误差项，从而可得更新后的状态向量以及状态误差协方差矩阵；
3.10.6)判断更新前后状态向量X中尺度λ的变化量是否小于阈值μ，若是，则输出更新后的尺度λ，并将更新后的状态向量输出，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用预测得到的状态向量，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续对尺度λ变化量进行判断，查看是否可以输出所得量。
步骤四、获取IMU数据流中的惯导数据，对惯导数据进行互补滤波得到姿态变化量，再结合预积分计算得到IMU位姿，并将IMU位姿变化量给步骤五作为计算当前视觉帧位姿的参考变化量。
其中，预积分指的是把关键帧之间的IMU测量值积分成相对运动的约束，谓之预积分。
其中，所述惯导数据包括三轴加速度和三轴角速度。
其中，所述对惯导数据进行互补滤波得到姿态变化量的步骤具体包括：
4.1)对加速度进行归一化得ab；
4.2)将标准重力加速度gn＝[0 0 1]T转换为IMU坐标系下得gb；
4.3)对gb和ab做向量叉乘得到角速度的校正补偿值e；
4.4)利用PI控制的方法，将校正补偿值e补偿到滤波后的角速度上，得到校正后的角速度；
4.5)将校正后的角速度通过四元数微分方程转换为四元数，并将得到的四元数归一化后得到姿态矩阵，计算出姿态变化量。
步骤五、逐帧提取视频流中的图像特征，利用提取的图像特征进行视觉跟踪和匹配，得到视觉位姿。
其中，所述图像特征为图像的局部描述符。
步骤六、判断步骤五中的视觉跟踪是否丢失，若视觉跟踪丢失并且不能重定位，则用IMU位姿进行运动定位；若视觉跟踪未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终用于定位导航的相机位姿。
其中，所述步骤六中通过IDSF(Double Sensor Iterated Extended KalmanFilter)的方法对视觉位姿和IMU位姿进行融合的步骤具体包括：
6.1)输入多个IMU位姿；
6.2)构建状态向量X，状态向量
Figure BDA0002241937180000151
6.3)根据扩展卡尔曼滤波，可以由状态向量X以及IMU的运动模型、噪声模型计算得到预测误差状态及其协方差矩阵，增加系统噪声协方差矩阵，补偿状态预测中的误差，同时对预测误差状态协方差矩阵进行加权扩大，至此，对系统状态向量进行了预测，并得到相应调整过的预测误差协方差矩阵；
6.4)输入视觉位姿；
6.5)通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，根据扩展卡尔曼滤波，进行系统状态向量的更新；
6.6)判断更新前后状态误差协方差矩阵的迹的变化量是否小于阈值δ，若是，则输出更新后的状态向量，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用调整过的预测误差协方差矩阵，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续判断状态误差协方差矩阵的迹的变化量是否收敛至设定阈值δ以下，从而查看是否可以输出所得量。
下面结合附图，对本发明技术方案进行详细解释，方便本领域技术人员理解。
图1所示一种单目视觉惯性组合的定位导航方法的流程图：首先，采集视频流和IMU数据流，并进行打包对齐；接着，进行初始化；然后，获取IMU数据流中的惯导数据，通过用互补滤波结合预积分的方法得到IMU位姿，同时跟踪视频流中的图像特征并参考IMU位姿变化量得到视觉位姿；最后，进行视觉跟踪是否丢失的决策，若视觉跟踪丢失并且不能重定位，则用IMU位姿进行运动定位；若视觉跟踪未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终的相机位姿。
图2所示一种单目视觉惯性组合的定位导航方法中初始化的流程图。可描述为：初始化实现过程为：
i、视觉初始化：选取特征点数目足够多的连续图像帧进行匹配，当前后帧匹配点对足够多时，进行视觉初始化；如果匹配点对不够多时，重新开始选择图像帧；如果可以进行视觉初始化，运用RANSAC 8点法同时计算单应矩阵和基础矩阵，择优选择计算得到相机的位姿，之后用视觉里程计持续计算相机位姿，同时相机位姿用步骤viii的方法计算得到视觉位姿；
ii、输出视觉初始化得到的视觉位姿；
iii、IMU初始化：配置相机与IMU外参，此时外参为标定后的初始值，
Figure BDA0002241937180000161
表示相机与IMU之间的平移，
Figure BDA0002241937180000162
表示相机与IMU之间的旋转四元数；配置初始协方差矩阵P，配置初始重力向量g，并用vii的方法计算IMU位姿；
iv、输出IMU初始化得到的IMU位姿以及重力向量g；
v、上述步骤iii中所述配置初始重力向量的方法有很多种，在此举例其中一种：通过视觉初始化得到某一静止图像帧的相机位姿，将与此静止图像帧对应的IMU帧的加速度计读数作为重力向量g；
vi、确定视觉坐标系与IMU的世界坐标系之间转换关系的计算方法为：若对于重力向量g按照上述步骤v中所述方法进行计算，则该静止图像帧的位置即为系统的视觉坐标系的原点，将该静止图像帧的位置与姿态作为视觉坐标系，该静止图像帧也是此系统的视觉第一帧，用vicon表示视觉坐标系，将与此静止图像帧对应的IMU帧的位置与姿态作为IMU的世界坐标系，同时该IMU帧也是此系统的IMU第一帧，用world表示IMU的世界坐标系，此时，相机与IMU外参为视觉坐标系与IMU的世界坐标系的转换关系，用
Figure BDA0002241937180000171
表示视觉坐标系与IMU的世界坐标系之间的平移，用
Figure BDA0002241937180000172
表示视觉坐标系与IMU的世界坐标系之间的旋转四元数，其中转换关系中涉及到的尺度λ为初始配置值；
vii、上述步骤iii中所述计算IMU位姿方法具体包括：按照上述步骤vi中所述确定视觉坐标系与IMU的世界坐标系之间转换关系后，通过对多个加速度积分得到速度，对多个速度积分得到位置，对惯导数据进行互补滤波得到姿态变化量，综合以上得到IMU位姿，
Figure BDA0002241937180000173
表示在IMU的世界坐标系下的IMU平移，表示在IMU的世界坐标系下的IMU旋转四元数，
Figure BDA0002241937180000175
表示在IMU的世界坐标系下的IMU速度；
viii、上述步骤i中所述计算视觉位姿方法具体包括：按照步骤vi确定的视觉坐标系，将相机位姿转换为此视觉坐标系下的视觉位姿，用表示在视觉坐标系下的相机平移，用表示在视觉坐标系下的相机旋转四元数；
ix、尺度λ初始化：根据相同时间内相机的运动与IMU的运动实际是一致的，利用视觉坐标系与IMU的世界坐标系之间转换关系、IMU运动模型以及已知的重力向量，得到IMU位姿和视觉位姿之间的转换关系，从而通过非线性优化求解最小二乘解，可得到尺度的初始估计；
x、为使坐标系间转换以及位姿计算更加准确，对尺度进行优化。通过λI-EKF(Lamdba Iterated Extended Kalman Filter)算法进一步精确尺度λ的值，并更新状态向量X,
Figure BDA0002241937180000181
其中bw、ba分别为IMU中陀螺仪的零漂、IMU中加速度计的零漂。
图3所示一种单目视觉惯性组合的定位导航方法中λI-EKF的流程图。可描述为：λI-EKF实现过程为：
i、输入IMU初始化得到的IMU位姿、重力向量g和尺度λ的初始值；
ii、构建状态向量X，为方便起见，令
Figure BDA0002241937180000183
因此状态向量
Figure BDA0002241937180000184
Figure BDA0002241937180000185
iii、由状态向量X以及IMU的运动模型、噪声模型，可以得出：状态向量的误差为
Figure BDA0002241937180000186
此
Figure BDA0002241937180000187
表示状态向量的估计值，误差向量
Figure BDA0002241937180000188
为25维系统状态误差项，由扩展卡尔曼滤波的预测过程，可得误差状态的转移矩阵：
Figure BDA0002241937180000189
其中：
Figure BDA00022419371800001810
Figure BDA00022419371800001811
Figure BDA0002241937180000191
Figure BDA0002241937180000192
E＝-A
Figure BDA0002241937180000193
Figure BDA0002241937180000194
Figure BDA0002241937180000197
表示角速度ω的反对称矩阵，am表示IMU的加速度测量值，wm表示IMU的角速度测量值，
Figure BDA0002241937180000198
表示加速度计零漂的估计值，
Figure BDA0002241937180000199
表示陀螺仪零漂的估计值，
Figure BDA00022419371800001910
表示加速度的估计值，
Figure BDA00022419371800001911
表示角速度的估计值，Δt表示两个状态向量的时间间隔，
Figure BDA00022419371800001912
为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，由上述推导出的矩阵，根据扩展卡尔曼滤波可以得到预测误差状态及其协方差矩阵，至此，对状态向量进行了预测，并得到相应预测误差协方差矩阵；
iv、输入视觉初始化的得到的视觉位姿；
v、通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，可得观测误差模型，对观测误差模型进行离散处理，可得观测矩阵Hv如下：
Figure BDA0002241937180000201
Figure BDA0002241937180000202
为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，通过上述推导，根据扩展卡尔曼滤波，可进行系统状态向量的更新，通过卡尔曼增益，可得状态误差项，从而可得更新后的状态向量以及状态误差协方差矩阵；
vi、判断更新前后状态向量X中尺度λ的变化量是否小于阈值μ，若是，则输出更新后的尺度λ，并将更新后的状态向量输出，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用预测得到的状态向量，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续对尺度λ变化量进行判断，查看是否可以输出所得量。
图3所示一种单目视觉惯性组合的定位导航方法中IDSF的流程图。可描述为：IDSF实现过程为：
i、输入多个IMU位姿；
ii、构建状态向量X，状态向量
Figure BDA0002241937180000203
iii、根据扩展卡尔曼滤波，可以由状态向量X以及IMU的运动模型、噪声模型计算得到预测误差状态及其协方差矩阵，增加系统噪声协方差矩阵，补偿状态预测中的误差，同时对预测误差状态协方差矩阵进行加权扩大，至此，对系统状态向量进行了预测，并得到相应调整过的预测误差协方差矩阵；
iv、输入视觉位姿；
v、通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，根据扩展卡尔曼滤波，进行系统状态向量的更新；
vi、判断更新前后状态误差协方差矩阵的迹的变化量是否小于阈值δ，若是，则输出更新后的状态向量，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用调整过的预测误差协方差矩阵，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续判断状态误差协方差矩阵的迹的变化量是否收敛至设定阈值δ以下，从而查看是否可以输出所得量。
针对现有的纯单目视觉导航方法存在尺度不确定，易受环境条件影响的缺点；现有的纯惯性导航方法因受其本身的漂移和积累误差的影响，存在随着时间的增加其精度会逐渐发散的缺点。本发明采用IDSF算法实现了单目相机与惯性传感器的融合，达到了在定位过程中保持更高精度、更高鲁棒性的效果。针对现有的单目视觉惯性组合导航方法存在尺度初始值为手动设定且最终得到的尺度值不够精确的缺点，本发明在初始化中采用非线性优化确定了尺度初始值，并采用λI-EKF算法对尺度进行求精估计，达到了尺度精度更高的效果。

Claims (10)

1.一种单目视觉惯性组合的定位导航方法，其特征在于，包括：
步骤一、通过单目相机采集视频流；通过IMU采集IMU数据流；
步骤二、将采集的视频流和IMU数据流进行对齐，再将对齐后的视频流和IMU数据流打包在一起；
步骤三、系统数据初始化；若初始化成功，则顺序执行步骤四和步骤五；若初始化失败，则重新进行初始化；初始化过程包括视觉初始化、IMU初始化、确定视觉坐标系和IMU世界坐标系之间的转换关系、非线性优化确定尺度初始值和用λI-EKF对尺度进行求精估计；
步骤四、获取IMU数据流中的惯导数据，对惯导数据进行互补滤波得到姿态变化量，再结合预积分计算得到IMU位姿，并将IMU位姿变化量给步骤五作为计算当前视觉帧位姿的参考变化量；
步骤五、逐帧提取视频流中的图像特征，利用提取的图像特征进行视觉跟踪和匹配，得到视觉位姿；
步骤六、判断步骤五中的视觉跟踪是否丢失，若视觉跟踪丢失并且不能重定位，则用IMU位姿进行运动定位；若视觉跟踪未丢失，则通过IDSF的方法将视觉位姿和IMU位姿进行融合得到最终用于定位导航的相机位姿。
2.如权利要求1所述的方法，其特征在于，所述步骤二中的对齐包括时间戳同步；打包包括数据打包。
3.如权利要求1所述的方法，其特征在于，所述步骤三中初始化的步骤具体包括：
3.1)视觉初始化：选取特征点数目大于设定值的连续图像帧进行匹配，当前后帧匹配点对大于设定值时，进行视觉初始化；如果匹配点对小于设定值时，重新开始选择图像帧；如果可以进行视觉初始化，运用RANSAC 8点法同时计算单应矩阵和基础矩阵，择优选择计算得到相机的位姿，之后用视觉里程计持续计算相机位姿，同时对相机位姿计算得到视觉位姿；
3.2)输出视觉初始化得到的视觉位姿；
3.3)IMU初始化：配置相机与IMU外参，此时IMU外参为标定后的初始值，
Figure FDA0002241937170000021
表示相机与IMU之间的平移，表示相机与IMU之间的旋转四元数；配置初始重力向量g，计算IMU位姿；
3.4)输出IMU初始化得到的IMU位姿以及重力向量g；
3.5)配置步骤3.3)中初始重力向量：通过视觉初始化得到某一静止图像帧的相机位姿，将与此静止图像帧对应的IMU帧的加速度计读数作为重力向量g；
3.6)确定视觉坐标系与IMU的世界坐标系之间转换关系：静止图像帧的位置即为系统的视觉坐标系的原点，将该静止图像帧的位置与姿态作为视觉坐标系，该静止图像帧也是此系统的视觉第一帧，用vicon表示视觉坐标系，将与此静止图像帧对应的IMU帧的位置与姿态作为IMU的世界坐标系，同时该IMU帧也是此系统的IMU第一帧，用world表示IMU的世界坐标系，此时，相机与IMU外参为视觉坐标系与IMU的世界坐标系的转换关系，用
Figure FDA0002241937170000023
表示视觉坐标系与IMU的世界坐标系之间的平移，用表示视觉坐标系与IMU的世界坐标系之间的旋转四元数，其中转换关系中涉及到的尺度λ为初始配置值；
3.7)非线性优化确定尺度初始值：根据相同时间内相机的运动与IMU的运动实际是一致的，利用视觉坐标系与IMU的世界坐标系之间转换关系、IMU运动模型以及已知的重力向量，得到IMU位姿和视觉位姿之间的转换关系，从而通过非线性优化求解最小二乘解，可得到尺度的初始估计；
3.8)用λI-EKF对尺度进行求精估计：通过λI-EKF(Lamdba Iterated Extended KalmanFilter)算法进一步精确尺度λ的值，并更新状态向量X,
Figure FDA0002241937170000031
Figure FDA0002241937170000032
其中bw、ba分别为IMU中陀螺仪的零漂、IMU中加速度计的零漂。
4.如权利要求3所述的方法，其特征在于，所述步骤3.1)中计算视觉位姿方法具体包括：按照步骤3.6)确定的视觉坐标系，将相机位姿转换为此视觉坐标系下的视觉位姿，用
Figure FDA0002241937170000033
表示在视觉坐标系下的相机平移，用
Figure FDA0002241937170000034
表示在视觉坐标系下的相机旋转四元数。
5.如权利要求3或4所述的方法，其特征在于，所述步骤3.3)中计算IMU位姿方法具体包括：按照上述步骤3.6)中所述确定视觉坐标系与IMU的世界坐标系之间转换关系后，通过对多个加速度积分得到速度，对多个速度积分得到位置，对惯导数据进行互补滤波得到姿态变化量，综合以上得到IMU位姿，
Figure FDA0002241937170000035
表示在IMU的世界坐标系下的IMU平移，
Figure FDA0002241937170000036
表示在IMU的世界坐标系下的IMU旋转四元数，
Figure FDA0002241937170000037
表示在IMU的世界坐标系下的IMU速度。
6.如权利要求3-5之一所述的方法，其特征在于，所述步骤3.8)具体包括：
4.1)输入IMU初始化得到的IMU位姿以及重力向量g，输入步骤3.7)得到的尺度λ的初始值；
4.2)构建状态向量X，为方便起见，令
Figure FDA0002241937170000038
Figure FDA0002241937170000039
因此状态向量
Figure FDA00022419371700000311
4.3)由状态向量X以及IMU的运动模型、噪声模型，可以得出：状态向量的误差为
Figure FDA00022419371700000312
此
Figure FDA00022419371700000313
表示状态向量的估计值，误差向量为25维系统状态误差项，由扩展卡尔曼滤波的预测过程，可得误差状态的转移矩阵：
其中：
Figure FDA0002241937170000042
Figure FDA0002241937170000043
Figure FDA0002241937170000044
Figure FDA0002241937170000045
E＝-A
Figure FDA0002241937170000046
Figure FDA0002241937170000047
Figure FDA0002241937170000048
Figure FDA0002241937170000049
Figure FDA00022419371700000410
表示角速度ω的反对称矩阵，am表示IMU的加速度测量值，wm表示IMU的角速度测量值，
Figure FDA00022419371700000411
表示加速度计零漂的估计值，
Figure FDA00022419371700000412
表示陀螺仪零漂的估计值，
Figure FDA00022419371700000413
表示加速度的估计值，
Figure FDA00022419371700000414
表示角速度的估计值，Δt表示两个状态向量的时间间隔，为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，由上述推导出的矩阵，根据扩展卡尔曼滤波可以得到预测误差状态及其协方差矩阵，至此，对状态向量进行了预测，并得到相应预测误差协方差矩阵；
4.4)输入视觉初始化的得到的视觉位姿；
4.5)通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，可得观测误差模型，对观测误差模型进行离散处理，可得观测矩阵Hv如下：
Figure FDA0002241937170000052
Figure FDA0002241937170000053
为IMU坐标系到IMU的世界坐标系的方向余弦矩阵，通过上述推导，根据扩展卡尔曼滤波，可进行系统状态向量的更新，通过卡尔曼增益，可得状态误差项，从而可得更新后的状态向量以及状态误差协方差矩阵；
4.6)判断更新前后状态向量X中尺度λ的变化量是否小于阈值μ，若是，则输出更新后的尺度λ，并将更新后的状态向量输出，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用预测得到的状态向量，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续对尺度λ变化量进行判断，查看是否可以输出所得量。
7.如权利要求1所述的方法，其特征在于，所述步骤四中惯导数据为：三轴加速度和三轴角速度；预积分指的是把关键帧之间的IMU测量值积分成相对运动的约束。
8.如权利要求1所述的方法，其特征在于，所述步骤四中对惯导数据进行互补滤波得到姿态变化量的步骤具体包括：
6.1)对加速度进行归一化得ab；
6.2)将标准重力加速度gn＝[0 0 1]T转换为IMU坐标系下得gb；
6.3)对gb和ab做向量叉乘得到角速度的校正补偿值e；
6.4)利用PI控制的方法，将校正补偿值e补偿到滤波后的角速度上，得到校正后的角速度；
6.5)将校正后的角速度通过四元数微分方程转换为四元数，并将得到的四元数归一化后得到姿态矩阵，计算出姿态变化量。
9.如权利要求1所述的方法，其特征在于，所述步骤五中的图像特征为：图像的局部描述符。
10.如权利要求1所述的方法，其特征在于，所述步骤六中通过IDSF(Double SensorIterated Extended Kalman Filter)的方法对视觉位姿和IMU位姿进行融合的步骤具体包括：
8.1)输入多个IMU位姿；
8.2)构建状态向量X，状态向量
8.3)根据扩展卡尔曼滤波，可以由状态向量X以及IMU的运动模型、噪声模型计算得到预测误差状态及其协方差矩阵，增加系统噪声协方差矩阵，补偿状态预测中的误差，同时对预测误差状态协方差矩阵进行加权扩大，至此，对系统状态向量进行了预测，并得到相应调整过的预测误差协方差矩阵；
8.4)输入视觉位姿；
8.5)通过视觉位姿对系统状态进行纠正，由视觉坐标系与IMU的世界坐标系间的转换关系以及相机与IMU外参，根据扩展卡尔曼滤波，进行系统状态向量的更新；
8.6)判断更新前后状态误差协方差矩阵的迹的变化量是否小于阈值δ，若是，则输出更新后的状态向量，若不是，则进行多次迭代更新，直到符合条件进行输出，每一次迭代更新的过程为：在目前更新后的状态向量处对观测矩阵进行泰勒展开，取得一阶近似后，根据扩展卡尔曼滤波的更新过程并利用调整过的预测误差协方差矩阵，重新计算卡尔曼增益，并使用新的卡尔曼增益求得更新后的状态向量以及状态误差协方差矩阵，从而得到多次迭代更新后的状态向量，继续判断状态误差协方差矩阵的迹的变化量是否收敛至设定阈值δ以下，从而查看是否可以输出所得量。
CN201911003161.5A 2019-10-22 2019-10-22 一种单目视觉惯性组合的定位导航方法 Pending CN110702107A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911003161.5A CN110702107A (zh) 2019-10-22 2019-10-22 一种单目视觉惯性组合的定位导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911003161.5A CN110702107A (zh) 2019-10-22 2019-10-22 一种单目视觉惯性组合的定位导航方法

Publications (1)

Publication Number Publication Date
CN110702107A true CN110702107A (zh) 2020-01-17

Family

ID=69200780

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911003161.5A Pending CN110702107A (zh) 2019-10-22 2019-10-22 一种单目视觉惯性组合的定位导航方法

Country Status (1)

Country Link
CN (1) CN110702107A (zh)

Cited By (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111307176A (zh) * 2020-03-02 2020-06-19 北京航空航天大学青岛研究院 一种vr头戴显示设备中视觉惯性里程计的在线标定方法
CN111390940A (zh) * 2020-04-20 2020-07-10 上海机器人产业技术研究院有限公司 一种工业机器人自动化标定系统及方法
CN111539982A (zh) * 2020-04-17 2020-08-14 北京维盛泰科科技有限公司 一种移动平台中基于非线性优化的视觉惯导初始化方法
CN111580596A (zh) * 2020-05-19 2020-08-25 北京数字绿土科技有限公司 多个imu时间同步方法、装置、终端
CN111581322A (zh) * 2020-05-12 2020-08-25 北京维盛沃德科技有限公司 视频中兴趣区域在地图窗口内显示的方法和装置及设备
CN112188037A (zh) * 2020-09-24 2021-01-05 影石创新科技股份有限公司 生成陀螺仪旋转方向的方法及计算机设备
CN112179338A (zh) * 2020-09-07 2021-01-05 西北工业大学 一种基于视觉和惯导融合的低空无人机自身定位方法
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112444246A (zh) * 2020-11-06 2021-03-05 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN112729294A (zh) * 2021-04-02 2021-04-30 北京科技大学 适用于机器人的视觉和惯性融合的位姿估计方法及系统
CN112734852A (zh) * 2021-03-31 2021-04-30 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN112862768A (zh) * 2021-01-28 2021-05-28 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN113237478A (zh) * 2021-05-27 2021-08-10 哈尔滨工业大学 一种无人机的姿态和位置估计方法及无人机
CN113326716A (zh) * 2020-02-28 2021-08-31 北京创奇视界科技有限公司 面向装配现场环境装配指导ar应用定位的回环检测方法
CN113670327A (zh) * 2021-08-11 2021-11-19 影石创新科技股份有限公司 视觉惯性里程计初始化方法、装置、设备和存储介质
CN113686334A (zh) * 2021-07-07 2021-11-23 上海航天控制技术研究所 一种提高星敏感器和陀螺在轨联合滤波精度的方法
CN113936120A (zh) * 2021-10-12 2022-01-14 北京邮电大学 一种无标记的轻量化Web AR方法及系统
CN114199275A (zh) * 2020-09-18 2022-03-18 阿里巴巴集团控股有限公司 传感器的参数确定方法和装置
CN114370870A (zh) * 2022-01-05 2022-04-19 中国兵器工业计算机应用技术研究所 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
CN114593735A (zh) * 2022-01-26 2022-06-07 奥比中光科技集团股份有限公司 一种位姿预测方法及装置
CN115128655A (zh) * 2022-08-31 2022-09-30 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法和装置、电子设备和存储介质
CN115752442A (zh) * 2022-12-07 2023-03-07 无锡恺韵来机器人有限公司 一种基于单目视觉辅助惯性定位方法
CN112461237B (zh) * 2020-11-26 2023-03-14 浙江同善人工智能技术有限公司 一种应用于动态变化场景下的多传感器融合定位方法
WO2023109249A1 (zh) * 2021-12-13 2023-06-22 成都拟合未来科技有限公司 一种imu数据与视频数据对齐的方法及系统
CN114593735B (zh) * 2022-01-26 2024-05-31 奥比中光科技集团股份有限公司 一种位姿预测方法及装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105931275A (zh) * 2016-05-23 2016-09-07 北京暴风魔镜科技有限公司 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法
CN108981693A (zh) * 2018-03-22 2018-12-11 东南大学 基于单目相机的vio快速联合初始化方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105931275A (zh) * 2016-05-23 2016-09-07 北京暴风魔镜科技有限公司 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法
CN108981693A (zh) * 2018-03-22 2018-12-11 东南大学 基于单目相机的vio快速联合初始化方法
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
潘林豪等: "单目相机-IMU外参自动标定与在线估计的视觉-惯导SLAM", 《仪器仪表学报》 *
熊敏君等: "基于单目视觉与惯导融合的无人机位姿估计", 《计算机应用》 *
陈文钊: "基于多传感器融合的移动机器人定位与地图创建", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (38)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113326716A (zh) * 2020-02-28 2021-08-31 北京创奇视界科技有限公司 面向装配现场环境装配指导ar应用定位的回环检测方法
CN113326716B (zh) * 2020-02-28 2024-03-01 北京创奇视界科技有限公司 面向装配现场环境装配指导ar应用定位的回环检测方法
CN111307176B (zh) * 2020-03-02 2023-06-16 北京航空航天大学青岛研究院 一种vr头戴显示设备中视觉惯性里程计的在线标定方法
CN111307176A (zh) * 2020-03-02 2020-06-19 北京航空航天大学青岛研究院 一种vr头戴显示设备中视觉惯性里程计的在线标定方法
CN111539982A (zh) * 2020-04-17 2020-08-14 北京维盛泰科科技有限公司 一种移动平台中基于非线性优化的视觉惯导初始化方法
CN111539982B (zh) * 2020-04-17 2023-09-15 北京维盛泰科科技有限公司 一种移动平台中基于非线性优化的视觉惯导初始化方法
CN111390940A (zh) * 2020-04-20 2020-07-10 上海机器人产业技术研究院有限公司 一种工业机器人自动化标定系统及方法
CN111581322A (zh) * 2020-05-12 2020-08-25 北京维盛沃德科技有限公司 视频中兴趣区域在地图窗口内显示的方法和装置及设备
CN111581322B (zh) * 2020-05-12 2023-07-18 北京维盛沃德科技有限公司 视频中兴趣区域在地图窗口内显示的方法和装置及设备
CN111580596A (zh) * 2020-05-19 2020-08-25 北京数字绿土科技有限公司 多个imu时间同步方法、装置、终端
CN111580596B (zh) * 2020-05-19 2022-04-15 北京数字绿土科技股份有限公司 多个imu时间同步方法、装置、终端
CN112179338A (zh) * 2020-09-07 2021-01-05 西北工业大学 一种基于视觉和惯导融合的低空无人机自身定位方法
CN114199275A (zh) * 2020-09-18 2022-03-18 阿里巴巴集团控股有限公司 传感器的参数确定方法和装置
CN112188037B (zh) * 2020-09-24 2023-03-24 影石创新科技股份有限公司 生成陀螺仪旋转方向的方法及计算机设备
CN112188037A (zh) * 2020-09-24 2021-01-05 影石创新科技股份有限公司 生成陀螺仪旋转方向的方法及计算机设备
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112444246A (zh) * 2020-11-06 2021-03-05 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN112444246B (zh) * 2020-11-06 2024-01-26 北京易达恩能科技有限公司 高精度的数字孪生场景中的激光融合定位方法
CN112461237B (zh) * 2020-11-26 2023-03-14 浙江同善人工智能技术有限公司 一种应用于动态变化场景下的多传感器融合定位方法
CN112862768A (zh) * 2021-01-28 2021-05-28 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN112862768B (zh) * 2021-01-28 2022-08-02 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN112734852A (zh) * 2021-03-31 2021-04-30 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN112729294B (zh) * 2021-04-02 2021-06-25 北京科技大学 适用于机器人的视觉和惯性融合的位姿估计方法及系统
CN112729294A (zh) * 2021-04-02 2021-04-30 北京科技大学 适用于机器人的视觉和惯性融合的位姿估计方法及系统
CN113237478A (zh) * 2021-05-27 2021-08-10 哈尔滨工业大学 一种无人机的姿态和位置估计方法及无人机
CN113237478B (zh) * 2021-05-27 2022-10-14 哈尔滨工业大学 一种无人机的姿态和位置估计方法及无人机
CN113686334B (zh) * 2021-07-07 2023-08-04 上海航天控制技术研究所 一种提高星敏感器和陀螺在轨联合滤波精度的方法
CN113686334A (zh) * 2021-07-07 2021-11-23 上海航天控制技术研究所 一种提高星敏感器和陀螺在轨联合滤波精度的方法
CN113670327A (zh) * 2021-08-11 2021-11-19 影石创新科技股份有限公司 视觉惯性里程计初始化方法、装置、设备和存储介质
CN113936120A (zh) * 2021-10-12 2022-01-14 北京邮电大学 一种无标记的轻量化Web AR方法及系统
WO2023109249A1 (zh) * 2021-12-13 2023-06-22 成都拟合未来科技有限公司 一种imu数据与视频数据对齐的方法及系统
CN114370870A (zh) * 2022-01-05 2022-04-19 中国兵器工业计算机应用技术研究所 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
CN114370870B (zh) * 2022-01-05 2024-04-12 中国兵器工业计算机应用技术研究所 适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法
CN114593735A (zh) * 2022-01-26 2022-06-07 奥比中光科技集团股份有限公司 一种位姿预测方法及装置
CN114593735B (zh) * 2022-01-26 2024-05-31 奥比中光科技集团股份有限公司 一种位姿预测方法及装置
CN115128655A (zh) * 2022-08-31 2022-09-30 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法和装置、电子设备和存储介质
CN115752442A (zh) * 2022-12-07 2023-03-07 无锡恺韵来机器人有限公司 一种基于单目视觉辅助惯性定位方法
CN115752442B (zh) * 2022-12-07 2024-03-12 运来智能装备(无锡)有限公司 一种基于单目视觉辅助惯性定位方法

Similar Documents

Publication Publication Date Title
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN111426318B (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN111780754B (zh) 基于稀疏直接法的视觉惯性里程计位姿估计方法
CN109520497B (zh) 基于视觉和imu的无人机自主定位方法
CN112240768A (zh) 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111780781B (zh) 基于滑动窗口优化的模板匹配视觉和惯性组合里程计
CN108731700B (zh) 一种视觉惯性里程计中的加权欧拉预积分方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN111340851A (zh) 基于双目视觉与imu融合的slam方法
CN111862316A (zh) 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法
CN109211230A (zh) 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法
CN115046545A (zh) 一种深度网络与滤波结合的定位方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及系统
CN112284381B (zh) 视觉惯性实时初始化对准方法及系统
CN112985450B (zh) 一种具有同步时间误差估计的双目视觉惯性里程计方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN114993338B (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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200117