一种基于加速度动态配置的空间机器人抖动抑制轨迹规划方法

摘要

一种基于加速度动态配置的空间机器人抖动抑制轨迹规划方法，属于机器人抖动抑制技术领域。为解决现有的机器人系统容易产生残余抖动，跟踪精度不高，导致机器人无法精确定位；机器人运行不平稳；时延造成的机器人执行任务时间的延长；计算量大，对机器人控制器运算能力要求高的问题，本发明提出了：根据输入的期望位移及最大加速度限幅预估轨迹段的最小执行周期；根据计算的最小执行周期及给定的轨迹周期确定最终的轨迹周期；根据机器人的运动学及动力学参数和机器人关节刚度参数计算在初始关节角度确定的构型下的机器人系统最低阶的抖动周期；根据轨迹周期，加速时间及抖动周期选择相应的轨迹规划方式。本发明适用于机器人的抖动抑制领域。

说明书

技术领域

本发明涉及一种基于加速度动态配置的空间机器人抖动抑制轨迹规划方法，属于机器 人抖动抑制技术领域。

背景技术

随着各国航天技术的发展，空间机器人在未来空间操作中的作用日益重要。空间机器 人除了对硬件系统的可靠性提出严苛的要求外，对其规划与控制方法也提出了很高的要 求。一般空间机器人的控制与规划相对于传统的地面机器人的难点主要在于：1)空间机 器人存在很强的结构柔性，并且空间机器人载体(或基座)的自由漂浮特性使得空间机器 人在运动中不仅会产生柔性抖动，而且对其载体的扰动会引起载体位姿的明显变化，从而 严重影响空间机器人的位姿精度。2)空间机器人的柔性抖动频率不仅受结构刚度的影响， 而且随着机器人关节角度的变化而变化。因此，空间机器人的轨迹规划需要同时考虑结构 柔性及机器人构型变化所引起的影响。但现有技术中存在机器人系统中加速度在整个插补 周期内不能动态配置，容易产生残余抖动，跟踪精度不高，导致机器人无法精确定位；机 器人构型变化时，系统一阶抖动频率变化所引起的变频率轨迹抖动导致的机器人运行不平 稳；不能有效克服传统开环输入整形控制所产生的时延，从而造成机器人执行任务时间的 延长；计算量大，对机器人控制器运算能力要求高的问题。

发明内容

本发明的目的是提出一种基于加速度动态配置的空间机器人抖动抑制轨迹规划方法， 以解决针对现有的机器人系统容易产生残余抖动，跟踪精度不高，导致机器人无法精确定 位；机器人运行不平稳；时延造成的机器人执行任务时间的延长；计算量大，对机器人控 制器运算能力要求高的问题。

本发明为解决上述技术问题所采用的技术方案是：

本发明所述的一种基于加速度动态配置的空间机器人抖动抑制轨迹规划方法，是按照 以下步骤实现的：

步骤一、根据输入的期望位移S_p及最大加速度限幅a_m预估轨迹段的最小执行周期 t_pmin：假设在整个轨迹规划周期内，整个轨迹段以最大加速度幅值的要求进行规划，则期 望位移与最大加速度限幅之间存在如下关系：

$\frac{1}{2}{S}_p=\frac{1}{2}{a}_m{t}_{p\min }^2---\left(1\right)$

因此可得，

$t_{p\min }=\sqrt{\frac{S_p}{a_m}}---\left(2\right);$

步骤二、根据计算的最小执行周期t_pmin及给定的轨迹周期确定最终的轨迹周期t_p：

若$t_{p_0}<t_{p\min },$则$t_p=t_{p\min }=\sqrt{\frac{S_p}{a_m}},$否则$t_p=t_{p_0};$

步骤三、根据机器人的运动学及动力学参数和机器人关节刚度参数计算在初始关节角 度q₀确定的构型下的机器人系统最低阶的抖动周期τ_d：

首先计算机器人在该构型下的惯量矩阵M(q₀)，根据机器人关节刚度矩阵K计算机 器人系统的各阶模态频率为：

$f=\frac{1}{2\pi }\sqrt{\mathrm{eig}\left[M{\left(q_0\right)}^{-1}K\right]}---\left(3\right)$

其中，频率的单位为Hz，机器人系统最低阶模态频率对应的抖动周期为：

${\tau }_d=\frac{1}{\min \left(f\right)}---\left(4\right);$

步骤四、根据轨迹周期t_p，加速时间t_acc及抖动周期τ_d选择相应的轨迹规划方式。

本发明由于采取以上技术方案，其具有以下优点：

1、基于该方法可以实现机器人关节运动轨迹位置、速度的连续性，加速度在整个插 补周期内的动态配置，解决了空间机器人控制中由于机器人关节角度变化而引起的不同构 型下机器人结构频率变化所导致的机器人运动的残余抖动问题。

2、该方法解决了由于机器人运动时构型变化导致的机器人系统一阶抖动频率变化所 引起的变频率轨迹抖动问题；机器人平稳运行。

3、该方法从加速度层面进行整形，将传统输入整形的思想集成于机器人轨迹规划算 法中，既保证了机器人的运行平稳性，又有效克服传统开环输入整形控制所产生的时延影 响。

4、基于该方法设计的轨迹规划方法具有很小的计算量，方便空间机器人嵌入式系统 的应用。

5、基于该方法设计的空间机器人运动轨迹可以在一定程度上降低空间机器人运行的 柔性抖动，提高空间机器人末端的跟踪精度。

6、本发明保证了机器人关节加速度在机器人运行过程中随构型变化而自适应调整， 并且调整后的机器人加速度与相应构型下的抖动频率相匹配，消除了由于机器人抖动频率 变化所导致的机器人末端残余抖动问题，大大改善了空间机器人的轨迹跟踪性能。

7、本发明的规划方法可以将90％以上的关节残余抖动消除。

8、本发明依据不同构型下的机器人抖动频率实时规划具有抖动抑制功能的机器人轨 迹，实现空间机器人的平稳运行，保证机器人末端位姿精度在合理范围内。

附图说明

图1是加速度动态配置轨迹规划算法流程图。图中可以看出，该规划方法的输入参数 主要有期望位移、规划周期、加速时间、最大加速度限幅、机器人的初始角度等。根据初 始的输入参数预测规划周期的近似范围，然后根据机器人的初始角度所对应的机器人构型 及机器人的运动学、动力学参数确定其残余抖动一阶抖动模态对应的抖动周期。根据规划 周期与抖动周期的对应关系，提出了四种具体的规划方式。因此，该规划方法可以根据规 划任务周期的大小自主选择规划方法，保证了机器人的运行轨迹既具有抖动抑制功能，又 提高了机器人的运行效率，实现了时间的最优配置。

图2是规划方式1的加速度及速度轨迹。根据加速度局部最优的思想配置每个常加速 度所对应的时间。该规划方式可实现周期需求短、需要快速执行的规划任务。

图3是规划方式2的加速度及速度轨迹。对于执行周期较短的规划任务而言，机器人 轨迹的总时间及加速时间均比规划方式1所示的时间稍长。根据加速时间的规划要求更加 细致的配置各段加速度周期，以实现速度轨迹的较平稳规划。

图4是规划方式3的加速度及速度轨迹。利用该规划方式实现较长时间周期的轨迹规 划。在该规划方式下，机器人加速度经过更加细致的配置，速度曲线可实现更加平稳的过 渡。

图5是规划方式4的加速度及速度轨迹。该规划方式可实现需要很长时间周期进行规 划的任务。加速度幅值及时间的配置可保证机器人即使在大范围内运行时也不激发柔性模 态，在机器人构型变化很大的情况下仍然可有效抑制机器人末端残余抖动。

图6为传统规划下的期望位置、期望速度、期望加速度及关节实际位置响应的曲线。

图7为本发明规划下的期望位置、期望速度、期望加速度及关节实际位置响应的曲线。

图8为不同规划方式下的关节残余抖动的比较曲线。

具体实施方式

具体实施方式一：本实施方式所述的一种基于加速度动态配置的空间机器人抖动抑制 轨迹规划方法，其特征在于所述方法是按照以下步骤实现的：

步骤一、根据输入的期望位移S_p及最大加速度限幅a_m预估轨迹段的最小执行周期 t_pmin：假设在整个轨迹规划周期内，整个轨迹段以最大加速度幅值的要求进行规划，则期 望位移与最大加速度限幅之间存在如下关系：

