CN111152224A - 一种双重寻优的机器人运动轨迹优化方法 - Google Patents

Abstract

本发明提供一种双重寻优的机器人运动轨迹优化方法，包括采用D‑H法构建出机器人相邻关节空间的数学模型并变换；采用三次样条插值法在变换后的机器人相邻关节空间的数学模型上进行运动轨迹规划，构建出表示机器人关于时间的运动轨迹多项式；基于运动轨迹多项式，以速度和加速度为约束条件，以每个关节总运动时间为优化目标，构建出目标函数；采用个体极值双重寻优的改进粒子群算法对目标函数进行优化，在适应度函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，得到约束条件下的整体潜在更优解，从而得到最优的机器人运动轨迹。实施本发明，使得机器人作业效率和平滑稳定性得到提升。

Description

一种双重寻优的机器人运动轨迹优化方法

技术领域

本发明涉及机器人技术领域和断路器自动装配技术领域，尤其涉及一种双重寻优的机器人运动轨迹优化方法。

背景技术

断路器是配电***中的重要保护性元件，在工业、民用等领域有广泛应用。目前断路器生产多以人工为主，自动化装配单元中柔性装配工艺缺失，装配流程复杂，只能完成单一规格产品的制造。工业机器人具有工作效率高、稳定可靠、重复精度好等优势，在制造行业得到了越来越多的应用。将工业机器人与断路器自动化制造相结合，研究断路器柔性化制造工艺及其相应的机器人协调控制方法，对于提升断路器制造效率和产品品质具有重要意义。

机器人进行装配作业时，其运动轨迹对于机器人的作业效率和平滑稳定性能有较大影响，轨迹规划是工业机器人运动控制的基础研究领域，决定着其作业效率和运动性能。

群智能算法被较多用来对机器人轨迹规划问题进行研究，包括时间最短、能耗最少和冲击最小等。一般通过群智能算法对机器人轨迹进行优化，需要设计相应的适应度函数或目标函数评判解的优劣性。如，马睿等通过遗传算法对三次多项式插值的轨迹时间进行优化，根据目标函数将轨迹中每段时间相加得到的值进行比对得到优化结果，然而其优化解每次结果各不相同，得到的结果不稳定。又如，冯斌等人通过粒子群算法对点到点之间的高次多项式插值轨迹时间进行了优化，并采用适应度函数对解的优劣进行评价，迭代结果在稳定性方面有待提高。又如，王玉宝等在对高次多项式轨迹优化过程中，通过动态调整学习因子改进了粒子群优化算法，虽然改进后的算法性能有了一定提升，但由于该方法只对群体极值进行了优化，没有考虑粒子迭代过程中分段最优极值，因而优化的群体极值之外仍可能存在潜在更优解。

针对上述问题，因此提出一种双重寻优的机器人轨迹优化新方法，在适应度函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，从而得到机器人运动速度和加速度约束条件下的整体潜在更优解。

发明内容

本发明实施例所要解决的技术问题在于，提供一种双重寻优的机器人运动轨迹优化方法，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，使得机器人运行时间有明显缩短，速度和加速度也更接近允许的最大值。

为了解决上述技术问题，本发明实施例提供了一种双重寻优的机器人运动轨迹优化方法，包括以下步骤：

根据机器人的关节结构及物理连接关系，采用D-H法构建出机器人相邻关节空间的数学模型，并将所述机器人相邻关节空间的数学模型基于基坐标系进行变换；

根据所述机器人作业任务要求，采用三次样条插值法在变换后的机器人相邻关节空间的数学模型上进行机器人运动轨迹规划，构建出表示机器人关于时间的运动轨迹多项式；其中，所述运动轨迹多项式为机器人在关节空间中各关节角度、运动速度和加速度关于时间的函数；

基于所述运动轨迹多项式，以运动速度和加速度为约束条件，以每个关节总运动时间为优化目标，构建出目标函数；

采用预设的个体极值双重寻优的改进粒子群算法对所述目标函数进行优化，在所述目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，求得最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹。

其中，所述机器人相邻关节空间的数学模型是通过齐次坐标变换来实现两个连杆上坐标的变换，且两相邻连杆之间的坐标变换通式(1)表示如下：

