CN109470245B - 主动查询式蓝牙/复合式mems惯导组合导航装置及方法 - Google Patents
主动查询式蓝牙/复合式mems惯导组合导航装置及方法 Download PDFInfo
- Publication number
- CN109470245B CN109470245B CN201811612212.XA CN201811612212A CN109470245B CN 109470245 B CN109470245 B CN 109470245B CN 201811612212 A CN201811612212 A CN 201811612212A CN 109470245 B CN109470245 B CN 109470245B
- Authority
- CN
- China
- Prior art keywords
- navigation
- bluetooth
- module
- sensor
- tag
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 239000002131 composite material Substances 0.000 title claims abstract description 27
- 238000000034 method Methods 0.000 title claims abstract description 22
- 238000004364 calculation method Methods 0.000 claims description 16
- 238000004891 communication Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 4
- 150000001875 compounds Chemical class 0.000 claims description 3
- 238000007499 fusion processing Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 239000003245 coal Substances 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000006641 stabilisation Effects 0.000 description 2
- 238000011105 stabilization Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000010587 phase diagram Methods 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 239000000523 sample Substances 0.000 description 1
- 230000000087 stabilizing effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本发明公开了一种主动查询式蓝牙/复合式MEMS惯导组合导航装置及方法，包括组合导航信号解算模块、传感器模块和电源模块，通过传感器模块采集巷道内车辆运动信息和蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块，将采集的巷道内车辆运动信息和蓝牙标签的数据进行解算、数据融合，将解算后的导航数据传输至外部终端设备，并显示车辆位置、速度及航向等信息。本发明通过融合蓝牙和惯导数据进行组合导航，降低了对单一蓝牙传感器的依赖，在无蓝牙信号区域可采用惯导位置递推，提高了定位导航连续性。该主动查询式蓝牙定位模式可有效增加蓝牙基站标签的使用寿命，降低成本，提高惯导系统在车辆严重颠簸和振动下的可靠性和精度。
Description
技术领域
本发明属于导航定位技术领域，涉及一种使用蓝牙标签、惯性导航系统进行组合导航的装置及方法。
背景技术
在我国煤矿安全生产过程中，井下人员和车辆的精确定位始终是核心内容之一，优秀的定位方案可以有效提高煤矿管理水平和煤矿运作效率。
基于蓝牙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捷联惯导的运行环境，有利于整体导航精度的提升。
本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。
Claims (7)
1.一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述方法采用的导航装置包括：
组合导航信号解算模块，用于通过数字信号处理器DSP对传感器模块采集的巷道内车辆运动信息和蓝牙标签的数据进行解算、数据融合，将解算后的导航数据传输至外部终端设备，并显示车辆位置、速度及航向信息；
传感器模块，用于通过MEMS惯性传感器采集巷道内车辆运动信息和通过主动式蓝牙传感器采集蓝牙标签的数据，并将采集的数据传输至组合导航信号解算模块；
电源模块，用于对组合导航信号解算模块和传感器模块提供电源；
所述传感器模块包括主动式蓝牙传感器和复合式MEMS惯导平台；
所述复合式MEMS惯导平台包括MEMS惯性传感器和微机械稳定转台，所述MEMS惯性传感器安装在微机械稳定转台上；
所述MEMS惯性传感器包括3轴MEMS陀螺、3轴MEMS加速度计和MEMS磁强计；
所述微机械稳定转台框架上设有方位、俯仰电机，方位、俯仰角度传感器和驱动模块；
所述主动式蓝牙传感器为查询式蓝牙传感器，只有当车载导航模块中的主动式蓝牙传感器向沿巷道墙壁布置的蓝牙标签基站发射查询指令的时候，蓝牙标签基站模块才会回传信号；
所述方法包括如下步骤：
1)检查导航装置与外部显示终端通信接口、导航装置与巷道内车辆车载终端CAN接口是否连接正常，若未连接则进行手动连接；
2)巷道内车辆发动机启动，电源开始供电，外部显示终端自动开机，导航装置开机；
3)复合式MEMS惯导平台初始化至水平位置，复合式MEMS惯导平台与数字信号处理器DSP构成的惯导系统自对准；
4)针对巷道内路况复杂，车辆颠簸较严重，会对MEMS惯导系统产生振动影响，导航装置中主动式蓝牙传感器发射蓝牙标签查询信号，搜索巷道内附近蓝牙标签，主动式蓝牙探测器每隔一段时间向外发射一次查询信号，车辆附近的蓝牙标签接收到查询信号后再发射含有标签信息的信号，再被蓝牙探测器接收；
若没有发现标签，持续搜索；若发现蓝牙标签，结合地图信息，确定参考航向，导航装置开始导航；
5)导航装置根据MEMS惯性传感器采集的数据进行惯性导航解算，解算得到位置姿态信息；3轴MEMS陀螺和3轴MEMS加速度计的数据可以融合以抑制陀螺漂移；组合导航信号解算模块解算出的位置姿态数据与方位、俯仰角度传感器数据比较后，得到方位、俯仰电机的控制量，进行复合式MEMS惯导平台稳定控制；
6)在蓝牙标签数据有效区域，数字信号处理器DSP采用卡尔曼滤波器进行蓝牙/MEMS惯性组合导航；在蓝牙标签数据无效区域，由惯导装置维持位置递推；
7)再次进入蓝牙标签数据有效的区域，发现新标签，若满足切换条件，进行标签切换；
8)重复步骤5)-步骤7)，直至导航结束。
2.根据权利要求1所述的一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述切换条件为，数字信号处理器DSP中预设蓝牙信号标签信号阈值M，新标签信号N若大于等于阈值M，则切换；若小于则不切换，维持惯性导航。
3.根据权利要求1所述的一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述步骤5)中，导航装置根据MEMS惯性传感器数据进行惯性导航解算，蓝牙/INS组合导航系统数据融合过程如下：
a)MEMS惯性传感器获取载体运动信息，MEMS捷联惯导解算得到包括巷道内车辆的姿态、位置和速度的导航信息；
b)通过主动蓝牙发射接收模块获取巷道内蓝牙标签信息，通过DSP中蓝牙探测子滤波器获取包括巷道内车辆位置、速度的蓝牙导航信息；
c)将步骤a)获取的导航信息和步骤b)获取的蓝牙导航信息进行输出信号比较，得到误差量，送入IMM卡尔曼滤波器进行误差估计，对MEMS捷联惯导进行参数修正后得到最终的导航信息，并传输至外部终端设备。
4.根据权利要求3所述的一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述导航信息与蓝牙导航信息进行输出信号比较，构建IMM卡尔曼滤波方程进行组合导航解算和误差估计，量测方程为：
式中，Z(k)，k＝1…6为实际三个位置三个速度的观测误差，X、Y、Z分别为相对当地参考坐标系的距离，VE、VN、VU分别为东向、北向和天向速度，Vk，k＝1…6为观测噪声，δX、δY、δZ、δVN、δVE、δVU分别为三个位置误差量和三个速度误差量。
5.根据权利要求1所述的一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述组合导航信号解算模块包括数字信号处理器DSP、存储器和串口通信模块。
6.根据权利要求1所述的一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述MEMS惯性传感器、主动式蓝牙传感器和微机械稳定转台分别通过串口与数字信号处理器DSP连接，数字信号处理器DSP通过串口与外部显示终端相连。
7.根据权利要求1所述的一种主动查询式蓝牙/复合式MEMS惯导组合导航方法，其特征在于，所述电源模块包括5V转3.3V模块、5V转1.8V模块和5V转1.2V模块；5V转3.3V模块为传感器模块供电，5V转3.3V模块、5V转1.2V模块和5V转1.8V模块为数字信号处理器DSP供电。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811612212.XA CN109470245B (zh) | 2018-12-27 | 2018-12-27 | 主动查询式蓝牙/复合式mems惯导组合导航装置及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811612212.XA CN109470245B (zh) | 2018-12-27 | 2018-12-27 | 主动查询式蓝牙/复合式mems惯导组合导航装置及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109470245A CN109470245A (zh) | 2019-03-15 |
| CN109470245B true CN109470245B (zh) | 2024-02-13 |
Family
ID=65676729
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811612212.XA Active CN109470245B (zh) | 2018-12-27 | 2018-12-27 | 主动查询式蓝牙/复合式mems惯导组合导航装置及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109470245B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111489572A (zh) * | 2020-04-27 | 2020-08-04 | 贵州锦丰矿业有限公司 | 一种井下车辆交通管理系统、管理方法和计算机设备 |
| CN111698774B (zh) * | 2020-06-23 | 2022-07-26 | 北京麦钉艾特科技有限公司 | 基于多源信息融合的室内定位方法及装置 |
| CN112461238B (zh) * | 2020-12-14 | 2023-03-10 | 北京航天控制仪器研究所 | 一种动态随机布设信标的室内人员定位导航系统及方法 |
| CN114383602B (zh) * | 2021-12-30 | 2024-03-29 | 北京无线电计量测试研究所 | 一种封闭或半封闭空间内车辆导航装置及导航方法 |
| CN114739401A (zh) * | 2022-04-12 | 2022-07-12 | 北京航空航天大学 | 基于mems惯性传感器和无线电广播信号的车载导航定位方法 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101754470A (zh) * | 2008-11-28 | 2010-06-23 | 中兴通讯股份有限公司 | 一种实现蓝牙主动发出信号的方法及手机 |
| CN104457748A (zh) * | 2013-09-18 | 2015-03-25 | 南京理工大学 | 一种嵌入式瞄准吊舱测姿系统及其传递对准方法 |
| CN104777451A (zh) * | 2014-05-28 | 2015-07-15 | 李杨 | 一种基于蓝牙及数字通讯定位技术的防丢失系统 |
| CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位系统及方法 |
| CN106303937A (zh) * | 2015-05-12 | 2017-01-04 | 广州杰赛科技股份有限公司 | 基于蓝牙的定位认证装置 |
| CN106949889A (zh) * | 2017-03-17 | 2017-07-14 | 南京航空航天大学 | 针对行人导航的低成本mems/gps组合导航系统及方法 |
| CN108255171A (zh) * | 2016-12-29 | 2018-07-06 | 百度（美国）有限责任公司 | 用于提高自主驾驶车辆的稳定性的方法和系统 |
| CN108924745A (zh) * | 2018-07-09 | 2018-11-30 | 上海伟梦物联网科技有限公司 | 一种消防疏散救援引导方法、系统、存储介质及移动终端 |
-
2018
- 2018-12-27 CN CN201811612212.XA patent/CN109470245B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101754470A (zh) * | 2008-11-28 | 2010-06-23 | 中兴通讯股份有限公司 | 一种实现蓝牙主动发出信号的方法及手机 |
| CN104457748A (zh) * | 2013-09-18 | 2015-03-25 | 南京理工大学 | 一种嵌入式瞄准吊舱测姿系统及其传递对准方法 |
| CN104777451A (zh) * | 2014-05-28 | 2015-07-15 | 李杨 | 一种基于蓝牙及数字通讯定位技术的防丢失系统 |
| CN106303937A (zh) * | 2015-05-12 | 2017-01-04 | 广州杰赛科技股份有限公司 | 基于蓝牙的定位认证装置 |
| CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位系统及方法 |
| CN108255171A (zh) * | 2016-12-29 | 2018-07-06 | 百度（美国）有限责任公司 | 用于提高自主驾驶车辆的稳定性的方法和系统 |
| CN106949889A (zh) * | 2017-03-17 | 2017-07-14 | 南京航空航天大学 | 针对行人导航的低成本mems/gps组合导航系统及方法 |
| CN108924745A (zh) * | 2018-07-09 | 2018-11-30 | 上海伟梦物联网科技有限公司 | 一种消防疏散救援引导方法、系统、存储介质及移动终端 |
Non-Patent Citations (2)
| Title |
|---|
| 惠飞.FPGA嵌入式系统开发与实例.西安电子科技大学出版社,2017,185-187. * |
| 施云波.无线传感器网络技术概论.西安电子科技大学出版社,2017,216-221. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109470245A (zh) | 2019-03-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109470245B (zh) | 主动查询式蓝牙/复合式mems惯导组合导航装置及方法 | |
| CN109443350B (zh) | 基于神经网络的蓝牙/光电/ins组合导航装置及方法 | |
| Wang et al. | Integration of GPS/INS/vision sensors to navigate unmanned aerial vehicles | |
| CN104848859B (zh) | 一种三轴惯性稳定平台及其自定位定向的控制方法 | |
| US20070241886A1 (en) | Inertial sensor tracking system | |
| AU2016101951A4 (en) | Navigation of mining machines | |
| CN105371871A (zh) | 井下采煤机捷联惯导系统的组合初始对准系统及对准方法 | |
| RU2768087C2 (ru) | Транспортное средство для подземных горных работ и способ его ориентации | |
| CN113029139B (zh) | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 | |
| CN109282804B (zh) | 一种单轴光纤陀螺寻北算法 | |
| CN102645665A (zh) | 基于bd、gps和dr的定位信息处理方法及装置 | |
| CN113220013A (zh) | 一种多旋翼无人机隧洞悬停方法及系统 | |
| US20210035456A1 (en) | Unmanned aircraft, and method and system for navigation | |
| Zheng et al. | An optimization-based UWB-IMU fusion framework for UGV | |
| CN114018273A (zh) | 煤矿井下自动驾驶车辆精确定位系统及其定位方法 | |
| CN111221020A (zh) | 一种室内外定位方法、装置及系统 | |
| CN103383260A (zh) | 一种无人机导航系统及其合作导航系统 | |
| CN105953797B (zh) | 利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法 | |
| CN107783163A (zh) | 一种智能轮式机器人行进航向角融合方法 | |
| CN209247031U (zh) | 一种主动查询式蓝牙/复合式mems惯导组合导航装置 | |
| CN108775899A (zh) | 基于伪卫星和惯性信息的深部开采井上下坐标系连接方法 | |
| Kubo et al. | DGPS/INS/VVheelSensor Integrationfor High Accuracy Land-Vehicle Positioning | |
| CN107807375A (zh) | 一种基于多gps接收机的无人机姿态追踪方法及系统 | |
| CN209247030U (zh) | 一种基于神经网络的蓝牙/光电/ins组合导航装置 | |
| CN101644578A (zh) | 基于网络计算的物体运动轨迹跟踪系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20230531 Address after: 415106 The first floor of the standardized factory building in Phase 4 of Zhonglian Avenue, High tech Zone, Changde City, Hunan Province Applicant after: Hunan Tongren Intelligent Technology Co.,Ltd. Address before: Room 10302, 3rd floor, building 3, East Industrial Building, modern enterprise center, No.2 zhangbawu Road, high tech Zone, Xi'an City, Shaanxi Province, 710077 Applicant before: XI'AN ZHONGKE PHOTOELECTRIC PRECISION ENGINEERING Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |