CN113334382B - 机械臂最优时间轨迹规划方法 - Google Patents

Abstract

本发明提供了一种机械臂最优时间轨迹规划方法，该方法在考虑了机械臂速度，加速度，跃度值等多约束条件情才况下，对粒子群的中的惯性权重和学习因子进行了改进，解决了粒子群易收敛，从而得到局部最优解的缺陷。同时，采用3‑5‑3多项式插值轨迹方法，使算法结构简单，参数易调整，避免了多项式阶次高、没有凸包性质等缺点。本发明对粒子群算法进行改进，使得机械臂运行时间有所缩短，得到平滑、稳定的关节位置、速度和加速度曲线。

Description

机械臂最优时间轨迹规划方法

技术领域

本发明涉及一种机械臂最优时间轨迹规划方法。

背景技术

如今，机械臂已广泛用于工业装配线和制造系统。在工业机器人应用中，轨迹规划问题一直是个热点，其中，时间最优性对于生产效率的最大化起着至关重要的作用。此外，机械臂在运动过程中还受到多重动力学约束的影响。因此，在多约束环境下，减少机械臂的工作时间是解决该问题的核心。

轨迹规划通常在工作空间和关节空间中进行。在工作空间中，末端执行器的轨迹可以自然地描述。然而，这样的轨迹规划将容易导致运动学奇异性和操纵器冗余。而面向关节的空间方法的确可以保证每次关节运动的平稳性，但会降低操作空间中的位置精度。基于此，在大多数情况下，轨迹规划是在关节空间中进行的。

在关节空间中，轨迹主要由多项插值函数组成，例如多项式函数，样条函数，Bezier曲线和NURUBS曲线等。如CN 110948488 A的中国发明专利名称为“一种基于时间最优的机器人自适应轨迹规划算法”中就采用的是梯形轨迹规划。然而，梯形规划在规划过程中只能保证速度曲线是形如梯形的曲线，并不能在多约束环境下使用。其主要的原因在于，梯形规划的加速度在中间稳定速度时为零。3-5-3多项式插值方法是多项式插值函数中应用比较成熟并且高效的一种方法，它能够避免传统多项式插值阶次高，没有凸包性质等缺点。

为了提高制造生产率和运动稳定性，规划就需要缩短工作时间和平滑轨迹。在过去的几十年中，已经研究了许多时间最优技术，比如，遗传算法，蚁群算法，粒子群算法等。这些算法中，粒子群算法容易实现，并且在多约束问题上较有优势，它还具有快速收敛到最佳状态的能力。如李小为,胡立坤,王琥等速度约束下PSO的六自由度机械臂时间最优轨迹规划.智能系统学报.2015,10(03):393-398.中将标准粒子群算法应用到六自由度的机械臂中，并且只考虑了速度约束。然而，标准粒子群存在早收敛和局部振荡等问题，容易得到局部最优值。当粒子的个体极值和全局最优附近时，粒子将快速移动到那个方向，并且不可能跳出该局部最优的范围。

因此，需要一种改进的粒子群算法在考虑多约束情况下解决粒子的方向，容易陷入局部振荡，从而得到局部最优结果的问题。

发明内容

本发明的目的在于提供一种机械臂最优时间轨迹规划方法。

为解决上述问题，本发明提供一种机械臂最优时间轨迹规划方法，包括：

S1：获取轨迹规划参数，所述轨迹规划参数包括：关节的起始点θ_i0，路径点θ_i1和θ_i2，末端点θ_i3，机器人关节数即自由度数，初始点和终点的速度和加速度；

S2：基于所述轨迹规划参数，建立第i个关节3-5-3次多项式的轨迹规划插值函数，以矩阵的形式表示，并综合考虑最优时间和多重约束的影响，定义适应度函数；

S3：采用改进的粒子群算法，求出约束条件下，通过改进惯性权重值，自适应调整自我认识和社会认知学习因子，提高粒子学习效率；

S4：寻找符合所述适应度函数的最小的点，判断是否陷入了局部最优状态；

