智能挖掘机直线挖掘作业的运动规划与控制研究

张维刚，杨光，周维 ，彭晓燕

（湖南大学 机械与运载工程学院，湖南 长沙 410082）

挖掘机是一种广泛应用于露天开采、城市建设、交通运输等工程中的重要装备.随着基础设施建设的投入不断增加，挖掘机市场的需求大幅提升，操作人员的作业强度越来越大，作业任务日益繁重，特别是恶劣的工作环境给操作者的人身安全带来了极大的隐患［1-2］.

在平坡、挖沟等任务中［3］，经常需要对同一水平面进行高精度的直线挖掘作业，这就要求操作者能通过操纵挖掘机的两个手柄对其动臂、斗杆和铲斗3个关节实现精准的操控，因此，未经过专业训练的操作人员通常难以保质保量地完成任务［2］.智能挖掘机的出现为解决这一问题提供了很好的途径.挖掘机的智能化不仅可降低操作者的劳动强度、减少人力成本，还能显著提升挖掘机的作业精度和稳定性［4-5］.

国内外研究人员对直线挖掘过程中的运动控制问题进行了诸多富有成效的研究.Wang 等［6］提出了一种交叉耦合的预补偿算法，与非线性比例积分控制器结合，对液压挖掘机多关节协调控制，根据规划轨迹，跟踪误差可降低到2～9 cm；Feng 等［7］通过分析液压系统原理和电液比例控制系统数学模型，使用参数辨识与改进蚁群算法对PID 控制进行优化，提高了液压系统的跟踪精度；Wind 等［8］提出一种基于非线性模型预测控制的挖掘机直线轨迹生成与跟踪控制方法，通过确定状态约束和输入约束，实现最优控制，而且在Simscape 中建立了微型挖掘机物理仿真模型，验证了该算法的性能.在液压系统建模［9］方面，由于电液系统固有的非线性特性，辨识的模型往往阶次较高或无法准确地获得模型参数.上述方法中均假设系统无外界负载或轻载，且在工作速度不高的情况下进行试验，当加快跟踪速度或改变负载时，控制器达不到理想的控制精度，因此，传统的PID控制器无法满足挖掘机作业系统的高精度运动控制要求.

本文通过预测斗杆和铲斗运动，随动规划动臂目标角度，通过各执行器的协调运动提高铲斗齿尖在任务坐标系内的控制精度.并且将挖掘机各关节状态空间方程进行一阶惯性环节模型简化，利用递推最小二乘法对模型参数在线辨识，实时预测延迟时间内关节角度变化来补偿系统的大延迟特性，避免电液系统的非线性特性和系统延迟所导致的跟踪误差.

1 动臂关节角度实时规划

1.1 运动学方程

挖掘机的工作装置可抽象地简化为一个4 自由度的机械臂［10］，只有实现了各关节的运动规划，挖掘机的无人化作业才能实现.为此，就挖掘机工作装置展开运动学分析，求解铲斗齿尖位姿、工作装置各关节间夹角和驱动油缸之间转换关系的运动学表达式.挖掘机回转坐标系以及工作装置坐标系简图如图1所示.

图1 挖掘机回转坐标系以及工作装置坐标系简图Fig.1 Excavator rotation coordinate system and working device coordinate system diagram

挖掘机铲斗末端I位姿｛xI，yI，zI，θw｝包括铲斗齿尖I到回转坐标系的距离xI、yI、zI，铲斗与斗杆连接绞点O3到铲斗齿尖I的连线与水平面所构成的夹角θw，即铲斗关节的绝对角度；关节空间包括回转、动臂、斗杆、铲斗关节的相对角度｛θ0，θ1，θ2，θ3｝，即该关节相对于前一关节的摆角，逆时针方向为正.挖掘机工作装置的各关节参数如表1所示.

表1 挖掘机工作装置的各关节参数Tab.1 Joint parameters of excavator working device

考虑到挖掘机工作装置自由度不多，使用几何法求解挖掘机位姿空间到关节空间的转换.

