基于轨迹规划的挖掘机遥控系统研究与开发

韩 斌 高公如 丁荣诚 林海波

（1.山东常林机械集团股份有限公司，临沂 276700；2.青岛理工大学 机械与汽车工程学院，青岛 266520）

挖掘机是经济建设中主要的工程机械之一。据中国工程机械工业协会统计，截至2017年底，工程机械主要设备保有量在690万～747万台，2018年挖掘机、装载机等工程机械产品销量均保持两位数增长，2020年各类挖掘机械产品同比增长39%。随着矿山开采、交通运输、港口建设和军事工程等对施工机械要求的不断提高，挖掘机行业得到了快速发展。目前，国内外的挖掘机大部分需要人工操作来完成作业，但是有些恶劣工作场景，如抢险救灾、矿藏开采等，工作环境复杂，常伴有强烈的振动、噪声和大量的尘埃，有时甚至面临辐射、爆炸、塌方以及有毒化学物等威胁，不适合人工操作。因此，遥控挖掘机的研究开发具有重要的应用价值。

国内外在遥控挖掘机方面进行了诸多研究，并取得了一些成果。20世纪70年代，德国卡尔斯鲁厄核研究中心研制并投入使用的12吨级牵引式挖掘机采用人工无线遥控方式；20世纪80年代，美国的Kraft TeleRobtics公司和John Deer公司等相继研发出遥控挖掘机，并进行了批量生产；日本的小松制作所对PC200R-2型液压挖掘机进行了改造，使其可以被遥控。同济大学于1986年开发了微机操纵的挖掘机试验台，开始了国内挖掘机控制方面的研究。近些年，遥控挖掘机发展迅速。2017年，QUANG H L等开发了一种基于头戴式显示器的1.5吨遥控挖掘机[1]；2018年，佘江雪等基于EPEC2024控制器直接驱动直线油门电机和电液比例减压阀组，实现了挖掘机发动机工作和行走装置的有效控制[2]；2019年，由三一、华为及跃薪智能等联合打造的全球第一台5G遥控挖掘机在上海世界移动通信大会正式亮相[3]；JOOSUNG L等将来自3D扫描仪、摄像机和传感器的数据结合在一起，以指导远程挖掘系统[4]；孙伟等提出了一种面向智能化矿用电铲的挖掘轨迹优化设计方法[5]；2020年，温时豪设计开发了一种基于无线通信的电动挖掘机远程遥控及监测系统[6]。

可见，作为使用量较多的传统工程机械，如何提高挖掘机的工作效率和保障工作安全，一直是研究的重点。本文针对遥控挖掘机操作的平顺性、可靠性以及安全性问题，从理论分析入手，对挖掘机动作进行分析建模，建立基于轨迹规划的动作控制模型，并在控制器中编程实现其控制算法。从通信协议、控制逻辑及界面显示等方面进行开发，以实现遥控挖掘机的安全、可靠工作。最后，对挖掘机遥控系统进行实验测试，以验证遥控系统的功能和性能。

1 建模仿真

在对挖掘机工作装置进行运动学和动力学分析的基础上，对挖掘机机械臂进行轨迹规划和控制，对处于作业状态的挖掘机各关节的平顺性进行建模 仿真。

1.1 建立运动坐标系

挖掘机的工作装置包含回转系统、动臂、斗杆和铲斗，是一个4自由度的结构。为表示挖掘机机械臂各连杆之间的关系，采用Denavit-Hartenberg（D-H）模型建立运动坐标系[7-9]，如图1所示。在此坐标系中，相邻的任意连杆在坐标系中的关系可用ai、αi、θi、di这4个参数表示。图1中各参数的定义如下：ai为zi轴与zi-1轴的交点在zi方向上到第i个坐标系原点的距离；αi为按右手规则zi从zi-1转到zi轴所转动的角度；θi为按右手规则zi-1从xi-1转到xi轴所转动的角度；di为zi轴到zi-1轴的距离；①和②表示的是两对平行直线；Linki-1和Linki表示的是连接杆；Axisi-1、Axisi和Axisi+1表示的是关节在坐标系中的位置。

1.2 正逆运动学分析

1.2.1 正运动学

