一种机器人连续轨迹实现方法与流程

本发明涉及机器人控制技术领域，尤其是指一种机器人连续轨迹实现方法。

背景技术：

随着科学技术的不断发展，工业机器人已广泛应用于各个领域的工业现场。至今，全世界每年都有近十万台新机器人投入运行。在很多特殊情况下，人为作业并不方便，且采用机器人作业，一方面可以缓解工作人员的疲劳，提高生产效率，同时也可避免工作人员的意外发生，因此，提高机器人运动控制技术对未来智能化生产具有重要意义。

现在使用最广的工业机器人，是六自由度垂直多关节机器人。世界上的不同机器人厂家都各有自己的设计标准和结构特点，但主要的机械结构和控制功能都大同小异，并趋于技术的稳定成熟。

机器人的连续轨迹实现是工业机器人实现稳定的核心技术。对于弧焊、涂胶、打磨等机器人应用来说，都希望机器人末端能够以一给定的速度稳定的按照规划的路径运动，并且要避免末端在运动时的抖动、蛇形运动等状况。因此连续轨迹的实现是机器人控制系统的的重点和难点。

技术实现要素：

本发明针对机器人的运动控制技术，提出的一种实现机器人末端连续匀速按规划轨迹运动的流程算法，避免了机器人在运动时震动、蛇形运动等问题。

本发明通过以下技术方案实现该目的：

本发明提出的机器人连续轨迹实现方法的算法在根据每次直线插补的距离和时间知道的情况下，各轴按照同步周期模式进行变化，各轴速度不是匀速，而末端运动是匀速。该方法包括以下步骤(1)至步骤(4)：

(1)人工进行遥操作设定规划点或者根据机器视觉寻找特征点。

(2)根据一种机器人几何逆解算法算得后续过程需要的关节角度。

(3)将规划好的点的信息保存到结构体中，以备末端在按规划路径运动时直接调用结构体中的变量读取各轴需要的变化量并输入给控制器。

(4)规划路径的轨迹再现。

进一步，所述步骤(1)中，在通过遥操作找点时，通过人工操纵遥杆给于计算机信号，末端进行笛卡尔坐标系下的运动。

以x轴正方向移动为例，将机器人初始状态下各轴所在位置的PUU(马达脉冲)换算为相对人工设置原点的角度制，再将各轴的角度给于机器人正解算法，计算出机器人末端当前在笛卡尔坐标系下的坐标位置。

在末端坐标x+1的情况下，通过机器人逆解算法算得x+1变化后的各轴的角度值，再将各轴的角度值转化为PUU以备后续输入给控制器。

若让所有轴按照一个固定的速度直接移动到x+1点，会出现某些轴先到达目标角度，但某些轴还未到达的情况(因每个轴需要移动的角度并不相同)，而导致蛇形运动。因此需要进行速度规划。

x+1后的各轴的角度(PUU)减去之前所在位置各轴的PUU，以移动变化最大的轴为基准，将该移动变化的角度除以规定的速度，得到所有轴移动到x+1处所需要的时间，其他轴根据该时间和自身所需移动的距离求出移动速度。这样就能保证不会出现蛇形运动。

将计算出的各轴的速度送入伺服驱动器，持续叠加驱动马达，即可让机械臂末端持续向x轴正方向移动，其他方向同理。

进一步，所述步骤(2)中，需要通过一种机器人几何逆解算法，通过笛卡尔坐标系下的坐标求出各轴的角度。

该逆解算法主要是通过先转腰，求出θ1＝atan2(PY,PX)，然后将空间几何问题转化为平面几何问题。

不考虑目标点的姿态的问题，以六自由度机器人为例，固定4轴和6轴的旋转，并且规定小臂一直与水平面成一定的角度β，在机械臂上建立新的平面坐标系{x,y}，该坐标系的原点与机器人本身坐标系{X,Y,Z}的原点重合，并且坐标系{x,y}的y轴与坐标系{X,Y,Z}的Z轴重合，这样，当先转腰后，机械臂上的所有点都处于同一个平面上，即可由平面解析几何知识求取各轴的关系。坐标系关系如图1所示。

当在坐标系{x,y}中计算时，各点的表达为：

P(x0,y0):y0＝Pz

P′(x0′,y0′):x0′＝x0-d6*cosβ；y0′＝y0+d6*sinβ

P″(x0″,y0″):x0″＝a2*cosθ2+a1；y0″＝a2*sinθ2

P″′(x0″′,y0″′)：x0″′＝x0″+a3*cos(θ2+θ3)；,y0^③＝y0″+a3*sin(θ2+θ3)

由

展开上式可求得：

因a3、d4为机械手本身参数，是确定的，且△P′P″P″′为Rt△,所以相应的角度也是确定的。

在坐标系{x,y}下，由机械臂构成的结构的几何关系可得：

