CN104476548A - 一种挖掘机器人自主作业控制系统及方法 - Google Patents

Abstract

本发明提供了一种挖掘机器人自主作业控制系统及方法，所述的系统中的主控计算机与立体激光雷达、操控盒、位置传感器和下位机控制器相连接，下位机控制器与驱动装置、位置传感器和压力传感器相连接。采用本发明能够实现挖掘机的无人自主挖掘作业，满足恶劣环境及不适宜有人参与情况下挖掘作业需求。本发明的智能化程度高、人机交互性好、运行稳定且位置重复精度高。

Description

一种挖掘机器人自主作业控制系统及方法

技术领域

本发明涉及一种挖掘机器人自主作业控制系统及方法，采用本发明能够选择遥控操作、示教再现和自主作业三种不同的工作模式，实现对挖掘机器人的遥控操作，示教再现作业以及通过完全自主的方式进行挖掘作业。

背景技术

智能挖掘机是功能最典型、结构最复杂、用途最广泛的工程机械之一。而无人自主挖掘机器人则代表着智能挖掘机的最高水平。它在工业与民用建筑、交通运输、水利电力工程、矿山采掘以及军事工程等施工中起着极为重要的作用。随着人类活动领域的拓展，对作业质量的要求不断提高，以及作业人员对工作环境的挑剔，这些应用领域中存在的一些问题愈来愈不容忽略，例如在高温、高粉尘、高污染等特殊环境中，作业环境恶劣、强烈的震动、轰鸣的噪声、飞扬的尘埃粉末，甚至突如其来的塌方、核辐射等威胁，使得人为近距离操作的挖掘机将失去作用；太空开发、水下作业等工作环境不便于人类直接进入。此外，作业的劳动强度较大，操作者往往被要求全神贯注地身兼“多职”，体力和脑力消耗极大，以至于很容易出现意想不到的事故发生，而且操作者必须进行长时间的培训才能完成高质量的作业。因此，实现挖掘机作业装置的自动化，利用挖掘机器人代替人自主作业是解决这些问题的最理想方案。

发明内容

本发明要解决的技术问题是提供一种用于挖掘机器人的自主作业控制系统，本发明要解决的另一技术问题是提供一种用于挖掘机器人的自主作业控制方法。采用本发明，能够实现挖掘机的无人自主挖掘作业，满足恶劣环境及不适宜有人参与情况下挖掘作业需求。

本发明的挖掘机器人自主作业控制系统，包括主控计算机、立体激光雷达、操控盒、下位机控制器、位置传感器、压力传感器和驱动装置，主控计算机分别与立体激光雷达、操控盒、位置传感器和下位机控制器连接，下位机控制器分别与驱动装置、位置传感器和压力传感器相连接。

位置传感器包含回转编码器和拉线编码器，通过回转编码器检测回转平台的旋转角度，通过拉线编码器检测动臂、斗杆、伸缩臂和铲斗的液压油缸活塞伸缩距离，经转换后得到动臂、斗杆、铲斗的旋转角度和伸缩臂伸缩距离。

压力传感器通过对挖掘机器人工作装置的控制阀中的主阀芯的A口和B口压力及压差进行检测，判断工作装置负载的变化。

驱动装置由手控阀、电液比例阀、梭阀和主阀芯组成，梭阀分别与电液比例阀、手控阀和主阀芯连接。

一种挖掘机器人自主作业控制方法，包括以下步骤：

a）开机后进行系统初始化和自诊断，各部分功能正常情况下操控盒进入主界面，若存在工作异常的部件，系统报警并通过操控盒显示故障信息；

b）可通过操控盒选择系统参数设置功能，设置挖掘机器人各工作装置的原点、运动范围、最大运动速度，并将设置的信息传输到主控计算机进行保存和处理；

c）操作人员通过操控盒选择工作模式，根据选择进入步骤d或步骤e或步骤f；