$\frac{1}{2}{S}_p=\frac{1}{2}{a}_m{t}_{p\min }^2---\left(5\right)$

因此可得，

$t_{p\min }=\sqrt{\frac{S_p}{a_m}}---\left(6\right);$

步骤二、根据计算的最小执行周期t_pmin及给定的轨迹周期确定最终的轨迹周期t_p：

若$t_{p_0}=t_{p\min }$，则$t_p=t_{p\min }=\sqrt{\frac{S_p}{a_m}},$否则$t_p=t_{p_0}$；

步骤三、根据机器人的运动学及动力学参数和机器人关节刚度参数计算在初始关节角 度q₀确定的构型下的机器人系统最低阶的抖动周期τ_d：

首先计算机器人在该构型下的惯量矩阵M(q₀)，根据机器人关节刚度矩阵K计算机 器人系统的各阶模态频率为：

$f=\frac{1}{2\pi }\sqrt{\mathrm{eig}\left[M{\left(q_0\right)}^{-1}K\right]}---\left(7\right)$

其中，频率的单位为Hz，机器人系统最低阶模态频率对应的抖动周期为：

${\tau }_d=\frac{1}{\min \left(f\right)}---\left(8\right);$

步骤四、根据轨迹周期t_p，加速时间t_acc及抖动周期τ_d选择相应的轨迹规划方式。

结合图1解释具体实施方式一。

具体实施方式二：本实施方式与具体实施方式一不同的是：步骤四的所述的规划方式 分为：

对于轨迹周期t_p，其大小将满足四个分布区间之一，即1)t_p＞8τ_d，2)8τ_d≥t_p＞6τ_d， 3)6τ_d≥t_p＞4τ_d，4)4τ_d≥t_p＞2τ_d；若t_p≤2τ_d，则将轨迹周期t_p的值修正为t_p＝2.2τ_d，使 之满足上述四个区间之一。

结合图1解释具体实施方式二，其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：还包括：

步骤四(一)、若轨迹周期t_p属于第1)个区间，即t_p＞8τ_d，则判断加速时间t_acc是否 满足t_acc＞4τ_d这一条件，若满足，则不修正加速时间t_acc的大小；

若不满足，则将加速时间t_acc修正为t_acc＝0.5t_p；然后判断轨迹周期t_p与加速时间t_acc的关系；若t_p＜2t_acc，则将轨迹周期t_p修正为t_p＝2t_acc；此时将选择规划方式4计算期望 轨迹的位置、速度、加速度的具体数值；

步骤四(二)、若轨迹周期t_p属于第2)个区间，即8τ_d≥t_p＞6τ_d，则判断加速时间t_acc是否满足t_acc＞3τ_d这一条件，若满足，则不修正加速时间t_acc的大小；

若不满足，则将加速时间t_acc修正为t_acc＝0.5t_p；然后判断轨迹周期t_p与加速时间t_acc的关系；若t_p＜2t_acc，则将轨迹周期t_p修正为t_p＝2t_acc；此时将选择规划方式3计算期望 轨迹的位置、速度、加速度的具体数值；

步骤四(三)、若轨迹周期t_p属于第3)个区间，即6τ_d≥t_p＞4τ_d，则判断加速时间t_acc是否满足t_acc＞2τ_d这一条件，若满足，则不修正加速时间t_acc的大小；

若不满足，则将加速时间t_acc修正为t_acc＝0.5t_p；然后判断轨迹周期t_p与加速时间t_acc的关系；若t_p＜2t_acc，则将轨迹周期t_p修正为t_p＝2t_acc；此时将选择规划方式2计算期望 轨迹的位置、速度、加速度的具体数值；

步骤四(四)、若轨迹周期t_p属于第4)个区间，即4τ_d≥t_p＞2τ_d，则判断加速时间t_acc是否满足t_acc＞τ_d这一条件，若满足，则不修正加速时间t_acc的大小；

若不满足，则将加速时间t_acc修正为t_acc＝0.5t_p；然后判断轨迹周期t_p与加速时间t_acc的关系；若t_p＜2t_acc，则将轨迹周期t_p修正为t_p＝2t_acc；此时将选择规划方式1计算期望 轨迹的位置、速度、加速度的具体数值。

结合图1解释具体实施方式三，其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：步骤四(一)所 述的规划方式4的规划方法为：

加速度按照五脉冲整形的方式进行配置，其中k₁、k₂、k₃、k₄为整形系数，由机器人 系统的阻尼比ζ决定；其计算方法为：

k₁＝0.11275+0.76632ζ+3.2916ζ²-1.4438ζ²(9)

k₂＝0.23698+0.61164ζ-2.5785ζ²+4.8522ζ³(10)

k₃＝0.30008-0.19062ζ-2.1456ζ²+0.13744ζ³>

k₄＝0.23775-0.73297ζ+0.46885ζ²-2.0865ζ³>

规划方式4将时间区间分为19个区间，即[τ_d，1.5τ_d]，[1.5τ_d，2τ_d]， [2τ_d，t_acc-2τ_d]，[t_acc-2τ_d，t_acc-1.5τ_d]，[t_acc-1.5τ_d，t_acc-τ_d]，[t_acc，t_p-t_acc]，[t_p-t_acc+τ_d，t_p-t_acc+1.5τ_d]，[t_p-t_acc+1.5τ_d，t_p-t_acc+2τ_d]，[t_p-t_acc+2τ_d，t_p-2τ_d]， [t_p-2τ_d，t_p-1.5τ_d]，[t_p-1.5τ_d，t_d-τ_d，]，和

在时间内产生的位移s₁为：

$s_1=\frac{1}{8}{k}_{1}a{\tau }_d^2---\left(13\right)$

在时间内产生的位移s₂为：

$s_2=\frac{1}{8}(3k_1+k_2)a{\tau }_d^2---\left(14\right)$

在[τ_d，1.5τ_d]时间内产生的位移s₃为：

$s_3=\frac{1}{8}(5k_1+3k_2+k_3)a{\tau }_d^2---\left(15\right)$

在[1.5τ_d，2τ_d]时间内产生的位移s₄为：

$s_4=\frac{1}{8}(7k_1+5k_2+3k_3+k_4)a{\tau }_d^2---\left(16\right)$

在[2τ_d，t_acc-2τ_d]时间内产生的位移s₅为：

$s_5=\frac{1}{2}({4k}_1+{3k}_2+{2k}_3+k_4)(t_{\mathrm{acc}}-{4\tau }_d){\mathrm{a\tau }}_d+\frac{1}{2}a{(t_{\mathrm{acc}}-{4\tau }_d)}^2---\left(17\right)$

在[t_acc-2τ_d，t_acc-1.5τ_d]时间内产生的位移s₆为：

$s_6=\frac{1}{8}({9k}_1+{7k}_2+{5k}_3+{3k}_4){\mathrm{a\tau }}_d^2+\frac{1}{2}a(t_{\mathrm{acc}}-{4\tau }_d){\tau }_d---\left(18\right)$

在[t_acc-1.5τ_d，t_acc-τ_d]时间内产生的位移s₇为：

$s_7=\frac{1}{8}({11k}_1+{9k}_2+{7k}_3+{4k}_4){\mathrm{a\tau }}_d^2+\frac{1}{2}a(t_{\mathrm{acc}}-{4\tau }_d){\tau }_d---\left(19\right)$

在[t_acc-τ_d，t_acc-0.5τ_d]时间内产生的位移s₈为：

$s_8=\frac{1}{8}({13k}_1+{11k}_2+{8k}_3+{4k}_4){\mathrm{a\tau }}_d^2+\frac{1}{2}a(t_{\mathrm{acc}}-{4\tau }_d){\tau }_d---\left(20\right)$

在[t_acc-0.5τ_d，t_acc]时间内产生的位移s₉为：

$s_9=\frac{1}{8}({15k}_1+{12k}_2+{8k}_3+{4k}_4){\mathrm{a\tau }}_d^2+\frac{1}{2}a(t_{\mathrm{acc}}-{4\tau }_d){\tau }_d---\left(21\right)$

在匀速段的t_p-2t_acc时间内产生的位移s₁₀为：

s₁₀＝[(4k₁+3k₂+2k₃+k₄)aτ_d+a(t_acc-4τ_d)](t_p-2t_acc)>

因此，整个规划周期内产生的总位移可表示为：

