CN113345018B - 一种动态场景下的激光单目视觉融合定位建图方法 - Google Patents

Abstract

本发明公开了一种动态场景下的激光单目视觉融合定位建图方法，该方法包括：步骤1，检测动态障碍物信息；步骤2，获取相对位姿，并输出满足要求的关键帧信息；步骤3，将关键帧插入到共视图中，生成新地图点，并优化关键帧的位姿和地图点；步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。本发明能够在动态场景中执行SLAM目的。

Description

一种动态场景下的激光单目视觉融合定位建图方法

技术领域

本发明涉及面向自动驾驶领域的激光单目视觉融合定位建图方法，特别是关于一种动态场景下的激光单目视觉融合定位建图方法。

背景技术

随着自动化、人工智能和机器人等技术的飞速发展，实际生活和工业场景中应用了越来越多具有感知、定位和导航功能的智能无人系统，例如扫地机器人、仓储机器人及无人驾驶出租车等。随着传感器技术及智能算法的不断迭代，这些无人系统的应用场景已经从简单的已知环境，扩展到完全未知的复杂环境。在未知环境中，机器人需要用来自传感器的信息来感知周围环境以及估计自身状态，以保证其能够在复杂未知环境中自主运动。因此，机器人在未知环境中将面临三个问题：1)我在哪？2)我周围都有什么？3)我要去哪里？这三个问题分别对应为：1)机器人的自主定位问题；2)机器人的地图构建问题；3)机器人的实时避障及路径规划问题。

SLAM(英文全称为：Simultaneous Localization and Mapping，中文全称为：定位与建图)是利用机器人自身传感器感知和定位到的周围环境信息构建环境地图的技术，其为前两个问题提供了答案，从而为机器人在未知环境中自主运动提供了基础保证。SLAM可以依据采集信息使用的传感器不同，可分为激光SLAM和视觉SLAM。其中，激光SLAM存在点云数据在垂直方向上稀疏性造成的俯仰角估计不准的问题，同时激光雷达所获取的环境信息也不如视觉传感器丰富，相对而言，在回环检测的效率上不及视觉传感器。视觉SLAM由于具有相对丰富的环境信息以及垂直方向上结构化数据的密集性，可在一定程度弥补上述问题缺陷。目前，绝大多数视觉SLAM问题的研究基于假设周围环境为静态，在动态场景下的激光单目视觉融合定位建图方法仍不成熟，相关理论有待进一步完善。

发明内容

本发明的目的在于提供一种动态场景下的激光单目视觉融合定位建图方法来克服或至少减轻现有技术的上述缺陷中的至少一个。

为实现上述目的，本发明提供一种动态场景下的激光单目视觉融合定位建图方法，该方法包括：

步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出；

步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜，在图像掩膜上提取ORB特征点及其与上一帧ORB特征点的匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；

步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；

步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。

进一步地，步骤1具体包括：

步骤11，将激光雷达输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云；

步骤12，将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇；

步骤13，估计步骤12获得的每个障碍物的位置、形状和尺寸信息；

步骤14，根据多帧步骤13估计得到的每个障碍物的差异程度计算差异度函数，建立关联矩阵，将多帧点云数据中障碍物关联起来；

步骤15，根据步骤14关联的障碍物情况，以及上一帧视觉里程计计算得到的两帧点云数据相对运动，判断障碍物运动状态，区分出动态障碍物和静态障碍物。

进一步地，步骤11具体包括：

步骤111，将点云数据按照空间位置信息放入预设定大小的栅格中，假设每个栅格立方体的边长为d₀，分别按照雷达坐标系的x、y、z三个维度进行分割，获得m×n×l个栅格，具体如下式(1)所示：

式中，ceil[·]表示向上取整函数，x_max表示所有点云在x维度上的最大值，x_min表示所有点云在x维度上的最小值，y_max表示所有点云在y维度上的最大值，y_min表示所有点云在y维度上的最小值，z_max表示所有点云在z维度上的最大值，z_min表示所有点云在z维度上的最小值；

故三维空间中任意一点p_w(x,y,z)∈P所在的栅格索引用下式(2)进行计算：

式中，floor[·]表示向下取整函数，x、y、z分别表示p_w(x,y,z)∈P的相应坐标值，index_x表示栅格中x维度的栅格索引，index_y表示栅格中y维度的栅格索引，index_z表示栅格中z维度的栅格索引；

步骤112，以z轴方向上的l个栅格为单位进行遍历，即遍历俯视方向上的m×n列栅格，将每列栅格中自下而上(z轴正方向)的第一个具有点云栅格作为地面点栅格，并将该栅格中点云清空；遍历完所有列栅格后，便得到了非地面点云。

进一步地，步骤12通过如下步骤实现：

步骤121，从步骤11输出的所有非地面点云取出一个点云作为聚类种子点，并对其进行邻域搜索，若在邻域半径ρ内存有点云，则将邻域搜索到的点云存入聚类生长点队列

步骤122，遍历

中的每个点云，进行半径阈值为ρ的邻域搜索，若邻域内存在点云，则将不属于

的点云存入聚类生长过渡队列

按照相同的方法遍历

中的所有点云，将

中点云存入第k帧点云数据中第n类障碍物的聚类容器

再将

中所有点云存入

直至某次生长时

为空集，此时该类别聚类结束；

重复步骤121和步骤122，直至处理完毕步骤11输出的所有非地面点云，得到所有障碍物聚类容器

并将所有障碍物信息中不满足足够阈值数量的类别进行剔除。

进一步地，步骤13通过如下步骤实现：

步骤131，将步骤12得到的每一个障碍物点云簇的质心定义为该障碍物的坐标；