其中，θ为关节角变量；d为沿关节连杆i轴线的两个公垂线的距离；a为两个关节轴线沿公垂线的距离，即连杆长度；α是在垂直a的平面内两个轴线的夹角，即连杆扭角。

其中，所述根据所述机器人作业任务要求，采用三次样条插值法在变换后的机器人相邻关节空间的数学模型上进行机器人运动轨迹规划，构建出表示机器人关于时间的运动轨迹多项式的具体步骤包括：

首先，构建样条曲线θ(t)，如公式(2)所示：

其中，θ_i(t)是时间t的三次函数，具体为θ_i(t)＝a_i+b_i(t-t_i)+c_i(t-t_i)²+d_i(t-t_i)³；a_i、b_i、c_i、d_i为多项式的未知系数；

其次，确定已知条件，包括n+1个数据点[t_i,q_i],i＝0,1,2,...n，每一分段函数都是三次多项式，节点处为二阶连续和起始端点速度为零；其中，[t_i,q_i],i＝0,1,2,...n表示机器人时间序列的关节值；

然后，由已知条件和需要满足的约束，可以求任意n+1个点的样条函数的系数，具体求解过程为：

由曲线本身的微分式插值及微分的连续性以及样条可得：

其中，q_i表示机器人关节角度位置；

将时间间隔h_i＝t_i+1-t_i带入样条曲线的条件中，可得多项式系数表达式：

其中，a为每段初始时的机器人关节角度，系数c可由已知点和时间间隔求得，系数b、d可由d表示，所以求出系数c即可求得所有系数；

由式(4)中，i的取值范围可知，共有n-1个公式，还需要另外的约束条件，根据实际情况对起始点的速度加以限制，机器人起点和终点速度为零，结合式(4)中关于系数c的方程，可得到如下表达式：

[M]·[C]＝[Q] (5)；

其中，

[C]＝[c₀ c₁ c₂ c₃ … c_n]^T；

最后，根据最终获得的样条函数的各项系数，求得表示机器人关于时间的运动轨迹多项式。

其中，所述目标函数为f(t)＝min(h_j1+h_j2+......+h_jn-1)，约束条件为

其中，θ_ji表示第j关节第i段多项式函数的值。

其中，所述采用预设的个体极值双重寻优的改进粒子群算法对所述目标函数进行优化，在所述目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，求得最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹的具体步骤包括：

第一步、粒子群参数初始化；包括种群数量M，迭代次数Maxgen，参数ω、c₁、c₂，机器人运动中间节点，粒子时间及其速度的随机解；

第二步、根据上一步得到的随机粒子计算对应每个粒子时间解的轨迹，该轨迹是机器人关节、关节速度和关节加速度的函数；通过机器人速度和加速度约束判断每个粒子对应轨迹的合理性，并根据适应度值判断符合要求的粒子所对应时间的优劣性，种群中所有粒子每一分段轨迹相加的最小值作为第一代群体极值，将群体极值最优的粒子通过G1表示，粒子的个体极值是每个粒子本身的解，通过H1表示；

第三步、进行算法第二次迭代；根据粒子群算法更新每个粒子的时间，再次由机器人速度加速度约束判断粒子时间的合理性，剔除不合理的解；然后，根据目标函数更新粒子群的个体极值H1和群体极值G1，对于得到的每个粒子的个体极值，从算法第二次迭代开始，在判断粒子群时间解的合理性后，除了对每个粒子适应度值对比得到粒子的个体极值矩阵H1之外，还需要通过和上一代粒子中每一维时间值进行对比，得到潜在的更小的个体极值矩阵H2，判断新的矩阵H2对应时间解的合理性；若H2中有粒子满足要求，则将H2中合理的粒子替换掉H1中对应的粒子，并更新二次比对之后新的群体极值G1；否则H1和G1均不变；

第四步、进行下一步迭代，直至迭代次数结束；

第五步、将迭代结束后最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹。

实施本发明实施例，具有如下有益效果：

本发明首先通过三次样条插值法可根据中间节点个数变化自发地机器人运动轨迹规划，具有计算量小、适应性强等优点，还进一步采用个体极值双重寻优的改进粒子群算法，在目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，使得机器人运行时间有明显缩短，速度和加速度也更接近允许的最大值，提高了机器人运动效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，根据这些附图获得其他的附图仍属于本发明的范畴。

