CN113799118B - 一种基于机器视觉和运动控制的煤矿搜救机器人 - Google Patents

Abstract

一种基于机器视觉和运动控制的煤矿搜救机器人，步骤1，采集煤矿搜救机器人的环境数据；步骤2，训练矿道遇难人员检测模型；步骤3，设计煤矿搜救机器人的运动控制系统：步骤4，设计煤矿搜救机器人的初级导航系统；步骤5，数据处理储存，在发生矿难的实际情况下，通信线路已经被完全摧毁或部分瘫痪，通信已无法使用，故将搜救机器人收集到的相关数据经过一定的编组封装，遵循SPI通信协议写入SD卡储存模块，并且作为返航的数据支持。本发明设计将井下数据通过机器人深入现场探测获取，有利于现场救援的规划及指挥，保障被困人员存活率，减少不必要的损失及灾害。

Description

一种基于机器视觉和运动控制的煤矿搜救机器人

技术领域

本发明涉及机器人控制领域，特别是涉及一种基于机器视觉和运动控制的煤矿搜救机器人。

背景技术

我国拥有十分巨大的煤炭资源储量，据资料显示，在世界上我国在煤炭领域是最大的消费国和生产国。据统计显示，在中国的能源产业总量中，煤炭所贡献的能源量占据全国一次能源消耗与生产的将近70％，由此趋势向后估计，该比例将于2050年达到50％以上。故此，煤炭的开采与使用，在中国现在与未来的发展始终的一个主要的能源支柱，由此引申出的煤炭产业，势必需要相关的支持及保障。保障的其中一个主要方面，就是矿难救援工作。我国的煤炭生产工业发达，产量巨大，巨大的基数和中国较差的矿井地质及矿井高浓度瓦斯势必也同时导致中国是一个矿难事件的高发地，频发的矿难事件给煤矿开采工作造成巨大困扰。

井下矿难事故发生后，井下情况复杂多变，多种灾害发生，危险的环境状况和未知的现场状况并不适合由救援人员第一时间进入现场展开勘察和救援工作。我国煤矿井结构复杂，瓦斯浓度较高等等原因不适合直接从国外引进相关救援机器人作为救援辅助工作。如今随着现代电子科技的高度发展，各项技术的逐步完善，由此衍生的智能技术、机器人技术使得由智能设备如机器人协调的救援方案成为现实。研发相关功能的救援机器人，可以代替人力进入危险环境展开救援工作，一来有利于保障人员的安全，二来可以更加迅速专业的采集各式数据情况。

井下矿难状况是一个复杂的复合的状态，所探测的数据包括有各式有害有毒化学气体浓度，现场温度，被困人员的位置及状态，现场的是否具备人类生存条件，矿道变形坍塌情况等等。将这些数据透过机器人深入现场探测获取，有利于现场救援的规划及指挥，保障被困人员存活率，减少不必要的损失及灾害。

发明内容

为解决上述问题，本发明提出了一种基于机器视觉和运动控制的煤矿搜救机器人，通过机器人深入现场探测获取井下数据，对遇难人员进行搜救，保障被困人员存活率，减少不必要的损失及灾害，本发明提供一种基于机器视觉和运动控制的煤矿搜救机器人，具体步骤如下，其特征在于：

步骤1，采集煤矿搜救机器人的环境数据：采集矿道的路径情况以及沿途矿道变形情况数据，由舵机配合距离传感器和CCD传感器对沿途矿道进行扫描获得矿道变形数据；

步骤2，训练矿道遇难人员检测模型：将检测的数据集分别提取图像的轮廓特征和轮廓特征的显著性检测值，并将显著性检测指作为输入训练BP神经网络分类模型；

步骤3，设计煤矿搜救机器人的运动控制系统：车体采用四个直流电机驱动的方式，并通过光电编码器测速装置、PID控制和PWM波输出完成对电机的合理调速，保障机器人行进方向的正确性防止偏离直线航线；

步骤4，设计煤矿搜救机器人的初级导航系统：该系统是一个测试性系统，对机器人进行导航设计，主要分为三个部分，分为遇难人员检测系统、智能避障系统和自动返航系统；

