一种挖掘机器人自主作业控制系统及方法
【专利摘要】本发明提供了一种挖掘机器人自主作业控制系统及方法，所述的系统中的主控计算机与立体激光雷达、操控盒、位置传感器和下位机控制器相连接，下位机控制器与驱动装置、位置传感器和压力传感器相连接。采用本发明能够实现挖掘机的无人自主挖掘作业，满足恶劣环境及不适宜有人参与情况下挖掘作业需求。本发明的智能化程度高、人机交互性好、运行稳定且位置重复精度高。
【专利说明】一种挖掘机器人自主作业控制系统及方法
【技术领域】
[0001] 本发明涉及一种挖掘机器人自主作业控制系统及方法，采用本发明能够选择遥控 操作、示教再现和自主作业三种不同的工作模式，实现对挖掘机器人的遥控操作，示教再现 作业以及通过完全自主的方式进行挖掘作业。
【背景技术】
[0002] 智能挖掘机是功能最典型、结构最复杂、用途最广泛的工程机械之一。而无人自主 挖掘机器人则代表着智能挖掘机的最高水平。它在工业与民用建筑、交通运输、水利电力工 程、矿山采掘以及军事工程等施工中起着极为重要的作用。随着人类活动领域的拓展，对 作业质量的要求不断提高，以及作业人员对工作环境的挑剔，这些应用领域中存在的一些 问题愈来愈不容忽略，例如在高温、高粉尘、高污染等特殊环境中，作业环境恶劣、强烈的震 动、轰鸣的噪声、飞扬的尘埃粉末，甚至突如其来的塌方、核辐射等威胁，使得人为近距离操 作的挖掘机将失去作用；太空开发、水下作业等工作环境不便于人类直接进入。此外，作业 的劳动强度较大，操作者往往被要求全神贯注地身兼"多职"，体力和脑力消耗极大，以至于 很容易出现意想不到的事故发生，而且操作者必须进行长时间的培训才能完成高质量的作 业。因此，实现挖掘机作业装置的自动化，利用挖掘机器人代替人自主作业是解决这些问题 的最理想方案。
【发明内容】
[0003] 本发明要解决的技术问题是提供一种用于挖掘机器人的自主作业控制系统，本发 明要解决的另一技术问题是提供一种用于挖掘机器人的自主作业控制方法。采用本发明， 能够实现挖掘机的无人自主挖掘作业，满足恶劣环境及不适宜有人参与情况下挖掘作业需 求。
[0004] 本发明的挖掘机器人自主作业控制系统，包括主控计算机、立体激光雷达、操控 盒、下位机控制器、位置传感器、压力传感器和驱动装置，主控计算机分别与立体激光雷达、 操控盒、位置传感器和下位机控制器连接，下位机控制器分别与驱动装置、位置传感器和压 力传感器相连接。
[0005] 位置传感器包含回转编码器和拉线编码器，通过回转编码器检测回转平台的旋转 角度，通过拉线编码器检测动臂、斗杆、伸缩臂和铲斗的液压油缸活塞伸缩距离，经转换后 得到动臂、斗杆、铲斗的旋转角度和伸缩臂伸缩距离。
[0006] 压力传感器通过对挖掘机器人工作装置的控制阀中的主阀芯的A 口和B 口压力及 压差进行检测，判断工作装置负载的变化。
[0007] 驱动装置由手控阀、电液比例阀、梭阀和主阀芯组成，梭阀分别与电液比例阀、手 控阀和主阀芯连接。
[0008] 一种挖掘机器人自主作业控制方法，包括以下步骤： a)开机后进行系统初始化和自诊断，各部分功能正常情况下操控盒进入主界面，若存 在工作异常的部件，系统报警并通过操控盒显示故障信息； b)可通过操控盒选择系统参数设置功能，设置挖掘机器人各工作装置的原点、运动范 围、最大运动速度，并将设置的信息传输到主控计算机进行保存和处理； C)操作人员通过操控盒选择工作模式，根据选择进入步骤d或步骤e或步骤f ; d) 进入示教工作模式，可以通过示教功能键进行示教操作，或者通过界面编辑示教程 序，也可以调用存储的程序执行再现功能； e) 进入自主作业工作模式，通过操控盒设置挖掘任务和初始信息，输入启动指令，立体 激光雷达开始工作，识别挖掘作业面的位置和状态，主控计算机根据挖掘任务和作业面状 态进行挖掘轨迹规划，形成铲斗轨迹序列，按控制周期将铲斗轨迹序列转换成各关节位置 控制指令发送给下位机控制器；下位机控制器根据各关节位置控制指令和位置传感器反馈 的各关节位置信息，采用模糊自适应PID控制策略进行控制解算，将解算得到的控制信号 发送到驱动装置，通过驱动装置实现对挖掘机器人工作装置的位置和速度控制；在此过程 中，主控计算机全程监控自主挖掘过程，通过压力传感器采集各工作装置油缸压力信息，判 断负载变化情况，随时可以发送控制信号修正或中断不当的挖掘动作； f) 进入遥控操作工作模式，通过操控盒发出遥控操作指令，经主控计算机处理得到控 制指令发送至下位机控制器，下位机控制器对指令进行解析后发送控制信号，通过驱动装 置实现对工作装置的远程遥控； g) 在系统工作过程中，主控计算机和下位机控制器同时接收位置传感器检测数据，作 为轨迹规划和轨迹跟踪控制的依据； h) 在系统工作过程中，下位机控制器实时采集压力传感器信息，经处理后传输至主控 计算机； i) 在系统工作工程中，主控计算机根据轨迹规划的需要发送启动命令给立体激光雷 达，立体激光雷达收到启动指令后，对挖掘面进行扫描，基于扫描的数据建立挖掘面的三维 物理模型； j )根据所建挖掘面三维物理模型，求取挖掘作业面尺寸、已挖掘的深度、底部平整度和 待挖掘面形状，将信息发送至主控计算机； k) 完成一次挖掘面扫描后，立体激光雷达停止工作，等待主控计算机下一次发送启动 命令； l) 工作过程中，操控盒实时接收主控计算机反馈的各类信息，并在界面上进行显示； m) 系统工作过程中，主控计算机具有系统自诊断功能，如果发现与立体激光雷达、下位 机控制器的数据交互出现异常，或者接收到立体激光雷达、下位机控制器发送的异常信息， 则通过操控盒进行报警和故障显示，并及时进行异常处理； η)系统工作过程中，立体激光雷达、下位机控制器具有系统自诊断功能，如果发现工作 异常，则向主控计算机发送异常信息。
[0009] 本发明的挖掘机器人自主作业控制系统是一个集环境描述、自主决策与规划、行 为控制等多种功能于一体的集成系统，特别适用于在高温、高粉尘、高污染的特殊环境中使 用，且该系统不受噪声、尘埃、核辐射的威胁。该自主作业控制系统使用立体激光雷达进行 挖掘作业面检测，使用操控盒将工业机器人示教和再现功能与挖掘机器人自主作业控制系 统进行结合，下位机采用自调整模糊PID控制技术，本发明具有智能化程度高、人机交互性 好、运行稳定且位置重复精度高的特点。
【专利附图】
【附图说明】
[0010] 图1是本发明各组成部分之间的连接关系框图； 图2是本发明中的挖掘机器人工作装置及坐标系示意图； 图3是本发明中的位置传感器安装位置示意图； 图4是本发明中的电液比例控制器的结构示意图； 图5是本发明的工作流程图。
【具体实施方式】
[0011] 下面结合附图和实施例对本发明做进一步详述。
[0012] 图1是本发明各组成部分之间的连接关系框图，从图1中可以看出，本发明的挖掘 机器人自主作业控制系统，包括主控计算机101、立体激光雷达102、操控盒103、下位机控 制器104、位置传感器105、压力传感器106和驱动装置107。主控计算机101与立体激光雷 达102、操控盒103、位置传感器105和下位机控制器104相连接，下位机控制器104与驱动 装置107、位置传感器105和压力传感器106相连接。
[0013] 主控计算机101与操控盒103、立体激光雷达102之间采用以太网通信，主控计 算机101与下位机控制器104、各关节位置编码器之间采用CAN总线通信。下位机控制器 104与各关节位置编码器、驱动器之间采用CAN总线通信，下位机控制器104通过A/D采样 获取油缸压力传感器数据。主控计算机101与下位机控制器104同时接收关节编码器采样 数据，主控计算机101与下位机控制器104通过软件控制实现同步，控制周期为100ms。
[0014] 主控计算机101是一个基于多任务实时操作系统的计算机设备，它的主要功能包 括与操控盒103交互确认用户信息，为完成用户任务自动生成与运动轨迹相对应的挖掘机 器人程序，自动生成挖掘点、路径规划、运动方程正逆解、周期实时控制、信息交互和故障自 诊断。立体激光雷达102由一线激光雷达和步进电机组成，步进电机带动一线激光雷达匀 速摆动实现对挖掘面进行扫描。操控盒103是基于嵌入式Linux系统和MiniGn的轻量级 界面系统，采用C语言进行程序设计，能够实时准确、可靠稳定地工作，用于完成挖掘机器 人的人机交互，工作模式设定，自主挖掘作业任务设置，示教运动控制，故障信息显示等功 能。下位机控制器104是基于DSP28335嵌入式控制平台之上，以实现挖掘机器人轨迹跟踪 的全闭环位置控制系统。通过在挖掘机器人各工作装置控制阀主阀芯401的A 口和B 口安 装压力传感器106,对压力及压差进行检测，作为判断工作装置负载变化的依据。压力传感 器106用于判断挖掘过程中是否遇到坚硬障碍物以选择合适的避障措施，拉线码盘传感器 用于对挖掘工作装置是否运动到位，与运动控制形成闭环。驱动装置107采用电液比例阀 实现控制信号对液压系统的控制。
[0015] 图2是本发明中的挖掘机器人工作装置及坐标系示意图，从图2中可以 看出，挖掘机器人工作装置由回转平台201、动臂202、斗杆203、伸缩臂204、铲斗 205五个关节组成。采用D-H法，建立工作装置连杆坐标系，O tl是回转原点、O 1是 动臂原点、O2是斗杆原点、O 3是伸缩臂原点、O4是铲斗原点、O5是铲斗末端位置、 乌为回转平台的转角，乌为动臂与水平面之间的夹角，为斗杆与动臂之间的夹角，名为铲 斗绞点到铲斗齿尖连线与动臂延长线之间的夹角，或为动臂坐标系原点与回转平台坐标系 原点之间的高度差，O1为动臂原点础回转平台原点之间的水平距离，A为斗杆原点到动臂 原点之间的距离，为伸缩臂原点到斗杆原点之间的距离，^为伸缩臂工作时伸出的长度,? 为铲斗齿到铲斗铰点之间的距离。1?规定逆时针为正，顺时针为负，即^向左为正，向右为 负，向上为正，向下为负。
[0016] 采用D-H位移矩阵法建立运动学正解方程如式（1)所示，若将铲斗斗齿的位姿表 示成向量|l ，则可得到铲斗斗齿位置和姿态：
【权利要求】
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)发送异常信息。
【文档编号】E02F9/20GK104476548SQ201410570531
【公开日】2015年4月1日 申请日期:2014年10月24日 优先权日:2014年10月24日
【发明者】骆云志, 张春华, 王钤, 吕卫强, 雷巍, 高洁, 李钰颖, 罗凌江, 欧阳长青 申请人:四川省绵阳西南自动化研究所