CN115574815A - 一种非可视环境导航系统、方法、计算机设备及存储介质 - Google Patents

一种非可视环境导航系统、方法、计算机设备及存储介质 Download PDF

Info

Publication number
CN115574815A
CN115574815A CN202211445347.8A CN202211445347A CN115574815A CN 115574815 A CN115574815 A CN 115574815A CN 202211445347 A CN202211445347 A CN 202211445347A CN 115574815 A CN115574815 A CN 115574815A
Authority
CN
China
Prior art keywords
navigation
attitude
visual environment
information
current
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.)
Granted
Application number
CN202211445347.8A
Other languages
English (en)
Other versions
CN115574815B (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.)
Xi'an Yuheng Navigation Technology Co ltd
Original Assignee
Xi'an Yuheng Navigation 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 Xi'an Yuheng Navigation Technology Co ltd filed Critical Xi'an Yuheng Navigation Technology Co ltd
Priority to CN202211445347.8A priority Critical patent/CN115574815B/zh
Publication of CN115574815A publication Critical patent/CN115574815A/zh
Application granted granted Critical
Publication of CN115574815B publication Critical patent/CN115574815B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

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

Abstract

本发明属于定位导航设备技术领域，公开了一种非可视环境导航系统、方法、计算机设备及存储介质，包括IMU惯性测量单元根据角速度传感器、加速度传感器测量的数据，计算设备当前坐标体系下的位置姿态；并根据测距装置提供的距离信息解算基本导航位置发送至上位机；距离测量装置利用激光测距传感测量每根钻杆的当前推进长度，并将数据传送至IMU惯性测量单元和上位机；上位机利用捷联导航算法、导航算法输出当前位置参数以及其他导航信息。本发明针对工程作业过程中无法采用光学方式进行观测的特殊环境设计，无需借助其他辅助设备或辅助工序；为地层、山体、井下钻探或钻进工程提供钻头位置定位和导航。

Description

