主动查询式蓝牙/复合式MEMS惯导组合导航装置及方法与流程

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

背景技术：

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

基于蓝牙4.0的车辆定位技术是基于最新的近场无线通信协议蓝牙4.0协议，应用于煤矿井下具有低成本、易于部署、功耗极低、实时性高等优点。但是目前蓝牙定位实际上是一种区域定位，只能实现5～10m的定位精度，在信号区域内距离不可分辨，且两站之间距离太远，中间无信号时，不能进行定位。目前蓝牙定位基站，都采用定时主动向外发送信号的方式进行定位，不管通道内有无车辆，这样蓝牙基站模块的电池使用年限为2～3年，由于布站数量多，更换起来很不方便。另外巷道内路况复杂，车辆颠簸较严重，会对mems惯导系统产生振动影响。

技术实现要素：

为解决现有技术中存在的上述缺陷，本发明的目的在于提供一种适用于井下巷道的组合导航方法。本发明通过改进传感器，并将传统主动广播式蓝牙基站改进为查询式，只有当车载导航模块向基站发射查询指令的时候，基站模块才会回传信号，这样，基站大部分时间处于待机状态，可大大节省电量，延长使用寿命。将传统捷联式mems惯导进行改进，增加微型机械稳定平台，以隔离车辆颠簸和振动，从而获得较好的纯惯导航效果，并将改进的传感器结合起来进行组合导航。

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

一种主动查询式蓝牙/复合式mems惯导组合导航装置，包括：

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

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

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

进一步，所述组合导航信号解算模块包括数字信号处理器dsp、存储器和串口通信模块。

进一步，所述传感器模块包括主动式蓝牙传感器和复合式mems惯导平台；

所述复合式mems惯导平台包括mems惯性传感器和微机械稳定转台，所述mems惯性传感器安装在微机械稳定转台上；

所述mems惯性传感器包括3轴mems陀螺、3轴mems加速度计和mems磁强计；

所述微机械稳定转台框架上设有方位、俯仰电机，方位、俯仰角度传感器和驱动模块。

进一步，所述mems惯性传感器、主动式蓝牙传感器和微机械稳定转台分别通过串口与数字信号处理器dsp连接，数字信号处理器dsp通过串口与外部显示终端相连。

进一步，所述主动式蓝牙传感器为查询式蓝牙传感器，只有当车载导航模块中的主动式蓝牙传感器向沿巷道墙壁布置的蓝牙标签基站发射查询指令的时候，蓝牙标签基站模块才会回传信号。

进一步，所述电源模块包括5v转3.3v模块、5v转1.8v模块和5v转1.2v模块；5v转3.3v模块为传感器模块供电，5v转3.3v模块、5v转1.2v模块和5v转1.8v模块为数字信号处理器dsp供电。

本发明进而给出了一种主动查询式蓝牙/复合式mems惯导组合导航方法，包括如下步骤：

1)检查导航装置与外部显示终端通信接口、导航装置与巷道内车辆车载终端can接口是否连接正常，若未连接则进行手动连接；

2)巷道内车辆发动机启动，电源开始供电，外部显示终端自动开机，导航装置开机；

3)复合式mems惯导平台初始化至水平位置，复合式mems惯导平台与数字信号处理器dsp构成的惯导系统自对准；

4)导航装置中主动式蓝牙传感器发射蓝牙标签查询信号，搜索巷道内附近蓝牙标签，若没有发现标签，持续搜索；若发现蓝牙标签，结合地图信息，确定参考航向，导航装置开始导航；

5)导航装置根据mems惯性传感器采集的数据进行惯性导航解算，解算得到位置姿态信息；3轴mems陀螺和3轴mems加速度计的数据可以融合以抑制陀螺漂移；组合导航信号解算模块解算出的位置姿态数据与方位、俯仰角度传感器数据比较后，得到方位、俯仰电机的控制量，进行复合式mems惯导平台稳定控制；

6)在蓝牙标签数据有效区域，数字信号处理器dsp采用卡尔曼滤波器进行蓝牙/mems惯性组合导航；在蓝牙标签数据无效区域，由惯导装置维持位置递推；

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

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

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

进一步，所述步骤5)中，导航装置根据mems惯性传感器数据进行惯性导航解算，蓝牙/ins组合导航系统数据融合过程如下：

a)mems惯性传感器获取载体运动信息，mems捷联惯导解算得到包括巷道内车辆的姿态、位置和速度的导航信息；

b)通过主动蓝牙发射接收模块获取巷道内蓝牙标签信息，通过dsp中蓝牙探测子滤波器获取包括巷道内车辆位置、速度的蓝牙导航信息；

c)将步骤a)获取的导航信息和步骤b)获取的蓝牙导航信息进行输出信号比较，得到误差量，送入imm卡尔曼滤波器进行误差估计，对mems捷联惯导进行参数修正后得到最终的导航信息，并传输至外部终端设备。

