CN114841260A - 矿井下多传感器信息融合自主避障导航方法 - Google Patents

Abstract

本发明公开了一种矿井下多传感器信息融合自主避障导航方法，包括以下步骤；步骤1：通过超声波测距方法，实时监测获取障碍物的距离信息；步骤2：构建无人驾驶车辆下位机控制系统；步骤3：通过环境特征提取，对无人车辆目的地进行定位；步骤4：对多源数据融合感知，实现矿井下无人车辆感知障碍、自主避障以及井下自主定位导航。本发明利用iBeacon粗定位和激光雷达矢量线段匹配细定位对井下环境进行定位导航，基于融合感知的方法获取障碍分布距离信息，并通过蓝牙基站信号强度与距离的关系拟合曲线降低距离误差，以此来提高定位导航精度，解决现有技术中存在的不足。

Description

矿井下多传感器信息融合自主避障导航方法

技术领域

本发明涉及多源信息融合技术领域，具体涉及一种矿井下多传感器信息融合自主避障导航方法。

背景技术

煤矿井下无人驾驶车辆是一种能够在无监督状态下实现井下物资运输的设备。井下无人驾驶技术的研发有助于降低因调度、操作失误而发生运输事故的概率。井下环境实施机车无人驾驶的主要苦难包括运输大巷人车共用、巷道狭窄、光照条件不均匀，无法利用卫星定位系统，缺乏有效的通信手段等。恶劣的环境因素给井下无人驾驶在自主导航定位及避障方面带来严峻挑战。

发明内容

为了克服以上技术问题，本发明提供了一种矿井下多传感器信息融合自主避障导航方法，利用iBeacon粗定位和激光雷达矢量线段匹配细定位对井下环境进行定位导航，基于融合感知的方法获取障碍分布距离信息，并通过蓝牙基站信号强度与距离的关系拟合曲线降低距离误差，以此来提高定位导航精度，解决现有技术中存在的不足。

为了实现上述目的，本发明采用的技术方案是：

一种矿井下多传感器信息融合自主避障导航方法，包括以下步骤；

步骤1：通过超声波测距方法，实时监测获取障碍物的距离信息；

步骤2：构建无人驾驶车辆下位机控制系统；

步骤3：通过环境特征提取，对无人车辆目的地进行定位；

步骤4：对多源数据融合感知，实现矿井下无人车辆感知障碍、自主避障以及井下自主定位导航。

所述步骤1通过在无人驾驶车辆前端安装超声波测距传感器，实时传感检测井下无人车辆前方是否存在障碍以及距离障碍的距离，用于判断井下无人车辆的运行环境是矿井巷道，需要躲避井下障碍物、工作人员以及其他运输车辆，并将位移信号S转化为电压信号U，同时在无人车辆前方安装红外热感应传感器，用于判别前方物体是障碍物还是井下工作人员，通过检测温度进行区分，将温度信号T转化为电压信号U最终判断是绕行避障还是等待工作人员通过之后再继续行进，基于上述两种传感信息建立避障模型。

所述避障模型具体包括以下步骤；

设计无人驾驶车辆传感器的排布，之后建立无人驾驶车辆的融合障碍分布距离以及温度信息的避障模型，将无人驾驶车辆抽象为二维俯视简图，其中超声波测距传感器排布在不同的水平面内，然后设定超声波测距传感器的距离检测阈值，将检测到的数据与阈值进行对比，当测量值大于阈值时，对比值为0；当测量值小于或等于阈值时，对比值设为1，此时认定障碍物进入当前范围，障碍物检测的具体过程如下：

(1)启动车辆上安装的9个超声波传感器以及1个温度传感器；

(2)若超声波测距传感器全部对比值为0，则为类型1，判断是无障碍；

(3)若1、2、3号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型2，判断是前方有障碍；

(4)若4、5、6号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型3，判断是左侧有障碍；

