[发明专利]一种基于余弦二阶的机器人轨迹规划方法

权利要求书

1.一种基于余弦二阶的机器人轨迹规划方法，其特征在于实现该方法的步骤包括：

步骤一、建立基于余弦二阶的机器人机械臂轨迹曲线的位置的数学模型、速度的数学模型、加速度的数学模型和加速度的导数的数学模型；

所述基于余弦二阶的机器人机械臂轨迹曲线的位置x(t)的数学模型为：

x(t)=b3cos(2πtT)+b2t2+b1t+b0,]]>

其中：b₀，b₁，b₂，b₃为系数，T为所述余弦的周期；

所述基于余弦二阶的机器人机械臂轨迹曲线的速度v(t)的数学模型为：

v(t)=-2πTb3sin(2πtT)+2b2t+b1;]]>

所述基于余弦二阶的机器人机械臂轨迹曲线的加速度a(t)的数学模型为：

a(t)=-4π2T2b3cos(2πtT)+2b2;]]>

所述基于余弦二阶的机器人机械臂轨迹曲线的加速度的导数Jerk(t)的数学模型为：

Jerk(t)=8π3T3b3sin(2πtT),]]>

步骤二、给定机器人机械臂运动的起始期望点位置S₁、起始期望点速度v₁、末端期望点位置S₂和末端期望点速度v₂，按最大加速度a_max和最大加速度的导数J_max进行基于余弦二阶的机器人机械臂轨迹曲线的规划；

所述的最大加速度a_max和最大加速度的导数J_max之间存在如下关系：

Jmax=πamaxT,]]>

步骤三、依据最短时间原则将机器人机械臂从起始期望点到末端期望点的轨迹分成四段，分别为加速度倾斜上升段、加速度平稳段、加速度倾斜下降段和速度平稳段；

所述的最短时间原则为：在加速度倾斜上升段以T/2时间达到加速度最大值a_max，

所述加速度倾斜上升段、加速度平稳段、加速度倾斜下降段和速度平稳段分别为：

加速度倾斜上升段的时间区间为[0，t1]，速度由v₁增加到v_a，运行距离为D1，加速度达到最大值a_max，

加速度平稳段的时间区间为[t1，t2]，速度由v_a增加到v_b，运行距离为D2，加速度保持最大值a_max，加速度的导数为0，

加速度倾斜下降段的时间区间为[t2，t3]，速度由v_b增加到v₂，运行距离为D3，加速度为由a_max下降到0，

速度平稳段的时间区间为[t3，t4]，速度保持v₂，运行距离为D4，加速度为0，加速度的导数为0，

步骤四、将加速度倾斜上升段的边界条件的值代入步骤一所述数学模型中，列出方程组并求解出所述数学模型的参数；

求解数学模型参数的方法为：

加速度倾斜上升段满足在T/2时间达到加速度最大值a_max，所述基于余弦二阶的机器人机械臂轨迹曲线在t＝0时位置x(0)、速度v(0)、加速度a(0)和加速度的导数Jerk(0)满足的约束条件为：

x(0)=S1v(0)=v1a(0)=0Jerk(0)=0,]]>

所述基于余弦二阶的机器人机械臂轨迹曲线在t＝T/2时位置x(T/2)、速度v(T/2)、加速度a(T/2)和加速度的导数Jerk(T/2)满足的约束条件为：

x(T/2)=S1+D1v(T/2)=vaa(T/2)=amaxJerk(T/2)=0,]]>

将上述两组边界约束条件方程组代入步骤一建立的四个数学模型中，获得有关四个系数b₀，b₁，b₂，b₃的值、加速度倾斜上升段末端速度v_a和加速度倾斜上升段运行距离D1：

b0=S1-amaxT28π2b1=v1b2=amax4b3=amaxT28π2,]]>va=v1+amaxT4D1=(116-14π2)amaxT2+v1T2,]]>

则加速度平稳段末端速度v_b为：

vb=v2-amaxT4,]]>

加速度平稳段的运行距离D2和加速度倾斜下降段的运行距离D3为：

D2=vb2-va22amaxD3=(116-14π2)amaxT2+v1T2,]]>

步骤五、判断是否满足条件：S₂-S₁≥D1+D2+D3，如果判断结果为是，执行步骤六，如果判断结果为否，执行步骤九，

步骤六、判断是否满足条件：v₂≥v_2min，

如果判断结果为是，执行步骤七，如果判断结果为否，执行步骤八，

其中，v_2min为基于余弦二阶的轨迹只包括加速度倾斜上升段和加速度倾斜下降段时的最小的末端速度值；

步骤七、基于余弦二阶的轨迹按最大加速度a_max和最大加速度的导数J_max进行规划，获得机器人机械臂期望的输出轨迹，基于余弦二阶的机器人轨迹规划方法完成，

本步骤所述的基于余弦二阶的轨迹规划包括加速度倾斜上升段、加速度平稳段、加速度倾斜下降段和速度平稳段四段，则：

D4＝S₂-S₁-D1-D2-D3

既而获得机器人机械臂期望的输出轨迹，

步骤八、基于余弦二阶的轨迹按加速度阈值a_f和最大加速度的导数J_max进行规划，获得机器人机械臂期望的输出轨迹，基于余弦二阶的机器人轨迹规划方法完成，

本步骤所述的基于余弦二阶的轨迹规划包括加速度倾斜上升段、加速度倾斜下降段和速度平稳段三段，且满足以下方程组：

v2=v1+afT2T=πafJmax,]]>

获得加速度阈值a_f和余弦周期T，既而获得机器人机械臂期望的输出轨迹，

步骤九、基于余弦二阶的轨迹按加速度阈值a_f和加速度的导数阈值J_f进行规划，获得机器人机械臂期望的输出轨迹，基于余弦二阶的机器人轨迹规划方法完成，

本步骤所述的基于余弦二阶的轨迹规划包括加速度倾斜上升段和加速度倾斜下降段两段，且满足以下方程组：

S2-S1=(v1+v2)T2v2=v1+afT2Jf=πafT,]]>

获得余弦周期T、加速度阈值a_f和加速度的导数阈值J_f：

T=2(S2-S1)v1+v2af=2(v2-v1)TJf=2π(v2-v1)T2,]]>

既而获得机器人机械臂期望的输出轨迹。