CN113192140A - 一种基于点线特征的双目视觉惯性定位方法和系统 - Google Patents

Abstract

本发明公开了一种基于点线特征的双目视觉惯性定位方法和系统，所述方法通过在机器人上设置双目相机和IMU传感器估计机器人位姿，进行机器人定位，所述机器人位姿估计包括：从双目相机中采集的图像中提取点特征和线特征，在机器人运行过程中对点特征和线特征进行追踪；将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，进行机器人位姿估计，得到机器人位姿。线特征受环境光照影响相对较小，在特征点缺失的弱纹理环境下仍然可能提取到线特征来进行运动估计，本发明将线特征融合到视觉惯性定位算法中，提高了移动机器人在弱纹理环境下的定位精度和鲁棒性。

Description

一种基于点线特征的双目视觉惯性定位方法和系统

技术领域

本发明属于机器人定位领域，更具体地，涉及一种基于点线特征的双目视觉惯性定位方法和系统。

背景技术

基于视觉和惯性融合的SLAM技术在机器人领域有着广泛的应用。利用相机(单目、双目或深度)作为单一传感器的视觉SLAM方法受环境光照影响大，并且在相机快速运动时可能由于缺乏特征而造成定位。而IMU(惯性测量)传感器是根据加速度和角速度积分来进行运动估计的，不受外部环境光照的影响，在短时间快速运动下可以提供较准确的运动估计，因此将IMU和相机结合可以在一定程度上提高视觉SLAM系统的鲁棒性。

但是在IMU长期运行时会存在累计误差，对于传统的基于特征点法的SLAM，当机器人长时间运行在光线不好的弱纹理环境时，仍然会由于特征不足而造成定位误差。

由此可见，现有技术在光线不好的弱纹理环境时存在特征不足、造成定位误差的技术问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于点线特征的双目视觉惯性定位方法和系统，由此解决现有技术在光线不好的弱纹理环境时存在特征不足、造成定位误差的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种基于点线特征的双目视觉惯性定位方法，所述方法通过在机器人上设置双目相机和IMU传感器估计机器人位姿，进行机器人定位，所述机器人位姿估计包括如下步骤：

(1)从双目相机中采集的图像中提取点特征和线特征，在机器人运行过程中对点特征和线特征进行追踪；

(2)将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，进行机器人位姿估计，得到机器人位姿。

优选地，所述步骤(2)包括：

将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，构建最小二乘问题；

通过滑动窗口优化最小二乘问题，在优化过程中通过相机与IMU松耦合的方式来实现初始化，最终优化得到机器人位姿。

优选地，所述线特征追踪误差为：在双目相机的像素平面中，线特征追踪过程中匹配线段的两个端点到投影直线的垂直距离。

优选地，所述方法还包括：

若当前帧同时满足三个条件，选取当前帧为最新关键帧进行回环检测，并将该关键帧插入滑动窗口中进行优化，由此对机器人位姿进行优化；三个条件为：当前帧与上一关键帧之间的匹配特征点数大于第一预设值、当前帧与上一关键帧之间的IMU预积分平移量大于第二预设值、当前帧与上一关键帧之间的IMU预积分旋转量小于第三预设值。

优选地，所述方法还包括：

若当前关键帧与上一关键帧之间匹配的特征点数量小于第一设定值，或者当前关键帧与上一关键帧之间匹配的特征点平均移动的距离大于第二设定值，则将滑动窗口中第一个处理的关键帧进行边缘化后移除，否则，将最新一个处理的关键帧边缘化后移除。

优选地，所述松耦合的具体实现方式为：

利用双目相机和IMU的旋转量对齐来进行陀螺仪偏置估计，根据相机和IMU的平移量对齐来求取重力加速度和初始速度。

优选地，所述线特征的提取包括：

调整线特征提取算法的四个内部参数，包括：图像提取线段时的高斯金字塔层数、图像的尺度、线段支持域中像素点与主方向相近的像素点占总像素点的比值、线段支持域中像素点的角度阈值；

