CN116482735A - 一种受限空间内外高精度定位方法 - Google Patents

一种受限空间内外高精度定位方法 Download PDF

Info

Publication number
CN116482735A
CN116482735A CN202310410221.5A CN202310410221A CN116482735A CN 116482735 A CN116482735 A CN 116482735A CN 202310410221 A CN202310410221 A CN 202310410221A CN 116482735 A CN116482735 A CN 116482735A
Authority
CN
China
Prior art keywords
error
matrix
state
positioning
inertial sensor
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
CN202310410221.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.)
Jiangsu University of Technology
Original Assignee
Jiangsu University of Technology
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 Jiangsu University of Technology filed Critical Jiangsu University of Technology
Priority to CN202310410221.5A priority Critical patent/CN116482735A/zh
Publication of CN116482735A publication Critical patent/CN116482735A/zh
Pending legal-status Critical Current

Links

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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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
    • 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/13—Receivers
    • G01S19/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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

Landscapes

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

Abstract

本发明提供了一种受限空间内外高精度定位方法，该方法基于UWB标签、IMU惯性传感器以及BD接收机等硬件设备，通过BP神经网络，融合滤波算法等将全局定位和局部定位相结合，增强在受限空间内外复杂环境下的定位稳定性和精度。本发明与传统的受限空间内定位方法相比，能有效抑制非视距以及多径效应等因素对定位精度的影响。同时，还可以减少定位位置切换过程中的乒乓效应的问题，提高不同区域之间的定位方法的切换速度，实现定位的无缝衔接。

Description

