CN109877838B - 基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法 - Google Patents

Abstract

本发明涉及基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法，属于多关节机械臂关节空间轨迹规划领域，以时间最优为性能指标，以速度最为约束条件，使用3‑5‑3多项式对机械臂进行轨迹规划，使机械臂能够在速度的约束下尽快的从起点运行到终点，提高整个系统的实时性。本发明使用智能优化算法求解各段轨迹所需的最优时间，首先使用速度作为约束条件，使用3段多项式的时间总和作为目标函数，并利用布谷鸟搜索算法在速度的约束下求解各段轨迹所需的最优时间，最后使用最优时间对机械臂进行轨迹规划。确保各个关节的速度满足速度的约束，加速度连续不突变，能够使得机械臂平稳的从起点运动到终点。

Description

基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法

技术领域

本发明属于多关节机械臂关节空间轨迹规划领域，涉及基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法。

背景技术

轨迹规划有利于延长电机等零件的寿命，避免产生振动，使得机械臂运行的更加平稳，提高系统的精度。机械臂的轨迹规划可分为关节空间中的轨迹规划和笛卡尔空间中的轨迹规划，关节空间中的轨迹规划方法通常有三次多项式插值、五次多项式插值等，笛卡尔空间中的轨迹规划方法通常有直线轨迹规划、圆弧轨迹规划等。

为了对机械臂进行实时控制，通常在对机械臂进行轨迹规划时选择计算简单的插值函数，而多项式插值函数便是在轨迹规划中常用的插值函数。为了保证各个关节的位移、速度以及加速度的连续，可使用分段式的多项式插值函数。常用的分段式多项式插值函数有4-3-4多项式、3-5-3多项式以及3-3-3-3-3多项式。4-3-4多项式和3-5-3多项式比3-3-3-3-3多项式计算简单，而3-5-3多项式比4-3-4多项式要更稳定。

使用3-5-3多项式进行轨迹规划时，需要手动的确定每段轨迹的运行时间后再进行插值。而当对某段轨迹分配的时间过短时，则会使关节速度过大，而机械臂的各个关节都有一个最大速度的限制，若超过最大速度，则会影响整个系统的精度以及电机的寿命；当对某段轨迹分配的时间过长时，则会影响整个系统的实时性。

发明内容

有鉴于此，本发明的目的在于提供一种基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法，使用智能优化算法来优化分段多项式插值问题，求解出每段多项式所需的最优时间，解决手动确定每段轨迹的运行时间时，关机速度过快或过慢，影响电机寿命以及整个系统实时性的问题。

为达到上述目的，本发明提供如下技术方案：

一种基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法，包括以下步骤：

S1：选择需要轨迹规划的关节j，确定关节j的起点、终点以及两个中间点；

S2：确定鸟窝和鸟蛋的个数，并对各个鸟窝中的鸟蛋进行初始化。

S3：对每一个鸟窝计算出3-5-3多项式的各个系数，然后求解每一段轨迹的最大速度，若其中有一段轨迹的最大速度大于关节的最大速度V_max，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度值为每一段轨迹运行到终点对应时刻的和；

S4：根据莱维飞行原则，产生新的鸟窝，计算各个鸟窝所对应的3-5-3多项式的系数并计算每段轨迹的最大速度，若不满足速度约束，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度值为每一段轨迹运行到终点对应时刻的和，将新的鸟窝与旧的鸟窝进行对比，保留表现较好的鸟窝；

S5：按照被发现的概率产生新的鸟窝，计算各个鸟窝所对应的3-5-3多项式的系数并计算每段轨迹的最大速度，若不满足速度约束，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度置为每一段轨迹运行到终点对应时刻的和，将新的鸟窝与旧的鸟窝进行对比，保留表现较好的鸟窝；

S6：重复S4和S5，当迭代次数到达最大迭代次数时退出循环，当前全局最优的鸟窝则为该关节的最优时间；

