CN114654471A - 基于激光扫描仪的锚护机械臂避障系统和方法 - Google Patents

Abstract

本发明属于煤矿井下锚护施工技术领域，具体是一种基于激光扫描仪的锚护机械臂避障系统及方法，包括S1、通过激光扫描仪分别采集巷道顶板和左右侧板云点信息；S2、对巷道顶板和左右侧板的云点分别进行平面拟合，分别找到巷道顶板和左右侧板的障碍物云点集；S3、建立锚护机器人机械臂关系坐标系，将巷道顶板和左右侧板的障碍物云点集内的障碍物云点进行坐标转化，转化到锚护机器人坐标系；S4、利用RRT算法计算机械臂末端的工作路径；S5、根据工作路径，控制钻臂完成锚护作业。本发明适用于光照条件不充分的矿井巷道环境，提高了路径规划的准确性。

Description

基于激光扫描仪的锚护机械臂避障系统和方法

技术领域

本发明属于煤矿井下锚护施工技术领域，具体是一种基于激光扫描仪的锚护机械臂避障系统及方法。

背景技术

随着工业化和科技水平的提高，矿井井下自动化生产成为一种趋势。目前锚杆支护依靠人工操作锚护设备避开障碍循环作业。环境建模是现有路径避障技术的基础，有采用数学表达式对环境建模，也有采用机器视觉依靠工业相机的方式对外界空间进行建模。基于以上环境建模的避障路径规划方法存在以下不足：

(1)对外界环境建立表达式的方法，规划效果极其依赖建立的表达式，在巷道环境中，由于地形和障碍物表达式不容易建立，而且随着开采和掘进的深入，外界环境不断发生变化，所以这种方式并不适用；

(2)采用机器视觉使用工业相机的方法，但这种方法的成像效果决定了避障效果。在矿井下，光照条件十分恶劣，成像效果受到严重影响，因此避障效果不佳。

基于以上原因，需要对现有技术的中锚护机械臂避障技术进行改进，提高自动避障效果。

发明内容

本发明克服现有技术存在的不足，所要解决的技术问题为：提供一种基于激光扫描仪的锚护机械臂避障系统及方法，以实现自动锚护机器人的自动作业。

为了解决上述技术问题，本发明采用的技术方案为：一种基于激光扫描仪的锚护机械臂避障方法，包括以下步骤：

S1、将锚护机器人移动到支护作业位置，通过激光扫描仪分别采集巷道顶板和左右侧板云点信息；

S2、对巷道顶板和左右侧板的云点分别进行平面拟合，分别找到巷道顶板和左右侧板的障碍物云点集；

S3、建立锚护机器人机械臂关系坐标系，将巷道顶板和左右侧板的障碍物云点集内的障碍物云点进行坐标转化，转化到锚护机器人坐标系；

S4、利用RRT算法计算机械臂末端的工作路径；

S5、根据工作路径，控制钻臂完成锚护作业。

所述步骤S1还包括以下步骤：

激光扫描仪Ⅰ固定在锚护机器人机械臂上，用来采集巷道顶板云点信息，激光扫描仪Ⅱ固定在锚护机器人顶端，用来采集巷道左边侧帮的云点信息，激光扫描仪Ⅲ(7)固定在锚护机器人后端盖上，用来采集巷道右边侧帮云点信息。

所述步骤S2具体包括以下步骤：

S201、分别将激光扫描仪Ⅰ，激光扫描仪Ⅱ和激光扫描仪Ⅲ采集到的云点信息P₁、P₂和P₃由最小二乘拟合成顶板平面和左右侧板平面，拟合计算公式为：

其中，A₁，B₁，C₁，D₁表示顶板的拟合平面参数，A₂，B₂，C₂，D₂表示左侧板的拟合平面参数，A₃，B₃，C₃，D₃表示右侧板的拟合平面参数，N₁、N₂、N₃分别表示云点信息P₁、P₂和P₃中的云点数量，(x_1n、y_1n、z_1n)、(x_2n、y_2n、z_2n)、(x_3n、y_3n、z_3n)分别表示云点信息P₁、P₂和P₃中第n个云点的三维坐标；

S202、分别确定顶板、左右侧板平面的距离阈值；

S203、计算云点信息P₁、P₂和P₃中的各个云点到对应的拟合平面的距离，若其大于对应的距离阈值，则判断为障碍物云点，将其放入对应的障碍物云点集中。

