CN111539982B - 一种移动平台中基于非线性优化的视觉惯导初始化方法 - Google Patents

Abstract

本发明提供了一种移动平台中基于非线性优化的视觉惯导初始化方法，包括：利用移动平台的相机采集视频流得到图像帧以及其共视点，利用移动平台的IMU采集IMU数据流并进行预积分得到IMU位姿；判断IMU是否充分运动，若IMU运动充分，则开始初始化；若IMU运动不充分，则继续采集视频流以及IMU数据流；利用共视点构成的向量减法三角形法则，联立N个图像帧以及其共视的点构成的线性方程组，计算得到重力初值、每个点到其可视相机帧光心的距离参数d初值以及相机速度初值；利用非线性紧耦合优化算法得到优化后的重力、相机位姿以及速度、共视点3D位置。本发明针对现有技术的不足，提高了移动平台初始估计的精确度，达到了更高定位精度、更高鲁棒性的初始化效果。

Description

一种移动平台中基于非线性优化的视觉惯导初始化方法

技术领域

本发明涉及定位技术领域，特别是涉及一种移动平台中基于非线性优化的视觉惯导初始化方法。

背景技术

近些年，定位技术是当下机器人、无人机等移动平台中研究的热点。在各种各样传感器的组合中，视觉和惯导融合成为了最具潜力而又经济实惠的方案。首先，移动平台中的相机可以提供环境的相关信息，使得定位技术能够定位相机以及识别出曾到过的地方。但纯视觉的初始化方法非常容易因为运动模糊造成失败，且存在尺度不确定性的缺点。其次，移动平台中的IMU(惯性测量单元)可以提供自运动的相关信息，这使得定位技术可以恢复单目视觉初始化无法做到的尺度信息，并且可以估计重力方向。但低精度的惯导初始化性能有限，且高精度的惯导初始化无法民用，费用太高。两者的结合初始化方法获得的参数估计更精确，使得定位技术可以高精度、健壮性地计算相机位姿以及移动平台的增量运动。

发明内容

本发明针对现有技术的不足，本发明采用共视点构成的向量三角形法则对重力等参数进行初始估计，并采用非线性紧耦合优化算法实现了视觉惯导的初始化，提供了更高精度、更高鲁棒性的一种移动平台中基于非线性优化的视觉惯导初始化方法。

本发明解决其技术问题所采用的技术方案是：

步骤一、利用移动平台中的相机采集视频流得到图像帧以及其共视点，利用移动平台中的IMU采集IMU数据流；

步骤二、对IMU数据流进行预积分得到与图像帧对齐的IMU位姿；

步骤三、判断IMU是否充分运动，若IMU运动充分，则开始初始化；若IMU运动不充分，则继续采集视频流以及IMU数据流；

步骤四、选取任意三个连续图像帧共视的点，利用向量减法三角形法则计算得到视觉位移变化，利用同一段时间内视觉位移变化与IMU位移变化相，联立N个图像帧以及其共视的点构成的线性方程组，进行最小二乘求解得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值，此世界坐标系是指以相机第一帧为参考坐标系；通过重力初值以及参数d初值，计算得到世界坐标系下相机第一帧的速度初值；

步骤五、IMU预积分计算得到姿态变化的过程中存在陀螺仪零漂的误差影响，通过对上述步骤四中线性方程组进行最小二乘求解，估计得到IMU陀螺仪零漂初值；

步骤六、对上述步骤四中优化后的视觉位移变化进行平均得到视觉位移变化的初值；利用上述步骤五中得到的陀螺仪零漂，计算得到优化后的IMU姿态变化，由于IMU与视觉帧对齐，可得到视觉姿态变化的初值；通过步骤四中算得的距离参数d计算得到每个共视点的初始位置；利用重力模值不变的特点，得到重力方向向量；选取连续两帧图像，同时选取共视点数大于阈值μ以及平移属于设定阈值区间的两帧非连续图像，对选出的图像进行重投影误差计算，对与上述图像帧对应的IMU进行IMU残差计算，通过去量纲分别得到重投影误差与IMU残差对应的无量纲误差；将得到的两个无量纲误差进行求和，利用LM(Levenberg-Marquardt)算法对此误差和进行最小化，得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。

所述步骤三中判断IMU是否充分运动具体为所获取IMU的加速度的标准差是否大于阈值α；若加速度的标准差大于阈值α，则认为IMU运动充分；若加速度的标准差小于或等于阈值α，则认为IMU运动不充分。