首先计算动臂与斗杆连接点O2和斗杆与铲斗连接点O3相对于坐标原点O的水平距离和竖直距离，进而求得O1O3的直线距离.

构造动臂与斗杆关节组成的三角形△O1O2O3，使用余弦定理求得三角形内角，动臂关节角度为O1O3与x1轴的夹角加上内角∠O2O1O3：

斗杆关节角度为内角∠O1O2O3的补角，且运动方向为顺时针方向，故角度为负：

结合铲斗的绝对角度，可得铲斗的关节角度为：

同理，关节角度到位姿空间的运动学正解为：

1.2 目标角度实时规划

首先对智能挖掘机运动进行轨迹规划，完整的工作循环包括挖掘、满斗提升、回转、卸料、返回5 个步骤［11］.本文主要考虑直线挖掘时的运动规划与控制，即挖掘起始点位于挖掘机正前方.轨迹规划方法采用笛卡尔规划，即在笛卡尔空间规划末端轨迹，然后在每个插补点通过求解逆运动方程计算对应的每个关节的角度.与关节空间规划相比，规划函数生成的值是挖掘机铲斗齿尖位姿，能够满足直线挖掘的规划要求，保证工作装置能够严格按照预先规划的轨迹进行运动.

输入目标挖掘位姿｛x*，0，z*，θw*｝，挖掘总时间tf，挖掘总长度l，在直线上离散n个点，在挖掘起始点与结束点之间插补均匀，则k时刻挖掘长度为：

梯形速度插值函数将直线挖掘分为匀加速、匀速、匀减速3 个阶段，保证了速度的连续性，设定每个时间均为tf/3，初速度与末速度为0，则k时刻挖掘长度为：

对铲斗轨迹进行轨迹跟踪，通常在关节空间设计控制器，各关节单独跟踪规划的关节角度，只接收自身的误差做反馈控制，若某个执行器控制误差较大，即使其他执行器的控制精度很高，直线挖掘精度也难以保证.为了提高直线挖掘铲斗轨迹的控制精度，设计目标轨迹规划预补偿算法进行随动规划，当斗杆和铲斗角度产生跟踪误差时，动臂关节随动规划目标角度按照其他关节的控制情况来对自身的控制进行优化调整，最终通过多关节的协调控制来减小齿尖误差.目标角度规划流程图如图2所示.

图2 目标角度规划流程图Fig.2 Objective angle planning flow chart

由于系统延迟的影响，根据同一时刻的斗杆和铲斗角度计算的当前时刻的动臂角度虽然是理想规划值，却不能及时跟踪，导致直线度降低.为了动臂关节角度规划方法的实时性，本文在随动规划的基础上，设计规划算法的输入为斗杆和铲斗的预测值，对动臂目标角度提前规划，保证协同运动下动臂关节跟踪可以补偿其他关节产生的跟踪误差.动臂关节角度实时规划原理图如图3所示.

图3 动臂关节角度实时规划原理图Fig.3 Boom joint angle planning flow chart

由图3可知，首先，通过预测模型计算当前状态动臂、斗杆、铲斗的预测角度值θ1p、θ2p、θ3p，根据运动学方程求解目标挖掘深度z*下的动臂关节补偿量Δθ1*.

2 关节角度跟踪

2.1 关节动力学模型参数在线辨识

在智能挖掘机的运动控制中，由于液压系统的机电液强耦合性、时变等特点，室外作业下的动态特性复杂多变，难以精确建模；同时由于比例阀死区、机械结构间隙等系统存在延迟，因此通过带延迟的一阶惯性环节模型的简化与模型参数的在线辨识可以快速准确地表征系统动态特性，还可以适应挖掘机负载的变化.

构建的一阶惯性环节传递函数：

式中：控制指令U（s）为系统输入；V（s）为关节速度，作为系统输出；KP为系统当前控制指令总的放大倍数；T为动力学过程的时间常数；Td为系统的纯滞后时间；s为拉普拉斯变换的复变量.

利用Z变换，把式（10）变换成差分方程的模式，具体公式为：

