自主挖掘的平滑连续特性轨迹规划研究

李海虹,林贞国

(太原科技大学 机械工程学院,山西 太原 030024)



自主挖掘的平滑连续特性轨迹规划研究

李海虹,林贞国

(太原科技大学 机械工程学院,山西 太原 030024)

为保证自主挖掘过程的平稳连续,通过对比选取了一种合理的轨迹规划方法.以某型挖掘机为研究对象,在笛卡尔坐标空间对一次作业路径进行规划.在关节空间对多个路径控制点在不同条件下分别采用n次多项式、傅里叶级数拟合法、分段多项式插值法进行轨迹规划;通过MATLAB仿真平台得到的各关节的关节角、角速度和角加速度的变化曲线是平滑连续的;对于函数拟合法与分段多项式插值法进行了动力学特性比较,结果表明,函数拟合法的轨迹规划更加合理,它可以使机构工作更加平稳连续,对机构的振动冲击强度也较低,能很好地满足实际工作中自主挖掘的要求.

挖掘机; 轨迹规划;n次多项式拟合; 傅里叶级数拟合; 分段多项式插值

挖掘机自主作业应用非常广泛,如核辐射区域、深海采集地,实现挖掘机自动化作业是非常必要的,因此必须解决挖掘机的轨迹规划问题.

国内外学者提出了多种轨迹规划的方法,KIM 等提出一种时间最短轨迹规划方法,这种方法是基于关节空间的,并考虑了包括动力学约束在内的各种实际约束条件,但这种方法较复杂,且只能离线完成;后来LIN 等提出了规划机器人连续运动轨迹的3次样条函数方法,它可以实现优化的关节规律,但当路径点多时,需要已知条件过多,计算量非常大;徐向荣等提出了多种不同条件下的3-5-3,4-3-4,3-3-3等多项式轨迹规方法[1],但是前两类由于分段过少,容易使得规划的轨迹曲线偏差较大,第3种分段方式,规划的轨迹不具有对称性,轨迹末端阶跃较大;刘凉等采用5次多项式插值法,尽管能够保证加速度连续,但是限制条件较多,包括中间路径点的速度和加速度都必须已知[2-3];朱世强等提出的7次样条曲线插值方法,能够避免振动问题,但是7次样条曲线计算复杂,效率比较低[4];赵云等对于电动式挖掘机采用对数螺旋线理论进行挖掘轨迹的研究[5];管成等提出了5次NURBS 曲线时间最优轨迹规划[6],它使得关节曲线的加速度连续,并且通过权因子调整挖掘轨迹.但是NURBS曲线的求导计算式比较繁琐,编程复杂[7].

本文以某型挖掘机为研究对象,规划了一次挖掘任务的空间挖掘轨迹,在关节空间对自主挖掘过程分别进行了5次多项式、2阶傅里叶级数拟合和分段多项式插值,并对轨迹规划结果中的动力特性进行了比较,选择综合性能较优的轨迹规划方法.

1 挖掘机工作装置

挖掘机在工作过程中,回转平台、动臂、斗杆和铲斗的3个油缸进行复合运动,使铲斗末端沿着预定的路径运动,完成其工作任务.挖掘机的工作装置如图1所示.其工作机构中共有11个铰点,即A,B,C,D,E,F,G,H,K,N,Q.

图1 挖掘机工作装置图Fig.1 Excavator working device

2 D-H坐标系下工作装置的运动学分析

2.1D-H坐标系下的工作装置

在挖掘机的轨迹规划中,为求运动学逆解,依据机器人学理论,在D-H坐标系下建立挖掘机工作装置[8],如图2所示.其中θ1为回转关节变量;θ2为动臂关节变量;θ3为斗杆关节变量;θ4为铲斗关节变量.

2.2 运动学的逆解

挖掘任务中挖掘控制点是在笛卡尔坐标系中描述的,而轨迹规划是在关节空间中进行的,所以必须将挖掘控制点的位姿坐标转换为关节空间各关节的转角,通过运动学的逆解来实现这一过程[8].