步骤5，数据处理储存，在发生矿难的实际情况下，通信线路已经被完全摧毁或部分瘫痪，通信已无法使用，故将搜救机器人收集到的相关数据经过一定的编组封装，遵循SPI通信协议写入SD卡储存模块，并且作为返航的数据支持。

进一步，步骤2，训练矿道遇难人员检测模型的过程可以表示为：

步骤2.1，通过CCD传感器采集矿道内的数据图像，组成训练数据图像集；

步骤2.2，提取数据图像的轮廓特征：

将采集的图像数据做二值化处理，计算图像质心(x_k,y_k),并计算图像二值数据至质心的距离d_i:

其中，(x_i,y_i)i＝1,2,…,n是二值图像数据；

步骤2.3，提取轮廓特征的显著性检测值：

c_i＝cos(r_i) (3)

Q_i＝cos^-1(s_i) (5)

式中，

表示轮廓特征的均值，T₁是显著性检测阈值，r_i表示轮廓特征去均值后的特征数据，Q_i表示轮廓特征的显著性检测值；

步骤2.4，将训练数据集提取的轮廓特征显著性检测值作为输入，矿道中遇难人员目标的标签作为输出训练三层BP神经网络分类模型，并获得训练完成的BP神经网络遇难人员检测模型。

进一步，步骤3中设计煤矿搜救机器人的运动控制系统的过程可以表示如下：

为实现搜救机器人的平稳运行以及实时避障，本发明使用PID算法对机器人的电机进行控制，通过光电测速编码器的输出进行解析，计算电机的转速值，将转速值作为反馈实现PID的运算。

PID控制的主要原理是从被控端读取控制数据，并由光电编码盘读取电机的实时转速，对比预设的转速参数(PWM波输出占空比)，将标定后作二者取差量e(t)，将偏差量 e(t)的比例项，积分项以及微分项相加运算后获得PWM波的占空比值，并发送给执行机构，最终PID控制的调整方程为：

其中，k_p，T_i，T_D分别是PID的比例积分和微分控制参数，u(t)为t时刻机器人PWM 波的占空比输出，PID控制的传输函数为：

PID控制在程序中执行时，PID调节为数字采样，因此需要对其进行离散化操作，由此离散PID控制的控制方程为：

在公式8中，u(n)为第n个采样时刻控制器的实时输出状态，e(n)为第n个采样时刻控制器输入量与上一时刻输出量的偏差量，式中，T为采样周期。

离散化的PID控制方式的积分项需要对第n个时刻前每个时刻的偏差量e(i)进行累加计算，由于在测量过程中，本就存在一些微小的不准确性，在多次测量中，这种微小的不准确性不停的进行叠加，造成大幅度的偏差，同时，连续的相加采样计算，对MCU造成极大的负担计算量，不适合实际操作。鉴于离散型PID控制的这些缺点，故在实际的 PID控制操作中，多采用增量式调节，该控制方式的表达式为：

Du(n)＝u(n)-u(n-1) (9)

对于PID参数的调整，其中工程整定法较为直接，能够在一次次的试验后，直接得出一套较为合适的参数，操作难度较低，同时参数可以直接使用，不用再经测试。本发明在程序上，先对主要的几个参数进行定义，再确定好采样周期，对PID控制的比例常数，积分常数，微分常数进行初步置数，用以之后测试实际控制情况，并最终进行整定。

之后，主程序设定好预设转速，读取实际转速，进入PID控制程序进行计算(计算方式参考公式(10)及(11))，得出偏差修正量，对输出PWM波进行占空比修正，从而到达PID控制目标。

本发明通过经过反复的测试，监控PID控制的测试情况，具体操作方法为向上位机串口打印实时转速信息，人工对其进行观察，确定控制效果。根据控制效果中反映的控制问题，例如调节时间，超调量，稳定性等等，反复调整比例常数，积分常数，微分常数，从而完成工程整定工作。特别的，在以上测试过程中，各参数需要在较为实际的载重情况下进行整定，实现对被控对象的实时调整监控。

进一步，步骤4中，设计煤矿搜救机器人的初级导航系统的过程可以表示如下：

