[发明专利]一种基于五段S型速度曲线的机械臂直线轨迹规划方法

权利要求书

1.一种基于五段S型速度曲线的机械臂直线轨迹规划方法，其特征在于，包括以下步骤：

1)对速度建模：将速度分为五段过程，分别为：加速度增加的加速运动、加速度减小的加速运动、匀速运动、加速度增加的减速运动和加速度减小的减速运动，再将速度分解到机械臂基座标系的x、y、z方向上，上述五段运动的时间均相同，首先在x、y、z方向上建立加加速度关于时间的模型，如式(1-1)、(1-2)、(1-3)所示：

其中，j_x(t)、j_y(t)、j_z(t)分别表示x方向上加加速度关于时间的函数、y方向上加加速度关于时间的函数、z方向上加加速度关于时间的函数；t、t₁分别表示时间变量和加速度增加的加速运动所用的时间；J_x、J_y、J_z分别表示x方向t时刻对应的加加速度、y方向t时刻对应的加加速度、z方向t时刻对应的加加速度，其次分别对x、y、z方向上建立的加加速度模型函数进行积分，从而得到x、y、z方向上加速度关于时间的模型函数，如式(1-4)、(1-5)、(1-6)所示：

其中，a_x(t)、a_y(t)、a_z(t)分别表示x方向上加速度关于时间的函数、y方向上加速度关于时间的函数、z方向上加速度关于时间的函数，再分别对x、y、z方向上的加速度模型函数进行积分，最终得到x、y、z方向上速度关于时间的函数，如式(1-7)、(1-8)、(1-9)所示：

其中，v_x(t)、v_y(t)、v_z(t)分别表示x方向上速度关于时间的函数、y方向上速度关于时间的函数、z方向上速度关于时间的函数；

2)求解位置函数：对步骤1)中建立的速度模型进行积分，得到x、y、z方向上五段位置关于时间的函数，分别为：加速度增加的加速阶段位置关于时间的函数、加速度减小的加速阶段位置关于时间的函数、匀速阶段位置关于时间的函数、加速度增加的减速阶段位置关于时间的函数、加速度减小的减速阶段位置关于时间的函数，如式(1-10)、(1-11)、(1-12)所示：

其中，x(t)、y(t)、z(t)分别表示x方向上位置关于时间的函数、y方向上位置关于时间的函数、z方向上位置关于时间的函数；x₀、y₀、z₀分别表示x方向的初始位置、y方向的初始位置、z方向的初始位置；

3)添加约束：对步骤2)中求解得到的x、y、z方向上位置关于时间的函数添加约束，约束条件为当速度衰减到0的时候要到达目标位置，x、y、z方向上位置约束，如式(1-13)：

将式(1-13)分别代入(1-10)、(1-11)、(1-12)中求得x、y、z方向上的加加速度J_x、J_y、J_z，如式(1-14)所示：

其中x_tar、y_tar、z_tar分别表示目标点的x坐标、目标点的y坐标、目标点的z坐标；

4)设计控制器：基于PD控制设计机械臂控制器，计算力矩发送给机械臂控制柜，控制机械臂每个关节使其到达指定位置，基于PD控制设计的机械臂控制器如式(1-15)所示：

其中，τ表示机械臂的力矩；J表示雅克比矩阵；K_p表示PD控制的比例增益，在机械臂控制中表示机械臂的刚度矩阵；K_d表示PD控制的微分控制，在机械臂控制中表示机械臂的阻尼矩阵；q_d表示期望位置；q表示实际位置；表示期望速度；表示实际速度。