(5)若7、8、9号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型4，判断是右侧有障碍；

(6)若1、2、3、4、5、6号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型5，判断是左侧和前方有障碍；

(7)若1、2、3、7、8、9号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型6，判断是右侧和前方有障碍；

(8)若超声波测距传感器全部对比值为1，则为类型7，判断是左右两侧和前方均有障碍。

所述步骤2中通过无人驾驶车辆下位机控制系统使用陀螺仪作为井下车辆航向角测量传感器，通过温度补偿和零位校正等获得可靠性高的航向角数据，该系统在无人车辆的驱动设备上安装编码器，采集反馈的脉冲数，以此来实时获取无人车辆驱动轮运动过程中的速度以及位移量等状态参数，实现井下车辆的闭环运行，然后，该系统将步骤1中获得的环境信息与无人车辆自状态信息输入到下位机控制模块STM32F103RCT6，设计闭环控制反馈算法，根据编码器的同源脉冲信息计算出采样间隔内驱动轮的转速，根据陀螺仪的异源信息进行航向角解算，结合无人车辆的驱动轮位移量差值来进一步优化运行稳定性，通过变速积分PID算法调整实际速度期望值大小，最终将电压U转换为驱动力矩M。

所述步骤3中采用基于激光雷达的室内定位导航功能，在建立矿井巷道环境全局二维地图的基础之上，利用激光雷达实时性好，鲁棒性高的特点进行井下定位导航，实现匹配局部路径进行导航以及智能自主路径判断，首先在矿井巷道安装蓝牙iBeacon基站，每个基站都具有特定的ID，可以区分激光雷达匹配信息，判断巷道环境，然后采用单独配置的用于特征数据处理的后台处理器miniPC对获取到的大量特征点信息进行分析，利用该方法监测信标之间的距离，确定无人车辆的目的地位置，并将位置信息反馈回后台处理系统，生成反馈控制信号。

通过蓝牙iBeacon粗定位和激光雷达矢量线段匹配细定位相结合进行复合定位导航；

首先，对激光雷达的同源信息进行融合，将实时扫描获取环境中的特征点信息融合构成矢量线段，将实时解算的矢量线段与全局二维地图进行局部特征匹配，其中，全局二维地图中的局部矢量线段特征信息表示为G＝{g_i|i＝1,2,…,a}，实时扫描解算的矢量线段特征信息表示为S＝{s_j|j＝1,2,…,b}，在扫描匹配矢量线段时，假设全局地图中g_i和实时扫描矢量线段s_j之间的匹配关系表示为m_ij＝(g_i,s_j)，根据特征信息匹配情况，在a×b的关系矩阵M_a×b中记录m_ij所有匹配关系，匹配度越高，表示g_i和s_j为同一特征的可能性越大，具体的匹配方法如下：

(1)优先级别最高的是矢量线段长度，假设L_i和L_j分别为g_i和s_j的长度，L_p和L_q分别为g_p和s_q的长度，长度差值阈值为ΔL。

考虑到扫描的s_j、s_q可能不是完整的g_i、g_p，所以实际扫描测量中一定存在L_j≤L_i以及L_q≤L_p。若L_i－L_j≤ΔL，则表示g_i和s_j长度匹配成立，认为m_ij可能是匹配的；若L_j＞L_i，则g_i和s_j不表示同一环境特征，那么无需比较分析其他的矢量线段关系，同理，可以对g_p和s_q做出相应的判断；

(2)第二优先级为相邻矢量线段夹角，假设A_ip为g_i和g_p的夹角，A_jq为s_j与s_q的夹角，角度差值阈值为ΔA；

在实际环境中，若g_i与s_j长度匹配，同时g_p和s_q长度也匹配，则A_ip和A_jq可能接近，若|A_ip－A_jq|≤ΔA，则认为m_ij或m_pq可能是匹配的；如果两对矢量线段中至少有一对长度不匹配，则不能确定A_ip与A_jq的关系，但可能其中一对矢量线段之间存在匹配的可能性；若|A_ip－A_jq|＞ΔA，说明至少存在一对不匹配，那么停止m_ij匹配，寻找下一个匹配矢量线段对；