令铲斗末端V点的坐标为(x,y,z),Q点的坐标为(xq,yq,zq),α,β,γ分别表示CF,CQ和CV与水平面的夹角,则有

图2 D-H坐标系下挖掘机工作装置简图Fig.2 D-H coordinate system of excavator workingdevice

(1)

式中:a1为回转中心与动力臂铰点之间的距离;a2为动臂连杆长度;a3为斗杆连杆长度；d1为底座回转平台中心的距离；ζ=θ2+θ3+θ4.

(2)

式中:a4为铲斗长度.

3 笛卡尔坐标系下的空间挖掘路径规划

挖掘机通过铲斗铲入土壤、拖拽和铲斗回旋出土3个步骤自主完成梯形深坑挖掘任务[9].在这3个步骤中,由3个关节角的复合运动来完成挖掘动作:由动臂关节运动来完成提升过程;接着回转装置旋转到指定卸载位置,通过铲斗关节变化来完成卸载;然后由3个关节的复合运动到达下一个指定挖掘点,再继续作业.

挖掘机完成一次挖掘任务的空间路径控制点坐标与挖掘机的工作参数有关[8],本文中规划的挖掘控制点坐标如表1所示.通过空间样条曲线插值完成一次挖掘作业的空间轨迹,如图3所示.

图3 挖掘机空间挖掘轨迹曲线Fig.3 Excavator space trajectory curve

4 关节空间自主挖掘过程的轨迹规划

挖掘机工作过程中,铲斗的运动(提升、回转、以及运行至下一起始点)属于空间点到点的运动,依据初始位置、速度、加速度等已知条件及连续性条件,采用5次多项式插值法规划轨迹,这样比较简单.挖掘轨迹规划属于连续路径规划,由起始点、终止点以及挖掘中间控制点组成挖掘路径,将该路径轨迹分为5段:铲入、回旋、拖拽、回旋和铲出[11].设y=0,则xz平面内自主挖掘过程的轨迹曲线如图4所示.

图4 xz平面内自主挖掘过程的轨迹曲线Fig.4 Process of self excavation trajectory curve within xz plan

在关节空间,多路径点处的关节角随着工作时间的变化形成一个关节序列,此关节序列通过多项式函数拟合成一条光滑的函数,得到挖掘机的轨迹;或者在每两个路径点之间采用多项式插值法,通过求1阶导数及2阶导数获得速度、加速度特性,得到轨迹规划的结果.在本文中多路径点的轨迹需根据路径中控制点的点数来选取合理的多项式形式,进行拟合与插值.

表1 控制点从位姿空间到关节空间的转换

4.1n次多项式拟合法的轨迹规划

由于高阶多项式及其1阶、2阶导数平滑连续的特性,对于过多路径点的轨迹规划,采用多项式来进行拟合,设多项式通式为

(3)

式中:p(t)为随时时间t变化的关节变量；n为路径点数p(t)为随时时间t变化的关节变量.

本文中n=6,则挖掘机各关节点的多项式为

pi(t)=p5it5+p4it4+p3it3+p2it2+p1it1+p0i

(4)

其中i=1,2,3.通过对光滑多项式分别求1阶、2阶导数,得到各关节的速度、加速度公式为

(5)

(6)

式中:v(t),a(t)分别为关节的速度、加速度函数.

通过各路径点的时间与表1中各关节点处的关节角,求得多项式的各项系数,从而得到关节角随时间变化的拟合光滑函数.通过MATLAB仿真得到各关节的轨迹规划结果,如图5—7 所示.

图5 动臂关节角、角速度、角加速度曲线图Fig.5 Boom joint angle,angular velocity,angular acceleration curve

图6 斗杆关节角、角速度、角加速度曲线图Fig.6 Arm joint angle,angular velocity,angular acceleration curve

图7 铲斗关节角、角速度、角加速度曲线图Fig.7 Bucket joint angle,angular velocity,angular acceleration curve

4.2 傅里叶级数拟合法轨迹规划