一种非可视环境导航系统、方法、计算机设备及存储介质
技术领域
本发明属于定位导航设备技术领域，尤其涉及一种非可视环境导航系统、方法、计算机设备及存储介质。
背景技术
目前，导航系统是地球物理勘探（GS，Geophysical survey）和水平定向钻（HDD，Horizontal Directional Drilling）等领域中不可缺少的核心系统，肩负着在地层或山体中为采集设备定位及导航的任务。此类导航设备的工作流程是：通过地下传感器对钻进过程中钻具姿态实时测量，利用测量数据计算出钻进轨迹，司钻人员依据钻进轨迹判断钻进是否按照预设钻孔轨迹进行，并通过钻机等地面设备实现对钻具的方向控制，以保证钻进轨迹按照预先设计的轨迹进行钻进。
目前，同类设备主要采用步行式定位方式（Walk-over locating system）或磁力导航系统（magnetic guidance system）。步行定位方式由地下探棒和地面接收机组成，探棒一般采用声纳或其他发射传感器，记录设备的方位角及温度等数据。探棒将这些信息通过无线电信号传输至地面，地面接收设备接收到无线信号后，解码显示至主界面。磁力导航系统又被称为有线式导航系统，它利用磁力传感器进行姿态和位置测量，使用时需要在地面铺设人工磁场作为定位验证的辅助手段。收集定位信息后，由地面上位机接收并计算出路径信息。
现有同类设备导航设备多采用步行式定位方式（Walk-over locating system）或MGS磁力导航系统（magnetic guidance system），其主要缺点包括：
（1）探测深度受限。步行或地磁定位需要采集地下或山体中的探棒信号，以确定设备位置。磁棒信号穿透力有限，无复杂地质条件下作用范围≤20m，复杂条件下可能减半或更低；
（2）易受外部环境影响。如作业环境存在天然强磁或其他复杂环境，将导致采集设备无法接收到探棒信号，从而影响定位精度，甚至完全无法定位；
（3）不支持自主导航。地磁定位设备需要通过额外增设的采集器手动收集位置信息，不能实现定位设备独立自主进行定位导航；
（4）存在不可测量环境。因设备工作中需要不断使用采集器寻找探棒，因此无法在河流、高山等工程人员移动困难的环境使用需其他辅助设备。在磁场环境较弱的地方，需要增设人工磁场以提高定位精度。
通过上述分析，现有技术存在的问题及缺陷为：现有的非可视导航方法探测深度受限、易受外部环境影响、不支持自主导航、存在不可测量环境，导航精度不佳。
发明内容
针对现有技术存在的问题，本发明提供了一种非可视环境导航系统、方法、计算机设备及存储介质。
本发明是这样实现的，一种非可视环境导航系统，所述非可视环境导航系统包括：
IMU惯性测量单元、距离测量装置以及上位机；
IMU惯性测量单元，包含角速度传感器、加速度传感器以及解算板；用于根据角速度传感器、加速度传感器测量的数据，计算设备当前坐标体系下的位置姿态；并根据测距装置提供的距离信息解算基本导航位置发送至上位机；
距离测量装置，用于利用激光测距传感测量每根钻杆的当前推进长度，并将数据传送至IMU惯性测量单元和上位机；
上位机，由主板、中央处理器CPU、图形处理器GPU及固态存储器SSD组成；用于接收IMU惯性测量单元发送的基本导航位置和姿态位置信息，接收距离测量装置发送的当前长度信息，利用捷联导航算法、导航算法输出当前位置参数以及其他导航信息。
进一步，所述角速度传感器为三个光纤采集光纤陀螺；所述加速度传感器为三个石英绕性加速度计；
所述解算板采用FPGA+DPS结构；
所述FPGA用于采集光纤陀螺及石英绕性加速度计测量数据、计程仪数据、采集光纤陀螺、石英绕性加速度计和温度信息以及显控指令发送给DSP；同时用于CAN口管理、外设寻址以及读写控制；
所述DSP用于进行导航解算以及网口管理；
所述解算板还设置由ARM芯片；所述ARM芯片由DSP、FPGA、ARM、通讯接口，CAN通信接口、以太网通信接口、及采集光纤陀螺和石英绕性加速度计或其他线路部分组成。
进一步，所述位置姿态包括航向角、方位角和滚转角。
进一步，所述非可视环境导航系统还包括：
交互模块，用于通过键盘及鼠标与上位机进行交互，获取当前导航位置的2D横、纵切面图，3D立体建模图、设备运行参数或其他信息。
本发明的另一目的在于提供一种应用于所述非可视环境导航系统的非可视环境导航方法，所述非可视环境导航方法包括：
步骤一，利用角速度传感器、加速度传感器进行角速度、加速度测量；根据角速度传感器、加速度传感器测量的数据，计算设备当前坐标体系下的位置姿态；
步骤二，利用激光测距传感测量每根钻杆的当前推进长度；根据每根钻杆的当前推进长度解算基本导航位置；
步骤三，基于设备当前坐标体系下的位置姿态、基本导航位置以及每根钻杆的当前推进长度，通过内置时钟计算时间节点，利用捷联导航算法、导航算法输出当前位置参数或其他导航信息。
进一步，所述计算设备当前坐标体系下的位置姿态包括：
（1）进行粗对准对准：采用解析的方法计算载体的初始姿态角：
Figure DEST_PATH_IMAGE001
，
Figure 500634DEST_PATH_IMAGE002
；
其中，
Figure DEST_PATH_IMAGE003
表示初始时刻载体惯性系；
Figure 83056DEST_PATH_IMAGE004
表示初始时刻导航惯性系；
Figure DEST_PATH_IMAGE005
表示
Figure 541981DEST_PATH_IMAGE003
系与
Figure 27189DEST_PATH_IMAGE006
系的方位关系；
Figure DEST_PATH_IMAGE007
表示重力矢量在
Figure 368303DEST_PATH_IMAGE004
系的投影；
Figure 426520DEST_PATH_IMAGE008
表示石英绕性加速度计的比力输出在
Figure 910591DEST_PATH_IMAGE003
系投影；
（2）进行精对准：
首先，建立惯导系统系统误差方程如下：
Figure DEST_PATH_IMAGE009
，
Figure 396061DEST_PATH_IMAGE010
；
Figure 863952DEST_PATH_IMAGE011
,
Figure DEST_PATH_IMAGE012
,
Figure 460280DEST_PATH_IMAGE013
；
Figure DEST_PATH_IMAGE014
,
Figure 690536DEST_PATH_IMAGE015
；
其中，
Figure DEST_PATH_IMAGE016
；
Figure 827468DEST_PATH_IMAGE017
表示等效采集光纤陀螺噪声，
Figure DEST_PATH_IMAGE018
表示等效石英绕性加速度计噪声；
Figure 844971DEST_PATH_IMAGE019
和
Figure DEST_PATH_IMAGE020
分别表示等效东向和北向速度量测噪声；
Figure 776149DEST_PATH_IMAGE021
表示静基座下噪声分配阵
Figure 438075DEST_PATH_IMAGE021
；
其次，采用Kalman滤波方法进行估计，得到粗对准失准角的最优估计。
进一步，所述利用捷联导航算法、导航算法输出当前位置参数或其他导航信息包括：
对3个正交安装的采集光纤陀螺角速度信息进行姿态解算模拟平台惯导系统的平台坐标系；将3个方向上的石英绕性加速度计比力信息变换到导航坐标系的比力信息，利用积分技术在导航坐标系中计算船体的速度和位置；
以地理坐标系为导航坐标系，固连在船体上的石英绕性加速度计和采集光纤陀螺敏感到的比力信号
Figure DEST_PATH_IMAGE022
和角速度信号
Figure 265347DEST_PATH_IMAGE023
经过各自的安装误差阵变换后得到各自在b系上的投影
Figure DEST_PATH_IMAGE024
和
Figure 865087DEST_PATH_IMAGE025
；经过导航解算和姿态更新解算得到相应的位置、速度、姿态以及角速度或其他导航参数。
进一步，所述姿态更新采用旋转矢量法，角速度信息通过实时更新的姿态矩阵，将采集光纤陀螺输出的载体坐标系下的角速度信息转换到导航坐标系下，同时去除地球自转以及载体线运动引起的姿态角运动。
本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行所述非可视环境导航方法的步骤。
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行所述非可视环境导航方法的步骤。
结合上述的技术方案和解决的技术问题，本发明所要保护的技术方案所具备的优点及积极效果为：
本发明采用的惯性导航设计不同于声纳和磁力导航，采用三个光纤采集光纤陀螺及三个石英绕性加速度计结合初始对准算法及捷联解算算法实现了非可视环境导航。
本发明针对工程作业过程中无法采用光学方式进行观测的特殊环境设计，无需借助其他辅助设备或辅助工序，即可为作业过程提供自主定位及导航服务。其主要功能是为地层、山体、井下钻探或钻进工程提供钻头位置定位和导航。
本发明的技术方案填补了国内外业内技术空白：目前，同类设备主要采用步行式定位方式（Walk-over locating system）或磁力导航系统（magnetic guidance system）。国外已经出现部分采用惯性导航原理的采集光纤陀螺定位系统（Gyro-based locatingsystem），其传感器仅采用了IMU惯性组合的部分传感器（采集光纤陀螺），不具备自主导航路线计算能力；国内能够生产步行式定位方式的导航产品，在惯性导航原理领域尚属空白。本发明突破性采用完整的惯性导航结构，能够实现自主导航定位，在国外同类产品领域实现了升级，在国内同类产品领域尚属首次。
附图说明
图1是本发明实施例提供的非可视环境导航方法流程图；
图2是本发明实施例提供的非可视环境导航系统结构示意图；
图3是本发明实施例提供的电路设计流程图；
图4是本发明实施例提供的惯性空间中观察重力矢量形成锥面示意图；
图5是本发明实施例提供的捷联导航算法解算原理图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
为了使本领域技术人员充分了解本发明如何具体实现，该部分是对权利要求技术方案进行展开说明的解释说明实施例。
如图1所示，本发明实施例提供的非可视环境导航方法包括：
S101，利用角速度传感器、加速度传感器进行角速度、加速度测量；根据角速度传感器、加速度传感器测量的数据，计算设备当前坐标体系下的位置姿态；
S102，利用激光测距传感测量每根钻杆的当前推进长度；根据每根钻杆的当前推进长度解算基本导航位置；
S103，基于所述设备当前坐标体系下的位置姿态、基本导航位置以及每根钻杆的当前推进长度，通过内置时钟计算时间节点，利用捷联导航算法、导航算法输出当前位置参数或其他导航信息。
如图2所示，本发明实施例提供的非可视环境导航系统包括：
非可视环境导航系统硬件结构采用SINS/OD(Strapdown Inertial NavigationSystem/Odometer)组合定位定向导航方案，主要由IMU惯性测量单元（InertialMeasurement Unit）、距离测量装置、上位计算机三部分组成。
IMU惯性测量单元内含角速度与加速度传感器及导航解算计算机，角速度传感器为三个光纤采集光纤陀螺，加速度传感器为三个石英绕性加速度计，解算板采用FPGA+DPS结构设计。IMU惯性测量单元的主要作用是根据角速度与加速度传感器测量的数据，计算出设备当前坐标体系下的位置姿态，包括航向角、方位角和滚转角，并根据测距装置提供的距离信息结算基本导航位置发送至上位机。
距离测量装置采用激光测距传感器，其作用主要是测量每根钻杆的当前推进长度，并将数据传送至IMU惯性测量单元和上位计算机。
上位计算机为加固计算机，主要包括主板、中央处理器CPU（central processingunit）、图形处理器GPU（graphics processing unit）及固态存储器SSD（Solid StateDrive）组成，用户操作系统及导航算法软件安装于固态存储器上。上位机的主要作用是接收惯性测量的基本导航位置和姿态位置信息，接收距离测量装置发送的当前长度信息，通过内置时钟计算时间节点后，将基本导航位置信息与长度信息输入内置的捷联导航算法，经过导航算法结算后，输出当前位置参数等导航信息。此外，用户可以通过键盘及鼠标与上位机软件进行交互，查看当前导航位置的2D横、纵切面图，3D立体建模图、设备运行参数等主要信息。
本发明实施例提供的非可视环境导航系统详细设计方案如下：
1．电路设计
IMU惯性测量单元解算架构采用DSP+FPGA组合，DSP频率浮点≥1000MHz，FPGA门数≥40万。FPGA负责采集光纤陀螺及石英绕性加速度计测量数据，DSP负责进行导航解算。此外，增加一枚ARM芯片，即主要由DSP、FPGA、ARM、通讯接口，CAN通信接口、以太网通信接口、及采集光纤陀螺和石英绕性加速度计等线路部分组成。DSP主要用来进行导航算法解算，并实现网口管理，FPGA用来采集光纤陀螺和加表数据、计程仪数据、采集光纤陀螺、石英绕性加速度计和温度信息以及显控指令发送给DSP，并负责CAN口管理，FPGA在线路板上还负责外设寻址，读写控制等功能。整体逻辑图如图3所示。
2．算法设计
算法设计主要包括两个部分：算法设计主要包括两个部分：初始对准算法及捷联解算算法。
2.1 初始对准算法
初始对准包括粗对准和精对准。粗对准一般用解析的方法计算载体的初始姿态角，精对准在粗对准的基础上，进一步估计姿态角误差。
(1) 粗对准
定义两个重要的惯性坐标系。①初始时刻载体惯性系(
Figure 645961DEST_PATH_IMAGE003
)：与初始对准开始瞬时的载体坐标系(
Figure DEST_PATH_IMAGE026
系)重合，随后相对于惯性空间无转动；②初始时刻导航惯性系(
Figure 319650DEST_PATH_IMAGE004
)：与初始对准开始瞬时的导航坐标系（
Figure 957305DEST_PATH_IMAGE027
系，即地理坐标系）重合，随后相对于惯性空间无转动。间接初始对准方法的关键是求解
Figure 982024DEST_PATH_IMAGE003
系与
Figure 566589DEST_PATH_IMAGE004
系的方位关系，即
Figure 563626DEST_PATH_IMAGE005
。
从惯性坐标系中观察地球表面上某固定点的重力矢量，它的方向将随着地球自转逐渐改变，24小时内恰好旋转一圈，形成一个锥面，如图4所示。重力矢量的方向为地球方向，而其变化率（微分）方向为东西方向，因而在重力矢量及其变化中包含了地理坐标系（水平及方位）信息。
首先，重力矢量在
Figure 637761DEST_PATH_IMAGE004
系的投影为：
Figure DEST_PATH_IMAGE028
(1)
其中，
Figure 764156DEST_PATH_IMAGE029
为常矢量，即
Figure DEST_PATH_IMAGE030
，而：
Figure 480309DEST_PATH_IMAGE031
(2)
由于
Figure DEST_PATH_IMAGE032
为常值，即
Figure 190907DEST_PATH_IMAGE027
系相对于
Figure 170364DEST_PATH_IMAGE004
系为定轴转动，由上式可解得：
Figure 248304DEST_PATH_IMAGE033
(3)
所以有：
Figure DEST_PATH_IMAGE034
(4)
其次，石英绕性加速度计的比力输出在
Figure 128666DEST_PATH_IMAGE003
系投影为：
Figure 598831DEST_PATH_IMAGE035
(5)
其中：
Figure DEST_PATH_IMAGE036
(6)
Figure 109709DEST_PATH_IMAGE037
为采集光纤陀螺的测量值，姿态阵初始值
Figure DEST_PATH_IMAGE038
，利用姿态更新算法可求得实时姿态阵
Figure 533999DEST_PATH_IMAGE039
。这里无需对
Figure DEST_PATH_IMAGE040
的大小做任何限制，因而间接对准算法具有很强的抗角运动干扰能力。
最后，通过
Figure 185429DEST_PATH_IMAGE005
建立重力与石英绕性加速度计比力量测之间关系。将
Figure 886931DEST_PATH_IMAGE041
的两边同时左乘
Figure 67246DEST_PATH_IMAGE040
，得：
Figure DEST_PATH_IMAGE042
(7)
即：
Figure 447674DEST_PATH_IMAGE043
(8)
其中，
Figure DEST_PATH_IMAGE044
表示在
Figure 947137DEST_PATH_IMAGE003
系的石英绕性加速度计测量误差及线加速度干扰。理论上，只要获得两个时刻的重力及其比力测量值，就可使用双矢量定姿算法求解
Figure 532839DEST_PATH_IMAGE045
。但是，为了降低线运动干扰的影响，在初始对准过程中对式(4)积分，记：
Figure DEST_PATH_IMAGE046
(9)
其中，
Figure 119940DEST_PATH_IMAGE047
且通常取
Figure DEST_PATH_IMAGE048
。根据双矢量定姿算法，忽略干扰
Figure 814096DEST_PATH_IMAGE049
的影响，可求得：
Figure DEST_PATH_IMAGE050
(10)
(2) 精对准
精对准是在粗对准基础上，建立惯导系统系统误差方程，并利用卡尔曼滤波对姿态误差进行进一步估计。
只考虑姿态误差、东向及北行速度误差，以及北向及天向采集光纤陀螺误差，建立7维随机系统模型如下：
Figure 43214DEST_PATH_IMAGE051
(11)
其中：
Figure DEST_PATH_IMAGE052
；
Figure 906259DEST_PATH_IMAGE053
,
Figure DEST_PATH_IMAGE054
,
Figure 195420DEST_PATH_IMAGE055
；
Figure DEST_PATH_IMAGE056
,
Figure 252238DEST_PATH_IMAGE015
；
Figure 425992DEST_PATH_IMAGE057
为采集光纤陀螺角速率白噪声，一般设
Figure DEST_PATH_IMAGE058
、
Figure 205861DEST_PATH_IMAGE059
，
Figure DEST_PATH_IMAGE060
为角度随机游走系数；
Figure 367720DEST_PATH_IMAGE061
为石英绕性加速度计比力白噪声，一般设
Figure DEST_PATH_IMAGE062
、
Figure 600250DEST_PATH_IMAGE063
，
Figure DEST_PATH_IMAGE064
为速度随机游走系数。
Figure 425031DEST_PATH_IMAGE065
和
Figure DEST_PATH_IMAGE066
分别为等效东向和北向速度量测噪声。
实际上，在静基座下噪声分配阵
Figure 528248DEST_PATH_IMAGE021
近似为常值矩阵，可将系统噪声
Figure 831315DEST_PATH_IMAGE067
作等效，记
Figure DEST_PATH_IMAGE068
，则系统模型(11)可简化为：
Figure 456200DEST_PATH_IMAGE009
(12)
其中：
Figure 175020DEST_PATH_IMAGE069
；
Figure DEST_PATH_IMAGE070
为等效采集光纤陀螺噪声，
Figure 100120DEST_PATH_IMAGE071
为等效石英绕性加速度计噪声。
将式(12)离散化，再采用Kalman滤波方法进行估计，便可获得粗对准失准角的最优估计，实现惯导系统的精对准。
2.2 捷联解算算法
本发明采用SINS/OD(Strapdown Inertial Navigation System/Odometer)组合定位定向导航方案，该导航方案属于捷联式惯性导航系统。由于捷联系统没有实体平台，需要模拟平台惯导系统的平台坐标系的功能，这可以通过计算机内的已编排好的导航程序对3个正交安装的采集光纤陀螺角速度信息进行姿态解算来实现。有了模拟平台后就可以把3个方向上的石英绕性加速度计比力信息变换到导航坐标系的比力信息，然后利用积分技术在导航坐标系中计算船体的速度和位置。以地理坐标系为导航坐标系，固连在船体上的石英绕性加速度计和采集光纤陀螺敏感到的比力信号
Figure 246192DEST_PATH_IMAGE022
和角速度信号
Figure 296057DEST_PATH_IMAGE023
，它们在经过各自的安装误差阵变换后得到各自在b系上的投影
Figure DEST_PATH_IMAGE072
和
Figure 739939DEST_PATH_IMAGE025
，经过导航解算和姿态更新可解算出相应的位置、速度、姿态以及角速度等等导航参数。姿态更新采用旋转矢量法，能够对动态误差做补偿。
其中角速度信息需要通过实时更新的姿态矩阵，将采集光纤陀螺输出的载体坐标系下的角速度信息转换到导航坐标系下，同时扣除地球自转以及载体线运动引起的姿态角运动。捷联惯导解算原理图如图5所示。
为了证明本发明的技术方案的创造性和技术价值，该部分是对权利要求技术方案进行具体产品上或相关技术上的应用实施例。
将本发明实施例提供的非可视环境导航方法应用于计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行本发明实施例提供的非可视环境导航方法的步骤。
将本发明实施例提供的非可视环境导航方法应用于一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行所述非可视环境导航方法的步骤。
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种非可视环境导航系统，其特征在于，所述非可视环境导航系统包括：IMU惯性测量单元、距离测量装置以及上位机；
IMU惯性测量单元，包含角速度传感器、加速度传感器以及解算板；用于根据角速度传感器、加速度传感器测量的数据，计算设备当前坐标体系下的位置姿态；并根据测距装置提供的距离信息解算基本导航位置发送至上位机；
距离测量装置，用于利用激光测距传感测量每根钻杆的当前推进长度，并将数据传送至IMU惯性测量单元和上位机；
上位机，由主板、中央处理器CPU、图形处理器GPU及固态存储器SSD组成；用于接收IMU惯性测量单元发送的基本导航位置和姿态位置信息，接收距离测量装置发送的当前长度信息，利用捷联导航算法、导航算法输出当前位置参数以及其他导航信息。
2.如权利要求1所述非可视环境导航系统，其特征在于，所述角速度传感器为三个光纤采集光纤陀螺；所述加速度传感器为三个石英绕性加速度计；
所述解算板采用FPGA+DPS结构；
所述FPGA用于采集光纤陀螺及石英绕性加速度计测量数据、计程仪数据、采集光纤陀螺、石英绕性加速度计和温度信息以及显控指令发送给DSP；同时用于CAN口管理、外设寻址以及读写控制；
所述DSP用于进行导航解算以及网口管理；
所述解算板还设置由ARM芯片；所述ARM芯片由DSP、FPGA、ARM、通讯接口，CAN通信接口、以太网通信接口、及采集光纤陀螺和石英绕性加速度计或其他线路部分组成。
3.如权利要求1所述非可视环境导航系统，其特征在于，所述位置姿态包括航向角、方位角和滚转角。
4.如权利要求1所述非可视环境导航系统，其特征在于，所述非可视环境导航系统还包括：
交互模块，用于通过键盘及鼠标与上位机进行交互，获取当前导航位置的2D横、纵切面图，3D立体建模图、设备运行参数或其他信息。
5.一种应用于如权利要求1-4任意一项所述非可视环境导航系统的非可视环境导航方法，其特征在于，所述非可视环境导航方法包括：
步骤一，利用角速度传感器、加速度传感器进行角速度、加速度测量；根据角速度传感器、加速度传感器测量的数据，计算设备当前坐标体系下的位置姿态；
步骤二，利用激光测距传感测量每根钻杆的当前推进长度；根据每根钻杆的当前推进长度解算基本导航位置；
步骤三，基于所述设备当前坐标体系下的位置姿态、基本导航位置以及每根钻杆的当前推进长度，通过内置时钟计算时间节点，利用捷联导航算法、导航算法输出当前位置参数或其他导航信息。
6.如权利要求5所述非可视环境导航方法，其特征在于，所述计算设备当前坐标体系下的位置姿态包括：
（1）进行粗对准：采用解析的方法计算载体的初始姿态角：
Figure 799099DEST_PATH_IMAGE001
，
Figure 523735DEST_PATH_IMAGE002
；
其中，
Figure 214741DEST_PATH_IMAGE003
表示初始时刻载体惯性系；
Figure 285597DEST_PATH_IMAGE004
表示初始时刻导航惯性系；
Figure 466174DEST_PATH_IMAGE005
表示
Figure 865056DEST_PATH_IMAGE003
系与
Figure 999234DEST_PATH_IMAGE004
系的方位关系；
Figure 476659DEST_PATH_IMAGE006
表示重力矢量在
Figure 936459DEST_PATH_IMAGE004
系的投影；
Figure 291479DEST_PATH_IMAGE007
表示石英绕性加速度计的比力输出在
Figure 167032DEST_PATH_IMAGE003
系投影；
（2）进行精对准：
首先，建立惯导系统系统误差方程如下：
Figure 320801DEST_PATH_IMAGE008
，
Figure 187388DEST_PATH_IMAGE009
；
Figure 826442DEST_PATH_IMAGE010
,
Figure 427057DEST_PATH_IMAGE011
,
Figure 264694DEST_PATH_IMAGE012
；
Figure 833341DEST_PATH_IMAGE013
,
Figure 661488DEST_PATH_IMAGE014
其中，
Figure 177046DEST_PATH_IMAGE015
；
Figure 774249DEST_PATH_IMAGE016
表示等效采集光纤陀螺噪声，
Figure 576114DEST_PATH_IMAGE017
表示等效石英绕性加速度计噪声；
Figure 998217DEST_PATH_IMAGE018
和
Figure 35574DEST_PATH_IMAGE019
分别表示等效东向和北向速度量测噪声；
Figure 942743DEST_PATH_IMAGE020
表示静基座下噪声分配阵
Figure 414045DEST_PATH_IMAGE020
；
其次，采用Kalman滤波方法进行估计，得到粗对准失准角的最优估计。
7.如权利要求5所述非可视环境导航方法，其特征在于，所述利用捷联导航算法、导航算法输出当前位置参数或其他导航信息包括：
对3个正交安装的采集光纤陀螺角速度信息进行姿态解算模拟平台惯导系统的平台坐标系；将3个方向上的石英绕性加速度计比力信息变换到导航坐标系的比力信息，利用积分技术在导航坐标系中计算船体的速度和位置；
以地理坐标系为导航坐标系，固连在船体上的石英绕性加速度计和采集光纤陀螺敏感到的比力信号
Figure 983829DEST_PATH_IMAGE021
和角速度信号
Figure 372347DEST_PATH_IMAGE022
经过各自的安装误差阵变换后得到各自在b系上的投影
Figure 678564DEST_PATH_IMAGE023
和
Figure 196133DEST_PATH_IMAGE024
；经过导航解算和姿态更新解算得到相应的位置、速度、姿态以及角速度或其他导航参数。
8.如权利要求6所述非可视环境导航方法，其特征在于，所述姿态更新采用旋转矢量法，角速度信息通过实时更新的姿态矩阵，将采集光纤陀螺输出的载体坐标系下的角速度信息转换到导航坐标系下，同时去除地球自转以及载体线运动引起的姿态角运动。
9.一种计算机设备，其特征在于，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求5-8任意一项所述非可视环境导航方法的步骤。
10.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如权利要求5-8任意一项所述非可视环境导航方法的步骤。
CN202211445347.8A 2022-11-18 2022-11-18 一种非可视环境导航系统、方法、计算机设备及存储介质 Active CN115574815B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211445347.8A CN115574815B (zh) 2022-11-18 2022-11-18 一种非可视环境导航系统、方法、计算机设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211445347.8A CN115574815B (zh) 2022-11-18 2022-11-18 一种非可视环境导航系统、方法、计算机设备及存储介质