(3)第三优先级为中点到相邻矢量线段的距离，Z_ip表示g_i的中点到g_p的距离，Z_jq表示s_j的中点到s_q的距离，距离差值阈值为ΔZ；

若g_i与l_j以及g_h与l_k满足长度、夹角要求，只要|Z_ip－Z_jq|≤ΔZ，则认为m_ij或m_pq存在较大的匹配可能性；若|Z_ip－Z_jq|＞ΔZ，说明至少存在一段矢量线段不匹配，那么停止m_ij匹配，寻找匹配下一个矢量线段对。

所述步骤4中首先对井下无人驾驶车辆的遇障情况进行抽象分析，设计不同的障碍物判断标准，然后基于自适应加权融合算法对测距信息进行分析处理，在获得障碍物距离信息以后，结合红外热感应传感器来判断障碍物的种类，通过激光雷达扫描获得的数据信息进行定位导航，利用特征点矢量线段匹配方法实时解算井下环境信息，根据无人车辆距巷道两壁的空间情况制定绕行路线，实现井下无人驾驶车辆的自主定位导航。

通过自适应加权融合算法对障碍距离信息进行处理分析，在保证总方差最小的条件下，根据传感器测量值，以自适应的方式寻找每个测量值对应的最优加权因子，提高无人驾驶车辆的测距精度和感知能力，假设所测量的实际距离值为L，而n个超声波测距传感器的测量值分别为l_i，其中i＝1,2,…,n，测量方差可以表示为σ_i ²(i＝1,2,…,n)，设定各个传感器的加权因子分别为a_i(i＝1,2,…,n)，应该满足式(1)中的关系：

测距传感器的总方差如式(2)所示：

根据式(1)和式(2)可以对采集的距离信息进行自适应加权融合解算，然后融合处理红外热感应传感器的异源信息对障碍物进行区分，判定监测到的障碍类别是固体障碍物还是矿井下的工作人员，其中，红外热感应传感器对于温度热量的监测属于一个开关量；

若红外热感应传感器监测到热量，则返回值为1，判定前方为井下工作人员，等待工作人员通过后继续行进；

若红外热感应传感器未监测到热量，则返回值为0，判定前方为固体障碍物，综合对比分析左右两侧障碍情况之后，选择较为空旷的一侧，进行绕行避障。

对蓝牙iBeacon基站的非线性传播信号进行信号强度与距离信息的关系曲线拟合，根据实际井下环境建立电磁波路径损耗模型计算无人车辆与iBeacon基站之间的直线距离，该模型的数学表达形式为：

RSSI(d)＝RSSI(d₀)-10αlg(d/d₀)+τ (3)

式中：

RSSI(d)——距离蓝牙iBeacon基站d米处接收到的信号强度值；

RSSI(d₀)——参考强度值，一般取参考距离d₀为1米；

α——无线信道衰减因子，其值由实际室内信道环境决定；

τ——均值为零，服从高斯分布的随机变量，由信号的多径效应引起；

设定无人车辆与蓝牙基站的距离阈值为3米，当连续检测到目标ID出的蓝牙基站信号强度值小于等于60dBm，则判定无人驾驶车辆已经进入目标巷道3米范围内，否则判定无人车辆还没有进入目标巷道。

本发明的有益效果：

本发明利用陀螺仪、超声波测距传感器、红外热感应传感器、激光雷达、编码器、蓝牙iBeacon等多种传感器获取的同源信息和异源信息，一方面对矿井巷道下可能存在的障碍物或工作人员进行距离检测以及避障控制，另一方面通过场景特征提取以及闭环反馈控制算法对目的地进行定位导航。该无人驾驶技术解决了矿井巷道环境下无人车辆的自动驾驶避障定位导航问题。

