(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210393401.2 (22)申请日 2022.04.15 (65)同一申请的已公布的文献号 申请公布号 CN 114494329 A (43)申请公布日 2022.05.13 (73)专利权人 之江实验室 地址 310023 浙江省杭州市余杭区文一西 路1818号 (72)发明人 钟心亮　李月华　朱世强　李强　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 贾玉霞 (51)Int.Cl. G06T 7/20(2017.01) G06T 17/05(2011.01) G06Q 10/04(2012.01) G06K 9/62(2022.01) G06V 10/762(2022.01) (56)对比文件 CN 113867340 A,2021.12.31CN 103869820 A,2014.0 6.18 CN 105843222 A,2016.08.10 US 2015008294 A1,2015.01.08 CN 110221614 A,2019.09.10 US 2021279 950 A1,2021.09.09 JP 2004100148 A,20 04.04.02 耿殿伍等.一种基 于高程图的危险区识别算 法. 《哈尔滨工程大 学学报》 .20 06,(第06期),第 946-949页. brunner m.b brueg gemann et al..Autonomously traversi ng obstacles- metrics for path plan ning of reconfigurable robots o n rough ter rain. 《internati onal conference o n informatics in control》 .2015,第1-10页. (续) 审查员 李亚楠 (54)发明名称 用于移动机器人在非平面环境自主探索的 导引点选取方法 (57)摘要 本发明公开了一种用于移动机器人在非平 面环境自主探索的导引点选取方法， 包括以下步 骤： 1） 按顺序获取3D激光雷达采集的激光帧数据 并确定运动轨迹； 2） 根据激光帧与其对应位姿构 建机器人坐标系下的高程地图； 3） 根据高程地图 计算初始导引点集合 以及每个栅格的坡度与粗 糙度信息； 4） 对初 始导引点集合进行mean ‑shift 聚类以及计算到聚类点的路径5） 根据聚类导引 点以及地图几何信息生成最优导引点。 本发明的 自主导航的导引点选取方法充分考虑了移动机 器人的运动特性， 导引点的选取充分考虑了时间 成本， 空间成本以及安全三大要素， 可用于移动机器人在非平面环境下的自主快速且安全的探 索。 [转续页] 权利要求书2页 说明书6页 附图5页 CN 114494329 B 2022.07.26 CN 114494329 B (56)对比文件 barrientos cruz et al. .Aerial remote sensing in agriculture： a practical approach to area covera ge and path planning for fle ets of mi ni aerial robots. 《journal of field robotics》 .2016, 第28卷第1-10页.2/2 页 2[接上页] CN 114494329 B1.一种用于移动机器人在非平面环境自主探索的导引点选取方法， 其特征在于： 包括 以下步骤： 步骤一： 按顺序获取3D激光雷达采集的激光帧数据并确定运动轨迹， 得到包含运动轨 迹的激光帧序列； 将第一帧激光雷达作为初始帧， 选取一定数量的连续的激光雷达帧构建 局部子地图； 且所述局部子地图的第一帧与最后一帧的位姿的运动平移距离大于预设距离 或旋转角度大于预设角度； 步骤二： 根据激光帧与其对应位姿构建机器人坐标系下的高程地图， 且该高程地图的 每个栅格信息要包 含栅格状态； 所述 栅格状态为占据、 空 闲与未知中的任一种； 步骤三： 根据高程 地图计算初始导引点 集合以及每 个栅格的坡度与粗 糙度信息； 步骤四： 对初始导引点集合进行mean ‑shift聚类， 得到聚类导引点， 并计算当前的机器 人中心到每 个聚类导引点的路径； 步骤五： 根据聚类导引点以及地图几何信息生成最优导引点； 所述步骤五具体通过如下的子步骤来实现： 遍历聚类导引点与其对应路径的集合中的二元组， 通过下式计算每个聚类导引点的最 终收益， 并将收益 最大的聚类导引点作为 最优导引点： 其中， 为三个控制参数， 为导引点 的信息增益， 其定义为以 为中心的栅格 中状态为未知的格子的个数； 为机器人中心与聚类导引点的欧式距离， 为 对 应的路径 中所有点对应栅格的坡度 与粗糙度 之和求平均， ； 为 机器人当前位置, 。 2.根据权利要求1所述的用于移动机器人在非平面环境自主探索的导引点选取方法， 其特征在于， 所述 步骤二具体通过如下的子步骤来实现： 创建以机器人中心为基点， 机器人朝向前左上分别 对应X‑Y‑Z构建坐标系； 构 建以机器 人中心为中心的高程 地图， 该高程 地图的每 个栅格包 含的信息为 ； 其中， 为第c个栅格的中心点坐标， hc为落在该栅格的所有3D 点的最大的 z值，sc为该栅格的状态， 其状态包括占据、 空闲与未知三种， 分别用 occ，free， un表示； 所述高程地图的边长的上限为机器人使用传感器的最大感知距离， 下限为所述预 设距离。 3.根据权利要求2所述的用于移动机器人在非平面环境自主探索的导引点选取方法， 其特征在于， 所述 步骤三具体通过如下的子步骤来实现： （1） 遍历步骤二中高程地图 的每一个栅格， 取出以该栅格为中心的4邻域与8邻域， 若该栅格状态为空闲且其4邻域中至少有一个的状态为未知， 则该栅格被标记为初始候选 导引点， 最终得到初始候选导引点 集合 ； 其中， ， 其中， W为整个高程地图的X方向的长度， H为整个高程地 图的Y方向的长度， r为每 个栅格的分辨 率，c表示第c个栅格， ；权　利　要　求　书 1/2 页 2 CN 114494329 B 3