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

说明书

本发明公开了一种锚杆支护机器人的工作臂轨迹规划方法及系统，涉及自动化锚杆支护技术领域，所述工作臂轨迹规划方法包括：根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置；根据标记关节的起终位置，确定标记关节的无障碍行驶轨迹；对标记关节的无障碍行驶轨迹进行离散处理，确定标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间；根据分段多项式算法、离散段数和每个离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定剩余关节的行驶轨迹。本发明通过采用分段多项式插值算法，保证了锚杆支护机器人的工作臂运动轨迹平滑。

技术领域

本发明涉及自动化锚杆支护技术领域，特别是涉及一种锚杆支护机器人的工作臂轨迹规划方法及系统。

背景技术

锚杆支护机器人是煤矿巷道支护的重要装备，对巷道高效安全掘进具有巨大的意义。锚杆支护机器人的工作臂轨迹规划是自动化锚杆支护的技术基础。锚杆支护机器人的工作臂轨迹规划表示为位移、速度、加速度等位姿运动量关于时间的函数，一般在关节空间或笛卡尔空间中进行规划和表示。由于煤矿井下的工作环境复杂，因此对锚杆支护机器人作业有很高的要求。锚杆支护机器人的工作臂在运动过程中需要合理地规避外界环境中的障碍，同时还需要保证运动平稳且无冲击。

目前，锚杆支护机器人的工作臂轨迹规划一般采用点对点轨迹规划，但是点对点的轨迹规划无法同时满足上述要求，且工作臂在两锚孔间运动时容易与机身或外界环境发生碰撞，为井下安全支护带来了严重的安全隐患。

发明内容

本发明的目的是提供一种锚杆支护机器人的工作臂轨迹规划方法及系统，保证锚杆支护机器人的工作臂运动轨迹平滑。

为实现上述目的，本发明提供了如下方案：

一种锚杆支护机器人的工作臂轨迹规划方法，包括：

根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置；所述标记关节为工作臂的任意关节；

根据所述标记关节的起终位置，确定所述标记关节的无障碍行驶轨迹；

对所述标记关节的无障碍行驶轨迹进行离散处理，确定所述标记关节的无障碍行驶轨迹的离散段数和每个离散段对应的离散时间；

根据分段多项式算法、所述离散段数和每个所述离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；所述剩余关节为工作臂中除所述标记关节之外的任意关节。

可选地，所述离散时间的计算过程，具体包括：

根据公式计算所述标记关节在第n离散段对应的离散时间；

其中，v_max为工作臂最大速度的速度约束，P_n为所述标记关节的第n离散段的末尾路径点，P_n-1为所述标记关节的第n离散段的起始路径点。

可选地，所述根据分段多项式算法、所述离散段数和每个所述离散段对应的离散时间，对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹，具体包括：

根据公式

对每个剩余关节的行驶路径进行规划，确定所述剩余关节的行驶轨迹；

其中，s_i(t)为第i个剩余关节的行驶轨迹，s_in(t)为第i个剩余关节在第n离散段的行驶轨迹，t_n为标记关节在第n离散段对应的离散时间，n＝1,2,…,k，k为离散段数；a_i1m、a_i2l、…、a_i(k-1)l、a_ikm均为系数向量，m＝0,1,2,3,4，l＝0,1,2,3。

可选地，所述根据第一锚孔位置和第二锚孔位置确定标记关节的起终位置，具体包括：