一种基于点线初始化的视觉惯性里程计方法技术

本发明专利技术提供了一种基于点线初始化的视觉惯性里程计方法，包括以下步骤：步骤一、通过相机采集视觉数据，通过IMU采集惯性数据；步骤二、图像特征提取跟踪：对任意两帧图像提取并且跟踪点特征和线特征，得到前后帧之间的特征对应关系；步骤三、IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值；步骤四、初始化：将步骤二提取和跟踪到的特征以及步骤三计算得到的IMU预积分值送入视觉惯性初始化模型，得到初始参数值；步骤五、紧耦合视觉惯性定位：将初始参数送入紧耦合视觉惯性估计器，实现对载体的实时定位；本发明专利技术增加了系统在无纹理环境下的初始化成功率，降低了初始化时间。降低了初始化时间。降低了初始化时间。

【技术实现步骤摘要】
一种基于点线初始化的视觉惯性里程计方法


[0001]本专利技术属于SLAM(Simultaneous Localization and Mapping，同步定位与建图)领域，具体涉及一种基于点线初始化的视觉惯性里程计方法。

技术介绍

[0002]由于相机和IMU(Inertial Measurement Unit,惯性测量单元)传感器的低成本性以及采集信息能力的互补性，VIO(Visual
‑
Inertial Odometry，视觉惯性里程计)已广泛应用于从机器人导航到自主驾驶等领域。作为VIO的初始步骤，初始化过程可以为后端提供尺度、前几个图像帧的位姿和速度、IMU零偏以及重力加速度方向等关键信息。因此，一个健壮的VIO系统必须要求有精确且快速收敛的初始化部分来提供后端所需要的初始信息。
[0003]VINS
‑
Mono是传统经典的VIO方案，其初始化阶段从利用特征点的纯视觉SFM(Structure from Motion,从运动中恢复结构)开始，然后通过将该结构与IMU预积分测量值松耦合对齐来恢复度量尺度、速度、重力加速度方向和IMU零偏。
[0004]但是在某些特殊的无纹理环境中，VIO系统无法在场景中提取到足够的特征点，即没有有效的场景结构与IMU预积分测量值对齐，最终导致初始化过程失败甚至整个VIO系统的发散。

技术实现思路