式中：v（k）为k时刻的关节速度；Δt为采样时间；纯滞后系统下的输入u为k-1-Td时刻输入的控制指令.由于该系统特性下延迟时间Td为变量，随控制指令的变化而变化，因此，离线标定不同控制指令作用下关节的延迟时间为Td.标定过程与结果分别如图4和表2所示.

表2 各关节延迟时间Tab.2 The delay time of each joint

图4 各关节延迟时间随控制指令变化关系Fig.4 The delay time of each joint varies with the control

对式（11）进行变换，其最小二乘的形式为：

式中：φ为观测数据向量；δ为待估计的系数向量.

采用递推最小二乘法对关节运动模型参数增益KP和时间常数T进行在线辨识，过程如下：

式中：λ为遗忘因子，取值为0.95；P（k）为协方差矩阵；K（k）为增益矩阵.利用新观测数据对前次估计值修正得到新的参数估计值，即可得到该控制指令下对应的模型参数.

由于系统模型特性与输入有关，各个控制指令对应的参数特性不一样.因此，本文对各个控制指令下的参数T和KP单独进行在线辨识，以便提高模型精度，更好掌握挖掘机的动态特性.斗杆关节部分控制指令的辨识参数如表3所示.

表3 斗杆关节部分控制指令的辨识参数Tab.3 Identification parameters of control commands for stick joints

为了验证辨识参数的准确性，对延迟时间Td内控制指令下的关节角度变化进行预测，结果表明预测的结果与实际结果误差很小，带延迟的一阶惯性环节模型可以满足控制模型的需要，描绘出原系统的非线性特征.关节角度预测值及其误差如图5所示.

图5 关节角度预测值及其误差Fig.5 Predicted value and error of joint angle

2.2 基于模型预测的PID跟踪控制

本文中所用的基于预测的控制系统结构框图如图6 所示.算法分为两层结构，在预测模型中，通过当前状态下各关节控制指令u、挖掘机关节角度θ，根据在线辨识的各个控制指令对应的动力学参数，计算k时刻到k+Td时刻的关节角度变化；预测值的准确性取决于辨识参数的准确性，进而影响跟踪的精度［11］.

图6 基于预测的控制系统结构框图Fig.6 Block diagram of control system based on prediction

控制器以关节目标角度θ*与预测角度θp的差做反馈控制，发出控制指令u来跟踪规划角度，如式（18）所示，避免了时间延迟过大导致的跟踪误差，提高了控制精度.

式中：θ*、θp分别为k时刻目标关节角度向量和k+Td时刻的关节预测角度向量；u（k）为由误差e（k）计算出的控制指令向量；Kp、Ki、Kd分别为控制系统的各关节比例、积分、微分系数向量［12］.

3 搭建试验平台及仿真模型

3.1 智能挖掘机改造

为了验证控制算法，本文采用的实验机为三一SY245h 挖掘机，整机重25.5 t.首先在挖掘机的每个关节处安装角度传感器，用以测量动臂、斗杆及铲斗的关节角度，获得工作装置的位姿；为了实现挖掘机的无人操作，按照挖掘机手柄运动的自由度要求及操作力要求，设计一套机械夹爪机构来代替人工操作挖掘机，该机构由2 个六轴机械手驱动，可以推动操纵手柄到工作空间的指定位置；同时安装基于ROS 系统的主控制器与无线数据传输模块，编写挖掘机3 种工作模式的控制程序，即手动作业、远程作业和自主作业［13］.系统使用CAN 总线进行通信，包括关节角度传感器数据的读取与控制指令的发出，最后对挖掘机的定位控制精度进行测试，以满足控制系统的精度要求.挖掘机改造效果图如图7所示.

图7 挖掘机改造效果图Fig.7 Effect of excavator reconstruction

3.2 液压系统建模

