CN114273282A - 一种基于机器人作业的隧道底部积渣清理方法及系统 - Google Patents

Abstract

本发明涉及一种基于机器人作业的隧道底部积渣清理方法及系统，方案借助工业相机，进行积渣信息采集；通过上位机进行隧道底部积渣分布计算，并进行姿态约束下的清渣装置的清渣运动路径规划；通过逆运动学计算，将路径坐标转换为各关节变量；下位机控制器根据上位机计算的关节变量和传感器实时反馈，实现清渣装置各关节的高精度控制和良好的清渣效果；通过传感器反馈，在上位机和远程监控器上进行清渣机器人姿态和作业路径的实时监测。本发明解决了敞开式TBM施工过程狭小空间下隧道底部积渣量大、人工操作装置视觉受限、人工操作容易发生碰撞，对操作技术要求高、作业效率低等问题，实现隧道底部积渣清理的无人化、智能化作业。

Description

一种基于机器人作业的隧道底部积渣清理方法及系统

技术领域

本发明属于隧道开挖设备控制领域，具体涉及一种掘进机清渣机器人的控制方法及控制系统。

背景技术

全断面硬岩隧道掘进机(TBM)已经成为隧道工程施工的重要装备，被广泛应用于水利隧洞、城市轨道、市政交通和铁路隧道等领域。TBM掘进过程中，常常伴有岩爆、顶部坍塌掉块等现象，导致隧道底部积渣严重，若渣土、石块清理不及时，将导致拱架拼装、仰拱块、轨道铺设等工序滞后而制约施工进度，同时影响支护质量。

为实现渣石快速清理，国内外进行隧道底部积渣清理装置研究。但由于隧道内特殊环境，清渣装置运动过程易与周围环境碰撞干涉；人工操作难以保证挖斗姿态，容易造成渣石从挖斗内泄漏；为提高清渣效率和作业的灵活性，需要多个关节联动。

公布号为CN110552713A的中国专利文件公开了一种硬岩隧道掘进机隧道底部积渣清理装置及其方法，具体给出一种清渣装置的机械结构，并提出将渣土运送到主梁下方的出渣小车的作业方式，但针对隧道底部特殊结构，未给出清渣装置具体控制方法。

《机械传动》贾良飞等人的论文《基于蒙特卡罗法的硬岩掘进机辅助清渣装置工作空间分析》给出一种6自由度清渣装置，并进行正运动学分析，通过三维软件模拟机械臂的位姿获得关节变量，未能实现关节变量的自动求解。

公布号为CN112428267A的中国专利文件公开了一种快速求解冗余自由度机器人逆解的方法，通过建立机器人逆解评价指标，以逆解评价指标为优化目标，采用改进粒子群算法通过一定次数的迭代才能实现逆运动学计算。

发明内容

本发明的目的是提供一种基于机器人作业的隧道底部积渣清理方法及系统，用以实现自动、智能、高效的清理掘进过程中隧道底部的积渣。

为实现上述目的，本发明的方案包括：

本发明的一种基于机器人作业的隧道底部积渣清理方法，包括如下步骤；

1)建立用于确定清渣机器人各关节之间连杆位姿的机器人模型、清渣机器人的基坐标系、相机坐标系和隧道坐标系；

2)通过设置在清渣机器人底座上的相机采集隧道底部的图像，进行积渣识别，获得积渣在相机坐标系中的坐标；

3)根据相机与清渣机器人之间的结构位置关系，将积渣在相机坐标系中的坐标，转换成积渣在基坐标系中的积渣坐标；

4)根据清渣机器人在隧道底部的作业范围，标定清渣机器人末端在所述隧道坐标系下运动路径点的路径点位置坐标；所述作业范围为隧道截面上清渣机器人沿隧道内壁弧线运动进行清渣作业的范围；

5)对清渣机器人进行末端姿态约束，以防止渣土掉落；根据所述路径点位置坐标和清渣机器人末端姿态约束，得到隧道坐标系下清渣机器人末端在各路径点的末端位姿矩阵；

6)根据清渣机器人与掘进系统之间的结构位置关系，将各路径点的末端位姿矩阵转换至基坐标系；

