[发明专利]一种用于爆炸物抓取的四轴机械臂控制方法

权利要求书

1.一种用于爆炸物抓取的四轴机械臂控制方法，其特征在于，所述四轴机械臂包括四个关节，从平台连接端到机械臂末端分别为第一肩关节、第二肩关节、肘关节以及腕关节，其中第一肩关节与平台连接，包括如下步骤：

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

顺次获取机械臂的末端位置点坐标(px py pz)，均采用逆运动计算流程计算第一肩关节的轴的旋转角度θ₁，第二肩关节的轴的旋转角度θ₂，肘关节的轴的旋转角度θ₃；

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

所述逆运动计算流程具体为：

获取机械臂的末端位置点坐标为(px py pz)；θ₁的范围为(-π/2，3π/2)、θ₂的范围为(-π，π/2)，θ₃的范围为(-π，0)；设置第一参数a₂＝0.394、第二参数a₃＝0.44；

θ₁公式如下：

θ₃公式如下：

θ₂公式如下：

所述控制端周期性地发送控制指令，具体为：

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

所述以20ms为间隔执行逆运动计算流程，之后还包括：

将所计算的每个20ms间隔对应的逆运动计算流程的计算结果按照时间顺序进行拼接，并对拼接后的计算结果再次进行曲线拟合，利用拟合得到曲线，然后分成10个2ms的时间间隔的点，将对应每个时间点的θ₁，θ₂和θ₃送入机械臂的执行器，用来控制不同关节的运动；经过了两次分割，且每次分割的数量均为10，所以该算法称为双十算法；

双十算法满足实时控制的需求。

2.如权利要求1所述的方法，其特征在于，所述对拼接后的计算结果再次进行曲线拟合，具体为：采用二阶抛物线进行曲线拟合。