基于QLPSO算法的机械臂多项式插值轨迹规划方法技术

本发明专利技术提供了一种基于QLPSO算法的机械臂多项式插值轨迹规划方法，包括：构建多关节机械臂各关节4

【技术实现步骤摘要】
基于QLPSO算法的机械臂多项式插值轨迹规划方法


[0001]本专利技术涉及一种机器人规划技术，特别是一种基于QLPSO算法的机械臂多项式插值轨迹规划方法。

技术介绍

[0002]轨迹规划可分为关节空间中轨迹规划和笛卡尔空间中轨迹规划两种。关节空间轨迹规划时，优点在于计算量小，适用场景多。多项式轨迹规划是关节空间轨迹规划算法中较为常用且易于理解的方法，以多项式曲线插值进行规划，通常采用三次到七次多项式，个别会用到九次多项式。但多项式的次数越高，需要的数据越多，计算量越大，插值结果也就越偏离原函数。在实际作业任务中，在不同阶段对速度、加速度等有不同的要求，所以相比较而言，分段多项式轨迹规划适用场合更多。有学者提出一种基于改进布谷鸟搜索算法的“3
‑5‑
3”多项式插值轨迹规划算法，在满足速度约束的前提下，实现了时间最优。为保证挖掘机在作业过程中保持轨迹平稳，减少抖震，降低冲击，研究者采用了“3
‑3‑5‑3‑
3”多项式插值算法，该方法实现了挖掘机各关节角速度、加速度曲线平滑连续。
[0003]实际作业任务中，既要考虑时间最优，又要考虑冲击最优，二者的关系是次长彼消的关系，因此需要一种算法折衷考虑时间最优和冲击最优。另一方面，无论机械臂采用“3
‑5‑
3”多项式插值，还是“3
‑3‑5‑3‑
3”多项式插值轨迹规划算法，都无法保证轨迹中间点处的冲击力平滑，即机械臂在中间点处会受到一个突变的冲击力，从而影响机械臂的使用寿命。CN201811135979专利涉及一种QPSO算法的机械臂多项式插值轨迹规划方法，该算法种群多样性较高，但由于位置的随机性，使得搜索效率较低，不能够始终跟随群体最优粒子搜索，且随着迭代的推进，粒子的差异性减少，群体多样性也降低，无法及时更正粒子群参数选择，同时，法实现加速度曲线平滑连续。

技术实现思路