d）进入示教工作模式，可以通过示教功能键进行示教操作，或者通过界面编辑示教程序，也可以调用存储的程序执行再现功能；

e）进入自主作业工作模式，通过操控盒设置挖掘任务和初始信息，输入启动指令，立体激光雷达开始工作，识别挖掘作业面的位置和状态，主控计算机根据挖掘任务和作业面状态进行挖掘轨迹规划，形成铲斗轨迹序列，按控制周期将铲斗轨迹序列转换成各关节位置控制指令发送给下位机控制器；下位机控制器根据各关节位置控制指令和位置传感器反馈的各关节位置信息，采用模糊自适应PID控制策略进行控制解算，将解算得到的控制信号发送到驱动装置，通过驱动装置实现对挖掘机器人工作装置的位置和速度控制；在此过程中，主控计算机全程监控自主挖掘过程，通过压力传感器采集各工作装置油缸压力信息，判断负载变化情况，随时可以发送控制信号修正或中断不当的挖掘动作；

f）进入遥控操作工作模式，通过操控盒发出遥控操作指令，经主控计算机处理得到控制指令发送至下位机控制器，下位机控制器对指令进行解析后发送控制信号，通过驱动装置实现对工作装置的远程遥控；

g）在系统工作过程中，主控计算机和下位机控制器同时接收位置传感器检测数据，作为轨迹规划和轨迹跟踪控制的依据；

h）在系统工作过程中，下位机控制器实时采集压力传感器信息，经处理后传输至主控计算机；

i）在系统工作工程中，主控计算机根据轨迹规划的需要发送启动命令给立体激光雷达，立体激光雷达收到启动指令后，对挖掘面进行扫描，基于扫描的数据建立挖掘面的三维物理模型；

j）根据所建挖掘面三维物理模型，求取挖掘作业面尺寸、已挖掘的深度、底部平整度和待挖掘面形状，将信息发送至主控计算机；

k）完成一次挖掘面扫描后，立体激光雷达停止工作，等待主控计算机下一次发送启动命令；

l）工作过程中，操控盒实时接收主控计算机反馈的各类信息，并在界面上进行显示；

m）系统工作过程中，主控计算机具有系统自诊断功能，如果发现与立体激光雷达、下位机控制器的数据交互出现异常，或者接收到立体激光雷达、下位机控制器发送的异常信息，则通过操控盒进行报警和故障显示，并及时进行异常处理；

n）系统工作过程中，立体激光雷达、下位机控制器具有系统自诊断功能，如果发现工作异常，则向主控计算机发送异常信息。

本发明的挖掘机器人自主作业控制系统是一个集环境描述、自主决策与规划、行为控制等多种功能于一体的集成系统，特别适用于在高温、高粉尘、高污染的特殊环境中使用，且该系统不受噪声、尘埃、核辐射的威胁。该自主作业控制系统使用立体激光雷达进行挖掘作业面检测，使用操控盒将工业机器人示教和再现功能与挖掘机器人自主作业控制系统进行结合，下位机采用自调整模糊PID控制技术，本发明具有智能化程度高、人机交互性好、运行稳定且位置重复精度高的特点。

附图说明

图1是本发明各组成部分之间的连接关系框图；

图2是本发明中的挖掘机器人工作装置及坐标系示意图；

图3是本发明中的位置传感器安装位置示意图；

图4是本发明中的电液比例控制器的结构示意图；

图5是本发明的工作流程图。

具体实施方式

下面结合附图和实施例对本发明做进一步详述。

图1是本发明各组成部分之间的连接关系框图，从图1中可以看出，本发明的挖掘机器人自主作业控制系统，包括主控计算机101、立体激光雷达102、操控盒103、下位机控制器104、位置传感器105、压力传感器106和驱动装置107。主控计算机101与立体激光雷达102、操控盒103、位置传感器105和下位机控制器104相连接，下位机控制器104与驱动装置107、位置传感器105和压力传感器106相连接。