所述步骤四具体包括：

4.1)在准备初始化的20个图像帧中，选取任意三个连续图像帧c₁、c₂、c₃，取其中一个共视的点y_i，该点到图像帧c₁、c₂、c₃光心的距离参数分别为分别为图像帧c₁、c₂、c₃光心指向该点的单位方向向量，利用向量减法三角形法则可得图像帧c₁、c₂间视觉位移变化s_c1c2：

R_c1c2为图像帧c₂到c₁的旋转矩阵，利用同一段时间内视觉位移变化与IMU位移变化相等，可得如下等式：

其中，R_IC、P_IC分别为IMU与相机的外参旋转、平移，R_b1b2为与图像帧c₂对齐的IMUb₂到与图像帧c₁对齐的IMUb₁的旋转矩阵，Δt₂₃为图像帧c₂与图像帧c₃的时间差，g_w为世界坐标系下相机第一帧的重力，Δ_p12.、Δv₁₂分别为IMU预积分得到的IMUb₂与b₁间的位移变化、速度变化；

4.2)联立20个图像帧以及其共视的点构成的线性方程组，进行最小二乘求解得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值，此世界坐标系是指以相机第一帧为参考坐标系，通过重力初值以及参数d初值，计算得到世界坐标系下相机第一帧的速度初值。

所述步骤六具体包括：

6.1)对上述步骤四中优化后的视觉位移变化进行平均得到视觉位移变化的初值，利用上述步骤五中得到的陀螺仪零漂，计算得到优化后的IMU姿态变化，由于IMU与视觉帧对齐，可得到视觉姿态变化的初值；

6.2)通过步骤四中算得的距离参数d计算得到每个共视点的初始位置，利用重力模值不变的特点，得到重力方向向量；

6.3)选取连续两帧图像，同时选取共视点数大于阈值μ以及平移属于设定阈值区间的两帧非连续图像，对选出的图像进行重投影误差计算，对与上述图像帧对应的IMU进行IMU残差计算，通过去量纲分别得到重投影误差与IMU残差对应的无量纲误差，将得到的两个无量纲误差进行求和，利用LM(Levenberg-Marquardt)算法对此误差和进行最小化，得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。

本发明与现有技术相比，具有如下优点和有益效果：

本发明采用共视点构成的向量三角形法则对重力等参数进行初始估计，相比于现有移动平台中的视觉惯导初始化方法，本发明的初始估计精确度更高；同时采用非线性紧耦合优化算法实现了视觉惯导的初始化，达到了更高定位精度、更高鲁棒性的移动平台初始化效果。

附图说明

图1是一种移动平台中基于非线性优化的视觉惯导初始化方法的流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明一具体实施方式，如图1所示，本发明提供了一种移动平台中基于非线性优化的视觉惯导初始化方法，包括步骤一至步骤六。

步骤一、利用移动平台中的相机采集视频流得到图像帧以及其共视点，利用移动平台中的IMU采集IMU数据流。

步骤二、对IMU数据流进行预积分得到与图像帧对齐的IMU位姿。

步骤三、判断IMU是否充分运动，若IMU运动充分，则开始初始化；若IMU运动不充分，则继续采集视频流以及IMU数据流。

其中，所述判断IMU是否充分运动具体为所获取IMU的加速度的标准差是否大于阈值α；若加速度的标准差大于阈值α，则认为IMU运动充分；若加速度的标准差小于或等于阈值α，则认为IMU运动不充分。

步骤四、选取任意三个连续图像帧共视的点，利用向量减法三角形法则计算得到视觉位移变化，利用同一段时间内视觉位移变化与IMU位移变化相等，联立N个图像帧以及其共视的点构成的线性方程组，进行最小二乘求解得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值，此世界坐标系是指以相机第一帧为参考坐标系；通过重力初值以及参数d初值，计算得到世界坐标系下相机第一帧的速度初值。

所述步骤四具体包括：

4.1)在准备初始化的20个图像帧中，选取任意三个连续图像帧c₁、c₂、c₃，取其中一个共视的点y_i，该点到图像帧c₁、c₂、c₃光心的距离参数分别为分别为图像帧c₁、c₂、c₃光心指向该点的单位方向向量，利用向量减法三角形法则可得图像帧c₁、c₂间视觉位移变化s_c1c2：

R_c1c2为图像帧c₂到c₁的旋转矩阵，利用同一段时间内视觉位移变化与IMU位移变化相等，可得如下等式：