傅里叶变换能将满足一定条件的某个函数表示成三角函数(正弦函数或余弦函数)或者它们的积分的线性组合.在不同的研究领域,傅里叶变换具有多种不同的变体形式,如连续傅里叶变换和离散傅里叶变换.傅里叶变换的基本性质决定了它是一种很好的函数拟合工具.

当通过路径点数为偶数点时,我们选取傅里叶函数在关节空间进行拟合光滑的轨迹曲线,设其通式为

(7)

式中:ai,bi为函数f(t)傅里叶系数;w为角频率；n=2m,m≥2;n为路径点数.

本文中n=6,所以其傅里叶拟合多项式为

(8)

分别对多项式求1阶、2阶导数得到各关节的速度、加速度公式:

2a2wsin(2wt)+ 2b2wcos(2wt)

(9)

4a2w2sin(2wt)-2b2w2sin(2wt)

(10)

通过各路径点的时间与表1中各关节点处的关节角,求得级数的各个未知系数,从而拟合求得关节角随时间变化的光滑函数.通过MATLAB仿真得到各关节的轨迹规划结果,如图8—10 所示.

图8 动臂关节角、角速度、角加速度曲线图Fig.8 Boom joint angle,angular velocity, angularaccelerationcurve

图9 斗杆关节角、角速度、角加速度曲线图Fig.9 Arm joint angle,angular velocity,angular acceleration curve

图10 铲斗关节角、角速度、角加速度曲线图Fig.10 Bucket joint angle,angular velocity,angular acceleration curve

4.3 分段多项式插值法轨迹规划

对于过多路径点的工作过程,采用合理分段点的轨迹规划方式,使得实际工作路径与规划路径偏差较小,轨迹、速度、加速度曲线平滑连续.本文采用了5段3-3-3-3-3高阶多项式进行轨迹规划.

设挖掘机任一关节的轨迹多项式为

(11)

其中ai0,ai1，ai2，ai3(i=1,2,3,4,5)表示挖掘任一关节在第i段路径的轨迹多项式是系数.

对轨迹多项式中的实际时间变量τ求1阶和2阶导数,得到各关节速度和加速度为

(12)

(13)

通过各关节在路径点处的关节角度值、初始速度、加速度等已知条件,以及位置、速度、加速度连续性条件,可以求得多项式的每个系数.通过MATLAB仿真软件得到动臂、斗杆、铲斗各关节的轨迹规划结果,如图11—13所示.

图11 动臂关节角、角速度、角加速度曲线图Fig.11 Boom joint angle,angular velocity,angular acceleration curve

图12 斗杆关节角、角速度、角加速度曲线图Fig.12 Arm joint angle,angular velocity,angular acceleration curve

图13 铲斗关节角、角速度、角加速度曲线图Fig.13 Bucket joint angle,angular velocity,angular acceleration curve

将以上3种轨迹规划结果中(图5,8,11;图6,9,12;图7,10,13)的各关节轨迹规划结果进行对比,得到的结果如表2所示.

从表2中数据对比可知,用傅里叶拟合与用5次多项式拟合的各关节角变化曲线平缓,阶跃比较小,而用3-3-3-3-3分段插值法规划的轨迹,各关节角的变化曲线变化范围比较大,阶跃比较大,因此5次多项式拟合法与傅里叶拟合法的轨迹规划的效果明显优于分段多项式插值法,因此用此法规划挖掘机的运动轨迹,可使挖掘机关节变化平稳连续,各工作装置所受的振动冲击较小,可以很好地实现自主挖掘功能.

5 结语

本文结合挖掘机实际工作参数,通过3次样条函数插值法在笛卡尔坐标系得到挖掘机空间挖掘的轨迹曲线,在关节空间对自主挖掘工作过程分别采用n次多项式、傅里叶级数拟合法和分段多项式插值法进行轨迹规划,通过对规划结果的动力学特性指标进行比较,得出n次多项式、傅里叶级数拟合法轨迹规划比分段多项式插值法效果更好,因此用此法规划挖掘机的运动轨迹,可使挖掘机关节变化平缓连续,各工作装置所受的振动冲击较小.另外,n次多项式适用于任何控制点数,而傅里叶级数则只适用于控制点数为偶数时的情况,所以必须根据实际情况来选用这两种方法.