然后，通过线特征提取算法从双目相机中采集的图像中提取线段作为线特征。

优选地，所述线特征提取后根据线段的空间几何关系进行线段合并，包括：

当两条线段的中点距离和两条线段4个端点距离中的最小值小于设定距离阈值，并且两条线段的角度θ小于设定的角度阈值时，则合并两条线段，取两条线段中最远的两个端点作为合并后线段的两个端点。

优选地，所述线特征提取后对无效线段进行滤除，包括：

计算长度阈值L_min：

其中，min[W_I，H_I]表示当前图像长度和宽度的最小值，η表示长度比例因子，l_num表示当前图像提取的线段总数，n表示线段数量影响因子；

将当前图像中小于长度阈值的线段删除。

按照本发明的另一方面，提供了一种基于点线特征的双目视觉惯性定位系统，包括：

双目相机，用于采集图像；

IMU传感器，用于测量角速度和加速度数据；

特征提取和匹配模块，用于从图像中提取点特征和线特征，在机器人运行过程中对点特征和线特征进行追踪；

机器人位姿估计模块，用于将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，进行机器人位姿估计。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)相比点特征，线特征受环境光照影响相对较小，在特征点缺失的弱纹理环境下仍然可能提取到线特征来进行运动估计。本发明将线特征融合到惯性定位中，提高了移动机器人在弱纹理环境下的定位精度和鲁棒性。为了保留历史观测数据对当前状态估计的影响，加入先验约束，进一步提高了定位精度。

(2)通过最小化所有误差项的和可以得到最大后验估计，采用松耦合的方式来实现视觉与IMU轨迹的对齐，利用线特征为运动估计提供额外的约束，通过结合点特征误差项、线特征误差项和IMU误差项构建非线性最小二乘问题，通过IMU与双目相机紧耦合的方式来进行移动机器人位姿求解，从而解决了短时间内视觉特征丢失的问题，提高了移动机器人的定位精度。

(3)加入了回环检测，对滑动窗口内的关键帧进行选取和移除，在加快优化速度的同时，进一步提高定位精度。通过调整线特征提取算法内部参数来加快线段提取的速度。根据三维空间线段的空间几何关系进行相近线段的合并，采用设置距离阈值、角度阈值策略进行线段合并，采用长度阈值的策略来对无效线段进行滤除，从而减少了数据处理的时间，提高了机器人定位的实时性。

附图说明

图1是本发明实施例提供的一种基于点线特征的双目视觉惯性定位方法的流程图；

图2(a)是本发明实施例提供的中点距离示意图；

图2(b)是本发明实施例提供的端点距离示意图；

图2(c)是本发明实施例提供的角度阈值示意图；

图3为本发明实施例提供的滑动窗口中的约束关系因子图；

图4为本发明实施例提供的关键帧的选取和边缘化策略图；

图5为本发明实施例提供的硬件系统结构框图；

图6为本发明实施例提供的软件系统ROS节点图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，一种基于点线特征的双目视觉惯性定位方法，包括：

步骤1、点特征的提取与追踪、线特征的提取与追踪。

从相机采集的图像中提取FAST角点作为点特征，采用KLT光流跟踪算法来进行点特征的追踪。

采用线特征提取(LSD)算法从相机采集的图像中提取线段作为线特征，通过调整LSD算法的四个内部参数来加速线特征提取的过程，图像提取线段时的高斯金字塔层数、图像的尺度、线段支持域中像素点与主方向相近的像素点占总像素点的比值、线段支持域中像素点的角度阈值。相近表示像素点与主方向距离在阈值(优选为0.75)内的所有像素点，其中，高斯金字塔的层数如果选择较大，会加大线段提取的计算量，如果太小会影响选段提取的精度，本发明采用默认值设为1。像素的尺度是在进行线段提取时选取的图像尺度大小，这个值默认值为0.8，本发明设置为0.6，线段支持域中像素点与主方向相近的像素点占总像素点的比值默认值为0.7，本发明设为0.6，线段支持域中像素点的角度阈值默认为22.5，本发明设置为18.5。

