CN114964221A - 基于rtk/uwb/imu多单元协同室内外定位方法 - Google Patents

基于rtk/uwb/imu多单元协同室内外定位方法 Download PDF

Info

Publication number
CN114964221A
CN114964221A CN202210385742.5A CN202210385742A CN114964221A CN 114964221 A CN114964221 A CN 114964221A CN 202210385742 A CN202210385742 A CN 202210385742A CN 114964221 A CN114964221 A CN 114964221A
Authority
CN
China
Prior art keywords
uwb
unit
imu
rtk
measured
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.)
Pending
Application number
CN202210385742.5A
Other languages
English (en)
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.)
Yanshan University
Original Assignee
Yanshan University
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 Yanshan University filed Critical Yanshan University
Priority to CN202210385742.5A priority Critical patent/CN114964221A/zh
Publication of CN114964221A publication Critical patent/CN114964221A/zh
Pending legal-status Critical Current

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
    • 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
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/04—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing carrier phase data
    • 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/08—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing integrity information, e.g. health of satellites or quality of ephemeris data
    • 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
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/01—Determining conditions which influence positioning, e.g. radio environment, state of motion or energy consumption
    • G01S5/011—Identifying the radio environment
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/01—Determining conditions which influence positioning, e.g. radio environment, state of motion or energy consumption
    • G01S5/012—Identifying whether indoors or outdoors
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • 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
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Abstract

本发明公开了基于RTK/UWB/IMU多单元协同室内外定位方法，属于导航定位技术领域，系统包括基站和多个定位单元；基站包括运行定位监控上位机软件的电脑、通信基站和RTK基站，每个定位单元包括RTK、UWB、IMU和通讯电台；方法包括：通过RTK的观测卫星数判断单元在室内还是在室外，在室外情况下，通过RTK和IMU松组合定位；在室内情况下，通过UWB之间的精度因子判断UWB之间是否能够通讯；若UWB之间能通讯，先结合RTK给同一单元UWB坐标，然后再通过IMU和UWB进行紧组合定位；若UWB之间不能通讯，则通过IMU导航算法刷新坐标。本发明实现了对室内外环境不受约束地进行连续精确定位。

Description

