CN112229392B - 一种高冗余的室内煤场导航方法及系统 - Google Patents

一种高冗余的室内煤场导航方法及系统 Download PDF

Info

Publication number
CN112229392B
CN112229392B CN202011026735.3A CN202011026735A CN112229392B CN 112229392 B CN112229392 B CN 112229392B CN 202011026735 A CN202011026735 A CN 202011026735A CN 112229392 B CN112229392 B CN 112229392B
Authority
CN
China
Prior art keywords
subsystem
data
navigation data
slam
navigation
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
CN202011026735.3A
Other languages
English (en)
Other versions
CN112229392A (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.)
Shanghai Shuchuang Technology Co ltd
Fujian Huadian Kemen Power Generation Co ltd
Original Assignee
Shanghai Shuchuang Technology Co ltd
Fujian Huadian Kemen Power Generation 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 Shanghai Shuchuang Technology Co ltd, Fujian Huadian Kemen Power Generation Co ltd filed Critical Shanghai Shuchuang Technology Co ltd
Priority to CN202011026735.3A priority Critical patent/CN112229392B/zh
Publication of CN112229392A publication Critical patent/CN112229392A/zh
Application granted granted Critical
Publication of CN112229392B publication Critical patent/CN112229392B/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/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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/021—Services related to particular areas, e.g. point of interest [POI] services, venue services or geofences
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Signal Processing (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种高冗余的室内煤场导航方法及系统，其中涉及的一种高冗余的室内煤场导航方法，包括步骤：S11.分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；S12.判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；S13.根据所述待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。本发明为室内煤场环境(GNSS信号微弱或不存在)下无人机飞行提供安全、准确的导航数据。

Description

一种高冗余的室内煤场导航方法及系统
技术领域
本发明涉及导航技术领域，尤其涉及一种高冗余的室内煤场导航方法及系统。
背景技术
火电站煤场的管理工作中，火电站煤场内环境恶劣，现有的煤量测量方法操作复杂、操作时间长。近年来随着机器视觉、传感器科学、人工智能等领域的快速发展，智能无人机的应用已经逐步从军用、消费领域拓展到工业生产中。利用无人机进行煤场盘煤可克服传统煤场测量方法存在的测量周期长，精度低，运行成本高等缺点。
半封闭式煤场内部结构复杂，障碍物较多，不能有效地利用GNSS信号。选用视觉UWB、INS、SLAM等代替性的定位技术能够在室内环境下获取位置信息，但上述三种定位技术在煤场环境下均有一定缺点，无法满足导航可靠性要求。
半封闭式煤场内部结构复杂，干扰因素较多，独立的UWB、INS、SLAM定位技术均无法满足无人机定位需求。超宽带(Ultra-Wideband，UWB)技术作为一种新兴的无线通信技术，其使用纳秒级(ns)至皮秒(ps)级的非正弦窄脉冲进行数据传输。针对传统无线技术在通信过程中存在的问题，利用超宽带技术开发的具有千兆赫兹容量的新型无线信道可以解决信道窄、容量小、易受干扰等问题。
基于上面这些技术的支持，UWB具有极高的信号带宽。它对信道衰落具有很强的免疫能力，具有时间分辨率高、穿透能力强、功耗低、测距精度高、系统成本较低等特点。但在实际使用中物体遮挡会造成的较大的精度误差，无法满足无人机控制的要求。
捷联式惯导系统(INS)是将加速度计、陀螺仪等惯性测量元件直接安装在载体内部，然后经过数学平台(由捷联矩阵经过转换到导航坐标系中的参量)输出，然后再将输出的数据解算出来。由于INS系统的位置量是由积分计算得到，误差数据会不断累积，因此较长的时间尺度下难以保持稳定性。
SLAM技术是一种基于图像(激光)的即时定位与建图技术，它根据图像数据，计算帧与帧之间的位置关系，在移动过程中根据位置估计和先验地图进行自身定位，同时建造环境地图。但由于在煤场作业中，煤堆体积、位置等地理信息变化较大，导致先验地图无法使用，这使得SLAM方法精度会随着时间推移逐渐下降，且稳定性不足。
因此，如何将上述三种定位技术进行融合且应用于煤场中是丞待解决的的问题。
发明内容
本发明的目的是针对现有技术的缺陷，提供了一种高冗余的室内煤场导航方法及系统，为室内煤场环境(GNSS信号微弱或不存在)下无人机飞行提供安全、准确的导航数据。
为了实现以上目的，本发明采用以下技术方案：
一种高冗余的室内煤场导航方法，包括步骤：
S1.分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
S2.判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；
S3.根据所述待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。
进一步的，所述步骤S1中获取的UWB子系统的导航数据为无人机当前时刻的位置xU；获取的INS子系统的导航数据为无人机当前的加速度aI；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS。
进一步的，所述步骤S2具体为：
S21.分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
S22.判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值，若是，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据。
进一步的，所述步骤S2中选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据具体为：
选取SLAM子系统中的加速度误差、速度误差、位置误差，并将选取的加速度误差、速度误差、位置误差作为组合导航系统的状态变量，表示为：
X(t)＝[ax，ay，az，δVx，δVy，δVz，δLx，δLy，δLz]T
其中，ax，ay，az分别表示x、y、z轴的加速度误差；δVx，δVy，δVz分别表示x、y、z轴的速度误差；δLx，δLy，δLz分别表示x、y、z轴的位置误差；
根据运动学公式，建立状态变量更新的状态方程：
Figure BDA0002702333510000031
其中，Fk表示状态转移矩阵；Gk表示噪声驱动矩阵；Wk表示均值为0，方差为Q的噪声。
Figure BDA0002702333510000032
表示k-1时刻状态量的后验，
Figure BDA0002702333510000033
表示k时刻状态量的先验。
Figure BDA0002702333510000034
Wk＝[ωx(k)，ωy(k)，ωz(k)]T
Figure BDA0002702333510000035
Figure BDA0002702333510000036
选取SLAM子系统与UWB子系统中的位置差值Δx，并将选取的位置差值Δx作为组合导航系统的观测量，表示为：
Z(t)＝xS-xU＝Δx＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪声；
进一步的，所述步骤S2中选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据具体为：
选取INS子系统中的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差，并将选取的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航系统的状态变量，表示为：
Figure BDA0002702333510000041
其中，
Figure BDA0002702333510000042
分别表示x、y、z轴的姿态误差；δVx，δVy，δVz分别表示x、y、z轴的速度误差；δLx，δLy，δLz分别表示x、y、z轴的位置误差；εbx，εby，εbz分别表示x、y、z轴的陀螺仪的常值漂移误差；Δax，Δay，Δaz分别表示x、y、z轴的加速度计的常值漂移误差；
根据运动学公式，建立状态变量更新的状态方程：
Figure BDA0002702333510000043
其中，Fk表示状态转移矩阵；Gk表示噪声驱动矩阵；Wk表示均值为0，方差为Q的噪声。
Figure BDA0002702333510000044
表示k-1时刻状态量的后验，
Figure BDA0002702333510000045
表示k时刻状态量的先验。
Figure BDA0002702333510000046
其中
Figure BDA0002702333510000047
Figure BDA0002702333510000048
表示INS测量得到的三个方向的比力值。
Figure BDA0002702333510000049
Figure BDA0002702333510000051
Figure BDA0002702333510000052
选取INS子系统与UWB子系统中的位置误差，并将选取的位置误差作为组合导航系统的观测量，表示为：
Z(t)＝xI-xU＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪声；xI表示INS子系统中无人机当前的位置。
进一步的，所述步骤S3中卡尔曼滤波模型中的卡尔曼滤波公式，表示为：
Figure BDA0002702333510000053
Pk|k-1＝FkPk-1|k-1Fk T+Qk
Figure BDA0002702333510000054
Sk＝HkPk|k-1Hk T+Rk
Kk＝Pk|k-1Hk TSk -1
其中，Qk表示系统误差的协方差；Pk|k-1表示状态量的协方差；
Figure BDA0002702333510000055
表示观测余量；Zk表示观测量；Hk为状态转移矩阵；Sk为中间量；Kk表示卡尔曼滤波系数；Rk表示测量误差的协方差。
进一步的，所述步骤S3中输出SLAM子系统或INS子系统误差量的估计值，表示为：
Figure BDA0002702333510000056
其中，
Figure BDA0002702333510000057
表示状态量的后验估计。
相应的，还提供一种高冗余的室内煤场导航系统，包括：
获取模块，用于分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
第一判断模块，用于判断获取的SLAM子系统的导航数据是否合格；
输出模块，用于根据待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。
进一步的，所述获取模块中获取的UWB子系统的导航数据为无人机当前时刻的位置xU；获取的INS子系统的导航数据为无人机当前的加速度aI；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS。
进一步的，所述判断模块包括：
计算模块，用于分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
第二判断模块，用于判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值。
与现有技术相比，本发明的有益效果为：
1、提出了一套基于松耦合原理的，将UWB、INS、SLAM导航系统进行数据融合的方法，能够为无人机提供精确位置坐标。
2、设计一种高冗余的导航方案，确保在部分子系统失效的情况下，导航系统仍能够正常工作。
3、设计了两种基于位置差分数据的卡尔曼滤波器，提高了系统导航数据的精确性。
附图说明
图1是实施例一提供的一种高冗余的室内煤场导航方法流程图；
图2是实施例一提供的第一状态方程设计数据融合结构示意图；
图3是实施例一提供的第二状态方程设计数据融合结构示意图；
图4是实施例二提供的一种高冗余的室内煤场导航系统结构图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
本发明的目的是针对现有技术的缺陷，提供了一种高冗余的室内煤场导航方法及系统。
实施例一
本实施例提供一种高冗余的室内煤场导航方法，如图1所示，包括步骤：
S11.分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
S12.判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；
S13.根据所述待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。
需要说明的是，本实施例的执行主体是导航系统。
在步骤S11中，分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据。
导航系统读取UWB子系统输出的导航数据、INS子系统输出的导航数据、SLAM子系统输出的导航数据。其中，UWB子系统输出的的导航数据为无人机当前时刻的位置xU；INS子系统输出的导航数据为无人机当前的加速度aI；SLAM子系统输出的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS。
需要说明的是，UWB子系统、INS子系统、SLAM子系统得到的导航数据均可通过现有技术实现。
在步骤S12中，判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据。
当获取到UWB子系统、INS子系统、SLAM子系统输出的导航数据后，对SLAM子系统输出的导航数据进行校验，判断其输出的导航数据是否满足导航要求。
在本实施例中，步骤S12中具体的校验方式为：
S121.分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
导航系统计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx，计算SLAM子系统的位置xS与UWB子系统的位置xU之间的协方差E。
S122.判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值，若是，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据。
首先预设SLAM子系统与UWB子系统的位置差值的阈值(即第一阈值)、预设SLAM子系统与UWB子系统位置之间的协方差的阈值(即第二阈值)，将预设的第一阈值和第二阈值存储于导航系统中。
需要说明的是，第一阈值与第二阈值的具体值可根据实际情况进行设定，本实施例不做限定。
判断SLAM子系统与UWB子系统的位置差值Δx是否超过第一预设阈值的波动，判断SLAM子系统与UWB子系统位置之间的协方差E是否超过第二预设阈值的波动。本实施例中判断的依据是只要满足上述两个判断条件的至少一个，则认为SLAM子系统输出有误，即输出的导航数据不合格。
若SLAM子系统输出的导航数据合格，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据。具体为：
选取SLAM子系统中的加速度误差、速度误差、位置误差，并将选取的加速度误差、速度误差、位置误差作为组合导航系统的状态变量，表示为：
X(t)＝[ax，ay，az，δVx，δVy，δVz，δLx，δLy，δLz]T
其中，ax，ay，az分别表示x、y、z轴的加速度误差；δVx，δVy，δVz分别表示x、y、z轴的速度误差；δLx，δLy，δLz分别表示x、y、z轴的位置误差；
根据运动学公式，建立状态变量更新的状态方程：
Figure BDA0002702333510000081
其中，Fk表示状态转移矩阵；Gk表示噪声驱动矩阵；Wk表示均值为0，方差为Q的噪声。
Figure BDA0002702333510000091
表示k-1时刻状态量的后验，
Figure BDA0002702333510000092
表示k时刻状态量的先验。
Figure BDA0002702333510000093
Wk＝[ωx(k)，ωy(k)，ωz(k)]T
Figure BDA0002702333510000094
Figure BDA0002702333510000095
选取SLAM子系统与UWB子系统中的位置差值Δx，并将选取的位置差值Δx作为组合导航系统的观测量，表示为：
Z(t)＝xS-xU＝Δx＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪声；
根据第一状态方程设计的数据融合结构如图2所示，其中图2中SLAM误差补偿器输出最终的定位位置。
若SLAM子系统输出的导航数据不合格，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据。
选取INS子系统中的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差，并将选取的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航系统的状态变量，表示为：
Figure BDA0002702333510000096
其中，
Figure BDA0002702333510000097
分别表示x、y、z轴的姿态误差；δVx，δVy，δVz分别表示x、y、z轴的速度误差；δLx，δLy，δLz分别表示x、y、z轴的位置误差；εbx，εby，εbz分别表示x、y、z轴的陀螺仪的常值漂移误差；Δax，Δay，Δaz分别表示x、y、z轴的加速度计的常值漂移误差；
根据运动学公式，建立状态变量更新的状态方程：
Figure BDA0002702333510000101
其中，Fk表示状态转移矩阵；Gk表示噪声驱动矩阵；Wk表示均值为0，方差为Q的噪声。
Figure BDA0002702333510000102
表示k-1时刻状态量的后验，
Figure BDA0002702333510000103
表示k时刻状态量的先验。
Figure BDA0002702333510000104
其中
Figure BDA0002702333510000105
Figure BDA0002702333510000106
表示INS测量得到的三个方向的比力值。
Figure BDA0002702333510000107
Figure BDA0002702333510000108
Figure BDA0002702333510000109
选取INS子系统与UWB子系统中的位置误差，并将选取的位置误差作为组合导航系统的观测量，表示为：
Z(t)＝xI-xU＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪声；xI表示INS子系统中无人机当前的位置。
根据第二状态方程设计的数据融合结构如图3所示，其中图3中INS误差补偿器输出最终的定位位置。
在步骤S13中，根据待融合数据建立卡尔曼滤波模型，根据建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。
根据待融合的数据结构建立卡尔曼滤波模型，获取状态量的最优估计，进一步根据状态变量计算出无人机位姿坐标。卡尔曼滤波公式如下：
Figure BDA0002702333510000111
Pk|k-1＝FkPk-1|k-1Fk T+Qk
Figure BDA0002702333510000112
Sk＝HkPk|k-1Hk T+Rk
Kk＝Pk|k-1Hk TSk -1
其中，Qk表示系统误差的协方差；Pk|k-1表示状态量的协方差；
Figure BDA0002702333510000113
表示观测余量；Zk表示观测量；Hk为状态转移矩阵；Sk为中间量；Kk表示卡尔曼滤波系数；Rk表示测量误差的协方差。
输出SLAM子系统或INS子系统误差量的估计值，表示为：
Figure BDA0002702333510000114
其中，
Figure BDA0002702333510000115
表示状态量的后验估计。
该估计值即对SLAM子系统或INS子系统误差量的估计值。
本实施例采用松耦合的结构设计使得各导航模块可以相互作为冗余，当UWB子系统失效时，导航系统可以依靠SLAM子系统/INS子系统继续完成对位姿的计算；当SLAM子系统发生故障时，系统可以使用INS子系统代替完成数据融合任务。
本实施例为室内煤场环境(GNSS信号微弱或不存在)下无人机飞行提供稳定、安全的定位导航方法，其创新点体现为：
1)提出了一套基于松耦合原理的，将UWB、INS、SLAM导航系统进行数据融合的方法，能够为无人机提供精确位置坐标。
2)设计一种高冗余的导航方案，确保在部分子系统失效的情况下，导航系统仍能够正常工作。
3)设计了两种基于位置差分数据的卡尔曼滤波器，提高了系统导航数据的精确性。
实施例二
本实施例提供一种高冗余的室内煤场导航系统，如图4所示，包括：
获取模块11，用于分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
第一判断模块12，用于判断获取的SLAM子系统的导航数据是否合格；
输出模块13，用于根据待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据。
进一步的，获取模块11中获取的UWB子系统的导航数据为无人机当前时刻的位置xU；获取的INS子系统的导航数据为无人机当前的加速度aI；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS。
进一步的，判断模块12包括：
计算模块，用于分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
第二判断模块，用于判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值。
需要说明的是，本实施例提供的一种高冗余的室内煤场导航系统与实施例一类似，在此不多做赘述。
与现有技术相比，本发明的有益效果为：
1、提出了一套基于松耦合原理的，将UWB、INS、SLAM导航系统进行数据融合的方法，能够为无人机提供精确位置坐标。
2、设计一种高冗余的导航方案，确保在部分子系统失效的情况下，导航系统仍能够正常工作。
3、设计了两种基于位置差分数据的卡尔曼滤波器，提高了系统导航数据的精确性。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (8)

