本发明属于机器人轨迹规划领域，具体涉及一种过渡位置与轨迹误差可控的机械臂直线轨迹自适应衔接规划方法。
背景技术：
机器人轨迹规划中，一个复杂操作命令往往包含有多种连续轨迹，而为提升机器人运动的整体效率，在满足最大轨迹误差的前提下，采用过渡轨迹代替原始轨迹能有效缩短运动时间，保持在连续轨迹上的连续运动。因此，研究一种精确性高、自适应能力强、过渡模型参数计算快速的衔接规划方法，可以有效地解决过渡轨迹的规划问题。
过渡位置与轨迹误差可控的机械臂直线轨迹自适应衔接规划方法，以过渡位置和最大轨迹误差为输入值，并将空间坐标系中的任意连续直线轨迹转换至平面坐标系中以简化运算，在每个分量上都构建三次多项式运动模型，再以机器人的运动学约束求取过渡时间，过渡速度和加速度。
技术实现要素：
本发明为实现机械臂连续直线轨迹的衔接规划，并实现过渡点位置与最大轨迹误差的可控性，并根据给定的原始轨迹自适应规划过渡轨迹和求取过渡点的过渡速度与加速度，提出一种过渡位置与轨迹误差可控的机械臂连续直线轨迹自适应衔接规划方法。该方法包括以下步骤：
步骤(1)：确定过渡点与最大轨迹误差
确定过渡点在原始直线轨迹上的位置，两个过渡点pw1与pw2需要与两段直线轨迹的交点pw3保持一样的距离l，以构建对称性。再确定原始轨迹与过渡轨迹之间所允许的最大轨迹误差值h。
步骤(2)：将两段直线轨迹所在的三维空间坐标系转换为平面坐标系
有且仅有一个交点的两条直线轨迹必然能构成一个平面。因此，以两个过渡点所在的直线为x轴，以垂直于x轴并经过第一过渡点的直线为y轴，将空间中两条相邻的直线轨迹由三维空间坐标系{w}转换到平面坐标系{p}，使得过渡部分恰存在于平面坐标系的第一象限中。两个过渡点之间的距离为d＝||pw1-pw2||，则在平面坐标系中，过渡点的坐标分别为p1＝(0,0)和p2＝(d,0),两段直线轨迹的交点
步骤(3)：过渡运动模型构建
由于原始轨迹存在的对称性，将此特征用于构建过渡轨迹的运动模型。因而，确定在过渡轨迹的顶点p4，速度方向平行于x轴，在y轴方向上的速度分量为0，即为vp＝(vpx,0)，而加速度则平行于y轴，在x轴上的分量为0，ap＝(0,apy)。对于第一过渡点p1，速度为vt＝(vtx,vty)，加速度为at＝(atx,aty)，且满足
以此再结合三次多项式构建从第一过渡点至过渡轨迹顶点的运动模型。在x轴与y轴两个方向上，三次多项式表达式为
其中ax、bx、cx、dx为x轴方向三次多项式模型系数，ay、by、cy、dy为y轴方向三次多项式模型系数；
根据从第一过渡点至过渡轨迹顶点的运动时间th，可得
则可求解多项式系数为
步骤(4)：根据运动学约束求取过渡时间
为确保整体运动都满足运动学约束，需要求取得合适的运动时间th。对于速度约束cv和加速度约束ca，在过渡点和过渡轨迹顶点都需要建立运动学约束。而对于加加速度约束cj，运动中的加加速度jm为恒定常数。建立整体的运动学约束：
选取由上述约束求得的五个时间的最大值作为最短运动时间
步骤(5)：求取过渡速度与过渡加速度
根据已获得的由第一过渡点至过渡轨迹顶点的最短运动时间
最终将整体的过渡轨迹再从平面坐标系还原到三维空间坐标系中，完成机械臂连续直线轨迹规划中过渡位置与轨迹误差可控的过渡轨迹自适应规划。
本发明相对于现有技术所具有的效果：本发明中的规划方法简单高效，具备过渡误差控制性并满足运动学约束，可运用于不同类型的机器人，具有广泛的可移植性。
具体实施方式
一种过渡位置与轨迹误差可控的机械臂连续直线轨迹自适应衔接规划方法。该方法包括以下步骤：
步骤(1)：确定过渡点与最大轨迹误差
确定过渡点在原始直线轨迹上的位置，两个过渡点pw1与pw2需要与两段直线轨迹的交点pw3保持一样的距离l，以构建对称性。再确定原始轨迹与过渡轨迹之间所允许的最大轨迹误差值h。
步骤(2)：将两段直线轨迹所在的三维空间坐标系转换为平面坐标系
有且仅有一个交点的两条直线轨迹必然能构成一个平面。因此，以两个过渡点所在的直线为x轴，以垂直于x轴并经过第一过渡点的直线为y轴，将空间中两条相邻的直线轨迹由三维空间坐标系{w}转换到平面坐标系{p}，使得过渡部分恰存在于平面坐标系的第一象限中。两个过渡点之间的距离为d＝||pw1-pw2||，则在平面坐标系中，过渡点的坐标分别为p1＝(0,0)和p2＝(d,0),两段直线轨迹的交点
步骤(3)：过渡运动模型构建
由于原始轨迹存在的对称性，将此特征用于构建过渡轨迹的运动模型。因而，确定在过渡轨迹的顶点p4，速度方向平行于x轴，在y轴方向上的速度分量为0，即为vp＝(vpx,0)，而加速度则平行于y轴，在x轴上的分量为0，ap＝(0,apy)。对于第一过渡点p1，速度为vt＝(vtx,vty)，加速度为at＝(atx,aty)，且满足
以此再结合三次多项式构建从第一过渡点至过渡轨迹顶点的运动模型。在x轴与y轴两个方向上，三次多项式表达式为
其中ax、bx、cx、dx为x轴方向三次多项式模型系数，ay、by、cy、dy为y轴方向三次多项式模型系数；
根据从第一过渡点至过渡轨迹顶点的运动时间th，可得
则可求解多项式系数为
步骤(4)：根据运动学约束求取过渡时间
为确保整体运动都满足运动学约束，需要求取得合适的运动时间th。对于速度约束cv和加速度约束ca，在过渡点和过渡轨迹顶点都需要建立运动学约束。而对于加加速度约束cj，运动中的加加速度jm为恒定常数。建立整体的运动学约束：
选取由上述约束求得的五个时间的最大值作为最短运动时间
步骤(5)：求取过渡速度与过渡加速度
根据已获得的由第一过渡点至过渡轨迹顶点的最短运动时间
最终将整体的过渡轨迹再从平面坐标系还原到三维空间坐标系中，完成机械臂连续直线轨迹规划中过渡位置与轨迹误差可控的过渡轨迹自适应规划。