基于RTK/UWB/IMU多单元协同室内外定位方法
技术领域
本发明涉及导航定位技术领域，尤其是基于RTK/UWB/IMU多单元协同室内外定位方法。
背景技术
随着5G技术的发展，高速率、低时延、大连接等特征为定位服务提供了更好的基础条件，也为位置信息等定位服务创造了巨大的市场，如自动驾驶领域就要极高的定位精度。然而最为人所知的GPS定位受环境影响较大，在复杂多样的环境中，定位信号常常会受到遮挡，从而导致可视卫星不足等原因无法满足定位要求，并且目前常见的室内定位方法，如UWB和IMU单独使用无法提供精确的地球坐标或不能满足精确定位要求，为了不受室内外环境约束地进行连续精确定位，考虑协同使用RTK/UWB/IMU多者融合相互补充的方式来进行室内外一体定位。
虽然目前存在多种融合定位方法，但室内外一体定位的便捷性和考虑情况的复杂性还可以进一步改进和完善。现有技术中，中国发明专利CN112946712 A公开了一种基于RTK和UWB的定位系统及方法，提出了用预先搭建UWB基站并设定坐标信的UWB和RTK来进行室外用RTK室内用UWB的室内外定位方式，因为需要提前布置UWB基站而极大的增加了局限性；中国发明专利CN110645979A公开了基于GNSS/INS/UWB组合的室内外无缝定位方法，虽然室外通过GNSS和INS组合提高了定位精度，室内通过UWB原始测量值和INS组合定位的方法，但其同样需要搭建UWB基站，而且如果场景更加复杂，如室内UWB之间存在遮挡，此室内定位方法将失效。
发明内容
本发明需要解决的技术问题是提供一种基于RTK/UWB/IMU多单元协同室内外定位方法，综合考虑对待测单元室外进入室内进行一体式定位方法，通过RTK与UWB的组合消除了需要搭建UWB基站的限制，且对室内情况进行多种情况考虑，更大程度上实现了不受室内外环境约束地进行连续精确定位。
为解决上述技术问题，本发明所采用的技术方案是：
基于RTK/UWB/IMU多单元协同室内外定位方法，定位系统包括基站和多个定位单元，其中基站包括运行定位监控处理器、通信基站和RTK基站，每个定位单元包括RTK、UWB、IMU模块和通讯电台，根据待测单元在室外或室内的各种不同情况，通过多个定位单元相互协同的方式，不间断的给出合适的定位方法；
开始搭载定位单元的定位载体和协同定位载体至少有4个在RTK信号良好的地方，基站和各定位单元上电启动后，根据RTK和UWB信号强弱判断各个定位单元的定位环境，为定位变量赋初值；RTK提供卫星定位信号，UWB运行在测距模型下，提供两两之间的距离信息；
通过RTK的观测卫星数判断单元在室内还是在室外，在室外情况下，通过RTK和IMU松组合定位；在室内情况下，通过UWB之间的精度因子判断UWB之间是否能够通讯；若UWB之间能通讯，先结合RTK给同一单元UWB坐标，然后再通过IMU和UWB进行紧组合定位；若UWB之间不能通讯，则通过IMU导航算法刷新坐标。
本发明技术方案的进一步改进在于：所述定位方法具体包括以下步骤：
步骤1，通过IMU对待测点进行姿态更新，速度更新，位置更新；
步骤2，判断待测单元RTK的观测卫星数，在待测单元RTK的观测卫星数大于4时，即代表待测单元在室外运动的情况，通过待测单元携带的RTK和IMU进行松组合定位，从而获得待测单元的位置信息；当待测单元RTK的观测卫星数小于4时，如待测单元进入室内，即待测单元RTK信号微弱，进入步骤3；
步骤3，判断UWB设备精度因子是否大于预设的精度阈值,若UWB的精度因子大于预设精度阀值，即进入室内的待测单元与任何其他单元的UWB都不能进行通信，则进入步骤4；若UWB的精度因子小于预设精度阀值，即存在以下两种情况：
3.1当室内待测单元的UWB模块与室外单元UWB模块至少存在一个可通讯时，通过室外RTK给绑在一个模块上的UWB位置信息，再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息，然后通过IMU与上述同一室外UWB模块测得距离信息，把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入，最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
3.2当室内待测单元的UWB模块与室外单元UWB模块没有一个可通讯时，若室内待测单元不能和任意室外单元的UWB进行通信，则考虑让多个单元进入室内协同测距，保证该进入室内的协助单元的UWB能够与至少四个室外UWB通信从而得到自身的位置信息，通过该得到位置信息的室内单元的UWB与待测单元的UWB通讯后得到距离信息，再算出IMU与该已知位置的室内单元之间的距离，将两者距离的差值作为卡尔曼滤波的输入，最后把卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
步骤4，通过丢失信号前的位置和IMU导航算法刷新坐标，从而在短时间内得到较为精准的定位信息。
本发明技术方案的进一步改进在于：步骤1中，IMU基于待测点的初始位置、速度和姿态信息，结合加速度计与陀螺仪输出的测量数据，计算每一更新时刻的待测点的位置、速度和姿态信息，计算公式分别如下：
设四元数为：
Q＝q0+q1i+q2j+q3k
其中，i、j、k为虚数，几何意义理解为一种旋转，q0、q1、q2、q3均为实数；
更新微分方程为：
Figure BDA0003593587780000041
Figure BDA0003593587780000042
其中，
Figure BDA0003593587780000043
为姿态角速率和输出角速率，
Figure BDA0003593587780000044
为以四元数表示的姿态矩阵，
Figure BDA0003593587780000045
分别为导航系下地球自转角速率的投影和导航系相对于地球的角速率，M′w(t)是姿态角速率
Figure BDA0003593587780000046
组成的反对称矩阵；
速度更新采用比力方程：
Figure BDA0003593587780000047
其中，
Figure BDA0003593587780000048
为加速度计的输出，
Figure BDA0003593587780000049
为地球自转角速度分量，
Figure BDA00035935877800000410
为运载体相对于地球的运动速度，gn为重力加速度矢量；
通过对速度积分得到待测单元位置的更新：
Figure BDA00035935877800000411
Figure BDA00035935877800000412
Figure BDA00035935877800000413
其中，λ、L、h分别为待测单元的经度、维度和高度信息，RN、RM分别为当地子午面曲线半径和当地卯酉曲面曲率半径，简记
Figure BDA00035935877800000414
为Vn，
Figure BDA00035935877800000415
为Vn分量。
本发明技术方案的进一步改进在于：步骤2中，所述待测单元运动到室外情况下，通过IMU和RTK对待测单元进行松组合定位，从而获得待测单元的位置信息；RTK/IMU松组合定位算法包括：
卡尔曼滤波的系统状态向量由18个状态组成：
Figure BDA0003593587780000051
其中，有位置误差δP＝[δL,δλ,δh]，速度误差δV＝[VE,VN,VU]，姿态角误差Φ＝[ΦE,ΦN,ΦU]，陀螺仪偏差
Figure BDA0003593587780000052
陀螺仪一阶马尔科误差
Figure BDA0003593587780000053
加速度计偏差
Figure BDA0003593587780000054
卡尔曼滤波的系统观测向量为：
Figure BDA0003593587780000055
Figure BDA0003593587780000056
其中，
Figure BDA0003593587780000057
为IMU测得的位置，
Figure BDA0003593587780000058
为RTK测得的位置，
Figure BDA0003593587780000059
为IMU测得的速度，
Figure BDA00035935877800000510
为RTK测得的速度，
Figure BDA00035935877800000511
为高斯白噪声；
H(k)是量测矩阵，公式如下：
Figure BDA00035935877800000512
本发明技术方案的进一步改进在于：步骤3中，所述待测单元运动到室内情况下，且室内待测单元UWB与室外UWB至少存在一个无遮挡时，通过室外RTK给同一单元的UWB位置信息，从而实现UWB和IMU对待测单元进行组合定位，UWB/IMU紧组合定位算法包括：
通过室外RTK给绑在一个模块上的UWB位置信息，再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息，然后通过IMU与上述同一室外UWB模块测得距离信息，把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入，最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
卡尔曼滤波的系统状态向量由15个状态组成：
Figure BDA0003593587780000061
其中，姿态角误差Φ＝[ΦE,ΦN,ΦU]，位置误差δP＝[δL,δλ,δh]，速度误差δV＝[VE,VN,VU]，陀螺仪零偏误差
Figure BDA0003593587780000062
加速度计零偏误差
Figure BDA0003593587780000063
所述卡尔曼滤波的系统观测向量为：
Z＝[Z1 Z2 Z3…Zn]T
Figure BDA0003593587780000064
其中，
Figure BDA0003593587780000065
表示室外RTK测得的坐标即同一单元UWB的坐标，
Figure BDA0003593587780000066
表示当前的IMU位置估计，
Figure BDA0003593587780000067
代表到第i个UWB测量基站的距离，n代表室内UWB与室外UWB能通讯的个数；
系统量测方程为：
Zk+1＝Hk+1Xk+1+Vr,k+1
Hk+1＝[0n×3 JF 0n×3 0n×3 0n×3]
Figure BDA0003593587780000068
其中，Vr代表系统量测噪声，也就是UWB距离测量噪声，(xAi yAi zAi)是RTK测得的同一模块UWB的坐标，(xIMU yIMU zIMU)由IMU解算的标签坐标。
本发明技术方案的进一步改进在于：步骤4中，所述待测单元运动到室内情况下，且待测单元UWB模块与其他UWB模块完全失去通讯功能时，则通过丢失信号前的位置和IMU导航算法刷新坐标，从而在短时间内得到较为精准的定位信息。
由于采用了上述技术方案，本发明取得的技术进步是：
1、本发明考虑了待测单元从室外到室内整个完整的过程。
2、本发明通过RTK与UWB的组合消除了使用UWB之前需要搭建UWB基站的限制，使UWB能够移动而不再是固定的形式，极大的提高了使用UWB进行定位的通用性，更加具有实用性。
3、本发明不依赖于对室内情况进行理想化假设，通过对室内进行多情况考虑，实现了对室内外环境不受约束地进行连续精确定位。
4、本发明采用了多个定位单元协同的方式，增强了在面对复杂情况下对待测单元进行对位的应对能力。
附图说明
图1为本发明实例中所述融合定位单元组成图；
图2为本发明实例中所述融合定位控制方法流程图；
图3为本发明实例中所述定位系统基本单元组成图；
图4为本发明实例中所述UWB间无遮挡时室内定位图；
图5为本发明实例中所述多单元协作的室内定位图；
图6为本发明实例中所述当待测单元无UWB进行通信时，仅靠IMU的定位图。
具体实施方式
下面结合附图及实施例对本发明做进一步详细说明：
如图1、2所示，基于RTK/UWB/IMU多单元协同室内外定位方法，是一种基于RTK/UWB/IMU融合的多定位单元协同的室内外一体定位方法。
如图3所示，基于RTK/UWB/IMU多单元协同的室内外定位系统包括基站和多个定位单元(即移动站)，其中基站包括运行定位监控处理器、通信基站和RTK基站，每个定位单元(即移动站)包括RTK、UWB、IMU模块和通讯电台；待测单元可以是无人机，移动机器人，机械狗等各种移动体，每个待测单元都同时包含RTK、UWB和IMU模块，且把三个模块一起安装在待测单元中心相同位置处，使UWB定位坐标系，IMU传感器测量坐标系原点重合，其中UWB系统采用DWM1000的TOF的双边双向测距算法进行三维定位，选取IMU模块为MPU6050模块。实验场景可以是空旷的厂房、玻璃幕高楼大厦，和某些楼房等。设定观测优先级为：RTK>IMU，先判断待测单元RTK的观测卫星数，在待测单元RTK的观测卫星数大于4时，即在室外情况下，任意一个单元都可以通过RTK和IMU的组合得到较为精准的定位信息。
由于进行三维定位，实验场景可如图4所示，在待测单元RTK的观测卫星数小于4时，即当待测单元进入室内；再判断UWB设备精度因子是否大于预设的精度阈值，若小于则表示室内外UWB能够通信，通过室外RTK给绑定在一个模块上的UWB位置信息，再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息，然后通过IMU与上述同一室外UWB模块测得距离信息，把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入，最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息。
如图5所示，则考虑让多个单元进入室内协同测距，保证该进入室内的协助单元的UWB能够与至少四个室外UWB通信从而得到自身的位置信息，再通过该得到位置信息的室内单元的UWB与待测单元的UWB通讯后经TWR测距算法从而得到距离信息，再算出IMU与该已知位置的室内单元之间的距离，将两者距离的差值作为卡尔曼滤波的输入，最后把卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息。
如图6所示，若判断UWB设备精度因子大于预设的精度阈值，则表示进入室内的待测单元与任何其他单元UWB都不能进行通信，则通过丢失信号前的位置和IMU导航算法刷新坐标，从而在短时间短时内得到较为精准的定位信息。
具体的实施方案步骤如下：
步骤1，通过IMU对待测点进行姿态更新，速度更新，位置更新，包括：
IMU基于待测点的初始位置、速度和姿态信息，结合加速度计与陀螺仪输出的测量数据，计算每一更新时刻的待测点的位置、速度和姿态信息，计算公式分别如下：
设四元数为：
Q＝q0+q1i+q2j+q3k
其中i、j、k为虚数，几何意义可以理解为一种旋转，q0、q1、q2、q3均为实数。
更新微分方程为：
Figure BDA0003593587780000091
Figure BDA0003593587780000092
其中
Figure BDA0003593587780000093
为姿态角速率和输出角速率，
Figure BDA0003593587780000094
为以四元数表示的姿态矩阵，
Figure BDA0003593587780000095
分别为导航系下地球自转角速率的投影和导航系相对于地球的角速率，M′w(t)是姿态角速率
Figure BDA0003593587780000096
组成的反对称矩阵。
速度更新采用比力方程：
Figure BDA0003593587780000097
其中
Figure BDA0003593587780000098
为加速度计的输出，
Figure BDA0003593587780000099
为地球自转角速度分量，
Figure BDA00035935877800000910
为运载体相对于地球的运动速度，gn重力加速度矢量。
通过对速度积分可以得到待测单元位置的更新:
Figure BDA00035935877800000911
Figure BDA00035935877800000912
Figure BDA0003593587780000101
其中，λ、L、h分别为待测单元的经度、维度和高度信息,RN、RM分别为当地子午面曲线半径和当地卯酉曲面曲率半径，简记
Figure BDA0003593587780000102
为Vn，
Figure BDA0003593587780000103
为Vn分量。
步骤2，RTK/IMU松组合定位算法
所述待测单元运动到室外情况下，通过IMU和RTK对待测单元进行松组合定位，从而获得待测单元的位置信息；
所述卡尔曼滤波的系统状态向量由18个状态组成：
Figure BDA0003593587780000104
其中，有位置误差δP＝[δL,δλ,δh]、速度误差(东北天向速度)δV＝[VE,VN,VU]、姿态角误差Φ＝[ΦE,ΦN,ΦU]、陀螺仪偏差
Figure BDA0003593587780000105
陀螺仪一阶马尔科误差
Figure BDA0003593587780000106
加速度计偏差
Figure BDA0003593587780000107
所述卡尔曼滤波的系统观测向量为：
Figure BDA0003593587780000108
Figure BDA0003593587780000109
其中，
Figure BDA00035935877800001010
为IMU测得的位置，
Figure BDA00035935877800001011
为RTK测得的位置，
Figure BDA00035935877800001012
为IMU测得的速度，
Figure BDA00035935877800001013
为RTK测得的速度，
Figure BDA00035935877800001014
为高斯白噪声；
H(k)是量测矩阵，公式如下：
Figure BDA00035935877800001015
步骤3，UWB/IMU紧组合定位算法
所述待测单元运动到室内情况下，且室内待测单元UWB与室外UWB至少存在一个无遮挡时，通过室外RTK给同一单元的UWB位置信息，从而实现UWB和IMU对待测单元进行组合定位，包括：
通过室外RTK给绑在一个模块上的UWB位置信息，再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息，然后通过IMU与上述同一室外UWB模块测得距离信息，把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入，最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
所述卡尔曼滤波的系统状态向量由15个状态组成：
Figure BDA0003593587780000111
其中，姿态角误差Φ＝[ΦE,ΦN,ΦU]，位置误差δP＝[δL,δλ,δh]，速度误差δV＝[VE,VN,VU]，陀螺仪零偏误差
Figure BDA0003593587780000112
加速度计零偏误差
Figure BDA0003593587780000113
所述卡尔曼滤波的系统观测向量为：
Z＝[Z1 Z2 Z3…Zn]T
Figure BDA0003593587780000114
其中，
Figure BDA0003593587780000115
表示室外RTK测得的坐标即同一单元UWB的坐标，
Figure BDA0003593587780000116
表示当前的IMU位置估计，
Figure BDA0003593587780000117
代表到第i个UWB测量基站的距离，n代表室内UWB与室外UWB能通讯的个数；
系统量测方程为：
Zk+1＝Hk+1Xk+1+Vr,k+1
Hk+1＝[0n×3 JF 0n×3 0n×3 0n×3]
Figure BDA0003593587780000121
其中，Vr代表系统量测噪声，也就是UWB距离测量噪声，(xAi yAi zAi)是RTK测得的同一模块UWB的坐标，(xIMU yIMU zIMU)由IMU解算的标签坐标：
步骤4，IMU导航定位算法刷新位置。
所述待测单元运动到室内情况下，且待测单元UWB模块与其他UWB模块完全失去通讯功能时，则通过丢失信号前的位置和IMU导航算法刷新坐标，从而在短时间短时内得到较为精准的定位信息。
本发明实施例讲述了待测单元从室外进入室内的一个完整的过程：
一开始，当待测单元在室外环境时，采用松组合方式进行卡尔曼滤波提高精度，具体如下：在RTK正常工作时，将IMU与其位置、速度之差作为量测信息送入卡尔曼滤波器，然后通过滤波器估计并校正IMU误差。
当待测单元在室外环境时进入室内时，若室内待测单元与室外待测单元至少存在一个能通信时采用紧组合的卡尔曼滤波形式得到待测单元位置信息，具体如下：通过室外RTK给同一单元UWB位置信息，然后用该室外UWB和待测单元UWB进行TWR测距算法后得到距离信息，将UWB之间得到的距离和待测单元IMU与能它通讯的同一室外UWB的距离作差，将得到的差值作为卡尔曼滤波器的量测输入，利用所述卡尔曼滤波器的输出值对IMU的解算结果进行修正。
若当待测单元在室内，且存在待测单元UWB不能和任何室外UWB通讯，则通过自身的IMU积分刷新位置信息。
综上所述，本发明综合考虑对待测单元室外进入室内进行一体式定位方法，目前存在的UWB使用方法均需要提前搭建UWB基站，而该方法通过RTK与UWB的组合消除了需要搭建UWB基站的限制；而且对室内情况进行多种情况讨论，更大程度上实现了不受室内外环境约束地进行连续精确定位。
以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的任何修改、等同替换、改进等，均应涵盖在本发明的保护范围。