从关节空间到位姿空间即为正运动学。通过控制4个关节角度，能够达到控制铲斗位姿的目的[10-12]。图2是挖掘机机械臂坐标系的简图。其中：θw表示挖掘机铲斗方向角；O0表示基坐标系；O1表示动臂关节坐标系；O2表示斗杆关节坐标系；O3表示铲斗关节坐标系；O4表示铲斗末端在基坐标系的位置。

通过上述连接关系可知，i=1Ti可表示为：

i=1Ti为从坐标系Oi-1的坐标系统变换到坐标系Oi的对应关系。

基坐标系相对于铲斗坐标系的坐标变换0T4为：

设挖掘机铲斗末端在此处坐标系的位置是(x,y,z,θw)，则通过坐标变换可得其位置点坐标和姿态角。挖掘机作业时，θ1通常不会发生改变，而θw是影响铲斗方向角的唯一因素，则从关节空间到位姿空间的映射可表示为：

1.2.2 逆运动学

从位姿空间到关节空间即为逆运动学，通常用几何法、代数法及迭代法求解逆运动学。因挖掘机结构相对简单，故采用几何法对其进行求解。挖掘机机械臂逆运动学计算示意图如图3所示。

求解步骤如下。

（1）求出θ1。

（2）求出将基坐标系原点平移后的机械臂铲斗末端坐标(x´,y´,z´)。

（3）求出基坐标系平移后O3的位置坐标(x3,y3,z3)。

（4）计算α、β、γ。

（5）计算θ2、θ3、θ4。

1.3 B样条曲线轨迹规划

通过传统B样条曲线算法得到的运动轨迹，曲线不经过所有的关节轨迹点，导致运行精度不高，因此需要对其进行改进[13-14]。

传统的B样条曲线表达式为：

式中：u为局部变量；Pi是坐标向量；Bi(u)是B样条基函数。

三次B样条基函数表达式为：

改进B样条，设已知P0、P1、P2、P3这4个型值点，如图4所示。过P1做P0P2的平行线，在这条线上取两点P1,0和P1,1，分别位于P1的两侧，并使其长度相等，等于m倍的P0P2。用同样的方法取P2,0和P2,1两点，使其平行于直线P1P3，长度为m倍的P1P3。将新得到的点当做新的型值点，于是曲线便会通过型值点P1和P2。当P1,0、P1,1和P1三点重合，P2,0、P2,1和P2三点重合，曲线通过P0和P3。

1.4 仿真分析

本文采用MATLAB的Robotic Toolbox工具箱进行轨迹规划的仿真分析，用运动学正解函数fkine求出由关节空间转换到位姿空间的变换矩阵，用ikine函数求运动学逆解算变换矩阵，用ctraj函数和jtraj函数实现笛卡尔空间轨迹规划和关节空间轨迹规划，用plot函数绘制挖掘机机械臂模型从初始位置到终止位置的轨迹规划运动。

ctraj的调用格式为：

其中：Tc是机械臂从起始点T0到终止点T1的笛卡尔轨迹规划；R是规划路径的距离向量，其取值范围为0～1；N是点的数量。

Jtraj的调用格式为：

其中：q为从状态q0到q1的关节空间轨迹规划；t是时间向量的长度；qd是指返回的轨迹规划的速度；qdd则是加速度。

得到的仿真曲线如图5～图8所示。通过位置、速度和加速度曲线可以看出，机械臂在运动过程中有较好的平顺性。

2 遥控系统开发

2.1 系统总体方案

本文设计的遥控挖掘机整体系统方案如图9所示。为提高系统安全性，该方案设计两个控制器局域网络（Controller Area Network，CAN），分别为由遥控接收器、车载显示器和车载控制器组成的CAN1，由车载控制器和发动机组成的CAN2。这样可将发动机与遥控接收器、车载显示器进行网络隔离，使外部命令必须通过控制策略过滤后才能控制发动机。系统通过对车载控制器的开发实现挖掘机的遥控化改造，同时在挖掘机中增加遥控、手动切换控制阀，实现在原有挖掘机操作基础上的远程控制。在遥控器上可以实现挖掘机的所有动作，并对挖掘机的发动机转速（油门）、工作模式（H、L、S）、行走模式、增压、大灯及喇叭等进行控制。遥控接收器通过CAN1与车载控制器连接，将遥控器命令输出给挖掘机，由车载控制器计算后发出控制命令，从而控制整车各种动作。车载控制器通过CAN2获取发动机运行参数进行发动机控制，并将参数通过CAN1发往车载显示器和遥控器。

