CN110411444B - 一种地面下采掘移动设备用惯性导航定位***与定位方法 - Google Patents

一种地面下采掘移动设备用惯性导航定位***与定位方法 Download PDF

Info

Publication number
CN110411444B
CN110411444B CN201910777601.6A CN201910777601A CN110411444B CN 110411444 B CN110411444 B CN 110411444B CN 201910777601 A CN201910777601 A CN 201910777601A CN 110411444 B CN110411444 B CN 110411444B
Authority
CN
China
Prior art keywords
mining mobile
underground mining
mobile equipment
coordinate system
optical fiber
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
Application number
CN201910777601.6A
Other languages
English (en)
Other versions
CN110411444A (zh
Inventor
邹健
李磊
贺媛
甘助
赵文
李兰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Saiao Aviation Technology Co ltd
Original Assignee
Shenzhen Saiao Aviation Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Saiao Aviation Technology Co ltd filed Critical Shenzhen Saiao Aviation Technology Co ltd
Priority to CN201910777601.6A priority Critical patent/CN110411444B/zh
Publication of CN110411444A publication Critical patent/CN110411444A/zh
Application granted granted Critical
Publication of CN110411444B publication Critical patent/CN110411444B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/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/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

一种地面下采掘移动设备用惯性导航定位***和定位方法，包括三轴一体光纤惯性测量装置、上位机综合显示与控制装置和电源，设有里程仪和光电摄像头，里程仪与三轴一体光纤惯性测量装置、上位机综合显示与控制装置相连接，并固定安装在地面下采掘移动设备的驱动***上，光电摄像头通过固定支架铰接固定在地面下采掘移动设备一侧的液压支架上，拍摄方向朝着地面下采掘移动设备运行方向，方法包括采用里程仪测量的地面下采掘移动设备位姿信息，通过惯性导航算法对惯性测量装置的测量误差进行校正。在不需要大幅度提高成本的前提下实现大幅度提高地面下采掘移动设备的导航测量精度，以及地面下采掘移动设备的工作效率和安全性，可以适应无人操作需要。

Description

一种地面下采掘移动设备用惯性导航定位***与定位方法
技术领域
本发明涉及导航定位，特别是涉及一种地面下采掘移动设备用惯性导航定位***与定位方法。
背景技术
现有的地面下采掘移动设备工作面直线度测量技术如视觉测量、行程测量等存在视觉盲区，测量数据只反映相邻支架位移偏差。由于地面下的特殊复杂的环境条件，通常采用的机械或光学测量***难以精确测量地面下采掘移动设备的运动轨迹，无法实现对地面下采掘移动设备位置的确定，不能达到通过自动控制使其按设定路线运行的目的。而采用惯性导航测量地面下采掘移动设备在工作面的运动轨迹，可以达到通过自动控制使其按设定路线运行的目的。
捷联惯性导航***(Strapdown inertial navigation system，缩略词为SINS)是在平台式惯性导航***基础上发展的一种无框架的导航参数解算***，其采用的三轴一体光纤惯性测量装置由三个高精度光学陀螺仪、三个加速度计、微处理器组成，三个高精度光学陀螺仪和三个加速度计按照地面下采掘移动设备的站心直角坐标系的X、Y、Z轴精确组合，固定连接在地面下采掘移动设备上。该***基于三个高精度光学陀螺仪的输出建立导航测量坐标系，且基于三个加速度计的输出通过高速积分解算出运载体在导航测量坐标系下的航向、速度和位姿信息。由于三轴一体光纤惯性测量装置在进入测量工作状态之前都需要进行初始对准，将地面下采掘移动设备的站心直角坐标系(又称东-北-天坐标系)即工作平台坐标系的速度和位姿信息变换到惯性导航***的导航测量坐标系，确定工作平台坐标系与导航测量坐标系重合，进行初始对准的算法需要得到地面下采掘移动设备的对地速度，然而，地面下采掘移动设备的工作场景是变化的，其包括航向角、俯仰角和横滚角的姿态角又存在误差，会导致检测结果不精确，而且，三个高精度光学陀螺仪的漂移误差和三个加速度计的测量误差会随时间逐渐积累，三轴一体光纤惯性测量装置长时间运行必将导致客观的积累误差，严重影响地面下采掘移动设备的目标分析与速度检测。此外，随着采掘工作面自动化程度的提高，对地面下采掘移动设备的精度、稳定性提出更高的要求，地面下采掘移动设备的安装误差角在一定程度上对***的稳定性产生很大的影响，有必要对其进行误差补偿、误差校正。
发明内容
本发明所要解决的一个技术问题是弥补上述现有技术的缺陷，提供一种地面下采掘移动设备用惯性导航定位***。
本发明所要解决的另一个技术问题是弥补上述现有技术的缺陷，提供一种地面下采掘移动设备用惯性导航定位方法。
本发明的地面下采掘移动设备用惯性导航定位***技术问题通过以下技术方案予以解决。
这种地面下采掘移动设备用惯性导航定位***，
包括三轴一体光纤惯性测量装置，所述三轴一体光纤惯性测量装置由三个高精度光学陀螺仪、三个加速度计和微处理器组成，三个高精度光学陀螺仪和三个加速度计按照地面下采掘移动设备的站心直角坐标系的X、Y、Z轴精确组合，固定连接在地面下采掘移动设备上，所述三轴一体光纤惯性测量装置在进入测量工作状态之前进行初始对准，将地面下采掘移动设备的站心直角坐标系的速度和位姿信息变换到惯性导航定位***的导航测量坐标系，确定站心直角坐标系与导航测量坐标系合成，所述三个高精度光学陀螺仪和三个加速度计是惯性敏感器件，分别对地面下采掘移动设备的站心直角坐标系的X、Y、Z轴三个方向的移动实时角速率、实时加速度进行测量，并将测量值传送所述微处理器，所述微处理器采用惯性导航算法进行解算，计算包括航向角、俯仰角和横滚角的姿态角，将三个加速度计测量的站心直角坐标系的比力分量转换到导航测量坐标系下，在导航测量坐标系下通过积分求解比力方程，得到地面下采掘移动设备相对地球的速度，在站心直角坐标系下由地面下采掘移动设备相对地球的速度积分得到地面下采掘移动设备的位姿信息，以及采用卡尔曼滤波器对所述三轴一体光纤惯性测量装置的测量误差进行校正，所述三轴一体光纤惯性测量装置还通过零速校正算法消除***的累积误差。
还包括上位机综合显示与控制装置，所述上位机综合显示与控制装置包括导航计算机和触摸屏，导航计算机通过通讯接口与所述微处理器连接，建立数据通讯，根据三轴一体光纤惯性测量装置的定位结果进行轨迹拟合，结合惯导粗对准后的误差模型建立状态方程，进行滤波融合，经数据融合运算定位，得到地面下采掘移动设备的运行轨迹，触摸屏通过图形、图表显示包括采掘深度、轨道偏差，以及包括航向角、俯仰角和横滚角的姿态角的位姿信息，还显示地面下采掘移动设备的水平运行曲线，以及实际运行过程中的偏航角，通过轨迹拟合算法与预定采掘轨迹比较迭代取值，结合地面下采掘移动设备支架布局迭代计算每个支架的偏差量，同时，对地面下采掘移动设备实现定位控制，地面下采掘移动设备依靠自适应功能沿着定位控制的轨道运行和工作。
还包括电源。
这种地面下采掘移动设备用惯性导航定位***的特点是：
还设有里程仪，所述里程仪分别通过通讯接口与所述三轴一体光纤惯性测量装置、所述上位机综合显示与控制装置相连接，并固定安装在地面下采掘移动设备的驱动***上，测量地面下采掘移动设备包括移动距离和速度的实时位姿信息，并将实时位姿信息通过通讯接口传送所述上位机综合显示与控制装置，里程仪与三个高精度光学陀螺仪构成航位推算***，采用里程仪测量的地面下采掘移动设备位姿信息，通过惯性导航算法对三轴一体光纤惯性测量装置的测量误差进行校正，所述三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度，在卫星导航无法探测的地面下，采用里程仪引入地面下采掘移动设备的地理坐标信息融合定位，实现地面下采掘移动设备的精确定位和运动轨迹解算，大幅度提高地面下采掘移动设备的导航测量精度。
还设有光电摄像头，所述光电摄像头通过固定支架铰接固定在地面下采掘移动设备一侧的液压支架上，且光电摄像头的拍摄方向朝着地面下采掘移动设备运行方向，光电摄像头对地面下采掘移动设备所处的工作面环境拍摄图像，拍摄频率为25帧/s，并将已拍摄的图像传送所述微处理器，当地面下采掘移动设备在拍摄环境中移动时，拍摄的目标图像发生变化，根据地面下采掘移动设备的移动方向确定其移动方向，进行定位。
本发明的地面下采掘移动设备用惯性导航定位***技术问题通过以下进一步的技术方案予以解决。
还设有防爆盒，所述三轴一体光纤惯性测量装置安装在防爆盒内，所述防爆盒通过固定支架安装固定在地面下采掘移动设备机体上。
所述防爆盒，是按照地面下采掘移动设备专用防爆盒的规范和标准设计的防爆盒，所述防爆盒外部供电电压：24V DC；外形尺寸：长×宽×高≤400mm×250mm×200mm。
所述固定支架是设有圆柱橡胶减震垫的燕尾槽型可调节柔性支撑骨架，所述圆柱橡胶减震垫对称固定在所述防爆盒底部。
所述电源是防爆型USB电源,用于保证地面下供电的三轴一体光纤惯性测量装置可靠运行。
本发明的地面下采掘移动设备用惯性导航定位***技术问题通过以下再进一步的技术方案予以解决。
所述微处理器包括微处理单元模块、通信模块、报警模块、数据存储模块、隔离电路和供电模块，所述微处理单元模块分别与通信模块、报警模块、数据存储模块、隔离电路和供电模块相连接。
所述微处理单元模块采用美国德州仪器(Texas Instruments，缩略词为TI)公司出品的具有数字信号处理器(Digital Signal Processor，缩略词为DSP)功能的单片机，通过总线从ROM中将应用程序调入所述上位机综合显示与控制装置的导航处理器进行数据处理，处理结果再次回送总线，对三轴一体光纤惯性测量装置进行误差补偿、辅助信号跟踪环路、显示测量信息、传送控制***和外部通信接口。
优选的是，所述微处理单元模块是型号为80196的单片机、型号为8086的单片机，以及型号为8087的单片机中的一种。
所述三个高精度光学陀螺仪是随机漂移稳定性为0.01°/h的惯性级三个光学陀螺仪，采用高精度惯性级光学陀螺仪既符合地面下采掘移动设备的定位精度要求，又有利于成本的控制。
所述三个加速度计是刻度因数稳定性为10-4mv/g数量级的三个加速度计，所述三个加速度计基于加速度的基本原理以地磁线为绝对参照物，在无外力加速度的情况下准确测量地面下采掘移动设备的航向、速度和位姿信息。
所述导航计算机是內嵌数据分析拟合及处理软件的导航计算机，所述数据分析拟合及处理软件是DSP软件,包括采用C语言在代码设计套件(Code Composer Studio，缩略词为CCS)平台上开发的外部通讯接口模块、运行模式分析模块、地面下采掘移动设备状态调校模块、故障自检模块和内部通讯接口模块,所述外部通讯接口模块、运行模式分析模块、地面下采掘移动设备状态调校模块、故障自检模块和内部通讯接口模块通过应用程序编程接口(Application Programming Interface，缩略词为API)相互调用运行。
本发明的地面下采掘移动设备惯性导航定位方法通过以下技术方案予以解决。
这种地面下采掘移动设备惯性导航定位方法，采用上述地面下采掘移动设备惯性导航定位***。
这种地面下采掘移动设备惯性导航定位方法的特点是：
包括预步骤：对三轴一体光纤惯性测量装置预设参量，以及对微处理器进行误差补偿、误差校正，还包括以下步骤：
1)测量地面下采掘移动设备包括移动距离、初速度和初始状态的角速率的实时位姿信息，并将实时位姿信息通过通讯接口传送上位机综合显示与控制装置；
2)更新计算地面下采掘移动设备的移动方向、速度和角速率的变化；
3)将站心直角坐标系转换为导航测量坐标系，通过整合两个不同的坐标系下的参数对地面下采掘移动设备的位姿信息不断进行修正；
4)将测量的速度和位置信息进行计算，得到地面下采掘移动设备的姿态变化及运动轨迹，三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿；
5)引用自身参数进行迭代运算，在地面下采掘移动设备运动测量中以旧的的参数变化值重复计算多次，对运动变化量和方向进行修正和逼近，如果运动变化量和方向NO，返回步骤1)；如果运动变化量和方向YES，进入步骤6)；
6)采用里程仪测量的地面下采掘移动设备位姿信息，通过卡尔曼滤波算法对三轴一体光纤惯性测量装置的测量误差进行校正；
7)在开机运行或出现突发异常情况时，上位机综合显示与控制装置进行人工干预调整，修正地面下采掘移动设备的航向角，确认定位对准和运行轨迹，以及进行动态调整；
8)基于上位机综合显示与控制装置触摸屏显示的包括采掘深度、轨道偏差，以及包括航向角、俯仰角和横滚角的姿态角的位姿信息确认定位是否对准，如果NO，返回预步骤对微处理器进行误差补偿、误差校正；如果YES，结束地面下采掘移动设备惯性导航定位。
本发明的地面下采掘移动设备惯性导航定位方法通过以下进一步的技术方案予以解决。
所述步骤1)包括以下分步骤；
1.1)光电摄像头对移动中的地面下采掘移动设备所处的环境拍摄实时图像，拍摄频率为25帧/s，并将已拍摄的目标的变化图像传送上位机综合显示与控制装置的导航计算机；
1.2)对三个高精度光学陀螺仪和三个加速度计的导航测量坐标系设置预设参量；
1.3)上位机综合显示与控制装置读取三轴一体光纤惯性测量装置的初始参数、地面下采掘移动设备的初速度和初始状态的角速率，根据其移动方向、初速度和初始状态的角速率，对地面下采掘移动设备的实际状况进行定位。
所述步骤2)包括以下分步骤；
2.1)三个高精度光学陀螺仪、三个加速度计分别测量表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，并将测量值传送微处理器，通过惯性导航算法解算地面下采掘移动设备的定位结果；
2.2)将惯性导航算法处理后的地面下采掘移动设备定位结果通过通讯接口传送微处理器，作为下一次进行位置解算的初值，同时更新计算地面下采掘移动设备的移动方向、速度和角速率的变化。
所述步骤3)包括以下分步骤；
3.1)将三个加速度计测量的站心直角坐标系的比力分量转换到导航测量坐标系，在导航测量坐标系下通过积分求解比力方程，得到地面下采掘移动设备相对地球的速度，在站心直角坐标系下由地面下采掘移动设备相对地球的速度积分得到地面下采掘移动设备的位姿信息；
3.2)采用捷联惯导的比力方程将比力信息投影到惯性坐标系，得到比力相对于惯性空间随地球旋转而引起的方向变化信息，通过整合两个不同的坐标系下的参数对地面下采掘移动设备的位姿信息不断进行修正。
所述步骤4)包括以下分步骤；
4.1)将测量的地面下采掘移动设备站心直角坐标系下的初始位置坐标传送惯性导航***，指导地面下采掘移动设备按设定路线推进；
4.2)将测量的速度和位置信息进行计算，得到地面下采掘移动设备的姿态变化及运动轨迹，上位机综合显示与控制装置显示地面下采掘移动设备的水平运行曲线；
4.3)三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度。
所述步骤4.3)包括以下子步骤；
4.3.1)地面下采掘移动设备停靠在起点时，启动***配置的初始对准参数，在里程仪参数位置传送标度数和失准角，点击“寻北指令”进行运算，在寻北过程将要结束时，将航向角数值在不超过地球正北向0.01°范围内设定为零值，达到计数后寻北动作完成，得到精确的航向起始角；
4.3.2)启动地面下采掘移动设备行走机构，地面下采掘移动设备从起点运行到终点后，得到沿路径测量的表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度；
4.3.3)根据里程仪提供的比例因数，传送实际起点与终点的相对位置偏差，以及表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，计算实际的里程仪标度、俯仰角的补偿角，以及航向角的补偿角；
4.3.4)再次启动地面下采掘移动设备行走机构，地面下采掘移动设备从终点运行至起点后，传送子步骤4.3.1)沿路径测量的表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，再次点击“寻北指令”进行运算，等待5～10秒的时间，测量地面下采掘移动设备的位姿变化的差值，完成地面下采掘移动设备初始对准的位置标定过程；
4.3.5)根据地面下采掘移动设备运动速度矢量，更新计算站心直角坐标系与导航测量坐标系之间的转换矩阵，计算更新地面下采掘移动设备在站心直角坐标系下的位置坐标；
4.3.6)重复计算步骤，在设定的惯性测量时段内，迭代计算得到每个时刻的地面下采掘移动设备在站心直角坐标系下的运动速度矢量及位置坐标，三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度。
所述步骤6)包括以下分步骤；
6.1)采用卡尔曼滤波算法处理三个高精度光学陀螺仪和三个加速度计互补滤波的数据，进行误差补偿和修正，地面下采掘移动设备正常工作，三轴一体光纤惯性测量装置正常运行，微处理器对信号强度是否满足需求进行判断；
如果信号强度满足需求，将三个高精度光学陀螺仪和三个加速度计测量的地面下采掘移动设备定位数据传送卡尔曼滤波算法进行优化；
如果信号强度不满足需求，只采用三轴一体光纤惯性测量装置测量的定位数据作为地面下采掘移动设备位置信息，通过分析确定当前工作面的直线度，并进行动态调整；
6.2)对惯性导航初始速度的误差进行姿态更新，并补偿对准过程中估计出的三个高精度光学陀螺仪的漂移，采用里程仪测量的地面下采掘移动设备位姿信息，通过卡尔曼滤波算法对三轴一体光纤惯性测量装置的测量误差进行校正。
所述步骤7)包括以下分步骤；
7.1)第一次校准：三轴一体光纤惯性测量装置进行上电操作，待其内置的三个高精度光学陀螺仪和三个加速度计工作稳定后，测量其在地球重力作用下的三轴加速度分量，如果三轴一体光纤惯性测量装置存在安装偏差角，三轴一体光纤惯性测量装置先绕自身X轴逆时针旋转角度a，直至Y'轴加速度输出为0，再绕自身Y轴顺时针旋转角度θ，直至X'轴加速度输出也为0；
7.2)第二次校准；如果三轴一体光纤惯性测量装置还存在安装偏差角，三轴一体光纤惯性测量装置先绕自身的X轴顺时针旋转90°，此时Y轴指向天，再绕自身Z轴逆时针旋转角度T，直至X'轴加速度输出为0，Z'轴加速度输出也为0；
7.3)在导航测量坐标系与站心直角坐标系合成后，通过导航测量坐标系与站心直角坐标系之间的转换矩阵计算导航测量坐标系与站心直角坐标系之间的相对位置，进行多次修正，以消除三轴一体光纤惯性测量装置存在的臂杆效应造成的重力加速度g的数值脉冲突变。
本发明与现有技术相比的有益效果是：
本发明采用里程仪测量的地面下采掘移动设备位姿信息，通过惯性导航算法对三轴一体光纤惯性测量装置的测量误差进行校正，可以提高姿态导航解算的精度，有效解决单纯的采用惯性导航定位存在抗冲击性差、稳定性和可靠性不足，以及累积误差不断增大的问题，在不需要大幅度提高成本的前提下实现地面下采掘移动设备定位装置与其惯性导航间安装角进行误差补偿、误差校正、精确定位和自主导航，大幅度提高地面下采掘移动设备的导航测量精度，以及地面下采掘移动设备的工作效率和安全性，可以适应无人操作需要。环境适应能力强，实时稳定性好，导航精度更高，安全性更高，效率更高，体积更小，成本更低。
附图说明
图1是本发明具体实施方式的惯性导航定位***组成方框图。
具体实施方式
下面结合具体实施方式并对照附图对本发明进行说明。
一种如图1所示的地面下采掘移动设备用惯性导航定位***包括三轴一体光纤惯性测量装置2，三轴一体光纤惯性测量装置2由包括三个高精度光学陀螺仪21、三个加速度计22和微处理器23组成。
三个高精度光学陀螺仪21是随机漂移稳定性为0.01°/h的惯性级光学陀螺仪，采用高精度惯性级光学陀螺仪既符合地面下采掘移动设备的定位精度要求，又有利于成本的控制，三个加速度计22是刻度因数稳定性为10-4mv/g数量级的加速度计，加速度计基于加速度的基本原理以地磁线为绝对参照物，在无外力加速度的情况下准确测量地面下采掘移动设备1的航向、速度和位姿信息。
三个高精度光学陀螺仪21和三个加速度计22按照地面下采掘移动设备1的站心直角坐标系的X、Y、Z轴精确组合，固定连接在地面下采掘移动设备1上，三轴一体光纤惯性测量装置2在进入测量工作状态之前进行初始对准，将地面下采掘移动设备1的站心直角坐标系的速度和位姿信息变换到惯性导航定位***的导航测量坐标系，确定站心直角坐标系与导航测量坐标系合成，三个高精度光学陀螺仪21和三个加速度计22是惯性敏感器件，分别对地面下采掘移动设备1的站心直角坐标系的X、Y、Z轴三个方向的移动实时角速率、实时加速度进行测量，并将测量值传送微处理器23。微处理器23包括微处理单元模块、通信模块、报警模块、数据存储模块、隔离电路和供电模块，微处理单元模块分别与通信模块、报警模块、数据存储模块、隔离电路和供电模块相连接，微处理单元模块采用美国TI公司出品的具有DSP功能的型号为80196的单片机，通过总线从ROM中将应用程序调入上位机综合显示与控制装置5的导航处理器进行数据处理，处理结果再次回送总线，对三轴一体光纤惯性测量装置2进行误差补偿、辅助信号跟踪环路、显示测量信息、传送控制***和外部通信接口。
微处理器23采用惯性导航算法进行解算，计算包括航向角、俯仰角和横滚角的姿态角，将三个加速度计22测量的站心直角坐标系的比力分量转换到导航测量坐标系下，在导航测量坐标系下通过积分求解比力方程，得到地面下采掘移动设备1相对地球的速度，在站心直角坐标系下由地面下采掘移动设备1相对地球的速度积分得到地面下采掘移动设备1的位姿信息，以及采用卡尔曼滤波器对三轴一体光纤惯性测量装置2的测量误差进行校正，三轴一体光纤惯性测量装置2还通过零速校正算法消除***的累积误差。
本具体实施方式的惯性导航定位***还包括上位机综合显示与控制装置5，上位机综合显示与控制装置5包括导航计算机和触摸屏，导航计算机是內嵌数据分析拟合及处理软件的导航计算机，数据分析拟合及处理软件是DSP软件，包括采用C语言在代码设计套件CCS平台上开发的外部通讯接口模块、运行模式分析模块、地面下采掘移动设备状态调校模块、故障自检模块和内部通讯接口模块，外部通讯接口模块、运行模式分析模块、地面下采掘移动设备状态调校模块、故障自检模块和内部通讯接口模块通过API相互调用运行，上位机综合显示与控制装置5的导航计算机通过通讯接口与微处理器23连接，建立数据通讯，根据三轴一体光纤惯性测量装置2的定位结果进行轨迹拟合，结合惯导粗对准后的误差模型建立状态方程，进行滤波融合，经数据融合运算定位，得到地面下采掘移动设备1的运行轨迹，触摸屏通过图形、图表显示包括采掘深度、轨道偏差，以及包括航向角、俯仰角和横滚角的姿态角的位姿信息，还显示地面下采掘移动设备1的水平运行曲线，以及实际运行过程中的偏航角，通过轨迹拟合算法与预定采掘轨迹比较迭代取值，结合地面下采掘移动设备1支架布局迭代计算每个支架的偏差量，同时，对地面下采掘移动设备1实现定位控制，地面下采掘移动设备1依靠自适应功能沿着定位控制的轨道运行和工作。
本具体实施方式的惯性导航定位***还包括用于保证地面下供电的三轴一体光纤惯性测量装置2可靠运行的防爆型USB电源6，
本具体实施方式的惯性导航定位***还设有里程仪3，里程仪3分别通过通讯接口与三轴一体光纤惯性测量装置2、上位机综合显示与控制装置5相连接，并固定安装在地面下采掘移动设备1的驱动***上，测量地面下采掘移动设备1包括移动距离和速度的实时位姿信息，并将实时位姿信息通过通讯接口传送上位机综合显示与控制装置5，里程仪3与三个高精度光学陀螺仪21构成航位推算***，采用里程仪3测量的地面下采掘移动设备1位姿信息，通过惯性导航算法对三轴一体光纤惯性测量装置2的测量误差进行校正，三个高精度光学陀螺仪21、三个加速度计22通过里程仪3提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度，在卫星导航无法探测的地面下，采用里程仪3引入地面下采掘移动设备1的地理坐标信息融合定位，实现地面下采掘移动设备1的精确定位和运动轨迹解算，大幅度提高地面下采掘移动设备的导航测量精度。
本具体实施方式的惯性导航定位***还设有光电摄像头4，光电摄像头4通过固定支架铰接固定在地面下采掘移动设备1一侧的液压支架上，且光电摄像头4的拍摄方向朝着地面下采掘移动设备1运行方向，光电摄像头4对地面下采掘移动设备1所处的工作面环境拍摄图像，拍摄频率为25帧/s，并将已拍摄的图像传送微处理器23，当地面下采掘移动设备1在拍摄环境中移动时，拍摄的目标图像发生变化，根据地面下采掘移动设备1的移动方向确定其移动方向，进行定位。
本具体实施方式的惯性导航定位***还设有按照地面下采掘移动设备1专用防爆盒的规范和标准设计的防爆盒7，防爆盒7外部供电电压：24V DC；外形尺寸：长×宽×高≤400mm×250mm×200mm。三轴一体光纤惯性测量装置2安装在防爆盒7内，防爆盒7通过固定支架安装固定在地面下采掘移动设备1机体上。固定支架是设有圆柱橡胶减震垫的燕尾槽型可调节柔性支撑骨架，圆柱橡胶减震垫对称固定在防爆盒7底部。
本具体实施方式的惯性导航定位方法，包括预步骤：对三轴一体光纤惯性测量装置2预设参量，以及对微处理器23进行误差补偿、误差校正，还包括以下步骤：
1)测量地面下采掘移动设备1包括移动距离、初速度和初始状态的角速率的实时位姿信息，并将实时位姿信息通过通讯接口传送上位机综合显示与控制装置，包括以下分步骤；
1.1)光电摄像头4对移动中的地面下采掘移动设备1所处的环境拍摄实时图像，拍摄频率为25帧/s，并将已拍摄的目标的变化图像传送上位机综合显示与控制装置5的导航计算机；
1.2)对三个高精度光学陀螺仪21和三个加速度计22的导航测量坐标系设置预设参量；
1.3)上位机综合显示与控制装置5读取三轴一体光纤惯性测量装置2的初始参数、地面下采掘移动设备1的初速度和初始状态的角速率，根据其移动方向、初速度和初始状态的角速率，对地面下采掘移动设备1的实际状况进行定位；
2)更新计算地面下采掘移动设备1的移动方向、速度和角速率的变化，包括以下分步骤；
2.1)三个高精度光学陀螺仪21、三个加速度计22分别测量表示地面下采掘移动设备1路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，并将测量值传送微处理器23，通过惯性导航算法解算地面下采掘移动设备1的定位结果；
2.2)将惯性导航算法处理后的地面下采掘移动设备1定位结果通过通讯接口传送微处理器23，作为下一次进行位置解算的初值，同时更新计算地面下采掘移动设备1的移动方向、速度和角速率的变化；
3)将站心直角坐标系转换为导航测量坐标系，通过整合两个不同的坐标系下的参数对地面下采掘移动设备的位姿信息不断进行修正，包括以下分步骤；
3.1)将三个加速度计22测量的站心直角坐标系的比力分量转换到导航测量坐标系，在导航测量坐标系下通过积分求解比力方程，得到地面下采掘移动设备1相对地球的速度，在站心直角坐标系下由地面下采掘移动设备1相对地球的速度积分得到地面下采掘移动设备1的位姿信息；
3.2)采用捷联惯导的比力方程将比力信息投影到惯性坐标系，得到比力相对于惯性空间随地球旋转而引起的方向变化信息，通过整合两个不同的坐标系下的参数对地面下采掘移动设备1的位姿信息不断进行修正；
4)将测量的速度和位置信息进行计算，得到地面下采掘移动设备1的姿态变化及运动轨迹，三个高精度光学陀螺仪21、三个加速度计22通过里程仪3提供的精确位姿信息进行误差补偿，包括以下分步骤；
4.1)将测量的地面下采掘移动设备1站心直角坐标系下的初始位置坐标传送惯性导航***，指导地面下采掘移动设备1按设定路线推进；
4.2)将测量的速度和位置信息进行计算，得到地面下采掘移动设备1的姿态变化及运动轨迹，上位机综合显示与控制装置5显示地面下采掘移动设备1的水平运行曲线；
4.3)三个高精度光学陀螺仪21、三个加速度计22通过里程仪3提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度，包括以下子步骤；
4.3.1)地面下采掘移动设备1停靠在起点时，启动***配置的初始对准参数，在里程仪3参数位置传送标度数和失准角，点击“寻北指令”进行运算，在寻北过程将要结束时，将航向角数值在不超过地球正北向0.01°范围内设定为零值，达到计数后寻北动作完成，得到精确的航向起始角；
4.3.2)启动地面下采掘移动设备1行走机构，地面下采掘移动设备1从起点运行到终点后，得到沿路径测量的表示地面下采掘移动设备1路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度；
4.3.3)根据里程仪3提供的比例因数，传送实际起点与终点的相对位置偏差，以及表示地面下采掘移动设备1路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，计算实际的里程仪标度、俯仰角的补偿角，以及航向角的补偿角；
4.3.4)再次启动地面下采掘移动设备1行走机构，地面下采掘移动设备1从终点运行至起点后，传送子步骤4.3.1)沿路径测量的表示地面下采掘移动设备1路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，再次点击“寻北指令”进行运算，等待5～10秒的时间，测量地面下采掘移动设备1的位姿变化的差值，完成地面下采掘移动设备1初始对准的位置标定过程；
4.3.5)根据地面下采掘移动设备1运动速度矢量，更新计算站心直角坐标系与导航测量坐标系之间的转换矩阵，计算更新地面下采掘移动设备1在站心直角坐标系下的位置坐标；
4.3.6)重复计算步骤，在设定的惯性测量时段内，迭代计算得到每个时刻的地面下采掘移动设备1在站心直角坐标系下的运动速度矢量及位置坐标，三个高精度光学陀螺仪21、三个加速度计22通过里程仪3提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度；
5)引用自身参数进行迭代运算，在地面下采掘移动设备1运动测量中以旧的的参数变化值重复计算多次，对运动变化量和方向进行修正和逼近，如果运动变化量和方向NO，返回步骤1)；如果运动变化量和方向YES，进入步骤6)；
6)采用里程仪3测量的地面下采掘移动设备1位姿信息，通过卡尔曼滤波算法对三轴一体光纤惯性测量装置2的测量误差进行校正，包括以下分步骤；
6.1)采用卡尔曼滤波算法处理三个高精度光学陀螺仪21和三个加速度计22互补滤波的数据，进行误差补偿和修正，地面下采掘移动设备1正常工作，三轴一体光纤惯性测量装置2正常运行，微处理器23对信号强度是否满足需求进行判断；
如果信号强度满足需求，将三个高精度光学陀螺仪21和三个加速度计22测量的地面下采掘移动设备1定位数据传送卡尔曼滤波算法进行优化；
如果信号强度不满足需求，只采用三轴一体光纤惯性测量装置2测量的定位数据作为地面下采掘移动设备1位置信息，通过分析确定当前工作面的直线度，并进行动态调整；
6.2)对惯性导航初始速度的误差进行姿态更新，并补偿对准过程中估计出的三个高精度光学陀螺仪21的漂移，采用里程仪测量的地面下采掘移动设备位姿信息，通过卡尔曼滤波算法对三轴一体光纤惯性测量装置2的测量误差进行校正；
7)在开机运行或出现突发异常情况时，上位机综合显示与控制装置5进行人工干预调整，修正地面下采掘移动设备的航向角，确认定位对准和运行轨迹，以及进行动态调整，包括以下分步骤；
7.1)第一次校准：三轴一体光纤惯性测量装置2进行上电操作，待其内置的三个高精度光学陀螺仪21和三个加速度计22工作稳定后，测量其在地球重力作用下的三轴加速度分量，如果三轴一体光纤惯性测量装置2存在安装偏差角，三轴一体光纤惯性测量装置2先绕自身X轴逆时针旋转角度a，直至Y'轴加速度输出为0，再绕自身Y轴顺时针旋转角度θ，直至X'轴加速度输出也为0；
7.2)第二次校准；如果三轴一体光纤惯性测量装置2还存在安装偏差角，三轴一体光纤惯性测量装置2先绕自身的X轴顺时针旋转90°，此时Y轴指向天，再绕自身Z轴逆时针旋转角度T，直至X'轴加速度输出为0，Z'轴加速度输出也为0；
7.3)在导航测量坐标系与站心直角坐标系合成后，通过导航测量坐标系与站心直角坐标系之间的转换矩阵计算导航测量坐标系与站心直角坐标系之间的相对位置，进行多次修正，以消除三轴一体光纤惯性测量装置2存在的臂杆效应造成的重力加速度g的数值脉冲突变；
8)基于上位机综合显示与控制装置5触摸屏显示的包括采掘深度、轨道偏差，以及包括航向角、俯仰角和横滚角的姿态角的位姿信息确认定位是否对准，如果NO，返回预步骤对微处理器23进行误差补偿、误差校正；如果YES，结束地面下采掘移动设备惯性导航定位。
本具体实施方式采用里程仪测量的地面下采掘移动设备位姿信息通过惯性导航算法对三轴一体光纤惯性测量装置的测量误差进行校正。在不需要大幅度提高成本的前提下实现地面下采掘移动设备定位装置与其惯性导航间安装角进行误差补偿、误差校正、精确定位和自主导航，大幅度提高地面下采掘移动设备的导航测量精度，以及地面下采掘移动设备的工作效率和安全性，可以适应无人操作需要。试用表明，采用里程仪与三轴一体光纤惯性测量装置融合定位，地面下采掘移动设备工作面直线度测量最大误差为90mm，平均误差为60mm，高程测量误差为20mm，符合工作面直线度测量精度和导航精度的要求。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下做出如果干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明由所提交的权利要求书确定的专利保护范围。