步骤132，将点云投影至鸟瞰图后的障碍物进行随机采样一致性直线分割，将分割出的直线方向作为该障碍物主方向；

步骤132，利用步骤132确定的主方向建立二维坐标系，再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x′、y′两个维度上的极大、极小值，最后将极大值与相应的极小值的差值作为二维包围框的长和宽。

进一步地，步骤14通过如下步骤实现：

步骤141，根据第k帧第u个障碍物位置为

第k-1帧第v个障碍物位置为

障碍物的长和宽分别表示

通过观测多帧点云数据构建车辆匀加速运动模型，进而预测得到的当前帧相对于上一帧运动为

则差异度函数定义为式(3)：

式(3)中，α为距离差异权重,β为长宽比差异权重，γ为高度差异权重,均取经验值；

步骤142，根据P_k-1中共检测到的障碍物的总数量为m个，记为

，其中u＝1,2,…,m；P_k中共检测到的障碍物的总数量为n个，记为

其中，v＝1,2,…,n，将关联矩阵

表示为下式(4)：

式中，

表示由式(3)计算的差异度，其中的每一行的第二个下标相同；

步骤143，关联当前帧P_k和上一帧P_k-1中的障碍物；

重复上述步骤，直至当前帧P_k的关联矩阵的每一行对应的障碍物关联完毕。

进一步地，步骤2具体包括：

步骤21，利用式(6)将步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜；

式中，

为像素坐标的齐次坐标形式，即

K为单目视觉2内参矩阵；

为雷达坐标系到相机坐标系的变换矩阵；

为点云在雷达坐标系中的齐次坐标形式，即

L表示雷达坐标系，C表示相机坐标系，I表示惯性坐标系；

步骤22，在步骤21中形成的图像掩膜的无效像素之外的区域提取ORB特征点；步骤22具体包括：

步骤221，在图像数据中划分栅格，在栅格内提取固定数量的ORB特征点；

步骤222，对提取出的ORB特征点进行四叉树划分，将其不断分裂，直至四叉树节点数量不小于需要提取的ORB特征点数量，并在四叉树产生的每个节点中只保留响应值最大的特征点；

步骤23，匹配相邻两帧图像数据的ORB特征点；

步骤24，选取ORB特征点周围最近的三个不共线的点云，进行平面拟合，并计算该拟合平面与单目视觉光心到被选的ORB特征点射线的交点，此交点的深度作为ORB特征点的深度，此交点记为该ORB特征点生成的地图点；

步骤25，获取当前帧与上一帧的相对位姿，其具体包括如下过程：

步骤251，定义步骤23匹配后得到的相邻两帧ORB特征点生成的地图点质心：

式中，

为第k帧ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

为第k-1帧ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

表示P_k中的第w个ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

表示P_k-1中的第w个ORB特征点生成的地图点质心在雷达坐标系L中的坐标，n表示成功匹配且生成地图点的ORB特征点总数目；

步骤252，将去质心坐标定义为

获得由式(7)表示的矩阵W，其为一个3×3矩阵：

步骤253，对W进行SVD分解，得到式(8)：

W＝UΣV^T (8)

式中，Σ为奇异值对角矩阵，U和V为正交矩阵。当W满秩时，旋转矩阵R_k表示为式(9)：

若此时

行列式为负，则取

为最优值；

步骤254，根据

按照

计算当前帧图像数据相对于上一帧图像数据的平移量。

进一步地，步骤2具体还包括：

步骤26，判断局部地图中是否存在地图点，若判定为是，则执行步骤27，反之，进入步骤28；

步骤27，将与当前帧在共视图中相邻的关键帧的地图点采用式(6)的投影方法投影到当前帧中，重复步骤22至步骤25，从而再次获取相对位姿，在增加约束的基础上进一步得到更精确的位姿；

步骤28，判断从步骤26或步骤27中处理后的当前帧是否同时满足以下两个条件，如果是，则作为关键帧插入局部地图模块中进行优化处理；

条件一，当前帧跟踪到的特征点数量大于预设数量；

条件二，距离上次关键帧插入至少经过预设帧数或两个关键帧的平移超过一定阈值。

进一步地，步骤3具体包括：

步骤31，将步骤2输出的关键帧信息插入到共视图中，并根据地图点共视程度更新此关键帧与其它关键帧的连接关系，同时更新生长树和词袋模型；

步骤32，判断当前关键帧与上一关键帧产生的平移是否满足三角化条件，若是，则进入行步骤33；否则，则进入步骤34；

步骤33，将能在至少3帧中跟踪到的ORB特征点进行融合，并根据三角化生成新地图点；

步骤34，局部BA优化地图：根据当前关键帧的本质图，找到其它相邻关键帧及地图点，构建非线性优化问题(10)，获得优化位姿ξ^*和地图点

其中，

为像素坐标的齐次坐标形式，即

K为单目视觉内参矩阵，

为地图点坐标的齐次坐标形式，即

z_w为地图点深度；ξ为单目视觉位姿的李代数；ξ^*为单目视觉位姿的李代数的最优估计值，

为地图点坐标的齐次坐标形式的最优估计值，ξ^为ξ的反对称矩阵；

步骤35，判断步骤34优化后的关键帧超过设定数量的ORB特征点是否能被至少其它三个关键帧观测到，若是，则判定当前关键帧为冗余关键帧，从共视图中删除冗余关键帧，同时更新生长树和词袋模型，并删除属于该关键帧的地图点，随后在局部地图中删除步骤31的冗余关键帧。

进一步地，步骤4具体包括：