S5：若没有陷入局部最优，且达到最大跌代次数，则为最终目标点；

S6：若陷入局部最优，则增加惯性权重，增加额外的扰动，使粒子能够脱离局部最优时间机械臂关节的位置和速度，之后返回S3。

进一步的，在上述方法中，所述S2中的第i个关节3-5-3次多项式的通式为：

其中，h_i1(t)、h_i2(t)、h_i3(t)分别代表3段多项式的轨迹，a_i1j、a_i2j、a_i3j分别为第i个关节第j个系数的各自段数的系数。

进一步的，在上述方法中，所述S2中，根据6个连续性条件和所述S1的8 个轨迹参数，将第i个关节3-5-3次多项式转化成矩阵为：

b＝[0 0 0 0 0 0 θ_i3 0 0 θ_i0 0 0 θ_i2 θ_i1]^T，

a＝A^-1·b＝[a_i13 a_i12 a_i11 a_i10 a_i25 a_i24 a_i23 a_i22 a_i21 a_i20 a_i33 a_i32 a_i31 a_i30]^T，

由此，求出3-5-3次多项式前面的系数。

进一步的，在上述方法中，所述S2中，综合考虑最优时间和多重约束的影响，定义适应度函数为：

其中，fitness(t)_s是关节s的适应度函数；j为时间维度函数的序数， j＝1,2,3,..,D；D为总时间维度；t为时间，t_sj为关节s的第j维度的时间，

分别为关节s在t时间的速度、加速度和跃度值，v_smax,a_smax,j_smax分别是关节s的最大速度、最大加速度和最大跃度值。

进一步的，在上述方法中，所述S3中，采用改进的粒子群算法，改进点如下：

标准粒子群的粒子更新公式为：

其中，i为粒子序数，k为当前迭代步长，v_ij为速度值，x_ij为位置值，p_ij为个体最优值，g_j为全局最优值，r₁和r₂为0到1之间的随机数；

改进的惯性权重，通过下式更新：

其中，ω_max、ω_min为惯性权重ω的边界值；rank_i根据粒子的最佳适应度对粒子群进行排序时第i个粒子的位置，n_p是粒子群的种群大小；

改进的自适应自我学习因子和社会学习因子，根据下式更新：

其中，a,b为常数,k是指当前第k次迭代，T是最大迭代次数。

进一步的，在上述方法中，所述S4中,局部最优判断条件为：个体最优值p_ij在n次之内是否发生改变。

进一步的，在上述方法中，所述S6中,增加的惯性权重的公式为：

其中，c是常数,k是指当前第k次迭代，T是最大迭代次数。

与现有技术相比，本发明通过建模和推导，将轨迹规划问题转变为改进粒子群算法求解全局最优问题。通过对惯性权重和学习因子进行改进，解决了标准粒子群过早收敛和振荡问题，加快了收敛速度。本发明所述的算法可以在考虑机械臂速度，加速度，跃度值等多重约束下快速搜寻到一条光滑且稳定的轨迹，提高机械臂的工作效率。

附图说明

图1a为四轴机械臂初始轨迹参数图；

图1b为四轴机械臂运动学约束图；

图2为基于改进的粒子群算法的轨迹规划工作流程图；

图3为改进粒子群算法与标准粒子群算法收敛对比图；

图4a为粒子群优化四轴机械臂的关节角度曲线图；

图4b为粒子群优化四轴机械臂的关节速度曲线图；

图4c为粒子群优化四轴机械臂关节加速度曲线图；

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1a为四轴机械臂初始轨迹参数图；图1b为四轴机械臂运动学约束图如图2所示，本发明提供一种机械臂最优时间轨迹规划方法，包括：

S1：获取轨迹规划参数，所述轨迹规划参数包括：关节的起始点θ_i0，路径点θ_i1和θ_i2，末端点θ_i3，机器人关节数即自由度数，初始点和终点的速度和加速度；

S2：基于所述轨迹规划参数，建立第i个关节3-5-3次多项式的轨迹规划插值函数，以矩阵的形式表示，并综合考虑最优时间和多重约束的影响，定义适应度函数；