$\left(\begin{array}{c}{S}_p=2\underset{i=1}{\overset{9}{\Sigma }}{s}_i+s_{10}\\ =-{\mathrm{at}}_{\mathrm{acc}}^2+[t_p+(4-{4k}_1-{3k}_2-{2k}_3-k_4){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}+({4k}_1+{3k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d{t}_p\end{array}\right)---\left(23\right)$

根据方程(23)可得，轨迹规划的加速度可配置为：

$a=\frac{S_p}{{-t}_{\mathrm{acc}}^2+[t_p+(4-{4k}_1-{3k}_2-{2k}_3-k_4){\tau }_d]t_{\mathrm{acc}}+({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d{t}_p}---\left(24\right)$

因此，针对上述19个不同的时间区间，规划方式4的轨迹表示为：

当$t\le \frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)={0.5k}_1{\mathrm{at}}^2\\ v\left(t\right)=k_1\mathrm{at}\\ a\left(t\right)=k_{1}a\end{array}\right)---\left(25\right)$

当$\frac{{\tau }_d}{2}<t\le {\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}a(k_1+k_2)t^2-\frac{1}{2}{\mathrm{ak}}_2{\tau }_{d}t+\frac{1}{8}{\mathrm{ak}}_2{\tau }_d^2\\ v\left(t\right)=\frac{1}{2}{k}_1{\mathrm{a\tau }}_d+(k_1+k_2)a(t-\frac{{\tau }_d}{2})\\ a\left(t\right)=(k_1+k_2)a\end{array}\right)---\left(26\right)$

当τ_d＜t≤1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(\frac{1}{8}{k}_2+\frac{1}{2}{k}_3){\mathrm{a\tau }}_d^2-(\frac{1}{2}{k}_2+k_3){\mathrm{a\tau }}_{d}t+\frac{1}{2}(k_1+k_2+k_3){\mathrm{at}}^2\\ v\left(t\right)=(k_1+\frac{k_2}{2}){\mathrm{a\tau }}_d+(k_1+k_2+k_3)a(t-{\tau }_d)\\ a\left(t\right)=(k_1+k_2+k_3)a\end{array}\right)---\left(27\right)$

当1.5τ_d＜t≤2τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(\frac{k_2}{8}+0.5k_3+\frac{9}{8}{k}_4){\mathrm{a\tau }}_d^2-({0.5k}_2+k_3+1.5k_4){\mathrm{a\tau }}_{d}t+0.5(k_1+k_2+k_3+k_4){\mathrm{at}}^2\\ v\left(t\right)=({1.5k}_1+k_2+0.5k_3){\mathrm{a\tau }}_d+(k_1+k_2+k_3+k_4)a(t-1.5{\tau }_d)\\ a\left(t\right)=(k_1+k_2+k_3+k_4)a\end{array}\right)---\left(28\right)$

当2τ_d＜t≤t_acc-2τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(2-{2k}_1-\frac{15}{8}{k}_2-1.5k_3-\frac{7}{8}{k}_4){\mathrm{a\tau }}_d^2+({2k}_1+1.5k_2+k_3+0.5k_4-2){\mathrm{a\tau }}_{d}t+0.5{\mathrm{at}}^2\\ v\left(t\right)=({2k}_1+1.5k_2+k_3+0.5k_4){\mathrm{a\tau }}_d+a(t-{2\tau }_d)\\ a\left(t\right)=a\end{array}\right)---\left(29\right)$

当t_acc-2τ_d＜t≤t_acc-1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(1-k_1-k_2-k_3-k_4){\mathrm{at}}_{\mathrm{acc}}({2\tau }_d-{0.5t}_{\mathrm{acc}})+(\frac{1}{8}{k}_2+{0.5k}_3+\frac{9}{8}{k}_4)a{\tau }_d^2\\ +[({4k}_1+3.5k_2+3k_3+2.5k_4-4){\tau }_d+(1-k_1-k_2-k_3-k_4)t_{\mathrm{acc}}]\mathrm{at}+0.5(k_1+k_2+k_3+k_4){\mathrm{at}}^2\\ v\left(t\right)=({2k}_1+1.5k_2+k_3+0.5k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+(k_1+k_2+k_3+k_4)a(t-t_{\mathrm{acc}}+{2\tau }_d)\\ a\left(t\right)=(k_1+k_2+k_3+k_4)a\end{array}\right)---\left(100\right)$