所述步骤S201中，拟合得到的拟合平面方程为：

顶板：

左侧板：

右侧板：

所述步骤S4中，利用RRT算法计算机械臂末端的工作路径的方法为：

S401、根据锚杆排布要求，确定需要锚孔作业的各个位置，确定起始点锚孔位置Fs和目标点锚孔位置F(s+1)，以起始点锚孔位置Fs作为初始树根节点生成随机树；

S402、在锚护机器人工作臂的工作空间中随机采样得到一个采样点F_rand，此采样点不在障碍物云点集中；

S403、找到随机树中离采样点F_rand最近的一个节点作为近节点F_nearest，在采样点F_rand和近节点F_nearest之间以扩展步长为单位扩展一个新节点F_new；

S404、检测新节点F_new和近节点F_nearest之间是否有障碍物点，若存在，舍弃新节点F_new，返回步骤S403重新扩展新节点F_new，若不存在，进行步骤S405；

S404、将新节点F_new加入到随机扩展树中，并以扩展步长扩展到采样点F_rand；

S405、以采样点F_rand为新的树根节点生成随机树，重复步骤S402～S404，直到找到目标点锚孔位置F(s+1)。

此外，本发明还提供了一种基于激光扫描仪的锚护机械臂避障系统，包括激光扫描仪Ⅰ，激光扫描仪Ⅱ，激光扫描仪Ⅲ和机载数据处理终端，激光扫描仪Ⅰ用于采集巷道顶板云点信息，激光扫描仪Ⅱ用于采集巷道左边侧帮的云点信息，激光扫描仪Ⅲ用于采集巷道右边侧帮云点信息；

数据处理终端用于处理激光扫描仪Ⅰ，激光扫描仪Ⅱ和激光扫描仪Ⅲ采集到的云点信息，并利用RRT算法得到锚护机器人机械臂的运动路径，控制锚护机器人根据运动路径控制钻臂完成锚护作业。

激光扫描仪Ⅰ固定在锚护机器人机械臂上，激光扫描仪Ⅱ固定在锚护机器人顶端，激光扫描仪Ⅲ固定在锚护机器人后端盖上。

所述数据处理终端包括：

障碍物识别单元：将采集到的顶板和左右侧板云点进行平面拟合，得到障碍物云点坐标；

坐标转换单元：用于将障碍物云点坐标转换到锚护机器人坐标系中；

路径规划单元：用于根据障碍物云点坐标，利用RRT算法规划出机械臂的运动路径。

本发明与现有技术相比具有以下有益效果：

(1)本发明通过激光扫描仪分别获取巷道顶板和侧帮环境的云点信息，相比于传统方法，不需要建立复杂的数学表达式来建立环境模型，表达更加直观，能够实时的根据巷道环境规划出避障路径。

(2)本发明通过激光扫描建立环境地图，相比于机器视觉采用工业相机的方式，更适用于光照条件极不充分的矿井巷道环境，提高了避障的准确性。

附图说明

图1为本发明实施例提供的一种基于激光扫描仪的锚护机械臂避障方法的流程示意图；

图2为本发明实施例中激光扫描仪的布置示意图；

图3为锚护机器人机械臂结构示意图；

图4为图3机械臂坐标系示意图；

图5为利用RRT算法规划路径示意图；

图中：1-巷道，2-障碍物Ⅰ，3-障碍物Ⅱ，4-激光扫描仪Ⅰ，5-激光扫描仪Ⅱ，6-机载数据处理终端，7-激光扫描仪Ⅲ，8-锚护机器人，9-钻臂，10-机械臂。11-拟合巷道顶板(或侧帮)平面，12-障碍物Ⅰ，13-障碍物Ⅱ，Fs-当前锚孔位置，F(s+1)目标锚孔位置。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

如图1所示，本发明实施例一提供了一种基于激光扫描仪的锚护机械臂避障方法，包括以下步骤：

S1、将锚护机器人移动到支护作业位置，通过激光扫描仪分别采集巷道顶板和左右侧板云点信息。

