液压挖掘机轨迹规划及运动仿真

胡海浪，殷晨波，周俊静，贾文华

(1. 南京工业大学 车辆与工程机械研究所，江苏 南京 211800; 2. 南京工程学院，江苏 南京 211167)

0 引言

液压挖掘机作为土石方施工机械，在各种工程建设领域所起到的作用越来越明显，在工程机械中占有十分重要的地位[1]。为实现挖掘机的智能化，提高挖掘机自助挖掘能力，需确定合理的挖掘作业轨迹规划。国内外学者围绕插值函数，提出诸多轨迹规划方法。如朱世强等7次样条曲线插值函数，该方法减振效果明显，但计算复杂，效率低[2]；刘凉等提出采用5次多项式插值法，能够保证加加速度平稳，但必须已知路径点速度及加速度[3]；Macfarlane提出最小脉动和脉动连续轨迹规划方法[4]。对于挖掘机工作装置的运动学分析，国内外学者一般采用作图法、矢量代数法和杆组分析法等。运动学仿真多采用MATLAB/SimMechanics、Pro/E和ADAMS等工具[5]，但其建模方法较为复杂。

考虑到挖掘机工作装置与机械手较为相似[6]，本文采用机械手运动仿真用的MATLAB/Robotics Toolbox工具箱对某21t级挖掘机工作装置进行轨迹规划及运动仿真，得出挖掘机工作装置姿态角、液压油缸长度以及铲斗齿尖位移变化曲线。

1 建立运动坐标系

为了描述相邻杆件之间平移和转动的关系，Denavit和Hartenberg提出了一种为关节链中的每一杆件建立附加坐标系的方法[7]。D-H矩阵法是为每个关节处的杆件建立四阶齐次变换矩阵，表示其与前一杆件坐标系的关系。本文运用D-H方法，在图1中建立挖掘机工作装置运动坐标系。

图1 挖掘机工作装置运动学坐标系

每个杆件分别用参数ai、θi、di、αi描述。其中ai表示zi-1轴与zi轴沿xi轴方向的距离；di表示xi+1轴与xi轴沿zi轴方向的距离；αi表示从轴zi到轴zi+1绕轴xi+1旋转的角度;θi表示从xi+1轴到xi轴绕zi旋转的角度。本文所选挖掘机结构参数如表1所示。

表1 SY215挖掘机工作装置D-H参数

2 运动学分析

2.1 正运动学

挖掘机工作装置具有4个自由度，需要在四维空间才能唯一表示出来。按选取描述变量不同可分为：以回转马达转角和三组液压缸长度所表示的驱动机构空间；回转平台与底座之间、动臂与回转平台之间、斗杆与动臂之间、铲斗与斗杆之间夹角所表示的关节空间；铲斗齿尖在基坐标系中的位置以及铲斗姿态角所表示的位姿空间。

如图1所示，建立挖掘机工作装置运动学坐标系。根据机器人运动学可知，空间任意坐标系中向量均可以通过齐次变换矩阵T转换到另一坐标系中，如坐标系i相对于坐标系i-1的变换矩阵[8]为：

(1)

将连杆坐标系变换矩阵相乘，可得铲斗齿尖的坐标系中相对于基坐标系的变换矩阵为:

(2)

其中：c0=cosθ0；c123=cos(θ1+θ2+θ3)；s0=sinθ0；s123=sin(θ1+θ2+θ3)，依次类推。

令P4为铲斗齿尖在坐标系O4x4y4z4中的坐标，则铲斗齿尖在极坐标系中坐标P0为:

(3)

(4)

铲斗姿态角β为:

β=θ1+θ2+θ3

(5)

由此便得出关节向量空间向铲斗位姿空间的正向变换。

不考虑挖掘机回转运动时，根据式(4)可得出如图2所示挖掘机工作装置包络图，铲斗齿尖理论上可以到达包络图边界内的所有区域。

图2 挖掘机工作装置包络图

2.2 逆运动学