当t_acc-1.5τ_d＜t≤t_acc-τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[0.5(k_1+k_2+k_3-1)t_{\mathrm{acc}}+(2-{2k}_1-{2k}_2-{2k}_3-0.5k_4){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}+(\frac{1}{8}{k}_2+0.5k_3){\mathrm{a\tau }}_d^2\\ +[({4k}_1+3.5k_2+{3k}_3+k_4-4){\tau }_d+(1-k_1-k_2-k_3)t_{\mathrm{acc}}]\mathrm{at}+0.5(k_1+k_2+k_3){\mathrm{at}}^2\\ v\left(t\right)=({2.5k}_1+{2k}_2+1.5k_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+(k_1+k_2+k_3)a(t-t_{\mathrm{acc}}+1.5{\tau }_d)\\ a\left(t\right)=(k_1+k_2+k_3)a\end{array}\right)---\left(30\right)$

当$t_{\mathrm{acc}}-{\tau }_d<t\le t_{\mathrm{acc}}-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=[0.5(k_1+k_2-1)t_{\mathrm{acc}}+(2-{2k}_1-{2k}_2-k_3-0.5k_4){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}+\frac{1}{8}{k}_2{\mathrm{a\tau }}_d^2\\ +[({4k}_1+3.5k_2+{2k}_3+k_4-4){\tau }_d+(1-k_1-k_2)t_{\mathrm{acc}}]\mathrm{at}+0.5(k_1+k_2){\mathrm{at}}^2\\ v\left(t\right)=({3k}_1+{2.5k}_2+2k_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+(k_1+k_2)a(t-t_{\mathrm{acc}}+{\tau }_d)\\ a\left(t\right)=(k_1+k_2)a\end{array}\right)---\left(31\right)$

当$t_{\mathrm{acc}}-\frac{{\tau }_d}{2}<t{\le t}_{\mathrm{acc}}$时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-1.5k_2-k_3-0.5k_4){\tau }_d+0.5(k_1-1)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}\\ +[({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d+(1-k_1)t_{\mathrm{acc}}]\mathrm{at}+0.5k_1{\mathrm{at}}^2\\ v\left(t\right)=({3.5k}_1+{3k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+k_{1}a(t-t_{\mathrm{acc}}+0.5{\tau }_d)\\ a\left(t\right)=k_{1}a\end{array}\right)---\left(32\right)$

当t_acc＜t≤t_p-t_acc时：

$\left(\begin{array}{c}s\left(t\right)=[({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d+t_{\mathrm{acc}}]a(t-0.5t_{\mathrm{acc}})\\ v\left(t\right)=({4k}_1+{3k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}\\ a\left(t\right)=0\end{array}\right)---\left(33\right)$

当$t_p-t_{\mathrm{acc}}<t\le t_p-t_{\mathrm{acc}}+\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-1.5k_2-k_3-0.5k_4){\tau }_d-0.5(1+k_1)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}-({0.5t}_p-t_{\mathrm{acc}})k_1{\mathrm{at}}_p\\ +[({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d+(1-k_1)t_{\mathrm{acc}}+k_1{t}_p]\mathrm{at}-0.5k_1{\mathrm{at}}^2\\ v\left(t\right)=({4k}_1+{3k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-k_{1}a(t-t_p+t_{\mathrm{acc}})\\ a\left(t\right)=-k_{1}a\end{array}\right)---\left(34\right)$

当$t_p-t_{\mathrm{acc}}+\frac{{\tau }_d}{2}<t\le t_p-t_{\mathrm{acc}}+{\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-k_2-k_3-{0.5k}_4){\tau }_d-0.5(1+k_1+k_2)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}-(k_1+k_2){\mathrm{at}}_p({0.5t}_p-t_{\mathrm{acc}})\\ -({0.5t}_p+\frac{1}{8}{\tau }_d)k_2{\mathrm{a\tau }}_d-0.5(k_1+k_2){\mathrm{at}}^2\\ +[({4k}_1+{3.5k}_2+{2k}_3+k_4-4){\tau }_d+(1-k_1-k_2)t_{\mathrm{acc}}+(k_1+k_2)t_p]\mathrm{at}\\ v\left(t\right)=({3.5k}_1+{3k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-(k_1+k_2)a(t-t_p+t_{\mathrm{acc}}-{0.5\tau }_d)\\ a\left(t\right)=-(k_1+k_2)a\end{array}\right)---\left(35\right)$

当t_p-t_acc+τ_d<t≤t_p-t_acc+1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-k_2-{0.5k}_4){\tau }_d-0.5(1+k_1+k_2+k_3)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}+(k_1+k_2+k_3){\mathrm{at}}_p(t_{\mathrm{acc}}-{0.5t}_p)\\ -[(\frac{1}{8}{k}_2+{0.5k}_3){\tau }_d+({0.5k}_2+k_3)t_p]{\mathrm{a\tau }}_d-0.5(k_1+k_2+k_3){\mathrm{at}}^2\\ +[({4k}_1+{3.5k}_2+{3k}_3+k_4-4){\tau }_d+(1-k_1-k_2-k_3)t_{\mathrm{acc}}+(k_1+k_2+k_3)t_p]\mathrm{at}\\ v\left(t\right)=({3k}_1+{2.5k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-(k_1+k_2+k_3)a(t-t_p+t_{\mathrm{acc}}-{\tau }_d)\\ a\left(t\right)=-(k_1+k_2+k_3)a\end{array}\right)---\left(36\right)$

当t_p-t_acc+1.5τ_d<t≤t_p-t_acc+2τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(2-{2k}_1-k_2+k_4){\mathrm{at}}_{\mathrm{acc}}{\tau }_d-0.5a(k_1+k_2+k_3+k_4){(t_p-t_{\mathrm{acc}})}^2-{0.5\mathrm{at}}_{\mathrm{acc}}^2-(\frac{k_2}{8}+{0.5k}_3+\frac{9}{8}{k}_4){\mathrm{a\tau }}_d^2\\ -({0.5k}_2+k_3+{1.5k}_4){\mathrm{a\tau }}_d{t}_p-0.5(k_1+k_2+k_3+k_4){\mathrm{at}}^2\\ +[({4k}_1+{3.5k}_2+{3k}_3+{2.5k}_4-4){\tau }_d+(1-k_1-k_2-k_3-k_4)t_{\mathrm{acc}}+(k_1+k_2+k_3+k_4)t_p]\mathrm{at}\\ v\left(t\right)=({2.5k}_1+{2k}_2+{1.5k}_3+k_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-(k_1+k_2+k_3+k_4)a(t-t_p+t_{\mathrm{acc}}-{1.5\tau }_d)\\ a\left(t\right)=-(k_1+k_2+k_3+k_4)a\end{array}\right)---\left(37\right)$

当t_p-t_acc+2τ_d<t≤t_p-2τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d-t_p]a({0.5t}_p-t_{\mathrm{acc}})+(2k_1+\frac{15}{8}{k}_2+1.5k_3+\frac{7}{8}{k}_4-2){\mathrm{a\tau }}_d^2\\ -{\mathrm{at}}_{\mathrm{acc}}^2+[({2k}_1+{1.5k}_2+k_3+{0.5k}_4-2){\tau }_d+t_p]\mathrm{at}-{0.5\mathrm{at}}^2\\ v\left(t\right)=({2k}_1+{1.5k}_2+k_3+{0.5k}_4-4){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-a(t-t_p+t_{\mathrm{acc}}-{2\tau }_d)\\ a\left(t\right)=-a\end{array}\right)---\left(38\right)$

当t_p-2τ_d<t≤t_p-1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[t_p-({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}-0.5(k_1+k_2+k_3+k_4){\mathrm{at}}_p^2-{\mathrm{at}}_{\mathrm{acc}}^2\\ -(\frac{1}{8}{k}_2+{0.5k}_3+\frac{9}{8}{k}_4){\mathrm{a\tau }}_d^2+({4k}_1+{3.5k}_2+{3k}_3+{2.5k}_4-4){\mathrm{a\tau }}_d{t}_p\\ +[(k_1+k_2+k_3+k_4)t_p-({0.5k}_2+k_3+{1.5k}_4){\tau }_d]\mathrm{at}-0.5(k_1+k_2+k_3+k_4){\mathrm{at}}^2\\ v\left(t\right)=({2k}_1+{1.5k}_2+k_3+{0.5k}_4){\mathrm{a\tau }}_d-(k_1+k_2+k_3+k_4)a(t-t_p+{2\tau }_d)\\ a\left(t\right)=-(k_1+k_2+k_3+k_4)a\end{array}\right)---\left(39\right)$

当t_p-1.5τ_d<t≤t_p-τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[(4-{4k}_1-{3k}_2-{2k}_3-k_4){\tau }_d+t_p]{\mathrm{at}}_{\mathrm{acc}}-0.5(k_1+k_2+k_3){\mathrm{at}}_p^2-(\frac{k_2}{8}+{0.5k}_3){\mathrm{a\tau }}_d^2-{\mathrm{at}}_{\mathrm{acc}}^2\\ +({4k}_1+{3.5k}_2+{3k}_3+k_4-4){\mathrm{a\tau }}_d{t}_p-0.5(k_1+k_2+k_3){\mathrm{at}}^2\\ +[(k_1+k_2+k_3)t_p-({0.5k}_2+k_3){\tau }_d]\mathrm{at}\\ v\left(t\right)=({1.5k}_1+k_2+{0.5k}_3){\mathrm{a\tau }}_d-(k_1+k_2+k_3)a(t-t_p+{1.5\tau }_d)\\ a\left(t\right)=-(k_1+k_2+k_3)a\end{array}\right)---\left(99\right)$

当$t_p-{\tau }_d<t\le t_p-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=[(4-{4k}_1-{3k}_2-{2k}_3-k_4){\tau }_d+t_p]{\mathrm{at}}_{\mathrm{acc}}-0.5(k_1+k_2){\mathrm{at}}_p^2-\frac{1}{8}{k}_2{\mathrm{a\tau }}_d^2-{\mathrm{at}}_{\mathrm{acc}}^2\\ +({4k}_1+{3.5k}_2+{2k}_3+k_4-4){\mathrm{a\tau }}_d{t}_p-0.5(k_1+k_2){\mathrm{at}}^2+[(k_1+k_2)t_p-{0.5k}_2{\tau }_d]\mathrm{at}\\ v\left(t\right)=(k_1+{0.5k}_2){\mathrm{a\tau }}_d-(k_1+k_2)a(t-t_p+{\tau }_d)\\ a\left(t\right)=-(k_1+k_2)a\end{array}\right)---\left(40\right)$

当$t_p-\frac{{\tau }_d}{2}<t\le t_p$时：

$\left(\begin{array}{c}s\left(t\right)=[({4k}_1+{3k}_2+{2k}_3+k_4-4){\tau }_d+t_{\mathrm{acc}}]a(t_p-t_{\mathrm{acc}})-{0.5k}_1{\mathrm{at}}_p^2+k_1{\mathrm{at}}_{p}t-{0.5k}_1{\mathrm{at}}^2\\ v\left(t\right)=0.5k_1{\mathrm{a\tau }}_d-k_{1}a(t-t_p+{0.5\tau }_d)\\ a\left(t\right)=-k_{1}a\end{array}\right)---\left(41\right).$

结合图5解释本实施方式，其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：步骤四(二)所 述的规划方式3的规划方法为：

加速度按照四脉冲整形的方式进行配置，其中k₁、k₂、k₃为整形系数，由机器人系统 的阻尼比ζ决定；其计算方法为：

k₁＝0.16054+0.76699ζ+2.2656ζ²-1.2275ζ³>

k₂＝0.33911+0.45081ζ-2.5808ζ²+1.7365ζ³>

k₃＝0.34089-0.61533ζ-0.68765ζ²+0.42261ζ³>

规划方式3将时间区间分为15个区间，即[τ_d，1.5τ_d]， [1.5τ_d，t_acc-1.5τ_d]，[t_acc-1.5τ_d，t_acc-τ_d]，[t_acc，t_p-t_acc]， [t_p-t_acc+τ_d，t_p-t_acc+1.5τ_d]， [t_p-t_acc+1.5τ_d，t_p-1.5τ_d]，[t_p-1.5τ_d，t_p-τ_d，]，和

在时间内产生的位移s₁为：

$s_1=\frac{1}{8}{k}_1{\mathrm{a\tau }}_d^2---\left(45\right)$

在时间内产生的位移s₂为：

$s_2=\frac{1}{8}({3k}_1+k_2){\mathrm{a\tau }}_d^2---\left(46\right)$

在[τ_d，1.5τ_d]时间内产生的位移s₃为：

$s_3=\frac{1}{8}({5k}_1+{3k}_2+k_3){\mathrm{a\tau }}_d^2---\left(47\right)$

在[1.5τ_d，t_acc-1.5τ_d]时间内产生的位移s₄为：

$s_4=\frac{1}{2}({3k}_1+{2k}_2+k_3)(t_{\mathrm{acc}}-{3\tau }_d){\mathrm{a\tau }}_d+\frac{1}{2}a{(t_{\mathrm{acc}}-{3\tau }_d)}^2---\left(48\right)$

在[t_acc-1.5τ_d，t_acc-τ_d]时间内产生的位移s₅为：

$s_5=\frac{1}{8}({7k}_1+{5k}_2+{3k}_3){\mathrm{a\tau }}_d^2+\frac{1}{2}{\mathrm{a\tau }}_d(t_{\mathrm{acc}}-{3\tau }_d)---\left(49\right)$

时间内产生的位移s₆为：

$s_6=\frac{1}{8}({9k}_1+{7k}_2+{4k}_3){\mathrm{a\tau }}_d^2+\frac{1}{2}{\mathrm{a\tau }}_d(t_{\mathrm{acc}}-{3\tau }_d)---\left(50\right)$

在时间内产生的位移s₇为：

$s_7=\frac{1}{8}({11k}_1+{8k}_2+{4k}_3){\mathrm{a\tau }}_d^2+\frac{1}{2}{\mathrm{a\tau }}_d(t_{\mathrm{acc}}-{3\tau }_d)---\left(51\right)$

在匀速段的t_p-2t_acc时间内产生的位移s₈为：

s₈＝[(3k₁+2k₂+k₃)aτ_d+a(t_acc-3τ_d)](t_p-2t_acc)(52)

因此，整个规划周期内产生的总位移可表示为：

$\left(\begin{array}{c}{S}_p=2\underset{i=1}{\overset{7}{\Sigma }}{s}_i+s_8\\ ={-\mathrm{at}}_{\mathrm{acc}}^2+[t_p-({3k}_1+{2k}_2+k_3-3){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}+({3k}_1+{2k}_2+k_3-3){\mathrm{a\tau }}_d{t}_p\end{array}\right)---\left(53\right)$

根据方程(53)可得，轨迹规划的加速度可配置为：

$a=\frac{S_p}{{-t}_{\mathrm{acc}}^2+[t_p-({3k}_1+{2k}_2+k_3-3){\tau }_d]t_{\mathrm{acc}}+({3k}_1+{2k}_2+k_3-3){\tau }_d{t}_p}---\left(54\right)$

因此，针对上述15个不同的时间区间，规划方式3的轨迹可表示为：

当$t\le \frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=0.5k_1{\mathrm{at}}^2\\ v\left(t\right)=k_1\mathrm{at}\\ a\left(t\right)=k_{1}a\end{array}\right)---\left(55\right)$

当$\frac{{\tau }_d}{2}<t\le {\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}a(k_1+k_2)t^2-\frac{1}{2}{\mathrm{ak}}_2{\tau }_{d}t+\frac{1}{8}{\mathrm{ak}}_2{\tau }_d^2\\ v\left(t\right)=\frac{1}{2}{k}_1{\mathrm{a\tau }}_d+(k_1+k_2)a(t-\frac{{\tau }_d}{2})\\ a\left(t\right)=(k_1+k_2)a\end{array}\right)---\left(56\right)$

当τ_d＜t≤1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(\frac{1}{8}{k}_2+\frac{1}{2}{k}_3){\mathrm{a\tau }}_d^2-(\frac{1}{2}{k}_2+k_3){\mathrm{a\tau }}_{d}t+\frac{1}{2}(k_1+k_2+k_3){\mathrm{at}}^2\\ v\left(t\right)=(k_1+\frac{k_2}{2}){\mathrm{a\tau }}_d+(k_1+k_2+k_3)a(t-{\tau }_d)\\ a\left(t\right)=(k_1+k_2+k_3)a\end{array}\right)---\left(57\right)$

当1.5τ_d＜t≤t_acc-1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(\frac{9}{8}-\frac{9}{8}{k}_1-k_2-\frac{5}{8}{k}_3){\mathrm{a\tau }}_d^2+(\frac{3}{2}{k}_1+k_2+\frac{1}{2}{k}_3-\frac{3}{2}){\mathrm{a\tau }}_{d}t+\frac{1}{2}{\mathrm{at}}^2\\ v\left(t\right)=\frac{1}{2}({3k}_1+{2k}_2+k_3){\mathrm{a\tau }}_d+a(t-\frac{3}{2}{\tau }_d)\\ a\left(t\right)=a\end{array}\right)---\left(58\right)$

当t_acc-1.5τ_d＜t≤t_acc-τ_d时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}({3\tau }_d-t_{\mathrm{acc}})(1-k_1-k_2-k_3){\mathrm{at}}_{\mathrm{acc}}+\frac{1}{8}(k_2+{4k}_3){\mathrm{a\tau }}_d^2\\ +[\frac{1}{2}({6k}_1+{5k}_2+{4k}_3-6){\tau }_d+(1-k_1-k_2-k_3)t_{\mathrm{acc}}]\mathrm{at}+\frac{1}{2}(k_1+k_2+k_3){\mathrm{at}}^2\\ v\left(t\right)=(\frac{3}{2}{k}_1+k_2+\frac{k_3}{2}-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+(k_1+k_2+k_3)a(t-t_{\mathrm{acc}}+\frac{3}{2}{\tau }_d)\\ a\left(t\right)=(k_1+k_2+k_3)a\end{array}\right)---\left(59\right)$