S7：重复S1到S6，直到求解出所有关节的最优时间后退出循环；

S8：对3-5-3多项式的每一段轨迹，在所有关节中选择最长的时间作为该段轨迹的最优时间；

S9：使用最优时间求解出对应的3-5-3多项式的各个系数，确定各段轨迹。

进一步，步骤S2中，鸟蛋的个数为3分别对应t₁、t₂和t₃，t₁、t₂和t₃表示每一段轨迹运行到终点对应的时刻。

进一步，步骤S3中，所述对每一个鸟窝计算出3-5-3多项式的各个系数，包括：

使用3-5-3多项式进行插值通常需要四个初始点，分别为起点、终点以及两个中间点， 3-5-3多项式的构造如下：

其中h_ji代表第j个关节的第i段轨迹。假设起点、中间点1、中间点2以及终点分别用x_j0、x_j1、x_j2以及x_j3表示，每一段轨迹运行到终点对应的时刻分别为t₁、t₂和t₃，则根据位移的约束得到：

a_j10＝x_j0

a_j20＝x_j1

a_j30＝x_j2

根据速度的连续性的约束得到：

a_j11＝0

a_j31＝0

根据加速度的连续性约束得到：

2a_j12＝0

6a_j13t₁+2a_j12＝2a_j22

a_j32＝0

将上述约束条件写成矩阵形式得到：

Aa＝b

其中：

a＝[a_j13 a_j12 a_j11 a_j10 a_j25 a_j24 a_j23 a_j22 a_j21 a_j20 a_j33 a_j32 a_j31 a_j30]

b＝[0 0 0 0 0 0 x_j3 0 0 x_j0 0 0 x_j2 x_j1]^T

确定每一段轨迹的运行时间后，则A和b是已知的，3-5-3多项式的系数由如下公式求得：

a＝inv(A)*b

其中inv()表示求矩阵的逆运算，若A不可逆，则求A的伪逆。

进一步，各段轨迹的运行时间即鸟蛋的值应当在(t_low,t_up)这样一个范围内产生，其中t_up表示各段轨迹最多需要的时间，t_low表示各段轨迹最少需要的时间，为了减少算法寻优的时间，使用如下公式产生t_low：

其中s表示该段轨迹的起点到终点的距离，V_max表示该关节的最大速度，则此时t_low实际为该关节以V_max的速度匀速从起点运行到终点的时间，因此最优时间一定是大于该时间的，这样做可以减少搜索空间，提高搜索速度。

本发明的有益效果在于：本发明使用布谷鸟搜索算法来求解最优时间，使得机械臂能够尽可能快的从起点运动到终点，提高了系统的实时性，同时使用最优时间求解的轨迹满足速度的约束，加速度连续没有发生突变，有利于机械臂的平稳运行。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为本发明所述基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明以时间最优为性能指标，以速度最为约束条件，使用3-5-3多项式对机械臂进行轨迹规划，使得机械臂能够在速度的约束下尽可能快的从起点运行到终点，提高整个系统的实时性。针对传统方法难以优化分段多项式插值方法的问题，本发明使用智能优化算法求解各段轨迹所需的最优时间，首先使用速度作为约束条件，使用3段多项式的时间总和作为目标函数，并利用布谷鸟搜索算法在速度的约束下求解各段轨迹所需的最优时间，最后使用最优时间对机械臂进行轨迹规划。确保各个关节的速度满足速度的约束，加速度连续不突变，能够使得机械臂平稳的从起点运动到终点。

使用3-5-3多项式进行插值通常需要四个初始点，分别为起点、终点以及两个中间点，3- 5-3多项式的构造如下：

其中h_ji代表第j个关节的第i段轨迹。假设起点、中间点1、中间点2以及终点分别用x_j0、x_j1、x_j2以及x_j3表示，每一段轨迹运行到终点对应的时刻分别为t₁、t₂和t₃，则根据位移的约束可以得到：

a_j10＝x_j0 (2)