Publications (2)

Publication Number Publication Date
CN115574815A true CN115574815A (zh) 2023-01-06
CN115574815B CN115574815B (zh) 2023-05-23

Family

ID=84589533

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211445347.8A Active CN115574815B (zh) 2022-11-18 2022-11-18 一种非可视环境导航系统、方法、计算机设备及存储介质

Country Status (1)

Country Link
CN (1) CN115574815B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116105731A (zh) * 2023-04-07 2023-05-12 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103591927A (zh) * 2013-11-11 2014-02-19 中煤科工集团西安研究院有限公司 井下钻机姿态测量仪及其测量方法
US20150276934A1 (en) * 2014-04-01 2015-10-01 The United States Of America As Represented By The Secretary Of The Navy Correlated gps pseudorange error estimation method
CN106052686A (zh) * 2016-07-10 2016-10-26 北京工业大学 基于dsptms320f28335的全自主捷联惯性导航系统
WO2017027925A1 (en) * 2015-08-17 2017-02-23 Precision Alignment Holdings Pty Ltd A drill rig positioning and drill rod alignment system
US20190257176A1 (en) * 2016-09-22 2019-08-22 Halliburton Energy Services, Inc. Downhole positioning control system with force compensation
CN110411444A (zh) * 2019-08-22 2019-11-05 深圳赛奥航空科技有限公司 一种地面下采掘移动设备用惯性导航定位系统与定位方法
CN111412911A (zh) * 2020-04-07 2020-07-14 中国煤炭科工集团太原研究院有限公司 一种煤矿井下连续采煤机器人多传感器组合导航系统
CN112781588A (zh) * 2020-12-31 2021-05-11 厦门华源嘉航科技有限公司 一种随钻陀螺定位定向仪导航解算方法
CN113203411A (zh) * 2021-04-23 2021-08-03 南京理工大学 一种弹性嵌入式gnss/惯性组合导航系统及方法
US20210348924A1 (en) * 2020-05-11 2021-11-11 Institute Of Geology And Geophysics, Chinese Academy Of Sciences Attitude measurement method

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103591927A (zh) * 2013-11-11 2014-02-19 中煤科工集团西安研究院有限公司 井下钻机姿态测量仪及其测量方法
US20150276934A1 (en) * 2014-04-01 2015-10-01 The United States Of America As Represented By The Secretary Of The Navy Correlated gps pseudorange error estimation method
WO2017027925A1 (en) * 2015-08-17 2017-02-23 Precision Alignment Holdings Pty Ltd A drill rig positioning and drill rod alignment system
CN106052686A (zh) * 2016-07-10 2016-10-26 北京工业大学 基于dsptms320f28335的全自主捷联惯性导航系统
US20190257176A1 (en) * 2016-09-22 2019-08-22 Halliburton Energy Services, Inc. Downhole positioning control system with force compensation
CN110411444A (zh) * 2019-08-22 2019-11-05 深圳赛奥航空科技有限公司 一种地面下采掘移动设备用惯性导航定位系统与定位方法
CN111412911A (zh) * 2020-04-07 2020-07-14 中国煤炭科工集团太原研究院有限公司 一种煤矿井下连续采煤机器人多传感器组合导航系统
US20210348924A1 (en) * 2020-05-11 2021-11-11 Institute Of Geology And Geophysics, Chinese Academy Of Sciences Attitude measurement method
CN112781588A (zh) * 2020-12-31 2021-05-11 厦门华源嘉航科技有限公司 一种随钻陀螺定位定向仪导航解算方法
CN113203411A (zh) * 2021-04-23 2021-08-03 南京理工大学 一种弹性嵌入式gnss/惯性组合导航系统及方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
A.G. LEDROZ; E. PECHT; D. CRAMER; M.P. MINTCHEV: "FOG-based navigation in downhole environment during horizontal drilling utilizing a complete inertial measurement unit: directional measurement-while-drilling surveying", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 *
丁磊香，许厚泽，王勇，等: "静基座捷联惯导解析法对准研究", 《武汉大学学报(信息科学版)》 *
杨波;彭培林;王跃钢;周小刚;: "里程计辅助捷联惯导运动基座对准方法", 中国惯性技术学报 *
陈小凤等: "船用光纤陀螺捷联航姿基准系统", 《大连海事大学学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116105731A (zh) * 2023-04-07 2023-05-12 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质

