CN109443350B - 基于神经网络的蓝牙/光电/ins组合导航装置及方法 - Google Patents

Abstract

本发明公开了一种基于神经网络的蓝牙/光电/INS组合导航装置及方法，包括组合导航解算模块、传感器模块、地图存储及显示终端和电源模块，通过传感器模块采集巷道内车辆运动信息和蓝牙标签的数据，并将数据传输至组合导航信号解算模块；将采集的巷道内车辆运动信息和蓝牙标签的数据进行解算、数据融合，并传输至外部终端显示车辆位置；将组合导航解算模块解算的导航数据进行数据显示。本发明采用卡尔曼滤波器将MEMS惯导、蓝牙传感器、光电传感器、里程计数据进行融合，有效减小庞大的蓝牙标签布站数量，便于标签维护和更换，可连续输出车辆航向、位置、速度、运行状态等信息，提高车辆运行监控的可靠性和实时性，提高整体定位精度。

Description

基于神经网络的蓝牙/光电/INS组合导航装置及方法

技术领域

本发明属于导航定位技术领域，涉及一种使用蓝牙标签、光电探测、惯性导航系统进行组合导航装置及方法。

背景技术

在我国煤矿安全生产过程中，井下人员和车辆的精确定位始终是核心内容之一，优秀的定位方案可以有效提高煤矿管理水平和煤矿运作效率。

基于蓝牙4.0的车辆定位技术是基于最新的近场无线通信协议蓝牙4.0协议，应用于煤矿井下具有低成本、易于部署、功耗极低、实时性高等优点。但是目前蓝牙定位实际上是一种区域定位，只能实现5～10m的定位精度，在信号区域内距离不可分辨，且两站之间距离太远，中间无标签信号区域，不能进行定位。

目前，已经出现的蓝牙和惯性器件的组合，一般用在行人导航上，采用MEMS惯性传感器是为了采集行人的步态和步长，根据步长累计推算行人走过的位置，这种方式不能用在矿井车辆上，因车辆运动特征并非行人，不具有步态和步长特征。井下车辆种类较多，各种车辆的发动机振动和运动特征也有区别，常规的解算方式难以适应所有车辆的振动情况。

MEMS惯导能够提供短时间一定精度的载体位姿信息，但随着时间增长，其位姿快速发散；蓝牙标签的位置固定，但是发射信号不稳定，单点探测随机噪声较大，且在无蓝牙信号或蓝牙信号弱的区域不能进行定位；里程计由于车辆轮胎压力的变化，导致其车轮半径变化，会产生累计误差。为了减小蓝牙标签的探测不确定性，在标签表面粘贴了反光胶贴，通过激光束的反射确定标签切换的具体位置。

发明内容

针对井下车辆目前大多采用单一传感器定位模式，本发明的目的在于提供一种适用于井下巷道的组合导航方法，采用蓝牙/光电/INS组合导航技术，一方面，通过多种惯性传感器融合算法抑制传感器漂移，提高车载情况下INS纯惯在一段时间内的导航精度，另一方面通过不同种类传感器之间的数据融合，可以弥补各传感器定位特点的不足，实现优势互补。可实现在蓝牙信号覆盖区域内距离可分辨，在蓝牙信号弱或者蓝牙信号不可用的短距离条件下，由惯导/里程计提供持续导航。

本发明是通过下述技术方案来实现的。

一种基于神经网络的蓝牙/光电/INS组合导航装置，包括：

组合导航解算模块，用于对传感器模块采集的巷道内车辆运动信息和蓝牙标签的数据，通过双数字信号处理器DSP进行解算、数据融合，将解算后的导航数据传输至外部终端显示车辆位置；

传感器模块，用于通过MEMS惯性传感器、里程计和光电传感器采集巷道内车辆运动信息和通过主蓝牙探测器采集巷道内蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块；

地图存储及显示终端，用于将组合导航解算模块解算的导航数据进行数据显示；

电源模块，用于为组合导航信号解算模块和传感器模块提供电源。

进一步，所述组合导航信号解算模块包括双数字信号处理器DSP，双数字信号处理器DSP通过串口与地图存储及显示终端连接。