[0004]本专利技术提供了一种基于QLPSO算法的机械臂多项式插值轨迹规划方法，该方法考虑时间最优和冲击最优的综合目标以及机械臂轨迹中间点处加加速度保持连续的问题，利用QLPSO算法，设计了多关节机械臂的时间
‑
冲击最优轨迹规划算法。
[0005]一种基于QLPSO算法的机械臂多项式插值轨迹规划方法，包括：
[0006]步骤1，给出机械臂末端轨迹的起始位置、中间位置以及目标位置，根据机械臂逆动力学，获取每个关节的角位置，给出每个关节的位置、速度、加速度以及加加速度约束条件；
[0007]步骤2，初始化第j个关节的粒子群参数和Q值；
[0008]步骤3，设立时间—冲击最优目标函数，更新粒子i的速度和位置，并计算时间—冲击目标函数值；
[0009]步骤4，计算粒子的轨迹，检测第j个关节的位置、速度、加速度、加加速度是否满足约束条件；若满足，执行步骤5；若不满足，则将该粒子的适应度值置为无穷大，随机设置粒
子的位置和速度，转到步骤3；
[0010]步骤5，将个体最佳P
ij
和全局最佳G
j
进行比较，若个体最佳P
ij
小于全局最佳G
j
，则将该个体最佳赋给全局最佳G
j
，完成更新G
j
，并得出该次迭代的关节j的最小时间—冲击值；
[0011]步骤6，更新每个粒子对应的Q值，选取最大Q值所对应的惯性权值w和学习因子c、d作为下次粒子群迭代的参数。
[0012]步骤7，如果迭代次数达到最大，则转到步骤8；如果迭代次数小于指定迭代次数，则转到步骤3；
[0013]步骤8，如果所有的关节都优化结束，则计算出最大的时间
‑
冲击寻优目标函数值，给出各段轨迹所需的时间，并根据此时间计算出各个关节轨迹，根据正运动学计算出机械臂末端轨迹；如果还有关节没有优化，则转到步骤2，对未优化的关节进行轨迹规划。
[0014]进一步地，步骤1的具体过程在于：
[0015]步骤1.1，构建机械臂各关节4
‑5‑
4多项式插值函数系数与各段轨迹所用时间之间的关系
[0016]θ
1j
(t)＝a
14_j
t4+a
13_j
t3+a
12_j
t2+a
11_j
t+a
10_j
[0017]θ
2j
(t)＝a
25_j
t5+a
24_j
t4+a
23_j
t3+a
22_j
t2+a
21_j
t+a
20_j
[0018]θ
3j
(t)＝a
34_j
t4+a
33_j
t3+a
32_j
t2+a
31_j
t+a
30_j
[0019]式中，t表示每段轨迹所用的时间，θ
ij
(t)表示机械臂第j个关节轨迹的第i段多项式插补函数，i为轨迹的段数，i∈{1,2,3}，j∈{1,2,
…
,N}，N表示关节总数，a
ik_j
表示插补多项式的系数，k∈{1,2,3,4,5}；
[0020]步骤1.2，获取轨迹中的系数与每段时间t
1j
、t
2j
和t
3j
之间的关系
[0021]a
10_j
＝s
oj
,a
11_j
＝0,a
12_j
＝0
[0022][0023][0024][0025]24a
14_j
t
1j
+6a
13_j
＝6a
23_j
[0026]a
20_j
＝s
1j
，a
30_j
＝s
2j
[0027][0028][0029][0030][0031][0032][0033][0034]进一步地，步骤3中记载的时间—冲击最优目标函数为
[0035][0036]s.t.
[0037][0038]式中，λ1为时间的权重系数，λ2为冲击的权重系数，η表示弹性系数，q
max
、v
max
、a
max
、jerk
max
为设定的最大约束，其中q
max
为关节角度，v
max
为设定的速度，a
max
为设定的加速度，jerk
max
为设定的加加度。
[0039]进一步地，步骤4中建立第j个关节轨迹规划的粒子群算法模型
[0040][0041]式中，w为粒子的惯性权值，c、d为学习因子，P
ij
为第j个关节的第本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于QLPSO算法的机械臂多项式插值轨迹规划方法，其特征在于，包括：步骤1，给出机械臂末端轨迹的起始位置、中间位置以及目标位置，根据机械臂逆动力学，获取每个关节的角位置，给出每个关节的位置、速度、加速度以及加加速度约束条件；步骤2，初始化第j个关节的粒子群参数和Q值；步骤3，设立时间—冲击最优目标函数，更新粒子i的速度和位置，并计算时间—冲击目标函数值；步骤4，计算粒子的轨迹，检测第j个关节的位置、速度、加速度、加加速度是否满足约束条件；若满足，执行步骤5；若不满足，则将该粒子的适应度值置为无穷大，随机设置粒子的位置和速度，转到步骤3；步骤5，将个体最佳P
ij
和全局最佳G
j
进行比较，若个体最佳P
ij
小于全局最佳G
j
，则将该个体最佳赋给全局最佳G
j
，完成更新G
j
，并得出该次迭代的关节j的最小时间—冲击值；步骤6，更新每个粒子对应的Q值，选取最大Q值所对应的惯性权值w和学习因子c、d作为下次粒子群迭代的参数。步骤7，如果迭代次数达到最大，则转到步骤8；如果迭代次数小于指定迭代次数，则转到步骤3；步骤8，如果所有的关节都优化结束，则计算出最大的时间
‑
冲击寻优目标函数值，给出各段轨迹所需的时间，并根据此时间计算出各个关节轨迹，根据正运动学计算出机械臂末端轨迹；如果还有关节没有优化，则转到步骤2，对未优化的关节进行轨迹规划。2.根据权利要求1所述的方法，其特征在于，步骤1的具体过程在于：步骤1.1，构建机械臂各关节4
‑5‑
4多项式插值函数系数与各段轨迹所用时间之间的关系θ
1j
(t)＝a
14_j
t4+a
13_j
t3+a
12_j
t2+a
11_j
t+a
10_j
θ
2j
(t)＝a
25_j
t5+a
24_j
t4+a
23_j
t3+a
22_j
t2+a
21_j
t+a
20_j
θ
3j
(t)＝a
34_j
t4+a
33_j
t3+a
32_j
t2+a
31_j
t+a
30_j
式中，t表示每段轨迹所用的时间，θ
ij
(t)表示机械臂第j个关节轨迹的第i段多项式插补函数，i为轨迹的段数，i∈{1,2,3}，j∈{1,2,
…
,N}，N表示关节总数，a
ik_j
表示插补多项式的系数，k∈{1,2,3,4,5}；步骤1.2，获取轨迹中的系数与每段时间t
1j
、t
2j
和t
3j
之间的关系a
10_j
＝s
oj
,a
11_j

【专利技术属性】
技术研发人员：樊春霞，周伟，
申请(专利权)人：南京邮电大学，
类型：发明
国别省市：