步骤41，利用离线训练的词袋模型对步骤3输出的每个关键帧进行图像语义描述，并与当前关键帧进行相似度计算，相似度达到一定阈值则认为出现回环，相似度计算方法如下式(16)：

其中，v_r＝{(w₁,η₁),(w₂,η₂),…,(w_N,η_N)}，w_N为词袋模型叶子节点，η_N为相应权重，v_r为描述第r个关键帧的词袋模型向量，v_o为述第o个关键帧的词袋模型向量；

步骤42，出现回环后，首先替换或填补当前关键帧与检测出来回环关键帧中存在冲突的地图点，随后在本质图上将当前关键帧与回环关键帧连接，更新本质图；

步骤43，根据步骤42产生的回环信息，进行全局BA，同时优化回环中的关键帧位姿和关键帧位姿中的地图点，获得优化后的关键帧位姿、特征点地图和点云地图。

本发明由于采取以上技术方案，其具有以下优点：

1.本发明采用视觉与激光融合的前端里程计弥补了激光里程计在俯仰角上估计不准的问题；

2.本发明使用点云数据进行深度估计可以得到比双目视觉更大的深度估计范围，从而增加SLAM系统的可靠性；

3.由于本发明将已知特征点匹配关系与特征点深度，可直接通过求解析解的方式得出位姿信息，相比常用的非线性优化技术，具有更快的位姿求解速度；

4.相对于深度学习技术，本发明使用点云数据的空间信息进行障碍物检测可以在降低计算量前提下具有更强的新环境鲁棒性；

5.相较于没有里程计信息的障碍物状态估计功能，本发明包含里程计信息的动态障碍物运动估计更加准确；

6.本发明使用了基于动态障碍物点云数据的Graham扫描法生成最小凸包，在此基础上进行的SLAM精度更高。

附图说明

图1是本发明实施例提供的硬件拓扑示意图。

图2是本发明实施例提供的动态场景下的激光单目视觉融合定位建图方法的原理性框图。

图3是图2中的动态障碍物检测方法的流程图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明所涉及的自动驾驶硬件包括激光雷达1、单目视觉2以及计算单元3。其中，激光雷达1用于检测动态障碍物点云数据，为单目视觉2提供绝对尺度下的深度先验信息，其对应为下文中的雷达坐标系。单目视觉2作为定位建图系统的主体传感器，用于采集动态场景的图像数据，其可以结合点云数据，用于动态场景下的定位建图，其对应为下文中的相机坐标系。计算单元3用于利用点云数据的空间信息与视觉里程计的位姿信息进行障碍物检测与状态估计，区分出动态障碍物，并根据动态点云数据在图像上的分布形成掩膜，在掩膜的基础上进行视觉定位建图。

在一个实施例中，如图2所示，计算单元3具体按照如下步骤进行动态场景下的激光单目视觉融合定位建图，每一个步骤占一个线程，通过采用多线程并行计算，可以获得更高的数据处理频率。

步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出。

步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜，在图像掩膜上提取ORB(英文全称为：Oriented Fast and Rotated Brief)特征点及其与上一帧ORB特征点的匹配，再估计ORB特征点的深度，通过位姿解算最后获取相对位姿，并输出满足要求的关键帧信息。其中，相对位姿可以理解为两帧间的相对位姿，其中大地坐标系固定为第一帧的位姿。关键帧信息包括点云数据、图像数据、特征点、地图点和位姿等信息，其中关键帧的位姿一般指的是绝对位姿，具体可以默认为单目视觉2的位置。

步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点。其中，本质图由所有关键帧根据相对位置关系连接组成。

步骤4，判断局部地图中的每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA(Bundle Adjustment，光束法平差)，获得优化后的关键帧位姿、特征点地图和点云地图。

在一个实施例中，步骤1具体包括：

在一个实施例中，点云数据由图1中示出的激光雷达1获得，步骤1具体包括：

步骤11，地面分割：将激光雷达1输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云。通过去除地面点，将地面点与非地面点进行分割，避免障碍物之间由于地面点云数据造成牵连和噪声干扰。

理想状态下，地面点云一般为激光雷达视场范围内的最低点云，但由于在没有标定激光雷达与车体坐标系外参的前提下，激光点云数据可能出现一定俯仰，使得地面不与雷达坐标系平行。鉴于此，步骤11具体包括：

步骤111，栅格化：将点云数据按照空间位置信息放入预设定大小的栅格中，假设每个栅格立方体的边长为d₀，分别按照雷达坐标系的x、y、z三个维度进行分割，则可以获得m×n×l个栅格，具体如下式(1)所示：

式中，ceil[·]表示向上取整函数，x_max表示所有点云在x维度上的最大值，x_min表示所有点云在x维度上的最小值，y_max表示所有点云在y维度上的最大值，y_min表示所有点云在y维度上的最小值，z_max表示所有点云在z维度上的最大值，z_min表示所有点云在z维度上的最小值。

故三维空间中任意一点p_w(x,y,z)∈P所在的栅格索引可以用下式(2)进行计算：

式中，floor[·]表示向下取整函数，x、y、z分别表示p_w(x,y,z)∈P的相应坐标值，index_x表示栅格中x维度的栅格索引，index_y表示栅格中y维度的栅格索引，index_z表示栅格中z维度的栅格索引。

步骤112，栅格搜索：以z轴方向上的l个栅格为单位进行遍历，即遍历俯视方向上的m×n列栅格，将每列栅格中自下而上(z轴正方向)的第一个具有点云栅格作为地面点栅格，并将该栅格中点云清空；遍历完所有列栅格后，便得到了非地面点云。

上述俯仰方向可以理解为雷达坐标系的Z轴方向，俯视方向可以理解为从上到下看的方向，反之为仰视方向。