主控计算机101与操控盒103、立体激光雷达102之间采用以太网通信,主控计算机101与下位机控制器104、各关节位置编码器之间采用CAN总线通信。下位机控制器104与各关节位置编码器、驱动器之间采用CAN总线通信，下位机控制器104通过A/D采样获取油缸压力传感器数据。主控计算机101与下位机控制器104同时接收关节编码器采样数据，主控计算机101与下位机控制器104通过软件控制实现同步，控制周期为100ms。

主控计算机101是一个基于多任务实时操作系统的计算机设备，它的主要功能包括与操控盒103交互确认用户信息，为完成用户任务自动生成与运动轨迹相对应的挖掘机器人程序，自动生成挖掘点、路径规划、运动方程正逆解、周期实时控制、信息交互和故障自诊断。立体激光雷达102由一线激光雷达和步进电机组成，步进电机带动一线激光雷达匀速摆动实现对挖掘面进行扫描。操控盒103是基于嵌入式Linux系统和MiniGUI的轻量级界面系统，采用C语言进行程序设计，能够实时准确、可靠稳定地工作，用于完成挖掘机器人的人机交互，工作模式设定，自主挖掘作业任务设置，示教运动控制，故障信息显示等功能。下位机控制器104是基于DSP28335嵌入式控制平台之上，以实现挖掘机器人轨迹跟踪的全闭环位置控制系统。通过在挖掘机器人各工作装置控制阀主阀芯401的A口和B口安装压力传感器106，对压力及压差进行检测，作为判断工作装置负载变化的依据。压力传感器106用于判断挖掘过程中是否遇到坚硬障碍物以选择合适的避障措施，拉线码盘传感器用于对挖掘工作装置是否运动到位，与运动控制形成闭环。驱动装置107采用电液比例阀实现控制信号对液压系统的控制。

图2是本发明中的挖掘机器人工作装置及坐标系示意图，从图2中可以看出，挖掘机器人工作装置由回转平台201、动臂202、斗杆203、伸缩臂204、铲斗205五个关节组成。采用D-H法，建立工作装置连杆坐标系，O₀是回转原点、O₁是动臂原点、O₂是斗杆原点、O₃是伸缩臂原点、O₄是铲斗原点、O₅是铲斗末端位置、                                                为回转平台的转角，为动臂与水平面之间的夹角，为斗杆与动臂之间的夹角，为铲斗绞点到铲斗齿尖连线与动臂延长线之间的夹角，为动臂坐标系原点与回转平台坐标系原点之间的高度差，为动臂原点础回转平台原点之间的水平距离，为斗杆原点到动臂原点之间的距离，为伸缩臂原点到斗杆原点之间的距离，为伸缩臂工作时伸出的长度，为铲斗齿到铲斗铰点之间的距离。规定逆时针为正，顺时针为负，即向左为正，向右为负，、、向上为正，向下为负。

采用D-H位移矩阵法建立运动学正解方程如式（1）所示，若将铲斗斗齿的位姿表示成向量，则可得到铲斗斗齿位置和姿态：

 （1）

采用几何法求解挖掘机器人运动学逆解。

图3是本发明中的位置传感器安装位置示意图，从图3中可以看出，通过回转编码器301检测回转平台旋转角度；通过动臂编码器302、斗杆编码器303、伸缩臂编码器304、铲斗编码器305检测动臂、斗杆、伸缩臂、铲斗液压油缸活塞伸缩距离，经转换后得到动臂202、斗杆203、铲斗205的旋转角度和伸缩臂伸缩距离。

回转台通过齿轮传动，使挖掘机驾驶室和工作装置整体在座圈上旋转，配置回转台方位编码器，通过编码器轴上直连的外齿轮与座圈上的内齿轮相啮合。采用多圈绝对编码器，用于检测回转台液压马达的旋转角度，经换算后得到回转台的旋转角度。

