[发明专利]一种非结构化动态环境下的同时定位与建图方法

权利要求书

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树进行最近邻搜索；

(52)将点云信息保存为KD树的结构后，在生成关键帧的同时将关键帧与历史关键帧进行KNN搜索，若是搜索到位姿邻近的关键帧，则使用当前的关键帧点云替换历史关键帧点云，从而实现了地图的更新，同时也减小了内存的占用。

2.如权利要求1所述的非结构化动态环境下的实时定位与建图方法，其特征在于：步骤(21)具体包括：首先将位于世界坐标系下的点云根据1/1800°的角分辨率以雷达为原点平行于水平面的位置划分1800个扇形，再以雷达为原点垂直于水平面的方向上将点云划分为90个扇形；通过这两步划分，将三维点云信息进行了降维，大部分点云都能得到标号，利用这些标号能够有序的存储与访问。通过计算同个扇形内相邻点云连线与水平面的夹角，并设定60度的阈值；超过阈值则归类于静态墙壁点云。

3.如权利要求1所述的非结构化动态环境下的实时定位与建图方法，其特征在于：步骤(51)具体包括：(1)从根节点开始，通过二叉树的搜索方法，比较查询节点和分裂节点在该维的数值，若前者小于或等于后者，则进入左子树分支，若大于，则进入右子树分支；(2)依照步骤1的逻辑进行向下搜索，直到最邻近的近似点，也就是与查询点处于同一空间的叶节点；(3)回溯搜索路，判断其它路径的叶节点是否是比当前近似点距离更近的点，若是，则更新近似点，若不是，则继续回溯搜索；(4)以查询点与近似的距离为半径，以查询点为圆心创建候选空间，若候选空间与回溯点的分割轴相交，则将轴另一边的节点添加到搜索路径中；(5)重复第2、3、4步骤直到搜索路径为空，此时保存的近似点则视为查询点的最邻近点；通过比较查询点与分割轴的垂直距离和查询半径相比较来进行判断候选空间与分割轴是否相交，其中r_i表示候选空间的半径，(x_i,y_i)表示查询点的坐标，假设分割点为垂直于轴，则分割轴的表达式为y＝y₀；若|y_i-y₀|r₁，则表示候选空间与分割轴相交，需要将分割轴另一边的节点添加到搜索路径中；反之，则表示不相交。