[0005]针对上述VIO存在的问题，提出一种基于点线初始化的视觉惯性里程计方法，提升VIO系统在无纹理环境下初始化的成功率以及定位精度。其初始化过程的主要特征在于在SFM过程中结合特征点和特征线，构建了一个存在点和线的结构。
[0006]本专利技术为解决上述技术问题采取如下技术方案：一种基于点线初始化的视觉惯性里程计方法，包括以下步骤：
[0007]步骤一、通过相机采集视觉数据，通过IMU采集惯性数据；
[0008]步骤二、图像特征提取跟踪：对任意两帧图像提取并且跟踪点特征和线特征，得到前后帧之间的特征对应关系；
[0009]步骤三、IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值；
[0010]步骤四、初始化：将步骤二提取和跟踪到的特征以及步骤三计算得到的IMU预积分值送入视觉惯性初始化模型，得到初始参数值；
[0011]步骤五、紧耦合视觉惯性定位：将初始参数送入紧耦合视觉惯性估计器，实现对载体的实时定位。
[0012]进一步地，步骤二所述图像特征提取跟踪包括如下具体步骤：
[0013]步骤2.1、对新帧快速提取角点，并且将所有的提取到的角点与上一帧的角点通过光流法匹配，得到前后两帧点特征之间的对应关系。
[0014]步骤2.2、通过LSD(Line Segment Detector，线段检测器)算法对新帧提取线特征
并且使用LBD(Line Binary Descriptor，线二进制描述子)算法计算它们的描述子，随后根据描述子对上一帧和新帧的线特征进行匹配，得到前后两帧线特征之间的对应关系。
[0015]进一步地，步骤三所述IMU预积分包括如下具体步骤：
[0016]步骤3.1、收集两个图像帧之间的所有IMU测量值，IMU测量值方程表示为：
[0017][0018]其中，a
t
和ω
t
分别为t时刻IMU坐标系下加速度真实值和角速度真实值，a
m
和ω
m
分别为t时刻IMU坐标系下加速度与角速度测量值，和分别为t时刻加速度计与陀螺仪零偏，g
w
是世界坐标系下的重力加速度，R
tw
为世界坐标系到t时刻IMU坐标系的旋转矩阵，n
a
和n
ω
分别为加速度计与陀螺仪的高斯噪声。
[0019]步骤3.2、根据步骤3.1得到的若干组IMU数据进行预积分：
[0020][0021]其中：
[0022][0023]其中和表示第k个图像帧到从该图像帧开始第i个IMU数据的三个预积分项，δt为相邻两个IMU数据之间的时间值，表示i时刻IMU坐标系到第k个图像帧的IMU坐标系的旋转矩阵，和分别表示i时刻加速度计与陀螺仪的测量值，表示四元数乘法。
[0024]步骤3.3、如果IMU零偏改变，则重新计算IMU预积分测量值：
[0025][0026]其中和表示重新计算之前的三个预积分项，和表示重新计算之后的三个预积分项，和分别为加速计与陀螺仪零偏的变化量。J表示各个预积分项关于IMU零偏的雅克比矩阵。
[0027]进一步地，步骤四所述初始化包括如下具体步骤：
[0028]步骤4.1、基于滑动窗口构造点线的结构
[0029]第一步，当新帧即将进入窗口时，判断窗口中是否存在与新帧的匹配特征点足够多并且视差足够大的帧；第二步，如果不存在该帧，实行边缘化，等待下一帧的到来，关于边
缘化详见步骤5.2；如果存在，将该帧视为参考帧c0，用五点法求得窗口中所有的帧到参考帧的旋转与平移，并且三角化窗口中所有的点特征和线特征；第三步，对窗口中所有的状态量做BA(Bundle Adjustment，光束法平差)，窗口中所有的状态量为：
[0030][0031]其中和分别为第k帧图像帧相机坐标系到参考帧相机坐标系之间平移量和四元数，l
i
和L
i
分别为参考帧相机坐标系下第i个点特征的三维坐标和线特征的普吕克坐标，k表示从0到n的正整数。
[0032]BA通过最小化如下函数：
[0033][0034]其中Fi和Gi分别是相机观察到点特征和线特征的集合，为第i个点特征从第j帧图像帧投影到参考帧相机坐标系下的二维归一化平面坐标，为第i个线特征从第j帧图像帧投影到参考帧相机坐标系下的普吕克坐标，ρ为鲁棒核函数，r
fi
表示l
i
取归一化平面坐标后与之间的二维差值，r
li
表示和L
i
之间的六维差值。
[0035]步骤4.2、估计陀螺仪偏差
[0036]给定IMU与相机之间的外参，将状态量从相机坐标系转化到IMU坐标系：
[0037][0038]其中s为构造点线结构的尺度信息，表示归一化平移量，q
bc
为相机到IMU的旋转四元数，和分别表示第k个相机的IMU坐标系到参考帧坐标系下的旋转四元数和平移量，q
bc
和p
bc
分别表示相机坐标系到IMU坐标系的旋转四元数和平移量。
[0039]将步骤4.1得到的结构与IMU预积分对齐，得到：
[0040][0041]其中，B表示滑动窗口中所有的图像帧，和分别表示第k+1和第k帧IMU坐标系到参考相机坐标系的旋转四元数。
[0042]步骤4.3本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于点线初始化的视觉惯性里程计方法，其特征在于：包括以下步骤：步骤一、通过相机采集视觉数据，通过IMU采集惯性数据；步骤二、图像特征提取跟踪：对任意两帧图像提取并且跟踪点特征和线特征，得到前后帧之间的特征对应关系；步骤三、IMU预积分：对任意两帧图像之间的所有IMU数据进行预积分，得到IMU先验值，若IMU零偏改变则根据新的零偏重新计算IMU预积分测量值；步骤四、初始化：将步骤二提取和跟踪到的特征以及步骤三计算得到的IMU预积分值送入视觉惯性初始化模型，得到初始参数值；步骤五、紧耦合视觉惯性定位：将初始参数送入紧耦合视觉惯性估计器，实现对载体的实时定位。2.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤二所述图像特征提取跟踪具体包括如下步骤：步骤2.1、对新帧快速提取角点，并且将所有的提取到的角点与上一帧的角点通过光流法匹配，得到前后两帧点特征之间的对应关系；步骤2.2、通过LSD算法对新帧提取线特征并且使用LBD算法计算它们的描述子，随后根据描述子对上一帧和新帧的线特征进行匹配，得到前后两帧线特征之间的对应关系。3.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤三所述IMU预积分具体包括如下步骤：步骤3.1、收集两个图像帧之间的所有IMU测量值，IMU测量值方程表示为：其中，a
t
和ω
t
分别为t时刻IMU坐标系下加速度真实值和角速度真实值，a
m
和ω
m
分别为t时刻IMU坐标系下加速度与角速度测量值，b
at
和b
ωt
分别为t时刻加速度计与陀螺仪零偏，g
w
是世界坐标系下的重力加速度，R
tw
为世界坐标系到t时刻IMU坐标系的旋转矩阵，n
a
和n
ω
分别为加速度计与陀螺仪的高斯噪声；步骤3.2、根据步骤3.1得到的若干组IMU数据进行预积分：其中：其中和表示第k个图像帧到从该图像帧开始第i个IMU数据的三个预积分项，δt为相邻两个IMU数据之间的时间值，表示i时刻IMU坐标系到第k个图像帧的IMU坐标系的旋转矩阵，和分别表示i时刻加速度计与陀螺仪的测量值，表示四元数乘法；
步骤3.3、如果IMU零偏改变，则重新计算IMU预积分测量值：其中和表示重新计算之前的三个预积分项，和表示重新计算之后的三个预积分项，和分别为加速计与陀螺仪零偏的变化量，J表示各个预积分项关于IMU零偏的雅克比矩阵。4.根据权利要求1所述的一种基于点线初始化的视觉惯性里程计方法，其特征在于：步骤四所述初始化具体包括如下步骤：步骤4.1、基于滑动窗口构造点线的结构第一步，当新帧即将进入窗口时，判断窗口中是否存在与新帧的匹配特征点足够多并且视差足够大的帧；第二步，如果不存在该帧，实行边缘化，等待下一帧的到来；如果存在，将该帧视为参考帧c0，用五点法求得窗口中所有的帧到参考帧的旋转与平移，并且三角化窗口中所有的点特征和线特征；第三步，对窗口中所有的状态量做BA，窗口中所有的状态量为：其中和分别为第k帧图像帧相机坐标系到参考帧相机坐标系之间平移量和四元数，l
i
和L
i
分别为参考帧相机坐标系下第i个点特征的三维坐标和线特征的普吕克坐标，k表示从0到n的正整数；BA通过最小化如下函数：其中Fi和Gi分别是相机观察到点特征和线特征的集合，为第i个点特征从第j帧图像帧投影到参考帧相机坐标系下的二维归一化平面坐标，为第i个线特征从第j帧图像帧投影到参考帧相机坐标系下的普吕克坐标，ρ为鲁棒核函数，...

【专利技术属性】
技术研发人员：熊剑，吴建峰，杨祖华，郭杭，衷卫声，余志鹏，黄涛，莫钊，
申请(专利权)人：南昌大学，
类型：发明
国别省市：