遥控器采用先进无线电接收发射系统、大规模集成电路、先进的可编程控制器以及高精度信号处理技术，能够保证信号传输过程的抗干扰能力和设备的抗恶劣环境能力，以提高自身控制精度，满足设备的精细化施工。同时，采用汽车级控制器标准，在野外及复杂电磁环境下具有很强的安全性和可靠性，可满足各种控制需求。控制系统中集成双安全系统，芯片在发射机接收加密数据时，可保证数据的安全性。指令数据对应单一的接收发射设备，不同发射机及接收机互不干扰，同时其他无线电信号不会对该设备造成干扰。系统具有纠错检测功能，可自动识别错误代码。电器发射端可收集障碍信号，一旦出现紧急情况，可通过发射机直接传输到接收机端。遥控器端设有紧急停止按钮，可及时停止工作，保证安全。

2.2 软件开发

挖掘机控制系统软件主要包括遥控器软件、车载控制器软件和车载显示器软件等。驾驶员操作遥控器控制挖掘机动作的精度是否与实际手动操纵时一致，人机协调，障碍预警、故障报警及紧急停止等工况的控制反馈，以及软件系统运行逻辑的稳定性、抗干扰能力等，均为软件系统的关键。本文采用目前工业界成熟的CodeSys系统进行系统软件开发。图10是核心控制器的开发界面，主要负责完成命令收发、通信协议、IO控制、逻辑判断以及控制算法等功能。

挖掘机液压系统具有非线性、机电液强耦合性及时变等特点。执行复合动作时，液压系统参数变化范围大，挖掘阻力复杂多变，是造成挖掘机动作卡顿的主要原因。在前面建模仿真的基础上，车载控制器对轨迹规划控制算法进行了编程实现，以保证挖掘动作的平顺性。它的控制流程如图11所示。

通过编程实现车载显示器与遥控器显示屏及时获取挖掘机运行信息，如图12和图13所示，以监测挖掘机是否正常工作，确保挖掘机运行的安全性。在车载显示器端，除了能够显示挖掘机运行状态，还能够调节挖掘机运行参数，使挖掘机能够适应复杂的作业环境。车载显示器、遥控器接收器与车载控制器之间的传输方式为CAN总线传输。报文不包含源地址或目标地址，仅用标志符指示功能信息和优先级信息，以确保信息传输的实时性。同时，为了提高它的安全性，对通信协议进行了用户定义，并在程序中加入了权限和校验判断，以防止外部连接恶意接入。

3 试验结论

软件系统开发完成后，将整个系统安装在力士德SC210型挖掘机上进行调试测试。系统模块安装如 图14所示。其中，车载显示器安装在驾驶舱内，车载控制器安装在驾驶舱后部，位于座位后面的空舱内，遥控接收器安装在挖掘机驾驶舱顶部，以利于信号的收发。车载显示器、车载控制器、遥控接收器以及发动机通过CAN总线线束连接，车载遥控器和车载传感器通过硬线线束直接连接。

试验时遥控器与挖掘机的距离为80 m左右。表1为远程端发送数据，表2为车载端发送数据。

表1 远程端数据发送结果

表2 车载端数据发送结果

在对挖掘机工作装置进行动作试验时，使用的电液比例阀组为HZ-TJ01，线圈电流最大为800 mA，系统电压为24 V。遥控器手柄位置发生改变时，电液比例阀反馈电流发生线性变化，电流最大误差±3 mA。

4 结语

本文研究开发的挖掘机遥控系统，经过试验测试达到了设计要求。运行情况表明，开发的挖掘机遥控系统能通过远程遥控实现挖掘机的各种动作和多种作业方式控制，整机运行平顺、安全、可靠。智能化是未来挖掘机的发展方向，遥控挖掘机的研发为未来智能挖掘机的发展奠定了坚实基础，对挖掘机等工程机械信息化、数字化及智能化的研究具有重要意义。