其中，R_IC、P_IC分别为IMU与相机的外参旋转、平移，R_b1b2为与图像帧c₂对齐的IMUb₂到与图像帧c₁对齐的IMUb₁的旋转矩阵，Δt₂₃为图像帧c₂与图像帧c₃的时间差，g_w为世界坐标系下相机第一帧的重力，Δ_p12.、Δv₁₂分别为IMU预积分得到的IMUb₂与b₁间的位移变化、速度变化；

4.2)联立20个图像帧以及其共视的点构成的线性方程组，进行最小二乘求解得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值，此世界坐标系是指以相机第一帧为参考坐标系，通过重力初值以及参数d初值，计算得到世界坐标系下相机第一帧的速度初值。

步骤五、IMU预积分计算得到姿态变化的过程中存在陀螺仪零漂的误差影响，通过对上述步骤四中线性方程组进行最小二乘求解，估计得到IMU陀螺仪零漂初值。

步骤六、对上述步骤四中优化后的视觉位移变化进行平均得到视觉位移变化的初值；利用上述步骤五中得到的陀螺仪零漂，计算得到优化后的IMU姿态变化，由于IMU与视觉帧对齐，可得到视觉姿态变化的初值；通过步骤四中算得的距离参数d计算得到每个共视点的初始位置；利用重力模值不变的特点，得到重力方向向量；选取连续两帧图像，同时选取共视点数大于阈值μ以及平移属于设定阈值区间的两帧非连续图像，对选出的图像进行重投影误差计算，对与上述图像帧对应的IMU进行IMU残差计算，通过去量纲分别得到重投影误差与IMU残差对应的无量纲误差；将得到的两个无量纲误差进行求和，利用LM(Levenberg-Marquardt)算法对此误差和进行最小化，得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。

所述步骤六具体包括：

6.1)对上述步骤四中优化后的视觉位移变化进行平均得到视觉位移变化的初值，利用上述步骤五中得到的陀螺仪零漂，计算得到优化后的IMU姿态变化，由于IMU与视觉帧对齐，可得到视觉姿态变化的初值；

6.2)通过步骤四中算得的距离参数d计算得到每个共视点的初始位置，利用重力模值不变的特点，得到重力方向向量；

6.3)选取连续两帧图像，同时选取共视点数大于阈值μ以及平移属于设定阈值区间的两帧非连续图像，对选出的图像进行重投影误差计算，对与上述图像帧对应的IMU进行IMU残差计算，通过去量纲分别得到重投影误差与IMU残差对应的无量纲误差，将得到的两个无量纲误差进行求和，利用LM(Levenberg-Marquardt)算法对此误差和进行最小化，得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。

下面结合附图，对本发明技术方案进行详细解释，方便本领域技术人员理解。

图1所示一种移动平台中基于非线性优化的视觉惯导初始化方法的流程图：首先，利用移动平台中的相机采集视频流得到图像帧以及其共视点，利用移动平台中的IMU采集IMU数据流并进行预积分得到与图像帧对齐的IMU位姿；接着，判断IMU是否充分运动，若IMU运动充分，则开始初始化；若IMU运动不充分，则继续采集视频流以及IMU数据流；然后，利用共视点构成的向量减法三角形法则计算得到视觉位移变化，联立N个图像帧以及其共视的点构成的线性方程组，计算得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值以及世界坐标系下相机第一帧的速度初值，此世界坐标系是指以相机第一帧为参考坐标系；接着，计算得到陀螺仪零漂；最后，利用非线性紧耦合优化算法得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。

针对现有移动平台中的纯视觉定位方法存在尺度不确定，易受环境条件影响的缺点；现有移动平台中的低精度惯导定位方法存在性能有限的缺点。本发明采用非线性紧耦合优化算法实现了视觉惯导的初始化，达到了更高定位精度、更高鲁棒性的移动平台初始化效果。针对现有移动平台中方法存在初始估计不精确的缺点，本发明采用共视点构成的向量三角形法则对重力等参数进行初始估计，达到了移动平台初始估计精确度更高的效果。

Claims (4)

1.一种移动平台中基于非线性优化的视觉惯导初始化方法，其特征在于，包括：

步骤一、利用移动平台中的相机采集视频流得到图像帧以及其共视点，利用移动平台中的IMU采集IMU数据流；

步骤二、对IMU数据流进行预积分得到与图像帧对齐的IMU位姿；

步骤三、判断IMU是否充分运动，若IMU运动充分，则开始初始化；若IMU运动不充分，则继续采集视频流以及IMU数据流；