步骤12，障碍物聚类：将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇，即障碍物目标。

例如：步骤12可以通过如下步骤实现，也可以通过本领域公知的DBSCAN、超体聚类或K-means聚类方法等获得：

步骤121，种子点选取：从步骤11输出的所有非地面点云取出一个点云作为聚类种子点，并对其进行邻域搜索，若在邻域半径ρ内存有点云，则将邻域搜索到的点云存入聚类生长点队列

其中，“邻域搜索”可以是栅格搜索，也可以是KD树搜索。

步骤122，生长聚类：遍历

中的每个点云，进行半径阈值为ρ的邻域搜索，若邻域内存在点云，则将不属于

的点云存入聚类生长过渡队列

按照相同的方法遍历

中的所有点云，将

中点云存入第k帧点云数据中第n类障碍物的聚类容器

再将

中所有点云存入

直至某次生长时

为空集，此时该类别聚类结束。

重复步骤121和步骤122，直至处理完毕步骤11输出的所有非地面点云，得到所有障碍物聚类容器

并将所有障碍物信息中不满足足够阈值数量的类别进行剔除。比如：阈值数量设置十个点，那么需要剔除少于十个点的障碍物。

步骤13，形状估计：估计步骤12获得的每个障碍物的位置、形状和尺寸信息。本实施例采用一个二维包围框将每个障碍物框出来，而二维包围框的长宽由其主方向和点云簇中的点云分布共同决定，以描述障碍物的位置、大小和主方向等信息。

例如：步骤13可以通过如下步骤实现，也可以通过本领域公知的最小面积法或其它公知方法获得：

步骤131，二维包围框的位置：将步骤12得到的每一个障碍物点云簇的质心定义为该障碍物的坐标。

步骤132，二维包围框的主方向：通常激光雷达只能照射到物体的“L”形区域，故本发明实施例将障碍物点云簇中直线特征最明显的方向，即将点云投影至鸟瞰图(移除z轴维度)后的障碍物进行随机采样一致性(RANSAC)直线分割，将分割出的直线方向作为该障碍物主方向。

步骤132，二维包围框的长、宽和高：利用步骤132确定的主方向建立二维坐标系，比如，设置所述主方向为x′轴，然后设置垂直该x′轴、并且垂直雷达坐标系的z轴y′轴。再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x′、y′两个维度上的极大、极小值。最后将极大值与相应的极小值的差值作为二维包围框的长和宽，即

其中，

表示二维包围框的长，即障碍物的长度；

表示二维包围框的宽，即障碍物的宽度。

步骤14，数据关联：根据多帧步骤13估计得到的每个障碍物的差异程度计算差异度函数，建立关联矩阵，再通过关联矩阵，可以将多帧点云数据中障碍物关联起来。

例如：步骤14可以通过如下步骤实现，也可以通过本领域公知的卡尔曼滤波跟踪或基于深度学习等障碍物跟踪方法获得：

步骤141，根据第k帧第u个障碍物位置为

第k-1帧第v个障碍物位置为

障碍物的长和宽分别表示

通过观测多帧点云数据构建车辆匀加速运动模型，进而预测得到的当前帧相对于上一帧运动为

则差异度函数定义为式(3)：

式(3)中，α为距离差异权重,β为长宽比差异权重，γ为高度差异权重,均取经验值，比如α＝0.4，β＝0.3,γ＝0.3。

步骤142，关联矩阵建立：根据P_k-1中共检测到的障碍物的总数量为m个，记为

其中u＝1,2,…,m；P_k中共检测到的障碍物的总数量为n个，记为

其中，v＝1,2,…,n，将关联矩阵

表示为下式(4)：

式中，

表示由式(3)计算的差异度，其中的每一行的第二个下标相同，这意味每一行对应着当前帧的同一个障碍物，那么，第一行的下标均为1，则表示第一障碍物；第二行的下标均为2，则表示第二障碍物；……；第v行的下标均为v，则表示第v障碍物。

步骤143，障碍物关联：关联当前帧P_k和上一帧P_k-1中的障碍物。具体地，关联某一障碍物时，将当前帧P_k的关联矩阵的选定行所对应的障碍物作为选定障碍物，再计算选定行中的各元素分别与上一帧P_k-1中关联矩阵中所有的元素的差异度，并以计算得到的最小差异度所对应的P_k-1中元素所对应的障碍物作为选定障碍物的关联障碍物，此时判定选定障碍物与关联障碍物是同一障碍物。

重复上述步骤，直至当前帧P_k的关联矩阵的每一行对应的障碍物关联完毕。

进一步地，若最小差异度所对应的P_k-1中元素的下标u＞v，则表明发生障碍物丢失；若最小差异度所对应的P_k-1中元素的下标u＜v，则表明有新动态障碍物出现。

比如：P_k中有10个障碍物，上一帧P_k-1有10个障碍物，并且这10个障碍物在这两帧点云数据中能够一一对应，那么，该关联矩阵为10行10列的矩阵，将第一行的10个元素对应的是当前帧P_k的第一个障碍物作为选定障碍物。而步骤313是将第一行的10个元素与P_k-1的关联矩阵中的所有元素，利用式(3)进行差异度计算，其中，计算得到的最小差异度所对应的P_k-1中元素所对应的障碍物作为选定障碍物的关联障碍物，二者被认定为同一个障碍物。

步骤15，状态估计：根据步骤14关联的障碍物情况，以及上一帧视觉里程计计算得到的两帧点云数据相对运动，判断障碍物运动状态，区分出动态障碍物和静态障碍物。

例如：步骤15可以通过如下步骤实现，也可以通过本领域公知的其它方法获得：

