一种多约束条件下最优时间的机械臂空间轨迹优化方法与流程

本发明属于多自由度机械臂控制技术领域，涉及机械臂空间轨迹优化方法，具体涉及一种多约束条件下最优时间的机械臂空间轨迹优化方法。

背景技术：

在日益现代化的工业制造领域，对机器臂有着日益强大的应用需求。其中，机器臂的轨迹规划与轨迹跟踪控制算法是机械臂研究的核心内容，该方面的研究可为机械臂平稳运行、提高工作效率等提供技术支持。

时间作为轨迹规划的重要指标，可以直观地表现机械臂的工作效率的要求，使机械臂能够高效运作；同时，对机械臂关节加入约束条件，使其在高效运作的同时更加平稳。

现有的关于最优时间的轨迹优化方法主要是根据粒子群算法进行单目标优化，没有考虑各个关节的约束条件，不能反映出整个机械臂运行时间的全局水平；另一方面，标准粒子群算法很容易陷入局部极值，从而不能得出时间的最优解，使得机械臂的工作效率降低。

技术实现要素：

本发明的目的是提供一种多约束条件下最优时间的机械臂空间轨迹优化方法。

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

一种多约束条件下最优时间的机械臂空间轨迹优化方法，其特征在于，包括如下步骤：

步骤1)根据机械臂在笛卡尔坐标系下的路径点，通过逆运动学求解得到各个关节在关节空间的插值角度；

步骤2)根据机械臂的在关节空间的插值关节角度，生成3-5-3多项式插值函数，得到机械臂的连续轨迹运动方程；

步骤3)根据优化目标要求以及约束条件生成各关节的适应度函数：

其中，fitness(t)s为第s个关节的适应度函数，m为机械臂的关节总个数，s表示关节序数,取值为1到m之间正整数,tsj为第s个关节的第j维度的时间，n为时间总维度，j表示维度序数,取值为1到n之间正整数,t表示时刻，为第s个关节在第t时刻的速度值，为第s个关节在第t时刻的加速度值，为第s个关节在第t时刻的跃度值，vsmax为第s个关节的最大速度值，asmax为第s个关节的最大加速度值，jsmax为第s个关节的最大跃度值；

步骤4)利用改进混沌粒子群算法对机械臂轨迹进行优化；

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

s1：初始化粒子群规模，学习因子，惯性权重，最大迭代次数，设置多样化参数m，用来参照粒子群的离散程度，设置每一维变量的变化范围；

s2：利用tent映射产生粒子的位置和速度，并映射到解空间，计算粒子的适应度函数值，选出较优部分作为初始群体，公式如下；

其中，zx代表第x个粒子的位置或速度值，x表示序数，为正整数，zx∈[0,1]；

s3：比较每个初始粒子的适应度函数值，将函数值最小的粒子作为全局最优粒子，并选出每个粒子的个体最优极值；

s4：按照以下公式进行粒子群位置和速度的更新；

vij(k+1)＝ωvij(k)+c1r1(pij(k)-xij(k))+c2r2(gj(k)-xij(k))

xij(k+1)＝xij(k)+vij(k+1)

其中，i表示粒子序数，为正整数，vij(k+1)为第i个粒子的第j维k+1代速度值，xij(k+1)为第i个粒子的第j维k+1代位置值，pij(k)为第k代第i个粒子的个体最优极值的第j维数值，gj(k)为第k代粒子群中全局最优极值的第j维数值，ω为惯性权重，c1、c2为学习因子，r1、r2为0到1之间的随机数；

若更新后粒子的位置及速度值超过设置的数值区间，则取区间的上下限作为粒子的位置及速度值；

s5：计算粒子群的适应度函数值方差n为粒子的总个数，m为多样性参数，用来衡量粒子群的离散程度，且m为常数，根据不同的情景具体赋值，若σ²>m，则说明粒子群的多样性程度高，执行步骤s7；若σ²<m，则说明粒子群的多样性程度低，执行步骤s6；

