一种适用于非结构化动态环境下的同时定位与建图方法，首先，基于惯性元件测量得到的姿态角对激光雷达点云数据去除运动畸变，再对无运动畸变的激光点云数据与惯性元件输出的姿态进行融合，通过几何特征对激光点云分割，得到较准确的船舱环境感知结果；其次，针对需要获取处于船舱内高动态环境下推耙机的实时位姿的情况，提出一种基于语义特征点匹配的激光里程计来估计推耙机在船舱内的位姿。基于多传感器融合得到的感知结果，利用其语义信息进行提取特征点，再采用点到点、点到线两种匹配方式实时估计推耙机的位姿；最后，为了降低点云地图内存占用率，设计了一种基于最近邻搜索的动态环境下地图更新方法，利用该方法降低算法的空间复杂度。法的空间复杂度。法的空间复杂度。
【技术实现步骤摘要】
一种非结构化动态环境下的同时定位与建图方法
[0001]本专利技术涉及非结构化动态环境下的同时定位与建图问题，针对高动态变化的环境，本专利技术提出了利用多传感器融合和语义特征点匹配的激光里程计方法，同时利用基于最近邻搜索的动态地图更新方法保证了非结构化动态环境下的同时定位与建图。
技术介绍
[0002]无人驾驶产业已经踏上了高速发展的道路，面对各种行业的无人驾驶应用，拥有一个完整的无人驾驶系统是不可或缺的。而对于特殊行业的无人驾驶应用，通过合理的利用特殊的环境信息，选用合适的软硬件策略才能促使无人驾驶落地，在一些特殊行业恶劣的工作环境下保障工作人员的身心健康。
[0003]在整个无人驾驶系统中，同时定位与建图是最开始和最基本的步骤，最重要的是正确地感知到外界环境与自身车辆的实时状态，对车身在环境中的位姿进行定位，同时建立外界环境的地图信息，之后才是路径规划和运动控制。感知模块往往要对地面或墙壁等平面进行分割提取，提取平面主要的分割方法有随机采样一致性算法(RANSAC)、区域生长算法和线性拟合地面分段算法等。随机采样一致性算法能够抑制点云噪声对特征分割的影响，且迭代计算效率较高，但是需要人工设置平面阈值进行提前建模，鲁棒性较差。区域增长算法初始点云的选择要求比较高，在区域生长过程中易受噪声点影响。线性拟合地面分段算法利用旋转激光雷达的特性，将每个点云分配到对应的扇形中进行有序化处理，在有序化处理后再对三维数据降维，该操作可以保留竖直维度的信息，同时又不损失水平维度的信息，最后对每个扇形进行拟合直线以及地面点的筛选。
[0004]激光雷达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)本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.非结构化动态环境下的实时定位与建图方法，包括以下步骤：步骤1：进行激光雷达和惯性导航元件数据的获取和预处理，包括：(11)用激光雷达获取环境点云数据，用惯性传感器获取车辆加速度和角速度数据；(12)去除测量过程中产生的无效值和车辆自身轮廓的无效点；(13)对多传感器间进行空间标定，利用标定好的结果矫正由于车辆运动产生的激光雷达点云畸变；步骤2：使用激光雷达和惯导传感器融合后的信息进行非结构化高动态环境下的点云语义分割；因为非结构化环境下物体会随着作业的进行不断地变化，而室内墙壁相对于动态变化的物体是静止的，所以需要将动态的物体和静态的墙壁分割开，包括：(21)设计适用于环境特征的线性拟合平面算法提取墙壁，通过计算同个扇形内相邻点云连线与水平面的夹角，并设定阈值；超过阈值且连续达到一定数量则归类于墙壁点云；(22)然后进行点云聚类，通过DBSCAN聚类后，再分析每个聚类结果中的点云，将高于垂直部分墙壁点云的点保留下，将其作为提取特征点的点云簇，然后输入给前端里程计模块；步骤3：基于之前具有语义信息的感知结果对点云数据进行点云降采样和语义特征点提取，包括：(31)选取静态的点，通过计算点位于局部小平面内的曲率来进行特征点的提取；(32)根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标，将特征点分为曲率高的边缘点和曲率低的平面点；(33)去除影响较大的遮挡点和平行点；步骤4：利用分割开动态物体后的特征点进行点云配准，包括：(41)基于不同的特征点采取不同的帧间匹配的策略，帧间配准则根据新的扫描特征点与缓存中的上一时刻的特征进行配准，建立以欧式距离为目标函数，以变换矩阵为优化变量的优化问题对变换矩阵进行求解，即得到移动机器人的局部位姿变换估计值；(42)以1赫兹的相对较低的频率将当前扫描的特征点与环境中根据主成分分析得到的特征进行配准得到优化后的变换矩阵；(43)优化后的变换矩阵将地图转换到世界坐标系下形成地图，并且对帧间配准得到的实时位姿估计进行修正；步骤5：使用最近邻搜索的动态环境下地图更新方法，包括：(51)对点云数据的三维X、Y、Z方向轮流做一次二叉树分割进行KD树的构建，基于KD树进行最近...
【专利技术属性】
技术研发人员：禹鑫燚，唐浩凯，羊俊华，欧林林，钱学成，周利波，魏岩，
申请(专利权)人：杭州登元科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。