Claims (4)

1.一种基于地面下采掘移动设备用惯性导航定位***的定位方法，所述地面下采掘移动设备用惯性导航定位***包括三轴一体光纤惯性测量装置，所述三轴一体光纤惯性测量装置由三个高精度光学陀螺仪、三个加速度计和微处理器组成，三个高精度光学陀螺仪和三个加速度计按照地面下采掘移动设备的站心直角坐标系的X、Y、Z轴精确组合，固定连接在地面下采掘移动设备上，所述三轴一体光纤惯性测量装置在进入测量工作状态之前进行初始对准，将地面下采掘移动设备的站心直角坐标系的位姿信息变换到惯性导航定位***的导航测量坐标系，确定站心直角坐标系与导航测量坐标系合成，所述三个高精度光学陀螺仪和三个加速度计是惯性敏感器件，分别对地面下采掘移动设备的站心直角坐标系的X、Y、Z轴三个方向的移动实时角速率、实时加速度进行测量，并将测量值传送所述微处理器，所述微处理器采用惯性导航算法进行解算，计算包括航向角、俯仰角和横滚角的姿态角，将三个加速度计测量的站心直角坐标系的比力分量转换到导航测量坐标系下，在导航测量坐标系下通过积分求解比力方程，得到地面下采掘移动设备相对地球的速度，在站心直角坐标系下由地面下采掘移动设备相对地球的速度积分得到地面下采掘移动设备的位姿信息，以及采用卡尔曼滤波器对所述三轴一体光纤惯性测量装置的测量误差进行校正，所述三轴一体光纤惯性测量装置还通过零速校正算法消除***的累积误差；
还包括上位机综合显示与控制装置，所述上位机综合显示与控制装置包括导航计算机和触摸屏，导航计算机通过通讯接口与所述微处理器连接，建立数据通讯，根据三轴一体光纤惯性测量装置的定位结果进行轨迹拟合，结合惯导粗对准后的误差模型建立状态方程，进行滤波融合，经数据融合运算定位，得到地面下采掘移动设备的运行轨迹，触摸屏通过图形、图表显示包括采掘深度、轨道偏差，以及包括航向角、俯仰角和横滚角的姿态角的位姿信息，还显示地面下采掘移动设备的水平运行曲线，以及实际运行过程中的偏航角，通过轨迹拟合算法与预定采掘轨迹比较迭代取值，结合地面下采掘移动设备支架布局迭代计算每个支架的偏差量，同时，对地面下采掘移动设备实现定位控制，地面下采掘移动设备依靠自适应功能沿着定位控制的轨道运行和工作；
还包括电源；
还设有里程仪，所述里程仪分别通过通讯接口与所述三轴一体光纤惯性测量装置、所述上位机综合显示与控制装置相连接，并固定安装在地面下采掘移动设备的驱动***上，测量地面下采掘移动设备包括移动距离和速度的实时位姿信息，并将实时位姿信息通过通讯接口传送所述上位机综合显示与控制装置，里程仪与三个高精度光学陀螺仪构成航位推算***，采用里程仪测量的地面下采掘移动设备位姿信息，通过惯性导航算法对三轴一体光纤惯性测量装置的测量误差进行校正，所述三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度，在卫星导航无法探测的地面下，采用里程仪引入地面下采掘移动设备的地理坐标信息融合定位，实现地面下采掘移动设备的精确定位和运动轨迹解算，大幅度提高地面下采掘移动设备的导航测量精度；
还设有光电摄像头，所述光电摄像头通过固定支架铰接固定在地面下采掘移动设备一侧的液压支架上，且光电摄像头的拍摄方向朝着地面下采掘移动设备运行方向，光电摄像头对地面下采掘移动设备所处的工作面环境拍摄图像，拍摄频率为25帧/s，并将已拍摄的图像传送所述微处理器，当地面下采掘移动设备在拍摄环境中移动时，拍摄的目标图像发生变化，根据地面下采掘移动设备的移动方向确定其移动方向，进行定位;
还设有防爆盒，所述三轴一体光纤惯性测量装置安装在防爆盒内，所述防爆盒通过固定支架安装固定在地面下采掘移动设备机体上；
所述电源是防爆型USB电源；
所述微处理器包括微处理单元模块、通信模块、报警模块、数据存储模块、隔离电路和供电模块，所述微处理单元模块分别与通信模块、报警模块、数据存储模块、隔离电路和供电模块相连接；
所述微处理单元模块采用美国德州仪器TI公司出品的具有数字信号处理器DSP功能的单片机；
所述三个高精度光学陀螺仪是随机漂移稳定性为0.01°/h的惯性级三个光学陀螺仪；
所述三个加速度计是刻度因数稳定性为10-4mv/g数量级的三个加速度计；
所述防爆盒，是按照地面下采掘移动设备专用防爆盒的规范和标准设计的防爆盒，所述防爆盒外部供电电压：24V DC；外形尺寸：长×宽×高≤400mm×250mm×200mm；
所述固定支架是设有圆柱橡胶减震垫的燕尾槽型可调节柔性支撑骨架，所述圆柱橡胶减震垫对称固定在所述防爆盒底部；
其特征在于：
定位方法包括预步骤：对三轴一体光纤惯性测量装置预设参量，以及对微处理器进行误差补偿、误差校正，还包括以下步骤：
1）测量地面下采掘移动设备包括移动距离、初速度和初始状态的角速率的实时位姿信息，并将实时位姿信息通过通讯接口传送上位机综合显示与控制装置；
2）更新计算地面下采掘移动设备的移动方向、速度和角速率的变化；
3）将站心直角坐标系转换为导航测量坐标系，通过整合两个不同的坐标系下的参数对地面下采掘移动设备的位姿信息不断进行修正和补偿，包括以下步骤：
3.1）将三个加速度计测量的站心直角坐标系的比力分量转换到导航测量坐标系，在导航测量坐标系下通过积分求解比力方程，得到地面下采掘移动设备相对地球的速度，在站心直角坐标系下由地面下采掘移动设备相对地球的速度积分得到地面下采掘移动设备的位姿信息；
3.2）采用捷联惯导的比力方程将比力信息投影到惯性坐标系，得到比力相对于惯性空间随地球旋转而引起的方向变化信息，通过整合两个不同的坐标系下的参数对地面下采掘移动设备的位姿信息不断进行修正和补偿；
4）将测量的速度和位置信息进行计算，得到地面下采掘移动设备的姿态变化及运动轨迹，三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿；
5）引用自身参数进行迭代运算，在地面下采掘移动设备运动测量中以旧的的参数变化值重复计算多次，对运动变化量和方向进行修正和逼近，如果运动变化量和方向NO，返回步骤1）；如果运动变化量和方向YES，进入步骤6）；
6）采用里程仪测量的地面下采掘移动设备位姿信息，通过卡尔曼滤波算法对三轴一体光纤惯性测量装置的测量误差进行校正；
7）在开机运行或出现突发异常情况时，上位机综合显示与控制装置进行人工干预调整，修正地面下采掘移动设备的航向角，确认定位对准和运行轨迹，以及进行动态调整；
8）基于上位机综合显示与控制装置触摸屏显示的包括采掘深度、轨道偏差，以及包括航向角、俯仰角和横滚角的姿态角的位姿信息确认定位是否对准，如果NO，返回预步骤对微处理器进行误差补偿、误差校正；如果YES，结束地面下采掘移动设备惯性导航定位。
2.如权利要求1所述基于地面下采掘移动设备用惯性导航定位***的定位方法，其特征在于：
所述步骤1）包括以下分步骤；
1.1）光电摄像头对移动中的地面下采掘移动设备所处的环境拍摄实时图像，拍摄频率为25帧/s，并将已拍摄的目标的变化图像传送上位机综合显示与控制装置的导航计算机；
1.2）对三个高精度光学陀螺仪和三个加速度计的导航测量坐标系设置预设参量；
1.3）上位机综合显示与控制装置读取三轴一体光纤惯性测量装置的初始参数、地面下采掘移动设备的初速度和初始状态的角速率，根据其移动方向、初速度和初始状态的角速率，对地面下采掘移动设备的实际状况进行定位；
所述步骤2）包括以下分步骤；
2.1）三个高精度光学陀螺仪、三个加速度计分别测量表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，并将测量值传送微处理器，通过惯性导航算法解算地面下采掘移动设备的定位结果；
2.2）将惯性导航算法处理后的地面下采掘移动设备定位结果通过通讯接口传送微处理器，作为下一次进行位置解算的初值，同时更新计算地面下采掘移动设备的移动方向、速度和角速率的变化。
3.如权利要求1所述基于地面下采掘移动设备惯性导航定位***的定位方法，其特征在于：
所述步骤4）包括以下分步骤；
4.1）将测量的地面下采掘移动设备站心直角坐标系下的初始位置坐标传送惯性导航***，指导地面下采掘移动设备按设定路线推进；
4.2）将测量的速度和位置信息进行计算，得到地面下采掘移动设备的姿态变化及运动轨迹，上位机综合显示与控制装置显示地面下采掘移动设备的水平运行曲线；
4.3）三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度；
所述步骤4.3）包括以下子步骤；
4.3.1）地面下采掘移动设备停靠在起点时，启动***配置的初始对准参数，在里程仪参数位置传送标度数和失准角，点击“寻北指令”进行运算，在寻北过程将要结束时，将航向角数值在不超过地球正北向0.01°范围内设定为零值，达到计数后寻北动作完成，得到精确的航向起始角；
4.3.2）启动地面下采掘移动设备行走机构，地面下采掘移动设备从起点运行到终点后，得到沿路径测量的表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度；
4.3.3）根据里程仪提供的比例因数，传送实际起点与终点的相对位置偏差，以及表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，计算实际的里程仪标度、俯仰角的补偿角，以及航向角的补偿角；
4.3.4）再次启动地面下采掘移动设备行走机构，地面下采掘移动设备从终点运行至起点后，传送子步骤4.3.1）沿路径测量的表示地面下采掘移动设备路径轨迹的站心直角坐标系下的X、Y、Z轴三个方向的移动实时角速率、实时加速度，再次点击“寻北指令”进行运算，等待5~10秒的时间，测量地面下采掘移动设备的位姿变化的差值，完成地面下采掘移动设备初始对准的位置标定过程；
4.3.5）根据地面下采掘移动设备运动速度矢量，更新计算站心直角坐标系与导航测量坐标系之间的转换矩阵，计算更新地面下采掘移动设备在站心直角坐标系下的位置坐标；
4.3.6）重复计算步骤，在设定的惯性测量时段内，迭代计算得到每个时刻的地面下采掘移动设备在站心直角坐标系下的运动速度矢量及位置坐标，三个高精度光学陀螺仪、三个加速度计通过里程仪提供的精确位姿信息进行误差补偿，以提高姿态导航解算的精度。
4.如权利要求1所述基于地面下采掘移动设备惯性导航定位***的定位方法，其特征在于：
所述步骤6）包括以下分步骤；
6.1）采用卡尔曼滤波算法处理三个高精度光学陀螺仪和三个加速度计互补滤波的数据，进行误差补偿和修正，地面下采掘移动设备正常工作，三轴一体光纤惯性测量装置正常运行，微处理器对信号强度是否满足需求进行判断；
如果信号强度满足需求，将三个高精度光学陀螺仪和三个加速度计测量的地面下采掘移动设备定位数据传送卡尔曼滤波算法进行优化；
如果信号强度不满足需求，只采用三轴一体光纤惯性测量装置测量的定位数据作为地面下采掘移动设备位置信息，通过分析确定当前工作面的直线度，并进行动态调整；
6.2）对惯性导航初始速度的误差进行姿态更新，并补偿对准过程中估计出的三个高精度光学陀螺仪的漂移，采用里程仪测量的地面下采掘移动设备位姿信息，通过卡尔曼滤波算法对三轴一体光纤惯性测量装置的测量误差进行校正；
所述步骤7）包括以下分步骤；
7. 1）第一次校准：三轴一体光纤惯性测量装置进行上电操作，待其内置的三个高精度光学陀螺仪和三个加速度计工作稳定后，测量其在地球重力作用下的三轴加速度分量，如果三轴一体光纤惯性测量装置存在安装偏差角，三轴一体光纤惯性测量装置先绕自身X轴逆时针旋转角度a，直至Y'轴加速度输出为0，再绕自身Y轴顺时针旋转角度θ，直至X'轴加速度输出也为0；
7. 2）第二次校准；如果三轴一体光纤惯性测量装置还存在安装偏差角，三轴一体光纤惯性测量装置先绕自身的X轴顺时针旋转90°，此时Y轴指向天，再绕自身Z轴逆时针旋转角度T，直至X'轴加速度输出为0，Z'轴加速度输出也为0；
7. 3）在导航测量坐标系与站心直角坐标系合成后，通过导航测量坐标系与站心直角坐标系之间的转换矩阵计算导航测量坐标系与站心直角坐标系之间的相对位置，进行多次修正，以消除三轴一体光纤惯性测量装置存在的臂杆效应造成的重力加速度g的数值脉冲突变。
CN201910777601.6A 2019-08-22 2019-08-22 一种地面下采掘移动设备用惯性导航定位***与定位方法 Active CN110411444B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910777601.6A CN110411444B (zh) 2019-08-22 2019-08-22 一种地面下采掘移动设备用惯性导航定位***与定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910777601.6A CN110411444B (zh) 2019-08-22 2019-08-22 一种地面下采掘移动设备用惯性导航定位***与定位方法

