CN103234513B - 基于摄像头和航姿仪的掘进自动导向系统及导向方法 - Google Patents

Abstract

本发明公开了一种基于摄像头和航姿仪的掘进自动导向系统，它包括设置在掘进机机身上的掘进机控制模块、设置在掘进机机身上的激光靶面、摄像头、航姿仪、数据采集模块、中央处理器，设置在掘进巷道上的激光指向仪，激光指向仪位于掘进机机身后方，其中，激光指向仪的激光发射头对准激光靶面的中心，摄像头投射在激光靶面上，摄像头的信号输出端连接数据采集模块，航姿仪的信号输出端连接中央处理器，数据采集模块的通信端连接中央处理器的通信端，中央处理器的控制信号输出端连接掘进机控制模块的控制信号输入端。本发明具有导向精度高、操作简单、成本低、易于实施便于推广的优点。

Description

基于摄像头和航姿仪的掘进自动导向系统及导向方法

技术领域

本发明涉及掘进机位姿测量技术领域，具体涉及一种基于摄像头和航姿仪的掘进自动导向系统及导向方法。

技术背景

目前，我国的巷道掘进导向技术一般采用激光指向仪，掘进机驾驶员通过目视端面上的激光光斑控制掘进机截割头在断面上截割运行，由于测量工作只能在掘进机停止截割的间隙中进行，因此测量浪费大量的时间，工作强度大，掘进的质量完全取决于驾驶员的熟练程度和经验，测量结果的准确性无法得到保证。

为解决以上问题，国内外的相关研究人员提出了很多的解决方案，公开号为CN101629807的中国专利《掘进机机身位姿测量系统及其方法》提出了利用扇形激光束在掘进机机身形成线性光斑，依靠带有光敏元件的激光标靶来产生电流信号，经过处理后得出掘进机的偏向角和偏向位移的方案。但是该方案在扇形激光束的获取上很有难度，且扇形激光束的加工工艺和精度难以保证。公开号为CN101266134A的中国专利《悬臂机头位姿的测量系统及其方法》提出了利用激光机动全站仪发射激光到掘进机，经过棱镜反射，再对反射回来的激光信号进行接收，然后通过无线信号的传输到达掘进机内的计算机，一次性地测量掘进机的全部位姿参数的方案。该方案虽理论上可行，但具有操作复杂，对操作人员要求高，成本高等缺点，不易推广使用。

发明内容

本发明的目的是针对上述技术问题，提供一种基于摄像头和航姿仪的掘进自动导向系统及导向方法，该导向系统和导线方法将CCD（Charge-coupled Device，电荷耦合元件）摄像头捕获激光光斑与航姿仪相结合，完成掘进机机身位姿参数（包括水平偏向位移和偏向角）的测量，并可根据偏向位移和偏向角对掘进机进行自动纠偏，实现掘进机的自动导向。

为实现此目的，本发明所设计的基于摄像头和航姿仪的掘进自动导向系统，它包括设置在掘进机机身上的掘进机控制模块，其特征在于：它还包括设置在掘进机机身上的激光靶面、CCD摄像头、航姿仪、数据采集模块、中央处理器以及设置在掘进巷道上的激光指向仪，所述激光指向仪位于掘进机机身后方，其中，所述激光指向仪的激光发射头对准激光靶面的中心，所述CCD摄像头投射在激光靶面上，所述CCD摄像头的信号输出端连接数据采集模块，航姿仪的信号输出端连接中央处理器，所述数据采集模块的通信端连接中央处理器的通信端，中央处理器的控制信号输出端连接掘进机控制模块的控制信号输入端。

本发明还包括分别设置在掘进机机身四个角上的四个光电开关，所述每个光电开关的信号输出端均连接中央处理器。

所述数据采集模块的通信端通过SPI（Serial Peripheral Interface，串行外围设备接口）总线连接中央处理器的通信端；所述航姿仪的信号输出端通过RS485总线连接中央处理器；所述中央处理器的控制信号输出端通过CAN (Controller Area Network,控制器局域网络)总线连接掘进机控制模块的控制信号输入端。

所述每个光电开关的信号输出端均通过RS485总线连接中央处理器。

一种基于摄像头和航姿仪的掘进自动导向系统的导向方法，其特征在于，它包括如下步骤：

步骤1:打开激光指向仪，并控制激光指向仪射出与掘进巷道平行的激光，该激光打在激光靶面的中心；