运动控制系统的系统包括：遇难人员检测系统、智能避障系统和自动返航系统；

遇难人员检测系统是由CCD传感器和BP神经网络遇难人员检测模型组成，煤矿搜救机器人在矿道行驶的过程中通过CCD传感器获得矿道图像数据，同时经过步骤2.2和步骤2.3后，将图像数据的显著性检测值输入至BP神经网络遇难人员检测模型中，获得矿道中遇难人员的检测目标，如果检测模型检测到图像数据中存在遇难人员，则启动紧急救助工作；

智能避障系统是由超声波测距传感器和电机组成，四个不同位置的超声波测距传感器检测机器人周围障碍物的距离，根据预设数值对小车行进至不同障碍情况进行合理的判断和决策，对于特定的、无法行进的障碍做出终点判断，触发返航指令；

自动返航系统主要由SD卡组成，在机器人进行搜救的过程中，先正向记录行进坐标数据，出现返航指令后，激活自动返航系统；首先从SD卡中逆向读取沿途坐标信息，通过MPU6050和测速编码器测算，计算实时的行进坐标，在与坐标对比后控制机器人向下一个目标坐标进行行进并实时矫正路径，此过程中智能避障系统弱化地进行指挥，保障行进安全。

本发明一种基于机器视觉和运动控制的煤矿搜救机器人，有益效果：本发明的技术效果在于：

1.本发明在提取图像中轮廓特征的技术上，对特征进行显著性检测，相比直接将特征输入到BP神经网络中，经显著性检测后的模型检测精度更高；

2.本发明解决了危险的环境状况和未知的现场状况并不适合由救援人员第一时间进入现场展开勘察和救援工作的问题。

3.本发明将井下数据通过机器人深入现场探测获取，有利于现场救援的规划及指挥，保障被困人员存活率，减少不必要的损失及灾害。

附图说明

图1为本发明的运动控制系统的硬件及逻辑构成图；

图2为本发明的运动控制系统硬件构成图；

图3为本发明的智能避障系统流程图；

图4为自动返航组流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提出了一种基于机器视觉和运动控制的煤矿搜救机器人，旨在有利于现场救援的规划及指挥，保障被困人员存活率，减少不必要的损失及灾害，本发明的步骤作详细介绍如下。

步骤1，采集煤矿搜救机器人的环境数据：采集矿道的路径情况以及沿途矿道变形情况数据，由舵机配合距离传感器和CCD传感器对沿途矿道进行扫描获得矿道变形数据；

矿道数据扫描系统是机器人的“核心工作”，运动控制平台的诸多设置，都是为了能够携带矿道扫描系统进入矿道内，执行它应有的任务，而且整个设计任务都是为了能够获取灾后矿道信息，为救援规划做好准备策划工作。所以，矿道扫描系统是整个系统的重中之重。

矿道扫描系统的职责主要在于对沿途路径的矿道形状信息进行采集，为绘制矿道3D概图提供数据支持，也可为救援人员判断矿道形变情况提供数据支持，同时也为遇难人员的检测模型提供检测数据。主要的研究步骤在于，舵机的驱动、CCD传感器和红外测距扫描。下属一个舵机机构用以带动扫描器件对矿道进行切面式扫描，一只红外测距模块作为扫描器件检测舵机在旋转各角度上的距离(高度)信息，CCD传感器定时采集矿道图像。

舵机驱动在于先理解舵机的工作方式，通过核心芯片产生特定频率及占空比的PWM波，完成对舵机旋转固定角度的测试，之后开始递增递减占空比，控制舵机旋转式驱动。

红外测距测试在于先对ADC转换进行测试，之后匹配测距模块与内部ADC转换模块的参考电压，进行红外测距的测试，之后向程序注入公式完成从转换后数字量到电压值的计算，优化采样(取平均数，去除坏值等)逻辑以调高测距数据的精度。最后对舵机和红外测距模块进行联调，在每一个角度上激活测距采样，完成既定任务。

步骤2，训练矿道遇难人员检测模型：将检测的数据集分别提取图像的轮廓特征和轮廓特征的显著性检测值，并将显著性检测指作为输入训练BP神经网络分类模型；

