室内弱纹理环境下移动无人系统同步定位与建图方法

摘要

本发明公开了一种室内弱纹理环境下移动无人系统同步定位与建图方法，包括提取、跟踪图像的点特征与LSD线特征，并基于所提取的线特征进一步提取消隐点以对所提取的线特征进行分组，使用紧耦合的方式融合视觉与惯性信息，构建基于滑动窗口的非线性优化后端，将点特征、线特征的重投影误差、消隐点残差和IMU测量误差共同纳入滑动窗口中；本发明引入了线特征并基于消隐点对线特征分组，得到具有方向信息的结构线条，还将线特征观测残差与消隐点残差纳入后端非线性优化框架中，解决了基于点特征的视觉惯性slam系统易跟踪丢失、稳定性差的问题，具有在室内弱纹理环境中定位精度高、鲁棒性强的优势。

权利要求

1.一种室内弱纹理环境下移动无人系统同步定位与建图方法，其特征在于，包括以下步骤：S1：获取来自相机的图像信息与来自IMU的环境信息作为输入信息；

S2：对来自相机的图像进行点线特征的检测与跟踪，引入线段间的距离信息，筛选出割裂线段并进行合并；

S3：针对所筛选出的线特征，更进一步提取消隐点，并根据消隐点对线特征分组；

S4：用紧耦合的方法融合图像与IMU信息，根据光束平差法构建基于滑动窗口的非线性优化后端，绘制并优化位姿与地图中的点线；

所述步骤S4具体为：

应用光束平差法建立基于滑动窗口的视觉‑惯性非线性优化框架，在此使用线的正交化表达方式，构建世界坐标系内的全局状态向量 为：其中， 表示第i帧IMU预积分后在世界坐标系下提供的信息，包括：位置 ，旋转四元数 ，速度 ，加速度计零偏 ，陀螺仪零偏 ； 表示空间内视觉点特征的逆深度，表示视觉线特征的正交化坐标；I表示滑动窗口的数量，J表示点特征点的数量，K表示线特征的数量；

所述基于滑动窗口的非线性优化方式的目标函数为：

其中， 为全局状态向量， 表示来自边缘化的先验信息， ，， ， 分别代表IMU残差，点特征残差，线特征残差以及消隐点残差； ， ， ， 分别代表IMU、点特征、线特征、消隐点测量的协方差矩阵； ，， ，分别代表点特征、线特征、消隐点特征测量值的鲁棒核函数；P，L，V分别代表被观测帧中点特征、线特征及消隐点测量值的集合， B为滑动窗口中所有预积分的IMU测量值的集合；

其中，消隐点残差 定义为：

其中， 表示消隐点的观测值； 消隐点的估计值，为 与图像平面的交点，而 为与被估计线特征平行且过相机中心的线。

2.根据权利要求1所述的一种室内弱纹理环境下移动无人系统同步定位与建图方法，其特征在于：所述步骤S1具体为：将相机获取的图像和IMU获取的环境信息作为输入信息，其中，相机和IMU是并行运行的，分别提取出图像信息和环境测量信息将两者用紧耦合的方式综合在一起作为输入信息。

3.根据权利要求1所述的一种室内弱纹理环境下移动无人系统同步定位与建图方法，其特征在于：所述步骤S2具体为：S21：根据输入的图像信息进行点线特征的提取与跟踪，通过Shi‑Tomasi角点提取点特征，使用KLT跟踪，利用RANSAC进行内嵌修正；

S22：选用LSD算法提取线特征，并根据线段的方向和距离合并被分割的小线段，最小化将一条线分割成不同的多个线段的影响；

S23：接着用LBD描述子描述提取的线特征，进行匹配跟踪。

4.根据权利要求1所述的一种室内弱纹理环境下移动无人系统同步定位与建图方法，其特征在于：所述步骤S3具体为：S31：构建极坐标网格，以图像的像主点为中点构建一个极坐标网格，然后计算出线段对每个子网格的响应度，并将其存储在每个网格中；

S32：生成消隐点假设，利用基于2条线段的最小解集生成第一个消隐点v1，然后在第一个消隐点v1对应的等效球面的圆弧上利用均匀采样的方法来得到第二个消隐点v2，最后利用第一个消隐点v1和第二个消隐点v2正交得到第三个消隐点v3；

S33：消隐点假设验证，所有在步骤S32中得到的消隐点假设，将其投影到图像平面上并找到其对应的极坐标网格，然后选取其中响应度之和最大的消隐点假设作为最后的结果；

