(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210966019.6 (22)申请日 2022.08.12 (71)申请人 亿嘉和科技股份有限公司 地址 210012 江苏省南京市雨 花台区安德 门大街57号5幢1楼至 3楼、 8楼至12 楼 (72)发明人 徐倩南　 (74)专利代理 机构 南京瑞弘专利商标事务所 (普通合伙) 32249 专利代理师 梁天彦 (51)Int.Cl. G06T 7/73(2017.01) G06V 10/75(2022.01) G06T 17/05(2011.01) G06T 1/00(2006.01) B25J 19/00(2006.01) (54)发明名称 基于点云匹配的巡检机器人充电控制方法 和系统 (57)摘要 本发明公开了一种基于点云匹配的巡检机 器人充电控制方法和系统， 方法包括步骤： 巡检 机器人接收到充电指令后， 运行到充电点； 采集 巡检机器人静止状态下的实时激光点云数据； 将 采集到的实时激光点云数据与预存的充电点激 光数据进行点云匹配， 得到巡检机器人的位姿差 值信息； 根据位姿差值信息进行位姿调整； 巡检 机器人位姿调整好后， 退到设置于充电桩与目标 充电点之间的目标位置， 巡检机器人的充电针插 入充电桩， 充电桩上电， 开始充电。 本发 明不依赖 充电房内部特征， 依据充电点的点云数据进行匹 配定位， 操作简单， 定位精确， 提高了充电成功 率。 权利要求书2页 说明书6页 附图7页 CN 115311361 A 2022.11.08 CN 115311361 A 1.一种基于点云匹配的巡检机器人充电控制方法， 其特征在于， 执行主体在于巡检机 器人， 包括 步骤： 接收充电指令， 获取充电指令中的目标充电点信息， 运动到目标充电点； 接收位姿差值信息， 根据位姿差值信息进行位姿调整； 位姿调整好后， 退到设置于目标充电点与充电桩之间的目标位置， 伸出充电针并插入 充电桩， 开始充电； 接收结束 充电指令， 收回充电针， 返回到目标充电点， 结束 充电； 其中， 所述位姿差值信息， 通过采集巡检机器人在充电点处静止状态下的实时激光点 云数据， 并将实时激光点云数据与预存的充电点激光数据进行点云匹配获得， 实时激光点 云数据是巡检机器人在当前充电点位姿下所扫描到的周围环境信息， 预存的充电点激光数 据是在采集 地图点位时所录取的巡检机器人位于充电点处激光扫描到的周围环境信息 。 2.一种基于点云匹配的巡检机器人充电控制方法， 其特征在于， 执行主体在于巡检机 器人服务器， 包括 步骤： 向巡检机器人发送充电指令； 待巡检机器人运行到设置于目标充电点 时， 采集巡检机器人静止状态下的实时激光点 云数据； 将采集到的实时激光点云数据与 预存的充电点激光数据进行点云匹配， 预存的充电点 激光数据是在采集地图点位时所录取的巡检机器人位于充电点处激光扫描到的周围环境 信息， 得到 巡检机器人的当前位姿与采集 地图点位时巡检机器人位姿的位姿差值信息； 巡检机器人位姿调整好后， 退到设置于目标充电点与充电桩之间的目标位置， 待巡检 机器人的充电针插 入充电桩后， 使 充电桩上电。 3.根据权利要求2所述的一种基于点云匹配的巡检机器人充电控制方法， 其特征在于， 充电桩上电后， 执 行以下步骤： 检测巡检机器人的充电针的 电流或电压， 判断电流或电压的数据是否正常， 如果数据 正常， 判断充电成功， 否则， 判断充电不成功； 统计判断巡检机器人在目标充电桩上充电不成功 的次数， 如果小于或等于预设次数， 向巡检机器人发出充电指令， 否则， 判断充电失败， 发出警告， 结束 充电。 4.根据权利要求2所述的一种基于点云匹配的巡检机器人充电控制方法， 其特征在于， 巡检机器人服 务器执行以下结束充电步骤： 向巡检机器人发送结束 充电指令； 判断巡检机器人是否在目标位置上， 如果是， 则使充电桩断电， 待巡检机器人收回充电 针、 返回充电点后， 结束 充电； 否则， 结束 充电。 5.根据权利要求1或2任一所述的基于点云匹配的巡检机器人充电控制方法， 其特征在 于， 采集多帧实时激光点云数据， 逐帧采用PLICP算法与预存的充电点激光数据进 行点云匹 配， 包括步骤： 将当前帧数据根据初始位姿投影到参 考帧坐标系下； 对于当前帧的点用最近邻法则在参 考帧中找到最近的两个点； 通过最近的两点连线， 计算当前帧的点到连线的距离作为 误差， 并去除误差过 大的点； 根据最小化 误差函数求 解旋转矩阵和平 移矩阵；权　利　要　求　书 1/2 页 2 CN 115311361 A 2对当前帧使用上一 步求得的平 移和旋转 参数， 得到新的变换点 集； 如果新的变换点集与参考点集满足两点集的平均距离小于某一给定 阈值， 则停止迭代 计算， 否则新的变换点 集作为新的当前帧继续迭代， 直到 达到目标函数的要求。 6.根据权利要求5所述的基于点云匹配的巡检机器人充电控制方法， 其特征在于， 目标 函数的计算公式如下： 其中， Pi表示第i个采样点， 为采样点在参考帧坐标系下最近邻的一个匹配点， ni为 两个最近邻匹配点的法向量， Rk+1,tk+1分别表示旋转向量和平 移向量。 7.一种基于点云匹配的巡检机器人充电系统， 其特征在于： 包括巡检机器人， 所述巡检 机器人设有控制指令 接收模块和位姿调整驱动模块， 控制指令 接收模块， 用于 接收外部发送的充电指令和结束 充电指令； 位姿调整驱动模块， 用于接收位姿差值信息， 根据位姿差值信息对巡检机器人进行位 姿调整； 导航模块， 用于根据充电指令 中的目标充电点信息， 运动到目标充电点， 或者在位姿调 整好后， 驱动巡检机器人 退到设置 于充电桩与目标充电点之间的目标位置； 充电针控制模块， 用于在控制充电针伸出并插 入充电桩， 或者从充电桩脱离 。 8.根据权利要求7所述的一种基于点云匹配的巡检机器人充电系统， 其特征在于： 包括 巡检机器人服务器， 巡检机器人服务器中设有充电点激光数据包录制模块、 激光静态匹配 模块、 实时数据采集模块和充电控制模块， 其中， 充电控制模块， 用于向巡检机器人发送充电指令和结束 充电指令； 充电点激光数据包录制模块， 用于在新建充电桩和充电点时， 预存巡检机器人位于充 电点且为静止状态时的充电点激光数据， 预存的充电点激光数据是在 采集地图点位时所录 取的巡检机器人位于充电点处激光扫描到的周围环境信息； 实时数据采集模块， 用于在巡检机器人运行到设置于目标充电点时， 采集巡检机器人 静止状态下的实时激光 点云数据； 激光静态匹配模块， 用于将采集到的实时激光点云数据与预存的充电点激光数据进行 点云匹配， 得到巡检机器人的当前位姿与采集地图点位时巡检机器人位姿的位姿差值信 息， 实时激光 点云数据是巡检机器人在当前充电点 位姿下所扫描到的周围环境信息； 发送模块， 用于将点云匹配得到的位姿差值信 息， 发送给巡检机器人， 所述位姿差值信 息， 通过采集巡检机器人在充电点处静止状态下 的实时激光点云数据， 并将实时激光点云 数据与预存的充电点激光数据进行点云匹配获得。权　利　要　求　书 2/2 页 3 CN 115311361 A 3