步骤2，训练矿道遇难人员检测模型的过程可以表示为：

步骤2.1，通过CCD传感器采集矿道内的数据图像，组成训练数据图像集；

步骤2.2，提取数据图像的轮廓特征：

将采集的图像数据做二值化处理，计算图像质心(x_k,y_k),并计算图像二值数据至质心的距离d_i:

其中，(x_i,y_i)i＝1,2,…,n是二值图像数据；

步骤2.3，提取轮廓特征的显著性检测值：

c_i＝cos(r_i) (3)

Q_i＝cos^-1(s_i) (5)

式中，

表示轮廓特征的均值，T₁是显著性检测阈值，r_i表示轮廓特征去均值后的特征数据，Q_i表示轮廓特征的显著性检测值；

步骤2.4，将训练数据集提取的轮廓特征显著性检测值作为输入，矿道中遇难人员目标的标签作为输出训练三层BP神经网络分类模型，并获得训练完成的BP神经网络遇难人员检测模型。

步骤3，设计煤矿搜救机器人的运动控制系统：车体采用四个直流电机驱动的方式，并通过光电编码器测速装置、PID控制和PWM波输出完成对电机的合理调速，保障机器人行进方向的正确性防止偏离直线航线；

步骤3中设计煤矿搜救机器人的运动控制系统的过程可以表示如下：

为实现搜救机器人的平稳运行以及实时避障，本发明使用PID算法对机器人的电机进行控制，通过光电测速编码器的输出进行解析，计算电机的转速值，将转速值作为反馈实现PID的运算。

PID控制的主要原理是从被控端读取控制数据，并由光电编码盘读取电机的实时转速，对比预设的转速参数(PWM波输出占空比)，将标定后作二者取差量e(t)，将偏差量 e(t)的比例项，积分项以及微分项相加运算后获得PWM波的占空比值，并发送给执行机构，最终PID控制的调整方程为：

其中，k_p，T_i，T_D分别是PID的比例积分和微分控制参数，u(t)为t时刻机器人PWM 波的占空比输出，PID控制的传输函数为：

PID控制在程序中执行时，PID调节为数字采样，因此需要对其进行离散化操作，由此离散PID控制的控制方程为：

在公式8中，u(n)为第n个采样时刻控制器的实时输出状态，e(n)为第n个采样时刻控制器输入量与上一时刻输出量的偏差量，式中，T为采样周期。

离散化的PID控制方式的积分项需要对第n个时刻前每个时刻的偏差量e(i)进行累加计算，由于在测量过程中，本就存在一些微小的不准确性，在多次测量中，这种微小的不准确性不停的进行叠加，造成大幅度的偏差，同时，连续的相加采样计算，对MCU造成极大的负担计算量，不适合实际操作。鉴于离散型PID控制的这些缺点，故在实际的 PID控制操作中，多采用增量式调节，该控制方式的表达式为：

Du(n)＝u(n)-u(n-1) (9)

对于PID参数的调整，其中工程整定法较为直接，能够在一次次的试验后，直接得出一套较为合适的参数，操作难度较低，同时参数可以直接使用，不用再经测试。本发明在程序上，先对主要的几个参数进行定义，再确定好采样周期，对PID控制的比例常数，积分常数，微分常数进行初步置数，用以之后测试实际控制情况，并最终进行整定。

之后，主程序设定好预设转速，读取实际转速，进入PID控制程序进行计算(计算方式参考公式(10)及(11))，得出偏差修正量，对输出PWM波进行占空比修正，从而到达PID控制目标。

本发明通过经过反复的测试，监控PID控制的测试情况，具体操作方法为向上位机串口打印实时转速信息，人工对其进行观察，确定控制效果。根据控制效果中反映的控制问题，例如调节时间，超调量，稳定性等等，反复调整比例常数，积分常数，微分常数，从而完成工程整定工作。特别的，在以上测试过程中，各参数需要在较为实际的载重情况下进行整定，实现对被控对象的实时调整监控。

