CN109278041A - 一种过渡误差可控的机械臂直线轨迹自适应衔接规划方法 - Google Patents

Abstract

本发明涉及一种过渡误差可控的机械臂直线轨迹自适应衔接规划方法，本发明包含确定过渡点与最大轨迹误差，空间坐标系与平面坐标系转换，过渡运动模型构建，根据运动学约束求取过渡时间和实际过渡速度与过渡加速度确定五个部分。本发明以实现对过渡点和最大轨迹误差的控制为基础，求取精确的过渡运动时间，对过渡过程产生精确控制，并确保生成的过渡运动整体都满足运动学约束。规划过程简单高效，适用于高实时性要求的连续直线轨迹的衔接规划，并对于任意类型的机器人轨迹规划都有适用性。

Description

一种过渡误差可控的机械臂直线轨迹自适应衔接规划方法

技术领域

本发明属于机器人轨迹规划领域，具体涉及一种过渡位置与轨迹误差可控的机械臂直线轨迹自适应衔接规划方法。

背景技术

机器人轨迹规划中，一个复杂操作命令往往包含有多种连续轨迹，而为提升机器人运动的整体效率，在满足最大轨迹误差的前提下，采用过渡轨迹代替原始轨迹能有效缩短运动时间，保持在连续轨迹上的连续运动。因此，研究一种精确性高、自适应能力强、过渡模型参数计算快速的衔接规划方法，可以有效地解决过渡轨迹的规划问题。

过渡位置与轨迹误差可控的机械臂直线轨迹自适应衔接规划方法，以过渡位置和最大轨迹误差为输入值，并将空间坐标系中的任意连续直线轨迹转换至平面坐标系中以简化运算，在每个分量上都构建三次多项式运动模型，再以机器人的运动学约束求取过渡时间，过渡速度和加速度。

发明内容

本发明为实现机械臂连续直线轨迹的衔接规划，并实现过渡点位置与最大轨迹误差的可控性，并根据给定的原始轨迹自适应规划过渡轨迹和求取过渡点的过渡速度与加速度，提出一种过渡位置与轨迹误差可控的机械臂连续直线轨迹自适应衔接规划方法。该方法包括以下步骤：

步骤(1)：确定过渡点与最大轨迹误差

确定过渡点在原始直线轨迹上的位置，两个过渡点P_W1与P_W2需要与两段直线轨迹的交点P_W3保持一样的距离L，以构建对称性。再确定原始轨迹与过渡轨迹之间所允许的最大轨迹误差值H。

步骤(2)：将两段直线轨迹所在的三维空间坐标系转换为平面坐标系

有且仅有一个交点的两条直线轨迹必然能构成一个平面。因此，以两个过渡点所在的直线为x轴，以垂直于x轴并经过第一过渡点的直线为y轴，将空间中两条相邻的直线轨迹由三维空间坐标系{W}转换到平面坐标系 {P}，使得过渡部分恰存在于平面坐标系的第一象限中。两个过渡点之间的距离为D＝||P_W1-P_W2||，则在平面坐标系中，过渡点的坐标分别为P₁＝(0,0)和 P₂＝(D,0),两段直线轨迹的交点过渡轨迹的顶点为第一条直线轨迹在平面空间中的斜率为K＝P_3y/P_3x；

步骤(3)：过渡运动模型构建

由于原始轨迹存在的对称性，将此特征用于构建过渡轨迹的运动模型。因而，确定在过渡轨迹的顶点P₄，速度方向平行于x轴，在y轴方向上的速度分量为0，即为V_p＝(V_px,0)，而加速度则平行于y轴，在 x轴上的分量为0，A_p＝(0,A_py)。对于第一过渡点P₁，速度为V_t＝(V_tx,V_ty)，加速度为A_t＝(A_tx,A_ty)，且满足

以此再结合三次多项式构建从第一过渡点至过渡轨迹顶点的运动模型。在x轴与y轴两个方向上，三次多项式表达式为

其中a_x、b_x、c_x、d_x为x轴方向三次多项式模型系数，a_y、b_y、c_y、d_y为y轴方向三次多项式模型系数；

根据从第一过渡点至过渡轨迹顶点的运动时间T_h，可得