a_j20＝x_j1 (4)

a_j30＝x_j2 (6)

根据速度的连续性的约束可以得到：

a_j11＝0 (8)

a_j31＝0 (11)

根据加速度的连续性约束可以得到：

2a_j12＝0 (12)

6a_j13t₁+2a_j12＝2a_j22 (13)

a_j32＝0 (15)

将上述约束条件写成矩阵形式可以得到：

Aa＝b (16)

其中：

a＝[a_j13 a_j12 a_j11 a_j10 a_j25 a_j24 a_j23 a_j22 a_j21 a_j20 a_j33 a_j32 a_j31 a_j30] (18)

b＝[0 0 0 0 0 0 x_j3 0 0 x_j0 0 0 x_j2 x_j1]^T (19)

确定每一段轨迹的运行时间后，则A和b是已知的，3-5-3多项式的系数可由如下公式求得：

a＝inv(A)*b (20)

其中inv()表示求矩阵的逆运算，在实际情况下A可能存在不可逆的情况，此时可求A的伪逆。

对于3-5-3多项式来说，最优时间应当是三段多项式的时间总和最优，假设三段多项式所用时间分别为t₁、t₂和t₃，因此该问题的目标函数为：

f＝min(t₁+t₂+t₃) (21)

在求解最优时间也应当考虑机械臂各个关节速度的约束，三段轨迹的速度分别用V₁、V₂和V₃表示，关节的最大速度用V_max表示，则上述目标函数的约束条件为：

max{|V₁|}＜V_max (22)

max{|V₂|}＜V_max (23)

max{|V₃|}＜V_max (24)

对于上诉问题使用传统方法很难处理，而智能优化算法却能很好的对最优时间进行求解，如图1所示，本发明使用基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法来求解上述问题，求解步骤如下：

Step1：选择需要轨迹规划的关节j，确定关节j的起点、终点以及两个中间点。

Step2：确定鸟窝和鸟蛋的个数，本文鸟蛋的个数为3分别对应t₁、t₂和t₃，并对各个鸟窝中的鸟蛋进行初始化。

Step3：使用公式(20)对每一个鸟窝计算出3-5-3多项式的各个系数，然后求解每一段轨迹的最大速度，若其中有一段轨迹的最大速度大于V_max，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度值为t₁、t₂和t₃的和。

Step4：根据莱维飞行原则，产生新的鸟窝，计算各个鸟窝所对应的3-5-3多项式的系数并计算每段轨迹的最大速度，若不满足速度约束，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度值为t₁、t₂和t₃的和。将新的鸟窝与旧的鸟窝进行对比，保留表现较好的鸟窝。

Step5：按照被发现的概率产生新的鸟窝，计算各个鸟窝所对应的3-5-3多项式的系数并计算每段轨迹的最大速度，若不满足速度约束，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度置为t₁、t₂和t₃的和。将新的鸟窝与旧的鸟窝进行对比，保留表现较好的鸟窝。

Step6：重复Step4和Step5，当迭代次数到达最大迭代次数时退出循环，当前全局最优的鸟窝则为该关节的最优时间。

Step7：重复Step1到Step6，直到求解出所有关节的最优时间后退出循环。

Step8：对3-5-3多项式的每一段轨迹，在所有关节中选择最长的时间作为该段轨迹的最优时间。

Step9：使用最优时间求解出对应的3-5-3多项式的各个系数，确定各段轨迹。

在实际操作中，各段轨迹的运行时间即鸟蛋的值应当在(t_low,t_up)这样一个范围内产生，其中t_up表示各段轨迹最多需要的时间，可根据实际要求选取，t_low表示各段轨迹最少需要的时间，通常取零。为了减少算法寻优的时间，本发明使用如下公式产生t_low：

其中s表示该段轨迹的起点到终点的距离，V_max表示该关节的最大速度，则此时t_low实际为该关节以V_max的速度匀速从起点运行到终点的时间，因此最优时间一定是大于该时间的，这样做可以减少搜索空间，提高搜索速度。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法，其特征在于：包括以下步骤：