进一步，所述传感器模块包括主蓝牙探测器、MEMS惯性传感器、里程计和光电传感器；所述主蓝牙探测器和MEMS惯性传感器分别通过串口连接双数字信号处理器DSP；所述里程计通过扩展串口连接双数字信号处理器DSP；所述光电传感器通过Cap捕捉连接双数字信号处理器DSP。

进一步，所述MEMS惯性传感器包括3轴MEMS陀螺、3轴MEMS加速度计和3轴MEMS磁强计。

进一步，所述光电传感器包括扇面激光发生器、探测器及整形电路，扇面激光发生器向巷道墙体发射一束竖直平面的扇面激光，被墙体上蓝牙标签表面贴有的反光胶贴反射，反射激光被探测器接收，经整形电路形成中断脉冲信号，送给双数字信号处理器DSP。

进一步，所述电源模块包括5V转3.3V模块、5V转1.8V模块和5V转1.2V模块；5V转3.3V模块为传感器模块供电；5V转3.3V模块、5V转1.8V模块和5V转1.2V模块为组合导航解算模块供电。

本发明进而给出了一种基于神经网络的蓝牙/光电/INS组合导航方法，包括如下步骤：

1)检查导航装置与地图存储及显示终端通信接口，导航装置与巷道内车辆车载终端CAN接口是否连接正常，若未连接则进行手动连接；

2)巷道内车辆发动机启动，电源开始供电，地图存储及显示终端自动开机，导航装置开机；

3)导航装置其中一个数字信号处理器DSP进行基本组合导航解算过程；另一个数字信号处理器DSP根据采集巷道内车辆发动机开机情况下的MEMS传感器中陀螺加速度计数据，并提取信号特征，通过数据特征分析车辆类型、振动情况，并设定相应的零速校正阈值及设定主卡尔曼滤波器相关参数；

4)导航装置搜索附近蓝牙标签，若没有发现标签，持续搜索；若发现标签，结合地图存储及显示终端信息确定起始及目的标签，开始导航；

5)导航装置在蓝牙标签数据有效的区域实现蓝牙/INS数据融合，修正位置信息，同时向地图存储及显示终端输出蓝牙起始标签ID、坐标、航向、速度及总里程信息；光电传感器每扫过一个蓝牙标签，就会向双数字信号处理器DSP发出一个中断脉冲，DSP通过最近接收到的蓝牙标签ID和中断脉冲时刻确定蓝牙标签的具体位置；

6)在蓝牙标签数据无效区域，由INS/里程计组合维持位置递推，并由里程计的速度信息修正MEMS惯性传感器产生的漂移；

7)再次进入标签数据的区域，发现新标签，若满足切换条件，进行标签切换；

8)重复步骤5)-步骤7)，直至导航结束。

进一步，所述切换条件为，双数字信号处理器DSP中预设蓝牙信号标签信号阈值M，新标签信号N若大于等于阈值M，则切换；若小于则不切换，维持惯性导航。

进一步，所述步骤3)中，其中双数字信号处理器DSP进行基本组合导航解算过程如下：

3a)3轴MEMS陀螺、3轴MEMS加速度计和3轴MEMS磁强计将采集的信息通过子卡尔曼滤波器进行姿态融合估计，并通过比力测量值分解后，经过纯惯性导航计算，将计算结果传输至主卡尔曼滤波器进行融合估计；

3b)主卡尔曼滤波器将估计出的零偏补偿给子卡尔曼滤波器的比力测量值；主卡尔曼滤波器根据地图标签和巷道方向约束估计出的方位零偏补偿给姿态初始计算值；子卡尔曼滤波器姿态融合估计出的零偏补偿给姿态初始估计值；

3c)主卡尔曼滤波器将经导航计算的巷道车辆的位置、速度和姿态信息和里程计采集的巷道车辆速度信息、主蓝牙探测器和光电传感器采集的巷道车辆位置信息进行融合估计，并将该最终融合后的导航数据传输至地图存储及显示终端。

进一步，所述步骤3)中，主卡尔曼滤波器解算过程如下：