则可求解多项式系数为

步骤(4)：根据运动学约束求取过渡时间

为确保整体运动都满足运动学约束，需要求取得合适的运动时间T_h。对于速度约束C_v和加速度约束C_a，在过渡点和过渡轨迹顶点都需要建立运动学约束。而对于加加速度约束C_j，运动中的加加速度J_m为恒定常数。建立整体的运动学约束：

选取由上述约束求得的五个时间的最大值作为最短运动时间

步骤(5)：求取过渡速度与过渡加速度

根据已获得的由第一过渡点至过渡轨迹顶点的最短运动时间得到三次多项式的各个系数值，并求取第一过渡点的过渡速度 V_t＝(V_tx,V_ty)＝(c_x,Kc_x)与过渡加速度A_t＝(A_tx,A_ty)＝(2b_x,2Kb_x)。由于存在对称性，从过渡轨迹的顶点到第二过渡点的运动规划同理完成。用此过渡速度与过渡加速作为原始轨迹规划的边界值，从而实现原始轨迹与过渡轨迹的平滑衔接。

最终将整体的过渡轨迹再从平面坐标系还原到三维空间坐标系中，完成机械臂连续直线轨迹规划中过渡位置与轨迹误差可控的过渡轨迹自适应规划。

本发明相对于现有技术所具有的效果：本发明中的规划方法简单高效，具备过渡误差控制性并满足运动学约束，可运用于不同类型的机器人，具有广泛的可移植性。

具体实施方式

一种过渡位置与轨迹误差可控的机械臂连续直线轨迹自适应衔接规划方法。该方法包括以下步骤：

步骤(1)：确定过渡点与最大轨迹误差

确定过渡点在原始直线轨迹上的位置，两个过渡点P_W1与P_W2需要与两段直线轨迹的交点P_W3保持一样的距离L，以构建对称性。再确定原始轨迹与过渡轨迹之间所允许的最大轨迹误差值H。

步骤(2)：将两段直线轨迹所在的三维空间坐标系转换为平面坐标系

有且仅有一个交点的两条直线轨迹必然能构成一个平面。因此，以两个过渡点所在的直线为x轴，以垂直于x轴并经过第一过渡点的直线为y轴，将空间中两条相邻的直线轨迹由三维空间坐标系{W}转换到平面坐标系 {P}，使得过渡部分恰存在于平面坐标系的第一象限中。两个过渡点之间的距离为D＝||P_W1-P_W2||，则在平面坐标系中，过渡点的坐标分别为P₁＝(0,0)和 P₂＝(D,0),两段直线轨迹的交点过渡轨迹的顶点为第一条直线轨迹在平面空间中的斜率为K＝P_3y/P_3x；

步骤(3)：过渡运动模型构建

由于原始轨迹存在的对称性，将此特征用于构建过渡轨迹的运动模型。因而，确定在过渡轨迹的顶点P₄，速度方向平行于x轴，在y轴方向上的速度分量为0，即为V_p＝(V_px,0)，而加速度则平行于y轴，在 x轴上的分量为0，A_p＝(0,A_py)。对于第一过渡点P₁，速度为V_t＝(V_tx,V_ty)，加速度为A_t＝(A_tx,A_ty)，且满足

以此再结合三次多项式构建从第一过渡点至过渡轨迹顶点的运动模型。在x轴与y轴两个方向上，三次多项式表达式为

其中a_x、b_x、c_x、d_x为x轴方向三次多项式模型系数，a_y、b_y、c_y、d_y为y轴方向三次多项式模型系数；

根据从第一过渡点至过渡轨迹顶点的运动时间T_h，可得

则可求解多项式系数为

步骤(4)：根据运动学约束求取过渡时间

为确保整体运动都满足运动学约束，需要求取得合适的运动时间T_h。对于速度约束C_v和加速度约束C_a，在过渡点和过渡轨迹顶点都需要建立运动学约束。而对于加加速度约束C_j，运动中的加加速度J_m为恒定常数。建立整体的运动学约束：

选取由上述约束求得的五个时间的最大值作为最短运动时间

步骤(5)：求取过渡速度与过渡加速度