在动臂202、斗杆203、铲斗205、伸缩臂204驱动油缸缸体上安装拉线式编码器，线头部分固定在油缸活塞杆端头上，油缸活塞杆伸缩时带动拉线伸缩，求得活塞杆伸缩距离，经几何转换后得到动臂202、斗杆203、铲斗205旋转角度和伸缩臂204的伸缩距离。

图4是本发明中的电液比例控制器的结构示意图，从图4中可以看出，采用电液比例控制技术对液压挖掘机工作装置进行计算机控制，当控制操作手柄404时，电液比例阀403控制回路被隔离；当操作电液比例控制回路时，手柄操作液压回路被隔离，隔离是通过梭阀402来实现的，实现了人工操作和电液比例操作的快速切换。

图5是本发明的工作流程图，从图5中可以看出，本发明的挖掘机器人自主控制系统工作步骤如下：

系统上电后对运行设置进行初始化（步骤501），初始化完成后对系统进行自诊断（步骤502）并判断是否存在故障（步骤503），如果存在故障，则报警并在操控盒103上输出故障信息（步骤504），如果不存在故障，则由操作人员通过操控盒103选择工作模式和设定任务，进入遥控操作、示教再现或自主作业。

当系统进入遥控操作时，通过操控盒103的人机接口输入各轴的位置和速度指令（步骤505），将该位置和速度指令发送至驱动装置107后由驱动装置107执行位置和速度指令（步骤506），驱动挖掘机器人工作装置完成相应动作，当一次动作执行完成后，判断遥控是否结束（步骤507），如果结束，则继续等待操作人员选择工作模式和设定任务，如果没有结束，则执行步骤505。

当系统进入示教再现时，操作者可以通过操控盒103选择输入示教动作（步骤508）或输入示教程序（步骤513）两种方式进行示教再现，当操作者输入示教动作（步骤508）时，由上位机判断是否示教成功（步骤509），如果示教成功，则由上位机生成运动轨迹和相应的机器人语言，如果不成功，则由操控盒103显示故障信息（步骤512）；当操作者选择示教程序（步骤513）时，驱动装置107按调用的机器人语言驱动挖掘机器人装置进行运动（步骤514），判断运动是否正常（步骤515），如果运动不正常，则由操控盒103报警并显示故障信息（步骤516），如果运动正常，则判断运动是否结束（步骤517），如果运动结束，则继续等待操作人员选择工作模式和设定任务，如果没有结束，则执行步骤514。

当系统进入自主作业时，由上位机设置任务和挖掘初始化信息（步骤518），将设置的任务和挖掘初始化信息发送至立体激光雷达102，由立体激光雷达102进行挖掘面检测（步骤519），根据对挖掘面检测的结果，由上位机进行挖掘轨迹规划（步骤520），根据轨迹规划的结果，进行运动学逆解得到挖掘机机器人装置各关节的运动位置（步骤521），将结算得到的信息传送至下位机，下位机再将控制结果发送至驱动装置107，由驱动装置完成对挖掘机器人装置各关节的驱动和控制（步骤522），完成步骤522后判断任务是否完成，如果已经完成，则继续等待操作人员选择工作模式和设定任务，如果没有完成则执行步骤520。

Claims (5)

1.一种挖掘机器人自主作业控制系统，其特征在于：所述的控制系统包含主控计算机（101）、立体激光雷达（102）、操控盒（103）、下位机控制器（104）、位置传感器（105）、压力传感器（106）和驱动装置（107）；主控计算机（101）分别与立体激光雷达（102）、操控盒（103）、位置传感器（105）和下位机控制器（104）相连接，下位机控制器（104）分别与驱动装置（107）、位置传感器（105）和压力传感器（106）相连接。

2.根据权利要求1所述的挖掘机器人自主作业控制系统，其特征在于：所述位置传感器（105）包含回转编码器和拉线编码器，通过回转编码器检测回转平台（201）的旋转角度，通过拉线编码器检测动臂（202）、斗杆（203）、伸缩臂（204）和铲斗（205）的液压油缸活塞伸缩距离，经转换后得到动臂（202）、斗杆（203）、铲斗（205）的旋转角度和伸缩臂（204）伸缩距离。