7)将基坐标系下的末端位姿矩阵，根据机器人模型并结合机器人根据传感器得到的实时位姿，得到对应路径点清渣机器人各关节的运动量，进而控制清渣机器人执行积渣清理动作。

本发明的一种基于机器人作业的隧道底部积渣清理方法应用于清渣机器人的控制，清渣机器人将渣石翻转至皮带运输机上，随皮带运输机运出洞外，无需冗余的运渣系统和二次转运；为保证作业过程渣斗内渣石不泄露，本发明的方法通过姿态约束下的机器人清渣路径的计算，实现了复杂、狭小空间下隧道底部积渣清理机器人清渣路径规划，最终实现清渣装置智能作业，提高了隧道底部积渣清理的效率，实现开挖隧道的及时支护，提高支护质量和施工安全。

进一步的，步骤1)中，以清渣机器人第一自由度的一个极限位置为原点o₀，隧道竖直向下为z₀建立所述基坐标系。

进一步的，步骤1)中，以开挖隧道的中心线与掘进系统主梁端面交点为原点o_w，隧道竖直向下为z_w轴正方向，隧道水平方向为y_w轴方向建立所述隧道坐标系。

进一步的，步骤1)中，以相机镜头光心为原点o_c，镜头光轴为z_c轴，x_c轴、y_c轴分别与相面的两边平行，建立相机坐标系。

本发明的方法考虑到隧道尺寸与掘进机尺寸的匹配关系，建立了隧道坐标系、机器人坐标系和相机坐标系及之间的转换关系，并在隧道坐标系下标定了清渣作业中机器人末端的路径点，在防止渣土洒出及与隧道壁干涉的约束下生成各个路径点的末端位姿，根据转换关系得到机器人坐标系下的末端位姿，结合当前机器人各个动作部件的当前位姿，然后通过逆运动学求解得出当前路径点下各个机器人动作部件的目标位姿，进而得到各个关节的运动量，基于该运动量控制对应关节动作，以简单可靠的控制策略实现机器人智能决策、自动作业。

进一步的，步骤1)中，采用D-H参数法建立机器人模型。

DH参数法对多自由度机器人建模方案成熟可靠。

进一步的，步骤2)中，积渣在相机坐标系中的坐标通过如下方法获得：将识别出的积渣在图像中的像素坐标，根据像元尺寸，变换得到对应积渣在图像坐标系中的坐标；再根据图像坐标系与相机坐标系之间的转换，得到积渣在相机坐标系中的坐标。

进一步的，根据三维到二维的透视投影关系得到相机坐标系到图像坐标系的转换关系，再将相机坐标系到图像坐标系的转换关系逆推得到图像坐标系与相机坐标系之间的转换关系。

进一步的，步骤3)中，积渣在相机坐标系中的坐标到基坐标系的转换，还可以为根据相机与掘进系统之间的结构位置关系，将积渣在相机坐标系中的坐标转换成积渣在隧道坐标系中的坐标，再根据掘进系统与清渣机器人之间的结构位置关系，将积渣在隧道坐标系中的坐标转换成积渣在基坐标系中的坐标。

进一步的，所述第一自由度为：清渣机器人底座沿设置于掘进系统主梁下侧面的与主梁延伸方向平行的轨道前后移动。

进一步的，步骤4)中，所述路径点位置坐标(p_x,p_y,p_z)为：

p_x＝λ₁

p_y＝R₁ sin(-β+kΔ)

p_z＝R₁ cos(-β+kΔ)

其中，k＝0,1,2,3,…,N，

2β为所述作业范围以隧道坐标系原点o_w为顶点的角度，Δ为清渣机器人的运动分辨率，λ₁为在第一自由度方向上相对作为原点o₀的极限位置的位移。

进一步的，步骤5)中，清渣机器人在隧道坐标系下末端姿态约束

为：

其中，R₁为清渣机器人末端执行积渣清理动作时摆动的圆弧半径；R₁小于隧道轮廓半径以避免清渣机器人末端与隧道洞壁干涉。