图1为本发明实施例提出的一种双重寻优的机器人运动轨迹优化方法的流程图；

图2为本发明实施例提出的一种双重寻优的机器人运动轨迹优化方法中双重寻优粒子群算法轨迹优化的流程图；

图3为本发明实施例提出的一种双重寻优的机器人运动轨迹优化方法应用于机器人分段作业的路径图；

图4为图3中机器人分段作业时使用改进后的粒子群算法对机器人每个阶段运动时间进行优化前后的各关节运动轨迹的对比示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述。

如图1所示，为本发明实施例中，提出的一种双重寻优的机器人运动轨迹优化方法，包括以下步骤：

步骤S1、根据机器人的关节结构及物理连接关系，采用D-H法构建出机器人相邻关节空间的数学模型，并将所述机器人相邻关节空间的数学模型基于基坐标系进行变换；

具体过程为，首先，根据机器人的关节结构及物理连接关系，采用D-H法构建出机器人相邻关节空间的数学模型，机器人相邻关节空间的数学模型是通过齐次坐标变换来实现两个连杆上坐标的变换，且两相邻连杆之间的坐标变换通式(1)表示如下：

其中，θ为关节角变量；d为沿关节连杆i轴线的两个公垂线的距离；a为两个关节轴线沿公垂线的距离，即连杆长度；α是在垂直a的平面内两个轴线的夹角，即连杆扭角。

其次，在关节空间进行轨迹规划对机器人进行运动学分析时，需要研究机器人末端执行器和基坐标系的对应关系，因此要将两相邻连杆之间的坐标变换即式(1)以基坐标系表示。

在一个实施例中，六轴机器人的D-H参数，如下表1所示：

表1

将上述坐标变换通式(1)变换为基坐标系，因此可得机器人末端执行器到基坐标系的齐次变换矩阵为：

步骤S2、根据所述机器人作业任务要求，采用三次样条插值法在变换后的机器人相邻关节空间的数学模型上进行机器人运动轨迹规划，构建出表示机器人关于时间的运动轨迹多项式；其中，所述运动轨迹多项式为机器人在关节空间中各关节角度、运动速度和加速度关于时间的函数；

具体过程为，轨迹规划是根据机器人作业任务要求，确定机器人各关节角度、运动速度和加速度关于时间的函数。对于断路器零件柔性装配任务，机械臂虽然在笛卡尔空间内不需要严格要求运动轨迹，但由于工作台面复杂，不仅需要规定机械臂的起始点，还要指明两点之间的若干中间点，如避障点、离开点、接近点等，需要沿经过这些点的特定路径运动。样条插值是一种常用的平滑曲线插值方法，本文采用三次样条来完成机器人在关节空间中的轨迹规划。

三次样条插值实质上是在机械臂运动过程中每段轨迹运动时间确定的情况下，求解样条函数的系数，再求导得到机器人速度和加速度对时间t变化的函数，从而确定机器人对时间t的运动情况。

首先，构建样条曲线θ(t)，如公式(2)所示：

其中，θ_i(t)是时间t的三次函数，具体为θ_i(t)＝a_i+b_i(t-t_i)+c_i(t-t_i)²+d_i(t-t_i)³；a_i、b_i、c_i、d_i为多项式的未知系数；

其次，确定已知条件，包括n+1个数据点[t_i,q_i],i＝0,1,2,…n，每一分段函数都是三次多项式，节点处为二阶连续和起始端点速度为零；其中，[t_i,q_i],i＝0,1,2,…n表示机器人时间序列的关节值；

然后，由已知条件和需要满足的约束，可以求任意n+1个点的样条函数的系数，具体求解过程为：

由曲线本身的微分式插值及微分的连续性以及样条可得：

其中，q_i表示机器人关节角度位置；

将时间间隔h_i＝t_i+1-t_i带入样条曲线的条件中，可得多项式系数表达式：

其中，a为每段初始时的机器人关节角度，系数c可由已知点和时间间隔求得，系数b、d可由d表示，所以求出系数c即可求得所有系数；