步骤2：掘进机开始工作并向前运动，CCD摄像头捕获激光靶面上的激光光斑，数据采集模块采集到激光光斑实时位置数据，数据采集模块将激光光斑实时位置数据传输给中央处理器，中央处理器运算得到实时激光光斑中心点坐标，通过实时激光光斑中心点坐标得到掘进机的实时水平偏向位移，所述水平偏向位移为在水平方向上偏离中心的距离；

步骤3：在掘进机向前运动的过程中，航姿仪实时采集掘进机运动时的偏向角，并将掘进机的实时偏向角数据输送到中央处理器；

步骤4：中央处理器将掘进机的实时水平偏向位移和掘进机的实时偏向角数据输送给掘进机控制模块，掘进机控制模块根据上述掘进机的实时水平偏向位移和掘进机的实时偏向角数据发出控制指令使掘进机进行动态纠偏，实现自动导向。

所述步骤4中，掘进机控制模块根据上述掘进机的实时水平偏向位移和掘进机的实时偏向角数据发出控制指令使掘进机进行动态纠偏的具体方法是：掘进机控制模块获取掘进机的水平偏向位移和偏向角后，首先掘进机控制模块调整掘进机右轮的速度，从而调整掘进机的偏向角，当掘进机的偏向角调整为0度时，再继续通过掘进机控制模块调整掘进机右轮的速度，从而调整掘进机的水平偏向位移，这样不断重复，若掘进机右偏则调慢掘进机右轮速度，若掘进机左偏则调快掘进机右轮速度；

上述掘进机控制模块控制掘进机右轮的实时速度由下式得到：

$V=V^{\′}\±K_c\×(X\×X-K_h\×X\×X^{\′})---\left(1\right)$

其中，V为掘进机控制模块控制掘进机右轮的实时速度，V＇为上一时刻掘进机右轮的行走速度，X为掘进机的实际水平偏向位移，X＇为掘进机上一时刻的水平偏向位移，K_c为掘进机车轮电机扭力调整系数，K_h是惯性抑制系数；通过上述调整，最终将掘进机调整至中心轴线上，继续行进，实现掘进机得自动导向。

所述步骤3和步骤4之间还包括步骤3.1：掘进机机身四个角上的光电开关向四周发出光信号，光信号碰到掘进巷道后被反射回对应的光电开关，每个光电开关以此得到光电开关与掘进巷道之间的距离，每个光电开关将该距离信息与光电开关上预先设好的阈值进行比较，并将比较结果传输给中央处理器，当有一个光电开关与掘进巷道之间的距离小于该阈值时，中央处理器控制掘进机控制模块改变运行轨迹，以使每个光电开关与掘进巷道之间的距离保持在大于该阈值。

所述步骤4的公式1中当掘进机的水平偏向位移左偏时，如果掘进机的运动速度增加，则K_c＝０.２２，K_h＝０.９；如果掘进机的运动速度减小，则K_c＝２.２，K_h＝０.７；当掘进机的水平偏向位移右偏时，如果掘进机的运动速度增加，则K_c＝２．２，K_h＝０.７；如果掘进机的运动速度减小，则K_c＝０．２２，K_h＝０.９。

所述步骤3.1中的阈值为20~40cm。

与现有技术相比，本发明具有如下优点：

1）本发明使用激光指向仪，CCD摄像头，航姿仪，光电开关、数据采集模块和中央处理器，完成掘进机位姿测量和自动导向，可获取掘进机相对预定轨迹的偏向角和偏向位移。本系统的测量精度可达到0.5°以内，偏离位移精度在5cm以内。

2）本发明将CCD摄像头捕获的激光光斑与航姿仪相结合，完成掘进机机身位姿参数（包括水平偏向位移和偏向角）的测量，并可根据偏向位移和偏向角对掘进机进行自动纠偏，实现掘进机的自动导向。

3）本发明使用光电开关确保掘进机不与巷道发生碰撞，保证了整个系统安全、正常地运行。

4）本发明相对背景技术中记载的两个技术方案，具有导向精度高、操作简单、成本低、易于实施便于推广的优点。

附图说明

图1为本发明的结构示意图；

图2为本发明的结构框图；

图3为掘进机的运行调整轨迹图；

图4为本发明光电开关分布示意图。

其中，1—掘进机机身、2—掘进机控制模块、3—激光靶面、4—CCD摄像头、5—航姿仪、6—数据采集模块、7—中央处理器、8—掘进巷道、9—激光指向仪、10—光电开关。

具体实施方式

以下结合附图和实施例对本发明作进一步的详细说明：

