基于改进PSO算法的林地作业车轨迹优化

庄徐 ，郑哲文，李科军，张恭宇

(1.中南林业科技大学 材料科学与工程学院，长沙 410004；2.湖南交通工程学院 交通运输工程学院,湖南 衡阳 421001)

0 引言

随着我国林木采伐强度增大，传统采伐方式对森林构成了极大的破坏，制约着社会的发展[1]。而林木产业是我国国民经济的重要组成部分，科学的采伐与转运能够更加有效利用森林资源，保护生态环境[2]。为了保证木材安全转运实现机械化、智能化，林地作业车结合了工程机械与工业机器人的特点，能够像工业机器人一样实现自主控制，对木材进行夹取、转运，有效地保障了工作效率且减少了林区工作安全事故的发生。林地作业车臂架末端的执行器作为臂架系统的重要组成部分，其运动轨迹关系着工作精度以及稳定性，因此对林地作业车臂架进行时间最优轨迹规划，对提升工作效率有着重要意义。

在林区等复杂环境下，为了使林地作业车臂架末端的执行器能够快速、准确、平稳地到达目标位置，提升自动化程度，对林地作业车进行最优时间的轨迹规划显得尤其重要。目前，对轨迹规划进行优化主要以实现时间最优、能量最优以及冲击最优开展[3-4]。陈晗等[5]采用复合形法对5-7-5分段多项式插值轨迹进行优化，达到了优化时间的目的，但分段多项式阶次较高，运行时间较长。李虹等[6]分别运用粒子群算法与差分进化算法实现了对4-3-3-3-4分段多项式插值轨迹的时间优化，提高了各关节的工作效率，但分段次数多，计算量大。Qiao等[7]采用改进遗传算法对五次多项式插值进行轨迹规划，实现了关节轨迹全局最优，但其插值采用五次多项式，相较于分段多项式插值，该方法用时较多。李国洪等[8]通过聚类方法对遗传算子进行改进，以新的变异操作提升了寻优效率，缩短了关节轨迹运行时间，但全局采用7次样条插值，阶次较高，计算量大。以上研究虽取得一定成果，但普遍存在计算量大、耗时较多等问题。

针对上述问题，本研究以林地作业车为研究对象，通过改进PSO算法对臂架末端进行以时间最短为目标的轨迹优化。首先通过标准型D-H(Denavit-Hartenberg)法构建林地作业车运动学模型，通过在工作空间中选取4个途径位置作为轨迹插值点，采用3-5-3分段多项式插值进行轨迹规划，最后在Matlab软件中通过改进PSO算法与标准PSO算法对轨迹运行时间进行优化，结果表明在相同运动学约束下，改进PSO算法能够更快搜索到全局最优解并验证了该方法的可行性。

1 林地作业车运动学分析

本研究的林地作业车如图1所示，其臂架系统可视为一款5自由度机械臂，结构紧凑，小巧灵活，能够在林区复杂工作环境中连续作业，保障生产效率。根据标准型D-H表达法建立连杆坐标系[9]，见表1，其中θ1—θ5为关节回转角度，d1—d5为连杆之间的偏距，a1—a5为各连杆的长度，α1—α5为各连杆的扭转角度。

图1 林地作业车Fig.1 Forest work vehicles

表1 林地作业车D-H参数

结合表1，可以通过齐次变换矩阵求得臂架的正运动学方程，其中第i个连杆相对于第i-1个连杆的齐次变换矩阵为[10]

(1)

将表1中的各参数代入公式(1)中可计算出臂架末端相对于基坐标系的位姿矩阵

(2)

公式(2)中，前3列为臂架末端相对于基坐标系的姿态，最后1列为臂架末端相对于基坐标系的位置。

为了确保臂架末端能够到达路径中的各点，需要计算出各关节在轨迹中的位置，基于Matlab软件Robotics Toolbox对各关节进行逆运动学求解。根据公式(2)可以确定臂架末端经过4个路径点时在笛卡尔空间(Cartesian space)中的位置，见表2。

表2 臂架末端运动轨迹路径坐标Tab.2 Trajectory path coordinates at the end of the boom

根据逆运动学求解方法[11]，可以求得各关节在路径点中的转角大小，结合Robotics Toolbox求得各关节路径点的解见表3。

表3 路径点各关节逆解Tab.3 Inverse kinematics of each joint at the path point

2 基于改进PSO算法的3-5-3多项式插值函数

2.1 分段多项式轨迹规划方法

根据3-5-3分段多项式轨迹(时间最优轨迹规划，就是三次多项式、五次多项式以及B样条插值等进行轨迹规划)连续、平滑无突变且计算量小的特点，本研究采用3-5-3分段多项式插值进行轨迹规划，通过计算各区间运动所需的时间获取整段轨迹的运行时间。3-5-3多项式的通式如下[12]

(3)

式中：Li1(t)、Li2(t)、Li3(t)为关节i(i=1,2,3,4,5)在3段多项式插值中的轨迹；aijm为关节i在第j段轨迹中的第m+1个未知数；t1、t2、t3表示各关节分别在3段区间中的运行时间。

为了保持臂架末端执行器轨迹的连续性与平顺性，各关节在初始点与终止点的速度与加速度为0,经过各路径点时的速度与加速度连续，即

(4)

根据以上约束，推导出未知系数与各插值点的关系式为

(5)

(6)

(7)

公式(5)中的t1、t2、t3表示关节在3段多项式中的插值时间；公式(6)中b为关节对应的角位移矩阵；公式(7)为关节系数矩阵。