步骤4，设计煤矿搜救机器人的初级导航系统：该系统是一个测试性系统，对机器人进行导航设计，主要分为三个部分，分为遇难人员检测系统、智能避障系统和自动返航系统；

步骤4中，设计煤矿搜救机器人的初级导航系统的过程可以表示如下：

本发明运动控制系统的系统包括：遇难人员检测系统、智能避障系统和自动返航系统。运动控制系统由四个直流电机，两个电机驱动模块，四只超声波测距模块，一个MPU6050三轴传感器和CCD传感器组成，运动控制系统的硬件及逻辑构成如图1所示。经过对于控制任务的考虑，选择STM32F103ZET6作为本次任务的核心控制芯片。为了读取小车的实时速度，并最终进行PID控制，需要对光电测速编码盘的输出进行解析，输出脉冲的周期即为光电测速编码盘接收到光信号的周期，即为电机旋转周期，故对脉冲的上升沿连续触发捕获并进行计算，即可最终计算出转速值。

STM32F103ZET6的高级定时器和基本定时器具有输入捕获功能，具体为通过定时器的输入端口，检测该端口的电平，在电平发生变化的瞬间，将实时的定时器中的计数值(TIMx_CNT)存入进对应的输入/比较寄存器(TIMx_CCRx)中去，即可记录下实时发送电平跳跃的时刻，煤矿搜救机器人的运动控制系统硬件构成图如图2所示。

在光电编码盘测速中，通过检测输出脉冲的快慢程度，结合计算公式，即可以实时地得到某个电机实时的转速，为PID控制提供数据支持。通过监测输入口上同一电平跳变方式的出现时刻，在连续记录数个时刻后依据定时器时钟频率计算相邻两个记录时刻的时间差，将多个时间差滤除坏值后取平均值，得到脉冲周期，即可完成测速。

电机驱动模块采样L298N直流电机驱动器，该驱动器通过外接12V供电电源，由STM32F103ZET6四路输出PWM波(两路为一组)接入驱动器，增强PWM波能量，将处理后的PWM波四路输出(两路为一组)，接入后续电机中，驱动电机工作。本专利所用直流电机安装于固定底盘四周，包括有轮子及光电测速编码盘，用以携带设备进行移动，主要器件为四只直流电机以及光电测速编码盘。将四组(每组两路PWM波)接入小车平台，驱动四只电机旋转，带动平台运动。由占空比控制电机转速，由两路PWM波其中一路置零控制电机正反转，例如一路输出占空比为4/9的PWM波，一路输出占空比为 0/9的PWM波。

对于输出的，携带实时转速的脉冲信号，在第一个上升沿信号到来时，将定时器计数寄存器(TIMx_CNT)置零，之后在每个上升沿到来时，使用相关函数，读取输入/比较寄存器(TIMx_CCRx)中的数值并记录于数组之中，连续读取(包括第一个上升沿) 五个上升沿后，将数组中的值做差得出时间差，去除偏差量过大坏值后，将剩余数值取平均值作为转速信息值。

依照定时器输出引脚，定时器输入引脚等规则，连接控制芯片，驱动器以及电机。经过整体项目考虑，芯片分配给电机驱动组的引脚如表1所示。

表1电机驱动组引脚分配

根据表1进行引脚分配，连接好电机驱动模块，电源驱动模块。

遇难人员检测系统是由CCD传感器和BP神经网络遇难人员检测模型组成，煤矿搜救机器人在矿道行驶的过程中通过CCD传感器获得矿道图像数据，同时经过步骤2.2和步骤2.3后，将图像数据的显著性检测值输入至BP神经网络遇难人员检测模型中，获得矿道中遇难人员的检测目标，如果检测模型检测到图像数据中存在遇难人员，则启动紧急救助工作。

智能避障系统是由超声波测距传感器和电机组成，四个不同位置的超声波测距传感器检测机器人周围障碍物的距离，根据预设数值对小车行进至不同障碍情况进行合理的判断和决策，对于特定的、无法行进的障碍做出终点判断，触发返航指令。智能避障系统如下所示：

(1)定时器输入捕获