本发明的基于摄像头和航姿仪的掘进自动导向系统，如图1和图2所示，它包括设置在掘进机机身1上的掘进机控制模块2、设置在掘进机机身1上的激光靶面3、CCD摄像头4、航姿仪5、数据采集模块6、中央处理器7，设置在掘进巷道8上的激光指向仪9，激光指向仪9位于掘进机机身1后方，其中，激光指向仪9的激光发射头对准激光靶面3的中心，CCD摄像头4投射在激光靶面3上，CCD摄像头4的信号输出端连接数据采集模块6，航姿仪5的信号输出端连接中央处理器7，数据采集模块6的通信端连接中央处理器7的通信端，中央处理器7的控制信号输出端连接掘进机控制模块2的控制信号输入端。

上述技术方案中，它还包括分别设置在掘进机机身1四个角上的四个光电开关10，如图4所示，每个光电开关10的信号输出端均连接中央处理器7。

上述技术方案中，数据采集模块6的通信端通过SPI总线连接中央处理器7的通信端；航姿仪5的信号输出端通过RS485总线连接中央处理器7；中央处理器7的控制信号输出端通过CAN总线连接掘进机控制模块2的控制信号输入端。

上述技术方案中，每个光电开关10的信号输出端均通过RS485总线连接中央处理器7。

上述技术方案中，数据采集模块6为ARM9，所述ARM9采用AT91SAM9G20。航姿仪5采用重庆冰刃科技有限公司的Avatar II型航姿仪，该航姿仪5以磁力传感器、重力加速度计以及陀螺仪为架构核心，该航姿仪5通过采集运动传感器的数据，融合卡尔曼滤波，输出实时掘进机机身的姿态角数据。CCD摄像头4采用工业级的图像传感器OV7725。所述中央处理器7使用C8051F040。光电开关10选用欧姆龙公司的E3G-L77漫反射型光电开关，其感应距离最远可达2m，且能适应黑色和粗糙的表面。

上述技术方案中，当掘进机在狭窄的掘进巷道8内进行姿态调整时，容易与掘进巷道8岩壁发生碰撞，使用光电开关10来提醒和强制避免掘进机与掘进巷道8发生碰撞。在掘进机机身1的四个角上安装光电开关10作为应急响应，当四个光电开关10当中的某一个与岩壁的距离小于安全阈值时，就让光电开关10作为主导，控制掘进机调整，让掘进机重新回到掘进巷道8中间。

本发明工作时：掘进机根据激光指向仪指向方向直行，如偏离激光指向方向2CM，就控制掘进机以半圆弧轨迹从偏向的反方向回到激光的中心轴线上，掘进机重新按照激光指向方向前进。将掘进机调整到中心轴线上的方法为：当摄像头检测到掘进机的水平偏向位移大于2cm时，通过调整掘进机左右轮的位移差来减小掘进机的水平偏向位移，当调整到掘进机的水平偏向位移小于2CM时，停止调整位移，通过原地调整角度，角度调整至初始状态。

图3中的轨迹A-B-C-D是掘进机的运行轨迹。A-B段时，掘进机的右轮速度大于左轮速度，目的是让掘进机做转向动作。且随着水平偏向位移（即与激光方向的位移）的增大，转向力度越大；B-C-D段时，左轮速度逐渐增大，最后掘进机处于右轮速度小于左轮速度的状态，目的是让水平偏向位移为0（即掘进机回到既定路线上）时掘进机运动方向与激光方向夹角处在一个可控状态（运动方向的夹角与激光方向夹角不宜过大）。

具体的计算公式如(2)和(3)所示。

$V_L=V_{\mathrm{set}}+k_c(S_c\×S_c-k_h\×S_c\×S_h)---\left(2\right)$

$V_R=V_{\mathrm{set}}-k_c(S_c\×S_c-k_h\×S_c\×S_h)---\left(3\right)$

其中V_L是指返回的掘进机左轮实际行走速度，V_R指返回的掘进机右轮实际行走速度，S_c是指激光测出的实时水平偏向位移，S_h是指激光测出的上一时刻的水平位移，k_c为掘进机车轮电机扭力调整系数，k_h为惯性抑制系数，V_set为掘进机初始的设定的速度；公式2、3中，当掘进机的水平偏向位移左偏时，如果掘进机的运动速度增加，则K_c＝０.２２，K_h＝０.９；如果掘进机的运动速度减小，则K_c＝２.２，K_h＝０.７；当掘进机的水平偏向位移右偏时，如果掘进机的运动速度增加，则K_c＝２．２，K_h＝０.７；如果掘进机的运动速度减小，则K_c＝０．２２，K_h＝０.９。

一种基于摄像头和航姿仪的掘进自动导向系统的导向方法，其特征在于，它包括如下步骤：