隧道清渣作业过程中，作业范围大、环境复杂，进行末端挖斗姿态约束下的清渣路径规划，避免了挖斗内渣土的泄漏和与隧道壁发生干涉。具体的，R₁可以根据视觉系统采集的积渣分布信息、厚度信息确定；例如当铲斗无法一次清理掉隧道底部积渣，则会根据积渣厚度确定确定R₁，R₁决定了机器人动作一次清理掉积渣的量；本领域技术人员应当明了，若积渣可被铲斗一次动作清理掉，则R₁至少应当小于隧道轮廓半径。

进一步的，步骤7)中，将基坐标系下的位姿矩阵根据机器人模型，得到对应路径点清渣机器人各关节的运动量的过程中，将第一自由度固定为λ₁，将清渣机器人末端挖斗的回转自由度固定为挖斗开口朝上时回转自由度的数值，降低清渣机器人自由度后进行逆运动学求解，根据清渣机器人末端姿态得到各关节的运动量。

本发明进一步根据作业过程的特点，将对应自由度动作量设置为定值，降低了机器人自由度，降低了逆运动学计算难度和计算量，降低了对硬件的需求且提高了控制的可靠性。具体将本发明适用的8自由度机器人在逆运动学计算过程中降至6自由度，保证清渣过程高效可靠的同时降低了逆运动学计算量。

进一步的，步骤7)，在积渣清理动作后，还将清渣机器人末端举升，以将挖斗中的积渣通过溜渣板倾倒在位于主梁内的渣土输送机上。

进一步的，举升过程中，以清渣机器人末端挖斗的开口朝上为约束，采用预存的固定路径程序控制清渣机器人。

举升过程路径相似，仅需约束挖斗使挖斗开口朝上防止渣土洒落，其余自由度按照预先设定的运动轨迹基于预设的程序控制，降低计算量的同时有较高的可靠性。

进一步的，将清渣机器人的末端挖斗举升到位后，通过控制挖斗的回转自由度旋转一周，完成积渣倾倒。

本发明的一种基于机器人作业的隧道底部积渣清理系统，包括控制系统，和与控制系统相连的电机驱动器、液压控制阀组、相机及设置在清渣装置本体上的传感器，所述控制系统执行指令以实现如上所述的基于机器人作业的隧道底部积渣清理方法。

针对隧道施工过程，隧道底部积渣严重，本发明提出一种机器人作业的隧道底部积渣清理系统。本领域技术人员应当明了，上述控制系统可以包括一个或多个控制器

本发明的系统借助工业相机，进行积渣信息采集；通过上位机进行隧道底部积渣分布计算，并进行姿态约束下的清渣装置的清渣运动路径规划；通过逆运动学计算，将路径坐标转换为各关节变量；下位机控制器根据上位机计算的关节变量和传感器实时反馈，实现清渣装置各关节的高精度控制和良好的清渣效果；通过传感器反馈，在上位机和远程监控器上进行清渣机器人姿态和作业路径的实时监测。

本发明解决了敞开式TBM施工过程狭小空间下隧道底部积渣量大、人工操作装置视觉受限、人工操作容易发生碰撞，对操作技术要求高、作业效率低等问题，实现隧道底部积渣清理的无人化、智能化作业。

附图说明

图1是本发明方法所适用的清渣机器人硬件系统示意图；

图2是本发明方法所适用的清渣机器人清渣作业流程示意图；

图3是本发明所适用的掘进机部分结构示意图；

图4是本发明所适用的掘进机的清渣装置清渣斗处于卸渣位时的状态图；

图5是本发明的方法中，隧道坐标系、机器人坐标系、相机坐标系示意图；

图6是本发明的方法中，清渣机器人运动路径点示意图；

图7是本发明方法所适用的清渣装置的部分的结构示意图；

图8是像素坐标系和像素坐标系转换关系示意图；

图9是相机坐标系和隧道坐标系转换关系示意图；

图10是相机坐标系和图像坐标系转换关系示意图。

具体实施方式

下面结合附图对本发明做进一步详细的说明。

方法实施例：

本发明提出一种基于机器人作业的隧道底部积渣清理方法。