S34：得到消隐点后通过计算每条线段与三个消隐点方向的夹角，取夹角最小的消隐点作为该线段的结构方向，实现结构线条的提取。

说明书

室内弱纹理环境下移动无人系统同步定位与建图方法

技术领域

[0001] 本发明提供了一种室内弱纹理环境下移动无人系统同步定位与建图方法，属于移动无人系统自主定位导航技术应用领域。

背景技术

[0002] 在实现无人系统自动化的过程中，其中一大关键要素便是无人系统能够安全地在环境中移动并达到给定目标点。为了实现这个目标，就需要解决无人系统领域中的定位、地图构建和导航问题。实时估计无人系统自身在环境中的位姿即定位问题，根据位姿对周围环境进行建模即地图构建问题。在已知其中一个的前提下，求解另一个问题往往比较容易。然而在未知环境中，无人系统既没有先验地图来完成定位，也没有精确的位姿来完成建图，就需要采用同步定位与地图构建（Simultaneous Localization and Mapping）SLAM技术，它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。

[0003] 在SLAM问题中，无人系统位姿估计和环境构建均依赖传感器的测量信息，并根据不同的运行环境采用不同的传感器。各种传感器在定位和导航领域均有着广泛的应用，视觉传感器可以提供较精确的环境观测信息，但存在尺度模糊，快速运动或光强变化大环境测量不稳定的缺陷。IMU可以提供自身三轴线加速度和角速度信息，但通过连续积分估计自身姿态，随着时间增长，累积误差会快速增长。图像信息可以有效校正惯性导航中的漂移问题，惯性导航信息可以弥补图像测量尺度模糊及弱纹理环境误差较大的问题。

[0004] 为了获得更高的定位精度，将视觉传感器与IMU融合是一种有效的途径。根据对图像的处理方式不同，可以分为直接法和特征点法。特征点法又被称为间接法，最早源于经典SLAM中的特征跟踪算法，其主要提取图像中具有不变性质的一些特征，通过特征匹配、特征跟踪等过程，计算特征逆投影误差，在相应的几何约束下实现位姿的估计。直接法从光流法演变而来，其基本假设是相同场景的光强具有一致性。它采用图像所有的像素信息，通过最小化光度误差来估计相机的位姿变化。

[0005] 目前主流的视觉 SLAM 包括视觉惯性融合的 SLAM 系统都是采用提取图像中的特征点作为视觉观测，而当环境纹理信息不丰富时则无法提取足够多准确可靠的特征点，SLAM 系统的定位效果就会大大降低，甚至直接造成系统定位失败。

发明内容

[0006] 多数视觉惯性融合系统仅将点特征作为视觉信息，在快速运动、弱纹理、弱照明及光照变化明显等场景中鲁棒性差，而线特征则在以上场景具有良好表现，且基于线特征构建的三维空间地图具有较好的结构性，可为之后导航提供语义信息。有鉴于此，本发明旨在提出一种面向室内弱纹理环境的基于点线特征的视觉惯性同步定位与建图方法，以能够在弱纹理场景下依然鲁棒且保证高定位精度。

[0007] 为了实现上述目的，本发明提供的技术方案是：

[0008] 一种室内弱纹理环境下移动无人系统同步定位与建图方法，包括以下步骤：

[0009] S1：获取来自相机的图像信息与来自IMU的环境信息作为输入信息；

[0010] S2：对来自相机的图像进行点线特征的检测与跟踪，引入线段间的距离信息，筛选出割裂线段并进行合并；

[0011] S3：针对所筛选出的线特征，更进一步提取消隐点，并根据消隐点对线特征分组；

[0012] S4：用紧耦合的方法融合图像与IMU信息，根据光束平差法构建基于滑动窗口的非线性优化后端，绘制并优化位姿与地图中的点线。

[0013] 作为优选的一种技术方案，所述步骤S1具体为：

[0014] 将相机获取的图像和IMU获取的环境信息作为输入信息，其中，相机和IMU是并行运行的，分别提取出图像信息和环境测量信息将两者用紧耦合的方式综合在一起作为输入信息。

[0015] 作为优选的一种技术方案，所述步骤S2具体为：

[0016] S21：根据输入的图像信息进行点线特征的提取与跟踪，通过Shi‑Tomasi角点提取点特征，使用KLT跟踪，利用RANSAC进行内嵌修正；

[0017] S22：选用LSD算法提取线特征，并根据线段的方向和距离合并被分割的小线段，最小化将一条线分割成不同的多个线段的影响；

[0018] S23：接着用LBD描述子描述提取的线特征，进行匹配跟踪。