步骤1: 打开激光指向仪9，并控制激光指向仪9射出与掘进巷道8平行的激光，该激光打在激光靶面3的中心；

步骤2：掘进机开始工作并向前运动，CCD摄像头4捕获激光靶面3上的激光光斑，数据采集模块6采集到激光光斑实时位置数据，数据采集模块6将激光光斑实时位置数据传输给中央处理器7，中央处理器7运算得到实时激光光斑中心点坐标，通过实时激光光斑中心点坐标得到掘进机的实时水平偏向位移，水平偏向位移为在水平方向上偏离中心的距离；

步骤3：在掘进机向前运动的过程中，航姿仪实时采集掘进机运动时的偏向角，并将掘进机的实时偏向角数据输送到中央处理器7；

步骤4：中央处理器7将掘进机的实时水平偏向位移和掘进机的实时偏向角数据输送给掘进机控制模块2，掘进机控制模块2根据上述掘进机的实时水平偏向位移和掘进机的实时偏向角数据发出控制指令使掘进机进行动态纠偏，实现自动导向。

上述技术方案的步骤4中，掘进机控制模块2根据上述掘进机的实时水平偏向位移和掘进机的实时偏向角数据发出控制指令使掘进机进行动态纠偏的具体方法为掘进机控制模块2获取掘进机的水平偏向位移和偏向角后，首先掘进机控制模块2调整掘进机右轮的速度，从而调整掘进机的偏向角，当掘进机的偏向角调整为0度时，再继续通过掘进机控制模块2调整掘进机右轮的速度，从而调整掘进机的水平偏向位移，这样不断重复，若掘进机右偏则调慢掘进机右轮速度，若掘进机左偏则调快掘进机右轮速度；

上述掘进机控制模块2控制掘进机右轮的实时速度由下式得到：

$V=V^{\′}\±K_c\×(X\×X-K_h\×X\×X^{\′})---\left(1\right)$

其中，V为掘进机控制模块2控制掘进机右轮的实时速度，V＇为上一时刻掘进机右轮的行走速度，X为掘进机的实际水平偏向位移，X＇为掘进机上一时刻的水平偏向位移，K_c为掘进机车轮电机扭力调整系数，K_h是惯性抑制系数；通过上述调整，最终将掘进机调整至中心轴线上，继续行进，实现掘进机得自动导向。

上述技术方案的步骤3和步骤4之间还包括步骤3.1：掘进机机身1四个角上的光电开关10向四周发出光信号，光信号碰到掘进巷道8后被反射回对应的光电开关10，每个光电开关10以此得到光电开关10与掘进巷道8之间的距离，每个光电开关10将该距离信息与光电开关10上预先设好的阈值进行比较，并将比较结果传输给中央处理器7，当有一个光电开关10与掘进巷道8之间的距离小于该阈值时，中央处理器7控制掘进机控制模块2改变运行轨迹，以使每个光电开关10与掘进巷道8之间的距离保持在大于该阈值。

上述技术方案的步骤4的公式1中当掘进机的水平偏向位移左偏时，如果掘进机的运动速度增加，则K_c＝０.２２，K_h＝０.９；如果掘进机的运动速度减小，则K_c＝２.２，K_h＝０.７；当掘进机的水平偏向位移右偏时，如果掘进机的运动速度增加，则K_c＝２．２，K_h＝０.７；如果掘进机的运动速度减小，则K_c＝０．２２，K_h＝０.９。

上述技术方案的步骤3.1中的阈值为20~40cm，优选为30cm。

本说明书未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (5)

1.一种基于摄像头和航姿仪的掘进自动导向系统的导向方法，所述基于摄像头和航姿仪的掘进自动导向系统它包括设置在掘进机机身(1)上的掘进机控制模块(2)，它还包括设置在掘进机机身(1)上的激光靶面(3)、CCD摄像头(4)、航姿仪(5)、数据采集模块(6)、中央处理器(7)以及设置在掘进巷道(8)上的激光指向仪(9)，所述激光指向仪(9)位于掘进机机身(1)后方，其中，所述激光指向仪(9)的激光发射头对准激光靶面(3)的中心，所述CCD摄像头(4)投射在激光靶面(3)上，所述CCD摄像头(4)的信号输出端连接数据采集模块(6)，航姿仪(5)的信号输出端连接中央处理器(7)，所述数据采集模块(6)的通信端连接中央处理器(7)的通信端，中央处理器(7)的控制信号输出端连接掘进机控制模块(2)的控制信号输入端，它还包括分别设置在掘进机机身(1)四个角上的四个光电开关(10)，所述每个光电开关(10)的信号输出端均连接中央处理器(7)，其特征在于，基于摄像头和航姿仪的掘进自动导向系统的导向方法包括如下步骤：