根据已获得的由第一过渡点至过渡轨迹顶点的最短运动时间得到三次多项式的各个系数值，并求取第一过渡点的过渡速度V_t＝(V_tx,V_ty)＝(c_x,Kc_x)与过渡加速度A_t＝(A_tx,A_ty)＝(2b_x,2Kb_x)。由于存在对称性，从过渡轨迹的顶点到第二过渡点的运动规划同理完成。用此过渡速度与过渡加速作为原始轨迹规划的边界值，从而实现原始轨迹与过渡轨迹的平滑衔接。

最终将整体的过渡轨迹再从平面坐标系还原到三维空间坐标系中，完成机械臂连续直线轨迹规划中过渡位置与轨迹误差可控的过渡轨迹自适应规划。

Claims (1)

1.一种过渡误差可控的机械臂直线轨迹自适应衔接规划方法，其特征在于，该方法的具体步骤是：

步骤(1)：确定过渡点与最大轨迹误差

确定过渡点在原始直线轨迹上的位置，两个过渡点P_W1与P_W2需要与两段直线轨迹的交点P_W3保持一样的距离L，以构建对称性；再确定原始轨迹与过渡轨迹之间所允许的最大轨迹误差值H；

步骤(2)：将两段直线轨迹所在的三维空间坐标系转换为平面坐标系

有且仅有一个交点的两条直线轨迹必然能构成一个平面；因此，以两个过渡点所在的直线为x轴，以垂直于x轴并经过第一过渡点的直线为y轴，将空间中两条相邻的直线轨迹由三维空间坐标系{W}转换到平面坐标系{P}，使得过渡部分恰存在于平面坐标系的第一象限中；两个过渡点之间的距离为D＝||P_W1-P_W2||，则在平面坐标系中，过渡点的坐标分别为P₁＝(0,0)和P₂＝(D,0),两段直线轨迹的交点过渡轨迹的顶点为第一条直线轨迹在平面空间中的斜率为K＝P_3y/P_3x；

步骤(3)：过渡运动模型构建

由于原始轨迹存在的对称性，将此特征用于构建过渡轨迹的运动模型；因而，确定在过渡轨迹的顶点P₄，速度方向平行于x轴，在y轴方向上的速度分量为0，即为V_p＝(V_px,0)，而加速度则平行于y轴，在x轴上的分量为0，A_p＝(0,A_py)；对于第一过渡点P₁，速度为V_t＝(V_tx,V_ty)，加速度为A_t＝(A_tx,A_ty)，且满足

以此再结合三次多项式构建从第一过渡点至过渡轨迹顶点的运动模型；在x轴与y轴两个方向上，三次多项式表达式为

其中a_x、b_x、c_x、d_x为x轴方向三次多项式模型系数，a_y、b_y、c_y、d_y为y轴方向三次多项式模型系数；

根据从第一过渡点至过渡轨迹顶点的运动时间T_h，可得

则可求解多项式系数为

步骤(4)：根据运动学约束求取过渡时间

为确保整体运动都满足运动学约束，需要求取得合适的运动时间T_h；对于速度约束C_v和加速度约束C_a，在过渡点和过渡轨迹顶点都需要建立运动学约束；而对于加加速度约束C_j，运动中的加加速度J_m为恒定常数；建立整体的运动学约束：

选取由上述约束求得的五个时间的最大值作为最短运动时间

步骤(5)：求取过渡速度与过渡加速度

根据已获得的由第一过渡点至过渡轨迹顶点的最短运动时间得到三次多项式的各个系数值，并求取第一过渡点的过渡速度V_t＝(V_tx,V_ty)＝(c_x,Kc_x)与过渡加速度A_t＝(A_tx,A_ty)＝(2b_x,2Kb_x)；由于存在对称性，从过渡轨迹的顶点到第二过渡点的运动规划同理完成；用此过渡速度与过渡加速作为原始轨迹规划的边界值，从而实现原始轨迹与过渡轨迹的平滑衔接；

最终将整体的过渡轨迹再从平面坐标系还原到三维空间坐标系中，完成机械臂连续直线轨迹规划中过渡位置与轨迹误差可控的过渡轨迹自适应规划。