S3：采用改进的粒子群算法，求出约束条件下，通过改进惯性权重值，自适应调整自我认识和社会认知学习因子，提高粒子学习效率；

S4：寻找符合所述适应度函数的最小的点，判断是否陷入了局部最优状态；

S5：若没有陷入局部最优，且达到最大跌代次数，则为最终目标点；

S6：若陷入局部最优，则增加惯性权重，增加额外的扰动，使粒子能够脱离局部最优时间机械臂关节的位置和速度，之后返回S3。

在此，本发明提出一种多约束条件下基于改进粒子群算法机械臂最优时间轨迹规划，采用通过对粒子群算法进行改进，通过改进惯性权重值，按照优劣程度对这代粒子进行学习。通过改进学习因子，自适应调整自我认识和社会认知，提高学习效率。当粒子进入局部最小值时，通过对惯性权重进行改进，加大权重，增加额外的扰动，使粒子能够脱离局部最优。可以解决现有规划算法中只适用于单约束，收敛慢，容易得到局部最优解的问题。

本发明的机械臂最优时间轨迹规划方法一实施例中，所述S2中的第i个关节 3-5-3次多项式的通式为：

其中，h_i1(t)、h_i2(t)、h_i3(t)分别代表3段多项式的轨迹，a_i1j、a_i2j、a_i3j分别为第i个关节第j个系数的各自段数的系数。

本发明的机械臂最优时间轨迹规划方法一实施例中，所述S2中，根据6个连续性条件和所述S1的8个轨迹参数，将第i个关节3-5-3次多项式转化成矩阵为：

b＝[0 0 0 0 0 0 θ_i3 0 0 θ_i0 0 0 θ_i2 θ_i1]^T，

a＝A^-1·b＝[a_i13 a_i12 a_i11 a_i10 a_i25 a_i24 a_i23 a_i22 a_i21 a_i20 a_i33 a_i32 a_i31 a_i30]^T，

由此，可以求出3-5-3次多项式前面的系数。

本发明的机械臂最优时间轨迹规划方法一实施例中，所述S2中，综合考虑最优时间和多重约束的影响，定义适应度函数为：

其中，fitness(t)_s是关节s的适应度函数；j为时间维度函数的序数， j＝1,2,3,..,D；D为总时间维度；t为时间，t_sj为关节s的第j维度的时间，

分别为关节s在t时间的速度、加速度和跃度值，v_smax,a_smax,j_smax分别是关节s的最大速度、最大加速度和最大跃度值。

本发明的机械臂最优时间轨迹规划方法一实施例中，所述S3中，采用改进的粒子群算法，改进点如下：

标准粒子群的粒子更新公式为：

其中，i为粒子序数，k为当前迭代步长，v_ij为速度值，x_ij为位置值，p_ij为个体最优值，g_j为全局最优值，r₁和r₂为0到1之间的随机数；

改进的惯性权重，通过下式更新：

其中，ω_max、ω_min为惯性权重ω的边界值；rank_i根据粒子的最佳适应度对粒子群进行排序时第i个粒子的位置，n_p是粒子群的种群大小；

改进的自适应自我学习因子和社会学习因子，根据下式更新：

其中，a,b为常数,k是指当前第k次迭代，T是最大迭代次数。

本发明的机械臂最优时间轨迹规划方法一实施例中，所述S4中,局部最优判断条件为：个体最优值p_ij在n次之内是否发生改变。

本发明的机械臂最优时间轨迹规划方法一实施例中，所述S6中,增加的惯性权重的公式为：

其中，c是常数,k是指当前第k次迭代，T是最大迭代次数。

具体的，本发明提供了一种基于改进粒子群算法的最优时间轨迹规划，该方法在考虑了机械臂速度，加速度，跃度值等多约束条件情况下，对粒子群的中的惯性权重和学习因子进行了改进，解决了粒子群易收敛，从而得到局部最优解的缺陷。同时，采用3-5-3多项式插值轨迹方法，使算法结构简单，参数易调整，避免了多项式阶次高、没有凸包性质等缺点。本发明对粒子群算法进行改进，使得机械臂运行时间有所缩短，得到平滑、稳定的关节位置、速度和加速度曲线。