步骤1:打开激光指向仪(9)，并控制激光指向仪(9)射出与掘进巷道(8)平行的激光，该激光打在激光靶面(3)的中心；

步骤2：掘进机开始工作并向前运动，CCD摄像头(4)捕获激光靶面(3)上的激光光斑，数据采集模块(6)采集到激光光斑实时位置数据，数据采集模块(6)将激光光斑实时位置数据传输给中央处理器(7)，中央处理器(7)运算得到实时激光光斑中心点坐标，通过实时激光光斑中心点坐标得到掘进机的实时水平偏向位移，所述水平偏向位移为在水平方向上偏离中心的距离；

步骤3：在掘进机向前运动的过程中，航姿仪实时采集掘进机运动时的偏向角，并将掘进机的实时偏向角数据输送到中央处理器(7)；

步骤4：中央处理器(7)将掘进机的实时水平偏向位移和掘进机的实时偏向角数据输送给掘进机控制模块(2)，掘进机控制模块(2)根据上述掘进机的实时水平偏向位移和掘进机的实时偏向角数据发出控制指令使掘进机进行动态纠偏，实现自动导向；

所述步骤4中，掘进机控制模块(2)根据上述掘进机的实时水平偏向位移和掘进机的实时偏向角数据发出控制指令使掘进机进行动态纠偏的具体方法是：掘进机控制模块(2)获取掘进机的水平偏向位移和偏向角后，首先掘进机控制模块(2)调整掘进机右轮的速度，从而调整掘进机的偏向角，当掘进机的偏向角调整为0度时，再继续通过掘进机控制模块(2)调整掘进机右轮的速度，从而调整掘进机的水平偏向位移，这样不断重复，若掘进机右偏则调慢掘进机右轮速度，若掘进机左偏则调快掘进机右轮速度；

上述掘进机控制模块(2)控制掘进机右轮的实时速度由下式得到：

V＝V'±K_c×(X×X-K_h×X×X')    (1)

其中，V为掘进机控制模块(2)控制掘进机右轮的实时速度，V'为上一时刻掘进机右轮的行走速度，X为掘进机的实际水平偏向位移，X'为掘进机上一时刻的水平偏向位移，K_c为掘进机车轮电机扭力调整系数，K_h是惯性抑制系数；通过上述调整，最终将掘进机调整至中心轴线上，继续行进，实现掘进机得自动导向；

所述步骤3和步骤4之间还包括步骤3.1：掘进机机身(1)四个角上的光电开关(10)向四周发出光信号，光信号碰到掘进巷道(8)后被反射回对应的光电开关(10)，每个光电开关(10)以此得到光电开关(10)与掘进巷道(8)之间的距离，每个光电开关(10)将该距离信息与光电开关(10)上预先设好的阈值进行比较，并将比较结果传输给中央处理器(7)，当有一个光电开关(10)与掘进巷道(8)之间的距离小于该阈值时，中央处理器(7)控制掘进机控制模块(2)改变运行轨迹，以使每个光电开关(10)与掘进巷道(8)之间的距离保持在大于该阈值。

2.根据权利要求1所述的基于摄像头和航姿仪的掘进自动导向系统的导向方法，其特征在于：所述数据采集模块(6)的通信端通过SPI总线连接中央处理器(7)的通信端；所述航姿仪(5)的信号输出端通过RS485总线连接中央处理器(7)；所述中央处理器(7)的控制信号输出端通过CAN总线连接掘进机控制模块(2)的控制信号输入端。

3.根据权利要求2所述的基于摄像头和航姿仪的掘进自动导向系统的导向方法，其特征在于：所述每个光电开关(10)的信号输出端均通过RS485总线连接中央处理器(7)。

4.根据权利要求1所述的基于摄像头和航姿仪的掘进自动导向系统的导向方法，其特征在于：所述步骤4的公式1中当掘进机的水平偏向位移左偏时，如果掘进机的运动速度增加，则K_c＝0.22，K_h＝0.9；如果掘进机的运动速度减小，则K_c＝2.2，K_h＝0.7；当掘进机的水平偏向位移右偏时，如果掘进机的运动速度增加，则K_c＝2.2，K_h＝0.7；如果掘进机的运动速度减小，则K_c＝0.22，K_h＝0.9。

5.根据权利要求1所述的基于摄像头和航姿仪的掘进自动导向系统的导向方法，其特征在于：所述步骤3.1中的阈值为20～40cm。