经子卡尔曼滤波器将传感器模块中3轴陀螺和3轴MEMS加速度计采集的信息进行信号处理，处理后的信号分别经信号特征提取和惯导解算，经信号特征提取的信号经神经网络信号识别，得到零速校正参数和主卡尔曼滤波器滤波参数，传输至主卡尔曼滤波器；经惯导解算的巷道车辆的位姿、速度信号，与主蓝牙探测器、里程计和光电传感器采集的位置及速度信息传输至主卡尔曼滤波器，主卡尔曼滤波器进行融合估计，并将该最终融合后的导航数据传输至地图存储及显示终端。

进一步，所述卡尔曼滤波器进行融合估计方法采用离散型卡尔曼滤波基本方程对误差作最优估计：

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 (2.3)

P_k/k-1＝ΦP_k-1Φ^T+Q_k-1 (2.4)

其中，k＝1,2,3....，为各状态离散化后k时刻的估计值，Φ为离散化后形成的状态矩阵，Φ^T为Φ矩阵的转置，Q_k-1为系统噪声协方差矩阵，K_k为k时刻增益矩阵，/>为K_k矩阵的转置，P_k为k时刻协方差矩阵，P_k/k-1为一步预测协方差矩阵，Z_k为k时刻观测向量，R_k为观测噪声协方差矩阵，H为观测矩阵，H^T为H矩阵的转置，I为单位矩阵。

本发明由于采取以上技术方案，其具有以下有益效果：

本发明采用改进的卡尔曼滤波器将MEMS惯导、蓝牙传感器、光电传感器、里程计数据进行融合，使得各传感器优势互补，能够获得远优于单个传感器的定位及导航精度。在标签数据有效区域，进行组合导航，对比蓝牙标签位置和MEMS惯导位置对MEMS惯导数据进行修正；在无标签信号或标签信号弱区域，短时间内可由MEMS惯导或者MEMS惯导/里程计组合持续提供导航信息，该方法有效减小庞大的蓝牙标签布站数量，便于标签维护和更换，可连续输出车辆航向、位置、速度、运行状态等信息。提高车辆运行监控的可靠性和实时性。

采用改进的卡尔曼滤波器增加了神经网络模式识别模块，能够通过传感器数据分析车辆类型、振动情况、传感器噪声等情况，并调整零速校正参数和主卡尔曼滤波器参数，使得组合导航滤波器对不同车辆和传感器场景具有自适应能力。

因为发动机振动情况不同，所对应的滤波参数也不相同。因此本发明所设计的改进型组合导航滤波器可以使导航模块适应不同类型发动机车辆，整个过程模块自动完成，无需人工调整参数。

由于蓝牙标签的探测不确定性较大，为了减小组合导航观测噪声，在蓝牙标签表面粘贴了反光胶贴，通过激光束的反射确定蓝牙标签切换的具体位置，从而可以提高整体定位精度。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，并不构成对本发明的不当限定，在附图中：

图1为组合导航模块基本硬件组成框图；

图2为光电传感器模块工作示意图；

图3为蓝牙/INS组合导航系统工作流程图；

图4为导航系统主要工作阶段图示；

图5为双DSP其中一个DSP进行基本组合导航解算过程框图；

图6为双DSP其中另一个DSP进行模式识别卡尔曼滤波器参数估计示意图；

图7-1为蓝牙/INS组合测量仿真结果；

图7-2为蓝牙/INS组合测量误差；

图7-3为蓝牙对MEMS惯导速度误差的估计。

具体实施方式

下面将结合附图以及具体实施例来详细说明本发明，在此本发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定。

如图1所示，本发明基于神经网络的蓝牙/光电/INS组合导航装置，包括组合导航解算模块、传感器模块、地图存储及显示终端和和电源模块，其中：

组合导航解算模块，用于对传感器模块采集的巷道内车辆运动信息和蓝牙标签的数据，通过双数字信号处理器DSP进行解算、数据融合，将解算后的导航数据传输至外部终端显示车辆位置。传感器模块，用于通过MEMS惯性传感器、里程计和光电传感器采集巷道内车辆运动信息和通过主蓝牙探测器采集巷道内蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块。地图存储及显示终端，用于将组合导航解算模块解算的导航数据进行数据显示。电源模块，用于为组合导航信号解算模块和传感器模块提供电源。

其中，组合导航信号解算模块包括双数字信号处理器DSP，双数字信号处理器DSP通过串口与地图存储及显示终端连接。传感器模块包括主蓝牙探测器、MEMS惯性传感器、里程计和光电传感器；主蓝牙探测器和MEMS惯性传感器分别通过串口连接双数字信号处理器DSP；里程计通过扩展串口连接双数字信号处理器DSP；光电传感器通过Cap捕捉连接双数字信号处理器DSP。

其中，MEMS惯性传感器包括3轴MEMS陀螺、3轴MEMS加速度计和3轴MEMS磁强计。

电源模块包括5V转3.3V模块、5V转1.8V模块和5V转1.2V模块；5V转3.3V模块为传感器模块供电，5V转3.3V模块、5V转1.8V模块和5V转1.2V模块为组合导航解算模块供电。

主蓝牙探测器可提供导航模块距离蓝牙标签基站的信号强度信息，经DSP处理后可转化成距离信息(噪声波动较大)；MEMS惯性传感器包括三轴MEMS陀螺、三轴MEMS加计、磁强计等，可获得导航模块的三轴角速率、加速度、磁航向等信息，传感器信息经过232接口送入DSP，经导航解算可得到载体位置、速度、姿态信息；里程计在车轮旋转一周情况下输出一个脉冲，可获得车辆速度和总里程信息，由于目前新到车辆都已安装里程计，因此导航模块只需要从车载终端的CAN接口获取里程计测速信息即可，无需额外单独安装里程计。

光电传感器组成及工作原理如图2所示，如图2所示，它是由扇面激光发生器、探测器及整形电路组成，扇面激光发生器向巷道墙体发射一束竖直平面的扇面激光，墙体上安装的蓝牙标签表面贴有反光胶贴，当激光束达到反光胶贴上时被反射，反射激光被探测器接收，经整形形成中断脉冲信号，送给DSP。由于蓝牙发射信号的波动，当收到某一蓝牙信号时，只能判断在标签一定范围内，位置不确定性较大，光电传感器可以较为准确的定位某一时刻的蓝牙标签的位置，减小位置观测误差。

组合导航解算模块主要由DSP核心电路及外围通信电路组成，用于对传感器数据进行解算，数据融合。解算后的导航数据通过RS232口送给地图存储及显示终端，地图存储及显示终端内部存储有矿井地图，可配合导航数据显示车辆位置。

如图3所示，蓝牙/INS组合导航模块按以下流程进行工作：

1)检查导航装置与地图存储及显示终端通信接口，导航装置与巷道内车辆车载终端CAN接口是否连接正常，若未连接则进行手动连接；

2)巷道内车辆发动机启动，电源开始供电，地图存储及显示终端自动开机，导航装置开机；

3)导航装置其中一个数字信号处理器DSP进行基本组合导航解算过程；另一个数字信号处理器DSP根据采集巷道内车辆发动机开机情况下的MEMS传感器中陀螺加速度计数据，并提取信号特征，通过数据特征分析车辆类型、振动情况，并通过设定相应的零速校正阈值及设定主卡尔曼滤波器相关参数；

如图5所示，双数字信号处理器DSP进行基本组合导航解算过程如下：

3a)3轴MEMS陀螺、3轴MEMS加速度计和3轴MEMS磁强计将采集的信息通过子卡尔曼滤波器进行姿态融合估计，并通过比力测量值分解后，经过纯惯性导航计算，将计算结果传输至主卡尔曼滤波器进行融合估计；

3b)主卡尔曼滤波器将估计出的零偏补偿给子卡尔曼滤波器的比力测量值；主卡尔曼滤波器根据地图标签和巷道方向约束估计出的方位零偏补偿给姿态初始计算值；子卡尔曼滤波器姿态融合估计出的零偏补偿给姿态初始估计值；