Also Published As

Publication number Publication date
CN115574815B (zh) 2023-05-23

Similar Documents

Publication Publication Date Title
CN110799727B (zh) 用于生成下向井眼惯性测量单元的输出的系统和方法
US8489333B2 (en) Device orientation determination
CN100489459C (zh) 适用于全光纤数字测斜仪的捷联惯性组合测量控制装置
CN201865663U (zh) 一种惯性测量单元以及动力调谐陀螺连续测斜系统
CN101424534B (zh) 惯性/重力组合导航半实物模拟装置
CN101493008A (zh) 基于mems器件的捷联惯性导航陀螺测斜仪
CN101876244A (zh) 一种惯性测量单元以及动力调谐陀螺连续测斜仪
CN109891049A (zh) 基于实时惯性感测的增量轨迹估计系统
CN102749065B (zh) 基于惯性测量技术的罐笼轨道变形监测方法
CN104110252B (zh) 基于惯性传感器的长大锚索钻孔孔道测量系统
Chen et al. Positioning accuracy of a pipeline surveying system based on MEMS IMU and odometer: Case study
CN109540135A (zh) 水田拖拉机位姿检测和偏航角提取的方法及装置
CN103114846B (zh) 一种基于光纤陀螺测斜仪的测斜数据的事后处理系统
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定系统及应用方法
CN107228664A (zh) 矿用陀螺测斜仪捷联惯导系统姿态解算及零速校正方法
CN110440746A (zh) 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法
CN115574815B (zh) 一种非可视环境导航系统、方法、计算机设备及存储介质
CN106842080A (zh) 一种磁场测量装置姿态摆动干扰去除方法
Han et al. An orientation navigation approach based on INS and odometer integration for underground unmanned excavating machine
CN105134171B (zh) 一种两轴光纤陀螺连续测斜系统的实现方法
Ji et al. An attitude improvement method of FOG-based measurement-while-drilling utilizing backtracking navigation algorithm
NO840482L (no) Apparat for oppmaaling av borehull
CN102033235A (zh) 基于gps/sins城镇地籍图根点快速测量及事后数据处理方法
CN112882118B (zh) 地固坐标系下动基座重力矢量估计方法、系统及存储介质
Ben et al. System reset of strapdown INS for pipeline inspection gauge

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