[发明专利]一种基于点线特征的视觉-惯性里程计状态估计方法

权利要求书

1.一种基于点线特征的视觉-惯性里程计状态估计方法，其特征在于包括以下步骤：

第一步，构建基于特征点和特征线的视觉-惯性里程计系统；基于特征点和特征线的视觉-惯性里程计系统由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成；

数据获取模块与视觉特征追踪模块、IMU预积分模块相连；数据获取模块从视觉-惯性里程计数据集下载数据，获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧，将单目相机图像帧发送给视觉特征追踪模块，将IMU数据发送给IMU预积分模块；

视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连；视觉特征追踪模块从数据获取模块接收单目相机图像帧，从单目相机图像帧中提取特征点和特征线信息，对连续单目相机图像帧中的特征进行匹配和追踪，将特征点和特征线信息以及特征追踪信息发送给特征重建模块，将特征点信息发送给闭环检测与重定位模块；

IMU预积分模块与数据获取模块、非线性优化模块相连；IMU预积分模块从数据获取模块接收IMU数据，利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理，将预积分处理后的IMU预积分数据发送给非线性优化模块；

特征重建模块与视觉特征追踪模块、非线性优化模块相连；特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息，分别构建特征点管理和特征线管理数据结构，对第一次出现的特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块；

闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连；闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存，通过特征点信息匹配，对每一帧单目相机图像帧中的特征点信息进行闭环检测，将闭环检测结果发送给非线性优化模块；

非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连；非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构，从IMU预积分模块获取最新两帧图像间的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、系统状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解，获取机器人状态；

第二步，令变量k＝1，数据获取模块从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧；令数据获取模块接收到第一帧左目相机图像帧时的时刻为t₀时刻；

2.1，t₀时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块；

2.2，视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧，视觉特征追踪模块利用FAST算法提取第一帧单目相机图像帧中的MAX_point个特征点，利用EDLines算法提取第一帧单目相机图像帧中MAX_line条特征线，根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息，并将特征点和特征线信息发送至特征重建模块，将特征点信息发送至闭环检测与重定位模块，MAX_point、MAX_line均为正整数；

2.3，特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息，特征重建模块构建第一帧特征点管理数据结构实例，该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标，特征重建模块构建第一帧特征线管理数据结构实例，该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；

2.4，闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息，采用DBoW2算法对第一帧单目相机图像中的特征点信息进行转化、存储；

第三步，数据获取模块从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，将双目图像帧中左目相机图像帧作为单目相机图像帧；数据获取模块在t_k时刻从视觉-惯性里程计数据集中接收到第k+1帧单目相机图像帧；数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块，将[t_k-1,t_k]时间区间内从视觉-惯性里程计数据集中接收到IMU数据发送给IMU预积分模块；

第四步，视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪，方法是：

4.1，视觉特征追踪模块利用KLT稀疏光流算法，在第k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点，得到第k+1帧单目相机图像中的特征点集合P_k+1，P_k+1中的元素为特征点，每个特征点由其在第k+1帧单目相机图像中的坐标表示；

4.2，视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAX_line条特征线段，得到第k+1帧单目相机图像中的特征线段集合L_k+1，L_k+1中的元素为特征线段l_a，a＝1,2,…,A，A表示L_k+1中特征线段的总数，l_a表示为l_a(s_a，e_a)，s_a为特征线段l_a的起点，e_a为特征线段l_a的终点，表示s_a在单目图像坐标系的x轴坐标，表示s_a在单目图像坐标系的y轴坐标；

4.3，视觉特征追踪模块对L_k+1中的特征线段进行整合，删除误检测线段，合并属于同一条直线的多条线段，得到整合后的新特征线段集合L_k+1；

4.4，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征点进行增补，得到新的特征点集合P_k+1；

4.5，视觉追踪模块根据特征点与特征线关系，对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配，以实现单目相机图像帧间特征线的追踪，方法是：

4.5.1，对L_k+1中的特征线段进行筛选，记录L_k+1中特征线与P_k+1中特征点之间的关系即每条特征线段上有哪些特征点，将L_k+1中特征线段上特征点数小于2的特征线段从L_k+1中剔除，对L_k+1中的特征线段由长至短重新排序，更新L_k+1；

4.5.2，对重新排序后的L_k+1中的线段进行编号，令线段l_c∈L_k+1，c＝1,2,…,C，C表示L_k+1中线段的总数，l_c可表示为l_c(s_c，e_c)，s_c为线段l_c的起点，e_c为线段l_c的终点，表示s_c在单目图像坐标系x轴坐标，表示s_c在单目图像坐标系y轴坐标；

4.5.3，选取L_k+1中的特征线段l_c，在第k帧单目相机图像特征线段集合L_k中依次寻找与l_c相匹配的特征线段，即属于不同图像帧的同一条特征线段：

4.5.3.1，选取L_k中的特征线段l_d，l_d∈L_k，d＝1,2,…,D，D表示L_k中线段的总数，若交换l_d起点与终点，即令l_d(s_d，e_d)＝(e_d，s_d)，转4.5.3.2；否则，直接转4.5.3.2；

4.5.3.2，若l_c与l_d拥有相同的特征点数超过2个，l_c与l_d线段长度差值小于阈值T_l，T_l大小为30个像素，且l_c起点s_c到l_d起点s_d距离小于阈值T_pp，T_pp大小为60个像素，且l_c终点e_c到l_d终点e_d距离小于阈值T_pp，则l_c与l_d匹配成功；转4.5.4；若上述条件有一项不满足，则l_c与l_d匹配不成功，转4.5.5；