s6：对粒子进行混沌化处理，以全局最优位置为初始条件产生新混沌序列，用新混沌群体的最优粒子替换当前群体中适应度最差的粒子：

将全局最优位置g＝(g1、g2.....gj)映射到下列式子：

其中，aj、bj为第j维粒子位置的上、下限；

以zj0为初始状态，利用tent映射产生h个新的混沌粒子z'＝[z1'、z2'...zh']，利用下列公式将新产生的混沌序列映射到解空间，得到h个新粒子的位置g'＝[g'1、g'2...g'h]

gl′＝aj+(bj-aj)zl′(l＝1、2...h)

计算h个粒子的适应度函数值，选出最优粒子位置g'best替换原粒子群的适应度最差的粒子；

s7：对每个粒子的个体最优值进行检测，若粒子个体最优值超过5次迭代为发生变化，则对粒子进行逃脱操作，按下列公式重新计算该粒子的惯性权重；

其中，k为粒子群当前的迭代次数，t为粒子群的最大迭代次数；

s8：判断算法是否到达最大迭代次数，若满足，则算法结束，否则跳转至步骤s4。

作为优选，所述学习因子c1、c2采取随时间动态调整的方法，在算法进行寻优初期，粒子具有较强的自我探索能力和较弱的群体学习能力，以加强粒子的全局搜索能力；在寻优后期，粒子具有较弱的自我探索能力与较强的群体学习能力，学习因子按照下列公式更新：

其中，c1max、c1min为认知系数的最大值和最小值，k为粒子群当前的迭代次数，t为粒子群的最大迭代次数，c1从2.5线性递减到1.5；c2max、c2min为社会系数的最大值和最小值，c2从1.5线性递增到2.5。

作为优选，所述惯性权重ω采取自适应调整的方法，根据粒子当前适应度以及迭代次数进行自动自适应调整，增加了粒子群全局搜索与局部搜索的协调性，公式如下；

其中，ωmax、ωmin分别为ω的最大值与最小值；为粒子平均适应度，fi为第i个粒子的适应度函数值，fmin为粒子最小适应度，t为最大迭代次数，k为当前迭代次数，a、b为常数系数，根据不同的优化问题来具体设置。

和现有技术相比，本发明具有如下技术效果：

(1)本发明充分考虑了机械臂每个关节的速度、加速度、跃度等约束条件，使得每个关节都能平稳工作。

(2)本发明利用混沌序列来增加粒子种群的多样性，并用多样性参数来检测粒子的离散状态，使粒子群在混沌和稳定之间达到平衡，改善了以往粒子群算法收敛过早，容易陷入局部极值的缺点。

(3)本发明检测每个粒子的个体最优极值，若迭代次数超过5次未发生改变，则对粒子进行逃脱操作，使得每个粒子陷入局部极值时能及时跳出。

(4)本发明采取惯性权重ω自适应调整的方法，根据粒子当前适应度以及迭代次数进行自动自适应调整，增加了粒子群全局搜索与局部搜索的协调性。

附图说明

图1是本发明机械臂空间轨迹优化方法流程图；

图2仿真实验后机械臂的关节一角度、速度、加速度的曲线变化图；

图3仿真实验后机械臂的关节二角度、速度、加速度的曲线变化图；

图4仿真实验后机械臂的关节三角度、速度、加速度的曲线变化图；

图5仿真实验后机械臂的关节四角度、速度、加速度的曲线变化图；

图6仿真实验后机械臂的关节五角度、速度、加速度的曲线变化图；

图7仿真实验后机械臂的关节六角度、速度、加速度的曲线变化图。

具体实施方式

下面结合附图对本发明作进一步说明。

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

1、根据笛卡尔坐标系下的路径点生成关节空间的路径点

将机械臂末端执行器的笛卡尔坐标系下的轨迹转换成关节坐标系下的角度变化，设机械臂完成整个轨迹需要经过的路径点θ：