3c)主卡尔曼滤波器将经导航计算的巷道车辆的位置、速度和姿态信息和里程计采集的巷道车辆速度信息、主蓝牙探测器和光电传感器采集的巷道车辆位置信息进行融合估计，并将该数据传输至地图存储及显示终端。

其中如图6所示，主卡尔曼滤波器解算过程如下：

经子卡尔曼滤波器将传感器模块中3轴陀螺和3轴MEMS加速度计采集的信息进行信号处理，处理后的信号分别经信号特征提取和惯导解算，经信号特征提取的信号经神经网络信号识别及自适应参数配置后，得到零速校正参数和主卡尔曼滤波器参数，传输至主卡尔曼滤波器；经惯导解算的巷道车辆的位姿、速度信号，与主蓝牙探测器、里程计和光电传感器采集的位置、速度信息传输至主卡尔曼滤波器，主卡尔曼滤波器进行融合估计，并将最终融合后的导航数据传输至地图存储及显示终端。

4)导航装置搜索附近蓝牙标签，若没有发现标签，持续搜索；若发现标签，结合地图存储及显示终端地图信息确定起始及目的标签，开始导航；

5)组合导航：导航装置在蓝牙标签数据有效且具有参考价值的区域实现蓝牙/INS数据融合，修正位置信息，同时向地图存储及显示终端输出起始标签ID、坐标、航向、速度及总里程信息；光电传感器每扫过一个蓝牙标签，就会向双数字信号处理器DSP发出一个中断脉冲，这样两两蓝牙标签的起止观测时刻可以较为准确的确定，可以减小组合导航的位置观测误差；

6)组合导航：在蓝牙标签数据无效区域，由INS/里程计组合维持位置递推，并由里程计较为准确的速度信息修正MEMS惯性传感器产生的漂移；

7)再次进入标签数据有效且具有参考价值的区域，发现新标签，若满足切换条件，进行标签切换；切换条件为，双数字信号处理器DSP中预设蓝牙信号标签信号阈值M，新标签信号N若大于等于阈值M，则切换；若小于则不切换，维持惯性导航；

8)重复步骤5)-步骤7)，直至导航结束。

主要工作阶段示意图如图4所示。

在10m间隔布站的情况下，由于蓝牙信号的覆盖范围为±5m，而可以有效为INS提供修正信息的距离只有±3m，因此中间大概有4m的距离是由INS/里程计组合得到。由于INS在短时间内可以维持一定精度，因此布站间隔可以增加到几十米甚至上百米。

图5为基本信号流动及解算过程，导航解算的基本步骤是进行姿态解算，获得转换矩阵，将固连于载体坐标系的加计数据通过转换矩阵投影到导航坐标系中。首先，载体的3轴MEMS陀螺仪提供3轴角速率信息，通过四元素算法获得基本的姿态信息，由于MEMS陀螺漂移较大，而3轴MEMS加速度计在静止情况下测得的姿态相对稳定，因此将3轴MEMS陀螺解算出的基本姿态和3轴MEMS加速度计解算姿态进行卡尔曼滤波融合估计，融合结果既保留了3轴MEMS陀螺仪测量姿态较好的动态特性，又抑制了陀螺漂移影响。姿态中的方位信息由3轴MEMS磁强计、陀螺数据、巷道地图的约束关系计算得到，卡尔曼滤波综合磁强计、陀螺数据和巷道方向信息可以提高方位测量的抗干扰能力。

获得最终的姿态矩阵后，将由加速度计得到的比力测量值f^b分解到导航坐标系中，在导航坐标系中通过MEMS惯性传感器和组合导航解算模块进行INS速度和位置计算，由于地球重力影响，还要补偿地球重力加速度。由INS输出的载体位置、速度信息在短时间内较为准确，随着时间增长会快速发散。因此，采用主蓝牙探测器获得的带噪声的绝对位置信息和里程计测量的速度信息作为观测量，通过置信度卡尔曼滤波器进行融合估计，数据融合后会抑制主蓝牙探测器噪声，同时估计出MEMS惯性传感器的漂移，并对MEMS惯性传感器数据进行修正。