本发明的方法适用的掘进机的部分结构如图3所示，包括刀盘1以及设置在刀盘1后侧的主梁2，主梁2内设置有用于将刀盘开挖出的土石向后(定义掘进机掘进的方向为前)输送的渣土输送机(未在图中示出)，为了将隧道底部积落的土石清运至渣土输送机上，掘进机还包括设置在主梁2上的清渣装置3。

为了使清渣装置3能够将积渣铲至主梁内的渣土输送机上，主梁2的侧壁上开设有投渣口211，并在投渣口211处设置有倾斜设置的溜渣板212。

清渣装置3为拥有8个自由度的清渣机器人，具体结构如图7所示，包括清渣机械臂和清渣斗4，清渣机械臂具体包括连接座5以及与连接座5铰接的活动臂6。其中，连接座5包括吊梁轨道7以及通过驱动轮组8可移动地吊装在吊梁轨道7下侧的总安装板9，吊梁钢轨7具体为工字钢形结构，且沿掘进机的掘进方向布置，上端用于与主梁2连接，驱动轮组8的滚轮设置在吊梁钢轨7的槽内，并与工字钢形结构的吊梁钢轨7的下侧翼板配合，在总安装板9上设置有用于驱动驱动轮组8转动的行走驱动电机10(行走机构)，使得清渣机械臂和清渣斗能够沿掘进方向长距离移动，形成清渣机器人的第一自由度。本实施例中，平行地设置两个吊梁钢轨7，对应的，在总安装板9上设置有两组驱动轮组8和行走驱动电机10组成组件。

总安装板9下侧通过回转支撑固定有连接臂16，通过总安装板9上板面上固定的回转驱动电机12(回转机构)，能够驱动连接臂16周向转动，连接臂16的周向转动形成清渣机器人的第二自由度。活动臂6铰接在连接臂16上，对于连接臂16的长度，满足其与活动臂6铰接的铰接点与回转支撑的中心之间的水平距离大于主梁2同侧侧壁壁与回转支撑的中心之间的水平距离。具体的，活动臂6包括第一臂节17(大臂)和第二臂节18(小臂)，第一臂节17一端与连接臂16铰接，在二者铰接处设置有驱动第一臂节17相对于连接臂16摆动的大臂摆动缸，第一臂节17相对于连接臂16的摆动形成清渣机器人的第三自由度；第一臂节17另外一端折弯后与第二臂节18铰接，在二者的铰接处设置有用于驱动第二臂节18相对于第一臂节17摆动的小臂摆动缸，第二臂节18相对于第一臂节17的摆动形成清渣机器人的第四自由度。本实施例中，驱动装置为驱动缸。

为保证将活动臂6能够将清渣斗4移动至设定位置，本实施例中将第二臂节18设置为伸缩臂节，第二臂节18的伸长和缩短形成清渣机器人的第五自由度，第二臂节18地伸缩由小臂伸缩油缸驱动。

第二臂节18另一端设置有转座19，转座19绕第二臂节18的中心轴线转动，形成清渣机器人的第六自由度，为了驱动转座19转动，对应转座19设置有液压驱动的腕部回转减速器20。

清渣斗4包括斗座21和斗体22(挖斗)，斗座21铰接在转座19上，在斗座21和转座19之间设置有驱动斗座21摆动的手部摆动缸，斗座21相对于转座19的摆动形成清渣机器人的第七自由度。斗体22转动安装在斗座21上，挖斗的回转形成清渣机器人的第八自由度；为了驱动斗体22转动，对应斗体22设置有带有液压驱动的手部回转减速器23。

具体工作时，通过总安装板9沿吊梁钢轨7的前后移动和连接臂16的转动，调整清渣斗4的位置，然后驱动活动臂6动作，由于清渣斗4并不是自由地铰接在活动臂6上，而是能够在各安装结构及驱动装置的约束和驱动下按照设定姿态运动，使得清渣斗4为主动式清渣斗，这样活动臂6能够通过自身动作驱动清渣斗4主动取渣，如图4所示，在活动臂6展开并将清渣斗4举至高于溜渣板212的位置后，清渣斗4位于活动臂6背向主梁2的一侧，且斗口朝上，此时，控制系统控制相应的驱动电机驱动转座19转动，使清渣斗4转至溜渣板212正上方到达卸渣位，然后控制系统控制相应的驱动电机驱动斗体22相对于斗座21转动(挖斗回转)，将渣料倾倒在溜渣板212上，渣料沿溜渣板212溜至主梁2内的渣土输送机上。

