自主挖掘的时间最优轨迹规划

蔡敢为，王芬，田军伟，齐港，张轲忱

(1.广西大学 机械工程学院，广西 南宁 530004;2.河池学院 人工智能与制造学院，广西 546300；3.湖南三一职业技术学院 工程技术学院, 湖南 长沙 410000)

0 引言

随着挖掘机作业精度和效率要求的不断提高以及为适应危险恶劣环境，自动化、智能化的研究已成挖掘机的主要发展趋势[1]。自动挖掘机的轨迹规划是其控制的基础，旨在规划出一条通过给定路径点并且满足边界约束条件的光滑的挖掘轨迹[2]。在此基础上，基于时间最优的轨迹规划能够提高挖掘作业的工作效率，近年来得到了国内外学者的广泛研究。对自动挖掘机进行时间最优轨迹规划，关键在于对其各个关节的规划，使其满足速度最值、加速度最值等约束条件。

挖掘机铲斗末端的笛卡儿坐标可由各个运动关节的驱动函数确定，而驱动函数多为多项式插值函数[3-7]。徐海黎等[8]使用三次多项式将关节空间中的插值节点连接起来，但需要已知每个节点的速度和加速度，不具有普遍性。章旭[9]采用带虚节点的三次多项式插值方法，虽然能够保证首末位置的速度、加速度为零，但在虚节点处末端轨迹不可控，误差较大。李海虹等[10]以角加速度为约束条件，修正3-3-5-3-3多项式的最高阶阶数，能更好地满足操作的平稳连续，但是求解相对复杂。孙志毅等[11]提出4-3-3-3-4多项式的方法，避免了采用高阶函数造成的关节往复运动，但求解相对复杂。唐建业等[12]采用4-4-7-4多项式插值的方法，可以保证加加速度连续，能够实现时间和平稳性的综合最优，但是计算量很大。刘凉等[13]采用分段五次样条曲线对关节角度进行插值，虽然能保证驱动力矩的连续性以及满足运动学和动力学的约束条件，但必须已知各插值节点的速度和加速度，计算麻烦。管成等[14]提出了一种基于NURBS插值多项式的挖掘轨迹规划方法，通过莱布尼兹公式求解多项式的高阶导矢，能够保证关节加加速度连续，但求导公式复杂且求解效率低。Liu等[15]提出七次NURBS曲线插值方法，虽然可以有效降低进给速度波动，但是约束条件计算复杂。胡小平等[16]提出一种多项式和Newton插值法相结合的机械手轨迹规划，虽然计算量小，但当节点数多时，中间段Newton插值多项式计算仍然很难。综上所述，多项式插值方法在更改目标节点后需重新计算整个优化函数，在目标节点较多时，计算量大，而NURBS样条插值，虽然不用计算整个优化函数；但求解效率低，因此提出一种能够适用于多目标节点的且算法效率高的插值方法，具有较重要的实际意义。

本文以某液压挖掘机为研究对象，为使末端路径更贴合规划路径，提出一种在关节空间内交叉使用三次插值多项式与五次插值多项式的轨迹插值方法，其中三次多项式的参数仅以角位移确定，以覆盖规划轨迹上更多的点，三次多项式之间通过五次多项式连接，使角速度、角加速度连续、保证轨迹的稳定性。最后以时间最优为目标，采用该插值方法对轨迹进行优化，得到贴合规划轨迹的关节角度曲线。

1 挖掘机运动学建模

1.1 Denavit-Hartenberg(D-H)参数法建模

根据D-H坐标系原则建立某反铲液压挖掘机的运动学模型，如图1所示。

图1 挖掘机的D-H坐标系

其中，坐标系{O0:x0y0z0}设置在挖掘机回转机构的中心，坐标系{O1:x1y1z1}设置在动臂与机身的铰接点处，坐标系{O2:x2y2z2}设置在动臂与斗杆的铰接点处，坐标系{O3:x3y3z3}设置在斗杆与铲斗的铰接点处，坐标系{O4:x4y4z4}设置在铲斗齿尖处。D-H坐标系中的参数ai为各连杆长度，即相邻两关节轴线之间的距离；αi为连杆扭角，即两关节轴线之间的夹角；di为连杆距离，即xi轴与xi-1轴之间的距离；θi为连杆转角，即xi轴与xi-1轴之间的夹角。挖掘机的D-H参数见表1，其中关节变量范围通过液压缸长度以及各部件长度、关节转角求出。

表1 挖掘机的D-H参数

根据图1的D-H坐标系以及表1的D-H参数可得铲斗齿尖的坐标表达式为

(1)

式中：cθ0=cosθ0；sθ0=sinθ0；cθ1=cosθ1；sθ1=sinθ1；c12=cos(θ1+θ2)；

c123=cos(θ1+θ2+θ3)；s12=sin(θ1+θ2)；s123=sin(θ1+θ2+θ3)。

结合铲斗齿尖的坐标表达式以及关节变量范围，在不考虑挖掘机回转的情况下，通过数值分析软件绘出挖掘机铲斗齿尖的工作范围如图2所示。同时在图2中以挖坑为例取点，具体数据见位姿空间到关节空间的转换表(表2)中的第二列数据，挖掘路径如图3所示。