如图2所示，本实施例中，锚护机器人包括：激光扫描仪Ⅰ4，激光扫描仪Ⅱ5，激光扫描仪Ⅲ7和机载数据处理终端6。激光扫描仪Ⅰ4固定在锚护机器人机械臂10上，用来采集巷道顶板云点信息，激光扫描仪Ⅱ5固定在锚护机器人8顶端，用来采集巷道左边侧帮的云点信息，激光扫描仪Ⅲ7固定在锚护机器人8后端盖上，用来采集巷道右边侧帮云点信息，激光扫描仪Ⅱ5可以扫描右边侧帮的环境云点，激光扫描仪Ⅲ7可以扫描左边侧帮的环境云点。

S2、对巷道顶板和左右侧板的云点分别进行平面拟合，分别找到巷道顶板和左右侧板的障碍物云点集。

所述步骤S2具体包括以下步骤：

S201、分别将激光扫描仪Ⅰ，激光扫描仪Ⅱ和激光扫描仪Ⅲ采集到的云点信息P₁、P₂和P₃由最小二乘拟合成顶板平面和左右侧板平面，拟合计算公式为：

其中，A₁，B₁，C₁，D₁表示顶板的拟合平面参数，A₂，B₂，C₂，D₂表示左侧板的拟合平面参数，A₃，B₃，C₃，D₃表示右侧板的拟合平面参数，N₁、N₂、N₃分别表示云点信息P₁、P₂和P₃中的云点数量，(x_1n、y_1n、z_1n)、(x_2n、y_2n、z_2n)、(x_3n、y_3n、z_3n)分别表示云点信息P₁、P₂和P₃中第n个云点的三维坐标；

所述步骤S201中，拟合得到的拟合平面方程为：

顶板：

左侧板：

右侧板：

S202、分别确定顶板、左右侧板平面的距离阈值；

S203、计算云点信息P₁、P₂和P₃中的各个云点到对应的拟合平面的距离，若其大于对应的距离阈值，则判断为障碍物云点，将其放入对应的障碍物云点集中。

S3、建立锚护机器人机械臂关系坐标系，将巷道顶板和左右侧板的障碍物云点集内的障碍物云点进行坐标转化，转化到锚护机器人坐标系；

如图3所示，为锚护机器人机械臂结构示意图；建立锚护机器人机械臂关节坐标系，如图4所示。锚护机器人机械臂共有6个自由度，包括绕z₁轴旋转θ₁，绕z₂轴旋转θ₂，沿z₃轴伸缩d₃，绕z₄轴旋转θ₄，绕z₅轴旋转θ₅，绕z₆轴旋转θ₆。O₀，O₁位置为基座上表面，分别对应坐标系S₀，S₁；O₂外套筒回转销中心，对应坐标系S₂；O₃，O₄调平座回转销中心，对应坐标系S₃，S₄；O₅调平座上表面，对应坐标系S₅；O₆回转油缸，对应坐标系S₆；O₇钻架夹钳口，对应坐标系S₇。激光扫描仪Ⅱ5和激光扫描仪Ⅲ7坐标系和锚护机器人坐标系S₀一致。

激光扫描仪Ⅰ4放置在S₂坐标系中，在S₂中的坐标为(x_Ⅰ4-2,y_Ⅰ4-2,0)，O₂在S₁中的坐标为(x_Ⅰ4-1,0,z_Ⅰ4-1)，激光扫描仪Ⅱ5放置在S₀坐标系中，在S₀中的坐标为(x_Ⅱ5,y_Ⅱ5,z_Ⅱ5)，激光扫描仪Ⅲ7放置在S₀坐标系中，在S₀中的坐标为(x_Ⅲ7,y_Ⅲ7,z_Ⅲ7)。上述参数x_Ⅰ4-2,y_Ⅰ4-2，x_Ⅰ4-1，z_Ⅰ4-1，x_Ⅱ5，y_Ⅱ5，z_Ⅱ5，x_Ⅲ7，y_Ⅲ7，z_Ⅲ7可以通过测量获得。

以激光扫描仪Ⅰ4为参照物的

转化到锚护机器人坐标系

机械臂各坐标系之间的齐次变换矩阵：

其中，

表示坐标系S1到坐标系S0的变换矩阵，

表示表示坐标系S2到坐标系S1的变换矩阵，

表示表示坐标系S3到坐标系S2的变换矩阵，

表示表示坐标系S3到坐标系S0的变换矩阵。

障碍物

相对锚护机器人的坐标为：

式中，(x_j1 y_j1 z_j1 1)由(x_j1,y_j1,z_j1)齐次后得到。(x_vj,y_vj,z_vj)＝(d₁₄，d₂₄，d₃₄)。