本发明适用的硬件系统如图1所示，包括用于远程监控的远程控制中心、视觉识别系统、上位机PC；以及用于实现机器人运动控制的下位机控制器(下位机控制器例如可以采用PLC)、液压控制阀组、电机驱动器、各关节执行元件(摆动缸、直线油缸、回转式减速器、电机)；还包括清渣装置本体(机器人本体)、用于获得机器人各运动部件位姿的传感器等。

远程监控中心用于隧道施工状态的实时监测和远程控制；视觉识别采用工业相机30，安装在积渣清理机器人的连接座5上，用于隧道底部积渣信息采集；上位机进行积渣特征识别并进行清渣决策判断、清渣机器人运动学计算、清渣路径计算、清渣机器人姿态可视化显示等；下位机根据上位机给定的清渣机器人关节运动变量和传感器反馈信息，通过控制液压控制阀组和电机驱动器实现清渣机器人动作控制；传感器包括位移传感器和角度传感器，用于清渣机器人各关节的运动监控和姿态识别。

其作业流程为如图2所示，上位机初始化后进行隧道建模和清渣机器人建模，借助传感器进行清渣机器人当前姿态识别，在判断进入清渣模式后执行清渣装置复位，清渣机器人机械臂进入到清渣动作初始位置，也即挖斗待弧线运动以挖掘积渣的初始状态，上述状态作为“起始点”，在清渣机器人进入“起始点”后在主梁吊梁轨道7上移动，同时通过视觉系统的工业相机进行积渣识别，在识别到积渣并判断进行清渣时进行挖渣路径规划，通过逆运动学求解得到机器人各关节运动量后控制机器人进行挖渣动作，挖渣后回复到“起始点”，然后执行固定路径程序，举起挖斗将渣土倾倒至溜渣板212上，最后再次回复到“起始点”。回到“起始点”后继续通过视觉系统进行积渣识别，在积渣未达到清理程度时判断不清渣，并进一步判断第一自由度是否到达了主梁的极限位置，若未到达，则继续控制清渣机器人在主梁吊梁轨道7上移动单位距离，移动后继续通过视觉系统进行积渣识别；若到达，则清渣任务完成。

具体的，本发明的清渣机器人在第一自由度下按照设定步长移动，每次移动后进行积渣识别，在识别到积渣后，或积渣量达到设定的清渣动作阈值时，清渣机器人的机械臂动作进行清渣，第一自由度的同一位置下(即主梁同一对应位置下方)，清渣机器人可能动作多次，直到该位置积渣清理完毕，清渣机器人在第一自由度下继续移动；直到在第一自由度达到主梁极限位置，完成一轮清渣动作。

本实施例中，以全断面硬岩隧道掘进机(TBM)为例对本发明的基于机器人作业的隧道底部积渣清理方法进行介绍，所述领域技术人员应当明了，本发明的方法可以适用任何带有清渣装置且需要对隧道底部渣料进行清运的掘进机。

具体的，本发明的方法包括如下步骤：

S1：隧道底部积渣清理机器人安装在全断面硬岩隧道掘进机主梁底部，用于隧道底部积渣的清理；工业相机30固定在积渣清理机器人底座，进行隧道底部积渣信息采集，借助上位机和积渣识别技术，进行积渣识别。

S2：实现机器人坐标系、隧道坐标系、相机坐标系的转换，建立清渣机器人数学模型；计算开挖隧道实际截面。

A)建立如图5所示的积渣清理机器人坐标系、隧道坐标系、相机坐标系。

以机器人在主梁上个移动的一个极限位置为原点o₀，隧道竖直向下为z₀轴正方向，隧道水平方向为y₀轴方向，根据右手定则确定x₀轴，完成机器人基坐标系建立。