Publications (2)

Publication Number Publication Date
CN110411444A CN110411444A (zh) 2019-11-05
CN110411444B true CN110411444B (zh) 2024-01-09

Family

ID=68368472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910777601.6A Active CN110411444B (zh) 2019-08-22 2019-08-22 一种地面下采掘移动设备用惯性导航定位***与定位方法

Country Status (1)

Country Link
CN (1) CN110411444B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110689505B (zh) * 2019-12-11 2020-07-17 长沙天仪空间科技研究院有限公司 一种基于场景的星载遥感仪器自适应校正方法和***
CN111121771B (zh) * 2019-12-31 2021-08-20 北京龙软科技股份有限公司 一种基于惯性导航器件和编码器的定位方法及装置
CN113490830B (zh) * 2020-01-31 2023-05-05 日本精工株式会社 旋转角度计算装置及其校正方法和校正装置、马达控制装置、电动致动器产品和电动助力转向装置
CN111307133B (zh) * 2020-03-04 2022-03-22 中国铁建重工集团股份有限公司 一种竖井垂直度测量方法、竖井姿态检测设备及存储介质
CN111878056B (zh) * 2020-05-11 2021-04-13 中国科学院地质与地球物理研究所 一种陀螺随钻测量***及方法
CN112797978B (zh) * 2021-04-14 2021-08-27 上海力信测量***有限公司 一种掘进机的导向方法、***和存储介质
CN113203428A (zh) * 2021-05-28 2021-08-03 拉扎斯网络科技（上海）有限公司 里程统计装置、基于里程统计装置的数据统计方法及接口
CN113670297A (zh) * 2021-08-23 2021-11-19 上海宇航***工程研究所 一种基于mems和轮式里程计的离线定位方法
CN113932808B (zh) * 2021-11-02 2024-04-02 湖南格兰博智能科技有限责任公司 一种适用于视觉导航扫地机器人视觉和陀螺仪融合校正的算法
CN114869604B (zh) * 2022-05-07 2023-07-11 江苏理工学院 物联网轮椅智能导航***
CN115574815B (zh) * 2022-11-18 2023-05-23 西安玉衡导航科技有限公司 一种非可视环境导航***、方法、计算机设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航***及其导航定位方法
WO2013059989A1 (zh) * 2011-10-25 2013-05-02 国防科学技术大学 一种惯性导航***的运动对准方法
CN104061930A (zh) * 2013-12-05 2014-09-24 东南大学 基于捷联惯性制导和多普勒计程仪的导航方法
WO2017008454A1 (zh) * 2015-07-10 2017-01-19 中国科学院深圳先进技术研究院 一种机器人的定位方法
CN109540130A (zh) * 2018-10-25 2019-03-29 北京航空航天大学 一种连采机惯性导航定位定向方法
CN210268669U (zh) * 2019-08-22 2020-04-07 深圳赛奥航空科技有限公司 一种地面下采掘移动设备用惯性导航定位***

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6697736B2 (en) * 2002-02-06 2004-02-24 American Gnc Corporation Positioning and navigation method and system thereof

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航***及其导航定位方法
WO2013059989A1 (zh) * 2011-10-25 2013-05-02 国防科学技术大学 一种惯性导航***的运动对准方法
CN104061930A (zh) * 2013-12-05 2014-09-24 东南大学 基于捷联惯性制导和多普勒计程仪的导航方法
WO2017008454A1 (zh) * 2015-07-10 2017-01-19 中国科学院深圳先进技术研究院 一种机器人的定位方法
CN109540130A (zh) * 2018-10-25 2019-03-29 北京航空航天大学 一种连采机惯性导航定位定向方法
CN210268669U (zh) * 2019-08-22 2020-04-07 深圳赛奥航空科技有限公司 一种地面下采掘移动设备用惯性导航定位***

Also Published As

Publication number Publication date
CN110411444A (zh) 2019-11-05

Similar Documents

Publication Publication Date Title
CN110411444B (zh) 一种地面下采掘移动设备用惯性导航定位***与定位方法
EP2841874B1 (en) Estimation of the relative attitude and position between a vehicle body and an implement operably coupled to the vehicle body
CN103335647B (zh) 一种盾构机姿态测量***及其测量方法
CN103674034B (zh) 多波束测速测距修正的鲁棒导航方法
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN112014849B (zh) 一种基于传感器信息融合的无人车定位修正方法
CN111637888B (zh) 基于惯导和激光雷达单点测距的掘进机定位方法及***
CN111307133B (zh) 一种竖井垂直度测量方法、竖井姿态检测设备及存储介质
CN112476433B (zh) 一种基于识别阵列边界的移动机器人定位方法
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定***及应用方法
EP3786757B1 (en) Camera stabilizer position correction method and device
CN111207743B (zh) 基于编码器与惯性设备紧耦合实现厘米级精确定位的方法
CN103712622A (zh) 基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置
CN110095135B (zh) 一种用于掘进机定位定向的方法及装置
CN210268669U (zh) 一种地面下采掘移动设备用惯性导航定位***
CN111207742B (zh) 一种附加外方位元素约束的采煤机定位定姿方法
CN110864692A (zh) 掘进机的位姿确定方法
CN115597535A (zh) 基于惯性导航的高速磁悬浮轨道不平顺检测***及方法
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能***姿估计方法
CN113236363A (zh) 开采设备导航定位方法、***、设备及可读存储介质
CN211012986U (zh) 一种基于惯导技术的无人自主巡航车导航***
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法
EP1206683B1 (en) Integrated inertial/vms navigation system
CN111637889A (zh) 基于惯导和激光雷达三点测距的掘进机定位方法及***
CN117128951B (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
GR01 Patent grant
GR01 Patent grant