[发明专利]一种直线轨迹间过渡运动的快速规划方法

权利要求书

1.一种直线轨迹间过渡运动的快速规划方法，其特征在于，该方法的具体步骤是：

步骤(1)：坐标系转换

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

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

由于原始轨迹存在的对称性，将此特征用于构建过渡轨迹的运动模型；因而，确定在过渡轨迹的顶点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，可得

则可求解多项式系数为

步骤(3)：过渡参数求取

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

加速度为A_t＝(A_tx,A_ty)需为已知数，为简化参数求解，设定A_tx＝A_ty＝0；选取由上述约束求得的五个时间的最大值作为最短运动时间总共的过渡时间为

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

步骤(4)：过渡轨迹规划

求得过渡系数和过渡时间后，在x、y和z三个分量上都采用五次多项式构建过渡模型；以x分量规划为例，位置、速度、加速度和加加速度上的运动模型为：

若始末位置分别为P_s(P_xs,P_ys,P_zs)和P_e(P_xe,P_ye,P_ze)，始末速度分别为V_s(V_xs,V_ys,V_zs)和V_e(V_xe,V_ye,V_ze)，始末加速度分别为A_s(A_xs,A_ys,A_zs)和A_e(A_xe,A_ye,A_ze)，过渡时间为T；计算五次多项式的各个系数，由此得到最终的过渡轨迹模型：

以此将整体的过渡轨迹再从平面坐标系还原到三维空间坐标系中，完成机械臂直线轨迹间过渡运动的快速规划。