一种实用的工业六轴机械臂直线运动轨迹规划方法及系统与流程

本发明涉及运动学领域的工业机器人领域，特别涉及一种实用的工业六轴机械臂直线运动轨迹规划方法。此外，本发明还涉及一种实用的工业六轴机械臂直线运动轨迹规划系统。
背景技术：
：随着科技的发展，工业机器人，特别是多自由度机械臂，其应用越来越广泛，且作业环境变得越来越复杂，人们对工业机器人轨迹规划的要求也随之越来越高，不仅要完成点位(PTP)和简单的连续(CP)运动，还需要兼顾机器人的工作效率和复杂轨迹的平滑性。然而，在目前的机器人直线运动轨迹规划方面，机器人的运动过程中，在设定时刻的机械臂末端的位姿不能方便地求取出来，不利于实际工程应用。因此，如何提供一种能够方便求取机械臂末端位姿的方法，是本领域技术人员目前需要解决的技术问题。技术实现要素：有鉴于此，本发明的目的是提供一种实用的工业六轴机械臂直线运动轨迹规划方法，能够方便求取机械臂末端位姿。本发明的另一目的是提供一种实用的工业六轴机械臂直线运动轨迹规划系统，能够方便求取机械臂末端位姿。为实现上述目的，本发明提供如下技术方案：一种实用的工业六轴机械臂直线运动轨迹规划方法，包括以下步骤：接收直线运动参数，包括机械臂末端以笛卡尔空间坐标表示的初始位置点位姿、期望位置点位姿、初始速度、预设加速度、预设减速度以及期望速度；根据所述直线运动参数计算期望位置点与初始位置点之间的路径长度S；根据所述直线运动参数以及所述路径长度S设定速度时间曲线，按照预设规则在所述速度时间曲线上设置插补点；计算所述插补点对应的插补总距离Snum，其中，所述插补点对应的插补距离为以该插补点在所述速度时间曲线上对应的插补速度匀速运行至相邻下一个所述插补点的距离，所述插补点与其之前所有插补点对应的所述插补距离累加得到该插补点对应的插补总距离Snum；结合所述插补点对应的插补总距离Snum占所述路径长度S的百分比、所述初始位置点位姿以及所述期望位置点位姿计算以笛卡尔空间坐标表示的相应的插补点位姿；由所述插补点位姿逆解出相应所述插补点的关节角度，并求出脉冲数。优选地，所述按照预设规则在所述速度时间曲线上设置插补点具体包括：对所述速度时间曲线按照加速运行阶段、减速运行阶段、匀速运行阶段划分运行阶段；在各所述运行阶段中，以预设时长为插补周期设置插补点。优选地，所述根据所述直线运动参数以及所述路径长度S设定速度时间曲线具体包括：利用所述初始速度、所述预设加速度、所述预设减速度、所述期望速度计算加速行走的距离Sa、减速行走的距离Sd；计算路径差Su＝S-Sa-Sd，判断是否所述路径差Su＞0，若是，设定所述速度时间曲线为依次经过加速运行阶段、匀速运行阶段、减速运行阶段的梯形运行曲线，否则，设定所述速度时间曲线为依次经过加速运行阶段、减速运行阶段的三角形运行曲线。优选地，所述计算所述插补点对应的插补总距离Snum中，具体包括：判断所述插补点对应的插补速度是否大于所述期望速度，若大于所述期望速度，停止该工业六轴机械臂直线运动轨迹规划的运行，若不大于所述期望速度，则计算所述插补点对应的所述插补距离。优选地，所述结合所述插补点对应的插补总距离Snum占所述路径长度S的百分比、所述初始位置点位姿以及所述期望位置点位姿计算以笛卡尔空间坐标表示的相应的插补点位姿之后，还包括：判断计算得到的所述插补点位姿是否在预设位姿范围内，若在所述预设位姿范围内，由所述插补点位姿逆解出相应所述插补点的关节角度，并求出脉冲数，若不在所述预设位姿范围内，停止该工业六轴机械臂直线运动轨迹规划的运行。优选地，所述插补周期为1ms。一种实用的工业六轴机械臂直线运动轨迹规划系统，包括：接收模块，用于接收直线运动参数，包括机械臂末端以笛卡尔空间坐标表示的初始位置点位姿、期望位置点位姿、初始速度、预设加速度、预设减速度以及期望速度；第一计算模块，用于根据所述直线运动参数计算期望位置点与初始位置点之间的路径长度S；插补模块，用于根据所述直线运动参数以及所述路径长度S设定速度时间曲线，按照预设规则在所述速度时间曲线上设置插补点；第二计算模块，用于计算所述插补点对应的插补总距离Snum，其中，所述插补点对应的插补距离为以该插补点在所述速度时间曲线上对应的插补速度匀速运行至相邻下一个所述插补点的距离，所述插补点与其之前所有插补点对应的所述插补距离累加得到该插补点对应的插补总距离Snum；第三计算模块，用于结合所述插补点对应的插补总距离Snum占所述路径长度S的百分比、所述初始位置点位姿以及所述期望位置点位姿计算以笛卡尔空间坐标表示的相应的插补点位姿；第四计算模块，由所述插补点位姿逆解出相应所述插补点的关节角度，并求出脉冲数。优选地，所述插补模块具体包括：划分单元，用于对所述速度时间曲线按照加速运行阶段、减速运行阶段、匀速运行阶段划分运行阶段；设定单元，用于在各所述运行阶段中，以预设时长为插补周期设置插补点。优选地，所述插补模块具体还包括：计算单元，用于利用所述初始速度、所述预设加速度、所述预设减速度、所述期望速度计算加速行走的距离Sa、减速行走的距离Sd；运行设定单元，用于计算路径差Su＝S-Sa-Sd，判断是否所述路径差Su＞0，若是，设定所述速度时间曲线为依次经过加速运行阶段、匀速运行阶段、减速运行阶段的梯形运行曲线，否则，设定所述速度时间曲线为依次经过加速运行阶段、减速运行阶段的三角形运行曲线。优选地，所述第二计算模块中，具体包括：第一判断单元，判断所述插补点对应的插补速度是否大于所述期望速度，若大于所述期望速度，停止该工业六轴机械臂直线运动轨迹规划的运行，若不大于所述期望速度，则计算所述插补点对应的所述插补距离。本发明提供的工业六轴机械臂直线运动轨迹规划系统方法以插补点作为设定时刻，可以通过各插补点的插补总距离与路径长度之间的关系计算出各插补点下的机械臂的位置和姿态，计算方便快捷，有利于及时方便地求取处设定时刻的位姿，具有很高的工程应用价值。一种优选的实施例中，通过获取的直线运动参数，不需要输入运行时间就可以根据获取的直线运动参数与路径巧妙地获取轨迹每个运行阶段的运行时间；采用1ms作为插补周期设置插补点，能够快速地获取插补次数以及各插补点的插补距离，在笛卡尔空间坐标下，不需要依赖于关节坐标下的复杂计算，即可在1毫秒时间内快速地进行轨迹插补运算，并且过渡过程中可以采用多次插补点实现位置点的精确转换与姿态快速转变，同时，采用1ms作为插补周期，由于时长极短，因插补运算产生的与实际运行的差值可以忽略不计，且能够保证运动的平滑性。本发明提供的工业六轴机械臂直线运动轨迹规划系统，能够方便求取机械臂末端位姿。附图说明为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。图1为本发明所提供方法的流程图；图2为本发明所提供系统的连接图；图3为梯形运行曲线的示意图；图4为三角形运行曲线的示意图。具体实施方式下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。本发明的核心是提供一种实用的工业六轴机械臂直线运动轨迹规划方法，能够方便求取机械臂末端位姿。本发明的另一核心是提供一种实用的工业六轴机械臂直线运动轨迹规划系统，能够方便求取机械臂末端位姿。请参考图1，图1为本发明所提供方法的流程图。本发明所提供实用的工业六轴机械臂直线运动轨迹规划方法的一种具体实施例中，包括以下步骤：步骤S1：接收直线运动参数，包括机械臂末端以笛卡尔空间坐标表示的初始位置点位姿、期望位置点位姿、初始速度、预设加速度、预设减速度以及期望速度。机器人的运动参数都是从上位机控制界面中获取的，上位机具体可以为示教器。如若需要机器人走直线运动，初始位置点位姿与期望位置点位姿可由笛卡尔空间坐标两个位姿来表示为：初始位置点位姿Ps＝(xs,ys,zs,As,BS,CS)，期望位置点位姿Pe＝(xe,ye,ce,Ae,Be,Ce)。接收的初始速度为vs，期望速度为v，预设加速度为va，预设减速度为vd。其中，期望速度为该直线运动的最大速度，该速度在运行过程中应为可以达到的。步骤S2：根据所述直线运动参数计算期望位置点与初始位置点之间的路径长度S。根据直线运动参数可知，期望位置点与初始位置点之间的路径长度S为：步骤S3：根据所述直线运动参数以及所述路径长度S设定速度时间曲线，按照预设规则在所述速度时间曲线上设置插补点。其中，插补点对应运行过程中的时刻值，即在速度时间曲线上按照预设规则选取若干个时刻作为插补点。步骤S4：计算所述插补点对应的插补总距离Snum，其中，所述插补点对应的插补距离为以该插补点在所述速度时间曲线上对应的插补速度匀速运行至相邻下一个所述插补点的距离，所述插补点与其之前所有插补点对应的所述插补距离累加得到该插补点对应的插补总距离Snum。也就是说，在该步骤中，需要计算出各插补点对应的插补速度、插补距离，以得到该插补点对应的插补总距离Snum。需要说明的是，在步骤S3中，插补点的设置数量应足够多，以保证插补总距离与机械臂末端运行至该插补点时的实际运行总距离之间的差值保持在预设的可接受范围内。步骤S5：结合所述插补点对应的插补总距离Snum占所述路径长度S的百分比、所述初始位置点位姿以及所述期望位置点位姿计算以笛卡尔空间坐标表示的相应的插补点位姿。一个插补点对应的插补总距离为Snum，对应插补总距离(即末端执行器行走路径)占路径长度S的百分比为：Per＝(Snum/S)×100％在该插补点处，机械臂末端对应的以笛卡尔空间坐标表示的相应的插补点位姿为：Ptemp＝(xtemp,ytemp,ztemp,Atemp,Btemp,Ctemp)：其中，xtemp＝xs+Per×(xe-xs)；ytemp＝ys+Per×(ye-ys)；ztemp＝zs+Per×(ze-zs)；Atemp＝As+Per×(Ae-As)；Btemp＝Bs+Per×(Be-Bs)；Ctemp＝Cs+Per×(Ce-Cs)。步骤S6：由所述插补点位姿逆解出相应所述插补点的关节角度，并求出脉冲数。根据获取到笛卡尔空间坐标表示的末端执行器所在的位姿，使用现有的逆解算法，逆解出各个关节角度：θi＝Rev(Ptemp)，其中，i＝1,2,3,4,5,6，i表示该插补点为从初始位置点起的整个运行过程中的第i个插补点；θ＝θi-θi,temp，其中，θi,temp为上一个插补点的关节角度，θi为当前插补点的关节角度，θ为关节角度差。由于机械臂的运动是电机在转动来实现的，同时，电机又靠伺服驱动器来产生脉冲来驱动。每个电机都有不同的轴变比、编码器，所以关系可以求得电机脉冲数量：Plus＝Rat×θ×Res/360，其中，Plus为脉冲数，Rat为轴变比125，Res为编码值131072。将所求的脉冲数写入伺服驱动器中，可以控制电机转动，进而完成直线运动，使机械臂末端到达期望位置点。本实施例基于工程背景，以插补点作为设定时刻，可以通过各插补点的插补总距离与路径长度之间的关系计算出各插补点下的机械臂的位置和姿态，计算方便快捷，有利于及时方便地求取处设定时刻的位姿，具有很高的工程应用价值。进一步地，在步骤S3中，按照预设规则在所述速度时间曲线上设置插补点，具体可以将速度时间曲线设置为梯形运行曲线或三角形运行曲线，具体可以包括：对所述速度时间曲线按照加速运行阶段、减速运行阶段、匀速运行阶段划分运行阶段；在各所述运行阶段中，以预设时长为插补周期设置插补点。通过该实施例所提供的方法，可以方便插补点的设置，使插补点的设置较规则，便于后续计算的进行。更进一步地，在步骤S3中，根据所述直线运动参数以及所述路径长度S设定速度时间曲线具体可以包括：利用所述初始速度、所述预设加速度、所述预设减速度、所述期望速度计算加速行走的距离Sa、减速行走的距离Sd；计算路径差Su＝S-Sa-Sd，判断是否所述路径差Su＞0，请参考图3和图4，若是，设定所述速度时间曲线为依次经过加速运行阶段、匀速运行阶段、减速运行阶段的梯形运行曲线；否则，设定所述速度时间曲线为依次经过加速运行阶段、减速运行阶段的三角形运行曲线。一般而言，机器人的轨迹规划的结果就是能够让电机转动。本实施例所提供的方法中，将速度时间曲线设定为梯形运行曲线或三角形运行曲线，简化了速度时间曲线，从而降低了对电机的控制难度，使机械臂末端能够在给定的运动参数下最快到达期望位置点。其中，根据获取的初始速度vs，期望速度v，预设加速度va，预设减速度vd可知：在加速运行阶段中：加速行走时间：Ta＝(v-vs)/va加速行走距离：Sa＝(vs+0.5×va×Ta)×Ta在减速运行阶段中：减速行走时间：Td＝v/vd减速行走距离：Sd＝(v-0.5×vd×Td)×Td可得，路径差为：Su＝S-Sa-Sd若Su＞0，说明机械臂末端助行器在进行直线运动时，经历加速运动后还有匀速运动，最后进行减速运动到达期望位置点。机械臂轨迹规划转换成电机加速度阶段、然后到匀速阶段最后减速阶段。此时的速度时间曲线为梯形运行曲线，其中，匀速运行阶段行走的时间为Tu＝Su/v。若Su≤0，说明机械臂末端助行器在进行直线运动时，经历加速运动后直接进行减速运动到达期望的位姿。机械臂轨迹规划转换成电机加速度阶段，然后到减速阶段。此时的速度时间曲线为三角形运行曲线。进一步地，为了保证运行的安全性，在步骤S4中，具体可以包括：判断所述插补点对应的插补速度是否大于所述期望速度，若大于所述期望速度，停止该工业六轴机械臂直线运动轨迹规划的运行，若不大于所述期望速度，则计算所述插补点对应的所述插补距离。为了进一步保证计算的准确性和运行的安全性，在步骤S5之后，还可以包括：判断计算得到的所述插补点位姿是否在预设位姿范围内，若在所述预设位姿范围内，执行步骤S6，即由所述插补点位姿逆解出相应所述插补点的关节角度，并求出脉冲数，若不在所述预设位姿范围内，停止该工业六轴机械臂直线运动轨迹规划的运行。其中，预设位姿范围可以根据实际需要进行设定，例如，对于每个插补点下的插补点位姿(xtemp,ytemp,ztemp,Atemp,Btemp,Ctemp)，xtemp范围为(0-1300cm)，ytemp范围为(-1500-1500cm)，ztemp范围为(100-1300cm)，Atemp范围(-180°-180°)，Btemp范围(0°-180°)，Ctemp范围(-180°-180°)。进一步地，当确定机械臂末端执行器按照何种速度时间曲线运行时，为了让末端执行器进行的直线运动轨迹更加平滑与顺畅，可以设定插补周期为1ms。当然，插补周期也可以为0.1ms、0.01ms或者其他数值。在进行插补总距离的计算过程中，具体地，在三角形运行曲线或梯形运行曲线的各运行阶段中，以1ms作为运算单位来获取此运行阶段的插补次数：Num＝T/TS此时，T为每个运行阶段的时长；Ts为插补周期且Ts＝1ms；Num为各运行阶段对应的以1ms作为插补周期的插补次数，同时为该运行阶段对应的插补点的数量。若处于加速运行阶段、减速运行阶段时，轨迹以每1ms的插补周期插补运算时，每次的插补速度为：其中，n表示该插补点为该运行阶段中的第n个插补点，每1ms插补距离为：Stemp＝vt×TS若处于匀速运行阶段时，轨迹以每1ms的插补周期插补运算时，每次的插补速度为：vt＝v，其中，v为期望速度；每1ms插补距离为：Stemp＝vt×TS其中，为了安全考虑，使机械臂在运动过程中每1ms插补速度不会过快，以防影响电机的寿命，每插补一次都会进行一次安全范围的检查，保证vt≤v。在一个插补点处，经过该点与之前每个插补点的插补距离的累加，可知该插补点的插补总距离为：其中，i＝1,2,3,4,5..Num；其中，Stemp(m)为从初始位置点起的整个运行过程中的第m个插补点的插补距离，i表示该插补点为从初始位置点起的整个运行过程中的第i个插补点。举例来说，设定：机器人初始位置点位姿为Px＝(903,0,1120,0,90,0)；期望位置点位姿Pe＝(1050,530,920,15,50,10)；矩阵表示位姿势为：经过走直线的一系列轨迹规划后，各关节转动的角度为：θ1θ2θ3θ4θ5θ627.57929.58577.262-168.6658.043175.78除了上述方法，本发明还提供一种应用上述方法的实用的工业六轴机械臂直线运动轨迹规划系统，请参考图2，图2为本发明所提供系统的连接图。该系统具体包括：接收模块1，用于接收直线运动参数，包括机械臂末端以笛卡尔空间坐标表示的初始位置点位姿、期望位置点位姿、初始速度、预设加速度、预设减速度以及期望速度；第一计算模块2，用于根据所述直线运动参数计算期望位置点与初始位置点之间的路径长度S；插补模块3，用于根据所述直线运动参数以及所述路径长度S设定速度时间曲线，按照预设规则在所述速度时间曲线上设置插补点；第二计算模块4，用于计算所述插补点对应的插补总距离Snum，其中，所述插补点对应的插补距离为以该插补点在所述速度时间曲线上对应的插补速度匀速运行至相邻下一个所述插补点的距离，所述插补点与其之前所有插补点对应的所述插补距离累加得到该插补点对应的插补总距离Snum；第三计算模块5，用于结合所述插补点对应的插补总距离Snum占所述路径长度S的百分比、所述初始位置点位姿以及所述期望位置点位姿计算以笛卡尔空间坐标表示的相应的插补点位姿；第四计算模块6，由所述插补点位姿逆解出相应所述插补点的关节角度，并求出脉冲数。该系统具体可以设置在嵌入式PC控制器中，嵌入式PC控制器中的输入端连接示教器且输出端连接伺服驱动器，伺服启动器的输出端连接六个电机的输入端，六个电机与六轴机械臂相连。本实施例提供的系统中，以插补点作为设定时刻，可以通过各插补点的插补总距离与路径长度之间的关系计算出各插补点下的机械臂的位置和姿态，计算方便快捷，有利于及时方便地求取处设定时刻的位姿，具有很高的工程应用价值。进一步地，插补模块具体可以包括：划分单元，用于对所述速度时间曲线按照加速运行阶段、减速运行阶段、匀速运行阶段划分运行阶段；设定单元，用于在各所述运行阶段中，以预设时长为插补周期设置插补点。进一步地，插补模块具体还可以包括：计算单元，用于利用所述初始速度、所述预设加速度、所述预设减速度、所述期望速度计算加速行走的距离Sa、减速行走的距离Sd；运行设定单元，用于计算路径差Su＝S-Sa-Sd，判断是否所述路径差Su＞0，若是，设定所述速度时间曲线为依次经过加速运行阶段、匀速运行阶段、减速运行阶段的梯形运行曲线，否则，设定所述速度时间曲线为依次经过加速运行阶段、减速运行阶段的三角形运行曲线。进一步地，所述第二计算模块中，具体可以包括：第一判断单元，判断所述插补点对应的插补速度是否大于所述期望速度，若大于所述期望速度，停止该工业六轴机械臂直线运动轨迹规划的运行，若不大于所述期望速度，则计算所述插补点对应的所述插补距离。进一步地，该系统中第三计算模块与第四计算模块之间还可以设有判断模块，用于判断计算得到的所述插补点位姿是否在预设位姿范围内，若在所述预设位姿范围内，驱动第四计算模块，若不在所述预设位姿范围内，停止该工业六轴机械臂直线运动轨迹规划的运行。本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。以上对本发明所提供的工业六轴机械臂直线运动轨迹规划方法及系统进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本
技术领域：
的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。当前第1页1 2 3