根据某障碍物在连续两帧点云数据中被关联，其在第k-1帧点云数据P_k-1中的坐标为

P_k中的坐标为

步骤1计算得到的两帧点云数据相对运动为

则利用式(5)计算障碍物的运动距离：

若其大于一定距离阈值μ，则被认定为是动态障碍物，反之为静态障碍物；根据激光雷达传感器的帧率为f，则动态障碍物的速度为V＝Δd×f。其中，μ的数值可以但不限于0.5米。

由于在数据关联阶段使用的当前帧位姿信息是通过结合前几帧数据的位姿信息预测得到，并非出自视觉程计，为了使动态障碍物的运动估计更加准确，故应在得到视觉里程计输出的当前帧位姿信息后，利用该位姿信息再次执行数据关联和运动估计操作，并将此次计算结果作为最终的动态障碍物速度信息输出，如图3所示。

需要说明的是，数据关联与状态估计需要进行两次，第一次是利用多帧数据预测得到的当前帧位姿信息进行数据关联与状态估计，第二次是利用视觉里程计解算得到的当前帧位姿信息进行数据关联与状态估计，从而使得动态障碍物信息更加准确。

在一个实施例中，步骤2中的“根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜”的方法包括如下过程，其中的步骤1输出的动态障碍物信息实质对应的是点云数据：

将步骤1输出的动态障碍物信息投影在图像数据上，并使属于动态障碍物区域的像素无效化。

例如，可以使用下式(6)实现将动态障碍物信息投影在图像数据上：

式中，

为像素坐标的齐次坐标形式，即

K为单目视觉2内参矩阵；

为雷达坐标系到相机坐标系的变换矩阵，即标定外参；

为点云在雷达坐标系中的齐次坐标形式，即

L表示雷达坐标系，C表示相机坐标系，I表示惯性坐标系(世界坐标系)。

还例如，可以利用Graham扫描法生成最小凸包，以此凸包为掩膜，覆盖于当前的原始图像数据上，实现将属于动态障碍物区域的像素无效化。

当然，也可以采用现有公知的其它方法实现将动态障碍物信息投影在图像数据以及将属于动态障碍物区域的像素无效化。

在一个实施例中，步骤2中的“图像数据”由图1中示出单目视觉2获得，步骤2具体包括：

步骤21，将步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜。其中，“标定”是指离线标定操作，“外参”指的是刚体变换矩阵，“相互标定外参”指的是将图像数据或点云数据相互投影。这为系统的可靠运行提供基础保障。本实施例可以默认雷达坐标系为车体坐标系，相机坐标系中的数据可以通过标定得到的外参矩阵转换到雷达坐标系中，且默认单目视觉的相对运动和激光雷达的相对运动相同，后面不再赘述。

步骤22，在步骤21中形成的图像掩膜的无效像素之外的区域提取ORB特征点，以排除动态障碍物对里程计的干扰。ORB特征点是某一像素周围邻域像素满足一定关系的像素集合。

目前主流的提取ORB特征点方法有SIFT、SURF、ORB等，由于SLAM系统对于算法实时性要求较高，故本实施例选择效率与尺度不变性、旋转一致性兼顾的ORB特征作为特征点。优选地，为了保持ORB特征点分布的均匀性，本实施例采用如下提供的在金字塔图像的每一层都使用基于四叉树方法提取ORB特征点：

步骤221，在图像数据中划分栅格，在栅格内提取固定数量的ORB特征点，若提取数量不足时，则降低ORB中的FAST阈值，这样能保证低纹理区域的ORB特征点不会过少。其中，“固定数量”一般数量理解为提取特征点总数量/栅格个数，提取特征点总数量跟图像分辨率有关。

步骤222，对提取出的ORB特征点进行四叉树划分，将其不断分裂，直至四叉树节点数量不小于需要提取的ORB特征点数量，并在四叉树产生的每个节点中只保留响应值最大的特征点。例如：角点代表这个像素和周围的像素差距比较大，而响应值反映了这个差异程度。其中，“ORB特征点数量”跟图像分辨率有关，例如KITTI数据集的图像分辨率为760*1250，大概需要提取的ORB特征点数量为2000个。

步骤23，匹配相邻两帧图像数据的ORB特征点，其具体包括如下过程：

选用汉明距离描述特征点间的相似关系，即对于图像帧A中某特征点，在与图像帧A相邻的图像帧B中找到具有与其汉明距离最小的特征点作为匹配点，同时在匹配中引入RANSAC算法与旋转直方图，以减少误匹配。

步骤23也可以采用投影匹配方式或其它现有匹配方法实现，在此不再赘述。

步骤24，估计ORB特征点的深度，ORB特征点的深度指的是ORB特征点提供的像素集合的中心物理位置与相机坐标系的原点沿z轴(光轴方向)的间距，其具体包括但不限于如下过程：

在ORB特征点匹配后，可以通过三角化估计ORB特征点的深度。也可以采用三角化结合ORB特征点与投影后的点云深度信息，估计特征点深度。具体地，选取ORB特征点周围最近的三个不共线的点云，进行平面拟合，并计算该拟合平面与单目视觉光心到被选的ORB特征点射线的交点，此交点深度即作为ORB特征点的深度，此交点记为该ORB特征点生成的地图点。

步骤25，获取当前帧(普通帧)与上一帧的相对位姿，其具体包括但不限于如下过程：

根据图像数据提供的相邻两帧的步骤22提取得到的ORB特征点与步骤24输出的相应的ORB特征点的深度，并通过SVD分解求得相对位姿的解析解：

步骤251，定义步骤23匹配后得到的相邻两帧ORB特征点生成的地图点质心：