根据线段的空间几何关系对相近的线段进行合并，并且对无效线段进行滤除。

采用角度阈值测量、距离阈值策略来合并相近的线段。当两条空间线段的距离和角度均小于一定阈值时，初步判断两条线段可以合并，然后通过判断描述子距离小于一定阈值来进行线段合并。

如图2(a)、2(b)和2(c)所示，当两条线段的中点距离d_mid和两条线段4个端点距离d₁₃、d₂₃、d₁₄、d₂₄中的最小值d_min小于设定距离阈值(本发明设置为1cm)并且两条线段的角度θ小于设定的角度阈值(本发明设为8度)时，则初步判断两条线段可以合并，然后通过LBD算法计算线段的描述子，通过判断描述子距离小于一定阈值(本发明设为20-30)来进行线段合并，如果可以合并，则取两条线段中最远的两个端点作为合并后线段的两个端点。

具体地，对于线特征，采取短线滤除的策略过滤掉较短的线段，保留匹配可能性较高的较长线段，线段长度选取策略公式为：

具体地，min[W_I，H_I]表示图像长度和宽度的最小值，η表示长度比例因子，l_num表示当前图像检测到的线段总数，n表示线段数量影响因子。本发明中η设置为0.125，为了防止线段数量过多时而滤除了较长的有效线段，当l_num＞300时，

设置为1，l_num＜300时，n设置300。

分别采用LBD算法和KNN(K邻近)算法来对线特征描述子计算和线段的追踪。

采用KNN算法来进行线段跟踪时，必须同时满足三个条件：①利用KNN算法进行匹配的线段之间的汉明距离必须超过25；②匹配线段之间的角度距离θ必须小于0.1弧度；③匹配线段之间的端点距离和终点距离的最小值d_min小于1cm。

步骤2、通过相机与IMU松耦合的方式来实现初始化。

IMU中包括陀螺仪和加速度计，陀螺仪测量三个方向的角速度，加速度计测量三个方向的加速度，IMU预积分为分别对三个方向的角速度和三个方向的加速度的进行积分，得到IMU位姿，并更新IMU位姿。但是该位姿的精度很低，误差很大，因此需要进行后续操作。

首先，通过三角测量和PnP算法求解相机初始位姿，采用IMU预积分来估计IMU初始位姿，将相机与IMU对齐，求解陀螺仪偏置、重力加速度和初始速度。利用相机和IMU的旋转量对齐来进行陀螺仪偏置估计，根据相机和IMU的平移量对齐来求取重力加速度和初始速度。

步骤3、位姿估计

为了保留历史观测数据对当前状态估计的影响，采用Schur算法加入先验约束因子。以先验约束、点特征追踪误差、线特征追踪误差和IMU测量误差之和最小为目标，构建最小二乘问题如下：

其中，{r_p，J_p}为先验约束，

为IMU测量误差项，

为点特征追踪误差项，

为线特征追踪误差项，待优化的变量χ为第i帧图像的位姿和线段的正交表示法坐标，r_p为先验误差，J_p为先验误差的雅可比矩阵，p表示先验信息，

为IMU的测量值，第二项和第四项中i为第i帧，b表示IMU，k表示点特征，β为机器人运动过程中采集的所有帧的IMU数据，第三项中(i，j)表示点的坐标，

为点特征追踪过程的测量值，c表示相机，L表示机器人运动过程中采集的所有的线特征，l为表示线特征，

为线特征追踪过程的测量值。ρ是鲁棒核函数，使用的Huber函数。先验约束为机器人历史运动轨迹的误差。

线特征追踪误差为线特征的重投影误差，定义为：在相机的像素平面中，匹配线段的两个端点到投影直线的垂直距离。

如图3所示，滑动窗口中的约束关系因子图包括点特征约束、线特征约束、IMU约束和先验约束，采用基于滑动窗口的优化算法来实现移动机器人的位姿估计，在滑动窗优化算法中，滑动窗口中只包含一定数量的关键帧，每次优化只考虑滑动窗口中的关键帧数据，当新的一帧来临时，将滑动窗口中旧的一帧数据移除，并将最新的关键帧加入滑动窗口中进行优化。