进一步，所述导航信息与蓝牙导航信息进行输出信号比较，构建imm卡尔曼滤波方程进行组合导航解算和误差估计，量测方程为：

式中，z(k)，k＝1…6为实际三个位置三个速度的观测误差，x、y、z分别为相对当地参考坐标系的距离，ve、vn、vu分别为东向、北向和天向速度，vk，k＝1…6为观测噪声，δx、δy、δz、δvn、δve、δvu分别为三个位置误差量和三个速度误差量。

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

本发明通过多传感器数据融合技术，融合蓝牙和惯导数据进行组合导航，降低了对单一蓝牙传感器的依赖，在中间无蓝牙信号区域，可采用惯导进行位置递推。提高了定位导航的连续性。

本发明通过传感器模块采集巷道内车辆运动信息和蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块，组合导航信号解算模块对传感器模块采集的巷道内车辆运动信息和蓝牙标签的数据进行解算、数据融合，将解算后的导航数据传输至外部显示终端设备，并显示车辆位置；电源模块对组合导航信号解算模块和传感器模块提供电源。组合导航方式较单一传感器精度和可靠性都更高。

采用主动查询式蓝牙定位，在蓝牙标签未收到主动式蓝牙传感器发出的查询信号时，处于待机状态，接收到主动式蓝牙传感器发射信号才唤醒并开始广播定位信号，这种方式较传统广播式蓝牙定位，可有效增加蓝牙基站标签的使用寿命，不用频繁更换标签，减小了使用和维护成本。

本发明采用了微机电稳定技术，将mems捷联惯导进行稳定，从而提高惯导系统在车辆严重颠簸和振动下的可靠性和精度。

附图说明

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

图1为组合导航模块组成层级图；

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

图3a为蓝牙探测基本构成框图；图3b为现有蓝牙标签广播时序图；图3c为主动式蓝牙探测器探测时序图；

图4为复合式mems惯导模块；

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

图6为数字信号处理器dsp基本组合导航解算过程框图；

图7为导航系统主要工作阶段图示。

具体实施方式

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

如图1、图2所示，本发明主动查询式蓝牙/复合式mems惯导组合导航装置，包括组合导航信号解算模块、传感器模块和电源模块，其中：

组合导航信号解算模块，用于通过数字信号处理器dsp对传感器模块采集的巷道内车辆运动信息和蓝牙标签的数据进行解算、数据融合，将解算后的导航数据传输至外部终端设备，并显示车辆位置、速度及航向等信息。传感器模块，用于通过mems惯性传感器采集巷道内车辆运动信息和通过主动式蓝牙传感器采集蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块。电源模块，用于对组合导航信号解算模块和传感器模块提供电源。

其中，组合导航信号解算模块包括数字信号处理器dsp、存储器和串口通信模块。传感器模块包括主动式蓝牙传感器和复合式mems惯导平台；复合式mems惯导平台包括mems惯性传感器和微机械稳定转台，mems惯性传感器安装在微机械稳定转台上。mems惯性传感器包括3轴mems陀螺、3轴mems加速度计和mems磁强计；mems陀螺用于敏感转轴上的角速率信息，mems加速度计用于敏感运动方向上的加速度信息，mems磁强计用于敏感地磁场。

微机械稳定转台框架上设有方位、俯仰电机，方位、俯仰角度传感器，驱动模块将dsp输出的控制信号放大后对方位俯仰电机进行驱动。

如图2所示，mems惯性传感器、主动式蓝牙传感器和微机械稳定转台分别通过串口与数字信号处理器dsp连接，数字信号处理器dsp通过串口与外部显示终端相连。电源模块包括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，经导航解算可得到载体位置、速度、姿态信息。微机械稳定转台包括方位轴和俯仰轴，车辆方位俯仰上的颠簸被mems陀螺敏感到以后，经过相应算法反馈给微机械稳定平台，以隔离载体扰动。

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

图3a示出了蓝牙探测模式的基本构成，图3b为常规蓝牙标签广播时序，图3c为主动式蓝牙探测器探测时序。主动式蓝牙探测器和常规蓝牙标签广播时序的区别。常规蓝牙标签每隔时间t0向外广播一次信号，各个蓝牙标签之间是独立的，异步的。而主动式蓝牙探测器每隔一段时间t0向外发射一次查询信号，车辆附近的蓝牙标签接收到查询信号后再发射含有标签信息的信号，再被蓝牙探测器接收，蓝牙标签在没有接收到查询信号时，处于待机状态。

图4为复合式mems惯导模块组成图，它是由复合式mems惯导平台、解算模块、驱动模块等组成，复合式mems惯导平台包括方位俯仰两轴两框架结构、方位俯仰电机、角度传感器、mems惯性传感器组成；mems惯性传感器由3轴陀螺、3轴加计和3轴磁强计组成。角度传感器用于敏感转台的转角，方位、俯仰电机用于驱动框架进行稳像。mems惯性传感器数据经过导线连接到捷联惯导解算模块，进行导航解算，得到平台的姿态和位置信息，姿态数据和角度传感器敏感到的方位俯仰角比较后，得到电机控制量，经过驱动放大后驱动电机运动进行稳定。