1.一种高冗余的室内煤场导航方法，其特征在于，包括步骤：
S1.分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
S2.判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；
S3.根据所述待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据；
所述步骤S1中获取的UWB子系统的导航数据为无人机当前时刻的位置xU；获取的INS子系统的导航数据为无人机当前的加速度aI；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS；
所述步骤S2具体为：
S21.分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
S22.判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值，若是，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据。
2.根据权利要求1所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S2中选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据具体为：
选取SLAM子系统中的加速度误差、速度误差、位置误差，并将选取的加速度误差、速度误差、位置误差作为组合导航系统的状态变量，表示为：
X(t)＝[ax，ay，az，δVx，δVy，δVz，δLx，δLy，δLz]T
其中，ax，ay，az分别表示x、y、z轴的加速度误差；δVx，δVy，δVz分别表示x、y、z轴的速度误差；δLx，δLy，δLz分别表示x、y、z轴的位置误差；
根据运动学公式，建立状态变量更新的状态方程：
Figure FDA0003783854810000011
其中，Fk表示状态转移矩阵；Gk表示噪声驱动矩阵；Wk表示均值为0，方差为Q的噪声；
Figure FDA0003783854810000012
表示k-1时刻状态量的后验，
Figure FDA0003783854810000013
表示k时刻状态量的先验；
Figure FDA0003783854810000014
Wk＝[ωx(k)，ωy(k)，ωz(k)]T
Figure FDA0003783854810000021
Figure FDA0003783854810000022
选取SLAM子系统与UWB子系统中的位置差值Δx，并将选取的位置差值Δx作为组合导航系统的观测量，表示为：
Z(t)＝xS-xU＝Δx＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪声。
3.根据权利要求1所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S2中选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据具体为：
选取INS子系统中的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差，并将选取的姿态误差、速度误差、位置误差以及陀螺仪和加速度计的常值漂移误差作为组合导航系统的状态变量，表示为：
Figure FDA0003783854810000023
其中，
Figure FDA0003783854810000024
分别表示x、y、z轴的姿态误差；δVx，δVy，δVz分别表示x、y、z轴的速度误差；δLx，δLy，δLz分别表示x、y、z轴的位置误差；εbx，εby，εbz分别表示x、y、z轴的陀螺仪的常值漂移误差；Δax，Δay，Δaz分别表示x、y、z轴的加速度计的常值漂移误差；
根据运动学公式，建立状态变量更新的状态方程：
Figure FDA0003783854810000025
其中，Fk表示状态转移矩阵；Gk表示噪声驱动矩阵；Wk表示均值为0，方差为Q的噪声；
Figure FDA0003783854810000026
表示k-1时刻状态量的后验，
Figure FDA0003783854810000027
表示k时刻状态量的先验；
Figure FDA0003783854810000028
其中
Figure FDA0003783854810000031
表示INS测量得到的三个方向的比力值；
Figure FDA0003783854810000032
Figure FDA0003783854810000033
Figure FDA0003783854810000034
选取INS子系统与UWB子系统中的位置误差，并将选取的位置误差作为组合导航系统的观测量，表示为：
Z(t)＝xI-xU＝[δLx+Nx，δLy+Ny，δLz+Nz]T
其中，Nx，Ny，Nz表示白噪声；xI表示INS子系统中无人机当前的位置。
4.根据权利要求2或3所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S3中卡尔曼滤波模型中的卡尔曼滤波公式，表示为：
Figure FDA0003783854810000035
Pk|k-1＝FkPk-1|k-1Fk T+Qk
Figure FDA0003783854810000036
Sk＝HkPk|k-1Hk T+Rk
Kk＝Pk|k-1Hk TSk -1
其中，Qk表示系统误差的协方差；Pk|k-1表示状态量的协方差；
Figure FDA0003783854810000037
表示观测余量；Zk表示观测量；Hk为状态转移矩阵；Sk为中间量；Kk表示卡尔曼滤波系数；Rk表示测量误差的协方差。
5.根据权利要求4所述的一种高冗余的室内煤场导航方法，其特征在于，所述步骤S3中输出SLAM子系统或INS子系统误差量的估计值，表示为：
Figure FDA0003783854810000041
其中，
Figure FDA0003783854810000042
表示状态量的后验估计。
6.一种高冗余的室内煤场导航系统，其特征在于，包括：
获取模块，用于分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
第一判断模块，用于判断获取的SLAM子系统的导航数据是否合格；
输出模块，用于根据待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据；
室内煤场导航方法包括步骤：
S1.分别获取设置于煤场的UWB子系统、INS子系统、SLAM子系统的导航数据；
S2.判断获取的SLAM子系统的导航数据是否合格，若是，则选取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；
S3.根据所述待融合数据建立卡尔曼滤波模型，根据所述建立的卡尔曼滤波模型输出SLAM子系统或INS子系统误差量的估计值，得到煤场的定位数据；
所述步骤S1中获取的UWB子系统的导航数据为无人机当前时刻的位置xU；获取的INS子系统的导航数据为无人机当前的加速度aI；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS；
所述步骤S2具体为：
S21.分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
S22.判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值，若是，则选取INS子系统的导航数据与UWB子系统的导航数据作为待融合数据；若否，则取SLAM子系统的导航数据与UWB子系统的导航数据作为待融合数据。
7.根据权利要求6所述的一种高冗余的室内煤场导航系统，其特征在于，所述获取模块中获取的UWB子系统的导航数据为无人机当前时刻的位置xU；获取的INS子系统的导航数据为无人机当前的加速度aI；获取的SLAM子系统的导航数据为无人机当前时刻的位置加速度aS、速度vS、位置xS。
8.根据权利要求7所述的一种高冗余的室内煤场导航系统，其特征在于，所述判断模块包括：
计算模块，用于分别计算SLAM子系统的位置xS与UWB子系统的位置xU之间的差值Δx以及协方差E；
第二判断模块，用于判断计算得到差值Δx是否超过第一预设阈值和/或判断计算得到的协方差E是否超过第二预设阈值。
CN202011026735.3A 2020-09-25 2020-09-25 一种高冗余的室内煤场导航方法及系统 Active CN112229392B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011026735.3A CN112229392B (zh) 2020-09-25 2020-09-25 一种高冗余的室内煤场导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011026735.3A CN112229392B (zh) 2020-09-25 2020-09-25 一种高冗余的室内煤场导航方法及系统