具体地，滑动窗口内需要考虑的待优化的变量包括：

其中，x_i表示的是第i帧数据(图像数据和IMU数据)中需要优化的IMU状态量，包括位置

方向

速度

以及陀螺仪偏置

和加速度计偏置

λ_k表示第k个特征点逆深度，

是第l个线特征的正交表示，n是IMU状态量在滑动窗口的起始索引，N是滑动窗口中待优化的关键帧数目，m是点特征的状态变量在滑动窗口的起始索引，M是滑动窗口中特征点的数目，o是线特征的状态变量在滑动窗口的起始索引，O是滑动窗口中线特征的数目，t_d为IMU和相机的时间偏差在线估计值。

将陀螺仪偏置、重力加速度和初始速度作为优化过程的初始值，基于滑动窗口的优化算法得到机器人位姿估计，即机器人的位置、速度和方向。

步骤4、回环检测

如图4所示，采用一种改进的关键帧选取和移除策略，并结合VINS-Mono的回环优化方法化来进行机器人位姿优化。

关键帧根据三个条件来选取：①当前帧与上一关键帧之间的匹配特征点数超过一定的阈值(本发明设置为30)；②当前帧与上一关键帧之间的IMU预积分平移量足够大(本发明设置为1cm)；③当前帧与上一关键帧之间的IMU预积分旋转量不能超过某一个阈值(本发明设置为10度)。当同时满足前面三个条件时，选取当前帧为最新关键帧进行回环检测，并将该关键帧插入滑动窗口中进行优化。

关键帧的移除策略为：如果当前关键帧与上一个关键帧之间匹配的特征点数量小于设定的阈值(本发明设置为20)，或者当前关键帧与上一个关键帧之间匹配的特征点平均移动的距离(视差)大于设定的阈值(本发明设置为10像素)，则说明最新的两个关键帧之间的共视点太少，此时，将滑动窗口中最老的关键帧进行边缘化并移除，否则，将上一个最近的关键帧边缘化后移除。

图5为本发明的硬件系统结构框图，本发明使用的硬件模块包括：轮式移动机器人、编码器、嵌入式控制板、ZED2双目相机、高精度IMU、TX2开发板、高性能计算平台(PC)。

具体地，本发明使用ZED2相机是一款内置高精度IMU测量单元的双目相机，最大深度测量距离可达20m，3米内测量误差小于1％，15米内测量误差小于5％，该相机的图像发布频率可选择，最高帧率可达100fps，IMU数据发布频率为400Hz。TX2开发板是一个轻量级的嵌入式系统开发平台，CPU为Arm Cortex-A57，内存为8G，32G eMMC硬盘，TX2主要用于传感器数据的预处理和数据转发。高性能计算平台的CPU型号为Intel Core i7-9750H，内存为16G，500G固体硬盘，主要用于运行SLAM算法。

具体地，嵌入式控制板主要用于移动机器人的数据采集和电机速度控制，选择的单片机为STM32F103RCT，电机速度的控制方式为脉冲序列控制。TX2开发板主要用于传感器数据的预处理和数据转发，ZED2相机(内置高性能IMU)通过USB3.0接口连接到TX2开发板上。由于TX2开发板同时进行传感器数据的处理和运行SLAM算法效果很差，因此，故本发明采用高性能计算平台用于运行ROS核心节点和SLAM算法，高性能计算平台和TX2开发板之间通过WIFI进行通讯。

图6为本发明的软件系统ROS节点图，TX2开发板运行的节点主要包括ROS Serial服务器节点(用于发布里程计数据)、ZED2相机节点(发布双目相机图像数据和IMU数据)、键盘控制节点(用于发布键盘速度控制指令)。高性能计算平台上运行的节点包括ROS Master节点、点特征提取跟踪节点、线特征提取跟踪节点、IMU标定补偿节点以及机器人状态估计节点。