以开挖隧道的中心线与主梁端面交点为原点o_w，隧道竖直向下为z_w轴正方向，隧道水平方向为y_w轴方向，根据右手定则确定x_w轴方向，完成隧道坐标系建立。

以工业相机镜头光心为原点o_c，镜头光轴为z_c轴，x_c轴、y_c轴分别与相面的两边平行，完成相机坐标系建立。

B)机器人模型的构建可采用但不局限于D-H参数法。

根据积渣清理机器人设计尺寸，令基坐标系原点为o₀，第一关节坐标系原点为o₁，第i关节坐标系原点为o_i(i＝1,2,3,…,8)，末端挖斗坐标系原点o₈。

基于齐次变换矩阵，相邻两连杆i与i-1的关系为：

式中，a_i、α_i、d_i、θ_i具体数值根据D-H参数法确定，其中a_i为z_i与z_i-1的公垂线长度；α_i为z_i与z_i-1之间的夹角；d_i为x_i与x_i-1之间的距离；θ_i为x_i与x_i-1之间的夹角。

则可得机器人连杆i在基坐标系下的位姿表示为：

根据传感器实时反馈的关节值，并将其代入式(2)，得到连杆i末端在基坐标下的坐标值，根据该数据实时更新上位机和远程监控器的清渣机器人的位姿，实现清渣机器人位姿识别和监测。

C)根据设计隧道直径，或通过对隧道进行扫描的雷达扫描数据，计算获得隧道实际轮廓半径R_实际。

D)采用工业相机进行图像采集，基于视觉识别技术，计算隧道底部积渣位置信息，并将其转换到机器人基坐标系下，其计算过程如下：

d1、像素坐标系到图像坐标系的变换：

像素坐标系和图像坐标系都在成像平面上，二者的区别是原点和度量单位不一样。图像坐标系原点一般在成像平面的正中心，单位为mm。二者之间可以通过像元尺寸(dx和dy表示每一列和每一行分别代表多少mm)联系起来，如图8及下式所示。

则像素坐标系到图像坐标系的变换可用矩阵表示为：

式中，u₀、v₀为主点在像素坐标系下的坐标。

d2、相机坐标系与隧道坐标系的转换：

相机坐标系到隧道坐标系的转换属于刚体变换，物体不会发生形变，只需要根据掘进机的结构尺寸进行旋转和平移，假设旋转矩阵为R，平移矩阵为T，如图9所示：

则p点在相机坐标系中的坐标为：

d3、相机坐标系与图像坐标系间的转换：

相机坐标系到图像坐标系，是从3D转换到2D，属于透视投影关系，如图10所示：

根据相似三角形原理，得出下式：

可用矩阵表示为：

综合上式得到：

由于

则

其中，

和

分别为相机的内外参矩阵，可以通过标定获取。从最终的转换关系可看出，一个三维中的坐标点可以在图像中找到对应的像素点，反过来，通过图像中的一个点也可以找到三维中的对应的点，即可计算出渣石轮廓点坐标，从而可以计算出渣石的粒径、面积等几何特征。

S3：由于清渣机器人作业环境相对复杂，作业范围大，且对末端运动轨迹要求严格，否则易导致机器人末端与隧道洞壁干涉。为解决该问题，本专利采用下述方法：

A)虽然积渣清理机器人有8个关节，无法采用常规解析法进行逆解计算。针对本专利的清渣机器人，机器人在主梁底部移动距离为λ₁可由传感器测得，将该值作为固定值；在积渣抓取过程，腕部回转关节无需运动，可将该自由度固定；通过固定这两个关节，可将机器人退化为6自由度机器人，简化逆运动学计算难度，提高计算效率。在渣石抓取，运动至固定位置，即挖斗运动至溜渣板212上后，腕部回转关节转动180°，将渣石倾倒至溜渣板212上，然后腕部回转关节转回原始姿态。通过上述步骤，将机器人自由度降至6个，以降低逆运动学计算量。

B)如图6所示，根据实际工况，积渣主要集中散落在隧道底部±45°范围内，设为完成清渣作业，清渣机器人在隧道底部的作业范围为±β(根据施工实际情况给定，且应满足β≥45°)，为保证末端运动精度，清渣机器人的运动分辨率为Δ，则在隧道坐标系o_w下，清渣机器人运动路径点的位置坐标可表示为：

