CN104020665B - 基于多目标粒子群算法的机械臂最小跃度轨迹优化方法 - Google Patents

Abstract

本发明公开了一种基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，首先利用三次样条插值函数生成各关节从任务起点到终点经过各路径节点的轨迹；根据生成的轨迹，确定优化目标函数中与之对应的各关节最小跃度均值；根据优化目标函数和任务的约束条件确定各关节的适应度函数；最后，根据各关节的适应度函数，利用多目标粒子群优化算法，得到优化目标函数对应的Pareto最优解集，然后按照一定的原则从最优解集中选择最优的轨迹规划方案。本发明综合考虑各个关节的跃度值，在降低路径中所有关节跃度的最大值以及各关节跃度值平均性和均衡分布方面都有明显改善，利用该方法，可有效减小机械臂的执行误差，减少运动过程对关节机构的磨损。

Description

基于多目标粒子群算法的机械臂最小跃度轨迹优化方法

技术领域

本发明涉及一种基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，属于机器人控制技术领域。

背景技术

随着自动控制和机器人领域的深入研究和技术发展，机械臂已广泛应用于半导体制造、工业、医疗、军事以及太空探索等领域，机械臂的研究热点和难点主要集中于运动轨迹规划及在此基础上的精确控制方面，其中，轨迹规划作为轨迹控制的基础，对机械臂的运行效率、平稳性、作业精度和能量消耗具有重要的意义。

轨迹规划是指给定起点和终点，根据任务要求寻求一条连接起点和终点的最优有效路径，然后将路径转换成机械臂各个关节的空间坐标，确定机械臂在运动过程中各关节的位移、速度、加速度和跃度，形成轨迹。有效的轨迹规划能够减少机械臂的磨损、节省作业时间、提高工作效率。

最小跃度作为轨迹优化的一个重要研究方向，是以一种间接的方式限制关节力矩的变化率，使机械臂的运动更加平稳，且可提高跟踪精度，减少机械臂的共振和机构磨损。

现有的关于最小跃度的轨迹优化方法主要分为两类：一类是根据单一目标函数(如机械臂的某个重要关节)进行单目标优化，如由Piazzi等提出的基于区间分析的算法，通过三次样条函数对轨迹插值，基于最大最小法使执行时间预设的路径的跃度最大绝对值全局最小，这类方法的缺点是，单目标函数只关注跃度最大时刻，不能反映整个任务过程中跃度的全局水平，即，只考虑某个关节的跃度，无法综合考虑到每个关节的跃度，导致机械臂中某个或某几个关节性能较好，而其余关节性能略逊，各关节的跃度不均衡会增加机械臂的执行误差，造成关节机构的磨损；另一类是，基于遗传算法的方法规划全局跃度最小的路径，由于遗传算法原理复杂，计算成本过大，不利于推广应用。

发明内容

鉴于上述原因，本发明的目的在于提供一种基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，该方法以降低机械臂各个关节的跃度为目标函数进行多目标优化，轨迹规划综合考虑到各关节的跃度，可使机械臂各关节在运动过程中性能均衡，减小机械臂的执行误差，避免某些关节在运动过程中受到严重磨损。

为实现上述目的，本发明采用以下技术方案：

基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，其特征在于，包括如下步骤：

1)在任务约束条件下，利用三次样条插值函数生成三次样条插值曲线，该三次样条插值函数为：

$\left\{\begin{array}{c}{Q}_{\mathrm{ji}}\left(t\right)=\frac{{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_i\right)}{6h_i}{(t_{i+1}-t)}^3+\frac{{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_{i+1}\right)}{6h_i}{(t-t_i)}^3\\ +[\frac{q_{j,i+1}}{h_i}-\frac{h_i{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_{i+1}\right)}{6}](t-t_i)\\ +[\frac{q_{j,i}}{h_i}-\frac{h_i{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_i\right)}{6}](t_{i+1}-t)\\ i=1,...,n-1\end{array}\right.$

其中，Q_ji(t)为时间区间[t_i，t_i+1]上的三次样条轨迹，为第i个路径节点处的加速度，q_ji为在起点及终点速度、加速度均为零的约束条件下第j个关节在第i个路径节点处的位置，h_i为相邻路径节点间的时间间隔，t_i为第i个路径节点处的时刻值，t_i+1为第i+1个路径节点处的时刻值，n为包括起点、终点的路径节点个数；

2)根据生成的三次样条插值曲线，确定优化目标函数中与之对应的各关节最小跃度：

$\begin{array}{c}{F}_{\mathrm{obj}}{\left(h\right)}_j=\underset{i-1}{\overset{n-1}{\mathrm{\Σ}}}|{\stackrel{...}{Q}}_{\mathrm{ji}}(h,t)\·h_i|\\ =\underset{i-1}{\overset{n-1}{\mathrm{\Σ}}}|\frac{({\mathrm{\α}}_{j,i+1}-{\mathrm{\α}}_{j,i})}{h_i}\·h_i|\\ =\underset{i-1}{\overset{n-1}{\mathrm{\Σ}}}|{\mathrm{\α}}_{j,i+1}-{\mathrm{\α}}_{j,i}|(j=1,...,m)\end{array}$

