CN114779307B - 面向港区的uwb/ins/gnss的无缝定位方法 - Google Patents

面向港区的uwb/ins/gnss的无缝定位方法 Download PDF

Info

Publication number
CN114779307B
CN114779307B CN202210683838.XA CN202210683838A CN114779307B CN 114779307 B CN114779307 B CN 114779307B CN 202210683838 A CN202210683838 A CN 202210683838A CN 114779307 B CN114779307 B CN 114779307B
Authority
CN
China
Prior art keywords
uwb
gnss
ins
confidence
observation equation
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
CN202210683838.XA
Other languages
English (en)
Other versions
CN114779307A (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.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
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 Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202210683838.XA priority Critical patent/CN114779307B/zh
Publication of CN114779307A publication Critical patent/CN114779307A/zh
Application granted granted Critical
Publication of CN114779307B publication Critical patent/CN114779307B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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
    • 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/52—Determining velocity
    • 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/53—Determining attitude

Landscapes

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

Abstract

一种面向港区的UWB/INS/GNSS的无缝定位方法，包括以下步骤：先对港区内部运载平台上接收器接收到的GNSS信号、UWB信号进行置信度计算，再分析GNSS信号是否可信、UWB信号是否可定位，然后选择解算GNSS系统得到的观测方程与INS系统得到的状态方程、解算UWB系统得到的观测方程与INS系统得到的状态方程、将GNSS系统得到的观测方程与UWB系统得到的观测方程进行加权得到综合观测方程并解算综合观测方程与INS系统得到的状态方程、解算INS系统输出结果中的一种定位方式得出运载平台的最优位置、速度、姿态信息。本发明不仅可以实现港区的无缝定位，而且定位结果稳定性好、准确性好。

Description

面向港区的UWB/INS/GNSS的无缝定位方法
技术领域
本发明涉及空间定位技术领域，尤其涉及一种面向港区的UWB/INS/GNSS的无缝定位方法。
背景技术
港区运载平台的定位定姿技术在港口高效作业、安全作业等方面起到重要作用，是港口信息化、智能化发展的关键技术。目前，港区主要采用自动导引运输（AGV）或智慧型引导运输车（IGV）技术，涉及地磁导航、卫星-惯性组合导航等技术。然而，由于港区环境复杂特殊，高集装箱和多种机械作业装备同时存在，容易产生多径效应，且港区常有恶劣天气、温湿度、电离层延迟、对流层延迟等因素影响严重，导致差分定位精度不高以及与惯导融合时无法修正惯导随时间产生的累计误差等问题。而老旧港口格局相对固定，基础设施陈旧，即使地磁导航可以在一定程度上解决GNSS信号不稳定对定位的影响，但由于需要额外安装磁钉等设施，灵活性差，改造、运营和维护成本高，不适合老港口的智能化发展。
发明内容
本发明的目的是克服现有技术中存在的定位结果的稳定性和准确性差的缺陷与问题，提供一种定位结果的稳定性和准确性好的面向港区的UWB/INS/GNSS的无缝定位方法。
为实现以上目的，本发明的技术解决方案是：一种面向港区的UWB/INS/GNSS的无缝定位方法，该方法包括以下步骤：
S1、对港区内部运载平台上接收器接收到的GNSS信号进行置信度计算、UWB信号进行置信度计算；
S2、对步骤S1中的GNSS信号的置信度与UWB信号的置信度进行分析，并根据分析结果选择以下四种定位方式中的一种；
第一种定位方式：若GNSS信号可信、UWB信号不可定位，则将由GNSS系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第二种定位方式：若GNSS信号不可信、UWB信号可定位，则将由UWB系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第三种定位方式：若GNSS信号可信、UWB信号可定位，则将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程，并将综合观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第四种定位方式：若GNSS信号不可信、UWB信号不可定位，则对INS系统输出结果进行解算，得出运载平台的最优位置、速度、姿态信息。
步骤S1中，GNSS信号的置信度计算方法为：先将港区环境的观测数据根据区域类别赋予不同的值，再将港区内部接收到的GNSS信号的观测数据和港区环境的观测数据与GNSS信号的置信度建立对应关系，建立两组互不相交的数据集，一组作为SVM模型的训练集，另一组作为SVM模型的测试集，然后通过上述数据集训练神经网络，输出GNSS信号的置信度。
步骤S1中，UWB信号的置信度计算方法为：
（1）计算测量残差
Figure 758465DEST_PATH_IMAGE001
及其协方差
Figure 770546DEST_PATH_IMAGE002
：
Figure 815862DEST_PATH_IMAGE003
；
Figure 679913DEST_PATH_IMAGE004
式中，
Figure 87760DEST_PATH_IMAGE005
为测量函数，
Figure 379064DEST_PATH_IMAGE006
为状态估计的当前协方差，
Figure 239573DEST_PATH_IMAGE007
为UWB测量值
Figure 907315DEST_PATH_IMAGE008
的协方差，
Figure 169669DEST_PATH_IMAGE009
为观测矩阵；
（2）计算UWB信号的置信度
Figure 631874DEST_PATH_IMAGE010
：
Figure 481144DEST_PATH_IMAGE011
。
步骤S2中，先将GNSS信号的置信度、UWB信号的置信度的实时测量值输入模糊器得到模糊集合，模糊集合包含GNSS置信度隶属度函数和UWB置信度隶属度函数，再将GNSS信号的置信度、UWB信号的置信度的实际值分别映射到GNSS置信度隶属度函数和UWB置信度隶属度函数得到模糊逻辑集合，然后将模糊逻辑集合输入到模糊推理引擎得到模糊判决集合，模糊判决集合包括以下四个不同的集合：解算由GNSS系统得到的观测方程与由INS系统得到的状态方程、解算由UWB系统得到的观测方程与由INS系统得到的状态方程、将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程并解算综合观测方程与由INS系统得到的状态方程、解算INS系统输出结果，再将模糊判决集合整合成一个模糊集，然后输入到解模糊化器得到标量值，通过标量值选择不同定位方式。
步骤S2的第一种定位方式中，基于GNSS系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程。
步骤S2的第二种定位方式中，基于UWB系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程。
步骤S2中，根据INS系统的误差模型建立的状态方程为：
Figure 421418DEST_PATH_IMAGE012
式中，
Figure 538279DEST_PATH_IMAGE013
为第
Figure 436964DEST_PATH_IMAGE014
时刻INS系统的状态量，
Figure 537644DEST_PATH_IMAGE015
为第
Figure 281610DEST_PATH_IMAGE016
时刻INS系统的噪声；
Figure 518556DEST_PATH_IMAGE017
式中，
Figure 322564DEST_PATH_IMAGE018
、
Figure 880846DEST_PATH_IMAGE019
、
Figure 428502DEST_PATH_IMAGE020
、
Figure 192059DEST_PATH_IMAGE021
分别为INS系统解算的载体北向位置误差、东向位置 误差、北向速度误差、东向速度误差；
Figure 557181DEST_PATH_IMAGE022
式中，
Figure 976661DEST_PATH_IMAGE023
为采样时间间隔。
步骤S2的第三种定位方式中，综合观测方程为：
Figure 452642DEST_PATH_IMAGE024
Figure 273967DEST_PATH_IMAGE025
式中，
Figure 544412DEST_PATH_IMAGE026
为第
Figure 716767DEST_PATH_IMAGE027
时刻的综合观测方程，
Figure 232324DEST_PATH_IMAGE028
为GNSS信号的置信度，
Figure 908156DEST_PATH_IMAGE029
为UWB信号的 置信度，
Figure 615081DEST_PATH_IMAGE030
为第
Figure 805891DEST_PATH_IMAGE027
时刻由GNSS系统得到的观测方程，
Figure 499041DEST_PATH_IMAGE031
为第
Figure 154013DEST_PATH_IMAGE032
时刻由UWB系统 得到的观测方程，
Figure 641626DEST_PATH_IMAGE033
为GNSS系统的观测转移矩阵，
Figure 647628DEST_PATH_IMAGE034
为UWB系统的观测转移矩 阵，
Figure 144469DEST_PATH_IMAGE035
为系统在
Figure 420991DEST_PATH_IMAGE027
时刻的状态向量，
Figure 79506DEST_PATH_IMAGE036
为GNSS系统的观测噪声，
Figure 838383DEST_PATH_IMAGE037
为UWB系 统的观测噪声。
步骤S2的第四种定位方式中，对INS系统输出结果进行解算为：利用惯性仪表测量运载平台在惯性空间中的角运动和线运动，用三个加速度计和三个陀螺仪组成惯性测量系统，测量出运载平台在三个轴上的角加速度和加速度，并算出运载平台在任一时刻的速度、姿态数据，从而算出运载平台的最优位置。
与现有技术相比，本发明的有益效果为：
本发明一种面向港区的UWB/INS/GNSS的无缝定位方法中，先根据GNSS信号、UWB信号的情况，分别计算出GNSS信号、UWB信号的置信度，再判断GNSS信号是否可信、UWB信号是否可定位，然后基于两种信号的置信度，对其进行模糊逻辑处理，判断切换哪种定位定姿模式，解决了港区运载平台GNSS信号易受干扰、波动大的问题。因此，本发明可以实现港区的无缝定位，提供稳定的、准确的定位结果。
附图说明
图1是本发明一种面向港区的UWB/INS/GNSS的无缝定位方法的流程图。
图2是本发明中GNSS信号的置信度计算方法的流程图。
图3是本发明中UWB信号的置信度计算方法的流程图。
图4是本发明中GNSS置信度隶属度函数示意图。
图5是本发明中UWB置信度隶属度函数示意图。
具体实施方式
以下结合附图说明和具体实施方式对本发明作进一步详细的说明。
参见图1至图5，一种面向港区的UWB/INS/GNSS的无缝定位方法，该方法包括以下步骤：
S1、对港区内部运载平台上接收器接收到的GNSS信号进行置信度计算、UWB信号进行置信度计算；
S2、对步骤S1中的GNSS信号的置信度与UWB信号的置信度进行分析，并根据分析结果选择以下四种定位方式中的一种；
第一种定位方式：若GNSS信号可信、UWB信号不可定位，则将由GNSS系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第二种定位方式：若GNSS信号不可信、UWB信号可定位，则将由UWB系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第三种定位方式：若GNSS信号可信、UWB信号可定位，则将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程，并将综合观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第四种定位方式：若GNSS信号不可信、UWB信号不可定位，则对INS系统输出结果进行解算，得出运载平台的最优位置、速度、姿态信息。
步骤S1中，GNSS信号的置信度计算方法为：先将港区环境的观测数据根据区域类别赋予不同的值，再将港区内部接收到的GNSS信号的观测数据和港区环境的观测数据与GNSS信号的置信度建立对应关系，建立两组互不相交的数据集，一组作为SVM模型的训练集，另一组作为SVM模型的测试集，然后通过上述数据集训练神经网络，输出GNSS信号的置信度。
步骤S1中，UWB信号的置信度计算方法为：
（1）计算测量残差
Figure 873335DEST_PATH_IMAGE001
及其协方差
Figure 768479DEST_PATH_IMAGE002
：
Figure 332316DEST_PATH_IMAGE003
Figure 312910DEST_PATH_IMAGE004
式中，
Figure 151553DEST_PATH_IMAGE005
为测量函数，
Figure 307728DEST_PATH_IMAGE006
为状态估计的当前协方差，
Figure 199723DEST_PATH_IMAGE007
为UWB测量值
Figure 277400DEST_PATH_IMAGE008
的协方差，
Figure 44368DEST_PATH_IMAGE009
为观测矩阵；
（2）计算UWB信号的置信度
Figure 523891DEST_PATH_IMAGE010
：
Figure 85322DEST_PATH_IMAGE038
同时进行单边假设检验，以确定残差是否可能来自该分布，来自动识别和消除多传感器融合框架中的错误测量：
Figure 650296DEST_PATH_IMAGE039
卡方检验的阈值设置为95%，高于该阈值的
Figure 955375DEST_PATH_IMAGE010
值被标记为异常；
步骤S2中，先将GNSS信号的置信度、UWB信号的置信度的实时测量值输入模糊器得到模糊集合，模糊集合包含GNSS置信度隶属度函数和UWB置信度隶属度函数，再将GNSS信号的置信度、UWB信号的置信度的实际值分别映射到GNSS置信度隶属度函数和UWB置信度隶属度函数得到模糊逻辑集合，然后将模糊逻辑集合输入到模糊推理引擎得到模糊判决集合，模糊判决集合包括以下四个不同的集合：解算由GNSS系统得到的观测方程与由INS系统得到的状态方程、解算由UWB系统得到的观测方程与由INS系统得到的状态方程、将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程并解算综合观测方程与由INS系统得到的状态方程、解算INS系统输出结果，再将模糊判决集合整合成一个模糊集，然后输入到解模糊化器得到标量值，通过标量值选择不同定位方式。
步骤S2的第一种定位方式中，基于GNSS系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程。
步骤S2的第二种定位方式中，基于UWB系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程。
步骤S2中，根据INS系统的误差模型建立的状态方程为：
Figure 289404DEST_PATH_IMAGE012
式中，
Figure 726464DEST_PATH_IMAGE013
为第
Figure 903368DEST_PATH_IMAGE014
时刻INS系统的状态量，
Figure 621925DEST_PATH_IMAGE015
为第
Figure 872778DEST_PATH_IMAGE016
时刻INS系统的噪声；
Figure 776012DEST_PATH_IMAGE017
式中，
Figure 315577DEST_PATH_IMAGE018
、
Figure 962459DEST_PATH_IMAGE019
、
Figure 271081DEST_PATH_IMAGE020
、
Figure 834962DEST_PATH_IMAGE021
分别为INS系统解算的载体北向位置误差、东向位置 误差、北向速度误差、东向速度误差；
Figure 596245DEST_PATH_IMAGE022
式中，
Figure 46818DEST_PATH_IMAGE023
为采样时间间隔。
步骤S2的第三种定位方式中，综合观测方程为：
Figure 944367DEST_PATH_IMAGE024
Figure 189403DEST_PATH_IMAGE025
式中，
Figure 765878DEST_PATH_IMAGE026
为第
Figure 256027DEST_PATH_IMAGE027
时刻的综合观测方程，
Figure 273662DEST_PATH_IMAGE028
为GNSS信号的置信度，
Figure 627283DEST_PATH_IMAGE029
为UWB信号的 置信度，
Figure 487792DEST_PATH_IMAGE030
为第
Figure 889954DEST_PATH_IMAGE027
时刻由GNSS系统得到的观测方程，
Figure 886729DEST_PATH_IMAGE031
为第
Figure 614514DEST_PATH_IMAGE032
时刻由UWB系统 得到的观测方程，
Figure 634422DEST_PATH_IMAGE033
为GNSS系统的观测转移矩阵，
Figure 964909DEST_PATH_IMAGE034
为UWB系统的观测转移矩 阵，
Figure 691557DEST_PATH_IMAGE035
为系统在
Figure 216341DEST_PATH_IMAGE027
时刻的状态向量，
Figure 661229DEST_PATH_IMAGE036
为GNSS系统的观测噪声，
Figure 795407DEST_PATH_IMAGE037
为UWB系 统的观测噪声。
步骤S2的第四种定位方式中，对INS系统输出结果进行解算为：利用惯性仪表测量运载平台在惯性空间中的角运动和线运动，用三个加速度计和三个陀螺仪组成惯性测量系统，测量出运载平台在三个轴上的角加速度和加速度，并算出运载平台在任一时刻的速度、姿态数据，从而算出运载平台的最优位置。
本发明的原理说明如下：
为了保证运载小车在港区高动态复杂环境下定位定姿的高精度高可靠性，根据不同定位区域信号条件设计四种基于UWB/GNSS/INS组合的定位定姿方式，并依靠实时UWB信号、GNSS信号置信度计算推理，实现不同定位方式无缝切换；其中四种不同的组合定位属于异构网络，所以其切换技术属于垂直切换；通过将实时的GNSS置信度和UWB置信度映射到隶属度函数中，得到隶属度的函数值；并根据模糊判决规则进行模糊逻辑判别，实时选择四种组合定位方式中的一种，实现自适应无缝定位模式切换方法。
本设计组建的综合观测方程较为科学，符合GNSS和UWB的定位特性，由于港区存在UWB与GNSS信号交接区，两种信号置信度不是非常高，若采用单一信号与INS进行融合，无法得到精确、可靠的定位结果；利用权重对观测方程进行误差修正，使定位结果有选择性的倾向具有更高置信度信号的参数数据，更好的保障了解算结果的精确性。
实施例：
参见图1，一种面向港区的UWB/INS/GNSS的无缝定位方法，该方法包括以下步骤：
S1、对港区内部运载平台上接收器接收到的GNSS信号进行置信度计算、UWB信号进行置信度计算；
参见图2，GNSS信号的置信度计算方法为：先将港区环境的观测数据根据区域类别赋予不同的值，再将港区内部接收到的GNSS信号的观测数据（信号载噪比、伪距残差、卫星仰角）和港区环境的观测数据（区域类别（货物装卸区、货物运输区、货物存放区）、货物密度等）与GNSS信号的置信度建立对应关系，建立两组互不相交的数据集，一组作为SVM模型的训练集，训练集通过对各类的样本进行训练，可获得GNSS信号置信度的SVM模型及其对每个样本点的识别正确率，该正确率称为训练误差；另一组作为SVM模型的测试集，测试集用于对训练集学习的模型进行测试，结果称为测试误差；训练误差用于判断对GNSS信号置信度所使用训练模型的复杂度是否过低，测试误差反应了模型复杂度是否过高；然后通过上述数据集训练神经网络，输出GNSS信号的置信度，且这个置信度数值经转换后可用于GNSS+UWB+INS组合导航时综合观测方程的权重；GNSS信号的置信度分为0～9，0代表GNSS信号受到了NLOS信号与MI信号的严重干扰，信号载噪比很低且伪距残差绝对值偏大，定位精度不高；9代表GNSS信号接收良好，信号载噪比很好，伪距残差趋近于0且抖动较小，定位准确；基于上述深度神经网络算法，根据GNSS信号情况，计算GNSS信号的置信度，将接收器接收到的GNSS信号分为可信和不可信两种；
参见图3，基于卡尔曼滤波测量残差，比较UWB定位数据与IMU融合定位数据，计算得到UWB信号的置信度；在UWB标签中，同时包含IMU，可以实现UWB-IMU的融合定位；从UWB模块中获取UWB信号的定位数据及UWB与IMU融合之后的定位数据，构成时域离散有序序列，分析UWB定位信息与IMU融合定位信息的测量残差，以此计算UWB信号的置信度；根据UWB信号定位数据与IMU定位数据融合前后的差值大小，计算UWB信号的置信度，从而将接收器接收到的UWB信号分为可定位和不可定位两种；
UWB信号的置信度计算方法为：
（1）计算测量残差
Figure 766774DEST_PATH_IMAGE001
及其协方差
Figure 305203DEST_PATH_IMAGE002
：
Figure 627600DEST_PATH_IMAGE003
Figure 175256DEST_PATH_IMAGE004
式中，
Figure 938813DEST_PATH_IMAGE005
为测量函数，
Figure 539821DEST_PATH_IMAGE006
为状态估计的当前协方差，
Figure 959301DEST_PATH_IMAGE007
为UWB测量值
Figure 435281DEST_PATH_IMAGE008
的协方差，
Figure 256607DEST_PATH_IMAGE009
为观测矩阵；
（2）计算UWB信号的置信度
Figure 464734DEST_PATH_IMAGE010
：
Figure 496144DEST_PATH_IMAGE038
；
由于测量残差
Figure 120023DEST_PATH_IMAGE001
被假定为服从具有协方差
Figure 186068DEST_PATH_IMAGE002
的多元高斯分布，其值的归一化平方 和应根据卡方（
Figure 502780DEST_PATH_IMAGE040
）分布进行分布，其自由度与测量值相同，因此，根据卡尔曼滤波测量残 差
Figure 693590DEST_PATH_IMAGE001
对UWB定位置信度进行计算；UWB信号的置信度分为0～10，0代表UWB信号受到了高处机 械建筑遮挡，产生严重的多径效应，定位精度不高；10代表UWB信号接收良好，定位准确；
S2、对步骤S1中的GNSS信号的置信度与UWB信号的置信度进行分析，并根据分析结果选择以下四种定位方式中的一种；
先将GNSS信号的置信度、UWB信号的置信度的实时测量值输入模糊器得到模糊集合，模糊集合包含GNSS置信度隶属度函数和UWB置信度隶属度函数，再将GNSS信号的置信度、UWB信号的置信度的实际值分别映射到GNSS置信度隶属度函数和UWB置信度隶属度函数得到模糊逻辑集合，然后将模糊逻辑集合输入到模糊推理引擎得到模糊判决集合，模糊判决集合包括以下四个不同的集合：解算由GNSS系统得到的观测方程与由INS系统得到的状态方程、解算由UWB系统得到的观测方程与由INS系统得到的状态方程、将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程并解算综合观测方程与由INS系统得到的状态方程、解算INS系统输出结果，再将模糊判决集合整合成一个模糊集，然后输入到解模糊化器得到标量值，通过标量值选择不同定位方式，实现自适应无缝定位模式切换方法；
如图4所示，在GNSS置信度隶属度函数中，x轴表示基于神经网络算法对GNSS信号置信度判断结果输出0～9置信度，y轴表示不同GNSS置信度的隶属度函数；如图5所示，在UWB置信度隶属度函数中，x轴表示的是UWB置信度0～10，y轴表示的是不同UWB置信度的隶属度函数；同时定义3个模糊集合分别为图中的置信度低、置信度中、置信度高；
第一种定位方式：若GNSS信号可信、UWB信号不可定位，则将由GNSS系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
基于GNSS系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程；
第二种定位方式：若GNSS信号不可信、UWB信号可定位，则将由UWB系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
基于UWB系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程；
根据INS系统的误差模型建立的状态方程为：
Figure 12838DEST_PATH_IMAGE012
式中，
Figure 808756DEST_PATH_IMAGE013
为第
Figure 421003DEST_PATH_IMAGE014
时刻INS系统的状态量，
Figure 302371DEST_PATH_IMAGE015
为第
Figure 330370DEST_PATH_IMAGE016
时刻INS系统的噪声；
Figure 105428DEST_PATH_IMAGE041
式中，
Figure 763943DEST_PATH_IMAGE018
、
Figure 257241DEST_PATH_IMAGE019
、
Figure 292193DEST_PATH_IMAGE020
、
Figure 593861DEST_PATH_IMAGE021
分别为INS系统解算的载体北向位置误差、东向位置 误差、北向速度误差、东向速度误差；
Figure 314955DEST_PATH_IMAGE022
式中，
Figure 905336DEST_PATH_IMAGE023
为采样时间间隔；
第三种定位方式：若GNSS信号可信、UWB信号可定位，则将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程，并将综合观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
以GNSS、UWB定位信号置信度为权重比例，并根据GNSS与UWB置信度的经验分布，推 理确定GNSS与UWB的权重之比为
Figure 603034DEST_PATH_IMAGE042
，修正权重因子以适应实际港区作业环境；
综合观测方程为：
Figure 228050DEST_PATH_IMAGE043
Figure 290684DEST_PATH_IMAGE025
式中，
Figure 492995DEST_PATH_IMAGE026
为第
Figure 135329DEST_PATH_IMAGE027
时刻的综合观测方程，
Figure 473907DEST_PATH_IMAGE028
为GNSS信号的置信度，
Figure 910704DEST_PATH_IMAGE029
为UWB信号的 置信度，
Figure 272415DEST_PATH_IMAGE030
为第
Figure 813380DEST_PATH_IMAGE027
时刻由GNSS系统得到的观测方程，
Figure 412989DEST_PATH_IMAGE031
为第
Figure 145322DEST_PATH_IMAGE032
时刻由UWB系统 得到的观测方程，
Figure 932012DEST_PATH_IMAGE033
为GNSS系统的观测转移矩阵，
Figure 775203DEST_PATH_IMAGE034
为UWB系统的观测转移矩 阵，
Figure 229318DEST_PATH_IMAGE035
为系统在
Figure 804656DEST_PATH_IMAGE027
时刻的状态向量，
Figure 468856DEST_PATH_IMAGE036
为GNSS系统的观测噪声，
Figure 991104DEST_PATH_IMAGE037
为UWB系 统的观测噪声；
第四种定位方式：若GNSS信号不可信、UWB信号不可定位，则对INS系统输出结果进行解算，得出运载平台的最优位置、速度、姿态信息；
对INS系统输出结果进行解算为：利用惯性仪表测量运载平台在惯性空间中的角运动和线运动，用三个加速度计和三个陀螺仪组成惯性测量系统，测量出运载平台在三个轴上的角加速度和加速度，并算出运载平台在任一时刻的速度、姿态数据，从而算出运载平台的最优位置。

Claims (8)

1.一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于，该方法包括以下步骤：
S1、对港区内部运载平台上接收器接收到的GNSS信号进行置信度计算、UWB信号进行置信度计算；
S2、对步骤S1中的GNSS信号的置信度与UWB信号的置信度进行分析，并根据分析结果选择以下四种定位方式中的一种；
第一种定位方式：若GNSS信号可信、UWB信号不可定位，则将由GNSS系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第二种定位方式：若GNSS信号不可信、UWB信号可定位，则将由UWB系统得到的观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
第三种定位方式：若GNSS信号可信、UWB信号可定位，则将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程，并将综合观测方程与由INS系统得到的状态方程输入卡尔曼滤波器进行解算，得出运载平台的最优位置、速度、姿态信息；
综合观测方程为：
Figure FDA0003787785910000011
Figure FDA0003787785910000012
式中，Z(k)为第k时刻的综合观测方程，X为GNSS信号的置信度，L为UWB信号的置信度，XGNSS(k)为第k时刻由GNSS系统得到的观测方程，XUWB(k)为第k时刻由UWB系统得到的观测方程，HGNSS(k)为GNSS系统的观测转移矩阵，HUWB(k)为UWB系统的观测转移矩阵，x(k)为系统在k时刻的状态向量，VGNSS(k)为GNSS系统的观测噪声，VUWB(k)为UWB系统的观测噪声；
第四种定位方式：若GNSS信号不可信、UWB信号不可定位，则对INS系统输出结果进行解算，得出运载平台的最优位置、速度、姿态信息。
2.根据权利要求1所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S1中，GNSS信号的置信度计算方法为：先将港区环境的观测数据根据区域类别赋予不同的值，再将港区内部接收到的GNSS信号的观测数据和港区环境的观测数据与GNSS信号的置信度建立对应关系，建立两组互不相交的数据集，一组作为SVM模型的训练集，另一组作为SVM模型的测试集，然后通过上述数据集训练神经网络，输出GNSS信号的置信度。
3.根据权利要求1所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S1中，UWB信号的置信度计算方法为：
(1)计算测量残差r及其协方差S：
Figure FDA0003787785910000021
S＝HPHT+R
式中，
Figure FDA0003787785910000022
为测量函数，P为状态估计的当前协方差，R为UWB测量值z的协方差，H为观测矩阵；
(2)计算UWB信号的置信度L：
L＝rTS-1r。
4.根据权利要求1所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S2中，先将GNSS信号的置信度、UWB信号的置信度的实时测量值输入模糊器得到模糊集合，模糊集合包含GNSS置信度隶属度函数和UWB置信度隶属度函数，再将GNSS信号的置信度、UWB信号的置信度的实际值分别映射到GNSS置信度隶属度函数和UWB置信度隶属度函数得到模糊逻辑集合，然后将模糊逻辑集合输入到模糊推理引擎得到模糊判决集合，模糊判决集合包括以下四个不同的集合：解算由GNSS系统得到的观测方程与由INS系统得到的状态方程、解算由UWB系统得到的观测方程与由INS系统得到的状态方程、将由GNSS系统得到的观测方程与由UWB系统得到的观测方程进行加权得到综合观测方程并解算综合观测方程与由INS系统得到的状态方程、解算INS系统输出结果，再将模糊判决集合整合成一个模糊集，然后输入到解模糊化器得到标量值，通过标量值选择不同定位方式。
5.根据权利要求1所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S2的第一种定位方式中，基于GNSS系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程。
6.根据权利要求1所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S2的第二种定位方式中，基于UWB系统解算出来的位置和INS系统解算出来的位置的差值建立观测方程；由IMU组成的INS系统算出速度、位置、姿态信息，并进行更新，然后根据INS系统的误差模型建立状态方程。
7.根据权利要求5或6所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S2中，根据INS系统的误差模型建立的状态方程为：
xk＝Φxk-1+ωk-1
式中，xk为第k时刻INS系统的状态量，ωk-1为第k-1时刻INS系统的噪声；
Figure FDA0003787785910000031
式中，ΔPN、ΔPE、ΔVN、ΔVE分别为INS系统解算的载体北向位置误差、东向位置误差、北向速度误差、东向速度误差；
Figure FDA0003787785910000041
式中，T为采样时间间隔。
8.根据权利要求1所述的一种面向港区的UWB/INS/GNSS的无缝定位方法，其特征在于：
步骤S2的第四种定位方式中，对INS系统输出结果进行解算为：利用惯性仪表测量运载平台在惯性空间中的角运动和线运动，用三个加速度计和三个陀螺仪组成惯性测量系统，测量出运载平台在三个轴上的角加速度和加速度，并算出运载平台在任一时刻的速度、姿态数据，从而算出运载平台的最优位置。
CN202210683838.XA 2022-06-17 2022-06-17 面向港区的uwb/ins/gnss的无缝定位方法 Active CN114779307B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210683838.XA CN114779307B (zh) 2022-06-17 2022-06-17 面向港区的uwb/ins/gnss的无缝定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210683838.XA CN114779307B (zh) 2022-06-17 2022-06-17 面向港区的uwb/ins/gnss的无缝定位方法

Publications (2)

Publication Number Publication Date
CN114779307A CN114779307A (zh) 2022-07-22
CN114779307B true CN114779307B (zh) 2022-09-13

Family

ID=82421343

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210683838.XA Active CN114779307B (zh) 2022-06-17 2022-06-17 面向港区的uwb/ins/gnss的无缝定位方法

Country Status (1)

Country Link
CN (1) CN114779307B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116086448B (zh) * 2023-04-12 2023-06-16 成都信息工程大学 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法
CN117148406B (zh) * 2023-10-30 2024-01-30 山东大学 一种室内外无缝弹性融合定位方法、系统、介质及设备

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100532589B1 (ko) * 2003-12-26 2005-12-01 한국전자통신연구원 무선인식/위성측위/관성항법을 결합한 통합 측위 장치 및그 방법
CN109946731B (zh) * 2019-03-06 2022-06-10 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
US10908299B1 (en) * 2019-08-30 2021-02-02 Huawei Technologies Co., Ltd. User equipment positioning apparatus and methods
CN110645979B (zh) * 2019-09-27 2021-09-21 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
CN111175795B (zh) * 2020-01-03 2023-05-26 暨南大学 Gnss/ins组合导航系统的两步抗差滤波方法及系统
CN111651245B (zh) * 2020-04-17 2024-04-16 宁波吉利汽车研究开发有限公司 基于车辆行驶环境切换定位设备的方法、装置、电子设备及存储介质
CN113219506A (zh) * 2021-05-07 2021-08-06 江苏俱为科技有限公司 一种多模融合无缝切换的定位方法
CN114114368A (zh) * 2021-12-10 2022-03-01 中国电信股份有限公司 车辆定位方法、系统、设备及存储介质
CN114440880B (zh) * 2022-01-28 2023-06-13 山东省路桥集团有限公司 基于自适应迭代ekf的施工现场控制点定位方法及系统

Also Published As

Publication number Publication date
CN114779307A (zh) 2022-07-22

Similar Documents

Publication Publication Date Title
CN114779307B (zh) 面向港区的uwb/ins/gnss的无缝定位方法
CN110196443B (zh) 一种飞行器的容错组合导航方法及系统
CN110954132B (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
CA2664994C (en) Hybrid ins/gnss system with integrity monitoring and method for integrity monitoring
CN110823217A (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN110161543B (zh) 一种基于卡方检验的部分粗差抗差自适应滤波方法
Bonnifait et al. Cooperative localization with reliable confidence domains between vehicles sharing GNSS pseudoranges errors with no base station
CN115420284B (zh) 一种组合导航系统故障检测与识别方法
CN114415224A (zh) 一种复杂受限环境中车辆融合定位系统及方法
CN107121684A (zh) 一种基于残差卡方检验法的gps诱骗识别和阈值决策方法
CN109238307B (zh) 一种基于多惯组信息辅助的飞行故障检测方法及装置
Cucci et al. On performance evaluation of inertial navigation systems: the case of stochastic calibration
CN112179347B (zh) 一种基于光谱红移误差观测方程的组合导航方法
CN117785898A (zh) 结合gnss的状态量更新方法、装置、设备和介质
Zhao et al. Intelligent Fault Diagnosis Method for AUV Integrated Navigation System
Xiong et al. Fuzzy adaptive Kalman filter for marine INS/GPS navigation
CN108981689A (zh) 基于dsp tms320c6748的uwb/ins组合导航系统
Rui K-means aided Kalman Filter noise estimation calibration for integrated GPS/INS Navigation
CN115236700A (zh) 基于卫星伪距的因子图导航故障检测方法
CN116592880B (zh) 一种uwb_ins组合定位系统自主完好性检测方法
Wang et al. A Novel Data-driven Adaptive Robust Filter based on TCN-ACKF Method for Autonomous Train Localization System
CN111854728B (zh) 一种基于广义相对熵的容错滤波方法
CN114415223B (zh) 一种优化设备运行轨迹的方法及系统
CN118443027A (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