对于运动学逆问题求解，主要有3种方法：迭代法、解析法和几何法。根据挖掘机工作装置的结构特性本文应用几何法逆向求解液压缸长度与挖掘机工作装置各关节角度的关系。

以铲斗液压缸为例，建立图3所示的铲斗结构简图。

图3 铲斗结构简图

图3中A为动臂与斗杆铰点；B为铲斗油缸与斗杆铰点；C为铲斗油缸、连杆和摇杆铰点；D为连杆与铲斗铰点；E为摇杆与斗杆铰点；F为铲斗与斗杆铰点；G为铲斗齿尖。则液压缸长度为：

(6)

∠BEC=∠BEF-∠CED-∠DEF

(7)

(8)

(9)

(10)

∠DFE=π-θ3-∠DFG-∠AFE

(11)

式中，λ3为铲斗油缸长度，θ3为铲斗关节角度，lBC为BC两点之间的距离，同理，其余各物理量几何参数如表2所示。

表2 铲斗逆运动学分析中各杆件参数

根据式(6)-式(11),可求得铲斗油缸长度与铲斗关节角的逆向运动学关系。同理可得动臂油缸长度λ1与动臂关节角度θ1、斗杆油缸长度λ2与斗杆关节角度θ2的对应关系，如图4所示。

图4 油缸长度与关节角度关系图

3 运动学仿真

3.1 Robotics Toolbox运动学建模

Robotics Toolbox是澳大利亚科学家Corke开发的基于MATLAB的机器人学工具箱，其提供了机器人学研究中许多函数，如正运动学、逆动力学和Jacobi矩阵。同时该工具箱还可以建立机器人杆件三维模型，并对其进行轨迹规划仿真[9]。

根据表1中挖掘机D-H参数，利用link([alpha A theta D sigma ,‘standard’])函数建立挖掘机工作装置运动学模型。其中，link函数中standard代表采用标准的D-H参数;参数alpha代表扭转角αi;参数A代表杆件长度ai;参数theta代表关节角θi;参数D代表横距di;参数sigma代表关节类型：0代表旋转关节，1代表移动关节。挖掘机4个关节均为旋转关节，所以本文中sigma均为0。

L0=link([pi/2,90,0,1877,0],'standard')；

%回转平台模型

L1=link([0,5700,0,0,0],'standard')；

%动臂关节模型

L2=link([0,2925,0,0,0],'standard')；

%斗杆关节模型

L3=link([0,1334,0,0,0],'standard')；

%铲斗关节模型

然后用link.qlim函数限定各关节角度变化范围，Li.qlim=[θimin,θimax],θimin与θimax分别表示表1中θi的最小角和最大角所对应的的弧度值。再用robot函数excavator=robot({L0L1L2L3})建立模型，运行drivebot(excavator)程序，得出如图5所示该挖掘机模型工作装置的运动调节界面和图6所示挖掘机工作装置三维视图。

图5 挖掘机工作装置运动调节界面

图6 挖掘机工作装置三维结构简图

本文不考虑挖掘机的回转运动，即令θ0恒等于0，基于此，为了观察更直观，可以用view(0,0)函数将图6三维视图转化为基坐标系xoz平面内的二维视图，用axis函数限定x轴与z轴坐标范围。

3.2 轨迹规划

挖掘机轨迹规划是根据具体的作业要求，计算出预期的运动轨迹以及各关节角度变化趋势。轨迹规划主要包括2种方法：点到点规划PTP(point to point)、连续路径规划CP(continuous path motion)。Robotics Toolbox提供的ctraj、jtraj和trinterp函数可以实现笛卡尔规划、关节空间规划和变换插值。

本文采用jtraj函数来实现挖掘机各关节连续路径规划仿真。首先选取轨迹规划起点与终点，起点选用最大挖掘高度A，终点选用最大挖掘深度B。使用view(0,0)将工作装置三维结构图转为xz平面二维结构简图，通过调节图5中关节角度滑块可以直观得出最大挖掘高度点与最大挖掘深度点对应的关节角变量，分别为qA=[0，1.033 2，-0.607 4，0.907 6]，qB=[0，-0.795 8，-0.775 2，0]。