式中，F_obj(h)_j为第j个关节在给定时间向量h下的优化目标函数值，是三次样条插值曲线的跃度，m为机械臂的关节个数，α_j，i为第j个关节在第i个路径节点处的加速度，n为包括起点、终点的路径节点个数，h_i为相邻路径节点间的时间间隔；

3)根据优化目标函数和任务约束条件，确定各关节的适应度函数：

fitness(X)_j＝F_obj(h)_j+r₁G_v(h)_j+r₂G_a(h)_j+r₃G_j(h)_j+r₄G_t(h)_j

式中，fitnsess(X)_j为第j个关节在给定决策向量X下的适应度函数值，r₁，…，r₄为惩罚系数，G_v，G_a，G_j，G_t分别为速度、加速度、跃度、时间约束条件下的惩罚项；

4)根据各关节的适应度函数，利用多目标粒子群优化算法，得到优化目标函数对应的Pareto最优解集。

进一步的，

所述步骤4)包括如下步骤：

S1：设置算法初始化的相关参数，给定种群规模，在任务约束条件的范围内随机生成初始粒子的速度与位置，产生初始群体，计算得到初始群体中各粒子适应度函数值，并依据Pareto主导准则将初始种群中所有粒子相互比较完成后得到初始群体中Pareto最优解，并将最优解存入外部存储器I中；

S2：将每个粒子的个体最优极值设置为对应粒子的当前位置；

S3：每个粒子的全局最优极值，从外部存储器I中随机选取；

S4：对于每个粒子，根据选取出的个体最优极值和全局最优极值，按照以下公式计算新的粒子速度并更新粒子位置，得到新群体；