一种受限空间内外高精度定位方法
技术领域
本发明涉及一种受限空间内外高精度定位方法。
背景技术
受限空间内复杂环境下高精度定位问题一直以来都是一个技术难题，受限于非视距因素的影响和多径效应的影响，单一的室内定位方法都存在无法实现高精度定位的问题，如WIFI指纹定位法、RFID定位法等。并且在现有的定位方法中，从受限空间内向受限空间外的定位切换算法，会存在较大的乒乓效应，定位延时过大，如阈值切换法。然而选择将多种传感融合使用机器学习等方法实现室内外定位算法的切换，可以实现受限空间内外的高精度定位。可以大大提高设备在城市内以及峡谷内等空间受限区域的定位精度，在物流以及救援等行业有着广泛的应用前景，为此本发明提供一种受限空间内外高精度定位方法，其采用多传感器融合以及多种算法切换的方式以实现受限空间内外高精度定位以及不同定位算法之间的切换。
发明内容
本发明是为了解决上述现有技术存在的问题而提供一种受限空间内外高精度定位方法。
本发明所采用的技术方案有：
一种受限空间内外高精度定位方法，包括如下步骤：
1)根据能否收到BD卫星信号将空间分为受限空间内区域、受限空间外区域以及受限空间内外交界区域，在受限空间内布局四个UWB基站，将UWB标签、IMU惯性传感器以及BD接收机绑在一起并形成定位标签；
分别在各个区域内测量BD接收机的工作状态，BD接收机接收定位标签所在测量位置的经纬度信息，UWB标签接收四个UWB基站传回来的信号值，将所述信号值作为BP神经网络算法的输入，BD接收机所处实际测量位置作为输出，由此建立BP神经网络训练模型M1；
2)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间内区域时，则通过使用ESKF融合滤波算法融合IMU惯性传感器和UWB标签；
3)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间外区域时，则通过使用卡尔曼滤波算法融合IMU惯性传感器和BD接收机接收；
4)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间内外交界区域时，将BD接收机的工作状态、BD接收机接收定位标签所在测量位置的经纬度信息以及UWB标签接收四个UWB基站传回来的信号值作为BP神经网络算法的输入，定位标签所在测量位置的实际二维坐标作为输出，由此建立BP神经网络训练模型M2；通过BP神经网络训练模型M2实现在判断定位标签处于受限空间内外交界区域时，可自动可以获得定位点的所在坐标；
5)将通过步骤2)-4)获得到的坐标信息通过坐标转换到导航坐标系中，通过串口协议传输至上位机，同时通过物联网模块将坐标信息传输至云端保存，方便存储和信息查询。
进一步地，步骤5)中坐标系转化关系为：通过四元数法将所得到的坐标信息转化至东北天坐标系中，其微分方程如下所示：
其中：
ω＝[ωxωyωz]T，ωx，ωy，ωz分别是载体坐标系下IMU惯性传感器中三轴陀螺仪输出的三轴角速率值；
q＝[q0q1q2q3]T是四元数且规范化的，即
四元数表示的载体坐标系-b系到导航坐标系-e系的旋转矩阵为：
四元数表示三轴陀螺仪的姿态信息横滚角γ、俯仰角θ和偏航角ψ如下:
θ＝asin(2(q0q2-q1q3))
其中，atan2()为反正切值函数,表示角度范围为(-π，π)；
通过旋转变化，可以将载体坐标系-b系下IMU惯性传感器中的三轴加速度计的测量值转化到载体坐标系-e系：
其中：表示载体坐标系-e系下的加速度，/>表示载体坐标系-b系下的加速度，g表示定位标签所处位置的加速度；对加速度ae积分，即得到导航坐标系-e系下的加速度和位置。
进一步地，步骤2)中，ESKF融合滤波算法在使用过程中把UWB标签和IMU惯性传感器的状态量分为三部分：真实状态、名义状态和误差状态，具体过程为：
21)名义状态方程的确立；
对UWB标签测量的伪距进行NLOS环境下的判断与修正，使用IMU惯性传感器测量定位标签的运动状态并进行位置更新，根据定位标签的运动方程与四元数姿态方程，则名义状态方程为：
其中：am、ab分别为定位标签运动时，IMU惯性传感器中三轴加速度计测量的加速度的测量值和名义值；
ωm、ωb分别为IMU惯性传感器中三轴陀螺仪测量角速度的测量值和名义值；
R为IMU惯性传感器到惯性系旋转的转移矩阵；
pk+1为k+1时刻名义位置矩阵；pk为k时刻名义位置矩阵；vk+1为k+1时刻名义矩阵；vk为k时刻名义矩阵；qk+1为k+1时刻姿态四元数矩阵；qk为k时刻姿态四元数矩阵；g为定位标签所处位置的加速度矩阵；
22)误差状态方程的确立
在步骤21)中通过IMU惯性传感器所测量的信息计算出的名义状态未考虑到噪声影响，如果使用该名义状态继续计算会使定位误差越来越大；所以需要通过确立误差状态，再利用误差状态对步骤21)中名义状态的误差进行修改，从而确保定位结果不受误差影响，过程为：
选取位移误差δp、速度误差δv、姿态误差δθ、陀螺仪零偏误差δωb及加速度计零偏误差δab作为误差状态，记作：
δx＝[δpTδvTδθTδωb Tδab T]T
则，IMU惯性传感器的误差方程为：
其中，为地球自转角速度的分量；
此时误差转移矩阵Ft为：
其中，
在IMU惯性传感器使用过程中存在零偏噪声，噪声矩阵W和噪声转移矩阵Bt分别为：
W＝[αωx αωy αωz ωωx ωωy ωωz]T
其中：αωx、αωy、αωz分别为IMU惯性传感器中三轴加速度计在X、Y及Z轴上的噪声；ωωx、ωωy、ωωy分别为IMU惯性传感器中三轴陀螺仪在X、Y以及Z轴上的噪声；
所以，误差状态方程为：
Xk+1|k＝FKXk|k+BKWK+1；
误差状态方程中：
Xk+1|k为通过k时刻状态矩阵得到的k+1时刻状态矩阵估计值；FK为k时刻状态转移矩阵；Xk|k为k时刻后验估计状态矩阵，是上一次滤波的结果；BK为输入到状态转换矩阵；Wk+1为k+1时刻的输入矩阵；
23)UWB融合IMU修正NLOS误差
经过步骤22)修正名义状态的误差之后，可得到定位位置的名义值，此时将各个UWB定位基站所测量得到的伪距值与名义状态中各个UWB定位基站到估计位置的距离的值进行对比，从而计算出预测距离di，pre:
di，pre＝||pt+1，pre-pi||
将UWB标签距离测量值与预测距离公式获得的距离预测值做差，得到误差估计值Δdi:
在NLOS情况下发生多径效应时，信号传播路径增长，因此，发生多径效应时误差估计值Δdi往往为正值；
IMU惯性传感器计算出的名义位置存在误差，当误差估计值在预设范围内时可认为合理，设置阈值γ，建立Huber权重函数ω(Δdi)
其中：γ为考虑到IMU惯性传感器名义位置误差设置的阈值，当误差估计值小于该阈值时，可认为其未发生NLOS，权重为1；当误差估计值大于该阈值时，认为发生NLOS，根据其误差值的大小设置权重，误差越大，权重越小；
根据以上计算出各基站的权重ωi，以此反应UWB各定位基站发生NLOS的程度；
将各UWB定位基站的权重构建权重矩阵W：
使用加权最小二乘法得到更新的坐标P：
P＝(XTWX)-1XTWY
以得到的新位置坐标求出新伪距，重复这个过程，最终收敛至各伪距权重均接近1，得到减小NLOS影响后的伪距；
24)误差状态量测更新
IMU惯性传感器的数据中夹杂大量噪声，需要使用修正后的伪距信息对误差状态进行修正，观测方程的基本形式为：
y＝h(x)+σ2
其中：y为观测数据；h(x)为UWB定位基站的观测函数；σ2为观测噪声的方差；
在ESKF融合滤波算法中，为更新误差状态，需要计算观测函数对于误差状态的雅克比矩阵：
应用链式法则，有：
其中：为观测h(x)的线性展开；
根据式中对状态变量的定义，则可表示为：
其中，
通过计算出的雅克比矩阵进行量测更新，包含卡尔曼增益的计算、误差状态协方差矩阵更新以及误差状态量测更新；
ESKF融合滤波算法中的量测更新方程为：
其中，
Kk+1为k+1时刻的卡尔曼增益；Pk+1|k为k+1时刻的先验估计协方差；H为状态量到测量的转移矩阵；V为测量噪声矩阵；Pk+1|k+1为k+1时刻的后验估计协方差；Yk为观测矩阵；Xk+1|k+1为k+1时刻后验估计状态矩阵；
将量测更新后的误差状态和名义状态合并，得到定位目标的状态信息：
进一步地，步骤3)中，受限空间区域外同时使用IMU惯性传感器和BD接收机接收进行定位，将IMU惯性传感器和BD接收机接收的接收信息通过卡尔曼滤波法进行融合，从而获得最优的位置，通过卡尔曼滤波法进行融合包括预测步和更新步，
预测步为：
X(k|k-1)＝AX(k-1|k-1)+ω
P(k|k-1)＝AP(k-1|k-1)AT+Q
其中，
X(k|k-1)是k时刻的预测值，X(k-1|k-1)是k-1时刻的最优估计值，A为n×n的状态转移矩阵，ω是输入的噪声误差，P(k|k-1)是k时刻的协方差矩阵，P(k-1|k-1)是k-1状态下的校正协方差矩阵，Q为过程噪声协方差；
更新步为：
K(k)＝P(k|k-1)H(k)T(H(k)P(k|k-1)H(k)T+R(k))-1
X(k|k)＝X(k|k-1)+K(k)(Z(k)-H(k)X(k|K-1))
P(k|k)＝(I-K(k)H)P(k|k-1)
其中K是k时刻的卡尔曼增益，H是k时刻的观测方程，R为测量误差方差矩阵，X(k|k)是k时刻的最优估计值，Z(k)是k时刻的测量值，P(k|k)是k时刻的校正协方差矩阵。
本发明具有如下有益效果：
本发明与传统的受限空间内定位方法相比，能有效抑制非视距以及多径效应等因素对定位精度的影响。同时，还可以减少定位位置切换过程中的乒乓效应的问题，提高不同区域之间的定位方法的切换速度，实现定位的无缝衔接，建立受限空间内外完整的定位***，本方法相对现有GNSS信号弱区域定位方法定位精度能够大幅提高，并且能够消除室内多径效应以及非视距因素对定位精度的影响，提高定位算法切换之间的稳定性，为室内高精度定位问题的解决提供了新的思路。
附图说明
图1为本发明实施例的方法流程图
具体实施方式
下面结合附图对本发明作进一步的说明。
如图1所示，本发明一种受限空间内外高精度定位方法，包括如下步骤：
1)选取一块GNSS信号弱的受限空间，根据能否收到BD卫星信号将空间分为受限空间内区域、受限空间外区域以及受限空间内外交界区域，在受限空间内布局四个UWB基站，将UWB标签、IMU惯性传感器以及BD接收机绑在一起并形成定位标签；
分别在各个区域内测量BD接收机的工作状态，BD接收机接收定位标签所在测量位置的经纬度信息，UWB标签接收四个UWB基站传回来的信号值，将所述信号值作为BP神经网络算法的输入，BD接收机所处实际测量位置作为输出，由此建立BP神经网络训练模型M1。
2)当定位标签通过BP神经网络训练模型M1判定是处在受限空间内区域时，则通过使用ESKF融合滤波算法融合IMU惯性传感器和UWB标签，UWB标签在密闭空间内可实现较高的定位精度，但也易受到非视距以及多径效应等因素的影响。为了解决这个问题，本发明使用ESKF融合滤波融合IMU和UWB的方法以提高在受限空间内的定位精度。主要思路为：在获取到IMU惯性传感器和UWB标签的测量数据之后，IMU惯性传感器测量数据在ESKF预测环节进行位置预测，得到预测误差状态和名义状态。将UWB各基站测量的伪距与名义状态中各基站到估计位置的距离进行对比，计算出各基站的权重，以此为初始值对伪距进行迭代重加权最小二乘处理，获取修正伪距。最后以修正伪距对误差状态进行修正，得到最终计算位置。
具体算法分为如下四步：(1)名义状态方程的确立；(2)误差状态方程的确立；(3)UWB融合IMU修正NLOS误差(4)误差状态量测更新。
21)名义状态方程的确立；
对UWB标签测量的伪距进行NLOS环境下的判断与修正，使用IMU惯性传感器测量定位标签的运动状态并进行位置更新，根据定位标签的运动方程与四元数姿态方程，则名义状态方程为：
其中：am、ab分别为定位标签运动时，IMU惯性传感器中三轴加速度计测量的加速度的测量值和名义值；
ωm、ωb分别为IMU惯性传感器中三轴陀螺仪测量角速度的测量值和名义值；
R为IMU惯性传感器到惯性系旋转的转移矩阵；
pk+1为k+1时刻名义位置矩阵；pk为k时刻名义位置矩阵；vk+1为k+1时刻名义矩阵；vk为k时刻名义矩阵；qk+1为k+1时刻姿态四元数矩阵；qk为k时刻姿态四元数矩阵；g为定位标签所处位置的加速度矩阵；
22)误差状态方程的确立
在步骤21)中通过IMU惯性传感器所测量的信息计算出的名义状态未考虑到噪声影响，如果使用该名义状态继续计算会使定位误差越来越大；所以需要通过确立误差状态，再利用误差状态对步骤21)中名义状态的误差进行修改，从而确保定位结果不受误差影响，过程为：
选取位移误差δp、速度误差δv、姿态误差δθ、陀螺仪零偏误差δωb及加速度计零偏误差δab作为误差状态，记作：
δx＝[δpTδvTδθTδωb Tδab T]T
则，IMU惯性传感器的误差方程为：
其中，为地球自转角速度的分量；
此时误差转移矩阵Ft为：
其中，
在IMU惯性传感器使用过程中存在零偏噪声，噪声矩阵W和噪声转移矩阵Bt分别为：
W＝[αωx αωy αωz ωωx ωωy ωωz]T
其中：αωx、αωy、αωz分别为IMU惯性传感器中三轴加速度计在X、Y及Z轴上的噪声；ωωx、ωωy、ωωy分别为IMU惯性传感器中三轴陀螺仪在X、Y以及Z轴上的噪声；
所以，误差状态方程为：
Xk+1|k＝FKXk|k+BKWK+1；
误差状态方程中：
Xk+1|k为通过k时刻状态矩阵得到的k+1时刻状态矩阵估计值；FK为k时刻状态转移矩阵；Kk|k为k时刻后验估计状态矩阵，是上一次滤波的结果；BK为输入到状态转换矩阵；Wk+1为k+1时刻的输入矩阵；
23)UWB融合IMU修正NLOS误差
经过步骤22)修正名义状态的误差之后，可得到定位位置的名义值，此时将各个UWB定位基站所测量得到的伪距值与名义状态中各个UWB定位基站到估计位置的距离的值进行对比，从而计算出预测距离di，pre:
di，pre＝||pt+1，pre-pi||
将UWB标签距离测量值与预测距离公式获得的距离预测值做差，得到误差估计值Δdi:
在NLOS情况下发生多径效应时，信号传播路径增长，因此，发生多径效应时误差估计值Δdi往往为正值；
IMU惯性传感器计算出的名义位置存在误差，当误差估计值在预设范围内时可认为合理，设置阈值γ，建立Huber权重函数ω(Δdi)
其中：γ为考虑到IMU惯性传感器名义位置误差设置的阈值，当误差估计值小于该阈值时，可认为其未发生NLOS，权重为1；当误差估计值大于该阈值时，认为发生NLOS，根据其误差值的大小设置权重，误差越大，权重越小；
根据以上计算出各基站的权重ωi，以此反应UWB各定位基站发生NLOS的程度；
将各UWB定位基站的权重构建权重矩阵W：
使用加权最小二乘法得到更新的坐标P：
P＝(XTWX)-1XTWY
以得到的新位置坐标求出新伪距，重复这个过程，最终收敛至各伪距权重均接近1，得到减小NLOS影响后的伪距；
24)误差状态量测更新
IMU惯性传感器的数据中夹杂大量噪声，需要使用修正后的伪距信息对误差状态进行修正，观测方程的基本形式为：
y＝h(x)+σ2其中：y为观测数据；h(x)为UWB定位基站的观测函数；σ2为观测噪声的方差；
在ESKF融合滤波算法中，为更新误差状态，需要计算观测函数对于误差状态的雅克比矩阵：
应用链式法则，有：
其中：为观测h(x)的线性展开；
根据式中对状态变量的定义，则可表示为：
其中，
通过计算出的雅克比矩阵进行量测更新，包含卡尔曼增益的计算、误差状态协方差矩阵更新以及误差状态量测更新；
ESKF融合滤波算法中的量测更新方程为：
其中，
Kk+1为k+1时刻的卡尔曼增益；Pk+1|k为k+1时刻的先验估计协方差；H为状态量到测量的转移矩阵；V为测量噪声矩阵；Pk+1|k+1为k+1时刻的后验估计协方差；Yk为观测矩阵；Xk+1|k+1为k+1时刻后验估计状态矩阵；
将量测更新后的误差状态和名义状态合并，得到定位目标的状态信息：
3)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间外区域时，此时GNSS信号较好，当定位标签通过模型M1判定处在受限空间外时，此时GNSS信号较好，所以此时使用IMU惯性传感器和BD接收机的融合算法进行定位。通过卡尔曼滤波算法融合IMU惯性传感器和BD接收机的接收值，缓解在过渡过程中产生的乒乓效应。卡尔曼具体推导过程如下：
预测步为：
X(k|k-1)＝AX(k-1|k-1)+ω
P(k|k-1)＝AP(k-1|k-1)AT+Q
其中，
X(k|k-1)是k时刻的预测值，X(k-1|k-1)是k-1时刻的最优估计值，A为n×n的状态转移矩阵，ω是输入的噪声误差，P(k|k-1)是k时刻的协方差矩阵，P(k-1|k-1)是k-1状态下的校正协方差矩阵，Q为过程噪声协方差；
更新步为：
K(k)＝P(k|k-1)H(k)T(H(k)P(k|k-1)H(k)T+R(k))-1
X(k|k)＝X(k|k-1)+K(k)(Z(k)-H(k)X(k|k-1))
P(k|k)＝(I-K(k)H)P(k|k-1)
其中K是k时刻的卡尔曼增益，H是k时刻的观测方程，R为测量误差方差矩阵，X(k|k)是k时刻的最优估计值，Z(k)是k时刻的测量值，P(k|k)是k时刻的校正协方差矩阵。
4)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间内外交界区域时，将BD接收机的工作状态、BD接收机接收定位标签所在测量位置的经纬度信息以及UWB标签接收四个UWB基站传回来的信号值作为BP神经网络算法的输入，定位标签所在测量位置的实际二维坐标作为输出，由此建立BP神经网络训练模型M2；通过BP神经网络训练模型M2实现在判断定位标签处于受限空间内外交界区域时，可自动可以获得定位点的所在坐标；
5)将通过步骤2)-4)获得到的坐标信息通过坐标转换到导航坐标系中，通过串口协议传输至上位机，同时通过物联网模块将坐标信息传输至云端保存，方便存储和信息查询。
本发明是通过四元数法将所得到的坐标信息转化至东北天坐标系中，其微分方程如下所示：
/>
其中：
ω＝[ωxωyωz]T，ωx，ωy，ωz分别是载体坐标系下IMU惯性传感器中三轴陀螺仪输出的三轴角速率值；
q＝[q0q1q2q3]T是四元数且规范化的，即
四元数表示的载体坐标系-b系到导航坐标系-e系的旋转矩阵为：
四元数表示三轴陀螺仪的姿态信息横滚角γ、俯仰角θ和偏航角ψ如下:
θ＝asin(2(q0q2-q1q3))
其中，atan2()为反正切值函数,表示角度范围为(-π，π)；
通过旋转变化，可以将载体坐标系-b系下IMU惯性传感器中的三轴加速度计的测量值转化到载体坐标系-e系：
其中：表示载体坐标系-e系下的加速度，/>表示载体坐标系-b系下的加速度，g表示定位标签所处位置的加速度；对加速度ae积分，即得到导航坐标系-e系下的加速度和位置。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (4)