与电机驱动组中光电测速编码盘的输入捕获逻辑有所不同的是，光电测速编码盘在读取速度信息时依据脉冲波的周期，故需要连续检测同一种触发条件(上升沿触发/下降沿触发)的间隔时间，但是在自动避障的超声波测距检测中，需要测量一个脉冲内高电平的持续时间。

在超声波脉冲波输入端测量的脉冲波高电平时间长度与该实时环境下声速(声速的主要决定因素是实时环境温度，该温度由MPU6050可以测出)的乘积，所得出的距离数据，就是此刻此超声波测距模块所采集到的该方向上的障碍距离。

为了完成对一个脉冲内的高电平持续时间的测量，需要改变输入捕获的捕获条件，由于超声波测距模块输出信号的特点，需要先将捕获条件设定为上升沿触发，在检测到上升沿时，重置定时器计数寄存器(TIMx_CNT)中的值，并且将触发条件设定为下降沿触发。在检测到下降沿时正常运作输入捕获程序即是将计数寄存器(TIMx_CNT)中的值存入对应的输入/比较寄存器(TIMx_CCRx)中去，读取输入/比较寄存器(TIMx_CCRx) 中的值并依据对应定时器的工作频率计算时间长度，最终计算距离信息。

程序编写上，对于单组超声波测距模块，只需要设置好输入捕获模式，启动超声波模块，在对应通道上升沿时，于中断服务函数中判断是由上升沿触发还是下降沿触发。确定为上升沿后，重置定时器计数寄存器(TIMx_CNT)中的值，并且使用独立极性设置函数，将触发条件设定为下降沿触发，清除对应中断标志位。在对应通道下降沿到来时，于中断服务函数中判断是由上升沿触发还是下降沿触发，确定为下降沿后，使用输入/比较寄存器(TIMx_CCRx)读取函数，读取寄存器中数值，计算时间差值并存入预设距离信息寄存器中，将触发条件设定为上升沿触发，清除对应中断标志位，开始下一次检测。

对于多组的超声波测距模块，需要用数组做好数据分类，依次进行单组测距工作，防止跨组触发中断服务函数，其具体操作过程及其流程图如图3所示。

(2)自动避障逻辑

自动避障逻辑主要是在基于超声波测距模块读取的距离信息，对比预设的距离数值，按照预设方案向电机驱动组下达避障命令(PWM波形式)。超声波测距模块装载位置的障碍信息及其避障方案如表2所示。(即一号超声波测距模块、即二号超声波测距模块、即三号超声波测距模块、即四号超声波测距模块)

表2避障方案

对于“正前”障碍，在该状态下，先进行强制15度左(右)转向(MPU6050监测转向角度)，防止连续抖动性转向，检测现状，若依然处于“正前”障碍状态，继续强制 15度左(右)转向，循环以上过程，寻找可以前进的角度。在强制左(右)转向累计90 度依然无法前进(“正前”障碍状态)，强制105度右(左)转向，检测状态，若依然处于“正前”障碍状态，继续强制15度右(左)转向，循环以上过程，寻找可以前进的角度。在强制左(右)转向累计90度依然无法前进(“正前”障碍状态),做出终点判断。

对于“左右侧”，对与差值造成的转向判断，加入阈值区，即二者差值必须大于10cm才能触发转向命令，防止连续抖动性转向。在二者任何值小于10cm时，进行终点判断。所谓终点判断，即为机器人判断已到达无法继续前进的位置，开始触发自动返航程序。

自动返航系统主要由SD卡组成，在机器人进行搜救的过程中，先正向记录行进坐标数据，出现返航指令后，激活自动返航系统。首先从SD卡中逆向读取沿途坐标信息，通过MPU6050和测速编码器测算，计算实时的行进坐标，在与坐标对比后控制机器人向下一个目标坐标进行行进并实时矫正路径，此过程中智能避障系统弱化地(降低警戒距离识别标准)进行指挥，保障行进安全。

自动返航需要对机器人进行实时定位处置，而在实时定位时除了使用光电测速编码盘读取速度信息而计算得出的距离信息外，还需要进行姿态检测计算行进角度。MPU6050可以执行角度检测任务，该芯片采用IIC通信方式与主控芯片进行连接。

(1)MPU6050六轴传感器