θ＝{θsr|s＝1,...m；r＝1,...n+1}

其中，θsr表示第s个关节的第r个路径点的位置，m为机械臂的关节个数，n+1为路径点的个数。

2、生成各关节从任务起点到终点的3-5-3插值函数

假定机械臂的一个关节从关节空间中a点路径上两点b和c点运动到终点d，则a点和b点之间的轨迹用3次多项式表示，b点和c点之间的轨迹用5次多项式表示，c点和d点之间的轨迹用3次多项式表示；根据生成的关节空间路径点，生成各关节的插值函数，其中3-5-3插值函数能满足机械臂各关节对于速度、加速度、跃度在时域的连续性，先对时间进行归一化处理，定义时间t:

其中，τ为轨迹段的实际时间，τ∈[τj-1，τj]，τj-1为第j段轨迹的起始时间，τj为第j段轨迹的末端点时间，j＝1，2，3.各段轨迹的方程如下：

θ1(t)＝a10+a11t1+a12t1²+a13t1³

θ2(t)＝a20+a21t2+a22t2²+a23t2³+a24t2⁴+a25t2⁵

θ3(t)＝a30+a31t3+a32t3²+a33t3³

其中，θ1(t)、θ2(t)、θ3(t)为三段多项式轨迹，设机械臂关节在起点和终点的位置、速度和加速度分别是θ0、θf、v0、vf、a0、af，根据轨迹及其中间点位置、速度和加速度连续的要求得到约束条件，结合上面的等式可以求得等式中的系数；

a10＝θ0a20＝θ1a30＝θ2a11＝v0δτ1a13＝δθ1-a11-a12b1＝δθ2-a21-a22a23＝10b1+b2-4b3a24＝-15b1-2b2+7b3a25＝6b1+b2-3b3

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

以机械臂运行时间最短为目标构建适应度函数：

每个机械臂关节都有其关节约束，对每个关节施加约束，并构造多约束条件下的适应度函数：

其中，fitness(t)s为第s个关节的适应度函数，m为机械臂的关节总个数，s表示关节序数,取值为1到m之间正整数,tsj为第s个关节的第j维度的时间，n为时间总维度，j表示维度序数,取值为1到n之间正整数,t表示时刻，为第s个关节在第t时刻的速度值，为第s个关节在第t时刻的加速度值，为第s个关节在第t时刻的跃度值，vsmax为第s个关节的最大速度值，asmax为第s个关节的最大加速度值，jsmax为第s个关节的最大跃度值。

对机械臂关节加入约束，在保证其正常运行的情况下缩短工作时间，当机械臂不能满足约束条件时，则将其适应度函数值设为最大值fitnessmax。

4、利用改进的混沌粒子群算法对各个关节的适应度函数进行优化求解，具体步骤如下：

s1：初始化粒子群规模，学习因子，惯性权重，最大迭代次数，设置多样化参数m，参数大小依据具体问题而定，用来参照粒子群的离散程度，并设置每一维变量的变化范围；

s2：利用tent映射(帐篷映射)产生粒子的位置和速度，并映射到解空间，计算粒子的适应度函数值，选出较优部分作为初始群体，公式如下；

其中，zx代表第x个粒子的位置或速度值，x表示序数，为正整数，zx∈[0,1]；利用下列公式映射到解空间；

zx＝aj+(bj-aj)zxx＝0、1、2...j＝1、2...n

其中，a、b分别为每一维变量的下限和上限。

s3：计算并比较每个初始粒子的适应度函数值，将函数值最小的粒子作为全局最优粒子，并选出每个粒子的个体最优极值；

s4：按照以下公式进行粒子群位置和速度的更新；

vij(k+1)＝ωvij(k)+c1r1(pij(k)-xij(k))+c2r2(gj(k)-xij(k))

xij(k+1)＝xij(k)+vij(k+1)