由式(4)中，i的取值范围可知，共有n-1个公式，还需要另外的约束条件，根据实际情况对起始点的速度加以限制，机器人起点和终点速度为零，结合式(4)中关于系数c的方程，可得到如下表达式：

[M]·[C]＝[Q] (5)；

其中，

[C]＝[c₀ c₁ c₂ c₃ … c_n]^T；

最后，根据最终获得的样条函数的各项系数，求得表示机器人关于时间的运动轨迹多项式。

步骤S3、基于所述运动轨迹多项式，以运动速度和加速度为约束条件，以每个关节总运动时间为优化目标，构建出目标函数；

具体过程为，断路器柔性化装配过程中，需要对机器人运动性能进行优化，在保证机器人速度平滑，加速度连续有界的情况下尽可能减小运动时间。通过三次样条插值分析机器人运动轨迹后，需要对轨迹运动时间进行评判，以寻找到机器人从开始运动、经过中间节点、到停止运动的最小时间。

因此，可以根据三次样条轨迹规划方法求得表示机器人关于时间的运动轨迹多项式后，采用粒子群算法对机器人运动时间进行优化，并通过机器人运动速度和加速度的约束条件判断运动轨迹的范围，从而确定粒子找到时间解的合理性，再与目标函数进行对比来判断每代粒子最优解的优劣性。

此时，设定目标函数为f(t)＝min(h_j1+h_j2+......+h_jn-1)，约束条件为

式中，θ_ji表示第j关节第i段多项式函数的值。

步骤S4、采用预设的个体极值双重寻优的改进粒子群算法对所述目标函数进行优化，在所述目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，求得最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹。

具体过程为，使用粒子群算法对机器人轨迹进行优化时，每次迭代均需判断粒子群中个体粒子时间解的合理性。对轨迹时间进行优化时，其目标函数为轨迹不同分段时间的和，通过比对该值来确定解的优劣性。而粒子每一维的数值是随机的，导致得到的适应度数值也是随机的，算法寻找最优解的能力不强。本文提出双重寻优的轨迹规划新方法，在判断适应度函数解的优劣性的基础上，再次判断粒子中每一分段轨迹所对应的时间极值，进行二次寻优，进而提高轨迹规划的可靠性与准确性。

如图2所示，双重寻优粒子群算法轨迹优化的具体步骤如下：

第一步、粒子群参数初始化；包括种群数量M，迭代次数Maxgen，参数ω、c₁、c₂，机器人运动中间节点，粒子时间及其速度的随机解；

第二步、根据上一步得到的随机粒子计算对应每个粒子时间解的轨迹，该轨迹是机器人关节、关节速度和关节加速度的函数；通过机器人速度和加速度约束判断每个粒子对应轨迹的合理性，并根据适应度值判断符合要求的粒子所对应时间的优劣性，种群中所有粒子每一分段轨迹相加的最小值作为第一代群体极值，将群体极值最优的粒子通过G1表示，粒子的个体极值是每个粒子本身的解，通过H1表示；

第三步、进行算法第二次迭代；根据粒子群算法更新每个粒子的时间，再次由机器人速度加速度约束判断粒子时间的合理性，剔除不合理的解；然后，根据目标函数更新粒子群的个体极值H1和群体极值G1，对于得到的每个粒子的个体极值，从算法第二次迭代开始，在判断粒子群时间解的合理性后，除了对每个粒子适应度值对比得到粒子的个体极值矩阵H1之外，还需要通过和上一代粒子中每一维时间值进行对比，得到潜在的更小的个体极值矩阵H2，判断新的矩阵H2对应时间解的合理性；若H2中有粒子满足要求，则将H2中合理的粒子替换掉H1中对应的粒子，并更新二次比对之后新的群体极值G1；否则H1和G1均不变；

第四步、进行下一步迭代，直至迭代次数结束；

第五步、将迭代结束后最终最优解为各关节总运动时间，并代入运动轨迹多项式中得到最优的机器人运动轨迹。

如图3和图4所示，对本发明实施例中的一种双重寻优的机器人运动轨迹优化方法的应用场景做进一步说明：