式中，

为第k帧ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

为第k-1帧ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

表示P_k中的第w个ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

表示P_k-1中的第w个ORB特征点生成的地图点质心在雷达坐标系L中的坐标，n表示匹配成功且成功生成地图点的ORB特征点总数目。

步骤252，为方便表达，将去质心坐标定义为

获得由式(7)表示的矩阵W，其为一个3×3矩阵：

步骤253，对W进行SVD分解，得到式(8)：

W＝UΣV^T (8)

式中，Σ为奇异值对角矩阵，U和V为正交矩阵。当W满秩时，旋转矩阵R_k表示为式(9)：

若此时

行列式为负，则取

为最优值。

步骤254，根据

按照

计算当前帧图像数据相对于上一帧图像数据的平移量即可。

由于上述步骤仅仅跟踪了一帧图像数据，而并没有使用全局信息，导致获得的相对位姿的误差较大。鉴于此，步骤2具体还包括：

步骤26，判断局部地图中是否存在地图点，若判定为是，则执行步骤27，反之，进入步骤28。

步骤27，将在共视图中与当前帧相邻的关键帧的地图点采用上述步骤式(6)的投影方法投影到当前帧中，重复步骤22至步骤25，从而再次获取相对位姿，在增加约束的基础上进一步得到更精确的位姿。

步骤28，判断从步骤26或步骤27中处理后的当前帧是否同时满足以下两个条件，如果是，则作为关键帧插入局部地图模块中进行优化等处理；如果否，则丢弃，不进行建图作用，仅用于定位。

条件一，当前帧跟踪到的特征点数量大于预设数量(比如：60)；

条件二，距离上次关键帧插入至少经过预设帧数(比如：10帧)或两个关键帧的平移超过一定阈值。

其中，“判断关键帧是否被插入地图中”的策略比如可以包括：当前帧相比上一个关键帧位置距离超过5m设定一个关键帧。

步骤2输出结果为一系列关键帧位姿，将其与相应点云数据进行变换叠加，得到具有全局一致性的稠密点云地图，此外，在算法过程中会产生特征点信息，故也可得到稀疏的特征点地图供定位使用。

在一个实施例中，步骤3用于根据步骤2输出的关键帧信息，管理地图和优化关键帧，其具体包括：

步骤31，将步骤2输出的关键帧信息插入到共视图中，并根据地图点共视程度更新此关键帧与其它关键帧的连接关系，同时更新生长树和词袋模型。

步骤32，判断当前关键帧与上一关键帧产生的平移是否满足三角化条件，若是，则进入行步骤33；否则，不进行三角化生成地图点操作，并进入步骤34。

步骤33，由于在步骤2仅生成存有激光雷达深度的地图点，为了更多的利用单目视觉2采集到的图像数据，将能在至少3帧中跟踪到的ORB特征点进行融合。

步骤34，局部BA优化地图：根据当前关键帧的本质图，找到其它相邻关键帧及地图点，构建非线性优化问题(10)，获得优化位姿ξ^*和地图点

其中，

为像素坐标的齐次坐标形式，即

K为单目视觉内参矩阵，

为地图点坐标的齐次坐标形式，即

z_w为地图点深度；ξ为单目视觉位姿的李代数；ξ^*为单目视觉位姿的李代数的最优估计值，

为地图点坐标的齐次坐标形式的最优估计值，ξ^为ξ的反对称矩阵。

在一个实施例中，使用李代数构建无约束优化问题，能够方便地通过高斯牛顿法或列文伯格-马夸尔特法等优化方法，求解步骤34构建非线性优化问题(10)，优化位姿和地图点，但在使用这些方法前，需要推导每个误差项关于优化变量的导数，即线性化下式(11)：

式中，Δξ为位姿增量对应的李代数增量，e(ξ+Δξ)为原位姿加了一个小增量后的误差项，e(ξ)为位姿增量对应的李代数，

为雅克比(导数)的转置，对应

假定世界坐标系I下点

在相机坐标系C下的齐次坐标为

从而单目视觉模型可表示为

其中单目视觉内参矩阵(12)：

式(12)中，f_x和f_y是单目视觉的焦距在像素坐标系下X和Y轴上的坐标值，c_x和c_y是单目视觉的光圈中心在像素坐标系下X和Y轴上的坐标值，这四个参数的具体数值可以根据单目视觉的型号，通过张正友标定方法获得。

对exp(ξ^)左乘扰动量δξ，考虑式(11)中的误差项e的变化关于扰动量的导数，利用链式法则可列写如下式(13)：

式中，第一项

为误差关于投影点的导数，第二项

为变换后的点关于李代数的导数，故由式(13)求得式(14)：

类似地，误差函数

相对于地图点的导数为式(15)：

其中，R为变换矩阵T的旋转部分。

步骤35，判断步骤34优化后的关键帧超过设定数量(比如90％)的ORB特征点是否能被至少其它三个关键帧观测到，若是，则判定当前关键帧为冗余关键帧，从共视图中删除冗余关键帧，同时更新生长树和词袋模型，并删除属于该关键帧的地图点，随后在局部地图中删除步骤31的冗余关键帧，从而达到在关键帧信息尽可能不丢失的前提下使其更可靠的目的。

步骤4，回环检测：针对由于步骤2出现累计误差造成的轨迹漂移问题，利用词袋模型语义信息提供的闭环约束进行全局BA，以消除累计误差，其具体包括：

步骤41，检测回环：利用离线训练的词袋模型对步骤3输出的每个关键帧进行图像语义描述，并与当前关键帧进行相似度计算，相似度达到一定阈值则认为出现回环，相似度计算方法如下式(16)：