1.一种受限空间内外高精度定位方法，其特征在于：包括如下步骤：
1)根据能否收到BD卫星信号将空间分为受限空间内区域、受限空间外区域以及受限空间内外交界区域，在受限空间内布局四个UWB基站，将UWB标签、IMU惯性传感器以及BD接收机绑在一起并形成定位标签；
分别在各个区域内测量BD接收机的工作状态，BD接收机接收定位标签所在测量位置的经纬度信息，UWB标签接收四个UWB基站传回来的信号值，将所述信号值作为BP神经网络算法的输入，BD接收机所处实际测量位置作为输出，由此建立BP神经网络训练模型M1；
2)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间内区域时，则通过使用ESKF融合滤波算法融合IMU惯性传感器和UWB标签；
3)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间外区域时，则通过使用卡尔曼滤波算法融合IMU惯性传感器和BD接收机接收；
4)当所述定位标签通过BP神经网络训练模型M1判定是处在受限空间内外交界区域时，将BD接收机的工作状态、BD接收机接收定位标签所在测量位置的经纬度信息以及UWB标签接收四个UWB基站传回来的信号值作为BP神经网络算法的输入，定位标签所在测量位置的实际二维坐标作为输出，由此建立BP神经网络训练模型M2；通过BP神经网络训练模型M2实现在判断定位标签处于受限空间内外交界区域时，可自动可以获得定位点的所在坐标；
5)将通过步骤2)-4)获得到的坐标信息通过坐标转换到导航坐标系中，通过串口协议传输至上位机，同时通过物联网模块将坐标信息传输至云端保存，方便存储和信息查询。
2.如权利要求1所述的受限空间内外高精度定位方法，其特征在于：步骤5)中坐标系转化关系为：通过四元数法将所得到的坐标信息转化至东北天坐标系中，其微分方程如下所示：
其中：
ω＝[ωx ωy ωz]T，ωx，ωy，ωz分别是载体坐标系下IMU惯性传感器中三轴陀螺仪输出的三轴角速率值；
q＝[q0 q1 q2 q3]T是四元数且规范化的，即
四元数表示的载体坐标系-b系到导航坐标系-e系的旋转矩阵为：
四元数表示三轴陀螺仪的姿态信息横滚角γ、俯仰角θ和偏航角ψ如下：
θ＝asin(2(q0q2-q1q3))
其中，atan2()为反正切值函数，表示角度范围为(-π，π)；
通过旋转变化，可以将载体坐标系-b系下IMU惯性传感器中的三轴加速度计的测量值转化到载体坐标系-e系：
其中：表示载体坐标系-e系下的加速度，/>表示载体坐标系-b系下的加速度，g表示定位标签所处位置的加速度；对加速度ae积分，即得到导航坐标系-e系下的加速度和位置。
3.如权利要求2所述的受限空间内外高精度定位方法，其特征在于：步骤2)中，ESKF融合滤波算法在使用过程中把UWB标签和IMU惯性传感器的状态量分为三部分：真实状态、名义状态和误差状态，具体过程为：
21)名义状态方程的确立；
对UWB标签测量的伪距进行NLOS环境下的判断与修正，使用IMU惯性传感器测量定位标签的运动状态并进行位置更新，根据定位标签的运动方程与四元数姿态方程，则名义状态方程为：
其中：am、ab分别为定位标签运动时，IMU惯性传感器中三轴加速度计测量的加速度的测量值和名义值；
ωm、ωb分别为IMU惯性传感器中三轴陀螺仪测量角速度的测量值和名义值；
R为IMU惯性传感器到惯性系旋转的转移矩阵；
pk+1为k+1时刻名义位置矩阵；pk为k时刻名义位置矩阵；vk+1为k+1时刻名义矩阵；vk为k时刻名义矩阵；qk+1为k+1时刻姿态四元数矩阵；qk为k时刻姿态四元数矩阵；g为定位标签所处位置的加速度矩阵；
22)误差状态方程的确立
在步骤21)中通过IMU惯性传感器所测量的信息计算出的名义状态未考虑到噪声影响，如果使用该名义状态继续计算会使定位误差越来越大；所以需要通过确立误差状态，再利用误差状态对步骤21)中名义状态的误差进行修改，从而确保定位结果不受误差影响，过程为：
选取位移误差δp、速度误差δv、姿态误差δθ、陀螺仪零偏误差δωb及加速度计零偏误差δab作为误差状态，记作：
δx＝[δpT δvT δθT δωb T δab T]T
则，IMU惯性传感器的误差方程为：
其中，为地球自转角速度的分量；
此时误差转移矩阵Ft为：
其中，
在IMU惯性传感器使用过程中存在零偏噪声，噪声矩阵W和噪声转移矩阵Bt分别为：
W＝[αωx αωy αωz ωωx ωωy ωωz]T
其中：αωx、αωy、αωz分别为IMU惯性传感器中三轴加速度计在X、Y及Z轴上的噪声；ωωx、ωωy、ωωy分别为IMU惯性传感器中三轴陀螺仪在X、Y以及Z轴上的噪声；
所以，误差状态方程为：
Xk+1|k＝FKXk|k+BKWK+1；
误差状态方程中：
Xk+1|k为通过k时刻状态矩阵得到的k+1时刻状态矩阵估计值；FK为k时刻状态转移矩阵；Xk|k为k时刻后验估计状态矩阵，是上一次滤波的结果；BK为输入到状态转换矩阵；Wk+1为k+1时刻的输入矩阵；
23)UWB融合IMU修正NLOS误差
经过步骤22)修正名义状态的误差之后，可得到定位位置的名义值，此时将各个UWB定位基站所测量得到的伪距值与名义状态中各个UWB定位基站到估计位置的距离的值进行对比，从而计算出预测距离di，pre：
di，pre＝||pt+1，pre-pi||
将UWB标签距离测量值di与预测距离公式获得的距离预测值做差，得到误差估计值Δdi：
在NLOS情况下发生多径效应时，信号传播路径增长，因此，发生多径效应时误差估计值Δdi往往为正值；
IMU惯性传感器计算出的名义位置存在误差，当误差估计值在预设范围内时可认为合理，设置阈值γ，建立Huber权重函数ω(Δdi)
其中：γ为考虑到IMU惯性传感器名义位置误差设置的阈值，当误差估计值小于该阈值时，可认为其未发生NLOS，权重为1；当误差估计值大于该阈值时，认为发生NLOS，根据其误差值的大小设置权重，误差越大，权重越小；
根据以上计算出各基站的权重ωi，以此反应UWB各定位基站发生NLOS的程度；
将各UWB定位基站的权重构建权重矩阵W：
使用加权最小二乘法得到更新的坐标P：
P＝(XTWX)-1XTWY
以得到的新位置坐标求出新伪距，重复这个过程，最终收敛至各伪距权重均接近1，得到减小NLOS影响后的伪距；
24)误差状态量测更新
IMU惯性传感器的数据中夹杂大量噪声，需要使用修正后的伪距信息对误差状态进行修正，观测方程的基本形式为：
y＝h(x)+σ2
其中：y为观测数据；h(x)为UWB定位基站的观测函数；σ2为观测噪声的方差；
在ESKF融合滤波算法中，为更新误差状态，需要计算观测函数对于误差状态的雅克比矩阵：
应用链式法则，有：
其中：为观测h(x)的线性展开；
根据式中对状态变量的定义，则可表示为：
其中，
通过计算出的雅克比矩阵进行量测更新，包含卡尔曼增益的计算、误差状态协方差矩阵更新以及误差状态量测更新；
ESKF融合滤波算法中的量测更新方程为：
其中，
Kk+1为k+1时刻的卡尔曼增益；Pk+1|k为k+1时刻的先验估计协方差；H为状态量到测量的转移矩阵；V为测量噪声矩阵；Pk+1|k+1为k+1时刻的后验估计协方差；Yk为观测矩阵；Xk+1|k+1为k+1时刻后验估计状态矩阵；
将量测更新后的误差状态和名义状态合并，得到定位目标的状态信息：
4.如权利要求3所述的受限空间内外高精度定位方法，其特征在于：步骤3)中，受限空间区域外同时使用IMU惯性传感器和BD接收机接收进行定位，将IMU惯性传感器和BD接收机接收的接收信息通过卡尔曼滤波法进行融合，从而获得最优的位置，通过卡尔曼滤波法进行融合包括预测步和更新步，
预测步为：
X(k|k-1)＝AX(k-1|k-1)+ω
P(k|k-1)＝AP(k-1|k-1)AT+Q
其中，
X(k|k-1)是k时刻的预测值，X(k-1|k-1)是k-1时刻的最优估计值，A为n×n的状态转移矩阵，ω是输入的噪声误差，P(k|k-1)是k时刻的协方差矩阵，P(k-1|k-1)是k-1状态下的校正协方差矩阵，Q为过程噪声协方差；
更新步为：
K(k)＝P(k|k-1)H(k)T(H(k)P(k|k-1)H(k)T+R(k))-1
X(k|k)＝X(k|k-1)+K(k)(Z(k)-H(k)X(k|k-1))
P(k|k)＝(I-K(k)H)P(k|k-1)
其中K是k时刻的卡尔曼增益，H是k时刻的观测方程，R为测量误差方差矩阵，X(k|k)是k时刻的最优估计值，Z(k)是k时刻的测量值，P(k|k)是k时刻的校正协方差矩阵。
CN202310410221.5A 2023-04-17 2023-04-17 一种受限空间内外高精度定位方法 Pending CN116482735A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310410221.5A CN116482735A (zh) 2023-04-17 2023-04-17 一种受限空间内外高精度定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310410221.5A CN116482735A (zh) 2023-04-17 2023-04-17 一种受限空间内外高精度定位方法