如图5所示，下面给出本发明的主动查询式蓝牙/复合式mems惯导组合导航方法过程：

1)检查导航装置与外部显示终端通信接口、导航装置与巷道内车辆车载终端can接口是否连接正常，若未连接则进行手动连接。

2)巷道内车辆发动机启动，电源开始供电，外部显示终端自动开机，导航装置开机。

3)复合式mems惯导平台初始化至水平位置，复合式mems惯导平台与dsp构成的惯导系统自对准。

4)导航装置中主动式蓝牙传感器发射蓝牙标签查询信号，搜索巷道内附近蓝牙标签，若没有发现标签，持续搜索；若发现蓝牙标签，结合外部显示终端地图信息，确定参考航向，导航装置开始导航。

5)解算：导航装置根据mems惯性传感器数据进行惯性导航解算，解算得到位置姿态信息，3轴陀螺和加计数据和3轴mems加速度计的数据可以融合以抑制陀螺漂移；组合导航信号解算模块解算出的姿态数据与方位、俯仰角度传感器数据比较后，得到方位、俯仰电机控制量，进行复合式mems惯导平台稳定控制。

如图6所示，导航装置根据mems惯性传感器数据进行惯性导航解算，蓝牙/ins组合导航系统数据融合过程如下：

a)mems惯性传感器获取载体运动信息，mems捷联惯导解算得到包括巷道内车辆的姿态、位置和速度的导航信息；

b)通过主动蓝牙发射接收模块获取巷道内蓝牙标签信息，通过dsp中探测子滤波器获取包括巷道内车辆位置、速度的蓝牙导航信息；

c)将步骤a)获取的导航信息和步骤b)获取的蓝牙导航信息进行输出信号比较，得到误差量，送入imm卡尔曼滤波器进行误差估计，对mems捷联惯导进行参数修正后得到最终的导航信息，并传输至外部终端设备。

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

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

式中，l为当地纬度，r为地球半径，h为高度，x、y、z分别为相对当地参考坐标系的距离，选取当地参考坐标系原点，朝东为x轴正向，朝北为y轴正向，朝天为z轴正向。ve、vn、vu分别为东向、北向和天向速度，ωie为地球自转角速率，εbi、(i＝e,n,u)为地理坐标系内陀螺的等效漂移和加速度计的等效偏置，εwi为陀螺角增量输出的随机游走。φe、φu、φn为三个方向上的姿态失准角。所有变量需要统一后加入卡尔曼滤波器输入。δx、δy、δz、δvn、δve、δvu分别为三个位置误差量和三个速度误差量。

状态变量为：

观测变量为蓝牙标签位置和惯导位置的三个位置误差信息，蓝牙标签观测速度和惯导速度的三个速度误差信息。构建imm卡尔曼滤波器的离散卡尔曼滤波方程进行组合导航解算和误差估计。imm卡尔曼滤波器中包含多个卡尔曼滤波器，区别在于观测噪声协方差矩阵r和系统噪声协方差矩阵q不同，统一的量测方程为：

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

将上述连续状态方程和观测方程离散化，采用离散型卡尔曼滤波基本方程对误差作最优估计。由于车辆在巷道内主要沿巷道方向进行运动，则可以通过约束方程约束垂直巷道方向和向上的位置和速度，以提高融合精度。

在卡尔曼滤波器中存在观测噪声协方差矩阵r和系统噪声协方差矩阵q，不同车辆环境和蓝牙标签布置情况下可能存在不同的q和r，因此，根据不同的q和r建立多个状态方程和观测方程，构建imm卡尔曼滤波器，即交互多模型卡尔曼滤波器，以提高导航系统对不同车辆环境和矿井环境的适应能力。

6)组合导航：在蓝牙标签数据有效区域，数字信号处理器dsp采用卡尔曼滤波器进行蓝牙/mems惯性组合导航；在蓝牙标签数据无效区域，由惯导装置维持位置递推。

7)再次进入蓝牙标签数据有效的区域，发现新标签，若满足切换条件，进行标签切换。dsp中预设蓝牙信号标签信号阈值m，新标签信号n若大于等于阈值m，则切换；若小于则不切换，维持惯性导航。

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

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

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

主动查询式蓝牙/复合式mems惯导组合导航模块将各单一传感器进行数据融合，取长补短，能够获得较单一传感器更好的导航精度和稳定性。该方法改进了传统蓝牙标签的广播模式，大大节省了标签耗电量，同时采用微机械稳定平台隔离载体颠簸扰动，改善了mems捷联惯导的运行环境，有利于整体导航精度的提升。

本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。