本发明设计了基于超声波传感器、红外热感应传感器以及下位机控制信号处理器的自主避障系统，该系统利用多源传感器，将采样获得的位移信号S以及采集温度信息T转化为电压信号U，并将该信号输入嵌入式处理器进行处理。构建多源传感器信号障碍物抽象化分析模型，判断障碍物和工作人员，利用自适应加权融合算法分析距离信息，最终对障碍物进行高精度定位解算，测距范围在2-450cm，测量精度为0.01cm，感应范围为±15°。

本发明设计了基于蓝牙iBeacon粗定位和激光雷达矢量线段匹配细定位相结合的复合定位导航方法，在矿井巷道内布置具有唯一ID的蓝牙基站获取全局地形信息，在无人车辆前端布置激光雷达获取井下巷道的环境特征信息，并将该信息转化为几何特征，融合特征点信息构成矢量线段，之后将该矢量线段与全局二维地图进行局部特征匹配，获得局部环境信息。为了避免井下无人车辆在相似度较高的巷道内迷失方向，通过蓝牙iBeacon基站对巷道的不同区域进行标记，根据基站信号强度值RSSI与距离之间的关系进行定位拟合，区分不同的巷道环境，解决巷道因相似度高导致的误判问题。

本发明通过多源传感信息融合的方法解决了矿井下无人驾驶车辆主动避障以及定位导航的问题，能够帮助矿井下的无人驾驶车辆规避障碍物，躲避井下工作人员，并在相似度高的矿井巷道内定位目标自动导航，降低了井下无人驾驶车辆因碰撞导致的事故发生机率，提高了其定位导航精度。

附图说明

图1是本发明9个超声波传感器以及测温传感器的布置位置示意图。

图2是本发明一实施例中自主避障流程设计图。

图3是本发明的测距传感器安装示意图。

图4是本发明的避障抽象化分析模型示意图。

图5是本发明的两级多传感器信息融合结构模型示意图。

具体实施方式

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

本发明的传感器布置方式如图1所示，分别在车体前端、左侧以及右侧各布置3各超声波测距传感器，超声波测距传感器的编号1-9如图中所示。在车体前端同时还布置有温度传感器，用于检测车辆前方是否有工作人员。本发明的自主避障流程如图2所示，该避障流程基于自适应加权融合算法，对布置在无人驾驶车辆前端的超声波测距传感器和红外热感应传感器获取到的障碍物信息进行加权融合。为了检测矿井巷道中大小各异，分布不具有规则性的障碍物，本发明采取了多高度、多角度的全方位立体式障碍物信息传感检测。

具体实施方案如下，如图1-图5所示；

第一步：设计无人驾驶车辆传感器的排布，如图3所示。之后建立无人驾驶车辆的融合障碍分布距离以及温度信息的避障模型，如图4所示，将无人驾驶车辆抽象为二维俯视简图，其中超声波测距传感器排布在不同的水平面内，以此增大传感器之间的间距，减小干扰信号，加强传感器对环境中障碍物尺寸、体积等信息的感知能力。然后设定超声波测距传感器的距离检测阈值，将检测到的数据与阈值进行对比，当测量值大于阈值时，对比值为0；当测量值小于或等于阈值时，对比值设为1，此时认定障碍物进入当前范围，障碍物检测的具体过程如下：

(1)若超声波测距传感器全部对比值为0，则为类型1，判断是无障碍；

(2)若1、2、3号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型2，判断是前方有障碍；

(3)若4、5、6号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型3，判断是左侧有障碍；

(4)若7、8、9号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型4，判断是右侧有障碍；

(5)若1、2、3、4、5、6号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型5，判断是左侧和前方有障碍；

(6)若1、2、3、7、8、9号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型6，判断是右侧和前方有障碍；

