一种用于爆炸物抓取的四轴机械臂控制方法

本发明属于防爆排爆技术领域，具体涉及一种用于爆炸物抓取的四轴机械臂的控制方法。

背景技术：

当前爆炸物的远程处置方式主要以排爆机器人为主，随着国内技术的发展，国内外的排爆机器人的技术水平已经趋近。但是无论国内还是国外的排爆机器人，机械臂控制均只提供轴空间的操作方式(与之对应的是本发明提出的基于笛卡尔空间的操作方式)。

如图1所示，所谓轴空间的操作方式指，对于常用的四轴排爆机器人来讲(机器人的一个控制轴也可称为一个关节)，由两个肩关节，一个肘关节，一个腕关节组成，然后通过腕关节连接用于爆炸物抓取的机械夹爪。轴空间的操作方式即分别控制四个关节的张角来完成对于爆炸物的抓取，每个关节张角需要两个方向的操控，同时机械夹爪需要两个方向的控制，总计需要10个方向的控制，即需要10个控制按钮的组合控制来完成一次抓取动作。此种操作方式不仅操作复杂，而且由于是通过操作各轴来完成对于机械夹爪的位姿的操控，所以非常不直观。

因此目前亟需一种操作简单、对机械臂能实现直观操控的机械臂控制方法，从而提高爆炸物处置效率。

技术实现要素：

有鉴于此，本发明提供了一种用于爆炸物抓取的四轴机械臂控制方法，能够从根本上降低设备操作难度，提高爆炸物处置效率。

为达到上述目的，本发明的技术方案为：四轴机械臂包括四个关节，从平台连接端到机械臂末端分别为第一肩关节、第二肩关节、肘关节以及腕关节，其中第一肩关节与平台连接，包括如下步骤：

根据机械臂的末端初始位置p1和目标点p2，对线段p1p2进行位置点的平均划分，所划分的位置点以及p1、p2组成机械臂的末端轨迹。

顺次获取机械臂的末端位置点坐标(pxpypz)，均采用逆运动计算流程计算第一肩关节的轴的旋转角度θ1，第二肩关节的轴的旋转角度θ2，肘关节的轴的旋转角度θ3。

控制端周期性地发送控制指令，控制指令中包含逆运动计算流程当前所计算的每个末端位置点对应的θ1，θ2和θ3，用于控制机械臂的第一肩关节、第二肩关节和肘关节的轴旋转角度，从而使得四轴机械臂末端由初始位置p1到达目标点p2。

进一步地，逆运动计算流程具体为：

获取机械臂的末端位置点坐标为(pxpypz)；θ1的范围为(-π/2，3π/2)、θ2的范围为(-π，π/2)，θ3的范围为(-π，0)；设置第一参数a2＝0.394、第二参数a3＝0.44。

θ1公式如下：

θ3公式如下：

θ2公式如下：

进一步地，控制端周期性地发送控制指令，具体为：

控制端单次操作的控制间隔时间为200ms，通过线性分割的方式将200ms分成10个20ms的间隔，然后以20ms为间隔执行逆运动计算流程，将逆运动计算流程的计算结果送入机械臂的执行器，用来控制机械臂不同关节的运动。

进一步地，以20ms为间隔执行逆运动计算流程，之后还包括：

将所计算的每个20ms间隔对应的逆运动计算流程的计算结果按照时间顺序进行拼接，并对拼接后的计算结果再次进行曲线拟合，利用拟合得到曲线，将对应每个时间点的θ1，θ2和θ3送入机械臂的执行器。

进一步地，对拼接后的计算结果再次进行曲线拟合，具体为：采用二阶抛物线进行曲线拟合。

有益效果：

1、本发明提供的一种用于爆炸物抓取的四轴机械臂控制方法，采用三轴逆向运动优化算法，在原有的逆运动学的基础上，针对排爆机器人三轴系统进行优化，减少了逆运动的计算时间，便于实现实时控制目的。即本发明可实现直接通过控制机械夹爪末端的三轴坐标的方位完成整个机械臂位置的控制，操作方式自动简单，而且操控直观，从而从根本上降低设备操作难度，提高爆炸物处置效率。

2、本发明提供的一种用于爆炸物抓取的四轴机械臂控制方法，其采用控制端单次操作的控制间隔时间为200ms，通过线性分割的方式分成10个20ms的间隔，然后以20ms为间隔进行逆运动计算，将计算后的结果通过曲线进行拟合，然后分成分成10个2ms的时间间隔的点，最后输出给机械臂的执行器，用来控制不同关节的运动。因为经过了两次分割，且每次分割的数量均为10，所以该算法称为双十算法。双十算法满足实时控制的需求。通过将控制器输出时间间隔进行线性分割，然后进行逆运动计算，再进行曲线拟合与分割，在保证控制精度的前提下极大减少了运算的时间，保证了实时性的要求。

3、本发明中采用二阶抛物线的规划算法。通过对逆运动算法输出散点的拟合，验证二阶抛物线的规划算法具备可行性，在此基础上进行插值算法，可输出满足机械臂执行器要求的2ms间隔点需求。

附图说明

图1为机械臂控制示意图；

图2为三轴逆运动优化算法坐标系说明示意图；

图3为双十算法原理示意图；如坐标系参数参考所示，ai表示当前轴的轴线与前一个轴的轴线的公垂线；di表示当前轴与前一个轴的轴线方向的位移；θi表示不同轴的旋转角度；

图4为不同控制方式400ms时间间隔位置曲线；

图5为控制方式说明示意图；

图6为本发明实施例提供的一种用于爆炸物抓取的四轴机械臂控制方法流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种用于爆炸物抓取的四轴机械臂控制方法，四轴机械臂包括四个关节，从平台连接端到机械臂末端分别为第一肩关节、第二肩关节、肘关节以及腕关节，其中第一肩关节与平台连接，该控制方法的流程如图6所示，包括如下步骤：

步骤一、根据机械臂的末端初始位置p1和目标点p2，对线段p1p2进行位置点的平均划分，所划分的位置点以及p1、p2组成机械臂的末端轨迹。

步骤二、顺次获取机械臂的末端位置点坐标(pxpypz)，均采用逆运动计算流程计算第一肩关节的轴的旋转角度θ1，第二肩关节的轴的旋转角度θ2，肘关节的轴的旋转角度θ3。

本发明实施例中，逆运动计算流程具体为：

获取机械臂的末端位置点坐标为(pxpypz)；θ1的范围为(-π/2，3π/2)、θ2的范围为(-π，π/2)，θ3的范围为(-π，0)；设置第一参数a2＝0.394、第二参数a3＝0.44。

在机械臂本体中构建如下四个坐标系：0号～4号坐标系，其中0号坐标系为平台的固定坐标系，1号坐标系为第一肩关节的轴所在坐标系，2号坐标系为第二肩关节的轴所在的坐标系，3号坐标系为肘关节的轴所在坐标系，4号坐标系为腕关节的轴的所在坐标系。所构建的坐标系如图2所示。

0号坐标系与1号坐标系之间的坐标变换矩阵为：

1号坐标系与2号坐标系之间的坐标变换矩阵为：

2号坐标系与3号坐标系之间的坐标变换矩阵为：

3号坐标系与4号坐标系之间的坐标变换矩阵为：

4号坐标系与0号坐标系之间的坐标变换矩阵为：

θ1公式如下：

θ3公式如下：

θ2公式如下：

步骤三、控制端周期性地发送控制指令，控制指令中包含逆运动计算流程当前所计算的每个末端位置点对应的θ1，θ2和θ3，用于控制机械臂的第一肩关节、第二肩关节和肘关节的轴旋转角度，从而使得四轴机械臂末端由初始位置p1到达目标点p2。

如图3所示，控制端单次操作的控制间隔时间为200ms，通过线性分割的方式分成10个20ms的间隔，然后以20ms为间隔进行逆运动计算，将计算后的结果通过曲线进行拟合，然后分成分成10个2ms的时间间隔的点，最后输出给机械臂的执行器，用来控制不同关节的运动。因为经过了两次分割，且每次分割的数量均为10，所以该算法称为双十算法。

控制端单次操作的控制时间间隔选取具备合理性。选取的间隔不能太长，否则会出现明显的控制卡顿现象，同时实时性不能很好的保证，设备控制的滞后时间等于单次操作的控制时间。选取时间间隔太短，会造成通信负担，特别对于远程控制采用无线方式连接的情况下。

优化后的单次逆运动计算在芯片中的运算时间在2～5ms左右，虽然优化大大减少了计算时间还是不能满足直接输出的需求，主要原因包括：(1)执行器的控制周期2ms小于优化后的单次逆运动的计算时间；(2)逆运动输出的仅是位置参数，不包含速度、加速度参数，为保证输出的平滑性，需要再进一步进行轨迹规划。这也是进行二阶抛物线规划的意义。

二阶抛物线规划属于轨迹规划的范畴，规划的目的是把原来因为逆运动计算拆分的时间间隔重新拼成200ms间距，然后再进行等时间间隔的划分，如图4所示。拼接的目的是便于整体进行抛物线拟合，因为抛物线具备位置、速度、加速度的连续性，所以经过抛物线拟合后的曲线，进行差分导出的位置点，抖动较小稳定性高。单次操作之间的优化算法不在本发明的考虑之内。

因为二阶抛物线的加速度为恒定值，与三阶插值相比加速度性能较差，但考虑排爆机器人的实际的作业情况均在低速下进行，加速度影响较小，同时二阶抛物线的计算效率要高于三级插值，所以此处二阶插值方法最为适合。

图5是对图4的补充，以机械臂笛卡尔空间的前后左右四个方向为例。单次操作为200ms，单次操作的时间间隔200ms是通过电机控制间隔2ms以及相关算法倒推，同时结合人的反应速度综合设定的值。图4给出的曲线的横坐标是时间，总长度为400ms，是控制两次的时间长度。直线控制是400ms一直按控制器的上按键；45度控制是前200ms按上按键，后200ms同时按上与右按键；反向控制是前200ms按上按键，后200ms按后下按键。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。