步骤四、选取任意三个连续图像帧共视的点，利用向量减法三角形法则计算得到视觉位移变化，利用同一段时间内视觉位移变化与IMU位移变化相，联立N个图像帧以及其共视点构成的线性方程组，进行最小二乘求解得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值，此世界坐标系是指以相机第一帧为参考坐标系；通过重力初值以及参数d初值，计算得到世界坐标系下相机第一帧的速度初值；

步骤五、IMU预积分计算得到姿态变化的过程中存在陀螺仪零漂的误差影响，通过对上述步骤四中线性方程组进行最小二乘求解，估计得到IMU陀螺仪零漂初值；

步骤六、对上述步骤四中优化后的视觉位移变化进行平均得到视觉位移变化的初值；利用上述步骤五中得到的陀螺仪零漂，计算得到优化后的IMU姿态变化，由于IMU与视觉帧对齐，可得到视觉姿态变化的初值；通过步骤四中算得的距离参数d计算得到每个共视点的初始位置；利用重力模值不变的特点，得到重力方向向量；选取连续两帧图像，同时选取共视点数大于阈值μ以及平移属于设定阈值区间的两帧非连续图像，对选出的图像进行重投影误差计算，对与上述图像帧对应的IMU进行IMU残差计算，通过去量纲分别得到重投影误差与IMU残差对应的无量纲误差；将得到的两个无量纲误差进行求和，利用LM算法对此误差和进行最小化，得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。

2.如权利要求1所述的一种移动平台中基于非线性优化的视觉惯导初始化方法，其特征在于，所述步骤三中判断IMU是否充分运动具体为所获取IMU的加速度的标准差是否大于阈值α；若加速度的标准差大于阈值α，则认为IMU运动充分；若加速度的标准差小于或等于阈值α，则认为IMU运动不充分。

3.如权利要求1所述的一种移动平台中基于非线性优化的视觉惯导初始化方法，其特征在于，所述步骤四具体包括：4.1)在准备初始化的20个图像帧中，选取任意三个连续图像帧c₁、c₂、c₃，取其中一个共视的点y_i，该点到图像帧c₁、c₂、c₃光心的距离参数分别为分别为图像帧c₁、c₂、c₃光心指向该点的单位方向向量，利用向量减法三角形法则可得图像帧c₁、c₂间视觉位移变化s_c1c2：

R_c1c2为图像帧c₂到c₁的旋转矩阵，利用同一段时间内视觉位移变化与IMU位移变化相等，可得如下等式：

其中，R_IC、P_IC分别为IMU与相机的外参旋转、平移，为与图像帧c₂对齐的IMUb₂到与图像帧c₁对齐的IMUb₁的旋转矩阵，Δt₂₃为图像帧c₂与图像帧c₃的时间差，g_w为世界坐标系下相机第一帧的重力，Δp₁₂、Δv₁₂分别为IMU预积分得到的IMUb₂与b₁间的位移变化、速度变化；4.2)联立20个图像帧以及其共视的点构成的线性方程组，进行最小二乘求解得到世界坐标系下相机第一帧的重力初值、每个共视点到其可视相机帧光心的距离参数d初值，此世界坐标系是指以相机第一帧为参考坐标系，通过重力初值以及参数d初值，计算得到世界坐标系下相机第一帧的速度初值。

4.如权利要求1所述的一种移动平台中基于非线性优化的视觉惯导初始化方法，其特征在于，所述步骤六具体包括：

6.1)对上述步骤四中优化后的视觉位移变化进行平均得到视觉位移变化的初值，利用上述步骤五中得到的陀螺仪零漂，计算得到优化后的IMU姿态变化，由于IMU与视觉帧对齐，可得到视觉姿态变化的初值；

6.2)通过步骤四中算得的距离参数d计算得到每个共视点的初始位置，利用重力模值不变的特点，得到重力方向向量；

6.3)选取连续两帧图像，同时选取共视点数大于阈值μ以及平移属于设定阈值区间的两帧非连续图像，对选出的图像进行重投影误差计算，对与上述图像帧对应的IMU进行IMU残差计算，通过去量纲分别得到重投影误差与IMU残差对应的无量纲误差，将得到的两个无量纲误差进行求和，利用LM算法对此误差和进行最小化，得到优化后的世界坐标系下相机第一帧的重力、世界坐标系下相机的位姿以及速度、每个共视点的3D位置、IMU的陀螺仪零漂与加速度计零漂。