[0019] 作为优选的一种技术方案，所述步骤S3具体为：

[0020] S31：构建极坐标网格，以图像的像主点为中点构建一个极坐标网格，然后计算出线段对每个子网格的响应度，并将其存储在每个网格中；

[0021] S32：生成消隐点假设，利用基于2条线段的最小解集生成第一个消隐点v1，然后在第一个消隐点v1对应的等效球面的圆弧上利用均匀采样的方法来得到第二个消隐点v2，最后利用第一个消隐点v1和第二个消隐点v2正交得到第三个消隐点v3；

[0022] S33：消隐点假设验证，所有在步骤S32中得到的消隐点假设，将其投影到图像平面上并找到其对应的极坐标网格，然后选取其中响应度之和最大的消隐点假设作为最后的结果；

[0023] S34：得到消隐点后通过计算每条线段与三个消隐点方向的夹角，取夹角最小的消隐点作为该线段的结构方向，实现结构线条的提取。

[0024] 作为优选的一种技术方案，所述步骤S4具体为：

[0025] 应用光束平差法建立基于滑动窗口的视觉‑惯性非线性优化框架，在此使用线的正交化表达方式，构建世界坐标系内的全局状态向量 为：

[0026]

[0027]

[0028] 其中， 表示第i帧IMU预积分后在世界坐标系下提供的信息，包括：位置 ，旋转四元数 ，速度 ，加速度计零偏 ，陀螺仪零偏 ； 表示空间内视觉点特征的逆深度， 表示视觉线特征的正交化坐标；I表示滑动窗口的数量，J表示点特征点的数量，K表示线特征的数量；

[0029] 所述基于滑动窗口的非线性优化方式的目标函数为：

[0030]

[0031] 其中， 为全局状态向量， 表示来自边缘化的先验信息， ，， ， 分别代表IMU残差，点特征残差，线特征残差以及消隐点
残差； ， ， ， 分别代表IMU、点特征、线特征、消隐点测量的协方差矩阵； ，， ，分别代表点特征、线特征、消隐点特征测量值的鲁棒核函数；P，L，V分别代表被观测帧中点特征、线特征及消隐点测量值的集合， B为滑动窗口中所有预积分的IMU测量值的集合；

[0032] 其中，消隐点残差 定义为：

[0033]

[0034] 其中， 表示消隐点的观测值； 消隐点的估计值，为 与图像平面的交点，而 为与被估计线特征平行且过相机中心的线。

[0035] 本发明相对于现有技术的有益效果为：

[0036] 1）多数视觉惯性融合系统仅将点特征作为视觉信息，在快速运动、弱纹理、弱照明及光照变化明显等场景易跟踪丢失、稳定性差，本发明引入了直线特征，在以上场景表现良好，鲁棒性强，不会发生特征跟踪丢失的情况，且基于线特征构建的三维空间地图具有较好的结构性，可为之后导航提供语义信息；

[0037] 2）本发明在使用LSD检测线后，引入线之间的距离信息合并被割裂的小线段，提高了线段检测的精度，并在提取线条的基础上进一步提取消隐点对线条分类以提取结构线条，结构线条带有很强的全局方向的约束，利用这个先验信息纠正相机位姿的估计，整体上提高定位精度；

[0038] 3）本发明在非线性优化后端定义并加入了消隐点残差，进一步提高总体的定位精度和鲁棒性。

附图说明

[0039] 为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

[0040] 图1是本发明所提出的一种室内弱纹理环境下移动无人系统同步定位与建图方法框架图；

[0041] 图2是本发明线段合并示意图；

[0042] 图3是本发明消隐点提取示意图；

[0043] 图4是本发明后端优化的因子图模型；

[0044] 图5是本发明消隐点残差定义说明图。

具体实施方式

[0045] 以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。

[0046] 图1是本发明实施例提供的一种室内弱纹理环境下移动无人系统同步定位与建图方法框架图，如图所示，本发明实施例提供的一种室内弱纹理环境下移动无人系统同步定位与建图方法包括：

[0047] S1：将来自相机的图像信息与来自IMU的环境信息作为输入信息；

[0048] 具体的，在本实施例中，将单目相机获取的图像和IMU获取的环境信息作为输入信息，相机和IMU是并行运行的，分别提取出图像信息和环境测量信息将两者用紧耦合的方式综合在一起作为输入信息。

[0049] S2：对来自相机的图像进行点线特征的检测与跟踪，引入线段间的距离信息，筛选出割裂线段并进行合并；