以激光扫描仪Ⅱ5为参照物的

转换到锚护机器人坐标系

坐标为：

(x_vk,y_vk,z_vk)＝(x_Ⅱ5+x_k1,y_Ⅱ5+y_k1,z_Ⅱ5+z_k1)； (8)

以激光扫描仪Ⅲ7为参照物的

转换到锚护机器人坐标系

坐标为：

(x_vt,y_vt,z_vt)＝(x_Ⅲ7+x_t1,y_Ⅲ7+y_t1,z_Ⅲ7+z_t1)。 (9)

S4、利用RRT算法计算机械臂末端的工作路径。

所述步骤S4中，利用RRT算法计算机械臂末端的工作路径的方法为：

S401、根据锚杆排布要求，确定需要锚孔作业的各个位置，确定起始点锚孔位置Fs和目标点锚孔位置F(s+1)，以起始点锚孔位置Fs作为初始树根节点生成随机树；

S402、在锚护机器人工作臂的工作空间中随机采样得到一个采样点F_rand，此采样点不在障碍物云点集中；

S403、找到随机树中离采样点F_rand最近的一个节点作为近节点F_nearest，在采样点F_rand和近节点F_nearest之间以扩展步长为单位扩展一个新节点F_new；

S404、检测新节点F_new和近节点F_nearest之间是否有障碍物点，若存在，舍弃新节点F_new，返回步骤S403重新扩展新节点F_new，若不存在，进行步骤S405；

S404、将新节点F_new加入到随机扩展树中，并以扩展步长扩展到采样点F_rand；

S405、以采样点F_rand为新的树根节点生成随机树，重复步骤S402～S404，直到找到目标点锚孔位置F(s+1)。

S5、根据工作路径，锚护机器人控制钻臂完成锚护作业。

实施例二

本发明实施例二提供了一种基于激光扫描仪的锚护机械臂避障系统，，包括激光扫描仪Ⅰ，激光扫描仪Ⅱ，激光扫描仪Ⅲ和机载数据处理终端，激光扫描仪Ⅰ用于采集巷道顶板云点信息，激光扫描仪Ⅱ用于采集巷道左边侧帮的云点信息，激光扫描仪Ⅲ用于采集巷道右边侧帮云点信息；数据处理终端用于处理激光扫描仪Ⅰ，激光扫描仪Ⅱ和激光扫描仪Ⅲ采集到的云点信息，并利用RRT算法得到锚护机器人机械臂的运动路径，控制锚护机器人根据运动路径控制钻臂完成锚护作业。

具体地，本实施例中，激光扫描仪Ⅰ固定在锚护机器人机械臂上，激光扫描仪Ⅱ固定在锚护机器人顶端，激光扫描仪Ⅲ固定在锚护机器人后端盖上。

进一步地，本实施例中，数据处理终端包括障碍物识别单元，坐标转换单元和RRT算法单元，障碍物识别单元将采集到的顶板和侧帮云点进行平面拟合，得到障碍物云点坐标，坐标转换单元将障碍物云点坐标转换到锚护机器人坐标系中，路径规划单元根据障碍物云点坐标，利用RRT算法规划出工作臂的运动路径，之后锚护机器人根据运动路径控制钻臂完成锚护作业。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种基于激光扫描仪的锚护机械臂避障方法，其特征在于，包括以下步骤：

S1、将锚护机器人移动到支护作业位置，通过激光扫描仪分别采集巷道顶板和左右侧板云点信息；

S2、对巷道顶板和左右侧板的云点分别进行平面拟合，分别找到巷道顶板和左右侧板的障碍物云点集；

S3、建立锚护机器人机械臂关系坐标系，将巷道顶板和左右侧板的障碍物云点集内的障碍物云点进行坐标转化，转化到锚护机器人坐标系；

S4、利用RRT算法计算机械臂末端的工作路径；

S5、根据工作路径，控制钻臂完成锚护作业。

2.根据权利要求1所述的一种基于激光扫描仪的锚护机械臂避障方法，其特征在于，所述步骤S1还包括以下步骤：

激光扫描仪Ⅰ(4)固定在锚护机器人机械臂(10)上，用来采集巷道顶板云点信息，激光扫描仪Ⅱ(5)固定在锚护机器人(8)顶端，用来采集巷道左边侧帮的云点信息，激光扫描仪Ⅲ(7)固定在锚护机器人(8)后端盖上，用来采集巷道右边侧帮云点信息。