以断路器柔性化装配作业对算法进行验证，在图3所示的装配作业过程中，机器人需要借助相应的辅助机构完成作业任务，其运动过程包括四个分段：AB段表示机器人从安全点启动到取件坐标点夹取零件；BC段表示机器人夹取零件到变换姿态将零件放到调整机构上；CD段表示机器人松开零件并变换姿态后以另一种姿态夹取调整机构上的零件；DE段表示机器人从调整机构上夹取零件并放置到零件托盘。

机器人作业过程中需要经过相应避障点，以避免机器人运动或位姿变换过程中与周围机构发生碰撞。以断路器磁***的某一姿态为例进行说明，机器人调整过程中需要经过的节点各关节值如下表2所示：

表2

对上述关节和各运动阶段分别进行优化。对于优化后每一阶段每一分段的时间，由于机器人从一个点运动到另一个点时各关节需要同时运动，所以取每一小段优化时间中最大值作为该段各关节的运动时间。其中由于关节4在AB段和DE段关节值完全没有变化，所以不考虑该关节在这两段轨迹时间的优化，此外，对其他关节点没有变化的分段，其两点之间的运动时间采用其他关节优化结果中的最大时间。

为进行对比，本文优化前时间设置为机器人可能的运动时间范围内的随机值。优化前后得到的各分段最大运动时间如表3所示，由该表可以看出，改进前后的算法在每一分段轨迹的所需最大运动时间结果上存在以下差异：改进前的优化结果可能出现各段时间极度不均衡的情况，而改进后的算法基本遵循运动关节所需运动角度越长则时间越长的规则，说明改进后算法得到的优化结果更加合理。此外，相比于优化前所取的随机值，改进后的时间结果都有所减小。同时，将表中机器人每个运动阶段总的时间进行对比，相比随机设置的运动时间，未进行改进的优化算法在AB段、BC段、CD段、DE段分别缩短了11.9％、22％、22.6％、9.9％。优化算法进行双重寻优以后，各分段分别缩短了37.9％、25.6％、39.5％、17.3％。由该结果可以看出，进行双重寻优改进后的算法相比改进前的算法更优成效，分别提高了26％、3.6％、16.9％和7.4％。

表3

图4为使用改进后的粒子群算法对机器人每个阶段运动时间进行优化前后的各关节运动轨迹。由该图可以看出，优化后轨迹较优化前轨迹运行时间有明显缩短，速度和加速度也更接近允许的最大值。

实施本发明实施例，具有如下有益效果：

本发明首先通过三次样条插值法可根据中间节点个数变化自发地机器人运动轨迹规划，具有计算量小、适应性强等优点，还进一步采用个体极值双重寻优的改进粒子群算法，在目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，使得机器人运行时间有明显缩短，速度和加速度也更接近允许的最大值，提高了机器人运动效率。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，所述的程序可以存储于一计算机可读取存储介质中，所述的存储介质，如ROM/RAM、磁盘、光盘等。

以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (5)

1.一种双重寻优的机器人运动轨迹优化方法，其特征在于，包括以下步骤：

根据机器人的关节结构及物理连接关系，采用D-H法构建出机器人相邻关节空间的数学模型，并将所述机器人相邻关节空间的数学模型基于基坐标系进行变换；

根据所述机器人作业任务要求，采用三次样条插值法在变换后的机器人相邻关节空间的数学模型上进行机器人运动轨迹规划，构建出表示机器人关于时间的运动轨迹多项式；其中，所述运动轨迹多项式为机器人在关节空间中各关节角度、运动速度和加速度关于时间的函数；

基于所述运动轨迹多项式，以运动速度和加速度为约束条件，以每个关节总运动时间为优化目标，构建出目标函数；

采用预设的个体极值双重寻优的改进粒子群算法对所述目标函数进行优化，在所述目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，求得最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹。

2.如权利要求1所述的双重寻优的机器人运动轨迹优化方法，其特征在于，所述机器人相邻关节空间的数学模型是通过齐次坐标变换来实现两个连杆上坐标的变换，且两相邻连杆之间的坐标变换通式(1)表示如下：

其中，θ为关节角变量；d为沿关节连杆i轴线的两个公垂线的距离；a为两个关节轴线沿公垂线的距离，即连杆长度；α是在垂直a的平面内两个轴线的夹角，即连杆扭角。