[0050] 具体的，在本实施例中，根据输入的图像信息进行点线特征的提取与跟踪，通过Shi‑Tomasi角点提取点特征，使用KLT跟踪，利用RANSAC进行内嵌修正。选用精度较高且实时性强的LSD算法提取线特征，并根据线段的方向和距离合并本应属于一条线却被分割的小线段，最小化将一条线分割成不同的多个线段的影响。接着用LBD描述子描述提取的线特征，进行匹配跟踪。如图2所示，l表示两线段端点之间的最小距离，d表示一条线段的中点到另一条线段的距离。如果d和l小于给定的阈值，则将这两条线段合并。接着用LBD描述子描述提取的线特征，并用KNN匹配。

[0051] S3：针对所提取的线特征，更进一步提取消隐点，并根据消隐点对线特征分组；

[0052] 具体的，根据所提取的线特征进一步提取消隐点，图3为本发明消隐点提取示意图，提取流程如下：

[0053] 1）构建极坐标网格，以图像的像主点为中点构建一个极坐标网格，然后计算出线段对每个子网格的响应度，并将其存储在每个网格中。

[0054] 2）生成消隐点假设，利用基于2条线段的最小解集生成第一个消隐点v1，然后在v1对应的等效球面的圆弧上利用均匀采样的方法来得到第二个消隐点v2，最后利用v1和v2正交得到第三个消隐点v3。

[0055] 3）消隐点假设验证，所有在第二步中得到的消隐点假设，将其投影到图像平面上并找到其对应的极坐标网格，然后选取其中响应度之和最大的消隐点假设作为最后的结果。

[0056] 得到消隐点后通过计算每条线段与三个消隐点方向的夹角，取夹角最小的消隐点作为该线段的结构方向，实现结构线条的提取。

[0057] S4：用紧耦合的方法融合图像与IMU信息，根据光束平差法（BA）构建基于滑动窗口的非线性优化后端，提出一种消隐点残差的定义方式并加入后端优化框架之中，优化位姿与地图中的点线。如图4所示，为本发明后端优化的因子图模型，在图4中， 为非结构线，则只添加线特征因子， 与 为结构线，则同时添加线特征因子与消隐点因子。

[0058] 应用光束平差法（BA）建立基于滑动窗口的视觉‑惯性非线性优化框架，在此使用线的正交化表达方式，构建世界坐标系内的全局状态向量为:

[0059]

[0060]

[0061] 其中， 表示第i帧IMU预积分后在世界坐标系下提供的信息，包括：位置 ，旋转四元数 ，速度 ，加速度计零偏 ，陀螺仪零偏 ； 表示空间内视觉点特征的逆深度， 表示视觉线特征的正交化坐标；I表示滑动窗口的数量，J表示点特征点的数量，K表示线特征的数量；

[0062] 所述基于滑动窗口的非线性优化方式的目标函数为：

[0063]

[0064] 其中， 为全局状态向量， 表示来自边缘化的先验信息， ，， ， 分别代表IMU残差，点特征残差，线特征残差以及消隐点
残差； ， ， ， 分别代表IMU、点特征、线特征、消隐点测量的协方差矩阵； ，， ，分别代表点特征、线特征、消隐点特征测量值的鲁棒核函数；P，L，V分别代表被观测帧中点特征、线特征及消隐点测量值的集合， B为滑动窗口中所有预积分的IMU测量值的集合。

[0065] 如图5所示为本发明消隐点残差定义的示意图，其中·—线代表被观测到的线特征及与其平行且过相机中心的线，——线代表估计的线特征及与其平行且过相机中心的线， 为与被观测到的线特征平行且过相机中心的线与图像平面的交点，代表消隐点的观测值。消隐点残差 定义为：

[0066]

[0067] 其中， 表示消隐点的观测值； 消隐点的估计值，为 与图像平面的交点，而 为与被估计线特征平行且过相机中心的线。

[0068] 上述方法通过加入了线特征作为视觉观测，在弱纹理室内场景中保持高定位精度和强鲁棒性，且基于线特征构建的三维空间地图具有较好的结构性，可为之后导航提供语义信息。

[0069] 另外，本发明在使用LSD检测线后，引入线之间的距离信息合并被割裂的小线段，提高了线段检测的精度，并在提取线条的基础上进一步提取消隐点对线条分类以提取结构线条，结构线条带有很强的全局方向的约束，利用这个先验信息纠正相机位姿的估计，整体上提高定位精度。

[0070] 最后，在非线性优化后端定义并加入了消隐点残差，进一步提高总体的定位精度和鲁棒性。

[0071] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。