p_x＝λ₁

p_y＝R₁ sin(-β+kΔ)

p_z＝R₁ cos(-β+kΔ)

k＝0,1,2,3,…,N，

λ₁表示清渣装置行走机构的行走距离，即第一关节的位移。

为保证渣土不泄漏，在清渣过程需要对清渣机器人末端的姿态进行约束，其约束可表示为：

则进行姿态约束的路径点k在机器人基座标系o₀下的表示为：

式中，

表示从隧道坐标系到基坐标系o₀的转换。

通过将

设置为定值即将λ₁和θ₈，将机器人自由度降至6个，即可对

进行逆运动学求解，将路径点k转换为机器人各关节的运动量[λ₁,θ_2k,θ_3k,λ_4k,θ_5k,θ_6k,θ_7k,θ₈]，下位机控制器通过控制液压控制阀组、电机驱动器和传感器的反馈信息，实现挖斗的高精度控制，渣石的抓取。

S5：为减少上位机计算量，根据清渣装置的作业特点，将清渣装置挖取渣土后，将渣土翻转到主机内皮带运输机的路径设置为“固定解析路径”。由于清渣机器人在放置渣土过程，需要保证挖斗朝上，避免挖斗内渣土泄漏，因此，参考S4，对末端挖斗位姿矩阵进行约束。

S6：通过上述步骤，将渣土倾倒至皮带运输机上，随皮带运输机运出洞外，随后，清渣机器人进行“复位”，回到初始位置。

S7：上位机根据工业相机采集积渣图像信息进行是否清渣判断，继续上述步骤直至该区域隧道底部积渣清理结束。

S8：机器人在主梁底部移动，运动至下一工位，进行视觉识别并重复上述步骤，开始下一区域积渣的清理，直至隧道底部积渣清理结束。

在此过程，在上位机和远程控制中心建立清渣装置的运动模型，将传感器反馈的各关节变量代入清渣装置的数学模型，并在上位机同步更新清渣装置的姿态和此次清渣的运动路径，实现对清渣机器人和清渣路径的实时监控。

系统实施例：

本发明的一种基于机器人作业的隧道底部积渣清理系统，采用如图1所示的清渣机器人硬件系统，其中下位机控制器采集各关节处布置的传感器信息，同时根据上位机PC的指令输出控制信号实现机器人控制；上位机PC用于实现机器人运动规划，结合视觉识别系统实现隧道底部的渣土识别，并且能够基于下位机反馈的机器人传感器信息进行机器人姿态识别和可视化显示，远程控制终端对本发明的系统进行远程监控。

上位机PC和下位机控制器共同构成本发明的控制系统，执行指令以实现方法实施例中的基于机器人作业的隧道底部积渣清理方法，基于机器人作业的隧道底部积渣清理方法及图1所示的清渣机器人硬件系统已在方法实施例中介绍的足够清楚，此处不再赘述。

Claims (16)

1.一种基于机器人作业的隧道底部积渣清理方法，其特征在于，包括如下步骤；

1)建立用于确定清渣机器人各关节之间连杆位姿的机器人模型、清渣机器人的基坐标系、相机坐标系和隧道坐标系；

2)通过设置在清渣机器人底座上的相机采集隧道底部的图像，进行积渣识别，获得积渣在相机坐标系中的坐标；

3)根据相机与清渣机器人之间的结构位置关系，将积渣在相机坐标系中的坐标，转换成积渣在基坐标系中的积渣坐标；

4)根据清渣机器人在隧道底部的作业范围，标定清渣机器人末端在所述隧道坐标系下运动路径点的路径点位置坐标；所述作业范围为隧道截面上清渣机器人沿隧道内壁弧线运动进行清渣作业的范围；

5)对清渣机器人进行末端姿态约束，以防止渣土掉落；根据所述路径点位置坐标和清渣机器人末端姿态约束，得到隧道坐标系下清渣机器人末端在各路径点的末端位姿矩阵；