其中，v_r＝{(w₁，η₁)，(w₂，η₂)，…，(w_N，η_N)}，w_N为词袋模型叶子节点，η_N为相应权重。v_r为描述第r个关键帧的词袋模型向量，v_o为述第o个关键帧的词袋模型向量。

步骤42，回环融合：出现回环后，首先替换或填补当前关键帧与检测出来回环关键帧中存在冲突的地图点，随后在本质图上将当前关键帧与回环关键帧连接，更新本质图。

步骤43，本质图优化：根据步骤42产生的回环信息，即新建立的约束信息，进行全局BA，同时优化回环中的关键帧位姿和关键帧位姿中的地图点，消除累计误差，以获得优化后的关键帧位姿、特征点地图和点云地图。其中，地图由关键帧相对位姿叠加得到，特征点地图是由ORB特征点生成的地图点与关键帧位姿叠加组合得到的地图，点云地图则是通过叠加点云和关键帧位姿得到。

本发明实施例利用点云数据的空间信息结合视觉里程计的位姿信息进行障碍物检测与状态估计，以区分出动态障碍物，并根据动态点云数据在图像上的分布形成掩膜，在掩膜的基础上进行视觉SLAM算法流程，从而达到可以在动态场景中执行SLAM目的；此外，点云数据可以提供比图像范围更大的绝对尺度深度先验信息，可以在解决单目视觉SLAM尺度模糊性问题的基础上进一步提升其结果可靠性。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (9)

1.一种动态场景下的激光单目视觉融合定位建图方法，其特征在于，包括：

步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出；

步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉上形成的图像掩膜，在图像掩膜上提取ORB特征点，并将提取的所述ORB特征点与上一帧ORB特征点匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；

步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；步骤3具体包括：

步骤31，将步骤2输出的关键帧信息插入到共视图中，并根据地图点共视程度更新此关键帧与其它关键帧的连接关系，同时更新生长树和词袋模型；

步骤32，判断当前关键帧与上一关键帧产生的平移是否满足三角化条件，若是，则进入步骤33；否则，则进入步骤34；

步骤33，将能在至少3帧中跟踪到的ORB特征点进行融合，并根据三角化生成新地图点；

步骤34，局部BA优化地图：根据当前关键帧的本质图，找到其它相邻关键帧及地图点，构建非线性优化问题(10)，获得优化位姿ξ^*和地图点

其中，

为像素坐标的齐次坐标形式，即

K为单目视觉内参矩阵，

为地图点坐标的齐次坐标形式，即

z_w为地图点深度；ξ为单目视觉位姿的李代数；ξ^*为单目视觉位姿的李代数的最优估计值，

为地图点坐标的齐次坐标形式的最优估计值，ξ^{^}为ξ的反对称矩阵，n表示成功匹配且生成地图点的ORB特征点总数目，下标w表示第w个ORB特征点；

步骤35，判断步骤34优化后的关键帧超过设定数量的ORB特征点是否能被至少其它三个关键帧观测到，若是，则判定当前关键帧为冗余关键帧，从共视图中删除冗余关键帧，同时更新生长树和词袋模型，并删除属于该关键帧的地图点，随后在局部地图中删除步骤31的冗余关键帧；

步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。

2.如权利要求1所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤1具体包括：

步骤11，将激光雷达输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云；

步骤12，将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇；

步骤13，估计步骤12获得的每个障碍物的位置、形状和尺寸信息；

步骤14，根据多帧步骤13估计得到的每个障碍物的差异程度计算差异度函数，建立关联矩阵，将多帧点云数据中障碍物关联起来；

步骤15，根据步骤14关联的障碍物情况，以及上一帧视觉里程计计算得到的两帧点云数据相对运动，判断障碍物运动状态，区分出动态障碍物和静态障碍物。

3.如权利要求2所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤11具体包括：

步骤111，将点云数据按照空间位置信息放入预设定大小的栅格中，每个栅格立方体的边长为d₀，分别按照雷达坐标系的x、y、z三个维度进行分割，获得m×n×l个栅格，具体如下式(1)所示：

式中，ceil[·]表示向上取整函数，x_max表示所有点云在x维度上的最大值，x_min表示所有点云在x维度上的最小值，y_max表示所有点云在y维度上的最大值，y_min表示所有点云在y维度上的最小值，z_max表示所有点云在z维度上的最大值，z_min表示所有点云在z维度上的最小值；

故三维空间中任意一点p_w(x,y,z)∈P所在的栅格索引用下式(2)进行计算：

式中，floor[·]表示向下取整函数，x、y、z分别表示p_w(x,y,z)∈P的相应坐标值，index_x表示栅格中x维度的栅格索引，index_y表示栅格中y维度的栅格索引，index_z表示栅格中z维度的栅格索引；

步骤112，以z轴方向上的l个栅格为单位进行遍历，即遍历俯视方向上的m×n列栅格，将每列栅格中沿z轴正方向自下而上的第一个具有点云栅格作为地面点栅格，并将该栅格中点云清空；遍历完所有列栅格后，便得到了非地面点云。

4.如权利要求2所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤12通过如下步骤实现：

步骤121，从步骤11输出的所有非地面点云取出一个点云作为聚类种子点，并对其进行邻域搜索，若在邻域半径ρ内存有点云，则将邻域搜索到的点云存入聚类生长点队列

步骤122，遍历

中的每个点云，进行半径阈值为ρ的邻域搜索，若邻域内存在点云，则将不属于

的点云存入聚类生长过渡队列

按照相同的方法遍历

中的所有点云，将

中点云存入第k帧点云数据中第n类障碍物的聚类容器