当$t_{\mathrm{acc}}-{\tau }_d<t\le t_{\mathrm{acc}}-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}(3-{3k}_1-{3k}_2-k_3){\mathrm{a\tau }}_d{t}_{\mathrm{acc}}+\frac{1}{8}{\mathrm{ak}}_2{\tau }_d^2-\frac{1}{2}(1-k_1-k_2){\mathrm{at}}_{\mathrm{acc}}^2\\ +[(1-k_1-k_2)t_{\mathrm{acc}}+({3k}_1+{2.5k}_2+k_3-3){\tau }_d]\mathrm{at}+\frac{1}{2}(k_1+k_2){\mathrm{at}}^2\\ v\left(t\right)=({2k}_1+{1.5k}_2+k_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+(k_1+k_2)a(t-t_{\mathrm{acc}}+{\tau }_d)\\ a\left(t\right)=(k_1+k_2)a\end{array}\right)---\left(60\right)$

当$t_{\mathrm{acc}}-\frac{{\tau }_d}{2}<t\le t_{\mathrm{acc}}$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}[(3-{2k}_2-{3k}_1-k_3){\tau }_d-(1-k_1)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}\\ +[({2k}_2+{3k}_1+k_3-3){\tau }_d+(1-k_1)t_{\mathrm{acc}}]\mathrm{at}+\frac{1}{2}{k}_1{\mathrm{at}}^2\\ v\left(t\right)=({2.5k}_1+{2k}_2+k_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}+k_{1}a(t-t_{\mathrm{acc}}+{0.5\tau }_d)\\ a\left(t\right)=k_{1}a\end{array}\right)---\left(61\right)$

当t_acc＜t≤t_p-t_acc时：

$\left(\begin{array}{c}s\left(t\right)=[(3-{2k}_2-{3k}_1-k_3){\tau }_d-t_{\mathrm{acc}}]a({0.5t}_{\mathrm{acc}}-t)\\ v\left(t\right)=({3k}_1+{2k}_2+k_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}\\ a\left(t\right)=0\end{array}\right)---\left(62\right)$

当$t_p-t_{\mathrm{acc}}<t\le t_p-t_{\mathrm{acc}}+\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=0.5[(3-{2k}_2-{3k}_1-k_3){\tau }_d-(1+k_1)t_{\mathrm{acc}}]-({0.5t}_p-t_{\mathrm{acc}})k_1{\mathrm{at}}_p\\ +[({3k}_1+{2k}_2+k_3-3){\tau }_d+(1-k_1)t_{\mathrm{acc}}+k_1{t}_p]\mathrm{at}-\frac{1}{2}{k}_1{\mathrm{at}}^2\\ v\left(t\right)=({3k}_1+{2k}_2+k_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-k_{1}a(t-t_p+t_{\mathrm{acc}})\\ a\left(t\right)={-k}_{1}a\end{array}\right)---\left(63\right)$