(7)若超声波测距传感器全部对比值为1，则为类型7，判断是左右两侧和前方均有障碍；

第二步：通过自适应加权融合算法对障碍距离信息进行处理分析，在保证总方差最小的条件下，根据传感器测量值，以自适应的方式寻找每个测量值对应的最优加权因子，提高无人驾驶车辆的测距精度和感知能力。假设所测量的实际距离值为L，而n个超声波测距传感器的测量值分别为l_i，其中i＝1,2,…,n，测量方差可以表示为

设定各个传感器的加权因子分别为a_i(i＝1,2,…,n)，应该满足式(1)中的关系：

测距传感器的总方差如式(2)所示：

根据式(1)和式(2)可以对采集的距离信息进行自适应加权融合解算，然后融合处理红外热感应传感器的异源信息对障碍物进行区分，判定监测到的障碍类别是固体障碍物还是矿井下的工作人员。其中，红外热感应传感器对于温度热量的监测属于一个开关量。

若红外热感应传感器监测到热量，则返回值为1，判定前方为井下工作人员，等待工作人员通过后继续行进；

若红外热感应传感器未监测到热量，则返回值为0，判定前方为固体障碍物，综合对比分析左右两侧障碍情况之后，选择较为空旷的一侧，进行绕行避障。

第三步：在自主避障的基础上，利用激光雷达以及蓝牙iBeacon基站进行目的地定位以及自主导航。本发明提出了一种基于蓝牙iBeacon粗定位和激光雷达矢量线段匹配细定位相结合的复合定位导航方法，用于解决矿井下巷道相似度高，定位导航容易出现偏差的问题。

首先，对激光雷达的同源信息进行融合，将实时扫描获取环境中的特征点信息融合构成矢量线段，将实时解算的矢量线段与全局二维地图进行局部特征匹配，其中，全局二维地图中的局部矢量线段特征信息表示为G＝{g_i|i＝1,2,…,a}，实时扫描解算的矢量线段特征信息表示为S＝{s_j|j＝1,2,…,b}。在扫描匹配矢量线段时，假设全局地图中g_i和实时扫描矢量线段s_j之间的匹配关系表示为m_ij＝(g_i,s_j)，根据特征信息匹配情况，在a×b的关系矩阵M_a×b中记录m_ij所有匹配关系，匹配度越高，表示g_i和s_j为同一特征的可能性越大，具体的匹配方法如下：

(1)优先级别最高的是矢量线段长度，假设L_i和L_j分别为g_i和s_j的长度，L_p和L_q分别为g_p和s_q的长度，长度差值阈值为ΔL。

考虑到扫描的s_j、s_q可能不是完整的g_i、g_p，所以实际扫描测量中一定存在L_j≤L_i以及L_q≤L_p。若L_i－L_j≤ΔL，则表示g_i和s_j长度匹配成立，认为m_ij可能是匹配的；若L_j＞L_i，则g_i和s_j不表示同一环境特征，那么无需比较分析其他的矢量线段关系。同理，可以对g_p和s_q做出相应的判断。

(2)第二优先级为相邻矢量线段夹角，假设A_ip为g_i和g_p的夹角，A_jq为s_j与s_q的夹角，角度差值阈值为ΔA。

在实际环境中，若g_i与s_j长度匹配，同时g_p和s_q长度也匹配，则A_ip和A_jq可能接近，若|A_ip－A_jq|≤ΔA，则认为m_ij或m_pq可能是匹配的；如果两对矢量线段中至少有一对长度不匹配，则不能确定A_ip与A_jq的关系，但可能其中一对矢量线段之间存在匹配的可能性；若|A_ip－A_jq|＞ΔA，说明至少存在一对不匹配，那么停止m_ij匹配，寻找下一个匹配矢量线段对。

(3)第三优先级为中点到相邻矢量线段的距离，Z_ip表示g_i的中点到g_p的距离，Z_jq表示s_j的中点到s_q的距离，距离差值阈值为ΔZ。

