CN101508113B - 一种基于余弦二阶的机器人轨迹规划方法 - Google Patents

Abstract

一种基于余弦二阶的机器人轨迹规划方法，属于机器人轨迹规划领域。本发明的目的是解决现有机器人轨迹采用抛物线拟合的线性段轨迹的方法存在加速度的阶跃变化，使驱动力矩及驱动力矩一阶导数突然增大，从而导致轨迹不容易跟踪的问题。本发明首先给定基于余弦的二阶轨迹曲线的位置、速度、加速度以及加速度的导数的数学模型，给定两个期望点的位置和速度。然后，将边界条件的值代入所述数学模型中，列出方程组并求解出模型的参数。最后，根据两个期望点间的位置和速度的关系及加速度和加速度的导数的幅值来限制加速度的导数的阀值。确定最终的规划轨迹。本发明适用于机器人的轨迹规划，能够生成平滑的曲线，容易跟踪的轨迹。

Description

一种基于余弦二阶的机器人轨迹规划方法

技术领域

本发明涉及一种基于余弦二阶的机器人轨迹规划方法，属于机器人轨迹规划领域。

背景技术

通常情况下，机器人的期望轨迹是事先给定一系列笛卡尔或关节空间的点，且给定通过该点的速度或两点之间的时间，另外还会限制机器人运动允许的最大速度。轨迹规划的目的就是根据这些约束条件，建立通过这些点的平滑轨迹。为了缩短运动路径，通常规划这些点之间通过直线相连，并在连接点处通过曲线过渡。最为常用的是直线段加抛物线轨迹规划。

具有抛物线拟合的线性段轨迹包括匀加速、匀速和匀减速阶段，在考虑最大运行速度的情况下达到经历时间最短。这种方法能够很好地保证轨迹的位置和速度连续，如图2-1至图2-3所示，但是，这种方法在过渡点t1、t2处都将存在加速度的跳跃，以及无穷大的加速度导数值，这就意味着驱动力矩及驱动力矩一阶导数突然增大，由此生成的轨迹并不容易跟踪。

发明内容

本发明的目的是解决现有机器人轨迹采用抛物线拟合的线性段轨迹的方法存在加速度的阶跃变化，使驱动力矩及驱动力矩一阶导数突然增大，从而导致轨迹不容易跟踪的问题，提供了一种基于余弦二阶的机器人轨迹规划方法。

本发明实现基于余弦二阶的机器人轨迹规划方法的步骤包括：

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

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

$x\left(t\right)=b_3\cos \left(\frac{2\mathrm{\πt}}{T}\right)+b_2{t}^2+b_{1}t+b_0,$

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

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

$v\left(t\right)=-\frac{2\mathrm{\π}}{T}{b}_3\sin \left(\frac{2\mathrm{\πt}}{T}\right)+2b_{2}t+b_1;$

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

$a\left(t\right)=-\frac{4{\mathrm{\π}}^2}{T^2}{b}_3\cos \left(\frac{2\mathrm{\πt}}{T}\right)+2b_2;$

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

$\mathrm{Jerk}\left(t\right)=\frac{8{\mathrm{\π}}^3}{T^3}{b}_3\sin \left(\frac{2\mathrm{\πt}}{T}\right),$

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

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

$J_{\max }=\frac{\mathrm{\π}{a}_{\max }}{T},$

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

所述的最短时间原则为：在加速度倾斜上升段以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)满足的约束条件为：

