(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210906471.3 (22)申请日 2022.07.29 (71)申请人 深圳亿嘉和科技研发有限公司 地址 518052 广东省深圳市南 山区粤海街 道大冲社区深南大道9672号大冲商务 中心(一期)2栋4 号楼14C-02 (72)发明人 李勇兵　徐伟　刘文博　李奇　 (74)专利代理 机构 南京瑞弘专利商标事务所 (普通合伙) 32249 专利代理师 梁天彦 (51)Int.Cl. B25J 5/00(2006.01) B25J 9/16(2006.01) B25J 19/02(2006.01) G01S 17/06(2006.01)G01S 17/88(2006.01) G06T 17/05(2011.01) (54)发明名称 室外带电作业机器人及其激光点云建模定 位方法 (57)摘要 本发明提供了一种室外带电作业机器人及 其激光点云建模定位方法， 在移动斗臂之前系统 自动将环 境完成建模， 然后基于点云地图让伸缩 机械臂自主移动到较为理想的作业位置， 接着使 用激光点云精确建模， 就可以让末端操作手高 效、 可靠的完成作业任务， 从而提升作业的自动 化程度， 提升作业效率， 有效降低对专业人员的 依赖。 权利要求书2页 说明书6页 附图3页 CN 115256334 A 2022.11.01 CN 115256334 A 1.一种室外带电作业机器人， 包括斗臂车， 斗臂车上通过伸缩臂安装有末端操作手， 其 特征在于： 所述斗臂车上设置有通过导轨移动的第一激光雷达， 末端操作手上设置有通过 导轨移动的第二激光雷达 。 2.根据权利要求1所述的室外带电作业机器人， 其特征在于： 所述第 一激光雷达和第 二 激光雷达均为多线激光雷达， 其中， 第一激光雷达采用固态激光雷达， 第二激光雷达采用机 械激光雷达 。 3.一种室外带电作业机器人的激光 点云建模定位方法， 其特 征在于： 1)通过斗臂车上沿导轨移动的第 一激光雷达进行全局环境点云建模， 得到全局点云地 图； 2)伸缩臂基于全局点云地图进行规划和运动控制， 通过全局点云找到末端操作手的理 想位置； 3)利用算法规划出伸缩臂的全局路径， 伸缩臂运动向理想位置移动过程中第 二激光雷 达与全局点云地图实时匹配定位； 4)到达理想位置后， 第二激光雷达开始 精确建模， 得到精确的点云地图。 4.根据权利要求3所述的室外带电作业机器人的激光点云建模定位方法， 其特征在于： 步骤1)和步骤4)中所述激光雷达建模过程如下： 假设有k帧激光点云数据， 将k 帧激光数据 提取体征点 拼接到一起得到点云地图。 5.根据权利要求4所述的室外带电作业机器人的激光点云建模定位方法， 其特征在于： 所述点云拼接分为两阶段， 包括 粗匹配和精匹配， 地图使用Pm表示； 粗匹配是指相邻两帧激光数据之间的匹配， 得到相邻两帧之间的位姿关系 精匹配是当前激光数据P(k+1)与前k帧激光数据拼接得到的点云地图匹配， 第k帧激光 与点云地图通过精确匹配得到了全局位姿 通过公式计算可得 此时 计算得到的 是k+1帧激光在Pm的一个不精确的全局 位姿， 以当前 的粗略位姿解算得到 当前激光帧点云在全局坐标系下的坐标， 然后与Pm进行匹配得到更精确的全局位姿 6.根据权利要求5所述的室外带电作业机器人的激光点云建模定位方法， 其特征在于： 所述精匹配时， 相 邻帧之间的坐标使用如下公式将k +1帧的激光点云坐标转换到k帧激光的 坐标系下： 上面式子中R表示k+1帧坐标系到k帧坐标系的旋转矩阵， t表示k+1 帧坐标系到k帧坐标 系的平移向量。 7.根据权利要求5所述的室外带电作业机器人的激光点云建模定位方法， 其特征在于： 所述精匹配时对点云进 行下采样， 下采样策略是5厘米的立方体方格中只保留一个激光点； 精匹配时， 当前帧与Pm匹配的策略是通过kdtree算法在Pm中寻找对应点， 包括角点匹配和 面点匹配； 角点匹配中， 角特征点在Pm中寻找与其距离最近的2个点， 计算点到线的距离； 面 点匹配中， 面特征点在Pm中寻找与其距离最近且不共线的3个点， 计算点到面的距离， 然后 通过代价 函数cost function求解得到当前激光帧在Pm中的位姿。 8.根据权利要求7所述的室外带电作业机器人的激光点云建模定位方法， 其特征在于：权　利　要　求　书 1/2 页 2 CN 115256334 A 2所述角点 匹配中， 为k+1帧激光投影到k帧坐标系下的角点集合； E(k)为第k帧激光 的角点集合； 从 中选取一个点i， 在E(k)中选取与i最近的点j， 以及在E(k)中选取 和点j相邻 扫描线中最近的点 l； 此时选取的3个点： 坐标分别记为 X(k,b),X (k,c)； 通过k+1帧激光的角点a和在k帧激光找到的距离最近的b,c两个激光点计算点a到线 bc的最短距离， 公式如下： 所述面点匹配中， 为第k+1帧激光点云通过投影矩阵(R,t)转换到第k帧坐标系 下的面特征点集合， H(k)为第 k帧激光点云的面特征点集合； 从 中寻找一个点i， 从H (k)中寻找与点i最近的点l， 并找到与点l相同激光扫描束的最近点j， 找寻相邻帧中与点j 最相近的m， 点j和m同属于 H(k)； j， l， m为 不共线的， 能构成一个平面的三个点； 此时选取了4个点： 坐标分别记为： 点i到平面jlm之间的最近距离， 公式表示如下： 9.根据权利要求8所述的室外带电作业机器人的激光点云建模定位方法， 其特征在于： 所述代价 函数cost function如下式表示， 上式中p表示角点的个数， q表示 面点的个数； 通过高斯 ‑牛顿或者列文 ‑马奎特法进行求解， 得到帧间的位姿关系(R,t)后把点云拼 接得到环境的点云模型。 10.根据权利要求4所述的室外带电作业机器人的激光点云建模定位方法， 其特征在 于： 所述激光数据提取过程具体为： 通过邻域点云的曲率大小提取出曲率最大的k个点作为 角点， 曲率最小n个点作为面点， 对相邻帧激光点云的角点和角点进行匹配， 面点和面点进 行匹配。权　利　要　求　书 2/2 页 3 CN 115256334 A 3