Publications (1)

Publication Number Publication Date
CN116482735A true CN116482735A (zh) 2023-07-25

Family

ID=87217124

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310410221.5A Pending CN116482735A (zh) 2023-04-17 2023-04-17 一种受限空间内外高精度定位方法

Country Status (1)

Country Link
CN (1) CN116482735A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117168447A (zh) * 2023-09-04 2023-12-05 北京泛源时空科技有限公司 一种通过高程约束增强的足绑式惯性行人无缝定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117168447A (zh) * 2023-09-04 2023-12-05 北京泛源时空科技有限公司 一种通过高程约束增强的足绑式惯性行人无缝定位方法
CN117168447B (zh) * 2023-09-04 2024-05-14 北京泛源时空科技有限公司 一种通过高程约束增强的足绑式惯性行人无缝定位方法

Similar Documents

Publication Publication Date Title
US20070282565A1 (en) Object locating in restricted environments using personal navigation
CN109807911B (zh) 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法
CN110702091A (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航***及方法
CN113252033A (zh) 基于多传感器融合的定位方法、定位***及机器人
CN113074732A (zh) 一种室内外无缝定位***及其定位方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
Wen et al. 3D LiDAR aided GNSS real-time kinematic positioning
Chu et al. Performance comparison of tight and loose INS-Camera integration
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
Afia et al. A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments
CN117320148A (zh) 多源数据融合定位方法、***、电子设备及存储介质
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN115523920B (zh) 一种基于视觉惯性gnss紧耦合的无缝定位方法
CN115542363B (zh) 一种适用于垂直下视航空吊舱的姿态测量方法
CN116878498A (zh) 一种多频bds/ins组合空中加油相对导航***和方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及***
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN105874352B (zh) 使用旋转半径确定设备与船只之间的错位的方法和装置
CN117346768B (zh) 同时适用于室内外的多传感器融合感知定位方法
CN117289322B (zh) 一种基于imu、gps与uwb的高精度定位算法

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