基于NURBS的挖掘机自主控制铲斗轨迹规划方法

刘 宇，刘春时，张义民

（东北大学 机械工程与自动化学院，辽宁 沈阳 110819）

目前，工程机械机器人化是国内外学者的一个研究热点问题.其主要目的是使工程机械可以在危险、恶劣作业环境下工作，如开采辐射性矿物，高温锅炉除渣，清除山体滑坡等，以保障驾驶员健康、生命不受到威胁和损伤.液压挖掘机是经济建设中最常用的工程机械之一，国内外学者针对挖掘机的机器人化进行了多方面的研究［1－2］.

挖掘机的轨迹规划是根据挖掘机作业任务设计合适的机械臂运行轨迹.往往作业任务只是一些关键点的数据，轨迹规划则需要依据这些数据设计出合适的运行轨迹来.本文提出一种基于非均匀有理B样条的轨迹规划方法来设计运行轨迹.

1 挖掘机机构简图

常用的反铲式液压挖掘机结构如图1所示，由行走装置、回转装置、动臂、斗杆和铲斗组成.每个杆件的运动是由液压阀控制相应的液压缸驱动杆件实现的.挖掘机机器人化是通过使用电液比例阀替换传统的手动换向阀，实现挖掘机的计算机对挖掘机的控制.控制系统根据传感器反馈信息对环境进行识别.作为挖掘机的“大脑”，智能控制模块实现工作任务分解.一般控制系统分为上层规划决策层和底层控制实现两级.上层是一个人工智能系统，而底层则是一个位置控制系统.

2 挖掘机的轨迹规划

图1挖掘机机构简图Fig.1 Excavtor mechanism

图2 挖掘机挖掘工作方式分类Fig.2 Working manner of excavtor

2.1 作业任务分解

反铲挖掘机的一个作业循环包括挖掘、回转、卸料、返回4个过程.挖掘的工作方式可分为沟端挖掘、沟侧挖掘、直线挖掘、曲线挖掘、一定角度挖掘、超深沟挖掘、沟坡挖掘等［3］.可以将挖掘机的挖掘过程分为以下4类，如图2所示.本文所指的轨迹规划是根据作业任务设计出铲斗尖处的轨迹.通过实时运动学解算后可以得到各个行走装置，以及各个关节角度的大小.

2.2 NURBS曲线定义

初始化图形交换规范（IGES）规定 NURBS表达为［4－5］

式中：C（u）为参数曲线；n为控制点个数减1；Ni，k（u）为由节点矢量U定义的k次B样条基函数，u为［a，b］范围内的控制参数；ωi为权重；Pi为为控制点.

定义在节点矢量U上的k次基函数递推定义为

式中：ui为节点.

因为3次NURBS曲线2次导数连续，轨迹的1，2阶导数分别是速度和加速度.因此采用3次NURBS曲线来规划挖掘机轨迹可以实现生成的指令速度和加速度连续.

2.3 基于NURBS曲线的挖掘机轨迹规划

如果按照图2中箭头指示的轨迹运行，存在以下问题：①轨迹不连续，存在速度、加速度突变；②没有可调参数，不易进行轨量调节，不利于挖掘机机器人化的实现.因此，拟利用NURBS曲线具有n－1次（n是曲线的次数）可导，而且每个控制点都具有权重调节因子的特性对挖掘机轨迹进行规划.

根据作业要求进行轨迹规划步骤如下：

（1）选择控制点.一般选择起点、终点作为首末控制点，然后选择曲线关键点即可.

为了采用3次NURBS曲线规划轨迹，对于图2b中以深度h挖掘长度l可以直接选择矩形的4个顶点作为控制点.图2c和图2d中以角度θ向下或向上挖掘长度l需要选择增加一个重复控制点，该重复控制点与挖掘最低点重合.

（2）确定各点权重因子矢量.权重因子是相对而言的，一般设定起点和终点的权重因子为1，然后设定各控制点的权重因子.设定的依据是，要求轨迹尽量接近该控制点，则增大权重因子，反之，减小权重因子.

（3）确定节点矢量.节点矢量的确定可以按照式（3）进行［6］：

一般情况下，令a＝0，b＝1.这样设定节点矢量的目的是保证生成的NURBS曲线通过首末控制点.

2.4 两种离散轨迹的方法

2.4.1 等步长法

等步长参数是指当获得了控制点、权重矢量、节点矢量之后，按照平均将参数u等分，以此方法生成参数序列，然后由式（1）求得轨迹点的方法.该方法的特点是运算简单，适合由单片微处理器组成的控制系统进行处理，但是该方法生成的轨迹点在起始和终止段间距较大，生成的速度曲线和加速度曲线有波动.

2.4.2 一阶泰勒近似方法

在进行NURBS曲线插补模块之前，需要进行预运算，进行轨迹规划.轨迹规划包括求NURBS曲线的长度，根据指令速度进行速度规划，常用的方法有梯形速度规划、S型速度规划等.NURBS曲线插补的算法流程图如图1所示.

不失一般性，参数曲线可以表示为

式中：u为样条控制参数；i，j，k为三维方向矢量.