获得修正后的INS可在短时间内维持满足要求的导航精度。在蓝牙信号弱或者蓝牙信号不可用的情况下，可由INS或者INS/里程计组合导航(MEMS惯性传感器和里程计与组合导航解算模块的组合)提供持续的导航数据。这里INS是惯性导航系统的简称，这里的INS和MEMS惯导是指同一装置。

由MEMS惯导平滑蓝牙定位噪声，由多站蓝牙定位信息与MEMS惯导数据进行比较以估计MEMS惯性传感器漂移，里程计测速提供速度观测量。

融合思想为：利用已经标定好的两两蓝牙基站之间的距离和惯导测量得到的两蓝牙标签之间距离差作为位置误差观测量，利用里程计测速和惯导速度差为速度误差观测量，利用巷道地图信息对航向信息进行约束，采用最优估计理论对惯导传感器漂移进行估计并修正。

融合算法描述如下：

组合导航解算按照下式(1.1)～(1.7)建立主卡尔曼滤波状态方程，按照(2.1)建立观测方程，通过卡尔曼滤波器来估计(1.8)式所示的状态变量，并对导航数据进行修正。具体方程描述如下。

在低成本MEMS惯导系统中，由于陀螺精度较低，不能敏感到地球自转信息，可以将捷联惯导误差传播方程简化，简化后的方程如下：

式中，L为当地纬度，R为地球半径，h为高度，X、Y、Z分别为相对当地参考坐标系的距离，选取当地参考坐标系原点，朝东为X轴正向，朝北为Y轴正向，朝天为Z轴正向。V_E、V_N、V_U分别为东向、北向和天向速度，ω_ie为地球自转角速率，ε_Bi、▽_i(i＝E,N,U)为地理坐标系内陀螺的等效漂移和加速度计的等效偏置，ε_Wi为陀螺角增量输出的随机游走。φ_E、φ_U、φ_N为三个方向上的姿态失准角。所有变量需要统一后加入卡尔曼滤波器输入。δX、δY、δZ、δV_N、δV_E、δV_U分别为三个位置误差量和三个速度误差量。

状态变量为：

观测变量为蓝牙标签位置和惯导位置的三个位置误差信息，里程计瞬时速度和惯导速度的三个速度误差信息。构建离散卡尔曼滤波方程进行组合导航解算和误差估计。

量测方程为：

式中，Z(k),(k＝1…6)为实际三个位置三个速度的观测误差，V_k,(k＝1…6)为观测噪声。

将上述连续状态方程和观测方程离散化，采用离散型卡尔曼滤波基本方程对误差作最优估计。采用离散型卡尔曼滤波基本方程对误差作最优估计：

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 (2.3)

P_k/k-1＝ΦP_k-1Φ^T+Q_k-1 (2.4)

其中，k＝1,2,3....，以上为离散卡尔曼滤波器估计方程。

其中，k＝1,2,3....，为各状态离散化后k时刻的估计值，Φ为离散化后形成的状态矩阵，Φ^T为Φ矩阵的转置，Q_k-1为系统噪声协方差矩阵，K_k为k时刻增益矩阵，/>为K_k矩阵的转置，P_k为k时刻协方差矩阵，P_k/k-1为一步预测协方差矩阵，Z_k为k时刻观测向量，R_k为观测噪声协方差矩阵，H为观测矩阵，H^T为H矩阵的转置，I为单位矩阵。

由于车辆在巷道内主要沿巷道方向进行运动，则可以通过约束方程约束垂直巷道方向和向上的位置和速度，以提高融合精度。在主卡尔曼滤波器中存在观测噪声协方差矩阵R_k和系统噪声协方差矩阵Q_k，不同车辆环境和蓝牙标签布置情况下可能存在不同的Q_k和R_k。