6)根据清渣机器人与掘进系统之间的结构位置关系，将各路径点的末端位姿矩阵转换至基坐标系；

7)将基坐标系下的末端位姿矩阵，根据机器人模型并结合机器人根据传感器得到的实时位姿，得到对应路径点清渣机器人各关节的运动量，进而控制清渣机器人执行积渣清理动作。

2.根据权利要求1所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤1)中，以清渣机器人第一自由度的一个极限位置为原点o₀，隧道竖直向下为z₀建立所述基坐标系。

3.根据权利要求2所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤1)中，以开挖隧道的中心线与掘进系统主梁端面交点为原点o_w，隧道竖直向下为z_w轴正方向，隧道水平方向为y_w轴方向建立所述隧道坐标系。

4.根据权利要求3所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤1)中，以相机镜头光心为原点o_c，镜头光轴为z_c轴，x_c轴、y_c轴分别与相面的两边平行，建立相机坐标系。

5.根据权利要求4所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤1)中，采用D-H参数法建立机器人模型。

6.根据权利要求5所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤2)中，积渣在相机坐标系中的坐标通过如下方法获得：将识别出的积渣在图像中的像素坐标，根据像元尺寸，变换得到对应积渣在图像坐标系中的坐标；再根据图像坐标系与相机坐标系之间的转换，得到积渣在相机坐标系中的坐标。

7.根据权利要求6所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，根据三维到二维的透视投影关系得到相机坐标系到图像坐标系的转换关系，再将相机坐标系到图像坐标系的转换关系逆推得到图像坐标系与相机坐标系之间的转换关系。

8.根据权利要求7所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤3)中，积渣在相机坐标系中的坐标到基坐标系的转换，还可以为根据相机与掘进系统之间的结构位置关系，将积渣在相机坐标系中的坐标转换成积渣在隧道坐标系中的坐标，再根据掘进系统与清渣机器人之间的结构位置关系，将积渣在隧道坐标系中的坐标转换成积渣在基坐标系中的坐标。

9.根据权利要求8所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，所述第一自由度为：清渣机器人底座沿设置于掘进系统主梁下侧面的与主梁延伸方向平行的轨道前后移动。

10.根据权利要求9所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤4)中，所述路径点位置坐标(p_x,p_y,p_z)为：

p_x＝λ₁

p_y＝R₁ sin(-β+kΔ)

p_z＝R₁ cos(-β+kΔ)

其中，k＝0,1,2,3,…,N，

2β为所述作业范围以隧道坐标系原点o_w为顶点的角度，Δ为清渣机器人的运动分辨率，λ₁为在第一自由度方向上相对作为原点o₀的极限位置的位移。

11.根据权利要求10所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤5)中，清渣机器人在隧道坐标系下末端姿态约束

为：

其中，R₁为清渣机器人末端执行积渣清理动作时摆动的圆弧半径；R₁小于隧道轮廓半径以避免清渣机器人末端与隧道洞壁干涉。

12.根据权利要求11所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤7)中，将基坐标系下的位姿矩阵根据机器人模型，得到对应路径点清渣机器人各关节的运动量的过程中，将第一自由度固定为λ₁，将清渣机器人末端挖斗的回转自由度固定为挖斗开口朝上时回转自由度的数值，降低清渣机器人自由度后进行逆运动学求解，根据清渣机器人末端姿态得到各关节的运动量。

13.根据权利要求12所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，步骤7)，在积渣清理动作后，还将清渣机器人末端举升，以将挖斗中的积渣通过溜渣板倾倒在位于主梁内的渣土输送机上。

14.根据权利要求13所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，举升过程中，以清渣机器人末端挖斗的开口朝上为约束，采用预存的固定路径程序控制清渣机器人。

15.根据权利要求14所述的基于机器人作业的隧道底部积渣清理方法，其特征在于，将清渣机器人的末端挖斗举升到位后，通过控制挖斗的回转自由度旋转一周，完成积渣倾倒。

16.一种基于机器人作业的隧道底部积渣清理系统，其特征在于，包括控制系统，所述控制系统执行指令以实现如权利要求1-15任一项所述的基于机器人作业的隧道底部积渣清理方法。

Images