若g_i与l_j以及g_h与l_k满足长度、夹角要求，只要|Z_ip－Z_jq|≤ΔZ，则认为m_ij或m_pq存在较大的匹配可能性；若|Z_ip－Z_jq|＞ΔZ，说明至少存在一段矢量线段不匹配，那么停止m_ij匹配，寻找匹配下一个矢量线段对。

综上所述，通过激光雷实时扫描环境特征点信息，解算获取矢量线段特征，基于矢量线段特征匹配法，将实时解算的矢量线段与之前存储的二维地图矢量线段特征信息进行局部匹配，可以实现矿井巷道环境的局部定位导航功能需求。

第四步：在基于激光雷达实现局部定位导航的基础之上，充分利用蓝牙iBeacon基站配置有唯一ID标识的优点，通过蓝牙iBeacon基站信号强度值RSSI与距离之间的关系方程拟合，区分相类似巷道环境，解决矿井下各巷道相似度较高导致误判的问题。

首先对蓝牙iBeacon基站的非线性传播信号进行信号强度与距离信息的关系曲线拟合，根据实际井下环境建立电磁波路径损耗模型计算无人车辆与iBeacon基站之间的直线距离，该模型的数学表达形式为：

RSSI(d)＝RSSI(d₀)-10αlg(d/d₀)+τ (3)

式中：

RSSI(d)——距离蓝牙iBeacon基站d米处接收到的信号强度值；

RSSI(d₀)——参考强度值，一般取参考距离d₀为1米；

α——无线信道衰减因子，其值由实际室内信道环境决定；

τ——均值为零，服从高斯分布的随机变量，由信号的多径效应引起；

设定无人车辆与蓝牙基站的距离阈值为3米，当连续检测到目标ID出的蓝牙基站信号强度值小于等于60dBm，则判定无人驾驶车辆已经进入目标巷道3米范围内，否则判定无人车辆还没有进入目标巷道。

综合上述步骤，本发明设计了一种多传感同源异源信息融合的自主避障以及井下定位导航技术，帮助矿井下无人驾驶车辆进行障碍物的分类定位测距以及相似巷道区分和目的地定位导航，对矿井下无人驾驶技术的应用具有极大的实用价值。

Claims (9)

1.一种矿井下多传感器信息融合自主避障导航方法，其特征在于，包括以下步骤；

步骤1：通过超声波测距方法，实时监测获取障碍物的距离信息；

步骤2：构建无人驾驶车辆下位机控制系统；

步骤3：通过环境特征提取，对无人车辆目的地进行定位；

步骤4：对多源数据融合感知，实现矿井下无人车辆感知障碍、自主避障以及井下自主定位导航。

2.根据权利要求1所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，所述步骤1通过在无人驾驶车辆前端安装超声波测距传感器，实时传感检测井下无人车辆前方是否存在障碍以及距离障碍的距离，用于判断井下无人车辆的运行环境是矿井巷道，需要躲避井下障碍物、工作人员以及其他运输车辆，并将位移信号S转化为电压信号U，同时在无人车辆前方安装红外热感应传感器，用于判别前方物体是障碍物还是井下工作人员，通过检测温度进行区分，将温度信号T转化为电压信号U最终判断是绕行避障还是等待工作人员通过之后再继续行进，基于上述两种传感信息建立避障模型。

3.根据权利要求2所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，所述避障模型执行以下步骤；

设计无人驾驶车辆传感器的排布，之后建立无人驾驶车辆的融合障碍分布距离以及温度信息的避障模型，将无人驾驶车辆抽象为二维俯视简图，其中超声波测距传感器排布在不同的水平面内，然后设定超声波测距传感器的距离检测阈值，将检测到的数据与阈值进行对比，当测量值大于阈值时，对比值为0；当测量值小于或等于阈值时，对比值设为1，此时认定障碍物进入当前范围，障碍物检测的具体过程如下：