挖掘机的液压系统是从复杂的电液多路阀和负载敏感系统下的变量泵为原理而设计的.AMESim作为高效、直观的系统建模和仿真分析工具，其流体、液压、机械等模型都经过严格的测试和实验验证，可以准确表征挖掘机液压系统的动态特性.本文通过在AMESim 中搭建虚拟样机，包括液压模型、关节机构模型、挖掘阻力模型等模拟推动挖掘机操作手柄到驱动油缸产生位移最终铲斗挖掘土堆的过程.挖掘机工作装置液压缸参数如表4所示.

表4 挖掘机工作装置液压缸参数Tab.4 Hydraulic cylinder parameters of excavator working device mm

SY245h 型挖掘机采用正流量变量泵，通过先导压力推动多路阀阀芯来改变各关节驱动油缸位移，因此，可以在AMESim 软件中设计各个控制指令对应的变量泵排量和主控制阀阀芯位移来模拟机械手到驱动油缸的液压控制系统，控制指令越大，对应的排量与换向阀开口越大，油缸速度越快.

搭建的挖掘机被控对象AMESim 模型如图8 所示.挖掘机作业时，控制器发出控制指令带动操纵杆行程，经过各个关节的液压回路推动各关节液压油缸进油或出油，最终实现挖掘机的直线挖掘.

图8 挖掘机AMESim模型Fig.8 AMESim model of excavator

为保障液压模型的准确性，在回路中加入局部子模型.先导式溢流阀模型设置在液压泵出口处，提供过载保护，保证液压元件的安全；限压补油回路设置在液压缸与换向阀之间，由一个限压阀和单向阀组成，在闭锁压力大于限压阀的调定压力时，作为卸荷阀打开，以保护液压元件和管路.挖掘负载子模型用来模拟铲斗进行土方挖掘的工程，同时计算挖土时铲斗受到的挖掘阻力.它运用土方运动机理，根据挖掘深度、切入角度、土壤密度及挖土量分别计算铲斗在x、y方向的挖掘阻力，用于模拟挖掘过程中铲斗负载的变化.具体建模原理见参考文献［14］.

3.3 仿真模型验证

为验证AMESim 模型的准确性，设计单关节运动试验与仿真结果进行对比.挖掘机接收控制器发出递减的控制指令分别控制动臂、斗杆、铲斗反复运动，角度传感器采集各关节角度变化，通过运动学方程求解驱动油缸的位移.在AMESim 模型中输入同样的控制指令，在对应的变量泵排量与换向阀阀芯位移的作用下输出各关节油缸位移，其仿真结果与试验结果的对比如图9所示.

图9 各关节单独运动油缸位移与仿真结果Fig.9 Displacement and simulation results of independent motion cylinder of each joint

4 控制算法验证

本文在Simulink 中建立包含目标角度实时规划的控制器，辨识模型参数与输出控制指令；建立油缸位移与关节角度转换的模型输出当前关节角度作为反馈；建立可变延时模块模拟机械手与液压系统导致的延迟时间特性；最终通过联合仿真验证控制算法［15］.

AMESim 与Simulink 交互模块包括3 个输入接口与4 个输出接口，输入为动臂、斗杆、铲斗关节的控制指令，输出为各关节液压缸位移与铲斗挖掘深度.Simulink 中通过运动学方程将液压缸行程转换为相应的关节角度、辨识模型参数、跟踪规划的目标关节角度；AMESim中挖掘机模型接收控制器发出的控制指令驱动各个液压缸的运动，输出各油缸位移，直线挖掘仿真效果图如图10 所示，挖掘机可以完成指定深度直线挖掘.

图10 直线挖掘仿真效果图Fig.10 Simulation rendering of linear mining

4.1 不同轨迹规划下运动轨迹跟踪的精度分析

设置直线挖掘起始位姿为｛8，0，-0.6，-85｝，挖掘起始位置x轴方向为8 m，铲斗切入角为45°，挖掘深度0.6 m，挖掘总长度为3 m，挖掘时间为12 s，直线轨迹规划分别采用匀速与梯形速度规划.匀速与梯形速度规划仿真关节角度与运动轨迹如图11所示.

图11 匀速与梯形速度规划仿真关节角度与运动轨迹Fig.11 Uniform and trapezoidal velocity planning simulation joint angle and motion trajectory