Claims (6)

1.基于RTK/UWB/IMU多单元协同室内外定位方法，其特征在于：定位系统包括基站和多个定位单元，其中基站包括运行定位监控处理器、通信基站和RTK基站，每个定位单元包括RTK、UWB、IMU模块和通讯电台，根据待测单元在室外或室内的各种不同情况，通过多个定位单元相互协同的方式，不间断的给出合适的定位方法；
开始搭载定位单元的定位载体和协同定位载体至少有4个在RTK信号良好的地方，基站和各定位单元上电启动后，根据RTK和UWB信号强弱判断各个定位单元的定位环境，为定位变量赋初值；RTK提供卫星定位信号，UWB运行在测距模型下，提供两两之间的距离信息；
通过RTK的观测卫星数判断单元在室内还是在室外，在室外情况下，通过RTK和IMU松组合定位；在室内情况下，通过UWB之间的精度因子判断UWB之间是否能够通讯；若UWB之间能通讯，先结合RTK给同一单元UWB坐标，然后再通过IMU和UWB进行紧组合定位；若UWB之间不能通讯，则通过IMU导航算法刷新坐标。
2.根据权利要求1所述的基于RTK/UWB/IMU多单元协同室内外定位方法，其特征在于：所述定位方法具体包括以下步骤：
步骤1，通过IMU对待测点进行姿态更新，速度更新，位置更新；
步骤2，判断待测单元RTK的观测卫星数，在待测单元RTK的观测卫星数大于4时，即代表待测单元在室外运动的情况，通过待测单元携带的RTK和IMU进行松组合定位，从而获得待测单元的位置信息；当待测单元RTK的观测卫星数小于4时，如待测单元进入室内，即待测单元RTK信号微弱，进入步骤3；
步骤3，判断UWB设备精度因子是否大于预设的精度阈值,若UWB的精度因子大于预设精度阀值，即进入室内的待测单元与任何其他单元的UWB都不能进行通信，则进入步骤4；若UWB的精度因子小于预设精度阀值，即存在以下两种情况：
3.1当室内待测单元的UWB模块与室外单元UWB模块至少存在一个可通讯时，通过室外RTK给绑在一个模块上的UWB位置信息，再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息，然后通过IMU与上述同一室外UWB模块测得距离信息，把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入，最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
3.2当室内待测单元的UWB模块与室外单元UWB模块没有一个可通讯时，若室内待测单元不能和任意室外单元的UWB进行通信，则考虑让多个单元进入室内协同测距，保证该进入室内的协助单元的UWB能够与至少四个室外UWB通信从而得到自身的位置信息，通过该得到位置信息的室内单元的UWB与待测单元的UWB通讯后得到距离信息，再算出IMU与该已知位置的室内单元之间的距离，将两者距离的差值作为卡尔曼滤波的输入，最后把卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
步骤4，通过丢失信号前的位置和IMU导航算法刷新坐标，从而在短时间内得到较为精准的定位信息。
3.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法，其特征在于：步骤1中，IMU基于待测点的初始位置、速度和姿态信息，结合加速度计与陀螺仪输出的测量数据，计算每一更新时刻的待测点的位置、速度和姿态信息，计算公式分别如下：
设四元数为：
Q＝q0+q1i+q2j+q3k
其中，i、j、k为虚数，几何意义理解为一种旋转，q0、q1、q2、q3均为实数；
更新微分方程为：
Figure FDA0003593587770000031
Figure FDA0003593587770000032
其中，
Figure FDA0003593587770000033
为姿态角速率和输出角速率，
Figure FDA0003593587770000034
为以四元数表示的姿态矩阵，
Figure FDA0003593587770000035
分别为导航系下地球自转角速率的投影和导航系相对于地球的角速率，M′w(t)是姿态角速率
Figure FDA0003593587770000036
组成的反对称矩阵；
速度更新采用比力方程：
Figure FDA0003593587770000037
其中，
Figure FDA0003593587770000038
为加速度计的输出，
Figure FDA0003593587770000039
为地球自转角速度分量，
Figure FDA00035935877700000310
为运载体相对于地球的运动速度，gn为重力加速度矢量；
通过对速度积分得到待测单元位置的更新：
Figure FDA00035935877700000311
Figure FDA00035935877700000312
Figure FDA00035935877700000313
其中，λ、L、h分别为待测单元的经度、维度和高度信息，RN、RM分别为当地子午面曲线半径和当地卯酉曲面曲率半径，简记
Figure FDA00035935877700000314
为Vn，
Figure FDA00035935877700000315
为Vn分量。
4.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法，其特征在于：步骤2中，所述待测单元运动到室外情况下，通过IMU和RTK对待测单元进行松组合定位，从而获得待测单元的位置信息；RTK/IMU松组合定位算法包括：
卡尔曼滤波的系统状态向量由18个状态组成：
Figure FDA0003593587770000041
其中，有位置误差δP＝[δL,δλ,δh]，速度误差δV＝[VE,VN,VU]，姿态角误差Φ＝[ΦE,ΦN,ΦU]，陀螺仪偏差
Figure FDA0003593587770000042
陀螺仪一阶马尔科误差
Figure FDA0003593587770000043
加速度计偏差
Figure FDA0003593587770000044
卡尔曼滤波的系统观测向量为：
Figure FDA0003593587770000045
Figure FDA0003593587770000046
其中，
Figure FDA0003593587770000047
为IMU测得的位置，
Figure FDA0003593587770000048
为RTK测得的位置，
Figure FDA0003593587770000049
为IMU测得的速度，
Figure FDA00035935877700000410
为RTK测得的速度，
Figure FDA00035935877700000411
为高斯白噪声；
H(k)是量测矩阵，公式如下：
Figure FDA00035935877700000412
5.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法，其特征在于：步骤3中，所述待测单元运动到室内情况下，且室内待测单元UWB与室外UWB至少存在一个无遮挡时，通过室外RTK给同一单元的UWB位置信息，从而实现UWB和IMU对待测单元进行组合定位，UWB/IMU紧组合定位算法包括：
通过室外RTK给绑在一个模块上的UWB位置信息，再通过该室外UWB与室内待测单元的UWB通讯经TWR算法测得距离信息，然后通过IMU与上述同一室外UWB模块测得距离信息，把两者的测得的距离信息做差后作为卡尔曼滤波器的量测输入，最后用卡尔曼滤波器的输出差值对IMU的位置进行修正从而得到待测单元的位置信息；
卡尔曼滤波的系统状态向量由15个状态组成：
Figure FDA0003593587770000051
其中，姿态角误差Φ＝[ΦE,ΦN,ΦU]，位置误差δP＝[δL,δλ,δh]，速度误差δV＝[VE,VN,VU]，陀螺仪零偏误差
Figure FDA0003593587770000052
加速度计零偏误差
Figure FDA0003593587770000053
所述卡尔曼滤波的系统观测向量为：
Z＝[Z1 Z2 Z3…Zn]T
Figure FDA0003593587770000054
其中，
Figure FDA0003593587770000055
表示室外RTK测得的坐标即同一单元UWB的坐标，
Figure FDA0003593587770000056
表示当前的IMU位置估计，
Figure FDA0003593587770000057
代表到第i个UWB测量基站的距离，n代表室内UWB与室外UWB能通讯的个数；
系统量测方程为：
Zk+1＝Hk+1Xk+1+Vr,k+1
Hk+1＝[0n×3 JF 0n×3 0n×3 0n×3]
Figure FDA0003593587770000058
其中，Vr代表系统量测噪声，也就是UWB距离测量噪声，(xAi yAi zAi)是RTK测得的同一模块UWB的坐标，(xIMU yIMU zIMU)由IMU解算的标签坐标。
6.根据权利要求2所述的基于RTK/UWB/IMU多单元协同室内外定位方法，其特征在于：步骤4中，所述待测单元运动到室内情况下，且待测单元UWB模块与其他UWB模块完全失去通讯功能时，则通过丢失信号前的位置和IMU导航算法刷新坐标，从而在短时间内得到较为精准的定位信息。
CN202210385742.5A 2022-04-13 2022-04-13 基于rtk/uwb/imu多单元协同室内外定位方法 Pending CN114964221A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210385742.5A CN114964221A (zh) 2022-04-13 2022-04-13 基于rtk/uwb/imu多单元协同室内外定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210385742.5A CN114964221A (zh) 2022-04-13 2022-04-13 基于rtk/uwb/imu多单元协同室内外定位方法