当$t_p-t_{\mathrm{acc}}+\frac{{\tau }_d}{2}<t\le t_p-t_{\mathrm{acc}}+{\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}[(3-k_2-{3k}_1-k_3){\tau }_d-(1+k_1+k_2)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}-\frac{1}{8}{k}_2({\tau }_d+{4t}_p){\mathrm{a\tau }}_d-(k_1+k_2){\mathrm{at}}_p({0.5t}_p-t_{\mathrm{acc}})\\ +[(1-k_1-k_2)t_{\mathrm{acc}}+(k_1+k_2)t_p+({3k}_1+{2.5k}_2+k_3-3){\tau }_d]\mathrm{at}-\frac{1}{2}(k_1+k_2){\mathrm{at}}^2\\ v\left(t\right)=({2.5k}_1+{2k}_2+k_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-(k_1+k_2)a(t-t_p+t_{\mathrm{acc}}-{0.5\tau }_d)\\ a\left(t\right)=-(k_1+k_2)a\end{array}---\left(64\right)\right)$

当t_p-t_acc+τ_d＜t≤t_p-t_acc+1.5τ_d时： $\left(\begin{array}{c}s\left(t\right)=0.5[(3+k_3-k_2-{3k}_1){\tau }_d-(1+k_1+k_2+k_3)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}-(\frac{k_2}{8}+\frac{k_3}{2}){\mathrm{a\tau }}_d^2\\ -0.5[(k_1+k_2+k_3)t_p+(2k_3+k_2){\tau }_d]{\mathrm{at}}_p+(k_1+k_2+k_3){\mathrm{at}}_{\mathrm{acc}}{t}_p\\ +[(1-k_1-k_2-k_3)t_{\mathrm{acc}}+(k_1+k_2+k_3)t_p+({2.5k}_2+{3k}_1+{2k}_3-3){\tau }_d]\mathrm{at}\\ -0.5(k_1+k_2+k_3){\mathrm{at}}^2\\ v\left(t\right)=({2k}_1+1.5k_2+k_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-(k_1+k_2+k_3)a(t-t_p+t_{\mathrm{acc}}-{\tau }_d)\\ a\left(t\right)=-(k_1+k_2+k_3)a\end{array}\right)---\left(65\right)$

当t_p-t_acc+1.5τ_d＜t≤t_p-1.5τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[({3k}_1+{2k}_2+k_3-3){\tau }_d-t_p]a({0.5t}_p-t_{\mathrm{acc}})+\frac{1}{8}({9k}_1+{5k}_3+{8k}_2-9){\mathrm{a\tau }}_d^2-{\mathrm{at}}_{\mathrm{acc}}^2\\ +[({1.5k}_1+k_2+{0.5k}_3-1.5){\tau }_d+t_p]\mathrm{at}-{0.5\mathrm{at}}^2\\ v\left(t\right)=({1.5k}_1+k_2+{0.5k}_3-3){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-a(t-t_p+t_{\mathrm{acc}}-{1.5\tau }_d)\\ a\left(t\right)=-a\end{array}\right)---\left(66\right)$

当t_p-1.5τ_d＜t≤t_p-τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(3-{3k}_1-{2k}_2-k_3){\mathrm{at}}_{\mathrm{acc}}{\tau }_d+({3k}_1+2.5k_2+{2k}_3-3){\mathrm{at}}_p{\tau }_d-0.5({0.25k}_2+k_3){\mathrm{a\tau }}_d^2\\ -0.5(k_1+k_2+k_3){\mathrm{at}}_p^2+{\mathrm{at}}_{\mathrm{acc}}(t_p-t_{\mathrm{acc}})+[(k_1+k_2+k_3)t_p-({0.5k}_2+k_3){\tau }_d]\mathrm{at}-0.5(k_1+k_2+k_3){\mathrm{at}}^2\\ v\left(t\right)=({1.5k}_1+k_2+{0.5k}_3){\mathrm{a\tau }}_d-(k_1+k_2+k_3)a(t-t_p+{1.5\tau }_d)\\ a\left(t\right)=-(k_1+k_2+k_3)a\end{array}\right)---\left(67\right)$

当$t_p-{\tau }_d<t\le t_p-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=[(3-{3k}_1-2k_2-k_3){\tau }_d-t_{\mathrm{acc}}]a{t}_{\mathrm{acc}}+[t_{\mathrm{acc}}-(3-{3k}_1-{2.5k}_2-k_3){\tau }_d]{\mathrm{at}}_p\\ -\frac{1}{8}{k}_2{\mathrm{a\tau }}_d^2-0.5(k_1+k_2){\mathrm{at}}_p^2+[(k_1+k_2)t_p-{0.5k}_2{\tau }_d]\mathrm{at}-0.5(k_1+k_2){\mathrm{at}}^2\\ v\left(t\right)=(k_1+{0.5k}_2){\mathrm{a\tau }}_d-(k_1+k_2)a(t-t_p+{\tau }_d)\\ a\left(t\right)=-(k_1+k_2)a\end{array}\right)---\left(68\right)$

当$t_p-\frac{{\tau }_d}{2}<t\le t_p$时：

$\left(\begin{array}{c}s\left(t\right)=[t_{\mathrm{acc}}-(3-{3k}_1-{2k}_2-k_3){\tau }_d]a(t_p-t_{\mathrm{acc}})-{0.5k}_1{\mathrm{at}}_p^2+k_1{\mathrm{at}}_{p}t-{0.5k}_1{\mathrm{at}}^2\\ v\left(t\right)={0.5k}_1{\mathrm{a\tau }}_d-k_{1}a(t-t_p+{0.5\tau }_d)\\ a\left(t\right)=-k_{1}a\end{array}\right)---\left(69\right).$

结合图4解释本实施方式，其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：步骤四(三)所 述的规划方式2的规划方法为：

加速度按照三脉冲整形的方式进行配置，其中k₁和k₂为整形系数，其取值为： k₁＝0.2625，k₂＝0.475；

规划方式2将时间区间分为11个区间，即[τ_d，t_acc-τ_d]， [t_acc，t_p-t_acc]，[t_p-t_acc+τ_d，t_p-τ_d，]，和

在时间内产生的位移s₁为：

$s_1=\frac{1}{2}{k}_{1}a{\left(\frac{{\tau }_d}{2}\right)}^2---\left(70\right)$

在时间内产生的位移s₂为：

$s_2=\frac{3}{8}{k}_1{\mathrm{a\tau }}_d^2+\frac{1}{8}{k}_2{\mathrm{a\tau }}_d^2---\left(71\right)$

在[τ_d，t_acc-τ_d]时间内产生的位移s₃为：

$s_3=\frac{1}{2}[{2k}_1{\mathrm{a\tau }}_d+k_2{\mathrm{a\tau }}_d+a(t_{\mathrm{acc}}-{2\tau }_d)](t_{\mathrm{acc}}-{2\tau }_d)---\left(72\right)$

在时间内产生的位移s₄为：

$s_4=\frac{1}{2}[\frac{5}{2}{k}_1{\mathrm{a\tau }}_d+\frac{3}{2}{k}_2{\mathrm{a\tau }}_d+2a(t_{\mathrm{acc}}-{2\tau }_d)]\frac{{\tau }_d}{2}---\left(73\right)$

在时间内产生的位移s₅为：

$s_5=\frac{1}{2}[\frac{7}{2}{k}_1{\mathrm{a\tau }}_d+{2k}_2{\mathrm{a\tau }}_d+2a(t_{\mathrm{acc}}-{2\tau }_d)]\frac{{\tau }_d}{2}---\left(74\right)$

在匀速段的t_p-2t_acc时间区间内产生的位移s₆为：

s₆＝[(2k₁+k₂)aτ_d+(t_acc-2τ_d)a](t_p-2t_acc)>

因此，整个规划周期内产生的总位移可表示为：

