(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210620535.3 (22)申请日 2022.06.02 (71)申请人 南京邮电大 学 地址 210023 江苏省南京市 鼓楼区新模范 马路66号 (72)发明人 樊春霞　周伟　 (74)专利代理 机构 南京正联知识产权代理有限 公司 32243 专利代理师 卢霞 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于QLPSO算法的机械臂多项式插值轨迹规 划方法 (57)摘要 本发明提供了一种基于QLPSO算法的机械臂 多项式插值轨迹规划方法， 包括： 构建多关节机 械臂各关节4 ‑5‑4多项式分段插值轨迹规划所需 的时间‑冲击最优目标函数与插值系数、 分段时 间之间的关系； 根据QLPSO算法， 利用粒子Q值来 选择粒子群算法的参数——惯性权值和学习因 子， 利用粒子的位置进行时间 ‑冲击最小寻优， 并 满足各关节的运动学约束条件——位置、 速度、 加速度以及加加速度约束， 从而获得位置平滑、 速度和加速度保持连续， 在路径轨迹插值点处加 加速度亦保持连续的机 械臂关节空间轨 迹。 权利要求书3页 说明书8页 附图10页 CN 114952848 A 2022.08.30 CN 114952848 A 1.一种基于Q LPSO算法的机 械臂多项式插值轨 迹规划方法， 其特 征在于， 包括： 步骤1， 给出机械臂末端轨迹的起始位置、 中间位置以及目标位置， 根据机械臂逆动力 学， 获取每 个关节的角位置， 给 出每个关节的位置、 速度、 加速度以及加 加速度约束条件； 步骤2， 初始化第j个关节的粒子群参数和Q 值； 步骤3， 设立时间—冲击最优目标函数， 更新粒子i的速度和位置， 并计算时间—冲击目 标函数值； 步骤4， 计算粒子的轨迹， 检测第j个关节的位置、 速度、 加速度、 加加速度是否满足约束 条件； 若满足， 执行步骤5； 若不满足， 则将该粒子的适应度值置为无穷大， 随机设置粒子的 位置和速度， 转到步骤3； 步骤5， 将个体最佳Pij和全局最佳Gj进行比较， 若个体最佳Pij小于全局最佳Gj， 则将该 个体最佳赋给全局最佳Gj， 完成更新Gj， 并得出该次迭代的关节j的最小时间—冲击值； 步骤6， 更新每个粒子对应的Q值， 选取最大Q值所对应的惯性权值w和学习因子c、 d作 为 下次粒子群迭代的参数。 步骤7， 如果迭代次数达到最大， 则转到步骤8； 如果迭代次数小于指定迭代次数， 则转 到步骤3； 步骤8， 如果所有的关节都优化结束， 则计算出最大的时间 ‑冲击寻优目标函数值， 给出 各段轨迹所需的时间， 并根据此时间计算出各个关节轨迹， 根据正运动学计算出机械臂末 端轨迹； 如果还有关节没有优化， 则转到步骤2， 对未优化的关节进行轨 迹规划。 2.根据权利要求1所述的方法， 其特 征在于， 步骤1的具体过程在于： 步骤1.1， 构 建机械臂各关节4 ‑5‑4多项式插值函数系数与各段轨迹所用时间之间的关 系 θ1j(t)＝a14_jt4+a13_jt3+a12_jt2+a11_jt+a10_j θ2j(t)＝a25_jt5+a24_jt4+a23_jt3+a22_jt2+a21_jt+a20_j θ3j(t)＝a34_jt4+a33_jt3+a32_jt2+a31_jt+a30_j 式中， t表示每段轨迹所用的时间， θij(t)表示机械臂第j个关节轨迹的第i段多项式插 补函数， i为轨迹的段数， i∈{1,2,3}， j∈{1,2, …,N}， N表示关节总数， aik_j表示插补多项 式的系数， k∈{1,2,3,4,5}； 步骤1.2， 获取轨 迹中的系数与每段时间t1j、 t2j和t3j之间的关系 a10_j＝soj,a11_j＝0,a12_j＝0 24a14_jt1j+6a13_j＝6a23_j a20_j＝s1j， a30_j＝s2j 权　利　要　求　书 1/3 页 2 CN 114952848 A 23.根据权利要求1所述的方法， 其特征在于， 步骤3中记载的时间—冲击最优目标函数 为 s.t. 式中， λ1为时间的权重系数， λ2为冲击的权重系数， η表示弹性系数， qmax、 vmax、 amax、 jerkmax为设定的最大约束， 其中qmax为关节角度， vmax为设定的速度， amax为设定的加速度， jerkmax为设定的加 加度。 4.根据权利要求1所述的方法， 其特征在于， 步骤4中建立第j个关节轨迹规划的粒子群 算法模型 式中， w为粒子的惯性权值， c、 d为学习因子， Pij为第j个关节的第i个粒子的最佳时间， Gj为第j个关节的所有粒子的最佳分段时间， 分别表示第j个关节 粒子i寻 优得到的每段轨迹所需时间和粒子i的速度， r1,r2,r3为[0,1]区间内满足正态分布的随机 数， k为迭代次数。 5.根据权利要求1所述的方法， 其特 征在于， 步骤6中Q 值的更新方法为 式 中 ， ν∈ ( 0 ,1 ) 是 学 习 因 子 ，γ 是 Q 学 习 过 程 中 的 折 扣 因 子 ，状 态 动作aij_k+1＝[wij(k+1),cij(k+1),dij(k+1)]， wij(k+1)是第 i个粒子第j个关节在第k+1次迭代所选择的惯性权重， cij(k+1)和dij(k+1)是第i个粒子第j 个关节在第k+1次迭代所选择的加速度系数， Qij(sij_k,aij_k)是状态—动作的累计Q值； Rij (sij_k,aij_k)是当前迭代次数下状态—动作的即时奖励， 规则为当前迭代下， 粒子目标函数 值比上一次迭代的目标函数值低， 则Rij(sij_k,aij_k)为正数， 否则为负数； 为 当前迭代 环境下目标函数最优粒子的Q 值。权　利　要　求　书 2/3 页 3 CN 114952848 A 3