一种非结构化动态环境下的同时定位与建图方法与流程

1.本发明涉及非结构化动态环境下的同时定位与建图问题，针对高动态变化的环境，本发明提出了利用多传感器融合和语义特征点匹配的激光里程计方法，同时利用基于最近邻搜索的动态地图更新方法保证了非结构化动态环境下的同时定位与建图。


背景技术：

2.无人驾驶产业已经踏上了高速发展的道路，面对各种行业的无人驾驶应用，拥有一个完整的无人驾驶系统是不可或缺的。而对于特殊行业的无人驾驶应用，通过合理的利用特殊的环境信息，选用合适的软硬件策略才能促使无人驾驶落地，在一些特殊行业恶劣的工作环境下保障工作人员的身心健康。
3.在整个无人驾驶系统中，同时定位与建图是最开始和最基本的步骤，最重要的是正确地感知到外界环境与自身车辆的实时状态，对车身在环境中的位姿进行定位，同时建立外界环境的地图信息，之后才是路径规划和运动控制。感知模块往往要对地面或墙壁等平面进行分割提取，提取平面主要的分割方法有随机采样一致性算法(ransac)、区域生长算法和线性拟合地面分段算法等。随机采样一致性算法能够抑制点云噪声对特征分割的影响，且迭代计算效率较高，但是需要人工设置平面阈值进行提前建模，鲁棒性较差。区域增长算法初始点云的选择要求比较高，在区域生长过程中易受噪声点影响。线性拟合地面分段算法利用旋转激光雷达的特性，将每个点云分配到对应的扇形中进行有序化处理，在有序化处理后再对三维数据降维，该操作可以保留竖直维度的信息，同时又不损失水平维度的信息，最后对每个扇形进行拟合直线以及地面点的筛选。
4.激光雷达slam方面，单线激光雷达slam中主要有基于粒子滤波的 gmapping算法，基于图优化的karto算法，基于非线性优化的cartographer算法，但由于单线激光雷达主要是进行二维的建图，对于一个三维的非结构化环境，无法获取到足够的环境空间变化信息来支持同时定位与建图。多线激光雷达slam 中主要有loam算法(zhang j,singh s.loam:lidar odometry and mapping inreal-time[c].robotics:science and systems.2014:33-41.)和cartographer算法(hess w, kohler d,rapp h h,et al.real-time loop closure in 2d lidar slam[c].ieee internationalconference on robotics and automation.2016:1271-1278.)，但他们都是基于一个结构化的相对静态的室内或室外场景进行建图，对于一个高动态变化的环境，如随着作业进行货物不断减少的仓库，往往无法取得很好的效果。同时传统的建图是增量式叠加的点云地图，点云会越来越多导致内存占用越来越大，且无法在已建的地图上进行更新。
[0005]
范海东提出一种应用于煤栅内的自动推煤机定位系统及其定位方法(范海东, 胡凯波,王林刚,李峰,夏志凌,朱宇超,沈炳华.一种应用于煤栅内的自动推煤机定位系统及其定位方法[p].浙江省：cn111367297a,2020-07-03.)。但是同样无法针对动态变化的环境在已有的建图上进行实时更新。


技术实现要素：