其中，i表示粒子序数，为正整数，vij(k+1)为第i个粒子的第j维k+1代速度值，xij(k+1)为第i个粒子的第j维k+1代位置值，pij(k)为第k代第i个粒子的个体最优极值的第j维数值，gj(k)为第k代粒子群中全局最优极值的第j维数值，ω为惯性权重，c1、c2为学习因子，r1、r2为0到1之间的随机数,惯性因子ω按下列公式更新：

其中，ωmax、ωmin分别为ω的最大值与最小值；为粒子平均适应度，fi为第i个粒子的适应度函数值，fmin为粒子最小适应度，t为最大迭代次数，k为当前迭代次数，a、b为常数系数，根据不同的优化问题来具体设置；

学习因子按照下列公式更新：

其中，c1max、c1min为认知系数的最大值和最小值，c1从2.5线性递减到1.5；c2max、c2min为社会系数的最大值和最小值，c2从1.5线性递增到2.5；

若更新后粒子的位置及速度值超过设置的数值区间，则取区间的上下限作为粒子的位置及速度值；

s5：计算粒子群的适应度函数值方差n为粒子的总个数，m为多样性参数，用来衡量粒子群的离散程度，m为常数，根据不同的情景具体赋值，若σ²>m，则说明粒子群的多样性程度高，执行步骤s7；若σ²<m，则说明粒子群的多样性程度低，执行步骤s6；

s6：对粒子进行混沌化处理，以全局最优位置为初始条件产生新混沌序列，用新混沌群体的最优粒子替换当前群体中适应度最差的粒子：

将全局最优位置g＝(g1、g2.....gj)映射到下列式子：

其中，aj、bj为第j维粒子位置的上、下限；

以zj0为初始状态，利用tent映射(帐篷映射)产生h个新的混沌粒子z'＝[z1'、z2'...zh']，利用下列公式将新产生的混沌序列映射到解空间，得到h个新粒子的位置g'＝[g'1、g'2...g'h]

gl′＝aj+(bj-aj)zl′(l＝1、2...h)

计算h个粒子的适应度函数值，选出最优粒子g'best替换原粒子群的适应度最差的粒子；

s7：对每个粒子的个体最优值进行检测，若粒子个体最优值超过5次迭代为发生变化，则对粒子进行逃脱操作，按下列公式重新计算粒子的惯性系数；

其中，k为粒子群当前的迭代次数，t为粒子群的最大迭代次数，粒子在迭代初期不易陷入局部极值，将ω设为e^-λ使其在迭代后期加大惯性因子，能够及时逃脱局部极值。

s8：判断算法是否到达最大迭代次数，若满足，则算法结束，否则跳转至步骤s4。

5、按照目标要求选出最优的轨迹规划方案

根据时间最短的要求选出最佳的方案，

具体实施案例

以6自由度的机械臂为例，首先预设关节空间的路径点，如下表所示：

对机械臂关节施加约束：速度约束为[-4040](deg/s)，加速度约束为[-6060](deg/s²)。

在给定的约束条件下，以最短时间为目标，利用matlab进行仿真。图2到图7是仿真实验后机械臂六个关节角度、速度、加速度的曲线变化图，机械臂的运动变化在约束范围内。利用本算法对时间进行优化，得到六个关节的运行时间，取每一维的最大值构成最后的最优时间：fitness＝[1.962.922.45]。

与标准粒子群算法进行比较，取第一个关节的时间为例：

从上表数据可知，与标准粒子群算法相比，本算法优化后的时间有所减少，从第三维的时间可知，标准粒子群在优化时陷入局部极值，导致时间明显增大，而本算法能够及时跳出局部极值。

综上所述，多约束条件下最优时间的机械臂空间轨迹优化方法，在充分考虑每个关节的运动约束条件下，对机械臂的工作时间进行明显的优化，并使其陷入局部极值的情况大大降低，使得机械臂能够快速平稳的运行，大大提高了工作效率。