图2 挖掘机铲斗齿尖的工作范围

图3 挖掘路径

1.2 逆运动学的数值解

轨迹规划是分别对各个关节进行轨迹求解，最后映射到铲斗齿尖的轨迹[17]，因此需要通过运动学逆解将铲斗齿尖对应的坐标转化为关节角度值。本文采用粒子群算法进行运动学逆解求解，考虑各角度变化尽可能小，以公式(2)作为适应度函数。

f=10 000((a3cos(A2+B2+C2)+a2cos(A2+B2)+a1cos(A2)-X2)2+

(a3sin(A2+B2+C2)+a2sin(A2+B2)+a1sin(A2)+d0-Y2)2)+

min(|A2-A1|+|B2-B1|+|C2-C1，|

(2)

式中：X2、Y2为目标点的铲斗位置坐标值；A1、B1、C1为初始点的关节角度值；A2、B2、C2为目标点的关节角度值。

本文给定初始点的关节角度值为(20°,-90°,-45°)，第一个目标点的坐标为(6 m, 0 m,-4 m)，并以第一个目标点对应的关节角度值作为第二个目标点对应的初始关节角度值，依次类推。粒子群算法基本参数设定如下：粒子数N=20,惯性权重ω=1，加速因子C1=C2=1.5，最后进化代数为1 200。位姿空间到关节空间的转换见表2。

表2 位姿空间到关节空间的转换表

2 交叉插值多项式函数

2.1 交叉插值多项式函数表达式

用运动学逆解方法结合粒子群算法求出每个作业路径点对应的关节空间角度值后，需要对各个关节拟合轨迹曲线。为保证工作效率及稳定性，本文在关节空间内采用一种三次插值多项式与五次插值多项式相交叉的新型轨迹插值方法对关节轨迹进行拟合。

假设一共有n段三次多项式，将五次多项式穿插于三次多项式之间后，再将整条轨迹始末各增加一段五次曲线，则五次曲线的段数为(n+1)段，第i个关节的插值表达式为公式(3)。

其中，θiqj(t)表示t时刻第i个关节的第j段五次曲线位移，θicj(t)表示t时刻第i个关节的第j段三次曲线位移，aiqjl表示第i个关节的第j段五次曲线的l次项系数，aicjl表示第i个关节的第j段三次曲线的l次项系数。

(3)

2.2 插值函数系数求解

为保证插值函数轨迹可以经过更多规划路径上的点，因此仅以4个目标点的角位移确定每段三次多项式的系数，即

(4)

其中tjck(k=0，1，…，3)依次为第j段三次曲线经过四个目标点的时刻。为计算简便，将每段三次曲线分开计算，令每段曲线的初始时刻tjc0=0。θijk为第i个关节的第j段三次曲线在tjck时刻时的角位移。将方程组写作矩阵形式，即

Ajxj=bj，

(5)

再考虑五次多项式，当五次多项式处于2个三次多项式之间时，其系数由相邻的三次多项式的速度及加速度确定，因此对式(4)在三次曲线的初始及结束时刻对时间求一阶导及二阶导，得

(6)

为保证轨迹段之间速度与加速度连续，使第j段五次曲线的起始时刻的速度、加速度为第j-1段三次曲线的结尾时刻的速度、加速度，其结束时刻的速度、加速度为第j段三次曲线的起始时刻的速度、加速度。

则此时五次多项式系数可由式(7)确定：

(7)

其中tjq0及tjq1分别为第j段五次曲线初始时刻与结束时刻。将每段五次曲线分开计算，令每段曲线的初始时刻tjq0=0。

将方程组写作矩阵形式，即

Bjyj=cj，

(8)

yj=(aiqj5,aiqj4,aiqj3,aiqj2,aiqj1,aiqj0)T；

当五次多项式处于整条轨迹起始位置或结尾时，只需将上式中的cj替换为

其中θi0及θi1n分别为第i个关节整条轨迹初始角位移及结尾角位移。

3 基于时间最优的轨迹规划

3.1 分段优化设计问题的建立

通过对上述三次方程和五次方程系数求解，可以看出只有插值时间是待定的，因此可以将插值时间进行优化确定方程系数。本文将总体时间最优化问题，分解为各段三次插值曲线与五次插值曲线两部分优化设计问题的组合，并分段建立优化设计问题。

在挖掘作业时，各关节相互联合运动完成作业，根据事先规划好的轨迹由液压缸调整驱动动臂，斗杆和铲斗三个关节同时运动且每个关节均需要满足对应的最大角速度和角加速度限制,因此每段三次曲线的优化目标函数为

(9)

式中hjk表示第j段三次曲线插值节点之间的时间间隔，即

hjk=tjck-tjc(k-1)。

假设一共有N个关节，则约束条件为

(10)

再考虑五次插值曲线，每个关节每段五次曲线的优化目标函数为

gj,min=minhj，

(11)

式中hj表示第j段五次曲线插值节点之间的时间间隔，即

hj=tjq1-tjq0。

每段五次曲线优化设计问题的约束条件为

(12)

3.2 优化设计问题的求解

活跃目标点粒子群优化(APSO)算法[18]是一种改进的粒子群算法，其基本思想是,在标准PSO速度更新公式中引入第3个目标点,称为活跃目标点,从而构成新的基于3目标点速度更新机制的粒子速度更新公式。APSO的优点是较好地克服了PSO的早熟收敛问题,并兼具复合形法射线搜索的能力。

本文将hjk、hj作为优化设计变量，结合APSO算法，分别对每段三次曲线及五次曲线进行优化，并汇总得到总体最优时间优化设计的结果。优化设计问题算法流程如图4所示。

图4 优化设计问题算法流程

4 仿真结果

将1.2节中规划的10个插值节点，根据新的插值方法分配节点，得到形如5-3-5-3-5的分配结果。各个关节对应的最大角速度和角加速度限制数值，其数值大小由挖掘机的物理约束条件(表3)给出。

表3 挖掘机的物理约束条件

由3.2节中的优化算法得到2段三次曲线以及3段五次曲线的最短时间。第1段三次曲线所经过的时间依次为0.891 7、1.340 9、2.176 9 s，对应的f1,min=4.409 6 s，第2段三次曲线所经过的时间依次为1.270 9、1.336 7、1.351 7 s，对应的f2,min=3.959 2 s。3个五次曲线插值时间分别为1.902 6、1.223 7、2.496 9 s。同时得到每个关节2段三次曲线的系数。

动臂关节交叉插值多项式的表达式为

斗杆关节交叉插值多项式的表达式为

铲斗关节交叉插值多项式的表达式为

本文选取10个插值节点中序号为1、3、5、6、8、10的6个关节角度值，以文献[5]中的4-3-3-3-4多项式为关节插值函数，通过APSO算法对关节插值时间进行优化求解，最终可以求出每个关节的插值曲线系数。动臂关节4-3-3-3-4的插值表达式为

斗杆关节4-3-3-3-4的插值表达式为

铲斗关节4-3-3-3-4的插值表达式为

分别对本文的交叉插值多项式和4-3-3-3-4多项式关节角度函数求取一阶和二阶导数，得出了挖掘机3个关节角度、角速度、角加速度变化曲线图，分别如图5、6所示。

(a)交叉插值多项式最优关节角度图

从图5可以看出，通过APSO算法得到的交叉插值多项式的角度、角速度、角加速度曲线连续，且都在给定的约束条件范围内，说明此方法的正确性和有效性。从图6可以看出，由4-3-3-3-4插值多项式得出的关节角度、角速度、角加速度图虽然连续，但在时间最优算法求解中，需要22维的矩阵求解以及数量相对多的速度、加速度约束条件，计算量较大。

(a)4-3-3-3-4时间最优关节角度

将交叉插值多项式和4-3-3-3-4的关节角度曲线表达式代入末端铲斗位姿的表达式中，可以得到末端笛卡儿空间的轨迹曲线图。挖掘路径对比图如图7所示，从中可以看出，交叉插值多项式与4-3-3-3-4插值多项式相比，其覆盖的路径点多，且更贴合实际路径曲线。为了更加准确地描述此特点，本文将规划路径均匀分为N个路径点，以每段规划路径点与实际位置的实时误差以及整段规划路径的平均误差作为指标进行比较分析，如式(13)所示，其中，Pos_thr(i)为第i个规划路径点的位置，Pos(i)为第i个路径点所对应的实际位置，error_real(i)为第i个规划路径点的实时误差，error_avg为整段规划路径的平均误差。

图7 挖掘路径对比图

error_real(i)=|Pos(i)-Pos_thr(i)|,

(13)

(14)

由式(13)可以分别计算出交叉插值多项式方法与4-3-3-3-4插值多项式方法的实时误差如图8所示。由图中可以看出交叉插值多项式方法与4-3-3-3-4插值多项式方法的实时误差最大值分别为0.428 5、0.580 1 m，本文方法的与规划路径的最大实时误差减少了26.1%。由式(14)可以分别计算出交叉插值多项式方法与4-3-3-3-4插值多项式方法的平均误差分别为0.197 3、0.281 7 m，本文方法的与规划路径的平均误差减少了30.0%。综上所述，本文方法比传统方法误差值小，更接近规划路径。

图8 交叉插值多项式与4-3-3-3-4多项式误差对比图

5 结论

根据挖掘机各液压缸长度范围以及构件长度算出三个关节角度范围值，利用粒子群算法得到各个末端位姿点到关节空间角度的转化值。本文中提出一种新型的交叉插值多项式轨迹函数，介绍了插值函数表达式的建立以及表达式中系数的求解方法。利用新型轨迹插值函数，建立时间最优轨迹规划优化问题，结合APSO算法给出优化问题的求解流程。给出算例，得到满足运动约束条件的最优时间，以及各关节角度、角速度及角加速度曲线图，并与4-3-3-3-4多项式方法对比，其最大误差减少26.1%，平均误差减少30.0%，说明此方法计算量小且末端轨迹更贴合实际轨迹。