$\left\{\begin{array}{c}x\left(0\right)=S_1\\ v\left(0\right)=v_1\\ a\left(0\right)=0\\ \mathrm{Jerk}\left(0\right)=0\end{array}\right.,$

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

$\left\{\begin{array}{c}x(T/2)=S_1+D1\\ v(T/2)=v_a\\ a(T/2)=a_{\max }\\ \mathrm{Jerk}(T/2)=0\end{array}\right.,$

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

$\left\{\begin{array}{c}{b}_0=S_1-\frac{a_{\max }{T}^2}{8{\mathrm{\π}}^2}\\ b_1=v_1\\ b_2=\frac{a_{\max }}{4}\\ b_3=\frac{a_{\max }{T}^2}{8{\mathrm{\π}}^2}\end{array}\right.,$ $\left\{\begin{array}{c}{v}_a=v_1+\frac{a_{\max }T}{4}\\ D1=(\frac{1}{16}-\frac{1}{4{\mathrm{\π}}^2})a_{\max }{T}^2+\frac{v_{1}T}{2}\end{array}\right.,$

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

$v_b=v_2-\frac{a_{\max }T}{4},$

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

$\left\{\begin{array}{c}D2=\frac{{v_b}^2-{v_a}^2}{2a_{\max }}\\ D3=(\frac{1}{16}-\frac{1}{4{\mathrm{\π}}^2})a_{\max }{T}^2+\frac{v_{1}T}{2}\end{array}\right.,$

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

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

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

其中，

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

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

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

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

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

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

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

$\left\{\begin{array}{c}{v}_2=v_1+\frac{a_{f}T}{2}\\ T=\frac{\mathrm{\π}{a}_f}{J_{\max }}\end{array}\right.,$

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

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

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

$\left\{\begin{array}{c}{S}_2-S_1=(v_1+v_2)\frac{T}{2}\\ v_2=v_1+\frac{a_{f}T}{2}\\ J_f=\frac{\mathrm{\π}{a}_f}{T}\end{array}\right.,$

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

$\left\{\begin{array}{c}T=\frac{2(S_2-S_1)}{v_1+v_2}\\ a_f=\frac{2(v_2-v_1)}{T}\\ J_f=\frac{2\mathrm{\π}(v_2-v_1)}{T^2}\end{array}\right.,$

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

本发明的优点是：本发明可计算出一条位置、速度、加速度可导，并且加速度的导数在某一个限定的范围内连续的轨迹。由此方法生成的轨迹，更容易被跟踪，可大大降低机器人的运动的位置跟踪误差。

附图说明

图1是本发明方法的流程图，图2-1是抛物线拟合的线性段轨迹-加速度曲线，图2-2是抛物线拟合的线性段轨迹-速度曲线，图2-3是抛物线拟合的线性段轨迹-位置曲线，图3是轨迹规划分段示意图(A表示持续脉冲，B表示脉冲，C表示平稳段，D表示倾斜段)，图4-1是基于余弦二阶的轨迹规划-位置曲线，图4-2是基于余弦二阶的轨迹规划-速度曲线，图4-3是基于余弦二阶的轨迹规划-加速度曲线，图4-4是基于余弦二阶的轨迹规划-加速度的导数曲线，图5-1是速度不能满足要求的基于余弦二阶的轨迹规划-位置曲线，图5-2是速度不能满足要求的基于余弦二阶的轨迹规划-速度曲线，5-3是速度不能满足要求的基于余弦二阶的轨迹规划-加速度曲线，图5-4是速度不能满足要求的基于余弦二阶的轨迹规划-加速度导数的曲线，图6-1是恒速下的基于余弦二阶的轨迹规划-位置曲线，图6-2是恒速下的基于余弦二阶的轨迹规划-速度曲线，图6-3是恒速下的基于余弦二阶的轨迹规划-加速度曲线，图6-4是恒速下的基于余弦二阶的轨迹规划-加速度的导数的曲线，图7-1是位移不能满足要求的基于余弦二阶的轨迹规划-位置曲线，图7-2是位移不能满足要求的基于余弦二阶的轨迹规划-速度曲线，图7-3是位移不能满足要求的基于余弦二阶的轨迹规划-加速度曲线，图7-4是位移不能满足要求的基于余弦二阶的轨迹规划-加速度的导数的曲线，图8-1是基于正弦一阶轨迹规划-位置曲线，图8-2是基于正弦一阶轨迹规划-速度曲线，图8-3是基于正弦一阶轨迹规划-加速度曲线，图8-4是基于正弦一阶轨迹规划-加速度的导数的曲线，图8-5是基于正弦一阶轨迹跟踪效果-误差曲线，图8-6是基于正弦一阶轨迹跟踪效果-力矩曲线，图9-1是基于余弦二阶轨迹规划-位置曲线，图9-2是基于余弦二阶轨迹规划-速度曲线，图9-3是基于余弦二阶轨迹规划-加速度曲线，图9-4是基于余弦二阶轨迹规划-加速度曲线，图9-5是基于余弦二阶轨迹跟踪效果-误差曲线，图9-6是基于余弦二阶轨迹跟踪效果-力矩曲线。

具体实施方式

具体实施方式一：下面结合图1、图3、图4-1～图4-4、图5-1～图5-4、图6-1～图6-4、图7-1～图7-4说明本实施方式，本实施方式实现基于余弦二阶的机器人轨迹规划方法包括以下步骤：

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

通常将两点间的轨迹根据控制参数(位置、速度和加速度)的变化分为两段：倾斜段D和平稳段C，参见图3所示。

控制参数发生变化的阶段为倾斜段D，例如速度上升段是指速度发生变化，属于倾斜段D。

控制参数不发生变化的阶段为平稳段C，例速度平稳是指恒速运行，属于平稳段C。

在图3中，控制参数经过上升倾斜段后紧接下降倾斜段称为脉冲B；经历上升倾斜段，平稳段后紧接下降倾斜段称为持续脉冲A。

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

$x\left(t\right)=b_3\cos \left(\frac{2\mathrm{\πt}}{T}\right)+b_2{t}^2+b_{1}t+b_0---\left(1\right)$

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

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

$v\left(t\right)=-\frac{2\mathrm{\π}}{T}{b}_3\sin \left(\frac{2\mathrm{\πt}}{T}\right)+2b_{2}t+b_1---\left(2\right)$

对公式(1)求导获得速度的数学模型；

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

$a\left(t\right)=-\frac{4{\mathrm{\π}}^2}{T^2}{b}_3\cos \left(\frac{2\mathrm{\πt}}{T}\right)+2b_2---\left(3\right)$

对公式(2)求导获得加速度的数学模型；

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

$\mathrm{Jerk}\left(t\right)=\frac{8{\mathrm{\π}}^3}{T^3}{b}_3\sin \left(\frac{2\mathrm{\πt}}{T}\right)---\left(4\right)$

对公式(3)求导获得加速度的导数的数学模型；

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

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

$J_{\max }=\frac{\mathrm{\π}{a}_{\max }}{T}---\left(5\right)$

最大加速度a_max由机器人机械臂的最大输出力矩性能决定的，为已知量，最大加速度的导数J_max也为已知量，由公式(3)和公式(4)可推导出公式(5)，由公式(5)可求得余弦周期T的数值。

步骤三、依据最短时间原则(在加速度倾斜上升段以T/2时间达到加速度最大值a_max)将机器人机械臂从起始期望点到末端期望点的轨迹分成四段，分别为加速度倾斜上升段、加速度平稳段、加速度倾斜下降段和速度平稳段，参见图4-1～图4-4，具体为：

加速度倾斜上升段的时间区间为[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。

给定起始期望点的位置和速度(S₁，v₁)、末端期望点的位置和速度(S₂，v₂)，且S₂＞S₁，v₂＞v₁，限定最大加速度和最大加速度的导数分别为a_max和J_max。期望在最短的时间内完成从S₁到S₂的过渡，因此理想的轨迹规划将以a_max和J_max让速度在最短的时间内达到末端期望点速度v₂，并以末端期望点速度v₂(最大的速度)匀速达到末端期望点位置S₂。

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

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

$\left\{\begin{array}{c}x\left(0\right)=S_1\\ v\left(0\right)=v_1\\ a\left(0\right)=0\\ \mathrm{Jerk}\left(0\right)=0\end{array}\right.---\left(6\right)$

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

$\left\{\begin{array}{c}x(T/2)=S_1+D1\\ v(T/2)=v_a\\ a(T/2)=a_{\max }\\ \mathrm{Jerk}(T/2)=0\end{array}\right.---\left(7\right)$

将方程组(6)和方程组(7)代入公式(1)至公式(4)中，获得有关四个系数b₀，b₁，b₂，b₃的值、加速度倾斜上升段末端速度v_a和加速度倾斜上升段运行距离D1：

$\left\{\begin{array}{c}{b}_0=S_1-\frac{a_{\max }{T}^2}{8{\mathrm{\π}}^2}\\ b_1=v_1\\ b_2=\frac{a_{\max }}{4}\\ b_3=\frac{a_{\max }{T}^2}{8{\mathrm{\π}}^2}\end{array}\right.,\left\{\begin{array}{c}{v}_a=v_1+\frac{a_{\max }T}{4}\\ D1=(\frac{1}{16}-\frac{1}{4{\mathrm{\π}}^2})a_{\max }{T}^2+\frac{v_{1}T}{2}\end{array}\right.---\left(8\right)$

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

$v_b=v_2-\frac{a_{\max }T}{4}---\left(9\right)$

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

$\left\{\begin{array}{c}D2=\frac{{v_b}^2-{v_a}^2}{2a_{\max }}\\ D3=(\frac{1}{16}-\frac{1}{4{\mathrm{\π}}^2})a_{\max }{T}^2+\frac{v_{1}T}{2}\end{array}\right.---\left(10\right)$

步骤五、判断是否满足条件：S₂-S₁≥D1+D2+D3，

判断结果为是，执行步骤六，判断结果为否，执行步骤九，

本步骤所述的基于余弦二阶的机器人机械臂运动轨迹按最大加速度a_max和最大加速度的导数J_max进行规划时需满足两个限定条件：

$\left\{\begin{array}{c}{S}_2-S_1\≥D1+D2+D3\\ v_2\≥v_{2\min }\end{array}\right.$

其中，D1+D2+D3为最小位移，v_2min为基于余弦二阶的轨迹只包括加速度倾斜上升段和加速度倾斜下降段时的最小的末端速度值，且v_2min满足以下关系式：

$v_{2\min }=v_1+\frac{a_{\max }T}{2}---\left(11\right)$

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

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

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

本步骤所述的是位移和速度两个限定条件都满足要求的情况，如图4-1～图4-4所示，

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

D4＝S₂-S₁-D1-D2-D3       (12)

依据公式(5)获得到余弦周期T，根据公式(9)获得加速度倾斜上升段末端速度v_a、加速度平稳段末端速度v_b以及四段的运行距离D1～D4，既而获得机器人机械臂期望的输出轨迹；

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

本步骤所述情况为：

$\left\{\begin{array}{c}{S}_2-S_1\&GreaterEqual;D1+D2+D3\\ v_2<v_{2\min }\end{array}\right.$

即速度不能满足要求的情况，表示末端速度如果在最大加速度a_max和最大加速度的导数J_max的限制条件下，能达到末端期望点位置S₂但将超出末端期望点的速度v₂。要想使轨迹规划的末端期望点能够满足要求，我们只能令加速度的幅值下降，设定加速度的阈值为a_f，但同时要保证时间最短，轨迹规划时应该令加速度的导数值仍旧为J_max，并且轨迹规划仅包括加速度倾斜上升段、加速度倾斜下降段和速度平稳段三段。其物理意义是让速度以最小的时间上升到v₂，并以v₂的速度匀速运动到末端期望点位置S₂

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

$\left\{\begin{array}{c}{v}_2=v_1+\frac{a_{f}T}{2}\\ T=\frac{\mathrm{\&pi;}{a}_f}{J_{\max }}\end{array}\right.---\left(13\right)$

获得加速度阈值a_f和余弦周期T，既而获得机器人机械臂期望的输出轨迹，如图5-1～图5-4如示，

当v₂＝v₁时，机器人机械臂期望的输出轨迹如图6-1～图6-4如示，为恒速状态，保持起始期望点速度v₁匀速行驶；

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

本步骤所述情况为位移不能满足要求：

S₂-S₁＜D1+D2+D3

表示无论末端期望点速度v₂无论如何给定，在最大加速度a_max和最大加速度的导数J_max的限制条件下，末端期望点的位置都将超过S₂。此时必须调整末端期望点速度v₂和加速度幅值，设定加速度的阈值为a_f，保证期望的轨迹不会超出末端期望点位置S₂。为了令位移最小并且保证加速度的导数的幅值恒等于J_max，规划的轨迹只包含加速度倾斜上升段和下降段。期望的轨迹满足如下关系：

$\left\{\begin{array}{c}{S}_2-S_1=\&lsqb;v_1+(v_1+\frac{a_{f}T}{2})\&rsqb;\frac{T}{2}\\ T=\frac{\mathrm{\&pi;}{a}_f}{J_{\max }}\end{array}---\left(14\right)\right.$

上式可整理为：

J_maxT³+4v₁πT-4(S₂-S₁)π＝0       (15)

且T只有一个正解。

在实际应用时，上述方程并不容易在数字控制器中计算；且上述方程将以牺牲末端期望点的速度为代价保证J_max，不适合于实际应用；另外通常是在较为精确的定时下(小于250ms)给定下一个期望点，当位移不能满足要求时主要发生在近距离逼近目标点状态，此时优先条件是机械臂的位移和速度均满足要求。为了令小位移步进下的加速度幅值最小，规划的轨迹仍只包括加速度倾斜上升段和加速度倾斜下降段，且满足以下方程组：

$\left\{\begin{array}{c}{S}_2-S_1=(v_1+v_2)\frac{T}{2}\\ v_2=v_1+\frac{a_{f}T}{2}\\ J_f=\frac{\mathrm{\&pi;}{a}_f}{T}\end{array}\right.---\left(16\right)$

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

$\left\{\begin{array}{c}T=\frac{2(S_2-S_1)}{v_1+v_2}\\ a_f=\frac{2(v_2-v_1)}{T}\\ J_f=\frac{2\mathrm{\&pi;}(v_2-v_1)}{T^2}\end{array}\right.$

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

当J_f＞J_max时，需警告上位机，给定的末端期望点速度v₂的值过大，加速度上升过快，即输出的扭矩瞬时变化过快，易引起机械响应速度不能跟随输入达到预期的扭矩值。

具体实施方式二：下面结合图8-1～图8-6、图9-1～图9-6说明本实施方式，本实施方式给出一个实施例。

本实施例以四自由度机器人为例。该机器人由四个模块化旋转关节构成。关节采用无刷直流电机驱动，谐波减速器(减速比1∶160)作为力矩传动。每个关节都安装有一个电位计和磁编码器用于测量关节和电机的角度。轨迹跟踪实验主要用于验证加速度的导数连续对系统性能的影响。本实施例参照基于正弦一阶数学模型的机器人机械臂的轨迹规划方法，突出本发明基于余弦二阶的机器人机械臂轨迹规划方法的优势。

基于正弦一阶机器人机械臂轨迹曲线的位置数学模型y(t)为：

$y\left(t\right)=a_2\sin \left(\frac{2\mathrm{\&pi;t}}{T_S}\right)+a_{1}t+a_0---\left(17\right)$

其中，T_S为所述正弦的周期，a₀，a₁，a₂为系数，则，基于正弦一阶机器人机械臂的速度数学模型v_S(t)为：

$v_S\left(t\right)=\frac{2\mathrm{\&pi;}}{T_S}{a}_2\cos \left(\frac{2\mathrm{\&pi;t}}{T_S}\right)+a_1---\left(18\right)$

基于正弦一阶机器人机械臂的加速度数学模型a_S(t)为：

$a_S\left(t\right)=-\frac{4{\mathrm{\&pi;}}^2}{{T_S}^2}{a}_2\sin \left(\frac{2\mathrm{\&pi;t}}{T_S}\right)---\left(19\right)$

基于正弦一阶机器人机械臂的加速度的导数数学模型J_S(t)为：

$J_S\left(t\right)=\frac{8{\mathrm{\&pi;}}^3}{{T_S}^3}{a}_2\cos \left(\frac{2\mathrm{\&pi;t}}{T_S}\right)---\left(20\right)$

正弦周期T为加速段所需的时间。由公式(18)～公式(20)可以看出，正弦一阶函数具有连续的位置、速度和加速度，但是加速度的导数在加速度静止到零时(t＝T)不连续且不能限制其幅值。

加速度和加速度的导数将直接影响机械臂的力矩和力矩导数。而基于位置的阻抗控制由位置外环和力矩内环组成，当输出力矩发生变化时将会对阻抗控制的跟踪效果造成明显的影响。实验时令四自由度机器人的第二关节在30度至60度间做往复运动，其最大速度在两种跟踪模式下均设置为15度/秒。在本发明所述基于余弦二阶轨迹方法中限定最大加速度a_max和最大加速度的导数J_max分别为6.5度/秒²和15度/秒³。

图8-5～图8-6是采用正弦一阶轨迹的跟踪效果图，图8-1～图8-4分别代表期望的位置、速度、加速度和加速度的导数的轨迹曲线。由图8-4可以看出加速度的导数值不连续且不能够限定其幅值。图8-5是二关节的跟踪误差曲线，其误差为±0.7度。图8-6是二关节所受外力矩(去除重力矩)的测量值，可以看出关节力矩容易出现波动，其波动范围为±8Nm。

图9-5～图9-6是基于余弦二阶轨迹的跟踪效果，图9-1～图9-4的曲线意义与图8-1～图8-4相仿。由图9-3～图9-4可以分析出二关节的加速度和加速度的导数值连续且都在限定范围内。由于输出力矩平稳，在阻抗控制下二关节的位置跟踪误差减小至±0.3度；所测量的力矩波动减小为±3Nm。

由本实施例可以看出基于余弦二阶的轨迹保证了电机侧输入力矩的平稳，从而在具有位置和力控制的阻抗控制中得到关节输出力矩的稳定。

Claims (1)

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

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

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

$x\left(t\right)=b_3\cos \left(\frac{2\mathrm{\&pi;t}}{T}\right)+b_2{t}^2+b_{1}t+b_0,$

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

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

$v\left(t\right)=-\frac{2\mathrm{\&pi;}}{T}{b}_3\sin \left(\frac{2\mathrm{\&pi;t}}{T}\right)+2b_{2}t+b_1;$

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

$a\left(t\right)=-\frac{4{\mathrm{\&pi;}}^2}{T^2}{b}_3\cos \left(\frac{2\mathrm{\&pi;t}}{T}\right)+2b_2;$

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

$\mathrm{Jerk}\left(t\right)=\frac{8{\mathrm{\&pi;}}^3}{T^3}{b}_3\sin \left(\frac{2\mathrm{\&pi;t}}{T}\right),$

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

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

$J_{\max }=\frac{\mathrm{\&pi;}{a}_{\max }}{T},$

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

所述的最短时间原则为：在加速度倾斜上升段以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)满足的约束条件为：

$\left\{\begin{array}{c}x\left(0\right)=S_1\\ v\left(0\right)=v_1\\ a\left(0\right)=0\\ \mathrm{Jerk}\left(0\right)=0\end{array}\right.,$

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

$\left\{\begin{array}{c}x(T/2)=S_1+D1\\ v(T/2)=v_a\\ a(T/2)=a_{\max }\\ \mathrm{Jerk}(T/2)=0\end{array}\right.,$

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

$\left\{\begin{array}{c}{b}_0=S_1-\frac{a_{\max }{T}^2}{8{\mathrm{\&pi;}}^2}\\ b_1=v_1\\ b_2=\frac{a_{\max }}{4}\\ b_3=\frac{a_{\max }{T}^2}{8{\mathrm{\&pi;}}^2}\end{array}\right.,$ $\left\{\begin{array}{c}{v}_a=v_1+\frac{a_{\max }T}{4}\\ D1=(\frac{1}{16}-\frac{1}{4{\mathrm{\&pi;}}^2})a_{\max }{T}^2+\frac{v_{1}T}{2}\end{array}\right.,$

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

$v_b=v_2-\frac{a_{\max }T}{4},$

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

$\left\{\begin{array}{c}D2=\frac{{v_b}^2-{v_a}^2}{2a_{\max }}\\ D3=(\frac{1}{16}-\frac{1}{4{\mathrm{\&pi;}}^2})a_{\max }{T}^2+\frac{v_{1}T}{2}\end{array}\right.,$

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

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

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

其中，

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

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

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

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

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

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

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

$\left\{\begin{array}{c}{v}_2=v_1+\frac{a_{f}T}{2}\\ T=\frac{\mathrm{\&pi;}{a}_f}{J_{\max }}\end{array}\right.,$

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

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

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

$\left\{\begin{array}{c}{S}_2-S_1=(v_1+v_2)\frac{T}{2}\\ v_2=v_1+\frac{a_{f}T}{2}\\ J_f=\frac{\mathrm{\&pi;}{a}_f}{T}\end{array}\right.,$

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

$\left\{\begin{array}{c}T=\frac{2(S_2-S_1)}{v_1+v_2}\\ a_f=\frac{2(v_2-v_1)}{T}\\ J_f=\frac{2\mathrm{\&pi;}(v_2-v_1)}{T^2}\end{array}\right.,$

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

Images