沿着曲线的进给速度为

可以得到

参数曲线C（u）是u的函数，参数u是时间t的函数.由当前参数ui以及u对时间的一、二阶导数和插补周期T，即可利用泰勒展开公式计算出下一个采样点的参数ui＋1，代入式（1）得到下一插补点C（ui）.利用泰勒公式求参数为

式中：O（t3）为时间t的三次余项.

由式（6）和式（7）可得到一阶、二阶NURBS插补公式为［4］

3 完整控制结构

挖掘机轨迹控制结构分为上层规划系统和底层控制系统，如图3所示.上层控制系统是具有人工智能的任务规划期，任务规划完成后生成铲斗目标轨迹曲线［7－10］.底层控制系统建立工作装置的动力学模型，将挖掘机各组件和关节的运动与液压系统的输出力相结合，将轨迹指令转化为驱动电液阀的电压指令.

图3 机器人化挖掘机控制结构Fig.3 Control system structure of robotic excavtor

4 仿真与实验研究

挖掘机机器人化控制系统采用工业控制机，通过USB口扩展CAN总线与外部设备相连.控制软件采用VB6.0编写.

以深度为400mm，挖掘长度为800mm的挖掘如图4所示.挖掘轨迹是由关键点（400mm，0），（400mm，－300mm），（－400mm，－300mm），（－400mm，0）组成的控制多边形内部的光滑曲线组成.权重因子分别为1，10，10，1.

以角度为30°，长度为400mm的挖掘如图5所示.关键点为（400mm，0），（－230mm，0），（－230mm，0），（0，0），权重因子ω设置不同值.

在挖掘机作业过程中，对于铲斗的轨迹要求能够进行调整.调整轨迹可以通过修改控制点的权重因子来实现.图5是关键点为（－230mm，0），权重因子分别为10，3，1时的轨迹曲线，可以看出，权重因子越大，轨迹距离控制点越近.

为了进行计算机控制，将图4，5中的挖掘轨迹采用一阶泰勒展开方法进行离散化，采样周期为50ms，插补误差控制为1mm，离散后的位移曲线如图6所示，速度曲线如图7所示.轨迹规划结果与理想轨迹相比较，误差如图8所示，轨迹误差很小，说明当采样周期相对较小时，该轨迹规划方法误差可以控制得非常小.

图4 挖掘实例1Fig.4 Diging case 1

5 结论

（1）利用NURBS曲线将挖掘机作业任务进行轨迹规划，具有轨迹连续，各关节无速度、加速度突变而且可以进行轨迹调节，适合挖掘机机器人化控制.

（2）通过调节关键点的权重因子可以改变挖掘轨迹.

（3）该轨迹规划方法产生的误差只有微米数量级.

［1］LEE S U，CHANG P H.Control of a heavy-duty robotic excavator using time delay control with integral sliding surface［J］.Control Engineering Practice，2002，10（7）：697－711.

［2］张海涛，何清华，张新海，等.机器人液压挖掘机运动系统的建模与控制［J］.机器人，2005，27（2）：113－117.

ZHANG Haitao，HE Qinghua，ZHANG Xinhai，et al.Modeling and control of the motion system of a hydraulic robotic excavator［J］.Robot，2005，27（2）：113－117.

［3］SHI Xiaobo，WANG Feiyue，LEVER P J A.Experimental results of robotic excavation using fuzzy behavior control［J］.Control Engineering Practice，1996，4（2）：145－152.

［4］刘宇，赵波，戴丽，等.基于传动系统动力学的 NURBS曲线插补算法研究［J］.机械工程学报，2009，22（1）：181－192.

LIU Yu，ZHAO Bo，DAI Li，et al.Study on a feedrate system dynamics based interpolator for nurbs curve［J］.Chinese Journal of Mechanical Engineering，2009，22（1）：181－192.

［5］刘宇，戴丽，刘杰，等.泰勒展开 NURBS曲线插补算法研究［J］.东北大学学报：自然科学版，2009，29（1）：117－120.

LIU Yu，DAI Li，LIU jie，et al.Study on nurbs interpolator using taylor expansion method［J］.Jonrnal of Northeastern University ：Natural Science，2009，29（1）：117－120.

［6］PIEGL L，TILLER W.The NURBS book［M］.Berlin：Springer，1997.

［7］TOMI M，KELERVO N，RAUNO H.A 3Dmodel based control of an excavator［J］.Automation in Space Research，2006，15（5）：571－577.

［8］HA Q P，NAGUYEN Q H，RYE D C，et al.Impedance control of a hydraulically actuated robotic excavator［J］.Automation in Construction，2000，9（5－6）：421－435.

［9］FOX B，JENNINGS L S，ZOMAYA A Y.On the modeling of actuator dynamics and the computation of prescribed trajectories［J］.Computers ＆Structure，2002，80（7－8）：605－614.

［10］BRADLEY D A，SEWARD D W，MANN J E，et al.Artificial intelligence in the control and operation of construction plant—the autonomous robot excavator［J］.Automation in Construction，1993，2（3）：217－228.