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

本发明专利技术公开了一种动态场景下的激光单目视觉融合定位建图方法，该方法包括：步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息；步骤2，在相互标定外参的单目视觉2上形成的图像掩膜，在图像掩膜上提取ORB特征点及其与上一帧ORB特征点的匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；步骤3，将关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。本发明专利技术能够在动态场景中执行SLAM目的。中执行SLAM目的。中执行SLAM目的。

【技术实现步骤摘要】
一种动态场景下的激光单目视觉融合定位建图方法


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

技术介绍

[0002]随着自动化、人工智能和机器人等技术的飞速发展，实际生活和工业场景中应用了越来越多具有感知、定位和导航功能的智能无人系统，例如扫地机器人、仓储机器人及无人驾驶出租车等。随着传感器技术及智能算法的不断迭代，这些无人系统的应用场景已经从简单的已知环境，扩展到完全未知的复杂环境。在未知环境中，机器人需要用来自传感器的信息来感知周围环境以及估计自身状态，以保证其能够在复杂未知环境中自主运动。因此，机器人在未知环境中将面临三个问题：1)我在哪？2)我周围都有什么？3)我要去哪里？这三个问题分别对应为：1)机器人的自主定位问题；2)机器人的地图构建问题；3)机器人的实时避障及路径规划问题。
[0003]SLAM(英文全称为：Simultaneous Localization and Mapping，中文全称为：定位与建图)是利用机器人自身传感器感知和定位到的周围环境信息构建环境地图的技术，其为前两个问题提供了答案，从而为机器人在未知环境中自主运动提供了基础保证。SLAM可以依据采集信息使用的传感器不同，可分为激光SLAM和视觉SLAM。其中，激光SLAM存在点云数据在垂直方向上稀疏性造成的俯仰角估计不准的问题，同时激光雷达所获取的环境信息也不如视觉传感器丰富，相对而言，在回环检测的效率上不及视觉传感器。视觉SLAM由于具有相对丰富的环境信息以及垂直方向上结构化数据的密集性，可在一定程度弥补上述问题缺陷。目前，绝大多数视觉SLAM问题的研究基于假设周围环境为静态，在动态场景下的激光单目视觉融合定位建图方法仍不成熟，相关理论有待进一步完善。

技术实现思路

[0004]本专利技术的目的在于提供一种动态场景下的激光单目视觉融合定位建图方法来克服或至少减轻现有技术的上述缺陷中的至少一个。
[0005]为实现上述目的，本专利技术提供一种动态场景下的激光单目视觉融合定位建图方法，该方法包括：
[0006]步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出；
[0007]步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜，在图像掩膜上提取ORB特征点及其与上一帧ORB特征点的匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；
[0008]步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；
[0009]步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。
[0010]进一步地，步骤1具体包括：
[0011]步骤11，将激光雷达输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云；
[0012]步骤12，将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇；
[0013]步骤13，估计步骤12获得的每个障碍物的位置、形状和尺寸信息；
[0014]步骤14，根据多帧步骤13估计得到的每个障碍物的差异程度计算差异度函数，建立关联矩阵，将多帧点云数据中障碍物关联起来；
[0015]步骤15，根据步骤14关联的障碍物情况，以及上一帧视觉里程计计算得到的两帧点云数据相对运动，判断障碍物运动状态，区分出动态障碍物和静态障碍物。
[0016]进一步地，步骤11具体包括：
[0017]步骤111，将点云数据按照空间位置信息放入预设定大小的栅格中，假设每个栅格立方体的边长为d0，分别按照雷达坐标系的x、y、z三个维度进行分割，获得m
×
n
×
l个栅格，具体如下式(1)所示：
[0018][0019]式中，ceil[
·
]表示向上取整函数，x
max
表示所有点云在x维度上的最大值，x
min
表示所有点云在x维度上的最小值，y
max
表示所有点云在y维度上的最大值，y
min
表示所有点云在y维度上的最小值，z
max
表示所有点云在z维度上的最大值，z
min
表示所有点云在z维度上的最小值；
[0020]故三维空间中任意一点p
w
(x,y,z)∈P所在的栅格索引用下式(2)进行计算：
[0021][0022]式中，floor[
·
]表示向下取整函数，x、y、z分别表示p
w
(x,y,z)∈P的相应坐标值，index_x表示栅格中x维度的栅格索引，index_y表示栅格中y维度的栅格索引，index_z表示栅格中z维度的栅格索引；
[0023]步骤112，以z轴方向上的l个栅格为单位进行遍历，即遍历俯视方向上的m
×
n列栅格，将每列栅格中自下而上(z轴正方向)的第一个具有点云栅格作为地面点栅格，并将该栅格中点云清空；遍历完所有列栅格后，便得到了非地面点云。
[0024]进一步地，步骤12通过如下步骤实现：
[0025]步骤121，从步骤11输出的所有非地面点云取出一个点云作为聚类种子点，并对其进行邻域搜索，若在邻域半径ρ内存有点云，则将邻域搜索到的点云存入聚类生长点队列
[0026]步骤122，遍历中的每个点云，进行半径阈值为ρ的邻域搜索，若邻域内存在点云，则将不属于的点云存入聚类生长过渡队列按照相同的方法遍历中的所有点云，将中点云存入第k帧点云数据中第n类障碍物的聚类容器再将中所有点云存入直至某次生长时为空集，此时该类别聚类结束；
[0027]重复步骤121和步骤122，直至处理完毕步骤11输出的所有非地面点云，得到所有障碍物聚类容器并将所有障碍物信息中不满足足够阈值数量的类别进行剔除。
[0028]进一步地，步骤13通过如下步骤实现：
[0029]步骤131，将步骤12得到的每一个障碍物点云簇的质心定义为该障碍物的坐标；
[0030]步骤132，将点云投影至鸟瞰图后的障碍物进行随机采样一致性直线分割，将分割出的直线方向作为该障碍物主方向；
[0031]步骤132，利用步骤132确定的主方向建立二维坐标系，再根据障碍物所对应点云簇中每个点云的空间信息，计算点云在该坐标系x
′
、y本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种动态场景下的激光单目视觉融合定位建图方法，其特征在于，包括：步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出；步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉2上形成的图像掩膜，在图像掩膜上提取ORB特征点及其与上一帧ORB特征点的匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。2.如权利要求1所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤1具体包括：步骤11，将激光雷达输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云；步骤12，将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇；步骤13，估计步骤12获得的每个障碍物的位置、形状和尺寸信息；步骤14，根据多帧步骤13估计得到的每个障碍物的差异程度计算差异度函数，建立关联矩阵，将多帧点云数据中障碍物关联起来；步骤15，根据步骤14关联的障碍物情况，以及上一帧视觉里程计计算得到的两帧点云数据相对运动，判断障碍物运动状态，区分出动态障碍物和静态障碍物。3.如权利要求2所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤11具体包括：步骤111，将点云数据按照空间位置信息放入预设定大小的栅格中，假设每个栅格立方体的边长为d0，分别按照雷达坐标系的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个，记为中共检测到的障碍物的总数量为m个，记为其中u＝1，2，
…
，m；P
k
中共检测到的障碍物的总数量为n个，记为其中，v＝1，2，
…
，n，将关联矩阵表示为下式(4)：式中，表示由式(3)计算的差...

【专利技术属性】
技术研发人员：秦晓辉，芦涛，尚敬，胡云卿，刘海涛，徐彪，谢国涛，秦兆博，胡满江，王晓伟，边有钢，秦洪懋，丁荣军，
申请(专利权)人：中车株洲电力机车研究所有限公司，
类型：发明
国别省市：