$\left(\begin{array}{c}{S}_p=2\underset{i=1}{\overset{5}{\Sigma }}{s}_i+s_6\\ ={-\mathrm{at}}_{\mathrm{acc}}^2+[t_p-({2k}_1+k_2-2){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}+({2k}_1+k_2-2){\mathrm{a\tau }}_d{t}_p\end{array}\right)---\left(76\right)$

根据方程(76)可得，轨迹规划的加速度可配置为：

$a=\frac{S_p}{{-t}_{\mathrm{acc}}^2+[t_p-({2k}_1+k_2-2){\tau }_d]t_{\mathrm{acc}}+({2k}_1+k_2-2){\tau }_d{t}_p}---\left(77\right)$

因此，针对上述11个不同的时间区间，规划方式2的轨迹可表示为：

当$t\le \frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)={0.5k}_1{\mathrm{at}}^2\\ v\left(t\right)=k_1\mathrm{at}\\ a\left(t\right)=k_{1}a\end{array}\right)---\left(78\right)$

当$\frac{{\tau }_d}{2}<t\le {\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{2}a(k_1+k_2)t^2-\frac{1}{2}{\mathrm{ak}}_2{\tau }_{d}t+\frac{1}{8}{\mathrm{ak}}_2{\tau }_d^2\\ v\left(t\right)=\frac{1}{2}{k}_1{\mathrm{a\tau }}_d+(k_1+k_2)a(t-\frac{{\tau }_d}{2})\\ a\left(t\right)=(k_1+k_2)a\end{array}\right)---\left(79\right)$

当τ_d＜t≤t_acc-τ_d时：

$\left(\begin{array}{c}s\left(t\right)=(\frac{1}{2}-\frac{1}{2}{k}_1-\frac{3}{8}{k}_2){\mathrm{a\tau }}_d^2+(k_1+\frac{1}{2}{k}_2-1){\mathrm{a\tau }}_{d}t+\frac{1}{2}{\mathrm{at}}^2\\ v\left(t\right)=(k_1+\frac{k_2}{2}){\mathrm{a\tau }}_d+a(t-{\tau }_d)\\ a\left(t\right)=a\end{array}\right)---\left(80\right)$

当$t_{\mathrm{acc}}-{\tau }_d<t\le t_{\mathrm{acc}}-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=({1-k}_1-k_2){\mathrm{at}}_{\mathrm{acc}}({\tau }_d-\frac{1}{2}{t}_{\mathrm{acc}})+\frac{1}{2}{k}_2{\mathrm{a\tau }}_d^2+[({2k}_1+\frac{3}{2}{k}_2-2){\tau }_d+(1-k_1-k_2)t_{\mathrm{acc}}]\mathrm{at}+\frac{1}{2}{(k_1+k_2)\mathrm{at}}^2\\ v\left(t\right)=(k_1+\frac{k_2}{2}){\mathrm{a\tau }}_d+a(t_{\mathrm{acc}}-2{\tau }_d)+a(k_1+k_2)(t-t_{\mathrm{acc}}+{\tau }_d)\\ a\left(t\right)=(k_1+k_2)a\end{array}\right)---\left(81\right)$

当$t_{\mathrm{acc}}-\frac{{\tau }_d}{2}<t\le t_{\mathrm{acc}}$时：

$\left(\begin{array}{c}s\left(t\right)=[(1-k_1-\frac{1}{2}{k}_2){\tau }_d-\frac{1}{2}(1-k_1)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}+[t_{\mathrm{acc}}(1-k_1)+({2k}_1+k_2-2){\tau }_d]\mathrm{at}+\frac{1}{2}{\mathrm{ak}}_1{t}^2\\ v\left(t\right)=(\frac{3}{2}{k}_1+k_2-2)a{\tau }_d+{\mathrm{at}}_{\mathrm{acc}}+k_{1}a(t-t_{\mathrm{acc}}+\frac{{\tau }_d}{2})\\ a\left(t\right)=k_{1}a\end{array}\right)---\left(82\right)$

当t_acc＜t≤t_p-t_acc时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-k_2){\tau }_d-t_{\mathrm{acc}}]a({0.5t}_{\mathrm{acc}}-t)\\ v\left(t\right)=(2k_1+k_2-2){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}\\ a\left(t\right)=0\end{array}\right)---\left(83\right)$

当$t_p-t_{\mathrm{acc}}<t\le t_p-t_{\mathrm{acc}}+\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=[{\tau }_d(1-k_1-\frac{1}{2}{k}_2){+k}_1{t}_p-\frac{1}{2}(1+k_1)t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}-\frac{1}{2}{\mathrm{ak}}_1{t}_p^2\\ +[(2k_1+k_2-2){\tau }_d+(1-k_1)t_{\mathrm{acc}}+k_1{t}_p]\mathrm{at}-\frac{1}{2}{\mathrm{ak}}_1{t}^2\\ v\left(t\right)=({2k}_1+k_2-2){\mathrm{a\tau }}_d+a{t}_{\mathrm{acc}}-k_{1}a(t-t_p+t_{\mathrm{acc}})\\ a\left(t\right)=-k_{1}a\end{array}\right)---\left(84\right)$

当$t_p-t_{\mathrm{acc}}+\frac{{\tau }_d}{2}<t\le t_p-t_{\mathrm{acc}}+{\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=[(1-k_1){\tau }_d-\frac{1+k_1+k_2}{2}{t}_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}-(\frac{1}{8}{\tau }_d+\frac{1}{2}{t}_p){\mathrm{ak}}_2{\tau }_d+(t_{\mathrm{acc}}-\frac{1}{2}{t}_p)(k_1+k_2){\mathrm{at}}_p\\ +[({2k}_1+\frac{3}{2}{k}_2-2){\text{τ}}_d+(1-k_1-k_2)t_{\mathrm{acc}}+(k_1+k_2)t_p]\mathrm{at}-\frac{1}{2}(k_1+k_2){\mathrm{at}}^2\\ v\left(t\right)=(\frac{3k_1}{2}+k_2-2){\mathrm{a\tau }}_d+a{t}_{\mathrm{acc}}-(k_1+k_2)a(t-t_p+t_{\mathrm{acc}}-\frac{{\tau }_d}{2})\\ a\left(t\right)=-(k_1+k_2)a\end{array}\right)---\left(85\right)$

当t_p-t_acc+τ_d＜t≤t_p-τ_d时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-k_2){\tau }_d-t_{\mathrm{acc}}]{\mathrm{at}}_{\mathrm{acc}}+\frac{1}{8}({4k}_1+{3k}_2-4){\mathrm{a\tau }}_d^2\\ +\frac{1}{2}[({2k}_1+k_2-2){\tau }_d+{2t}_{\mathrm{acc}}-t_p]a{t}_p+[t_p+\frac{1}{2}({2k}_1+k_2-2){\tau }_d]\mathrm{at}-\frac{1}{2}{\mathrm{at}}^2\\ v\left(t\right)=(k_1+\frac{k_2}{2}-2){\mathrm{a\tau }}_d+{\mathrm{at}}_{\mathrm{acc}}-a(t-t_p+t_{\mathrm{acc}}-{\tau }_d)\\ a\left(t\right)=-a\end{array}\right)---\left(86\right)$

当$t_p-{\tau }_d<t\le t_p-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=[(2-{2k}_1-k_2)t_{\mathrm{acc}}+\frac{1}{2}({4k}_1+{3k}_2-4)t_p-\frac{1}{8}{k}_2{\tau }_d]{\mathrm{a\tau }}_d-\frac{1}{2}(k_1+k_2){\mathrm{at}}_p^2\\ +(t_p-t_{\mathrm{acc}}){\mathrm{at}}_{\mathrm{acc}}+[(k_1+k_2)t_p-\frac{1}{2}{k}_2{\tau }_d]\mathrm{at}-\frac{1}{2}(k_1+k_2){\mathrm{at}}^2\\ v\left(t\right)=(k_1+\frac{k_2}{2}){\mathrm{a\tau }}_d-(k_1+k_2)a(t-t_p+{\tau }_d)\\ a\left(t\right)=-(k_1+k_2)a\end{array}\right)---\left(87\right)$

当$t_p-\frac{{\tau }_d}{2}<t\le t_p$时：

$\left(\begin{array}{c}s\left(t\right)=-\frac{1}{2}a{k}_1{t}^2+{\mathrm{ak}}_1{t}_{p}t+(t_{\mathrm{acc}}-t_p)(2-{2k}_1-k_2){\mathrm{a\tau }}_d-\frac{1}{2}{\mathrm{ak}}_1{t}_p^2+a{t}_{\mathrm{acc}}(t_p-t_{\mathrm{acc}})\\ v\left(t\right)=\frac{k_1}{2}{\mathrm{a\tau }}_d-k_{1}a(t-t_p+\frac{1}{2}{\tau }_d)\\ a\left(t\right)=-k_{1}a\end{array}\right)---\left(88\right).$

