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

本发明专利技术公开了一用于爆炸物抓取的四轴机械臂控制方法，属于防爆排爆技术领域，能够从根本上降低设备操作难度，提高爆炸物处置效率。包括如下步骤：根据机械臂的末端初始位置P

【技术实现步骤摘要】
一种用于爆炸物抓取的四轴机械臂控制方法
本专利技术属于防爆排爆
，具体涉及一种用于爆炸物抓取的四轴机械臂的控制方法。
技术介绍
当前爆炸物的远程处置方式主要以排爆机器人为主，随着国内技术的发展，国内外的排爆机器人的技术水平已经趋近。但是无论国内还是国外的排爆机器人，机械臂控制均只提供轴空间的操作方式(与之对应的是本专利技术提出的基于笛卡尔空间的操作方式)。如图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.一种用于爆炸物抓取的四轴机械臂控制方法，其特征在于，所述四轴机械臂包括四个关节，从平台连接端到机械臂末端分别为第一肩关节、第二肩关节、肘关节以及腕关节，其中第一肩关节与平台连接，包括如下步骤：/n根据机械臂的末端初始位置P

【技术特征摘要】
1.一种用于爆炸物抓取的四轴机械臂控制方法，其特征在于，所述四轴机械臂包括四个关节，从平台连接端到机械臂末端分别为第一肩关节、第二肩关节、肘关节以及腕关节，其中第一肩关节与平台连接，包括如下步骤：
根据机械臂的末端初始位置P1和目标点P2，对线段P1P2进行位置点的平均划分，所划分的位置点以及P1、P2组成机械臂的末端轨迹；
顺次获取机械臂的末端位置点坐标(pxpypz)，均采用逆运动计算流程计算第一肩关节的轴的旋转角度θ1，第二肩关节的轴的旋转角度θ2，肘关节的轴的旋转角度θ3；
控制端周期性地发送控制指令，所述控制指令中包含逆运动计算流程当前所计算的每个末端位置点对应的θ1，θ2和θ3，用于控制所述机械臂的第一肩关节、第二肩关节和肘关节的轴旋转角度，从而使得所述四轴机械臂末端由初始位置P1到达目标点P2。


2.如权利要求1所述的方法，其特征在于，所述逆运动计算流程具体为：
获取机械臂的末端位置点坐标为(pxpypz)；θ1的范围为(-π/2，3π/2)、θ2的范围为(-π，π/2)，θ3的范围为(-π，0)；设置第一参...

【专利技术属性】
技术研发人员：黄广炎，刘海博，郭庆波，王芳，王博，
申请(专利权)人：北京理工大学重庆创新中心，北京理工大学，
类型：发明
国别省市：重庆;50