(1)启动车辆上安装的9个超声波传感器以及1个温度传感器；

(2)若超声波测距传感器全部对比值为0，则为类型1，判断是无障碍；

(3)若1、2、3号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型2，判断是前方有障碍；

(4)若4、5、6号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型3，判断是左侧有障碍；

(5)若7、8、9号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型4，判断是右侧有障碍；

(6)若1、2、3、4、5、6号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型5，判断是左侧和前方有障碍；

(7)若1、2、3、7、8、9号超声波测距传感器对比值为1，而其他传感器对比值为0，则为类型6，判断是右侧和前方有障碍；

(8)若超声波测距传感器全部对比值为1，则为类型7，判断是左右两侧和前方均有障碍。

4.根据权利要求1所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，所述步骤2中通过无人驾驶车辆下位机控制系统使用陀螺仪作为井下车辆航向角测量传感器，通过温度补偿和零位校正等获得可靠性高的航向角数据，在无人车辆的驱动设备上安装编码器，采集反馈的脉冲数，以此来实时获取无人车辆驱动轮运动过程中的速度以及位移量等状态参数，实现井下车辆的闭环运行，然后，该系统将步骤1中获得的环境信息与无人车辆自状态信息输入到下位机控制模块STM32F103RCT6，设计闭环控制反馈算法，根据编码器的同源脉冲信息计算出采样间隔内驱动轮的转速，根据陀螺仪的异源信息进行航向角解算，结合无人车辆的驱动轮位移量差值来进一步优化运行稳定性，通过变速积分PID算法调整实际速度期望值大小，最终将电压U转换为驱动力矩M。

5.根据权利要求1所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，所述步骤3中采用基于激光雷达的室内定位导航功能，在建立矿井巷道环境全局二维地图的基础之上，实现匹配局部路径进行导航以及智能自主路径判断，首先在矿井巷道安装蓝牙iBeacon基站，每个基站都具有特定的ID，可以区分激光雷达匹配信息，判断巷道环境，然后采用单独配置的用于特征数据处理的后台处理器miniPC对获取到的大量特征点信息进行分析，利用该方法监测信标之间的距离，确定无人车辆的目的地位置，并将位置信息反馈回后台处理系统，生成反馈控制信号。

6.根据权利要求5所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，通过蓝牙iBeacon粗定位和激光雷达矢量线段匹配细定位相结合进行复合定位导航；

首先，对激光雷达的同源信息进行融合，将实时扫描获取环境中的特征点信息融合构成矢量线段，将实时解算的矢量线段与全局二维地图进行局部特征匹配，其中，全局二维地图中的局部矢量线段特征信息表示为G＝{g_i|i＝1,2,…,a}，实时扫描解算的矢量线段特征信息表示为S＝{s_j|j＝1,2,…,b}，在扫描匹配矢量线段时，假设全局地图中g_i和实时扫描矢量线段s_j之间的匹配关系表示为m_ij＝(g_i,s_j)，根据特征信息匹配情况，在a×b的关系矩阵M_a×b中记录m_ij所有匹配关系，匹配度越高，表示g_i和s_j为同一特征的可能性越大，具体的匹配方法如下：

(1)优先级别最高的是矢量线段长度，假设L_i和L_j分别为g_i和s_j的长度，L_p和L_q分别为g_p和s_q的长度，长度差值阈值为ΔL。

考虑到扫描的s_j、s_q可能不是完整的g_i、g_p，所以实际扫描测量中一定存在L_j≤L_i以及L_q≤L_p，若L_i－L_j≤ΔL，则表示g_i和s_j长度匹配成立，认为m_ij可能是匹配的；若L_j＞L_i，则g_i和s_j不表示同一环境特征，那么无需比较分析其他的矢量线段关系，同理，可以对g_p和s_q做出相应的判断；