3.根据权利要求1所述的挖掘机器人自主作业控制系统，其特征在于：所述的压力传感器（106）通过对挖掘机器人工作装置的控制阀中的主阀芯（401）的A口和B口压力及压差进行检测，判断工作装置负载的变化。

4.根据权利要求1所述的挖掘机器人自主作业控制系统，其特征在于：所述驱动装置（107）由手控阀（404）、电液比例阀（403）、梭阀（402）和主阀芯（401）组成，梭阀（402）分别与电液比例阀（403）、手控阀（404）和主阀芯（401）连接。

5.一种用于权利要求1的挖掘机器人自主作业控制方法，包括以下步骤：

a）开机后进行系统初始化和自诊断，各部分功能正常情况下，操控盒（103）进入主界面；若存在工作异常的部件，系统报警并通过操控盒（103）显示故障信息；

b）通过操控盒（103）选择系统参数并设置挖掘机器人各工作装置的原点、运动范围和最大运动速度，将设置的信息传输到主控计算机（101）进行保存和处理；

c）通过操控盒（103）选择工作模式，根据选择进入步骤d或步骤e或步骤f； 

d）进入示教操作工作模式，通过示教功能键进行示教操作，或者通过界面编辑示教程序，或者调用存储的程序执行再现功能；

e）进入自主作业工作模式，通过操控盒（103）设置挖掘任务和初始信息，输入启动指令，立体激光雷达（102）开始工作，识别挖掘作业面的位置和状态，主控计算机（101）根据挖掘任务和作业面状态进行挖掘轨迹规划，形成铲斗轨迹序列，按控制周期将铲斗轨迹序列转换成各关节位置控制指令发送给下位机控制器（104）；下位机控制器（104）根据各关节位置控制指令和位置传感器（105）反馈的各关节位置信息，采用模糊自适应PID控制策略进行控制解算，将解算得到的控制信号发送到驱动装置（107），通过驱动装置（107）实现对挖掘机器人工作装置的位置和速度控制；在此过程中，主控计算机（101）全程监控自主挖掘过程，通过压力传感器（106）采集各工作装置油缸压力信息，判断负载变化情况，随时可以发送控制信号修正或中断不当的挖掘动作；

f）进入遥控操作工作模式，通过操控盒（103）发出遥控操作指令，经主控计算机（101）处理得到控制指令发送至下位机控制器（104），下位机控制器（104）对指令进行解析后发送控制信号，通过驱动装置（107）实现对工作装置的远程遥控；

g）主控计算机（101）和下位机控制器（104）同时接收位置传感器（105）检测数据，作为轨迹规划和轨迹跟踪控制的依据；

h）下位机控制器（104）实时采集压力传感器信息，经处理后传输至主控计算机（101）；

i）主控计算机（101）根据轨迹规划的需要发送启动命令给立体激光雷达（102），立体激光雷达（102）收到启动指令后，对挖掘面进行扫描，基于扫描的数据建立挖掘面的三维物理模型；

j）根据所建挖掘面三维物理模型，求取挖掘作业面尺寸、已挖掘的深度、底部平整度和待挖掘面形状，将信息发送至主控计算机（101）；

k）完成一次挖掘面扫描后，立体激光雷达（102）停止工作，等待主控计算机（101）下一次发送启动命令；

l）操控盒（103）实时接收主控计算机（101）反馈的各类信息，并在界面上进行显示；

m）主控计算机（101）具有系统自诊断功能，如果发现与立体激光雷达（102）、下位机控制器（104）的数据交互出现异常，或者接收到立体激光雷达（102）、下位机控制器（104）发送的异常信息，则通过操控盒（103）进行报警和故障显示，并及时进行异常处理；

n）立体激光雷达（102）、下位机控制器（104）分别具有系统自诊断功能，如果发现工作异常，则向主控计算机（101）发送异常信息。