表2 不同轨迹规划方法的动态特性指标对比

[1] 徐向荣,马香峰.机器人运动轨迹规划分析与算法[J].机器人,1988,2(6):18-24.

XU Xiangrong,MA Xiangfeng.Analysis and alorithm for trajectory planning of robot manipulators [J]Robotics,1988,2(6):18-24.

[2] 刘凉,陈超英.考虑关节摩擦的并联机器人平滑轨迹规划[J].机械工程学报,2014,50(19):9-15.

LIU Liang,CHEN Chaoying.Smooth trajectory planning for parallel manipulator with joint friction torque[J].Journal of Mechanical Engineering,2014,50(19):9-15.

[3] 黎波,严骏,郭刚,等.挖掘机工作装置复杂轨迹跟踪实验[J].农业机械学报,2012,43(12):39-43.

LI Bo,YAN Jun,GUO Gang,et al.Experiment of complex path tracking for excavator working unit [J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(12):39-43.

[4] 朱世强,刘松国.机械手时间最优脉动连续轨迹规划算法[J].机械工程学报,2010,46(3):47-52.

ZHU Shiqiang,LIU Songguo.Time-optimal and Jerk-continuous trajectory planning algorithm for Manipulators[J].Journal of Mechanical Engineering,2010,46(3):47-52.

[5] 赵云.电动式挖掘机动力学与轨迹规划理论的研究[D].沈阳:东北大学,2007.

ZHAO Yun.Invefigation of dynamics and trck design for eletronic excator[D].Shenyang:Northeastern Universty,2007.

[6] 管成,张登雨.基于NURBS的挖掘机器人时间最优轨迹规划[J].吉林大学学报:工学版,2015,45(2):540-546.

GUAN Cheng,ZHANG Dengyu.NURBS-basedtime-optimal trajectory planning on robotic excavators[J].Journal of Jilin University：Engineering and Technology,2015,45(2):540-546.

[7] SARAVANAN R,RAMABALAN S,BALAMURUGAN C.Multiobjective trajectory planner for industrial robots with payload constraints[J].Robotica,2008,26(6):753-765.

[8] 宋军,贺继林.液压挖掘机自主挖掘技术研究[D].长沙:中南大学,2013.

SONG Jun,HE Jilin.Self-mining technology of hydraulic excavator[D].Changsha:Central South University,2013.

[9] 邵辉,胡伟石.自动挖掘机的动作规划[J].控制工程.2012,19(2):594-597

SHAO Hui,HU Weishi.Action planning of automatic excavator[J].Control Engineering of China,2012,19(2):594-597.

[10] 陈小立,严宏志.基于遗传算法的四自由度混联机器人轨迹规划[J]计算机仿真,2014,31(5)346-350.

CHEN Xiaoli,YAN Hongzhi.Trajectory planning of four DOF hybrid robot based on genetic algorithm [J].Computer Simulation,2014,31(5):346-350.

Smooth continuous track planning on autonomous mining

LI Hai-hong,LIN Zhen-guo

(School of Mechanical Engineering ,Taiyuan University of Science and Technology,Taiyuan 030024 ,China)

In order to ensure the smoothness and continuity of autonomous mining,a rational track planning method is comparatively selected for a specific excavator.By planning a job path in Cartesian coordinate space,the track planning is conducted on joint space via n-degree polynomial,Fourier series fitting and piecewise polynomial interpolation under different path control points and conditions.To this end,the curves of joint angle,angular velocity and acceleration are smooth and continuous.In comparison with dynamic properties of functional fitting and piecewise polynomial interpolation,it is found that the mechanism via functional fitting works more smoothly and continuously with lower impact strength so as to meet autonomous mining demands.

excavator; track planning; n-degree polynomial fitting; fourier fitting; piecewise polynomial

国家自然科学基金资助项目(51541501);山西省青年科技研究基金(2010021021-2)

李海虹，副教授，博士. E-mail:lihaihongty@sina.com

TP 29

A

1672-5581(2016)02-0093-06