MPU6050是全球首款整合性6轴运动处理组件，该芯片组由InvenSense公司推出。其芯片内自带有3轴陀螺仪以及3轴加速度传感器，硬件板上集成的第二IIC接口，可用于对外部磁力传感器的连接。

MPU6050通过使用自带数字运动处理器(DMP:Digital Motion Processor)硬件加速引擎整合计算角度信息，之后通过主IIC通信接口，向应用端输出完整的9轴姿态融合演算数据。

此次设计并未集成外部磁力传感器，仅采用内部6轴传感器进行运动姿态的解算工作，故在数据通信上，仅使用主IIC接口。再使用AD0进行地址设置，片选引脚默认长期选中，不使用MPU6050产生的中断信号。

在MPU6050初始化上，先对IIC通信接口进行初始化，再对MPU6050进行复位操作(由电源管理寄存器1(0X6B)控制)，接着设置角速度传感器和加速度传感器的满量程测量范围(由陀螺仪配置寄存器(0X1B)和加速度传感器配置寄存(0X1C)设置)，接着按需求设置一些其他寄存器(例如，中断使能寄存器(0X38)、由户控制寄存器(0X6A)、FIFO 使能寄存器(0X23)、采样率分频寄存器(0X19)、配置寄存器(0X1A)等)，接着设置系统时钟(电源管理寄存器1(0X6B))，一般将x轴陀螺的PLL作为时钟源，完成基本设置后，最后启动角速度传感器(陀螺仪)和加速度传感器(由电源管理寄存器2(0X6C)配置)。

(2)自动返航逻辑

编写自动返航，需要先获取到返航前机器人行进过程中的坐标信息，这就需要在机器人进行过程中读取实时的速度信息(光电测速编码盘提供)和角度信息(MPU6050提供)。

路径的记录方式采用以直代曲的方式进行，只记录二维行进信息即方向角信息，在每个方向角变化的位置，一般为激活自动避障时，记录转向信息和导航坐标位置(即此刻实时坐标)，即时坐标位置由实时的速度信息和角度信息即时更新。

在两次的导航坐标记录点之间，用基本定时器进行计时并与实时速度乘积计算距离，将小距离累加记为两个坐标点之前的距离，与导航坐标时一同顺序写入SD卡。

特别的，在写入导航信息组时需要实时更新SD卡扇区位置信息，用于返航时正确的进行逆序信息读取。

在终点判断(避障组提供)下达时，启动返航程序，从SD卡读取导航信息组，机器人180度转向，开始按照坐标以及坐标距离信息进行导航，需要注意的是，从SD卡读取的角度信息需要进行二次计算。

于此同时，即时坐标位置依然处于实时更新状态，在每个转向坐标验证位置信息是否正确，计算二者坐标，若直线偏差距离超过30cm则进行矫正操作。循环上述过程，返回坐标(0.0)位置即机器人投放点，自动返航组流程图如图4所示。

步骤5，数据处理储存，在发生矿难的实际情况下，通信线路已经被完全摧毁或部分瘫痪，通信已无法使用，故将搜救机器人收集到的相关数据经过一定的编组封装，遵循SPI通信协议写入SD卡储存模块，并且作为返航的数据支持。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (1)

1.一种基于机器视觉和运动控制的煤矿搜救机器人，具体步骤如下，其特征在于：

步骤1，采集煤矿搜救机器人的环境数据：采集矿道的路径情况以及沿途矿道变形情况数据，由舵机配合距离传感器和CCD传感器对沿途矿道进行扫描获得矿道变形数据；

步骤2，训练矿道遇难人员检测模型：将检测的数据集分别提取图像的轮廓特征和轮廓特征的显著性检测值，并将显著性检测指作为输入训练BP神经网络分类模型；

步骤2，训练矿道遇难人员检测模型的过程表示为：

步骤2.1，通过CCD传感器采集矿道内的数据图像，组成训练数据图像集；

步骤2.2，提取数据图像的轮廓特征：

将采集的图像数据做二值化处理，计算图像质心(x_k,y_k),并计算图像二值数据至质心的距离d_i:

其中，(x_i,y_i)i＝1,2,…,n是二值图像数据；

步骤2.3，提取轮廓特征的显著性检测值：

c_i＝cos(r_i) (3)

Q_i＝cos^-1(s_i) (5)

式中，

表示轮廓特征的均值，T₁是显著性检测阈值，r_i表示轮廓特征去均值后的特征数据，Q_i表示轮廓特征的显著性检测值；

步骤2.4，将训练数据集提取的轮廓特征显著性检测值作为输入，矿道中遇难人员目标的标签作为输出训练三层BP神经网络分类模型，并获得训练完成的BP神经网络遇难人员检测模型；

步骤3，设计煤矿搜救机器人的运动控制系统：车体采用四个直流电机驱动的方式，并通过光电编码器测速装置、PID控制和PWM波输出完成对电机的合理调速，保障机器人行进方向的正确性防止偏离直线航线；

步骤3中设计煤矿搜救机器人的运动控制系统的过程表示如下：

为实现搜救机器人的平稳运行以及实时避障，使用PID算法对机器人的电机进行控制，通过光电测速编码器的输出进行解析，计算电机的转速值，将转速值作为反馈实现PID的运算；

PID控制是从被控端读取控制数据，并由光电编码盘读取电机的实时转速，对比预设的PWM波占空比转速参数，将标定后作二者取差量e(t)，将偏差量e(t)的比例项，积分项以及微分项相加运算后获得PWM波的占空比值，并发送给执行机构，最终PID控制的调整方程为：

其中，k_p，T_i，T_D分别是PID的比例积分和微分控制参数，u(t)为t时刻机器人PWM波的占空比输出，PID控制的传输函数为：

PID控制在程序中执行时，PID调节为数字采样，对其进行离散化操作，由此离散PID控制的控制方程为：

在公式8中，u(n)为第n个采样时刻控制器的实时输出状态，e(n)为第n个采样时刻控制器输入量与上一时刻输出量的偏差量，式中，T为采样周期；

离散化的PID控制方式的积分项需要对第n个时刻前每个时刻的偏差量e(i)进行累加计算，由于在测量过程中存在累计偏差，故采用增量式调节，该控制方式的表达式为：

Du(n)＝u(n)-u(n-1) (9)

对于PID参数的调整，采用工程整定法得出控制参数；

步骤4，设计煤矿搜救机器人的初级导航系统：该系统是一个测试性系统，对机器人进行导航设计，分为三个部分，分别为遇难人员检测系统、智能避障系统和自动返航系统；

步骤4中，设计煤矿搜救机器人的初级导航系统的过程表示如下：

运动控制系统的系统包括：遇难人员检测系统、智能避障系统和自动返航系统；

遇难人员检测系统是由CCD传感器和BP神经网络遇难人员检测模型组成，煤矿搜救机器人在矿道行驶的过程中通过CCD传感器获得矿道图像数据，同时经过步骤2.2和步骤2.3后，将图像数据的显著性检测值输入至BP神经网络遇难人员检测模型中，获得矿道中遇难人员的检测目标，如果检测模型检测到图像数据中存在遇难人员，则启动紧急救助工作；

智能避障系统是由超声波测距传感器和电机组成，四个不同位置的超声波测距传感器检测机器人周围障碍物的距离，根据预设数值对小车行进至不同障碍情况进行合理的判断和决策，对于特定的、无法行进的障碍做出终点判断，触发返航指令；

自动返航系统主要由SD卡组成，在机器人进行搜救的过程中，先正向记录行进坐标数据，出现返航指令后，激活自动返航系统；首先从SD卡中逆向读取沿途坐标信息，通过MPU6050和测速编码器测算，计算实时的行进坐标，在与坐标对比后控制机器人向下一个目标坐标进行行进并实时矫正路径，此过程中智能避障系统弱化地进行指挥，保障行进安全；

步骤5，数据处理储存，在发生矿难的实际情况下，通信线路已经被完全摧毁或部分瘫痪，通信已无法使用，故将搜救机器人收集到的相关数据经过一定的编组封装，遵循SPI通信协议写入SD卡储存模块，并且作为返航的数据支持。

Images