(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210473585.3 (22)申请日 2022.04.29 (71)申请人 中国煤炭科工集团太原研究院有限 公司 地址 030006 山西省太原市山西示范区科 技创新城科荟路1号 申请人 山西天地煤 机装备有限公司 　 煤炭科学研究总院 (72)发明人 高志强　郭治富　邓镓敏　阎志伟　 金江　刘伟立　赵永红　 (74)专利代理 机构 太原晋科知识产权代理事务 所(特殊普通 合伙) 14110 专利代理师 任林芳　赵江艳 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于激光扫描仪的锚护机械臂避障系统和 方法 (57)摘要 本发明属于煤矿井下锚护施工技术领域， 具 体是一种基于激光扫描仪的锚护机械臂避障系 统及方法， 包括S1、 通过激光扫描仪分别采集巷 道顶板和左右侧板云点信息； S2、 对巷道顶板和 左右侧板的云点分别进行平面拟合， 分别找到巷 道顶板和左右侧板的障碍物云点集； S3、 建立锚 护机器人机械臂关系坐标系， 将巷道顶板和左右 侧板的障碍物云点集内的障碍物云点进行坐标 转化， 转化到锚护机器人坐标系； S4、 利用RRT算 法计算机械臂末端的工作路径； S5、 根据工作路 径， 控制钻臂完成锚护作业。 本发明适用于光照 条件不充分的矿井巷道环境， 提高了路径规划的 准确性。 权利要求书2页 说明书6页 附图3页 CN 114654471 A 2022.06.24 CN 114654471 A 1.一种基于 激光扫描仪的锚护机 械臂避障方法， 其特 征在于， 包括以下步骤： S1、 将锚护机器人移动到支护作业位置， 通过激光扫描仪分别采集巷道顶板和左右侧 板云点信息； S2、 对巷道顶板和左右侧板的云点分别进行平面拟合， 分别找到巷道顶板和左右侧板 的障碍物云点 集； S3、 建立锚护机器人机械臂关系坐标系， 将巷道顶板和左右侧板的障碍物云点集内的 障碍物云点进行坐标转 化， 转化到锚护机器人坐标系； S4、 利用R RT算法计算机 械臂末端的工作路径； S5、 根据工作路径， 控制钻臂完成锚护作业。 2.根据权利要求1所述的一种基于激光扫描仪的锚护机械臂避障方法， 其特征在于， 所 述步骤S1还 包括以下步骤： 激光扫描仪 Ⅰ(4)固定在锚护机器人机械臂(10)上， 用来采集巷道顶板云点信息， 激光 扫描仪Ⅱ(5)固定在锚护机器人(8)顶端， 用来采集巷道左边侧帮的云点信息， 激光扫描仪 Ⅲ(7)固定在锚护机器人(8)后端盖上， 用来采集巷道 右边侧帮云点信息 。 3.根据权利要求1所述的一种基于激光扫描仪的锚护机械臂避障方法， 其特征在于， 所 述步骤S2具体包括以下步骤： S201、 分别将激光扫 描仪Ⅰ， 激光扫描仪 Ⅱ和激光扫描仪 Ⅲ采集到的云点信息P1、 P2和P3 由最小二乘拟合成顶板平面和左右侧板平面， 拟合计算公式为： 其中， A1， B1， C1， D1表示顶板的拟合平面参数， A2， B2， C2， D2表示左侧板的拟合平面参数， A3， B3， C3， D3表示右侧 板的拟合平面参数， N1、 N2、 N3分别表示云点信息P1、 P2和P3中的云点数 量， (x1n、 y1n、 z1n)、 (x2n、 y2n、 z2n)、 (x3n、 y3n、 z3n)分别表示 云点信息P1、 P2和P3中第n个云点的三 维坐标； S202、 分别确定顶板、 左右侧板平面的距离阈值； S203、 计算云点信息P1、 P2和P3中的各个云点到对应的拟合平面的距离， 若其大于对应 的距离阈值， 则判断为障碍物云点， 将其 放入对应的障碍物云点 集中。 4.根据权利要求3所述的一种基于激光扫描仪的锚护机械臂避障方法， 其特征在于， 所 述步骤S201中， 拟合得到的拟合平面方程 为： 顶板： A1x1n+B1y1n+C1z1n+D1＝0； 左侧板： A2x2n+B2y2n+C2z2n+D2＝0； 右侧板： A3x3n+B3y3n+C3z3n+D3＝0。 5.根据权利要求3所述的一种基于激光扫描仪的锚护机械臂避障方法， 其特征在于， 所 述步骤S4中， 利用R RT算法计算机 械臂末端的工作路径的方法为： S401、 根据锚杆排布要求， 确定需要锚孔作 业的各个位置， 确定起始点锚孔位置Fs和目 标点锚孔 位置F(s+1)， 以起始点锚孔 位置Fs作为初始树 根节点生成随机树；权　利　要　求　书 1/2 页 2 CN 114654471 A 2S402、 在锚护机器人工作臂的工作空间中随机采样得到一个采样点Frand， 此采样点不在 障碍物云点 集中； S403、 找到随机树中离采样点Frand最近的一个节点作为近节点Fnearest， 在采样点Frand和 近节点Fnearest之间以扩展步长为单位扩展一个新节点Fnew； S404、 检测新节点Fnew和近节点Fnearest之间是否有障碍物点， 若存在， 舍弃新节点Fnew， 返 回步骤S40 3重新扩展新节点Fnew， 若不存在， 进行步骤S40 5； S404、 将新节点Fnew加入到随机扩展树中， 并以扩展步长扩展到采样点Frand； S405、 以采样点Frand为新的树根节点生成 随机树， 重复步骤S402～S404， 直到找到目标 点锚孔位置F(s+1)。 6.一种基于激光扫描仪的锚护机械臂避 障系统， 其特征在于， 包括激光扫描仪 Ⅰ， 激光 扫描仪Ⅱ， 激光扫描仪 Ⅲ和机载数据处理终端， 激光扫描仪 Ⅰ用于采集巷道顶板云点信息， 激光扫描仪 Ⅱ用于采集巷道左边侧帮的云点信息， 激光扫描仪 Ⅲ用于采集巷道右边侧帮云 点信息； 数据处理终端用于处理激光扫描仪 Ⅰ， 激光扫描仪 Ⅱ和激光扫描仪 Ⅲ采集到的云点信 息， 并利用RRT算法得到锚护机器人机械臂的运动路径， 控制锚护机器人根据运动路径控制 钻臂完成锚护作业。 7.根据权利要求6所述的一种基于激光扫描仪的锚护机械臂避障系统， 其特征在于， 激 光扫描仪 Ⅰ固定在锚护机器人机械臂上， 激光扫描仪 Ⅱ固定在锚护机器人顶端， 激光扫描仪 Ⅲ固定在锚护机器人后端盖上。 8.根据权利要求6所述的一种基于激光扫描仪的锚护机械臂避障系统， 其特征在于， 所 述数据处 理终端包括： 障碍物识别单元： 将采集到的顶板和左右侧板云点进行平面拟合， 得到障碍物云点坐 标； 坐标转换 单元： 用于将障碍物云点 坐标转换到锚护机器人坐标系中； 路径规划单 元： 用于根据障碍物云点 坐标， 利用R RT算法规划出机 械臂的运动路径。权　利　要　求　书 2/2 页 3 CN 114654471 A 3