4.5.4，此时l_c与l_d匹配成功，表示第k帧单目图像中的特征线段l_d在第k+1帧单目图像中追踪成功，记录匹配与追踪信息，包括l_c与l_d在L_k+1与L_k中的序号，若cC，令c＝c+1，转4.5.3；若c＝C，转4.5.6；

4.5.5，此时l_c与l_d匹配不成功，若dD，令d＝d+1，转4.5.3.1；若d＝D且cC，令c＝c+1，转4.5.3；若d＝D且c＝C，转4.5.6；

4.5.6，得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息，即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系；

4.6，视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块，将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块，将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块；

第五步，IMU预积分模块对从数据获取模块得到的[t_k-1,t_k]时间区间内的IMU数据，采用预积分算法进行预积分处理，将预积分处理后的[t_k-1,t_k]时间区间内的IMU预积分值发送至非线性优化模块；

第六步，特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息，以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息，根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例，对特征进行三维重建，更新特征点管理和特征线管理数据结构，将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块，方法如下：

6.1，特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息，构建第k+1帧特征点管理数据结构实例，该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标；

6.2，特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息，构建第k+1帧特征线管理数据结构实例，该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合；

6.3，若k4，特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步；

6.4，若k≥4，特征检测模块对点特征和线特征进行三维重建，方法是：

6.4.1，特征重建模块对于第k+1帧单目相机图像中的特征点，筛选出至少在第k帧中出现过的特征点，联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息，利用奇异值分解算法，对这些特征点在相机坐标系中的深度进行求解，并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度；

6.4.2，特征重建模块对于第k+1帧单目相机图像中的特征线，筛选出至少在第k帧中出现过的特征线l_f，l_f∈L_F，L_F表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合，基于l_f上特征点信息，进行三维重建；

6.4.3，特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块，转第七步；

第七步，闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息，并基于DBoW2算法对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索，并利用VINS-Mono中的闭环检测算法，对第k+1帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配，若匹配成功，闭环检测与重定位模块将闭环检测结果发送给非线性优化模块，转第八步；若匹配不成功，直接转第八步；

第八步，非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构，从IMU预积分模块获取[t_k-1,t_k]时间区间内的IMU预积分数据，从闭环检测与重定位模块获取闭环检测结果，若k9，直接转第九步；若k≥9，非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、系统状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解，获取机器人状态，方法是：

8.1，非线性优化模块构造滑动窗口，窗口内包括{t_k-9,t_k-8,…,t_k-1,t_k}共十个时刻内机器人系统相关信息，机器人系统相关信息包括从特征重建模块接收到的第k-8,k-7,…,k+1共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[t_k-10,t_k-9],[t_k-9,t_k-8],…,[t_k-1,t_k]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除t_k时刻外每个时刻机器人的位姿信息；

8.2，非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数；

8.3，非线性优化模块根据滑动窗口的信息，构造优化变量χ：

其中，x_k-9,x_k-8,…,x_k表示滑动窗口内十个时刻机器人系统状态，λ₀,λ₁,…,λ_n表示滑动窗口内十个时刻中所有特征点在三维世界中的深度，表示滑动窗口内十个时刻中所有特征线段的正交法表达参数，t_i时刻机器人系统状态x_i可以表达为：

其中，代表t_i时刻机器人在IMU坐标系相对世界坐标系的位移向量，代表t_i时刻机器人在IMU坐标系相对世界坐标系的速度向量，代表t_i时刻机器人在IMU坐标系相对世界坐标系的四元数，代表t_i时刻机器人在IMU坐标系中的IMU加速度计偏置，代表t_i时刻机器人在IMU坐标系中的IMU陀螺仪偏置；

8.4，非线性优化模块根据8.3中构造的优化变量构造优化损失函数：

其中：

表示IMU预积分残差项，表示IMU坐标系中从t_i时刻到t_i+1时刻IMU预积分值，表示IMU预积分数据集合，表示IMU从t_i时刻到t_i+1时刻IMU预积分残差，该残差的具体构造方式同VINS-Mono；

表示特征点残差项，表示相机坐标系中t_i时刻第j个特征点的重投影误差，j≤l，表示滑动窗口内所有特征点的集合，表示t_i时刻第j个特征点残差，该残差的具体构造方式同VINS-Mono；

表示特征线残差项，表示相机坐标系中t_i时刻第m个特征点的重投影误差，m≤n，表示滑动窗口内所有特征线的集合，表示t_i时刻第m个特征点残差，该残差的具体构造方式同PL-VIO；

表示滑动窗口外系统状态估计历史数据残差项，该残差项的具体构造方式同VINS-Mono；

表示闭环检测重定位残差项，若闭环检测发生在t_i时刻与t_v时刻之间，即t_v时刻单目图像帧和t_i时刻单目图像帧被判定为发生闭环检测，t_v时刻可为t_i时刻之前任一时刻，vi，表示相机坐标系中t_i时刻第j个特征点在t_v时刻单目图像帧上的重投影误差，表示世界坐标系中t_v时刻机器人的旋转四元数，表示世界坐标系中t_v时刻机器人的位移向量，下标w表示该数据为世界坐标系中的数据，表示闭环检测模块中存储的图像信息集合，表示滑动窗口内所有特征点的集合，表示t_i时刻闭环检测残差，该残差的具体构造方式同VINS-Mono；

8.5，非线性优化模块采用最小二乘优化方法对优化损失函数求解，获得t_k时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值，转第九步；

第九步，令k＝k+1，若还能从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，返回第三步，若不能从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据，转第十步；

第十步，结束。