2.2 改进PSO算法

本研究采用改进粒子群算法计算各关节最短运动时间，通过将关节完成运动所用的时间t1、t2、t3组成1个三维粒子群，随机生成N个粒子，其适应度函数(fitness，f(t))为[13]

f(t)=min(t1+t2+t3) 。

(8)

式中，ti分别为关节在第i段轨迹中的运动时间。

初始化种群后，根据函数适应度计算每个粒子的适应度值，将当代粒子中适应度最小的粒子作为局部最优粒子pid，经迭代完成后，将所有粒子中适应度最小的粒子作为全局最优粒子pgd。粒子群算法的速度与位置更新公式如下[14]

(9)

(10)

标准PSO算法的惯性权重系数(ω)以及学习因子c1、c2为固定值，在计算过程中容易早熟收敛至局部最优且迭代收敛速度慢[16]。针对此问题，本研究采用一种惯性权重线性递减且动态调整学习因子的自适应方式，保证了粒子的前期搜索能力，加快了收敛速度，计算方式为

(11)

式中：k为当前迭代次数；n为全局迭代次数；迭代前期c2逐渐增大使粒子从当前位置逐渐向全局最优位置搜索；后期c1逐渐增大使粒子个体搜寻能力加强，抑制了粒子在最优解附近进行重复搜索，避免陷入局部最优[17]。

通过对各关节进行速度与加速度的限制，保证末端执行器平稳运行，以时间t为寻优量，对关节轨迹运行时间进行优化，步骤如下。

(1)初始化种群，对每个关节3段轨迹的插值时间分别随机生成M个粒子，初始化粒子的位置与速度。

(2)将M组插值时间带入公式(5)中，进而计算出多项式中的关节系数矩阵a。并将矩阵a中的各项系数带入公式(3)中，根据时间计算出各关节的位置，并求导计算出各关节的速度与加速度。

(3)计算粒子个体适应度，将所计算出的各关节速度与加速度与设定的最大速度与加速度进行对比分析，若在约束范围内，则通过公式(8)作为该粒子的适应度值，若存在大于约束值的现象，则将一个极大值赋值予此粒子，便于后期迭代过程中淘汰该粒子。

(4)经n次迭代后，选择每个粒子所经过的最优位置赋予pid，选取pid中适应度最小的粒子为pgd。将pid、pgd代入公式(9)与公式(10)中生成新的种群。

(5)判断是否达到最大迭代值或找到全局最优解，满足条件则输出最优时间值，迭代结束，否则返回至步骤2。

3 仿真结果分析

根据公式(2)可以得到臂架末端相对于基坐标系的齐次变换矩阵。通过Matlab软件中Robotics Toolbox对林地作业车进行建模，如图2所示，根据工作特点，在不考虑回转的情况下，设定插值时间为10 s，t1=3 s,t2=4 s,t3=3 s，分别为对林地作业车4个关节采用3-5-3分段多项式插值进行轨迹规划，各关节轨迹曲线如图3所示。

分析图3可知，林地作业车臂架各关节角度位置、速度、加速度变化连续，但仍存在一定优化空间。

图2 林地作业车三维模型Fig.2 3D model of forest work vehicles

图3 3-5-3分段多项式插值轨迹曲线Fig.3 3-5-3 segmented polynomial interpolated trajectory curve

根据表4中各关节的约束条件，分别应用标准PSO算法和改进PSO算法，按照步骤1—步骤5对各关节运行时间进行优化，设置总迭代次数N=100，随机散布M=50个粒子，标准PSO算法的惯性权重系数范围为[0.4,0.9],学习因子c1、c2取值为2。改进PSO算法的惯性权重系数与学习因子根据公式(11)变化，分析其动态变化对算法收敛速度与适应度的影响，迭代曲线如图4所示。

表4 各关节约束条件Tab.4 Constraints of individual joint

图4 各关节迭代曲线Fig.4 Iterative curves for each joint

由各关节的迭代曲线可以看出，采用改进PSO算法的适应度曲线收敛速度比采用标准PSO算法的适应度曲线收敛速度快，且收敛后的最优适应度更低，验证了改进PSO算法求解优于标准PSO算法，因此基于改进PSO算法的3-5-3多项式插值具有更好的收敛速度与寻优效率。

为了保证臂架各关节能够同时将末端工作装置运达指定工作位置，因此选用各关节的最大运行时间作为插值时间，优化后各关节的位置、速度、加速曲线如图5和图6所示。

图5 标准PSO算法优化各关节轨迹曲线Fig.5 The standard PSO algorithm optimized the trajectory curve of each joint

图6 改进PSO算法优化各关节轨迹曲线Fig.6 The improved PSO algorithm optimized the trajectory curve of each joint

分析图5和图6可以看出，在速度与加速度约束条件下，采用改进PSO算法相对于标准PSO算法，其关节速度与加速度更大，且最大速度与加速度均在约束范围内，符合预期设定。所用时间缩短了1.3 s，缩短约13%，有效提升了各关节的工作效率。

4 结论

本研究以林地作业车为研究对象，通过标准型D-H法建立运动学模型，采用3-5-3分段多项式插值对工作空间中4个途经点进行轨迹规划，在各关节运动学约束下采用改进PSO算法对轨迹运行时间进行优化，并对比分析与标准PSO算法的优化性能。研究结果表明，改进PSO算法的收敛速度更快，且收敛后的适应度更小，各关节的速度与加速度曲线由9.64 s缩短至8.34 s，缩短约13%，提高了林地作业车的工作效率。