3.如权利要求1所述的双重寻优的机器人运动轨迹优化方法，其特征在于，所述根据所述机器人作业任务要求，采用三次样条插值法在变换后的机器人相邻关节空间的数学模型上进行机器人运动轨迹规划，构建出表示机器人关于时间的运动轨迹多项式的具体步骤包括：

首先，构建样条曲线θ(t)，如公式(2)所示：

其中，θ_i(t)是时间t的三次函数，具体为θ_i(t)＝a_i+b_i(t-t_i)+c_i(t-t_i)²+d_i(t-t_i)³；a_i、b_i、c_i、d_i为多项式的未知系数；

其次，确定已知条件，包括n+1个数据点[t_i,q_i],i＝0,1,2,…n，每一分段函数都是三次多项式，节点处为二阶连续和起始端点速度为零；其中，[t_i,q_i],i＝0,1,2,…n表示机器人时间序列的关节值；

然后，由已知条件和需要满足的约束，可以求任意n+1个点的样条函数的系数，具体求解过程为：

由曲线本身的微分式插值及微分的连续性以及样条可得：

其中，q_i表示机器人关节角度位置；

将时间间隔h_i＝t_i+1-t_i带入样条曲线的条件中，可得多项式系数表达式：

其中，a为每段初始时的机器人关节角度，系数c可由已知点和时间间隔求得，系数b、d可由d表示，所以求出系数c即可求得所有系数；

由式(4)中，i的取值范围可知，共有n-1个公式，还需要另外的约束条件，根据实际情况对起始点的速度加以限制，机器人起点和终点速度为零，结合式(4)中关于系数c的方程，可得到如下表达式：

[M]·[C]＝[Q] (5)；

其中，

[C]＝[c₀ c₁ c₂ c₃…c_n]^T；

最后，根据最终获得的样条函数的各项系数，求得表示机器人关于时间的运动轨迹多项式。

4.如权利要求1所述的双重寻优的机器人运动轨迹优化方法，其特征在于，所述目标函数为f(t)＝min(h_j1+h_j2+......+h_jn-1)，约束条件为

其中，θ_ji表示第j关节第i段多项式函数的值。

5.如权利要求1所述的双重寻优的机器人运动轨迹优化方法，其特征在于，所述采用预设的个体极值双重寻优的改进粒子群算法对所述目标函数进行优化，在所述目标函数评判得到的优化粒子集合中，分别比对机器人每一分段轨迹对应的分段时间并进行二次寻优，求得最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹的具体步骤包括：

第一步、粒子群参数初始化；包括种群数量M，迭代次数Maxgen，参数ω、c₁、c₂，机器人运动中间节点，粒子时间及其速度的随机解；

第二步、根据上一步得到的随机粒子计算对应每个粒子时间解的轨迹，该轨迹是机器人关节、关节速度和关节加速度的函数；通过机器人速度和加速度约束判断每个粒子对应轨迹的合理性，并根据适应度值判断符合要求的粒子所对应时间的优劣性，种群中所有粒子每一分段轨迹相加的最小值作为第一代群体极值，将群体极值最优的粒子通过G1表示，粒子的个体极值是每个粒子本身的解，通过H1表示；

第三步、进行算法第二次迭代；根据粒子群算法更新每个粒子的时间，再次由机器人速度加速度约束判断粒子时间的合理性，剔除不合理的解；然后，根据目标函数更新粒子群的个体极值H1和群体极值G1，对于得到的每个粒子的个体极值，从算法第二次迭代开始，在判断粒子群时间解的合理性后，除了对每个粒子适应度值对比得到粒子的个体极值矩阵H1之外，还需要通过和上一代粒子中每一维时间值进行对比，得到潜在的更小的个体极值矩阵H2，判断新的矩阵H2对应时间解的合理性；若H2中有粒子满足要求，则将H2中合理的粒子替换掉H1中对应的粒子，并更新二次比对之后新的群体极值G1；否则H1和G1均不变；

第四步、进行下一步迭代，直至迭代次数结束；

第五步、将迭代结束后最终最优解为各关节总运动时间，并代入所述运动轨迹多项式中得到最优的机器人运动轨迹。

Images