S1：本发明以四轴机械臂为例进行说明，轨迹规划参数如图1a所示，包括关节的起始点θ_i0，路径点θ_i1和θ_i2，末端点θ_i3，轨迹的初始点和终点的速度和加速度都为0。

S2：其主要思想是利用S1的初始条件构造矩阵关系函数，并考虑时间和约束的影响，最终确定适应度函数。

先构造轨迹规划插值函数，在四个关节中，第i个关节3-5-3次多项式的通式为：

其中，h_i1(t)、h_i2(t)、h_i3(t)分别代表3段多项式的轨迹，a_i1j、a_i2j、a_i3j分别为第i个关节第j个系数的各自段数的系数。

然后，将上面多项式通式转化成关系矩阵，根据S1的8个轨迹参数和6个连续性条件，可将3-5-3次多项式转化成矩阵为：

b＝[0 0 0 0 0 0 θ_i3 0 0 θ_i0 0 0 θ_i2 θ_i1]^T，

a＝A^-1·b＝[a_i13 a_i12 a_i11 a_i10 a_i25 a_i24 a_i23 a_i22 a_i21 a_i20 a_i33 a_i32 a_i31 a_i30]^T，

由此，可以求出3-5-3次多项式前面的系数。

最后，考虑时间最优和速度，加速度，跃度值等多约束的适应度函数为：

其中，fitness(t)_s是关节s的适应度函数；j为时间维度函数的序数， j＝1,2,3,...,D；D为总时间维度；t为时间，t_sj为关节s的第j维度的时间，

分别为关节s在t时间的速度、加速度和跃度值，v_smax,a_smax,j_smax分别是关节s的最大速度，最大加速度，最大跃度值。

S3：采用改进的粒子群算法，流程图如图2所示，改进点如下：

标准粒子群的粒子更新公式为：

ν_ij(k+1)＝ων_ij(k)+c₁r₁(p_ij(k)-x_ij(k))+c₂r₂(g_j(k)-x_ij(k))

x_ij(k+1)＝x_ij(k)+ν_ij(k+1)

其中，i为粒子序数，k为当前迭代步长，v_ij为速度值，x_ij为位置值，p_ij为个体最优值，g_j为全局最优值，r₁和r₂为0到1之间的随机数。

改进的惯性权重，通过下式更新：

其中，ω_max、ω_min为惯性权重ω的边界值；rank_i根据粒子的最佳适应度对粒子群进行排序时第i个粒子的位置，n_p是粒子群的种群大小。

改进的自适应自我学习因子和社会学习因子，根据下式更新：

其中，a,b为常数，k是指当前第k次迭代，T是最大迭代次数。

S4：局部最优判断条件为：个体最优值p_ij在n次之内是否发生改变，这里的n取值为7。

S5：若没有陷入局部最优，且达到最大跌代次数，则为最终目标点。

S6：若陷入局部最优，则增加惯性权重，之后返回S3，直到迭代到最大迭代次数为止，输出结果。改进粒子群与标准粒子群收敛结果比较图如图3所示，最后的各关节角度、速度、加速度曲线如图4a～4b所示。

增加惯性权重的公式为：

其中，c是常数，k是指当前第k次迭代，T是最大迭代次数。

与现有技术相比，本发明的有益效果：本发明通过建模和推导，将轨迹规划问题转变为改进粒子群算法求解全局最优问题。通过对惯性权重和学习因子进行改进，解决了标准粒子群过早收敛和振荡问题，加快了收敛速度。本发明所述的算法可以在考虑机械臂速度，加速度，跃度值等多重约束下快速搜寻到一条光滑且稳定的轨迹，提高机械臂的工作效率。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

显然，本领域的技术人员可以对发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包括这些改动和变型在内。

Claims (5)

1.一种机械臂最优时间轨迹规划方法，其特征在于，包括：