再将

中所有点云存入

直至某次生长时

为空集，此时该类别聚类结束；

重复步骤121和步骤122，直至处理完毕步骤11输出的所有非地面点云，得到所有障碍物聚类容器

并将所有障碍物信息中不满足足够阈值数量的类别进行剔除。

5.如权利要求2所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤13通过如下步骤实现：

步骤131，将步骤12得到的每一个障碍物点云簇的质心定义为该障碍物的坐标；

步骤132，将点云投影至鸟瞰图后的障碍物进行随机采样一致性直线分割，将分割出的直线方向作为该障碍物主方向；

步骤132，利用步骤132确定的主方向建立二维坐标系，再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x′、y′两个维度上的极大、极小值，最后将极大值与相应的极小值的差值作为二维包围框的长和宽。

6.如权利要求2所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤14通过如下步骤实现：

步骤141，根据第k帧第u个障碍物位置为

第k-1帧第v个障碍物位置为

障碍物的长和宽分别表示为

通过观测多帧点云数据构建车辆匀加速运动模型，进而预测得到的当前帧相对于上一帧运动为

则差异度函数定义为式(3)：

式(3)中，α为距离差异权重,β为长宽比差异权重，γ为高度差异权重,均取经验值；

步骤142，P_k-1中共检测到的障碍物的总数量为m个，记为

其中u＝1,2,…,m；P_k中共检测到的障碍物的总数量为n个，记为

其中，v＝1,2,…,n，将关联矩阵

表示为下式(4)：

式中，

表示由式(3)计算的差异度，其中的每一行的第二个下标相同；

步骤143，关联当前帧P_k和上一帧P_k-1中的障碍物；

重复上述步骤，直至当前帧P_k的关联矩阵的每一行对应的障碍物关联完毕。

7.如权利要求1-6中任一项所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤2具体包括：

步骤21，利用式(6)将步骤1输出的动态障碍物信息在相互标定外参的单目视觉上形成图像掩膜：

式中，

为像素坐标的齐次坐标形式，即

K为单目视觉内参矩阵；

为雷达坐标系到相机坐标系的变换矩阵；

为点云在雷达坐标系中的齐次坐标形式，即

L表示雷达坐标系，C表示相机坐标系，I表示惯性坐标系；

步骤22，在步骤21中形成的图像掩膜的无效像素之外的区域提取ORB特征点；步骤22具体包括：

步骤221，在图像数据中划分栅格，在栅格内提取固定数量的ORB特征点；

步骤222，对提取出的ORB特征点进行四叉树划分，将其不断分裂，直至四叉树节点数量不小于需要提取的ORB特征点数量，并在四叉树产生的每个节点中只保留响应值最大的特征点；

步骤23，匹配相邻两帧图像数据的ORB特征点；

步骤24，选取ORB特征点周围最近的三个不共线的点云，进行平面拟合，并计算该拟合平面与单目视觉光心到被选的ORB特征点射线的交点，此交点的深度作为ORB特征点的深度，此交点记为该ORB特征点生成的地图点；

步骤25，获取当前帧与上一帧的相对位姿，其具体包括如下过程：

步骤251，定义步骤23匹配后得到的相邻两帧ORB特征点生成的地图点质心：

式中，

为第k帧ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

为第k-1帧ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

表示P_k中的第w个ORB特征点生成的地图点质心在雷达坐标系L中的坐标，

表示P_k-1中的第w个ORB特征点生成的地图点质心在雷达坐标系L中的坐标；

步骤252，将去质心坐标定义为

获得由式(7)表示的矩阵W，其为一个3×3矩阵：

步骤253，对W进行SVD分解，得到式(8)：

W＝UΣV^T (8)

式中，Σ为奇异值对角矩阵，U和V为正交矩阵，当W满秩时，旋转矩阵R_k表示为式(9)：

若此时

行列式为负，则取

为最优值；

步骤254，根据

按照

计算当前帧图像数据相对于上一帧图像数据的平移量。

8.如权利要求7所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤2具体还包括：

步骤26，判断局部地图中是否存在地图点，若判定为是，则执行步骤27，反之，进入步骤28；

步骤27，将与当前帧在共视图中相邻的关键帧的地图点采用式(6)的投影方法投影到当前帧中，重复步骤22至步骤25，从而再次获取相对位姿，在增加约束的基础上进一步得到更精确的位姿；

步骤28，判断从步骤26或步骤27中处理后的当前帧是否同时满足以下两个条件，如果是，则作为关键帧插入局部地图模块中进行优化处理；

条件一，当前帧跟踪到的特征点数量大于预设数量；

条件二，距离上次关键帧插入至少经过预设帧数或两个关键帧的平移超过一定阈值。

9.如权利要求1所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤4具体包括：

步骤41，利用离线训练的词袋模型对步骤3输出的每个关键帧进行图像语义描述，并与当前关键帧进行相似度计算，相似度达到一定阈值则认为出现回环，相似度计算方法如下式(16)：

其中，v_r＝{(w₁,η₁),(w₂,η₂),…,(w_N,η_N)}，w_N为词袋模型叶子节点，η_N为相应权重，v_r为描述第r个关键帧的词袋模型向量，v_o为述第o个关键帧的词袋模型向量；

步骤42，出现回环后，首先替换或填补当前关键帧与检测出来回环关键帧中存在冲突的地图点，随后在本质图上将当前关键帧与回环关键帧连接，更新本质图；

步骤43，根据步骤42产生的回环信息，进行全局BA，同时优化回环中的关键帧位姿和关键帧位姿中的地图点，获得优化后的关键帧位姿、特征点地图和点云地图。

Images