具体地，ROS Master节点是核心节点，用于管理各节点之间的通讯，点特征提取跟踪节点实现了点特征提取和追踪相关的算法，线特征提取跟踪节点实现了线特征提取和追踪相关的算法，IMU标定补偿节点实现了IMU预积分相关的算法。机器人状态估计节点接收其他节点发布的数据以紧耦合的方式进行数据融合，通过滑动窗口优化算法进行机器人位姿求解，从而实现对移动机器人的状态估计。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于点线特征的双目视觉惯性定位方法，所述方法通过在机器人上设置双目相机和IMU传感器估计机器人位姿，进行机器人定位，其特征在于，所述机器人位姿估计包括如下步骤：

(1)从双目相机中采集的图像中提取点特征和线特征，在机器人运行过程中对点特征和线特征进行追踪；

(2)将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，进行机器人位姿估计，得到机器人位姿。

2.如权利要求1所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述步骤(2)包括：

将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，构建最小二乘问题；

通过滑动窗口优化最小二乘问题，在优化过程中通过双目相机与IMU松耦合的方式来实现初始化，最终优化得到机器人位姿。

3.如权利要求1或2所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述线特征追踪误差为：在双目相机的像素平面中，线特征追踪过程中匹配线段的两个端点到投影直线的垂直距离。

4.如权利要求2所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述方法还包括：

若当前帧同时满足三个条件，选取当前帧为最新关键帧进行回环检测，并将该关键帧插入滑动窗口中进行优化，由此对机器人位姿进行优化；三个条件为：当前帧与上一关键帧之间的匹配特征点数大于第一预设值、当前帧与上一关键帧之间的IMU预积分平移量大于第二预设值、当前帧与上一关键帧之间的IMU预积分旋转量小于第三预设值。

5.如权利要求2所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述方法还包括：

若当前关键帧与上一关键帧之间匹配的特征点数量小于第一设定值，或者当前关键帧与上一关键帧之间匹配的特征点平均移动的距离大于第二设定值，则将滑动窗口中第一个处理的关键帧进行边缘化后移除，否则，将最新一个处理的关键帧边缘化后移除。

6.如权利要求2所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述松耦合的具体实现方式为：

利用双目相机和IMU的旋转量对齐来进行陀螺仪偏置估计，根据相机和IMU的平移量对齐来求取重力加速度和初始速度。

7.如权利要求1或2所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述线特征的提取包括：

调整线特征提取算法的四个内部参数，包括：图像提取线段时的高斯金字塔层数、图像的尺度、线段支持域中像素点与主方向相近的像素点占总像素点的比值、线段支持域中像素点的角度阈值；

然后，通过线特征提取算法从双目相机中采集的图像中提取线段作为线特征。

8.如权利要求7所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述线特征提取后根据线段的空间几何关系进行线段合并，包括：

当两条线段的中点距离和两条线段4个端点距离中的最小值小于设定距离阈值，并且两条线段的角度θ小于设定的角度阈值时，则合并两条线段，取两条线段中最远的两个端点作为合并后线段的两个端点。

9.如权利要求7所述的一种基于点线特征的双目视觉惯性定位方法，其特征在于，所述线特征提取后对无效线段进行滤除，包括：

计算长度阈值L_min：

其中，min[W_I，H_I]表示当前图像长度和宽度的最小值，η表示长度比例因子，l_num表示当前图像提取的线段总数，n表示线段数量影响因子；

将当前图像中小于长度阈值的线段删除。

10.一种基于点线特征的双目视觉惯性定位系统，其特征在于，包括：

双目相机，用于采集图像；

IMU传感器，用于测量角速度和加速度数据；

特征提取和匹配模块，用于从图像中提取点特征和线特征，在机器人运行过程中对点特征和线特征进行追踪；

机器人位姿估计模块，用于将机器人历史运动轨迹的误差作为先验约束，以先验约束、点特征追踪误差、线特征追踪误差和IMU传感器测量误差之和最小为目标，进行机器人位姿估计。

Images