$\left\{\begin{array}{c}{v}_{\mathrm{ij}}(t+1)=\mathrm{\ω}\left(t\right)v_{\mathrm{ij}}\left(t\right)+c_1{r}_1({\mathrm{pbest}}_{\mathrm{ij}}\left(t\right)-x_{\mathrm{ij}}\left(t\right))\\ +c_2{r}_2({\mathrm{gbest}}_j\left(t\right)-x_{\mathrm{ij}}\left(t\right))\\ x_{\mathrm{ij}}(t+1)=x_{\mathrm{ij}}\left(t\right)+v_{\mathrm{ij}}(t+1)\end{array}\right.$

其中，j为粒子的第j维，j＝1，2，…，n，i表示第i个粒子，i＝1，2，…，M，c₁和c₂为学习因子，pbest_ij(t)为第t代种群中第i个粒子个体最优极值的第j维分量，gbest_j(t)为第t代种群中所有粒子全局最优极值的第j维分量；r₁和r₂为两个相互独立并服从正态分布的随机函数，ω为惯性权重；

x_ij(t)为第t代种群中第i个粒子位置的第j维分量，v_ij(t)为第t代种群中第i个粒子速度的第j维分量；

S5：依据任务的约束条件，将新群体中所有粒子的速度、位置都限定在约束条件范围内；

S6：利用新群体更新外部存储器I，若I中的粒子个数超过一定规模，计算每个粒子相应的拥挤度，保留拥挤度较大的粒子；

S7：对新群体中的所有粒子，按照基于约束主导的Pareto排序法更新每个粒子的个体最优极值；粒子的全局最优极值仍然从外部存储器I中随机选取；

S8：判断算法是否满足结束条件，若满足，则执行步骤S9，否则跳转至步骤S4；

S9：输出外部存储器I中的所有粒子作为所述优化目标函数对应的Pareto最优解集。

其中，从初始群体或更新后的新群体中确定Pareto最优解的方法是：

根据当前群体中每个粒子的位置信息确定当前各路径节点之间的时间间隔向量，根据该时间间隔向量以及任务的约束条件计算各个关节的适应度函数值，并将得到的适应度函数值依据Pareto主导准则与外部存储器I中所有粒子相互比较完成后确定是否为所述优化目标函数对应的Pareto最优解，若是则将该粒子存入外部存储器I中，否则对当前群体中其他粒子继续比较。

该方法还包括：

得到优化目标函数对应的Pareto最优解集后，按照各关节跃度均值方差最小且最大关节跃度值较小的原则，从所述Pareto最优解集中选择最优的轨迹规划方案。

本发明的优点在于：

本发明综合考虑了各个关节的跃度值，在降低路径中所有关节跃度的最大值以及各关节跃度值平均性和均衡分布方面都有明显改善，利用该方法，可有效减小机械臂的执行误差，减少运动过程对关节机构的磨损。

附图说明

图1是机械臂的运动规划系统。

图2是本发明的方法流程示意图。

图3是于一具体实施例中利用本发明的方法得到的Pareto前沿部分分量的散点示意图。

图4至图9是一具体实施例中机械臂最终选取规划方案中与六个关节对应的位置、速度、加速度及跃度曲线的示意图。

具体实施方式

以下结合附图和实施例对本发明作进一步详细的描述。

研究表明，降低机械臂各关节在运动过程中的跃度可以有效减小机械臂的执行误差，并可减少运动过程对关节机构的磨损，本发明即是提出了一种基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，具体说明如下：

一、最小跃度的轨迹优化问题

图1是机械臂的运动规划系统，如图所示，机械臂在执行任务前，需利用该系统针对任务需求对机械臂进行轨迹规划，其中，上层规划器用于根据任务需求生成路径节点，机械臂上的各个关节必须在规定的任务总时间之内，在速度、加速度、跃度等运动学约束的条件下，依次经过各路径节点以完成任务。

设定机械臂完成任务必须要经过的路径节点D为：

D＝{d_ij|i＝1，…，s；j＝1，…，m} (1)

其中，d_ij为机械臂上第j个关节在第i个路径节点处的位置，s为路径节点个数，m为机械臂的关节个数。

以机械臂上各关节的跃度均值最小为优化目标，构造如下优化目标函数：

$\left\{\begin{array}{c}\min \frac{1}{T}{\∫}_0^T\left|{\stackrel{...}{q}}_j\left(t\right)\right|\mathrm{dt}\\ j=1,...,m\end{array},---\left(2\right)\right.$

其中，T为任务总时间，为第j个关节在t时刻的跃度，m为机械臂的关节个数。与采用跃度平方积分作为优化目标相比，各个关节跃度均值的优化目标函数值与路径中各个关节跃度数值大小呈线性正比，可以避免关节跃度数值较大时，相应的目标函数值过大，导致优化算法不易达到运动学约束上限，不能充分发挥关节运动性能，从而难以获得各个关节跃度最小的Pareto最优解的问题。

在给定任务总时间的约束条件下，各关节的跃度均值最小的优化问题为：

$\left\{\begin{array}{c}\mathrm{find}:\\ \min \frac{1}{T}{\∫}_0^T\left|{\stackrel{...}{q}}_j\left(t\right)\right|\mathrm{dt}(j=1,...,m)\\ \mathrm{subject\; to}:\\ \left|{\stackrel{.}{q}}_j\left(t\right)\right|\≤{\mathrm{VC}}_j,j=1,...,m\\ \left|{\stackrel{..}{q}}_j\left(t\right)\right|\≤{\mathrm{WC}}_j,j=1,...,m\\ \left|{\stackrel{...}{q}}_j\left(t\right)\right|\≤{\mathrm{JC}}_j,j=1,...,m\\ h={[h_1,...,h_{s-1}]}^T\\ \underset{i=1}{\overset{s-1}{\mathrm{\Σ}}}{h}_i=T\end{array}\right.---\left(3\right)$

其中，为第j个关节在t时刻的速度，为第j个关节在t时刻的加速度，VC_j为第j个关节的速度最大值，WC_j为第j个关节的加速度最大值，JC_j为第j个关节的跃度最大值，h_i为两个相邻路径节点之间的时间间隔，T为任务总时间。

二、根据优化问题，实现最小跃度的轨迹优化方法

图2是本发明的方法流程图，结合图2所示：

1、利用三次样条插值函数生成各关节从任务起点到终点途径各路径节点的轨迹

在给定的路径节点之间插入额外的节点，可使机械臂的运动轨迹在速度、加速度、跃度上保持连续，进而使机械臂的整个运动过程更加平稳。

假设运动路径上的起点、终点的速度和加速度均为零，则，在该约束条件下，路径节点更新为：

Q＝{q_ji|j＝1，…，m；i＝1，…，n} (4)

其中，q_ji为在起点及终点速度、加速度均为零的约束条件下第j个关节在第i个节点处的位置，q_j，1为根据任务设定好的起点位置，q_j，n为根据任务设定好的终点位置，m为机械臂的关节个数，n＝s+2，s为路径节点个数。

令每个路径节点处的时刻值为t1，…，t_n，其中：t₁＝0、t_n＝T，则，相邻路径节点间的时间间隔为：

$\left\{\begin{array}{c}{h}_i=t_{i+1}-t_i\\ i=1,...,n-1\end{array}\right.---\left(5\right)$

令Q_ji(t)为时间区间[t_i，t_i+1]上的三次样条轨迹，对于一个关节，需要计算n-1次三次样条插值函数实现整个任务路径的插值，为避免求解4(n-1)个未知系数，对Q_ji(t)的二阶导数进行线性插值：

$\left\{\begin{array}{c}{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t\right)=\frac{t_{i+1}-t}{h_i}{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_i\right)+\frac{t-t_i}{h_i}{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_{i+1}\right)\\ i=1,...,n-1\end{array}\right.---\left(6\right)$

对公式(6)进行两次积分，并根据Q_ji(t_i)＝q_j，i和Q_ji(t_i+1)＝q_j，i+1两个边界条件确定积分常数，得到以下有关关节位置的三次样条插值函数：

$\left\{\begin{array}{c}{Q}_{\mathrm{ji}}\left(t\right)=\frac{{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_i\right)}{6h_i}{(t_{i+1}-t)}^3+\frac{{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_{i+1}\right)}{6h_i}{(t-t_i)}^3\\ +[\frac{q_{j,i+1}}{h_i}-\frac{h_i{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_{i+1}\right)}{6}](t-t_i)\\ +[\frac{q_{j,i}}{h_i}-\frac{h_i{\stackrel{..}{Q}}_{\mathrm{ji}}\left(t_i\right)}{6}](t_{i+1}-t)\\ i=1,...,n-1\end{array}\right.---\left(7\right)$

若给定时间向量该三次样条插值函数只与各路径节点处的加速度有关，由于起点和终点的加速度均设定为零，则需要求解起点与终点之间的n-2个路径节点处的加速度，为此，对公式(7)进行求导运算，同时利用各节点间速度连续的约束条件Q_i(t_i)＝Q_i-1(t_i)，建立如下线性方程：

$\left\{\begin{array}{c}{\mathrm{AZ}}_j=B_j\\ j=1,...,m\end{array}\right.---\left(8\right)$

令向量Z_j＝[α_j，2，，α_j，n-1]^T为n-2个路径节点处的加速度，为求解向量Z_j＝[α_j，2，，α_j，n-1]^T，其系数矩阵A只与h^*有关，

$\begin{array}{c}A=\left[\begin{array}{cccccc}{a}_{11}& a_{12}& & & & \\ a_{21}& a_{22}& a_{23}& & O& \\ & a_{32}& a_{33}& a_{34}& & \\ & & & .& & \\ & & & .& & \\ & & & .& & \\ O& & & & a_{n-2,n-3}& a_{n-2,n-2}\end{array}\right]\\ =\left[\begin{array}{cccccccc}{3h}_1+{2h}_2+{h_1}^2/h_2& h_2& & & & & & O\\ h_2-{h_1}^2/h_2& 2(h_2+h_3)& h_3& & & & & \\ & h_3& 2(h_3+h_4)& h_4& & & & \\ & & & & .& & & \\ & & & & .& & & \\ & & & & .& & & \\ & & & & & h_{n-3}& 2(h_{n-3}+h_{n-2})& h_{n-2}-\frac{h_{n-1}^2}{h_{n-2}}\\ O& & & & & & h_{n-2}& {3h}_{n-1}+{2h}_{n-2}+\frac{h_{n-1}^2}{h_{n-2}}\end{array}\right]\end{array}---\left(9\right)$

常向量B与q_ji和h_i具体关系如下：

$\begin{array}{c}B={[B_2,...,B_{n-1}]}^T\\ =\left[\begin{array}{c}\frac{6}{h_2}*[(\frac{q_3}{h_2}+\frac{q_1}{h_1})-(\frac{1}{h_1}+\frac{1}{h_2})\·(q_1+h_1{v}_1+\frac{{h_1}^2}{3}{\mathrm{\α}}_1)]-\frac{h_1}{h_2}{\mathrm{\α}}_1\\ \frac{6}{h_2{h}_3}(q_1+h_1{v}_1+\frac{{h_1}^2}{3}{\mathrm{\α}}_1)+\frac{{6q}_4}{h_3^2}-\frac{{6q}_3}{h_3}(\frac{1}{h_2}+\frac{1}{h_3})\\ \frac{6}{h_i}[\frac{q_{i-1}-q_i}{h_i}-\frac{q_i-q_{i-1}}{h_{i-1}}],i=\mathrm{4,5},...,n-3\\ \frac{6}{h_{n-2}^2}(q_n-v_n{h}_{n-1}+\frac{h_{n-1}^2}{3}{\mathrm{\α}}_n)-\frac{{6q}_{n-2}}{h_{n-2}}(\frac{1}{h_{n-2}}+\frac{1}{h_{n-3}})+\frac{{6q}_{n-3}}{h_{n-2}{h}_{n-3}}\\ \frac{-6}{h_{n-1}}(\frac{1}{h_{n-1}}+\frac{1}{h_{n-2}})(q_n-v_n{h}_{n-1}+\frac{h_{n-1}^2}{3}{\mathrm{\α}}_n)+\frac{{6q}_n}{h_{n-1}^2}+\frac{{6q}_{n-2}}{h_{n-1}{h}_{n-2}}-{\mathrm{\α}}_n]\end{array}\right]\end{array}3---\left(10\right)$

根据矩阵A、B求得向量Z_j＝[α_j，2，…，α_j，n-1]^T，然后根据向量Z_j可得到在给定的时间向量h^*下的三次样条插值函数Q_ji(t)，根据三次样条插值函数Q_ji(t)，即可确定额外插值节点的位置，以及各关节在所有任务节点之间的三次样条轨迹。

2、依据生成的起点到终点途径各路径节点的轨迹，确定优化目标函数中与之对应的各关节最小跃度均值

由于三次样条插值曲线的跃度在区间[t_i，t_i+1]上为常数，若对各关节的跃度进行积分，可将公式(3)所述的优化问题的目标函数向量转化为：

$\begin{array}{c}{F}_{\mathrm{obj}}{\left(h\right)}_j=\underset{i-1}{\overset{n-1}{\mathrm{\Σ}}}|{\stackrel{...}{Q}}_{\mathrm{ji}}(h,t)\·h_i|\\ =\underset{i-1}{\overset{n-1}{\mathrm{\Σ}}}|\frac{({\mathrm{\α}}_{j,i+1}-{\mathrm{\α}}_{j,i})}{h_i}\·h_i|\\ =\underset{i-1}{\overset{n-1}{\mathrm{\Σ}}}|{\mathrm{\α}}_{j,i+1}-{\mathrm{\α}}_{j,i}|(j=1,...,m)\end{array}---\left(11\right)$

其中，F_obj(h)_j为第j个关节在给定时间向量h下的优化目标函数值，是三次样条插值曲线的跃度，m为机械臂的关节个数，α_j，i为公式(8)中向量Z_i中的各个向量值，其由相邻路径节点间的时间间隔h_i决定，因此，优化问题转化为在各种约束条件下，如何实现两个相邻路径节点之间的最优时间分配。

3、依据优化目标函数和任务的约束条件，确定各关节的适应度函数

(1)时间约束条件

定义一个n-2维决策向量

X＝[x₁，…，x_n-2]^T (12)

(2)运动学约束和时间非负约束条件：

$\left\{\begin{array}{c}\left|{\stackrel{.}{Q}}_j\left(t\right)\right|\≤{\mathrm{VC}}_j,j=1,...,m\\ \left|{\stackrel{..}{Q}}_j\left(t\right)\right|\≤{\mathrm{WC}}_j,j=1,...,m\\ \left|{\stackrel{...}{Q}}_j\left(t\right)\right|\≤{\mathrm{JC}}_j,j=1,...,m\\ h_i>0,i=1,...,n-1\end{array}\right.---\left(14\right)$

利用罚函数法在各关节的适应度函数中加入约束条件，分别计算各约束条件对应的惩罚项：

A、速度约束惩罚项：

速度在区间[t_i，t_i+1]上是抛物线，其速度的最大值只能在区间的两端或者抛物线的顶点上，以表示关节j在第i条子路径上速度曲线达到顶点的时刻：

$t_{j,i}^{*}=t_i+\frac{h_i{\mathrm{\α}}_{j,i}}{{\mathrm{\α}}_{j,i}-{\mathrm{\α}}_{j,i+1}}---\left(15\right)$

定义最大速度绝对值超过速度约束的大小值：

$H_{j,i}\left(h\right)=\max \left\{\right|{\stackrel{.}{Q}}_{j,i}\left(t_i\right)|,|{\stackrel{.}{Q}}_{j,i}\left(t_{j,i}^{*}\right)|,|{\stackrel{.}{Q}}_{j,i}\left(t_{i+1}\right)\left|\right\}-{\mathrm{VC}}_j---\left(16\right)$

则，速度约束惩罚项G_v(h)为：

$G_v\left(h\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}\underset{i=1}{\overset{n-1}{\mathrm{\Σ}}}\left\{H_{j,i}\left(h\right)\right|H_{j,i}\left(h\right)>0\}---\left(17\right)$

B、加速度约束惩罚项：

加速度在区间[t_i，t_i+1]上为直线段，其约束惩罚项为：

$G_a\left(h\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left\{\begin{array}{cc}\left|{\stackrel{..}{Q}}_{j,i}(h,t_i)\right|-{\mathrm{WC}}_j,& \left|{\stackrel{..}{Q}}_{j,i}(h,t_i)\right|>{\mathrm{WC}}_j\\ 0,& \left|{\stackrel{..}{Q}}_{j,i}(h,t_i)\right|\≤{\mathrm{WC}}_j\end{array}\right.---\left(18\right)$

C、跃度约束惩罚项：

跃度在区间[t_i，t_i+1]上为常数，其约束惩罚项为：

$G_j\left(h\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}\underset{i=1}{\overset{n-1}{\mathrm{\Σ}}}\left\{\begin{array}{cc}\left|{\stackrel{...}{Q}}_{j,i}\left(h\right)\right|-{\mathrm{JC}}_j,& \left|{\stackrel{...}{Q}}_{j,i}\left(h\right)\right|>{\mathrm{JC}}_j\\ 0,& \left|{\stackrel{...}{Q}}_{j,i}\left(h\right)\right|\≤{\mathrm{JC}}_j\end{array}\right.---\left(19\right)$

D、时间非负约束惩罚项：

时间非负的惩罚项G_t只需满足t_i＞0(i＝1，…，n-1)，即，

$G_t\left(h\right)=\underset{i-1}{\overset{n-1}{\mathrm{\&Sigma;}}}\left\{\right|h_i|,h_i<0\}---\left(20\right)$

最终，根据优化目标函数和各种约束条件，得到各关节的适应度函数：

fitness(X)_j＝F_obj(h)_j+r₁G_v(h)_j+r₂G_a(h)_j+r₃G_j(h)_j+r₄G_t(h)_j (21)

其中fitness(X)_j为第j个关节在给定决策向量X下的适应度函数值，r₁，…，r₄为惩罚系数，G_v，G_a，G_j，G_t分别为速度、加速度、跃度、时间约束条件下的惩罚项。

4、根据各关节的适应度函数，利用多目标粒子群优化算法，得到优化目标函数对应的Pareto最优解集

(1)多目标粒子群优化算法(MOPSO算法)描述

本发明通过带有惯性系数的标准MOPSO算法对优化问题进行求解，MOPSO算法为：

$\left\{\begin{array}{c}{v}_{\mathrm{ij}}(t+1)=\mathrm{\&omega;}\left(t\right)v_{\mathrm{ij}}\left(t\right)+c_1{r}_1({\mathrm{pbest}}_{\mathrm{ij}}\left(t\right)-x_{\mathrm{ij}}\left(t\right))\\ +c_2{r}_2({\mathrm{gbest}}_j\left(t\right)-x_{\mathrm{ij}}\left(t\right))\\ x_{\mathrm{ij}}(t+1)=x_{\mathrm{ij}}\left(t\right)+v_{\mathrm{ij}}(t+1)\end{array}\right.---\left(22\right)$

惯性系数ω采用线性递减策略：

$\mathrm{\&omega;}\left(t\right)=({\mathrm{\&omega;}}_{\mathrm{init}}-{\mathrm{\&omega;}}_{\mathrm{end}})\frac{(T_{\max }-t)}{T_{\max }}+{\mathrm{\&omega;}}_{\mathrm{end}}---\left(23\right)$

其中，j表示为粒子的第j维，j＝1，2，…，n，i表示第i个粒子，i＝1，2，…，M，c₁和c₂为学习因子，pbest_ij(t)为第t代种群中第i个粒子个体最优极值的第j维分量，gbest_j(t)为第t代种群中所有粒子全局最优极值的第j维分量；r₁和R₂为两个相互独立并服从正态分布的随机函数。惯性权重ω用于控制粒子以前速度对当前速度的影响，它将影响粒子的全局和局部搜索能力。

x_ij(t)为第t代种群中第i个粒子位置的第j维分量，v_ij(t)为第t代种群中第i个粒子速度的第j维分量

(2)利用MOPSO算法，根据各关节的适应度函数，求解最小跃度优化问题的Pareto最优解集，具体步骤如下：

S1：设置算法初始化的相关参数，给定种群规模N，在任务约束条件的范围内随机生成初始粒子的速度与位置，产生初始群体，从初始群体中确定Pareto最优解，并将最优解存入外部存储器I(非劣解集)中；

其中，从群体中确定Pareto最优解的方法是：

初始生成或是更新后的群体包含了每个粒子的速度和位置信息，根据每个粒子的位置信息，公式(12)中每个粒子对应的决策向量可确定，公式(13)中当前各个路径节点之间的时间向量h也就随之确定，将时间间隔向量和约束条件代入公式(21)所示的各关节的适应度函数计算出适应度值，得到的适应度值再依据Pareto主导准则可确定出Pareto最优解。

S2：将每个粒子的个体最优极值Pbest(t)设置为对应粒子的当前位置；

S3：每个粒子的全局最优极值Gbest(t)，从外部存储器I中随机选取；

S4：对于每个粒子，根据选取出的个体最优极值和全局最优极值，按照公式(22)计算新的粒子速度并更新粒子位置，得到新群体；

S5：依据任务的约束条件，将新群体中所有粒子的速度、位置都限定在约束条件范围内(如，速度不能超过速度最大值，若超过则将速度设定为速度最大值)；

S6：利用新群体更新外部存储器I，若I中的粒子个数超过一定规模，计算每个粒子相应的拥挤度，保留拥挤度较大的粒子；

S7：对新群体中的所有粒子，按照基于约束主导的Pareto排序法更新每个粒子的个体最优极值Pbest(t)；粒子的全局最优极值仍然从外部存储器I中随机选取；

S8：判断算法是否满足结束条件，若满足，则执行步骤S9，否则跳转至步骤S4；

算法结束的条件是达到了设定的最大迭代次数，或是已经得到了满足要求的最优解集。

S9：输出外部存储器I中的所有粒子作为优化问题的Pareto最优解集

最后，根据一定的原则(如各关节跃度均值方差最小且最大关节跃度值较小)，从Pareto解集中选取出最优的轨迹规划方案。

四、具体实施例

于一具体实施例中，对一个包括六个关节的机械臂进行轨迹规划，任务的约束条件给定为：

表一

表一中的数据表示：任务中所有中间节点初始关节角，其中任务节点2和5是为了满足起点和终点运动学约束而加入的额外节点。

表二

表二中，给定了各关节的速度、加速度、跃度的约束条件。

在给定的任务及其约束条件下，利用本发明的方法，通过Matlab软件进行了仿真实验。其中，MOPSO算法的参数设定为：最大迭代次数T_max＝800，种群规模M＝100，取c₁＝c₂＝2，取v_max＝2，ω随迭代时间从ω_init＝0.9线性过渡到ω_end＝0.4，h^*随机生成。

图3是仿真实验后得到的Pareto前沿部分分量的散点示意图，如图所示，由于各关节跃度值之间的均匀分布性是由这组数值之间的方差衡量的，且方差越小所有关节的最大跃度值也越小，各关节之间的跃度值分布更平均、更均衡，因此从最终所得Pareto解集中选取各关节跃度均值方差最小时所对应的决策向量，该决策向量所对应的h^*为：

h^*＝[0.6362，2.5230，3.1451，1.9390，0.8566]

图4至图9分别是仿真实验后机械臂六个关节的位置、速度、加速度及跃度曲线示意图。如图所示，六个关节均在任务总时间内，经过规定的路径节点到达终点位置，且速度、加速度连续，所有运动学曲线均在运动学约束范围内。

为进一步体现本发明的方法与现有技术相比具有明显的改进之处，以下将本发明的方法与Gasparetto、Piazzi方法得到的实验结果数据进行比较：

表三

从对比的实验数据可知，与上述的现有方法相比，利用本发明的方法，所有关节中的最大跃度均值(关节3)的跃度均值明显减小，关节5的跃度均值略有减小，虽然其它关节的跃度均值略有增加，但是各个关节跃度均值之间的方差明显减小；而且，所有关节的平均速度和平均加速度总体减小。

综上所述，本发明的基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，以机械臂的各个关节的跃度绝对值积分函数值作为多个优化目标，首先利用三次样条插值函数生成起点到终点经过各路径节点的轨迹，根据生成的轨迹确定最小跃度的优化目标函数，根据目标函数和任务的约束条件确定各关节的适应度函数，最后利用多目标粒子群优化算法，根据各关节的适应度函数，求得最小跃度优化问题的Pareto最优解集，并根据需要，按照一定的原则从最优解集中选择最优的轨迹规划方案。本发明综合考虑各个关节的跃度值，在降低路径中所有关节跃度的最大值以及各关节跃度值平均性和均衡分布方面都有明显改善，利用该方法，可有效减小机械臂的执行误差，减少运动过程对关节机构的磨损。

以上所述是本发明的较佳实施例及其所运用的技术原理，对于本领域的技术人员来说，在不背离本发明的精神和范围的情况下，任何基于本发明技术方案基础上的等效变换、简单替换等显而易见的改变，均属于本发明保护范围之内。

Claims (3)

1.基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，其特征在于，包括如下步骤：

1)在任务约束条件下，利用三次样条插值函数生成三次样条插值曲线，该三次样条插值函数为：

$\left\{\begin{array}{c}{Q}_{ji}\left(t\right)=\frac{{\stackrel{\&CenterDot;\&CenterDot;}{Q}}_{ji}\left(t_i\right)}{6h_i}{(t_{i+1}-t)}^3+\frac{{\stackrel{\&CenterDot;\&CenterDot;}{Q}}_{ji}\left(t_{i+1}\right)}{6h_i}{(t-t_i)}^3\\ +\&lsqb;\frac{q_{j,i+1}}{h_i}-\frac{h_i{\stackrel{\&CenterDot;\&CenterDot;}{Q}}_{ji}\left(t_{i+1}\right)}{6}\&rsqb;(t-t_i)\\ +\&lsqb;\frac{q_{j,i}}{h_i}-\frac{h_i{\stackrel{\&CenterDot;\&CenterDot;}{Q}}_{ji}\left(t_i\right)}{6}\&rsqb;(t_{i+1}-t)\\ i=1,...,n-1\end{array}\right.$

其中，Q_ji(t)为时间区间[t_i,t_i+1]上的三次样条轨迹，为第i个路径节点处的加速度，q_ji为在起点及终点速度、加速度均为零的约束条件下第j个关节在第i个路径节点处的位置，h_i为相邻路径节点间的时间间隔，t_i为第i个路径节点处的时刻值，t_i+1为第i+1个路径节点处的时刻值，n为包括起点、终点的路径节点个数；

2)根据生成的三次样条插值曲线，确定优化目标函数中与之对应的各关节最小跃度：

$\begin{array}{c}{F}_{obj}{\left(h\right)}_j=\underset{i-1}{\overset{n-1}{\&Sigma;}}|{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{Q}}_{ji}(h,t)\&CenterDot;h_i|\\ =\underset{i-1}{\overset{n-1}{\&Sigma;}}|\frac{({\mathrm{\&alpha;}}_{j,i+1}-{\mathrm{\&alpha;}}_{j,i})}{h_i}\&CenterDot;h_i|\\ \begin{array}{cc}=\underset{i-1}{\overset{n-1}{\&Sigma;}}|{\mathrm{\&alpha;}}_{j,i+1}-{\mathrm{\&alpha;}}_{j,i}|& (j=1,\mathrm{...},m)\end{array}\end{array}$

式中，F_obj(h)_j为第j个关节在给定时间向量h下的优化目标函数值，是三次样条插值曲线的跃度，m为机械臂的关节个数，α_j,i为第j个关节在第i个路径节点处的加速度，n为包括起点、终点的路径节点个数，h_i为相邻路径节点间的时间间隔；

3)根据优化目标函数和任务约束条件，确定各关节的适应度函数：

fitness(X)_j＝F_obj(h)_j+r₁G_v(h)_j+r₂G_a(h)_j+r₃G_j(h)_j+r₄G_t(h)_j

式中，fitness(X)_j为第j个关节在给定决策向量X下的适应度函数值，r₁,...,r₄为惩罚系数，G_v,G_a,G_j,G_t分别为速度、加速度、跃度、时间约束条件下的惩罚项；

4)根据各关节的适应度函数，利用多目标粒子群优化算法，得到优化目标函数对应的Pareto最优解集；

所述步骤4)包括如下步骤：

S1：设置算法初始化的相关参数，给定种群规模，在任务约束条件的范围内随机生成初始粒子的速度与位置，产生初始群体，计算得到初始群体中各粒子适应度函数值，并依据Pareto主导准则将初始种群中所有粒子相互比较完成后得到初始群体中Pareto最优解，并将最优解存入外部存储器I中；

S2：将每个粒子的个体最优极值设置为对应粒子的当前位置；

S3：每个粒子的全局最优极值，从外部存储器I中随机选取；

S4：对于每个粒子，根据选取出的个体最优极值和全局最优极值，按照以下公式计算新的粒子速度并更新粒子位置，得到新群体；

$\left\{\begin{array}{c}{v}_{ij}(t+1)=\mathrm{\&omega;}\left(t\right)v_{ij}\left(t\right)+c_1{r}_1\left({\mathrm{pbest}}_{ij}\right(t)-x_{ij}(t\left)\right)\\ +c_2{r}_2\left({\mathrm{gbest}}_j\right(t)-x_{ij}(t\left)\right)\\ x_{ij}(t+1)=x_{ij}\left(t\right)+v_{ij}(t+1)\end{array}\right.$

其中，j为粒子的第j维，j＝1,2,…,n，i表示第i个粒子，i＝1,2,...,M，c₁和c₂为学习因子，pbest_ij(t)为第t代种群中第i个粒子个体最优极值的第j维分量，gbest_j(t)为第t代种群中所有粒子全局最优极值的第j维分量；r₁和r₂为两个相互独立并服从正态分布的随机函数，ω为惯性权重；

x_ij(t)为第t代种群中第i个粒子位置的第j维分量，v_ij(t)为第t代种群中第i个粒子速度的第j维分量；

S5：依据任务的约束条件，将新群体中所有粒子的速度、位置都限定在约束条件范围内；

S6：利用新群体更新外部存储器I，若I中的粒子个数超过一定规模，计算每个粒子相应的拥挤度，保留拥挤度较大的粒子；

S7：对新群体中的所有粒子，按照基于约束主导的Pareto排序法更新每个粒子的个体最优极值；粒子的全局最优极值仍然从外部存储器I中随机选取；

S8：判断算法是否满足结束条件，若满足，则执行步骤S9，否则跳转至步骤S4；

S9：输出外部存储器I中的所有粒子作为所述优化目标函数对应的Pareto最优解集。

2.如权利要求1所述的基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，其特征在于：

从初始群体或更新后的新群体中确定Pareto最优解的方法是：

根据当前群体中每个粒子的位置信息确定当前各路径节点之间的时间间隔向量，根据该时间间隔向量以及任务的约束条件计算各个关节的适应度函数值，并将得到的适应度函数值依据Pareto主导准则与外部存储器I中所有粒子相互比较完成后确定是否为所述优化目标函数对应的Pareto最优解，若是则将该粒子存入外部存储器I中，否则对当前群体中其他粒子继续比较。

3.如权利要求1或2所述的基于多目标粒子群算法的机械臂最小跃度轨迹优化方法，其特征在于，该方法还包括：

得到优化目标函数对应的Pareto最优解集后，按照各关节跃度均值方差最小且最大关节跃度值较小的原则，从所述Pareto最优解集中选择最优的轨迹规划方案。