(2)第二优先级为相邻矢量线段夹角，假设A_ip为g_i和g_p的夹角，A_jq为s_j与s_q的夹角，角度差值阈值为ΔA；

在实际环境中，若g_i与s_j长度匹配，同时g_p和s_q长度也匹配，则A_ip和A_jq可能接近，若|A_ip－A_jq|≤ΔA，则认为m_ij或m_pq可能是匹配的；如果两对矢量线段中至少有一对长度不匹配，则不能确定A_ip与A_jq的关系，但可能其中一对矢量线段之间存在匹配的可能性；若|A_ip－A_jq|＞ΔA，说明至少存在一对不匹配，那么停止m_ij匹配，寻找下一个匹配矢量线段对；

(3)第三优先级为中点到相邻矢量线段的距离，Z_ip表示g_i的中点到g_p的距离，Z_jq表示s_j的中点到s_q的距离，距离差值阈值为ΔZ；

若g_i与l_j以及g_h与l_k满足长度、夹角要求，只要|Z_ip－Z_jq|≤ΔZ，则认为m_ij或m_pq存在较大的匹配可能性；若|Z_ip－Z_jq|＞ΔZ，说明至少存在一段矢量线段不匹配，那么停止m_ij匹配，寻找匹配下一个矢量线段对。

7.根据权利要求1所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，所述步骤4中首先对井下无人驾驶车辆的遇障情况进行抽象分析，设计不同的障碍物判断标准，然后基于自适应加权融合算法对测距信息进行分析处理，在获得障碍物距离信息以后，结合红外热感应传感器来判断障碍物的种类，通过激光雷达扫描获得的数据信息进行定位导航，利用特征点矢量线段匹配方法实时解算井下环境信息，根据无人车辆距巷道两壁的空间情况制定绕行路线，实现井下无人驾驶车辆的自主定位导航。

8.根据权利要求7所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，通过自适应加权融合算法对障碍距离信息进行处理分析，假设所测量的实际距离值为L，而n个超声波测距传感器的测量值分别为l_i，其中i＝1,2,…,n，测量方差可以表示为

设定各个传感器的加权因子分别为a_i(i＝1,2,…,n)，应该满足式(1)中的关系：

测距传感器的总方差如式(2)所示：

根据式(1)和式(2)可以对采集的距离信息进行自适应加权融合解算，然后融合处理红外热感应传感器的异源信息对障碍物进行区分，判定监测到的障碍类别是固体障碍物还是矿井下的工作人员，其中，红外热感应传感器对于温度热量的监测属于一个开关量；

若红外热感应传感器监测到热量，则返回值为1，判定前方为井下工作人员，等待工作人员通过后继续行进；

若红外热感应传感器未监测到热量，则返回值为0，判定前方为固体障碍物，综合对比分析左右两侧障碍情况之后，选择较为空旷的一侧，进行绕行避障。

9.根据权利要求8所述的一种矿井下多传感器信息融合自主避障导航方法，其特征在于，对蓝牙iBeacon基站的非线性传播信号进行信号强度与距离信息的关系曲线拟合，根据实际井下环境建立电磁波路径损耗模型计算无人车辆与iBeacon基站之间的直线距离，该模型的数学表达形式为：

RSSI(d)＝RSSI(d₀)-10αlg(d/d₀)+τ (3)

式中：

RSSI(d)——距离蓝牙iBeacon基站d米处接收到的信号强度值；

RSSI(d₀)——参考强度值，一般取参考距离d₀为1米；

α——无线信道衰减因子，其值由实际室内信道环境决定；

τ——均值为零，服从高斯分布的随机变量，由信号的多径效应引起；

设定无人车辆与蓝牙基站的距离阈值为3米，当连续检测到目标ID出的蓝牙基站信号强度值小于等于60dBm，则判定无人驾驶车辆已经进入目标巷道3米范围内，否则判定无人车辆还没有进入目标巷道。

Images