Publications (2)

Publication Number Publication Date
CN112229392A CN112229392A (zh) 2021-01-15
CN112229392B true CN112229392B (zh) 2022-11-18

Family

ID=74108182

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011026735.3A Active CN112229392B (zh) 2020-09-25 2020-09-25 一种高冗余的室内煤场导航方法及系统

Country Status (1)

Country Link
CN (1) CN112229392B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114625155B (zh) * 2022-05-13 2022-08-19 向量传感科技(宁波)有限公司 自动驾驶软件三重冗余管理系统及方法
CN117459898A (zh) * 2023-12-22 2024-01-26 浙江深寻科技有限公司 一种应急定位通信方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059942A (zh) * 2018-08-22 2018-12-21 中国矿业大学 一种井下高精度导航地图构建系统及构建方法
CN109556632A (zh) * 2018-11-26 2019-04-02 北方工业大学 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7839916B1 (en) * 2003-10-08 2010-11-23 L-3 Communications Corporation Systems and methods for communication in a global positioning system (GPS) device
CN108827318A (zh) * 2018-08-20 2018-11-16 中科物栖（北京）科技有限责任公司 无人机室内定位方法及装置
CN110487267B (zh) * 2019-07-10 2021-06-04 湖南交工智能技术有限公司 一种基于vio＆uwb松组合的无人机导航系统及方法
CN110446159B (zh) * 2019-08-12 2020-11-27 上海工程技术大学 一种室内无人机精确定位与自主导航的系统及方法
CN110850457A (zh) * 2019-10-22 2020-02-28 同济大学 一种用于室内煤场的无人机定位导航方法
CN110879071B (zh) * 2019-12-06 2021-05-25 成都云科新能汽车技术有限公司 基于车路协同的高精度定位系统及定位方法
CN111175795B (zh) * 2020-01-03 2023-05-26 暨南大学 Gnss/ins组合导航系统的两步抗差滤波方法及系统

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059942A (zh) * 2018-08-22 2018-12-21 中国矿业大学 一种井下高精度导航地图构建系统及构建方法
CN109556632A (zh) * 2018-11-26 2019-04-02 北方工业大学 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法