3.根据权利要求1所述的一种基于激光扫描仪的锚护机械臂避障方法，其特征在于，所述步骤S2具体包括以下步骤：

S201、分别将激光扫描仪Ⅰ，激光扫描仪Ⅱ和激光扫描仪Ⅲ采集到的云点信息P₁、P₂和P₃由最小二乘拟合成顶板平面和左右侧板平面，拟合计算公式为：

其中，A₁，B₁，C₁，D₁表示顶板的拟合平面参数，A₂，B₂，C₂，D₂表示左侧板的拟合平面参数，A₃，B₃，C₃，D₃表示右侧板的拟合平面参数，N₁、N₂、N₃分别表示云点信息P₁、P₂和P₃中的云点数量，(x_1n、y_1n、z_1n)、(x_2n、y_2n、z_2n)、(x_3n、y_3n、z_3n)分别表示云点信息P₁、P₂和P₃中第n个云点的三维坐标；

S202、分别确定顶板、左右侧板平面的距离阈值；

S203、计算云点信息P₁、P₂和P₃中的各个云点到对应的拟合平面的距离，若其大于对应的距离阈值，则判断为障碍物云点，将其放入对应的障碍物云点集中。

4.根据权利要求3所述的一种基于激光扫描仪的锚护机械臂避障方法，其特征在于，所述步骤S201中，拟合得到的拟合平面方程为：

顶板：A₁x_1n+B₁y_1n+C₁z_1n+D₁＝0；

左侧板：A₂x_2n+B₂y_2n+C₂z_2n+D₂＝0；

右侧板：A₃x_3n+B₃y_3n+C₃z_3n+D₃＝0。

5.根据权利要求3所述的一种基于激光扫描仪的锚护机械臂避障方法，其特征在于，所述步骤S4中，利用RRT算法计算机械臂末端的工作路径的方法为：

S401、根据锚杆排布要求，确定需要锚孔作业的各个位置，确定起始点锚孔位置Fs和目标点锚孔位置F(s+1)，以起始点锚孔位置Fs作为初始树根节点生成随机树；

S402、在锚护机器人工作臂的工作空间中随机采样得到一个采样点F_rand，此采样点不在障碍物云点集中；

S403、找到随机树中离采样点F_rand最近的一个节点作为近节点F_nearest，在采样点F_rand和近节点F_nearest之间以扩展步长为单位扩展一个新节点F_new；

S404、检测新节点F_new和近节点F_nearest之间是否有障碍物点，若存在，舍弃新节点F_new，返回步骤S403重新扩展新节点F_new，若不存在，进行步骤S405；

S404、将新节点F_new加入到随机扩展树中，并以扩展步长扩展到采样点F_rand；

S405、以采样点F_rand为新的树根节点生成随机树，重复步骤S402～S404，直到找到目标点锚孔位置F(s+1)。

6.一种基于激光扫描仪的锚护机械臂避障系统，其特征在于，包括激光扫描仪Ⅰ，激光扫描仪Ⅱ，激光扫描仪Ⅲ和机载数据处理终端，激光扫描仪Ⅰ用于采集巷道顶板云点信息，激光扫描仪Ⅱ用于采集巷道左边侧帮的云点信息，激光扫描仪Ⅲ用于采集巷道右边侧帮云点信息；

数据处理终端用于处理激光扫描仪Ⅰ，激光扫描仪Ⅱ和激光扫描仪Ⅲ采集到的云点信息，并利用RRT算法得到锚护机器人机械臂的运动路径，控制锚护机器人根据运动路径控制钻臂完成锚护作业。

7.根据权利要求6所述的一种基于激光扫描仪的锚护机械臂避障系统，其特征在于，激光扫描仪Ⅰ固定在锚护机器人机械臂上，激光扫描仪Ⅱ固定在锚护机器人顶端，激光扫描仪Ⅲ固定在锚护机器人后端盖上。

8.根据权利要求6所述的一种基于激光扫描仪的锚护机械臂避障系统，其特征在于，所述数据处理终端包括：

障碍物识别单元：将采集到的顶板和左右侧板云点进行平面拟合，得到障碍物云点坐标；

坐标转换单元：用于将障碍物云点坐标转换到锚护机器人坐标系中；

路径规划单元：用于根据障碍物云点坐标，利用RRT算法规划出机械臂的运动路径。

Images