(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210095426.4 (22)申请日 2022.01.26 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 申请人 杭州登元 科技有限公司 (72)发明人 禹鑫燚　唐浩凯　羊俊华　欧林林　 钱学成　周利波　魏岩　 (74)专利代理 机构 杭州天正专利事务所有限公 司 33201 专利代理师 王兵　王幸祥 (51)Int.Cl. G06T 17/05(2011.01) G06V 20/56(2022.01) G06K 9/62(2022.01)G01S 13/86(2006.01) G06V 10/44(2022.01) (54)发明名称 一种非结构化动态环境下的同时定位与建 图方法 (57)摘要 一种适用 于非结构化动态环境下的同时定 位与建图方法， 首先， 基于惯性元件测量得到的 姿态角对激光雷达点云数据去除运动畸变， 再对 无运动畸变的激光点云数据与惯性元件输出的 姿态进行融合， 通过几何特征对激光点云分割， 得到较准确的船舱环境感知结果； 其次， 针对需 要获取处于船舱内高动态环境下推耙机的实时 位姿的情况， 提出一种基于语义特征点匹配的激 光里程计来估计推耙机在船舱内的位姿。 基于多 传感器融合得到的感知结果， 利用其语义信息进 行提取特征点， 再采用点到点、 点到线两种匹配 方式实时估计推耙机的位姿； 最后， 为了降低点 云地图内存占用率， 设计了一种基于最近邻搜索 的动态环 境下地图更新方法， 利用该方法降低算 法的空间 复杂度。 权利要求书2页 说明书5页 附图3页 CN 114565726 A 2022.05.31 CN 114565726 A 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 °的角分辨率以雷达为原点平 行于水平 面的位置划分18 00个扇形， 再以雷达为原 点垂直于水平 面的方向上将点云划分为 90个扇形； 通过这两步划分， 将三维点云信息进行了降维， 大部分点云都能得到标号， 利用 这些标号能够有序的存储与访问。 通过计算同个扇形内相邻点云连线与水平面的夹角， 并 设定60度的阈值； 超过阈值则归类于静态墙壁 点云。 3.如权利要求1所述的非结构化动态环境下的实时定位与建图方法， 其特征在于： 步骤权　利　要　求　书 1/2 页 2 CN 114565726 A 2(51)具体包括： (1)从根节点开始， 通过二叉树的搜索方法， 比较查询节点和分裂节点在该 维的数值， 若前者小于或等于后者， 则进入左子树分支， 若大于， 则进入右子树分支； (2)依 照步骤1的逻辑进 行向下搜索， 直到最邻近的近似点， 也就是与查询点处于同一空间的叶节 点； (3)回溯搜索路， 判断其它路径的叶节点是否是比当前近似点距离更近的点， 若 是， 则更 新近似点， 若不是， 则继续回溯搜索； (4)以查询点与近似的距离为半径， 以查询点为圆心创 建候选空间， 若候选空间与回溯点的分割轴相交， 则将轴另一边的节点添加到搜索路径中； (5)重复第2、 3、 4步骤直到搜索路径为空， 此时保存的近似点则视为查询点的最邻近点； 通 过比较查询点与分割轴的垂直距离和 查询半径相比较来进行判断候选空间与分割轴是否 相交， 其中ri表示候选空间的半径， (xi,yi)表示查询点的坐标， 假设分割点为垂直于轴， 则 分割轴的表达式为y＝y0； 若|yi‑y0|<r1， 则表示候选空间与分割轴相交， 需要将分割轴另一 边的节点添加到 搜索路径中； 反 之， 则表示 不相交。权　利　要　求　书 2/2 页 3 CN 114565726 A 3