[发明专利]一种锚杆支护机器人工作臂轨迹规划方法

权利要求书

1.一种锚杆支护机器人工作臂轨迹规划方法，其特征在于：包括以下步骤，

S100～确定巷道安装锚杆处相对锚护机器人的位置信息；

S200～确定锚杆支护机器人工作臂的运动路径；

步骤S200的具体过程为，根据工作臂的避障需求，在起始位置点S₀和终止位置点S_k之间确定一条路径，即使用一条光滑曲线连接起始位置点S₀和终止位置点S_k，并通过包围盒算法检测工作臂按照该路径运动是否会与巷道、锚杆支护机器人自身发生碰撞，然后离散为k-1个路径点[S₁, S₂, S₃, …, S_k-1]，并根据工作臂的运动学模型，计算出所有路径点[S₀, S₁,S₂, S₃, …, S_k-1, S_k]相对于锚杆支护机器人的关节坐标；

S300～根据工作臂运动路径，确定工作臂各关节的B样条轨迹的插值时间和节点矢量；

所述的步骤S300的具体过程为，

B样条轨迹的模型为：

其中，为关节i的B样条轨迹；t为工作臂的运动时间；i表示机器人工作臂的第i个关节；p为B样条轨迹的阶数；为关节i的B样条轨迹的控制点；的表达式为：

式中，时间t_i为B样条轨迹的时间节点矢量；

对分别求一阶导和二阶导，则有：

式中，，

，

，

，

，

在锚杆支护机器人工作臂运动的时间范围中，确定B样条轨迹的时间节点矢量为：

；

S400～确定关节i的B样条轨迹的控制点；

S500～根据S400确定的控制点，确定关节i的B样条轨迹，计算得到关节i的速度和加速度。

2.根据权利要求1所述的锚杆支护机器人工作臂轨迹规划方法，其特征在于：步骤S100的具体过程为，通过锚杆支护机器人视觉模块识别出巷道内需要安装锚杆的位置，选取相邻两个锚固位置作为工作臂的起始位置点S₀和终止位置点S_k，为了满足锚杆支护作业要求，锚杆支护机器人工作臂在起始位置点S₀和终止位置点S_k的速度和加速度均要求为0。

3.根据权利要求2所述的锚杆支护机器人工作臂轨迹规划方法，其特征在于：所述的步骤S400的具体过程为，

根据S100确定的工作臂在起始位置和终止位置速度和加速度均为零，即：

根据下式计算出B样条轨迹的控制点

式中：

，，。