使用plot命令进行运动仿真，命令如下:

t=[0:0.05:10] %产生时间向量

qA=[0,1.0332,-0.6074,0.9076]；

qB=[0,-0.7958,-0.7752,0]；

%输入起止点关节坐标.

[q qd qdd] =jtraj(qA,qB,t);

%轨迹规划命令

再运行程序plot(t,q(：，i))，i=2、3、4，就可得出各关节角度变化曲线，如图7所示。

根据已规划的关节角度轨迹以及逆运动学分析，可以在Matlab中求解出挖掘机各工作装置相应液压缸的长度变化曲线，如图8所示。

再根据轨迹规划好的角度变化，采用正运动学求解函数S=fkine(excavator，q)，可求得挖掘机模型铲斗齿尖在x、z方向上位移量，如图9所示。

图7 关节角度变化曲线

图8 油缸长度变化曲线

图9 铲斗齿尖在x、z方向的位移

4 仿真结果分析

通过调节图5中滑块位置，可以直观看出该型号挖掘机最大挖掘高度约为9.3 m，最大挖掘深度约为6.5 m，最大挖掘半径约为9.5 m，满足中型挖掘机作业空间要求，说明了该型号挖掘机工装装置尺寸参数设计的合理性。

在运动仿真过程中，从图6中观察到挖掘机铲斗由最大挖掘高度运动到最大挖掘深度时各个关节的运动情况(运动过程是动态的，文中无法显示这一过程)，由图7可以看出各个关节运动情况正常。图8得出在运动过程中，动臂油缸由3.15 m缩为1.87 m，斗杆油缸由2.075 m伸长到2.17 m，铲斗油缸由1.68 m伸长到2.08 m，油缸伸缩速度稳定。图9得出挖掘机模型末端在作业空间内位置变化平缓，且在运动起始和终止位置速度和角加速度均趋向于零，理论上没有较大的冲击。

5 结语

1) 在机器人理论基础上，对某21t级中型挖掘机进行了正、逆运动学分析。在MATLAB 2010a环境下，运用Robotics Toolbox编写程序语句，对挖掘机工作装置进行运动学建模和作业空间测定，该方法建模简单，可直观获取挖掘机作业空间。

2) 对挖掘机工作装置进行点到点轨迹规划，得出各关节姿态角、油缸长度以及铲斗齿尖平滑连续的运动曲线。

3) 运动仿真结果表明所得出的挖掘机各关节运动速度和角加速度变化平稳，油缸伸缩速度稳定，铲斗齿尖运动变化平缓，挖掘机在工作过程中没有较大的冲击。验证了该型号挖掘机各关键部位杆件参数设置的合理性，为挖掘机轨迹规划和运动学仿真提供了一种简便的方法，并为挖掘机自主挖掘研究奠定了运动学基础。

参考文献:

[1] 张启君,刘伟.国内挖掘机行业发展的探讨[J]. 筑路机械与施工机械化, 2005,22(2):1-6.

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

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

[4] Macfarlane S,Croft E.Jerk-bounded manipulator trajectory planning: design for real-time applications[J]. IEEE Transactions on Robotics & Automation, 2003, 19(1):42-52.

[5] 马肖丽,周志鸿. 基于ADAMS的液压挖掘机工作装置的运动学仿真[J]. 建筑机械, 2011(5):102-105.

[6] 黄德银,严世榕,许锁林. 基于Robotics Toolbox挖掘机工作装置的运动仿真[J]. 建筑机械,2013(17):91-94.

[7] 陈支,邹树梁,唐德文,等. D-H坐标系下挖掘机工作装置运动学建模与仿真[J]. 机械设计与制造, 2014(11):188-190.

[8] 蔡自兴. 机器人学[M]. 北京：清华大学出版社，2009.

[9] 左富勇,胡小平,谢珂,等. 基于MATLAB Robotics工具箱的SCARA机器人轨迹规划与仿真[J]. 湖南科技大学学报(自然科学版),2012,27(2):41-44.