同时，也可求得

根据几何关系可得：

同样，根据几何关系有：，即

这样，在进行连续轨迹控制的过程中的逆解的有关角度都可由该几何逆解的方法求出，具体平面坐标系下的关系如图2所示。

进一步，所述步骤(3)中，将规划点的相关信息保存到结构体中，在记录规划点信息数据时，还进行了直线插补相关计算以便轨迹规划时使用。

在保存点时，会判断是否为轨迹的第一个点，如果是，则只是保存了当前各轴的绝对位置(PUU)；若是非首个点，则需做进一步计算上一个保存点与当前保存点的机器人正解，得到两个点的空间坐标。

根据两点的空间坐标、插补一次所需要的时间(同步周期)、插补速度(根据具体要求给定)，计算插补次数与末端空间X、Y、Z移动速度，并保存在对应的结构体内。结构体中的具体信息如图3所示。

进一步，所述步骤(4)中，当保存完最后一个点后，进行轨迹再现时，要从轨迹的最后一个点先返回至起始点，这时，要对该两点进行一次直线插补。

在进行两个规划点之间的轨迹再现时，调用之前对该点保存的相关信息，得到两点之间空间X、Y、Z方向的移动速度，进行移动。因两点之间是按照直线插补进行运动，每插补一次，叠加到当前空间位置，得出下一次插补需要移动到的空间位置。

将下一次插补需要移动到的空间位置进行逆解，得到实际每个轴需要移动到的角度，将角度转化为各轴实际需要移动的PUU，给予伺服驱动器，使机器人开始移动。

调用之前保存的点的相关信息，查看当前插补次数是否达到要求，如果没有达到预期插补次数，则继续进行下一次插补；如果达到了预期的插补次数，为了保证机器人末端绝对移动到当时保存该点所在的位置，要将之前保存的轨迹点的各轴的绝对坐标给予伺服驱动器，进行一次周期同步模式，直接移动到保存点。

因两点之间每次插补的距离是一样的，且插补的时间是一样的，但是转化为各轴的角度变化是不一样，连续匀速轨迹的实现是相对于机器人末端来说的，末端的速度是人为给定的，因此，各轴的角速度时刻在变化，但是末端一直按照一定的速度运动，在点与点之间切换时，将上一路径点的终点作为下一路径点的起始点，紧接着就进行下一阶段的插补，这样也就不必担心各轴角速度变化过大而引起的抖动问题。

在机器人开始运动时，速度会由0在一个很短的时间内加速到指定的速度，之后在轨迹再现阶段保持匀速运动，当判断到最后一个轨迹点时，速度会在到达目标点之前同样在一个很短的时间内减速到0。具体的速度变化图如图4所示。

在末端运动完所有的规划点时，即完成了机器人连续轨迹的运动，本发明算法简单，过程简练，大大降低了机器人运动控制系统的复杂性。具体流程图如图5所示。

附图说明

图1是本发明的针对六自由度机械手空间坐标系下手臂结构示意图。

图2是本发明的简化为平面坐标系后的结构示意图。

图3是本发明的保存点信息的结构体的说明。

图4是本发明的机器人启动和停止的速度变化图。

图5是本发明的总体流程图。

具体实施方式

以下结合附图对本发明作进一步说明。

本发明提出的机器人连续轨迹实现方法的算法在根据每次直线插补的距离和时间知道的情况下，各轴按照同步周期模式进行变化，各轴速度不是匀速，而末端运动是匀速。该方法包括以下步骤(1)至步骤(4)：

(1)人工进行遥操作设定规划点或者根据机器视觉寻找特征点。

(2)根据一种机器人几何逆解算法算得后续过程需要的关节角度。

(3)将规划好的点的信息保存到结构体中，以备末端在按规划路径运动时直接调用结构体中的变量读取各轴需要的变化量并输入给控制器。

(4)规划路径的轨迹再现。

进一步，所述步骤(1)中，在通过遥操作找点时，通过人工操纵遥杆给于计算机信号，末端进行笛卡尔坐标系下的运动。

以x轴正方向移动为例，将机器人初始状态下各轴所在位置的PUU(马达脉冲)换算为相对人工设置原点的角度制，再将各轴的角度给于机器人正解算法，计算出机器人末端当前在笛卡尔坐标系下的坐标位置。

在末端坐标x+1的情况下，通过机器人逆解算法算得x+1变化后的各轴的角度值，再将各轴的角度值转化为PUU以备后续输入给控制器。

若让所有轴按照一个固定的速度直接移动到x+1点，会出现某些轴先到达目标角度，但某些轴还未到达的情况(因每个轴需要移动的角度并不相同)，而导致蛇形运动。因此需要进行速度规划。