结合图3解释本实施方式，其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：步骤四(四)所 述的规划方式1的规划方法为：

加速度按照两脉冲整形的方式进行配置，其中k为整形系数，由机器人系统的阻尼比 ζ决定；其计算方法为：

$k=\frac{1}{1+e^{\frac{-\mathrm{\zeta \pi }}{\sqrt{1-{\zeta }^2}}}}---\left(89\right)$

规划方式1将时间区间分为7个区间，即和

在时间内产生的位移s₁为：

$s_1=\frac{1}{8}k{\mathrm{a\tau }}_d^2---\left(90\right)$

在时间内产生的位移s₂为：

$s_2=\frac{1}{2}\mathrm{ka}(t_{\mathrm{acc}}-{\tau }_d){\tau }_d+\frac{1}{2}a{(t_{\mathrm{acc}}-{\tau }_d)}^2---\left(91\right)$

在时间内产生的位移s₃为：

$s_3=\frac{3}{8}{\mathrm{ka\tau }}_d^2+\frac{1}{2}a(t_{\mathrm{acc}}-{\tau }_d){\tau }_d---\left(92\right)$

在匀速段的t_p-2t_acc时间区间内产生的位移s₄为：

s₄＝[kaτ_d+a(t_acc-τ_d)](t_p-2t_acc)(93)

轨迹的减速段产生的位移与加速段相同；因此，整个规划周期内产生的总位移可表示 为：

$\left(\begin{array}{c}{S}_p=2\underset{i=1}{\overset{3}{\Sigma }}{s}_i+s_4\\ ={-\mathrm{at}}_{\mathrm{acc}}^2+[t_p+(1-k){\tau }_d]{\mathrm{at}}_{\mathrm{acc}}-(1-k){\mathrm{a\tau }}_d{t}_p\end{array}\right)---\left(94\right)$

根据方程(94)可得，轨迹规划的加速度可配置为：

$a=\frac{S_p}{{-t}_{\mathrm{acc}}^2+[t_p+(1-k){\tau }_d]t_{\mathrm{acc}}-(1-k){\tau }_d{t}_p}---\left(95\right)$

因此，针对上述的7个不同的时间区间，规划方式1的轨迹可表示为：其中s(t)表示 位置，v(t)表示速度，a(t)表示加速度：

当$t\le \frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=0.5{\mathrm{kat}}^2\\ v\left(t\right)=\mathrm{kat}\\ a\left(t\right)=\mathrm{ka}\end{array}\right)---\left(96\right)$

当$\frac{{\tau }_d}{2}<t\le t_{\mathrm{acc}}-\frac{{\tau }_d}{2}$时：

$\left(\begin{array}{c}s\left(t\right)=\frac{1}{8}{\mathrm{ka\tau }}_d^2+0.5\mathrm{ka}{\tau }_d(t-0.5{\tau }_d)+\frac{1}{2}a{(t-0.5{\tau }_d)}^2\\ v\left(t\right)=0.5{\mathrm{ka\tau }}_d+a(t-0.5{\tau }_d)\\ a\left(t\right)=a\end{array}\right)---\left(97\right)$

当$t_{\mathrm{acc}}-\frac{{\tau }_d}{2}<t\le t_{\mathrm{acc}}$时：

$\left(\begin{array}{c}s\left(t\right)=(1-k)a(t_{\mathrm{acc}}-{\tau }_d)(t-\frac{1}{2}{t}_{\mathrm{acc}})+\frac{1}{2}{\mathrm{kat}}^2\\ v\left(t\right)=0.5{\mathrm{ka\tau }}_d+a(t_{\mathrm{acc}}-{\tau }_d)+\mathrm{ka}(t-t_{\mathrm{acc}}+\frac{{\tau }_d}{2})\\ a\left(t\right)=\mathrm{ka}\end{array}\right)---\left(98\right)$

当t_acc<t≤t_p-t_acc时：

$\left(\begin{array}{c}s\left(t\right)=[t_{\mathrm{acc}}-(1-k){\tau }_d]a(t-\frac{1}{2}{t}_{\mathrm{acc}})\\ v\left(t\right)={\mathrm{ka\tau }}_d+a(t_{\mathrm{acc}}-{\tau }_d)\\ a\left(t\right)=0\end{array}\right)---\left(99\right)$

当$t_p-t_{\mathrm{acc}}<t\le t_p-t_{\mathrm{acc}}+\frac{1}{2}{\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=-\frac{1}{2}(1+k){\mathrm{at}}_{\mathrm{acc}}^2-{\mathrm{kat}}_p(\frac{1}{2}{t}_p-t_{\mathrm{acc}})+\frac{1}{2}(1-k){\mathrm{at}}_{\mathrm{acc}}{\tau }_d+((1-k)(t_{\mathrm{acc}}-{\tau }_d)+{\mathrm{kt}}_p)\mathrm{at}-\frac{1}{2}{\mathrm{kat}}^2\\ v\left(t\right)={\mathrm{ka\tau }}_d+a(t_{\mathrm{acc}}-{\tau }_d)-\mathrm{ka}(t-t_p+t_{\mathrm{acc}})\\ a\left(t\right)=-\mathrm{ka}\end{array}\right)---\left(100\right)$

当$t_p-t_{\mathrm{acc}}+\frac{1}{2}{\tau }_d<t\le t_p-\frac{1}{2}{\tau }_d$时：

$\left(\begin{array}{c}s\left(t\right)=(1-k){\mathrm{a\tau }}_d(t_{\mathrm{acc}}-\frac{1}{8}{\tau }_d-\frac{1}{2}{t}_p)-a(\frac{1}{2}{t}_p^2-t_{\mathrm{acc}}{t}_p+t_{\mathrm{acc}}^2)+(t_p-\frac{1}{2}(1-k){\tau }_d)\mathrm{at}-\frac{1}{2}{\mathrm{at}}^2\\ v\left(t\right)=-\frac{1}{2}(1-k){\mathrm{a\tau }}_d-a(t-t_p)\\ a\left(t\right)=-a\end{array}\right)---\left(101\right)$

当$t_p-\frac{1}{2}{\tau }_d<t\le t_p$时：

$\left(\begin{array}{c}s\left(t\right)=(1-k){\mathrm{a\tau }}_d(t_{\mathrm{acc}}-t_p)-a(t_{\mathrm{acc}}^2-t_{\mathrm{acc}}{t}_p+\frac{1}{2}{\mathrm{kt}}_p^2)+{\mathrm{kat}}_{p}t-\frac{1}{2}{\mathrm{kat}}^2\\ v\left(t\right)=\frac{1}{2}{\mathrm{ka\tau }}_d-\mathrm{ka}(t-t_p+\frac{1}{2}{\tau }_d)\\ a\left(t\right)=-\mathrm{ka}\end{array}\right)---\left(102\right).$

结合图2解释本实施方式，其它步骤及参数与具体实施方式一至六之一相同。

结合图6～图8本发明给出了一个应用实例：

本实施例以一种空间柔性关节机器人原型样机为例。该机器人关节采用谐波减速器作 为传动机构。谐波减速器由波发生器、刚轮和柔轮组成。谐波减速器的使用增加了关节柔 性，关节的扭转刚度K约为4.0×10⁴Nm/rad。以机器人的基关节为研究对象，基关节的 等效惯量J约为4198.0kgm²。因此，该系统的抖动频率f可计算为：

$f=\frac{1}{2\pi }\sqrt{\frac{K}{J}}=0.49\mathrm{Hz}$

因此，抖动周期τ_d＝1/f＝2.04秒。

若规划关节运动10°的角度，期望的规划周期t_p为4.5秒，加速时间t_acc为1.5秒， 参考的最大加速度限幅为0.5°/s²。

传统规划下的期望位置、期望速度、期望加速度及关节实际位置响应的曲线如图6 所示。

本发明的规划下的期望位置、期望速度、期望加速度及关节实际位置响应的曲线如图 7所示。

不同规划方式下的关节残余抖动的比较曲线如图8所示。

通过比较可知，在传统规划下，关节角位置约有±0.2°左右的残余抖动；而在本发 明的规划下，关节角位置的残余抖动在±0.02°以内，本发明的规划方法可以将90％以上 的关节残余抖动消除。