[0006]
本发明要克服现有技术的不足之处，提供了一种适用于非结构化动态环境下的同时定位与建图方法。
[0007]
本发明所解决的问题是：(1)由于非结构化动态环境下，如仓库中的货物会随着作业不断减少，传统的slam算法会将动态变化的货物堆提取为特征点，最后因为定位无法修正的而导致定位失效。本发明利用线性拟合平面算法提取墙壁，将动态物体与静态墙壁分割开，然后采用静态墙壁提取出来的语义特征点进行之后的帧间匹配；(2)传统的建图是增量式叠加的点云地图，点云会越来越多导致内存占用越来越大，同时已到达过的点云不会更新，与货物随着作业越来越少的实际情况不符，本发明采用最近邻搜索的动态环境下地图更新方法优化地图更新策略。因此本发明提出了利用多传感器融合和语义特征点匹配的激光里程计方法，同时利用基于最近邻搜索的动态地图更新方法保证了非结构化动态环境下的同时定位与建图。
[0008]
本发明提供的非结构化动态环境下的同时定位与建图方法，包括以下步骤：
[0009]
步骤1：进行激光雷达和惯性导航元件数据的获取和预处理，包括：
[0010]
(11)用激光雷达获取环境点云数据，用惯性传感器获取车辆加速度和角速度数据；
[0011]
(12)去除测量过程中产生的无效值和车辆自身轮廓的无效点；(13)对多传感器间进行空间标定，利用标定好的结果矫正由于车辆运动产生的激光雷达点云畸变。
[0012]
步骤2：使用激光雷达和惯导传感器融合后的信息进行非结构化高动态环境下的点云语义分割。因为非结构化环境下物体会随着作业的进行不断地变化，而室内墙壁相对于动态变化的物体是静止的，所以需要将动态的物体和静态的墙壁分割开，包括：
[0013]
(21)设计适用于环境特征的线性拟合平面算法提取墙壁，通过计算同个扇形内相邻点云连线与水平面的夹角，并设定阈值。超过阈值且连续达到一定数量则归类于墙壁点云。
[0014]
(22)然后进行点云聚类，通过dbscan聚类后，再分析每个聚类结果中的点云，将高于垂直部分墙壁点云的点保留下，将其作为提取特征点的点云簇，然后输入给前端里程计模块。
[0015]
步骤3：基于之前具有语义信息的感知结果对点云数据进行点云降采样和语义特征点提取，包括：
[0016]
(31)选取静态的点，通过计算点位于局部小平面内的曲率来进行特征点的提取；
[0017]
(32)根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标，将特征点分为曲率高的边缘点和曲率低的平面点；
[0018]
(33)去除影响较大的遮挡点和平行点。
[0019]
步骤4：利用分割开动态物体后的特征点进行点云配准，包括：
[0020]
(41)基于不同的特征点采取不同的帧间匹配的策略，帧间配准则根据新的扫描特征点与缓存中的上一时刻的特征进行配准，建立以欧式距离为目标函数，以变换矩阵为优化变量的优化问题对变换矩阵进行求解，即得到移动机器人的局部位姿变换估计值；
[0021]
(42)以1赫兹的相对较低的频率将当前扫描的特征点与环境中根据主成分分析得到的特征进行配准得到优化后的变换矩阵；
[0022]
(43)优化后的变换矩阵将地图转换到世界坐标系下形成地图，并且对帧间配准得到的实时位姿估计进行修正。
[0023]
步骤5：使用最近邻搜索的动态环境下地图更新方法，包括：
[0024]
(51)对点云数据的三维x、y、z方向轮流做一次二叉树分割进行kd树的构建，基于kd树进行最近邻搜索；
[0025]
(52)将点云信息保存为kd树的结构后，在生成关键帧的同时将关键帧与历史关键帧进行knn搜索，若是搜索到位姿邻近的关键帧，则使用当前的关键帧点云替换历史关键帧点云，从而实现了地图的更新，同时也减小了内存的占用。
[0026]
本发明的优点和积极效果是：
[0027]
1、本发明用线性拟合平面算法提取墙壁，将动态物体与静态墙壁分割开，然后利用静态墙壁提取出来的语义特征点进行之后的帧间匹配，解决了非结构化动态环境下的定位与建图问题，如仓库中的货物会随着作业的进行而不断减少，作为动态物体的仓库被提取为特征点，会因为定位无法修正的而导致定位失效。
[0028]
2、本发明对多传感器间进行空间标定，利用标定好的结果对激光雷达点云的运动畸变进行矫正，利用多传感器融合的感知算法使车辆具有更好的地形适应性与感知精确性。
[0029]
3、本发明采用最近邻搜索的动态环境下地图更新方法优化地图更新策略。解决了传统增量式叠加点云地图，点云会越来越多导致内存占用越来越大，同时已到达过的点云不会更新的问题，更符合高动态变换环境的环境特征。
附图说明
[0030]
图1是本发明的感知模块流程图。
[0031]
图2是本发明的激光里程计模块流程图。
[0032]
图3是本发明的knn查找示意图。
[0033]
图4是本发明方法在仓库中的点云分割图。
[0034]
图5是本发明方法在仓库中的定位与建图。
[0035]
图6是本发明方法在室外场景的定位与建图。
具体实施方式
[0036]
以下结合附图对本发明的适用于非结构化动态环境下的同时定位与建图方法通过简单实例作进一步详述。
[0037]
本发明提供的非结构化动态环境下的同时定位与建图方法，包括以下步骤：
[0038]
步骤1：感知模块流程图如图1所示，进行激光雷达和惯性导航元件数据的获取和预处理；(1)用激光雷达获取环境点云数据，用惯性传感器获取车辆加速度和角速度数据；(2)去除测量过程中产生的无效值和车辆自身轮廓的无效点；(3)对多传感器间进行空间标定，利用标定好的结果矫正由于车辆运动产生的激光雷达点云畸变。
[0039]
步骤2：使用激光雷达和惯导传感器融合后的信息进行非结构化高动态环境下的点云语义分割。因为非结构化环境下物体会随着作业的进行不断地变化，而室内墙壁相对于动态变化的物体是静止的，所以需要将动态的物体和静态的墙壁分割开。(1)设计适用于
环境特征的线性拟合平面算法提取墙壁，首先将位于世界坐标系下的点云根据1/1800
°
的角分辨率以雷达为原点平行于水平面的位置划分1800个扇形，再以雷达为原点垂直于水平面的方向上将点云划分为90个扇形。通过这两步划分，将三维点云信息进行了降维，大部分点云都能得到标号，利用这些标号能够有序的存储与访问。通过计算同个扇形内相邻点云连线与水平面的夹角，并设定60度的阈值。超过阈值则归类于静态墙壁点云；(2)然后进行点云聚类，通过dbscan聚类后，再分析每个聚类结果中的点云，将高于垂直部分墙壁点云的点保留下，将其作为提取特征点的点云簇，然后输入给前端里程计模块，通过垂直墙壁的投影可以计算最小外接矩形用于仓库角点校准，仓库点云分割结果如图4所示，红色部分点云为地面和货物，梅红色部分点云为垂直墙壁，绿色部分为天花板。
[0040]
步骤3：激光里程计模块流程图如图2所示，基于之前具有语义信息的感知结果对点云数据进行点云降采样和语义特征点提取。(1)选取静态的点，通过计算点位于局部小平面内的曲率来进行特征点的提取；(2)根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标，将特征点分为曲率高的边缘点和曲率低的平面点；(3)去除影响较大的遮挡点和平行点。
[0041]
步骤4：利用分割开动态物体后的特征点进行点云配准。(1)基于不同的特征点采取不同的帧间匹配的策略，帧间配准则根据新的扫描特征点与缓存中的上一时刻的特征进行配准，建立以欧式距离为目标函数，以变换矩阵为优化变量的优化问题对变换矩阵进行求解，即得到移动机器人的局部位姿变换估计值；(2) 同时，以1赫兹的相对较低的频率将当前扫描的特征点与环境中根据主成分分析得到的特征进行配准得到优化后的变换矩阵；(3)优化后的变换矩阵将地图转换到世界坐标系下形成地图，并且对帧间配准得到的实时位姿估计进行修正。
[0042]
步骤5：使用最近邻搜索的动态环境下地图更新方法。对点云数据的三维x、 y、z方向轮流做一次二叉树分割进行kd树的构建，基于kd树进行最近邻搜索。具体步骤为：(1)从根节点开始，通过二叉树的搜索方法，比较查询节点和分裂节点在该维的数值，若前者小于或等于后者，则进入左子树分支，若大于，则进入右子树分支；(2)依照步骤1的逻辑进行向下搜索，直到最邻近的近似点，也就是与查询点处于同一空间的叶节点；(3)回溯搜索路，判断其它路径的叶节点是否是比当前近似点距离更近的点，若是，则更新近似点，若不是，则继续回溯搜索；(4)以查询点与近似的距离为半径，以查询点为圆心创建候选空间，若候选空间与回溯点的分割轴相交，则将轴另一边的节点添加到搜索路径中；(5) 重复第2、3、4步骤直到搜索路径为空，此时保存的近似点则视为查询点的最邻近点。如图3所示，可以通过比较查询点与分割轴的垂直距离和查询半径相比较来进行判断候选空间与分割轴是否相交。其中ri表示候选空间的半径，(xi,yi) 表示查询点的坐标，假设分割点为垂直于轴，则分割轴的表达式为y＝y0。若 |y
i-y0|《r1，则表示候选空间与分割轴相交，需要将分割轴另一边的节点添加到搜索路径中。反之，则表示不相交。将点云信息保存为kd树的结构后，在生成关键帧的同时将关键帧与历史关键帧进行knn搜索，若是搜索到位姿邻近的关键帧，则使用当前的关键帧点云替换历史关键帧点云。从而实现了地图的更新，同时也减小了内存的占用。此方法在非结构化动态的室内环境有良好的定位与建图效果，在室外环境也有较好的效果，在仓库内的定位与建图结果如图5所示，在室外的定位与建图如图6所示，白色的点为车辆历史路经点。
[0043]
需要强调的是，本发明所述的实施案例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方案中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出类似的其它实施方式，同样属于本发明的保护范围。