Publications (1)

Publication Number Publication Date
CN114964221A true CN114964221A (zh) 2022-08-30

Family

ID=82971255

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210385742.5A Pending CN114964221A (zh) 2022-04-13 2022-04-13 基于rtk/uwb/imu多单元协同室内外定位方法

Country Status (1)

Country Link
CN (1) CN114964221A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116699665A (zh) * 2023-08-08 2023-09-05 山东科技大学 一种适用于海上光伏电厂环境的无人船定位系统及方法

Similar Documents

Publication Publication Date Title
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN110274588B (zh) 基于无人机集群信息的双层嵌套因子图多源融合导航方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN104729497A (zh) 超小型双涵道无人机组合导航系统及双模式导航方法
CN110955261A (zh) 一种基于ros的固定翼无人机自动驾驶仪的仿真方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
US7957898B2 (en) Computational scheme for MEMS inertial navigation system
CN103697889A (zh) 一种基于多模型分布式滤波的无人机自主导航与定位方法
CN105723240A (zh) 用加速度/减速度确定设备和船只间的错位的方法和装置
CN105928515B (zh) 一种无人机导航系统
CN109084760B (zh) 一种楼宇间导航系统
CN113074732A (zh) 一种室内外无缝定位系统及其定位方法
CN111426320A (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN116086448B (zh) 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法
CN111207745A (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
CN111221020A (zh) 一种室内外定位方法、装置及系统
CN114964221A (zh) 基于rtk/uwb/imu多单元协同室内外定位方法
Skobeleva et al. Extended Kalman Filter for indoor and outdoor localization of a wheeled mobile robot
CN109506662B (zh) 一种小天体着陆初始对准方法、其相对导航基准确定方法及装置
CN114554594B (zh) 一种面向搜索救援场景的无人机定位方法及设备
CN113670318A (zh) 协同定位方法及定位系统
Xu et al. Study on extended kalman filtering for attitude estimation of micro flight vehicle
CN105874352B (zh) 使用旋转半径确定设备与船只之间的错位的方法和装置
Ready et al. Inertially aided visual odometry for miniature air vehicles in gps-denied environments
Moussa et al. Investigation of different low-cost land vehicle navigation systems based on CPD sensors and vehicle information

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