由图11 可知，动臂、斗杆关节角度跟踪误差在0.5°以内，铲斗关节的跟踪误差在1°以内，表明可以较好地补偿控制指令延迟的影响，并且动臂目标角度随动规划保证了铲斗齿尖深度始终在目标深度附近，直线度误差均在60 mm 以内，满足实际工作的要求.

分别采用各关节单独跟踪与随动策略规划动臂目标角度协同运动进行仿真试验，动臂随动策略下运动轨迹对比如图12 所示.挖掘初始阶段，斗杆和铲斗关节角度的跟踪误差较大，动臂再次规划后，由运动学正解可知，对动臂关节的跟踪可以补偿其他关节的控制误差，直线挖掘最终达到较好的跟踪精度.比较可知，本文所提出的规划方法下的跟踪精度明显提高，验证了随动策略的有效性.

图12 动臂随动策略下运动轨迹对比Fig.12 Comparison of motion trajectories under boom following strategy

4.2 不同挖掘深度下运动轨迹跟踪的精度分析

设置挖掘深度为0.4 m，更换仿真环境，不同挖掘深度下运动轨迹和挖掘阻力对比分别如图13 和图14 所示.当设置挖掘深度为0.6 m 时，挖掘阻力增大，并且挖掘深度越深，挖掘阻力越大，直线挖掘的误差依然较小.不同深度下同一控制指令的辨识参数不同，深度较深时增益KP较小，说明在线辨识可以适应对负载的变化，进而提高跟踪的控制精度.不同深度下斗杆关节部分控制指令的辨识参数如表5所示.

表5 不同深度下斗杆关节部分控制指令的辨识参数Tab.5 Identification parameters of control commands for stick joints at different depths

图13 不同挖掘深度下运动轨迹对比Fig.13 Comparison of motion trajectories under different excavation depths

图14 不同挖掘深度下挖掘阻力对比Fig.14 Comparison of excavation resistance under different excavation depths

4.3 不同速度下运动轨迹跟踪的精度分析

设置挖掘总时间tf为8 s，直线挖掘速度提升，关节角度跟踪效果与铲斗齿尖位移轨迹如图15所示.

图15 关节角度跟踪效果与铲斗齿尖位移轨迹Fig.15 Joint angle tracking effect and bucket tip displacement trajectory

挖掘时间减少，各关节目标速度提升，对应各关节控制指令增大，仿真结果显示各关节仍可以准确地跟踪目标角度，直线度误差依然在60 mm 以内，验证了算法的可靠性.

总体直线度的误差分为两部分，一部分是动臂随动规划不准确带来的误差，这是由于斗杆和铲斗预测值不准确，进而导致根据预测值计算的动臂目标值有误差；另一部分为跟踪动臂目标角度带来的误差，Kp、Ki、Kd控制参数存在优化空间.可以通过测定液压元件特性，对挖掘机电液伺服系统更高阶建模来提高预测精度进而提高控制精度［16］；或者通过采集大量实验数据，选用系统参数依赖度低的智能控制算法，有效处理挖掘机轨迹控制问题，实现动作的精确控制.

5 结论

本文对预测控制在液压挖掘机工作装置空间轨迹控制中的应用进行了研究，提出基于协同控制的轨迹预测控制方法.通过对动臂关节角度进行随动规划补偿斗杆与铲斗的运动误差，提高直线挖掘的精度；利用递推最小二乘法在线辨识挖掘机关节动力学参数，可避免因挖掘机工作负载不确定、室外作业液压系统参数变化范围大等原因造成的控制误差；控制器跟踪延迟时间内关节预测角度，避免延迟过大导致的跟踪误差，获得了较好的控制效果.为了验证控制算法，对挖掘机工作装置进行了智能化改造，在AMESim 软件中建立了挖掘机机械臂的机构模型和液压模型，实现了AMESim 与Simulink的联合仿真，并通过联合仿真验证了直线挖掘规划与控制算法的有效性，其具有较好的稳定性和鲁棒性，能够满足实际控制的需要.