本发明的数据融合策略为，由于矿井车辆种类较多，每种车辆的发动机振动频率和车况不同，固定的零速校正阈值和滤波参数不能适应所有车辆的情况。本发明采用神经网络模块对车辆停止且发动机开机情况下的传感器数据进行分析，提取信号特征，分析出目前的车辆状态及最优的滤波参数，并自适应调整零速校正阈值。车辆运动起来后，神经网络模块可以分析其他传感器数据的噪声波动，并对观测噪声方差进行调整。如图6所示，Q_k为过程噪声方差矩阵，R_k为观测噪声方差矩阵，L₁为零速校正阈值，是发动机开机情况下车辆运动和静止两种状态的判断阈值，可由神经网络模块分析得到。

对以上模型进行相关仿真如下。

蓝牙标签按照接近10m沿水平方向等间隔布站，模拟小车初始位置为(5m，10m)，速度(3m/s，0m/s)，若不采用光电传感器，蓝牙标签观每10m段的观测噪声幅值±3m，设定惯导速度误差为0.3m/s，进行仿真分析，数据组合结果如图7-1所示，融合后蓝牙观测噪声被平滑掉，滤波后的定位误差在1m以内。

图7-2显示了滤波前后的误差情况，滤波后蓝牙观测噪声被降低，综合位置估计精度得到提高。图7-3显示了利用蓝牙和INS位置观测误差对INS速度误差的估计，估计正确收敛到设定的误差值。

在速度观测中加入里程计测速，作为共同的速度观测量，里程计观测前需要在线对其进行标定，消除胎压引起的测速误差，量化噪声部分作为观测噪声的一部分。

由以上建模分析可以看出，利用蓝牙标签已标定位置信息就可以对MEMS惯导参数变化进行估计修正，而数据融合后，蓝牙噪声也得到了减小和平滑。仿真定位精度1.5m，满足使用要求。

采用光电探测器后，蓝牙位置观测不确定性降低，观测噪声降低至±0.5m以内，整体估计和修正精度会提高。

蓝牙/光电/MEMS惯性组合导航方法将各单一传感器进行数据融合，取长补短，能够获得较单一传感器更好的导航精度和稳定性。该方法通过光电探测模块来减小蓝牙位置探测噪声，结合巷道地图信息来约束航向姿态的发散，从而保证航向估计误差维持在一定范围内。

Claims (8)

1.一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，导航方法采用的装置包括：

组合导航解算模块，用于对传感器模块采集的巷道内车辆运动信息和蓝牙标签的数据，通过双数字信号处理器DSP进行解算、数据融合，将解算后的导航数据传输至外部终端显示车辆位置；

传感器模块，用于通过MEMS惯性传感器、里程计和光电传感器采集巷道内车辆运动信息和通过主蓝牙探测器采集巷道内蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块；

地图存储及显示终端，用于将组合导航解算模块解算的导航数据进行数据显示；

电源模块，用于为组合导航信号解算模块和传感器模块提供电源；

所述光电传感器包括扇面激光发生器、探测器及整形电路，扇面激光发生器向巷道墙体发射一束竖直平面的扇面激光，被墙体上蓝牙标签表面贴有的反光胶贴反射，反射激光被探测器接收，经整形电路形成中断脉冲信号，送给双数字信号处理器DSP；

导航方法，包括如下步骤：

1)检查导航装置与地图存储及显示终端通信接口，导航装置与巷道内车辆车载终端CAN接口是否连接正常，若未连接则进行手动连接；

2)巷道内车辆发动机启动，电源开始供电，地图存储及显示终端自动开机，导航装置开机；

3)导航装置其中一个数字信号处理器DSP进行基本组合导航解算过程；另一个数字信号处理器DSP根据采集巷道内车辆发动机开机情况下的MEMS传感器中陀螺加速度计数据，并提取信号特征，通过数据特征分析车辆类型、振动情况，并设定相应的零速校正阈值及设定主卡尔曼滤波器相关参数；

4)导航装置搜索附近蓝牙标签，若没有发现标签，持续搜索；若发现标签，结合地图存储及显示终端信息确定起始及目的标签，开始导航；

5)导航装置在蓝牙标签数据有效的区域实现蓝牙/INS数据融合，修正位置信息，同时向地图存储及显示终端输出蓝牙起始标签ID、坐标、航向、速度及总里程信息；光电传感器每扫过一个蓝牙标签，就会向双数字信号处理器DSP发出一个中断脉冲，DSP通过最近接收到的蓝牙标签ID和中断脉冲时刻确定蓝牙标签的具体位置；