S1：选择需要轨迹规划的关节j，确定关节j的起点、终点以及两个中间点；

S2：确定鸟窝和鸟蛋的个数，并对各个鸟窝中的鸟蛋进行初始化；

S3：对每一个鸟窝计算出3-5-3多项式的各个系数，然后求解每一段轨迹的最大速度，若其中有一段轨迹的最大速度大于关节的最大速度V_max，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度值为每一段轨迹运行到终点对应时刻的和；所述对每一个鸟窝计算出3-5-3多项式的各个系数，包括：

使用3-5-3多项式进行插值通常需要四个初始点，分别为起点、终点以及两个中间点，3-5-3多项式的构造如下：

其中h_ji代表第j个关节的第i段轨迹，假设起点、中间点1、中间点2以及终点分别用x_j0、x_j1、x_j2以及x_j3表示，每一段轨迹运行到终点对应的时刻分别为t₁、t₂和t₃，则根据位移的约束得到：

a_j10＝x_j0

a_j20＝x_j1

a_j30＝x_j2

根据速度的连续性的约束得到：

a_j11＝0

a_j31＝0

根据加速度的连续性约束得到：

2a_j12＝0

6a_j13t₁+2a_j12＝2a_j22

a_j32＝0

将上述约束条件写成矩阵形式得到：

Aa＝b

其中：

a＝[a_j13 a_j12 a_j11 a_j10 a_j25 a_j24 a_j23 a_j22 a_j21 a_j20 a_j33 a_j32 a_j31 a_j30]

b＝[0 0 0 0 0 0 x_j3 0 0 x_j0 0 0 x_j2 x_j1]^T

确定每一段轨迹的运行时间后，则A和b是已知的，3-5-3多项式的系数由如下公式求得：

a＝inv(A)*b

其中inv()表示求矩阵的逆运算，若A不可逆，则求A的伪逆；

S4：根据莱维飞行原则，产生新的鸟窝，计算各个鸟窝所对应的3-5-3多项式的系数并计算每段轨迹的最大速度，若不满足速度约束，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度值为每一段轨迹运行到终点对应时刻的和，将新的鸟窝与旧的鸟窝进行对比，保留表现较好的鸟窝；

S5：按照被发现的概率产生新的鸟窝，计算各个鸟窝所对应的3-5-3多项式的系数并计算每段轨迹的最大速度，若不满足速度约束，则将该鸟窝的适应度值置为无穷，反之，则该鸟窝的适应度置为每一段轨迹运行到终点对应时刻的和，将新的鸟窝与旧的鸟窝进行对比，保留表现较好的鸟窝；

S6：重复S4和S5，当迭代次数到达最大迭代次数时退出循环，当前全局最优的鸟窝则为该关节的最优时间；

S7：重复S1到S6，直到求解出所有关节的最优时间后退出循环；

S8：对3-5-3多项式的每一段轨迹，在所有关节中选择最长的时间作为该段轨迹的最优时间；

S9：使用最优时间求解出对应的3-5-3多项式的各个系数，确定各段轨迹；

各段轨迹的运行时间即鸟蛋的值应当在(t_low,t_up)这样一个范围内产生，其中t_up表示各段轨迹最多需要的时间，t_low表示各段轨迹最少需要的时间，为了减少算法寻优的时间，使用如下公式产生t_low：

其中s表示该段轨迹的起点到终点的距离，V_max表示该关节的最大速度，则此时t_low实际为该关节以V_max的速度匀速从起点运行到终点的时间。

2.根据权利要求1所述的基于布谷鸟搜索算法的时间最优机械臂轨迹规划方法，其特征在于：步骤S2中，鸟蛋的个数为3分别对应t₁、t₂和t₃，t₁、t₂和t₃表示每一段轨迹运行到终点对应的时刻。

Images