Also Published As

Publication number Publication date
CN112229392A (zh) 2021-01-15

Similar Documents

Publication Publication Date Title
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN112505737B (zh) 一种gnss/ins组合导航方法
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN112229392B (zh) 一种高冗余的室内煤场导航方法及系统
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN101858748A (zh) 高空长航无人机的多传感器容错自主导航方法
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN112923931A (zh) 一种基于固定路线下的特征地图匹配与gps定位信息融合方法
Yu et al. GPS/INS/Odometer/DR integrated navigation system aided with vehicular dynamic characteristics for autonomous vehicle application
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN115060257A (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
CN114136310B (zh) 一种惯性导航系统误差自主抑制系统及方法
CN111141286A (zh) 一种无人机飞控多传感器姿态置信解算方法
CN114415224A (zh) 一种复杂受限环境中车辆融合定位系统及方法
CN114088091A (zh) 一种基于多传感器的井工矿位姿融合方法和系统
CN112729283A (zh) 一种基于深度相机/mems惯导/里程计组合的导航方法
CN116594000A (zh) 基于位置观测的激光多普勒测速仪在线标定方法和装置
CN114162170B (zh) 一种轨道测量系统及测量方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
Wang et al. Uav attitude measurement based on enhanced mahony complementary filter
CN104655132A (zh) 一种基于加速度计的机体弹性变形角估计方法
CN113237482B (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