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

权利要求书

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)；由于存在对称性，从过渡轨迹的顶点到第二过渡点的运动规划同理完成；用此过渡速度与过渡加速作为原始轨迹规划的边界值，从而实现原始轨迹与过渡轨迹的平滑衔接；

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