6)在蓝牙标签数据无效区域，由INS/里程计组合维持位置递推，并由里程计的速度信息修正MEMS惯性传感器产生的漂移；

7)再次进入标签数据的区域，发现新标签，若满足切换条件，进行标签切换；

8)重复步骤5)-步骤7)，直至导航结束。

2.根据权利要求1所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述传感器模块包括主蓝牙探测器、MEMS惯性传感器、里程计和光电传感器；所述主蓝牙探测器和MEMS惯性传感器分别通过串口连接双数字信号处理器DSP；所述里程计通过扩展串口连接双数字信号处理器DSP；所述光电传感器通过Cap捕捉连接双数字信号处理器DSP。

3.根据权利要求2所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述MEMS惯性传感器包括3轴MEMS陀螺、3轴MEMS加速度计和3轴MEMS磁强计。

4.根据权利要求1所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述电源模块包括5V转3.3V模块、5V转1.8V模块和5V转1.2V模块；5V转3.3V模块为传感器模块供电；5V转3.3V模块、5V转1.8V模块和5V转1.2V模块为组合导航解算模块供电。

5.根据权利要求1所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述切换条件为，双数字信号处理器DSP中预设蓝牙信号标签信号阈值M，新标签信号N若大于等于阈值M，则切换；若小于则不切换，维持惯性导航。

6.根据权利要求1所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述步骤3)中，其中双数字信号处理器DSP进行基本组合导航解算过程如下：

3a)3轴MEMS陀螺、3轴MEMS加速度计和3轴MEMS磁强计将采集的信息通过子卡尔曼滤波器进行姿态融合估计，并通过比力测量值分解后，再经过纯惯性导航解算，将计算结果传输至主卡尔曼滤波器进行融合估计；

3b)主卡尔曼滤波器将估计出的零偏补偿给子卡尔曼滤波器的比力测量值；主卡尔曼滤波器根据地图标签和巷道方向约束估计出的方位零偏补偿给姿态初始计算值；子卡尔曼滤波器姿态融合估计出的零偏补偿给姿态初始估计值；

3c)主卡尔曼滤波器将经导航计算的巷道车辆的位置、速度和姿态信息和里程计采集的巷道车辆速度信息、主蓝牙探测器和光电传感器采集的巷道车辆位置信息进行融合估计，并将该数据传输至地图存储及显示终端。

7.根据权利要求6所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述步骤3)中，主卡尔曼滤波器解算过程如下：

经子卡尔曼滤波器将传感器模块中3轴陀螺和3轴MEMS加速度计采集的信息进行信号处理，处理后的信号分别经信号特征提取和惯导解算，经信号特征提取的信号经神经网络信号识别，得到零速校正参数和主卡尔曼滤波器滤波参数，传输至主卡尔曼滤波器；经惯导解算的巷道车辆的位姿、速度信号，与主蓝牙探测器、里程计和光电传感器采集的位置及速度信息传输至主卡尔曼滤波器，主卡尔曼滤波器进行融合估计，并将该最终融合后的导航数据传输至地图存储及显示终端。

8.根据权利要求6所述的一种基于神经网络的蓝牙/光电/INS组合导航方法，其特征在于，所述卡尔曼滤波器进行融合估计方法采用离散型卡尔曼滤波基本方程对误差作最优估计：

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 (2.3)

P_k/k-1＝ΦP_k-1Φ^T+Q_k-1 (2.4)

其中，k＝1,2,3....，为各状态离散化后k时刻的估计值，Φ为离散化后形成的状态矩阵，Φ^T为Φ矩阵的转置，Q_k-1为系统噪声协方差矩阵，K_k为k时刻增益矩阵，/>为K_k矩阵的转置，P_k为k时刻协方差矩阵，P_k/k-1为一步预测协方差矩阵，Z_k为k时刻观测向量，R_k为观测噪声协方差矩阵，H为观测矩阵，H^T为H矩阵的转置，I为单位矩阵。