x+1后的各轴的角度(PUU)减去之前所在位置各轴的PUU，以移动变化最大的轴为基准，将该移动变化的角度除以规定的速度，得到所有轴移动到x+1处所需要的时间，其他轴根据该时间和自身所需移动的距离求出移动速度。这样就能保证不会出现蛇形运动。

将计算出的各轴的速度送入伺服驱动器，持续叠加驱动马达，即可让机械臂末端持续向x轴正方向移动，其他方向同理。

进一步，所述步骤(2)中，需要通过一种机器人几何逆解算法，通过笛卡尔坐标系下的坐标求出各轴的角度。

该逆解算法主要是通过先转腰，求出θ1＝atan2(PY,PX)，然后将空间几何问题转化为平面几何问题。

如图1所示，不考虑目标点的姿态的问题，以六自由度机器人为例，固定4轴和6轴的旋转，并且规定小臂一直与水平面成一定的角度β，在机械臂上建立新的平面坐标系{x,y}，该坐标系的原点与机器人本身坐标系{X,Y,Z}的原点重合，并且坐标系{x,y}的y轴与坐标系{X,Y,Z}的Z轴重合，这样，当先转腰后，机械臂上的所有点都处于同一个平面上，即可由平面解析几何知识求取各轴的关系。

当在坐标系{x,y}中计算时，各点的表达为：

P(x0,y0):y0＝Pz

P′(x0′,y0′):x0′＝x0-d6*cosβ；y0′＝y0+d6*sinβ

P″(x0″,y0″):x0″＝a2*cosθ2+a1；y0″＝a2*sinθ2

P″′(x0″′,y0″′)：x0″′＝x0″+a3*cos(θ2+θ3)；,y0^③＝y0″+a3*sin(θ2+θ3)

由

展开上式可求得：

因a3、d4为机械手本身参数，是确定的，且△P′P″P″′为Rt△,所以相应的角度也是确定的。

在坐标系{x,y}下，由机械臂构成的结构的几何关系可得：

同时，也可求得

根据几何关系可得：

同样，根据几何关系有：，即

这样，在进行连续轨迹控制的过程中的逆解的有关角度都可由该几何逆解的方法求出，具体平面坐标系下的关系如图2所示。

进一步，所述步骤(3)中，将规划点的相关信息保存到结构体中，在记录规划点信息数据时，还进行了直线插补相关计算以便轨迹规划时使用。

在保存点时，会判断是否为轨迹的第一个点，如果是，则只是保存了当前各轴的绝对位置(PUU)；若是非首个点，则需做进一步计算上一个保存点与当前保存点的机器人正解，得到两个点的空间坐标。

根据两点的空间坐标、插补一次所需要的时间(同步周期)、插补速度(根据具体要求给定)，计算插补次数与末端空间X、Y、Z移动速度，并保存在对应的结构体内。结构体中的具体信息如图3所示。

进一步，所述步骤(4)中，当保存完最后一个点后，进行轨迹再现时，要从轨迹的最后一个点先返回至起始点，这时，要对该两点进行一次直线插补。

在进行两个规划点之间的轨迹再现时，调用之前对该点保存的相关信息，得到两点之间空间X、Y、Z方向的移动速度，进行移动。因两点之间是按照直线插补进行运动，每插补一次，叠加到当前空间位置，得出下一次插补需要移动到的空间位置。

将下一次插补需要移动到的空间位置进行逆解，得到实际每个轴需要移动到的角度，将角度转化为各轴实际需要移动的PUU，给予伺服驱动器，使机器人开始移动。

调用之前保存的点的相关信息，查看当前插补次数是否达到要求，如果没有达到预期插补次数，则继续进行下一次插补；如果达到了预期的插补次数，为了保证机器人末端绝对移动到当时保存该点所在的位置，要将之前保存的轨迹点的各轴的绝对坐标给予伺服驱动器，进行一次周期同步模式，直接移动到保存点。

因两点之间每次插补的距离是一样的，且插补的时间是一样的，但是转化为各轴的角度变化是不一样，连续匀速轨迹的实现是对于机器人末端来说，末端的速度是人为给定的，因此，各轴的角速度时刻在变化，但是末端一直按照一定的速度运动，在点与点之间切换时，将上一路径点的终点作为下一路径点的起始点，紧接着就进行下一阶段的插补，这样也就不必担心各轴角速度变化过大而引起的抖动问题。

在机器人开始运动时，速度会由0在一个很短的时间内加速到指定的速度，之后在轨迹再现阶段保持匀速运动，当判断到最后一个轨迹点时，速度会在到达目标点之前同样在一个很短的时间内减速到0。具体的速度变化图如图4所示。

在末端运动完所有的规划点时，即完成了机器人连续轨迹的运动，本发明算法简单，过程简练，大大降低了机器人运动控制系统的复杂性。具体流程图如图5所示。