S1：获取轨迹规划参数，所述轨迹规划参数包括：关节的起始点θ_i0，路径点θ_i1和θ_i2，末端点θ_i3，机器人关节数即自由度数，初始点和终点的速度和加速度；

S2：基于所述轨迹规划参数，建立第i个关节3-5-3次多项式的轨迹规划插值函数，以矩阵的形式表示，并综合考虑最优时间和多重约束的影响，定义适应度函数；

S3：采用改进的粒子群算法，求出约束条件下，通过改进惯性权重值，自适应调整自我认识和社会认知学习因子，提高粒子学习效率；

S4：寻找符合所述适应度函数的最小的点，判断是否陷入了局部最优状态；

S5：若没有陷入局部最优，且达到最大跌代次数，则为最终目标点；

S6：若陷入局部最优，则增加惯性权重，增加额外的扰动，使粒子能够脱离局部最优时间机械臂关节的位置和速度，之后返回S3；

所述S3中，采用改进的粒子群算法，改进点如下：

标准粒子群的粒子更新公式为：

ν_ij(k+1)＝ων_ij(k)+c₁r₁(p_ij(k)-x_ij(k))+c₂r₂(g_j(k)-x_ij(k))，

x_ij(k+1)＝x_ij(k)+ν_ij(k+1)

其中，i为粒子序数，k为当前迭代步长，v_ij为速度值，x_ij为位置值，p_ij为个体最优值，g_j为全局最优值，r₁和r₂为0到1之间的随机数；

改进的惯性权重，通过下式更新：

其中，ω_max、ω_min为惯性权重ω的边界值；rank_i根据粒子的最佳适应度对粒子群进行排序时第i个粒子的位置，n_p是粒子群的种群大小；

改进的自适应自我学习因子和社会学习因子，根据下式更新：

其中，a,b为常数,k是指当前第k次迭代，T是最大迭代次数；

所述S6中,增加的惯性权重的公式为：

其中，c是常数,k是指当前第k次迭代，T是最大迭代次数。

2.如权利要求1所述的机械臂最优时间轨迹规划方法，其特征在于，所述S2中的第i个关节3-5-3次多项式的通式为：

h_i1(t)＝a_i13t₁ ³+a_i12t₁ ²+a_i11t₁+a_i10

h_i2(t)＝a_i25t₂ ⁵+a_i24t₂ ⁴+a_i23t₂ ³+a_i22t₂ ²+a_i21t₂+a_i20，

h_i3(t)＝a_i33t₃ ³+a_i32t₃ ²+a_i31t₃+a_i30

其中，h_i1(t)、h_i2(t)、h_i3(t)分别代表3段多项式的轨迹，a_i1j、a_i2j、a_i3j分别为第i个关节第j个系数的各自段数的系数。

3.如权利要求1所述的机械臂最优时间轨迹规划方法，其特征在于，所述S2中，根据6个连续性条件和所述S1的8个轨迹参数，将第i个关节3-5-3次多项式转化成矩阵为：

b＝[0 0 0 0 0 0 θ_i3 0 0 θ_i0 0 0 θ_i2 θ_i1]^T，

a＝A^-1·b＝[a_i13 a_i12 a_i11 a_i10 a_i25 a_i24 a_i23 a_i22 a_i21 a_i20 a_i33 a_i32 a_i31 a_i30]^T，

由此，求出3-5-3次多项式前面的系数。

4.如权利要求1所述的机械臂最优时间轨迹规划方法，其特征在于，所述S2中，综合考虑最优时间和多重约束的影响，定义适应度函数为：

其中，fitness(t)_s是关节s的适应度函数；j为时间维度函数的序数，j＝1,2,3,..,D；D为总时间维度；t为时间，t_sj为关节s的第j维度的时间，

分别为关节s在t时间的速度、加速度和跃度值，v_smax,a_smax,j_smax分别是关节s的最大速度、最大加速度和最大跃度值。

5.如权利要求1所述的机械臂最优时间轨迹规划方法，其特征在于，所述S4中,局部最优判断条件为：个体最优值p_ij在n次之内是否发生改变。

Images