一种锚杆支护机器人工作臂轨迹规划方法与流程

1.本发明属于锚杆支护机器人自动锚杆支护作业的技术领域，具体是一种锚杆支护机器人工作臂轨迹规划方法。


背景技术：

2.锚杆支护机器人是煤矿巷道支护的重要装备，对巷道高效安全掘进具有巨大的意义。锚杆支护机器人工作臂的轨迹规划是自动化锚杆支护的技术基础。锚杆支护机器人工作臂轨迹规划表示为位移、速度和加速度等位姿运动量关于时间的函数，一般在关节空间和笛卡尔空间中进行。煤矿井下复杂的工作环境，对锚杆支护机器人作业有很高的要求。锚杆支护机器人工作臂在运动过程中需要保证运动平稳且无冲击，同时还需要合理的规避外界环境中的障碍。
3.然而目前应用广泛的点对点轨迹规划方法（多项式轨迹规划、梯形轨迹规划等方法）无法同时满足上述要求，工作臂在相邻两锚孔运动时容易与机身和外界环境发生碰撞，为井下安全支护带来了严重的安全隐患。


技术实现要素：

4.本发明针对点对点轨迹规划方法的问题，提出了一种基锚杆支护机器人工作臂轨迹规划方法。本发明方法能够保证锚杆支护机器人工作臂可以经过预设的路径点，满足锚杆支护机器人工作时的避障要求；同时能够保证锚杆支护机器人工作臂在运动过程中对速度和加速度的连续性要求，使得锚杆支护机器人工作臂运动平稳且无冲击。
5.本发明采取以下技术方案：一种锚杆支护机器人工作臂轨迹规划方法，包括以下步骤，s100～确定巷道安装锚杆处相对锚护机器人的位置信息；s200～确定锚杆支护机器人工作臂的运动路径；s300～根据工作臂运动路径，确定工作臂各关节的b样条轨迹的插值时间和节点矢量；s400～确定关节i的b样条轨迹的控制点p
ij
；s500～根据s400确定的控制点，确定关节i的b样条轨迹，计算得到关节i的速度和加速度。
6.步骤s100的具体过程为，通过锚杆支护机器人视觉模块识别出巷道内需要安装锚杆的位置，选取相邻两个锚固位置作为工作臂的起始位置点s0和终止位置点sk，为了满足锚杆支护作业要求，锚杆支护机器人工作臂在起始位置点s0和终止位置点sk的速度和加速度均要求为0。
7.步骤s200的具体过程为，根据工作臂的避障需求，在起始位置点s0和终止位置点sk之间确定一条路径，即使用一条光滑曲线连接起始位置点s0和终止位置点sk，并通过包围盒算法检测工作臂按照该路径运动是否会与巷道、锚杆支护机器人自身发生碰撞，然后离散为k-1个路径点[s1, s2, s3,
ꢀ…
, s
k-1
]，并根据工作臂的运动学模型，计算出所有路径点[s0, s1, s2, s3,
ꢀ…
, s
k-1
, sk]相对于锚杆支护机器人的关节坐标。
[0008]
步骤s300的具体过程为，
b样条轨迹的模型为：其中，为关节i的b样条轨迹；t为工作臂的运动时间；i表示机器人工作臂的第i个关节；p为b样条轨迹的阶数；为关节i的b样条轨迹的控制点；的表达式为：式中，ti为b样条轨迹的时间节点矢量；对分别求一阶导和二阶导，则有：式中，，，，，，在锚杆支护机器人工作臂运动的时间范围中，确定b样条轨迹的时间节点矢量为：
。
[0009]
步骤s400的具体过程为，根据s100确定的工作臂在起始位置和终止位置速度和加速度均为零，即：根据下式计算出b样条轨迹的控制点式中：，，。
[0010]
与现有技术相比，本发明保留了b样条曲线的优良特性，根据各关节约束条件计算得到的b样条轨迹，既可以保证锚杆机器人工作臂经过预设的路径点，又可以满足锚杆机器人工作臂的速度和加速度等约束条件，使得锚杆支护机器人在井下作业时可以避开外界环境障碍，又可以运动连续平稳。
附图说明
[0011]
图1是本发明的流程图；图2是本发明用于锚杆支护机器人工作臂得到的关节1运动轨迹、运动速度和加速度图；图3是本发明用于锚杆支护机器人工作臂得到的关节2运动轨迹、运动速度和加速度图；图4是本发明用于锚杆支护机器人工作臂得到的关节3运动轨迹、运动速度和加速度图；图5是本发明用于锚杆支护机器人工作臂得到的末端运动路径图。
具体实施方式
[0012]
本发明提出的一种锚杆支护机器人工作臂轨迹规划方法，其步骤为：s100，确定巷道安装锚杆处相对锚护机器人的位置信息。
[0013]
通过锚杆支护机器人视觉模块识别出巷道内需要安装锚杆的位置，然后在巷道空间中计算出一条无障碍路径，离散成k 1个路径点，选取相邻两个锚固位置作为工作臂的起始路径点s0和终止路径点sk。为了满足锚杆支护的作业要求，锚杆支护机器人工作臂在起始位置和终止位置的速度和加速度均要求为0。
[0014]
s200，确定锚杆支护机器人工作臂的运动路径。
[0015]
首先通过md-h参数法得到锚杆支护机器人n自由度工作臂的运动学模型：式中：为工作臂关节n相对n-1的位姿矩阵，为工作臂末端钻架的姿态矩阵，为工作臂末端钻架上的钻头的位置向量。
[0016]
根据工作臂的运动学模型，可以得到工作臂的逆运动学模型：已知安装在工作臂末端钻架上的钻头位姿，即可得到对应的工作臂关节坐标。
[0017]
然后根据工作臂运动的避障需求，由锚杆支护机器人的规划模块确定工作臂在自动锚杆支护时的无障碍运动路径，并根据工作臂的逆运动学模型计算出路径点[s0, s1, s2, s3,
ꢀ…
, s
k-1
, sk]的关节坐标。
[0018]
s300，确定b样条曲线轨迹规划的插值时间和节点矢量。
[0019]
根据b样条曲线的性质，得到关节i的b样条轨迹的模型为：
其中，i表示机器人工作臂的第i个关节；p为b样条轨迹的阶数；为关节i的b样条轨迹的控制点；的表达式为：式中，时间t为b样条轨迹的时间节点矢量。
[0020]
对分别求一阶导和二阶导，则有：式中，，，，，。
[0021]
在锚杆支护机器人工作臂运动的时间范围中，确定b样条轨迹的时间节点矢量为：。
[0022]
s400，确定b样条曲线的控制点。
[0023]
根据s100确定的工作臂在起始位置和终止位置速度、加速度均为零，即：
计算出b样条曲线的控制点坐标其中：，，。
[0024]
式中，为路径点[s0, s1, s2, s3,
ꢀ…
, s
k-1
, sk]对应的关节i的坐标向量（含起始点和终止点的速度和加速度），由步骤s200得到。
[0025]
s500，根据s400确定的控制点，确定关节i的b样条轨迹，进而求一阶导和二阶导得到关节i的速度和加速度。得到的轨迹将作为锚杆支护机器人工作臂控制器的输入，进而实现工作臂在狭小的井下实现无碰撞自动锚杆支护作业。
[0026]
实施例：以锚杆支护机器人从相邻两锚孔之间的路径为例，采用本发明所述的b样条曲线插值轨迹规划方法，取k=6，即路径离散成7个路径点，同时采用4阶b样条曲线，即p=4。
[0027]
根据锚杆支护机器人的视觉模块，识别锚孔，并计算锚孔相对锚杆支护机器人工作臂基坐标系的坐标，得到锚孔相对工作臂基的关节坐标为[-12.4457
°
，14.2120
°
，
443.1127mm，-14.2120
°
，12.4457
°
，0]，得到锚孔相对工作臂基的关节坐标为[-3.9344
°
，14.5603
°
，381.6752mm，-14.5603
°
，3.9344
°
，0]；同时，工作臂在起始位置点和终止位置点处的速度v0、v6和加速度a0、a6均为零。
[0028]
根据锚杆支护机器人工作臂的避障需求，计算路径点[s1, s2, s3, s4, s5]的关节坐标。
[0029]
根据工作臂各关节的速度、加速度和连续性约束条件，求得各分段多项式的插值时间均为1.5s，由此确定b样条轨迹的时间节点为[0, 0, 0, 0, 0, 0.75, 2.25, 3.75, 5.25, 6.75, 8.25, 9, 9, 9, 9, 9]。
[0030]
根据工作臂各关节的时间节点和路径点关节坐标，计算得到各关节的b样条轨迹，关节1、关节2和关节3的轨迹、速度和加速度分别如附图2、附图3和附图4所示，关节4和关节5的轨迹分别与关节2、1的轨迹互为相反数，进而得到工作臂末端的运动路径，如附图5所示。