CN111982102A - 一种复杂环境下基于bp-ekf的uwb-imu定位方法 - Google Patents

一种复杂环境下基于bp-ekf的uwb-imu定位方法 Download PDF

Info

Publication number
CN111982102A
CN111982102A CN202010798618.2A CN202010798618A CN111982102A CN 111982102 A CN111982102 A CN 111982102A CN 202010798618 A CN202010798618 A CN 202010798618A CN 111982102 A CN111982102 A CN 111982102A
Authority
CN
China
Prior art keywords
uwb
ekf
value
positioning
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010798618.2A
Other languages
English (en)
Other versions
CN111982102B (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.)
Tianjin Bintou Xinzhi Technology Co.,Ltd.
Tianjin Chengjian University
Original Assignee
Tianjin Taida Binhai Cleaning Energy Group Co ltd
Tianjin Chengjian 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 Tianjin Taida Binhai Cleaning Energy Group Co ltd, Tianjin Chengjian University filed Critical Tianjin Taida Binhai Cleaning Energy Group Co ltd
Priority to CN202010798618.2A priority Critical patent/CN111982102B/zh
Publication of CN111982102A publication Critical patent/CN111982102A/zh
Application granted granted Critical
Publication of CN111982102B publication Critical patent/CN111982102B/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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/044—Recurrent networks, e.g. Hopfield networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G06N3/084—Backpropagation, e.g. using gradient descent

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • General Health & Medical Sciences (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Biophysics (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Computational Linguistics (AREA)
  • Biomedical Technology (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提供了一种复杂环境下基于BP‑EKF的UWB‑IMU定位方法，该方法有效地将全局定位和局部定位结合起来，采用BP神经网络算法和EKF算法对IMU数据进行处理，并对IMU与UWB数据进行融合，增强在高动态和复杂情况下定位的稳定性和可信度。本发明有益效果：与传统UWB定位方法相比，能有效抑制由于复杂环境影响产生的NLOS对定位估计的影响，提高定位系统的精度和鲁棒性。

Description

一种复杂环境下基于BP-EKF的UWB-IMU定位方法
技术领域
本专利属于定位方法领域，尤其是涉及一种复杂环境下基于BP-EKF的 UWB-IMU定位方法。
背景技术
随着科学技术的发展，定位系统的应用日益广泛，在工厂内、矿井下、 隧道内等领域取得了广泛的应用。相比于空旷的室外场景，室内环境由于建 筑结构复杂、存在各种障碍物，并且环境动态多变，故为复杂环境。在室内 复杂环境下多径效应(Multipatheffects)、非视距传输(None line of sight， NLOS)等时有发生，存在定位误差变大，最终导致定位精度下降，显著增 加室内定位任务的挑战性。超宽带技术(Ultra Wideband，UWB)是一种基 于无线电定位的方法，可获得较高的定位精度，可用于室内复杂环境下的定 位，但是UWB定位过程中所产生的NLOS误差会严重影响系统的位置估计， 存在定位精度低、鲁棒性差的缺点。现有的技术虽然在一定程度上抑制了 NLOS误差，但NLOS误差还是会对定位造成较大的影响，需要能有效抑制 NLOS干扰对定位估计的影响且提高系统的定位精度和鲁棒性的定位方法。
发明内容
为解决上述技术问题，本发明的目的是提供一种复杂环境下基于 BP-EKF的UWB-IMU定位方法。将处理后的惯性测量单元(Inertial Measurement unit，IMU)数据与UWB数据进行融合，有效地将全局定位和 局部定位相结合，采用BP神经网络算法和扩展卡尔曼滤波(Extended kalman Filter，EKF)算法对IMU数据进行处理，通过降低UWB在NLOS误差较大时的权重抑制NLOS误差对定位结果的影响。通过将IMU与UWB相结 合的方式，可以增强定位的准确性，同时通过两者的数据融合可以增强在高 动态和复杂情况下定位的稳定性和可信度。
为实现上述目的，本发明采用的技术方案是：一种复杂环境下基于 BP-EKF的UWB-IMU定位方法，包括如下步骤：
步骤S1、获取IMU位置目标的第一定位数据：
IMU包括加速度计和陀螺仪两部分，设置加速度计、陀螺仪将检测的 速度、角速度发送给IMU位置计算单元，经BP-EKF处理后解算出IMU位 置目标的X、Y、Z三轴空间坐标xu，yu，zu的状态滤波估计值
Figure BDA0002626545420000021
作为 IMU位置目标的第一定位数据；
步骤S2、获取UWB位置目标的第二定位数据：具体为，
步骤S21、建立基于到达时间的UWB定位模型，通过获取每个基站到 位置目标的距离，采用加权最小二乘法解算出对UWB位置目标的X、Y、Z 三轴空间坐标xu，yu，zu的估计值
Figure BDA0002626545420000022
步骤S22、计算NLOS的误差S，并通过NLOS的误差S得到UWB的 方差σ2 2，计算公式为：
σ2 2＝eS-1 (1)
Figure BDA0002626545420000023
其中
Figure BDA0002626545420000024
分别为对位置目标X、Y、Z三轴坐标xu，yu，zu经过UWB 定位方法的加权最小二乘估计后的估计值，
Figure BDA0002626545420000025
分别为IMU经BP-EKF 处理后的位置目标X、Y、Z三轴坐标xu，yu，zu的状态滤波估计值；
步骤S23、进行UWB可信度判断，可信度判断采用的原则如下：设置 UWB的采样间隔，对于UWB接收到的信号进行采样，由于复杂环境下障 碍物的阻挡会出现基站信号丢失现象使UWB信号为0值，视其为不可信信 号，由于物体在是运动的，因此对于UWB不变的值，视其为不可信信号， 同时在信号传播的过程中会出现信号反射导致信号接收延迟现象，对于UWB的突变值，也会视为不可信信号，突变值判断按照如下公式：
|d(k+1)-d(k)|<12.5 (3)
其中d(k)为第k次采样时UWB获得的数据，突变值由多次测量所得；
接着，根据UWB可信度判断结果对UWB的方差σ2 2值进行修正，如 下式所示：
Figure BDA0002626545420000031
其中
Figure BDA0002626545420000032
为UWB的修正后方差值，θ为UWB可信度判断变量；当θ＝0 时，表示此时UWB信号为不可信信号，则将得到的方差σ2 2修正为一个很 大的值，即
Figure BDA0002626545420000033
当θ＝1时，表示此时UWB信号为可信信号，则保持 方差σ2 2不变，即
Figure BDA0002626545420000034
步骤S3、进行数据融合，经过BP-EKF处理之后的IMU定位数据符合 高斯模型
Figure BDA0002626545420000035
UWB定位数据经处理后符合高斯模型
Figure BDA0002626545420000036
进行数据融合得出融合后的高斯模型N(μ,σ2)，计算公式如下：
Figure BDA0002626545420000037
Figure BDA0002626545420000038
其中μ,σ2为融合后的均值和方差，最终定位值由μ值确定。
进一步的，步骤S1所述BP-EKF算法是为减小EKF舍弃Talor展开式 舍弃高次项带来的误差，将BP神经网络与EKF算法相结合构造BP-EKF算 法，该算法采用BP神经网络预测实际值与滤波值的误差从而对EKF误差值 进行补偿，通过这种方法可以有效地减小使用EKF舍弃Talor展开式高次项 带来的误差，增强IMU定位精度。
进一步的，所述BP-EKF算法包括如下两个步骤：
(1)BP神经网络的训练，对于建立的BP神经网络模型，以经过EKF 进行处理得到k(k)、
Figure BDA0002626545420000041
和新息序列作为模型的输入，以
Figure BDA0002626545420000042
作为模型的输出，将这些数据输入到建立的BP神经网络模型 之中进行训练，确定输入层与隐藏层和隐藏层与输出层之间的权值；
(2)算法的实时估计，使用训练完的BP神经网络计算出实际值与滤 波值之间的误差，对于EKF的估计值进行补偿从而获得更加准确的结果。 本发明具有的优点和有益效果是：
本发明提供了一种复杂环境下基于BP-EKF的UWB-IMU定位方法，该 方法有效地将全局定位和局部定位结合起来，采用BP神经网络算法和EKF 算法对IMU数据进行处理,并对IMU与UWB数据进行融合。与传统UWB 定位方法相比，该方法能有效抑制由于复杂环境影响产生的NLOS对定位估 计的影响，提高定位系统的精度和鲁棒性。
附图说明
图1为本发明的IMU与UWB数据融合框图；
图2为本发明的基于到达时间定位原理图；
图3为本发明的BP神经网络结构图；
图4为本发明的BP-EKF算法原理图；
图5为本发明的定位误差(EKF和BP-EKF)图；
图6为本发明的UWB与UWB-IMU数据融合误差图。
具体实施方式
下面结合附图对本发明做详细说明：
如图1所示，本发明通过UWB-IMU在室内的定位方法，将IMU与UWB 相结合的方式，可以增强定位的准确性，同时通过两者的数据融合可以增强 在高动态和复杂情况下定位的稳定性和可信度。具体包括如下步骤：
步骤S1、获取IMU位置目标的第一定位数据：
IMU包括加速度计和陀螺仪两部分，设置加速度计、陀螺仪将检测的 速度、角速度发送给IMU位置计算单元，经BP-EKF处理后解算出IMU位 置目标的X、Y、Z三轴空间坐标xu，yu，zu的状态滤波估计值
Figure BDA0002626545420000051
作为 IMU位置目标的第一定位数据；
步骤S2、获取UWB位置目标的第二定位数据：具体为，
步骤S21、建立基于到达时间的UWB定位模型，通过获取每个基站到 位置目标的距离，采用加权最小二乘法解算出对UWB位置目标的X、Y、Z 三轴空间坐标xu，yu，zu的估计值
Figure BDA0002626545420000052
其中基于到达时间定位原理如图2所示，其中r1、r2、r3分别为三个 基站到位置目标的距离，分别以三个基站为圆心，以基站到标签的距离为 半径，而生成的三个圆形的交点即为对位置目标的定位。
建立基于到达时间的UWB定位模型采用如下方法：假设被测点标签的 坐标为[xu,yu,zu]，设置有N个基站，设其中的第L个基站坐标为[xL,yL,zL]， 该基站所测到标签半径为rL，根据几何意义可得：
(xL-xu)2+(yL-yu)2+(zL-zu)2＝rL 2 (1)
其中L＝1,2,…,N。
令AL＝xL 2+yL 2+zL 2 (2)
Bu＝xu 2+yu 2+zu 2 (3)
za＝[xu,yu,zu,Bu]T (4)
H＝[r1 2-A1,r2 2-A2,L rN 2-AN]T (5)
Figure RE-GDA0002668919760000061
则有：
H＝D*za (7)
其中H、D、za均为构造矩阵，za为构造的包含被测点标签坐标[xu,yu,zu]的 矩阵，故对位置目标的基于到达时间定位转化为对构造矩阵za的求解。
采用加权最小二乘法解算出对UWB位置目标的X、Y、Z三轴空间坐 标xu，yu，zu的估计值
Figure BDA0002626545420000062
即为采用加权最小二乘法得到构造矩阵za近 似估计
Figure BDA0002626545420000063
计算公式为：
Figure BDA0002626545420000064
其中Q0是对角阵：
Q0＝diag(σ1 2,σ2 2,LσN 2) (9)
其中σ1 2,σ2 2,LσN 2分别为各个基站到被测点的UWB测量值的方差。
步骤S22、计算NLOS的误差S，并通过NLOS的误差S得到UWB的 方差σ2 2，计算公式为：
σ2 2＝eS-1 (10)
Figure BDA0002626545420000065
其中
Figure BDA0002626545420000066
分别为对位置目标X、Y、Z三轴坐标xu，yu，zu经过UWB 定位方法的加权最小二乘估计后的估计值，
Figure BDA0002626545420000067
分别为IMU经BP-EKF 处理后的位置目标X、Y、Z三轴坐标xu，yu，zu的状态滤波估计值；
步骤S23、进行UWB可信度判断，可信度判断采用的原则如下：设置UWB的采样间隔，对于UWB接收到的信号进行采样，由于复杂环境下障 碍物的阻挡会出现基站信号丢失现象使UWB信号为0值，视其为不可信信 号，由于物体在是运动的，因此对于UWB不变的值，视其为不可信信号， 同时在信号传播的过程中会出现信号反射导致信号接收延迟现象，对于UWB的突变值，也会视为不可信信号，突变值判断按照如下公式：
|d(k+1)-d(k)|<12.5 (12)
其中d(k)为第k次采样时UWB获得的数据，突变值由多次测量所得；
接着，根据UWB可信度判断结果对UWB的方差σ2 2值进行修正，如 下式所示：
Figure BDA0002626545420000071
其中
Figure BDA0002626545420000072
为UWB的修正后方差值，θ为UWB可信度判断变量；当θ＝0 时，表示此时UWB信号为不可信信号，则将得到的方差σ2 2修正为一个很 大的值，即
Figure BDA0002626545420000073
当θ＝1时，表示此时UWB信号为可信信号，则保持 方差σ2 2不变，即
Figure BDA0002626545420000074
步骤S3、进行数据融合，经过BP-EKF处理之后的IMU定位数据符合 高斯模型
Figure BDA0002626545420000075
UWB定位数据经处理后符合高斯模型
Figure BDA0002626545420000076
进行数据融合得出融合后的高斯模型N(μ,σ2)，计算公式如下：
Figure BDA0002626545420000077
Figure BDA0002626545420000078
其中μ,σ2为融合后的均值和方差，最终定位值由μ值确定。
IMU定位是一种使用广泛的定位方法，IMU包括加速度计和陀螺仪两 部分，加速度计可以直接测得加速度的大小，陀螺仪可以获取角速度，常用 的IMU有捷联式与平台式系统之分，捷联式系统和平台式系统相比，不需 要惯性平台。载体、加速度计以及陀螺仪连接在一块，因此捷联式导航系统 的体积较小，成本较低。但是IMU的定位误差会随着时间不断累积造成定 位结果误差较大，因此如何获得准确的IMU定位数据与UWB融合就显得 尤为重要。
为了获得更加准确地IMU定位数据采用BP-EKF算法对于IMU数据进 行处理。
如图3所示，BP神经网络具有优秀的自学习和自适应的能力，可以通 过一定的学习之后进行预测。BP神经网络是误差反向传播的前向神经网络。 其算法大致分两个阶段：
(1)将样本经由输入层、隐藏层、输出层传播，得到最终输出。
(2)计算目标输出和实际输出差值平方和，若不满足要求则反向传播， 经由输出层、隐藏层来调整权值。当得到目标输出后，判断是否满足终止迭 代要求，如误差达到要求或网络学习次数达到设定的最大次数。当满足要求 便终止计算，否则继续调整权值及阈值进行训练学习。
本文采用输入层有3个神经元、隐含层有4个神经元、输出层有1个神 经元的三层BP神经网络模型来预测实际值与滤波值的误差，补偿因使用 EKF舍弃Talor展开式的高次项而带来的误差。
其中，EKF算法即扩展卡尔曼滤波算法，是从KF算法即卡尔曼滤波算 法推导出来的一种高效的非线性数据处理方法，并将KF的应用范围由线性 拓展到非线性。但其因为舍弃了Talor展开式的高次项不可避免的会引入误 差，影响精度。EKF算法的离散非线性动态方程表示为：
Figure BDA0002626545420000091
其中X(k)为k时刻的状态值，X(k+1)为k+1时刻的状态值，以要求解 的被测点位置坐标为状态值，Z(k)为k时刻IMU的测量值，f[k,X(k)],h[k, X(k)]分别为状态过程与观测过程的非线性函数,W(k)为状态噪声,V(k)为观 测噪声。将非线性函数f[k,X(k)],h[k,X(k)]围绕求解X(k)的滤波估计值
Figure BDA0002626545420000092
做Talor展开并保留一次项，如下：
Figure BDA0002626545420000093
令
Figure BDA0002626545420000094
将非线性问题进行线性化处理后，使 用KF算法进行处理结果迭代如下：
Figure BDA0002626545420000095
P(k+1|k)＝A(k)P(k|k)A(k)T+Q(k+1) (19)
Figure BDA0002626545420000096
Figure BDA0002626545420000097
P(k+1)＝[I-K(k+1)B(k)]×P(k+1|k) (22)
其中Q,R分别为W(k),V(k)的方差阵。
对于BP-EKF算法，使用BP神经网络补偿因使用EKF舍弃Talor展开 式的高次项而带来的误差，增强IMU定位精度。BP-EKF算法结构如图4 所示，包含如下两个步骤：
(1)BP神经网络的训练。对于建立的BP神经网络模型，以经过EKF 进行处理得到k(k)、
Figure BDA0002626545420000101
和新息序列作为模型的输入，以
Figure BDA0002626545420000102
作为模型的输出，将这些数据输入到建立的BP神经网络模型 之中进行训练，确定输入层与隐藏层和隐藏层与输出层之间的权值；
(2)算法的实时估计。使用训练完的BP神经网络计算出实际值与滤 波值之间的误差，对于EKF的估计值进行补偿从而获得更加准确的结果。
为验证BP-EKF对于处理IMU数据的优越性建立地心坐标系其以地球 的中心为原点，以地球中心到北极点方向作为Z轴的正方向，X轴指向赤道 和子午面的交点，Y轴的正方向和X轴、Z轴正方向符合右手系规则。当在 物体运动时若信息采集间隔很短可以视为做具有加速度的直线运动，在k时 刻，在X、Y、Z轴的坐标分别用xI(k)，yI(k)，zI(k)表示，在X、Y、Z轴的 速度分量分别用Vx，Vy，Vz表示，由IMU测得的在X、Y、Z轴的加速度用 Ux(k)，Uy(k)，Uz(k)表示，IMU观测值用Z(k)表示，其为k时刻距离初始值 的距离,则可以将系统如下表示：
Figure BDA0002626545420000103
X(k)＝[xI(k),Vx(k),yI(k),Vy(k),zI(k),Vz(k)]T (24)
U(k)＝[Ux(k),Uy(k),Uz(k)]T (25)
系统的状态方程表示为：
X(k+1)＝ΦX(k)+ΓU(k)+W(k) (26)
其中
Figure BDA0002626545420000111
系统的观测方程表示为：
Figure BDA0002626545420000112
其中W(k)，V(k)分别为过程噪声矩阵与观测噪声矩阵。
仿真过程中使用到的参数如下：初始位置及速度为X(0)＝[-100,2,200, 2,100,2]T，过程噪声方差阵Q＝diag(0.5,1,2)，协方差P(0)＝diag(2,1,1,0. 5,0.5,0.2),观测噪声方差R＝5，数据采集间隔设为1s，采样次数设为60次。
通过仿真可以得到结果如图5所示。分析图5可以得出以下结论。使用 EKF处理IMU信息会因为舍弃Talor展开式高次项不可避免的带来误差导致 在X、Y、Z轴上定位产生误差。使用BP-EKF处理IMU信息可以在一定程 度上减小舍弃Talor展开式高次项带来的误差。
使用TOA测得三维空间的坐标则至少需要设置四个及以上的基站，实 验设置其坐标分别为E1＝[120,300,80],E2＝[90,400,110],E3＝[90,420,140], E4＝[120,300,80]。四个基站分别测得UWB数据随后进行NLOS误差计算及 UWB可信度判断得到UWB的方差随后进行数据融合仿真可得结果如图6 所示。由图6分析可知在环境状况复杂的环境下即使采样的次数比较少也会 产生NLOS误差，产生的NLOS误差会对于定位产生很大的影响,使用改进 的IMU与UWB融合可以有效提升复杂环境下的定位精度。
UWB定位是一种全局定位的方法，IMU是一种局部定位的方法，UWB 位虽然误差较低，但是由于环境的复杂性导致基站无法及时接收到信号产生 NLOS误差,本文提出了一种基于UWB和IMU数据融合的定位方法，该方 法能有效地减少IMU使用EKF带来的因舍弃Talor展开式高次项带来的误 差，能有效抑制UWB的NLOS干扰对定位结果的影响，增加了定位的准确 性和定位系统的鲁棒性，具有一定的应用价值。
以上对本发明的一个实施例进行了详细说明，但所述内容仅为本发明 的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请 范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之 内。

Claims (3)

1.一种复杂环境下基于BP-EKF的UWB-IMU定位方法，包括如下步骤：
步骤S1、获取IMU位置目标的第一定位数据：IMU包括加速度计和陀螺仪两部分，设置加速度计、陀螺仪将检测的速度、角速度发送给IMU位置计算单元，经BP-EKF处理后解算出IMU位置目标的X、Y、Z三轴空间坐标xu，yu，zu的状态滤波估计值
Figure FDA0002626545410000011
作为IMU位置目标的第一定位数据；
步骤S2、获取UWB位置目标的第二定位数据：具体为，
步骤S21、建立基于到达时间的UWB定位模型，通过获取每个基站到位置目标的距离，采用加权最小二乘法解算出对UWB位置目标的X、Y、Z三轴空间坐标xu，yu，zu的估计值
Figure FDA0002626545410000012
步骤S22、计算NLOS的误差S，并通过NLOS的误差S得到UWB的方差σ2 2，计算公式为：
σ2 2＝eS-1 (1)
Figure FDA0002626545410000013
其中
Figure FDA0002626545410000014
分别为对位置目标X、Y、Z三轴坐标xu，yu，zu经过UWB定位方法的加权最小二乘估计后的估计值，
Figure FDA0002626545410000015
分别为IMU经BP-EKF处理后的位置目标X、Y、Z三轴坐标xu，yu，zu的状态滤波估计值；
步骤S23、进行UWB可信度判断，可信度判断采用的原则如下：设置UWB的采样间隔，对于UWB接收到的信号进行采样，由于复杂环境下障碍物的阻挡会出现基站信号丢失现象使UWB信号为0值，视其为不可信信号，由于物体在是运动的，因此对于UWB不变的值，视其为不可信信号，同时在信号传播的过程中会出现信号反射导致信号接收延迟现象，对于UWB的突变值，也会视为不可信信号，突变值判断按照如下公式：
|d(k+1)-d(k)|<12.5 (3)
其中d(k)为第k次采样时UWB获得的数据，突变值由多次测量所得；
接着，根据UWB可信度判断结果对UWB的方差σ2 2值进行修正，如下式所示：
Figure FDA0002626545410000021
其中
Figure FDA0002626545410000022
为UWB的修正后方差值，θ为UWB可信度判断变量；当θ＝0时，表示此时UWB信号为不可信信号，则将得到的方差σ2 2修正为一个很大的值，即
Figure FDA0002626545410000023
当θ＝1时，表示此时UWB信号为可信信号，则保持方差σ2 2不变，即
Figure FDA0002626545410000024
步骤S3、进行数据融合，经过BP-EKF处理之后的IMU定位数据符合高斯模型
Figure FDA0002626545410000025
UWB定位数据经处理后符合高斯模型
Figure FDA0002626545410000026
进行数据融合得出融合后的高斯模型N(μ,σ2)，计算公式如下：
Figure FDA0002626545410000027
Figure FDA0002626545410000028
其中μ,σ2为融合后的均值和方差，最终定位值由μ值确定。
2.根据权利要求1所述的一种复杂环境下基于BP-EKF的UWB-IMU定位方法，其特征在于：步骤S1所述BP-EKF算法是为减小EKF舍弃Talor展开式舍弃高次项带来的误差，将BP神经网络与EKF算法相结合构造BP-EKF算法，该算法采用BP神经网络预测实际值与滤波值的误差从而对EKF误差值进行补偿，通过这种方法可以有效地减小使用EKF舍弃Talor展开式高次项带来的误差，增强IMU定位精度。
3.根据权利要求2所述的一种复杂环境下基于BP-EKF的UWB-IMU定位方法，其特征在于：所述BP-EKF算法包括如下两个步骤：
(1)BP神经网络的训练，对于建立的BP神经网络模型，以经过EKF进行处理得到k(k)、
Figure FDA0002626545410000031
和新息序列作为模型的输入，以
Figure FDA0002626545410000032
作为模型的输出，将这些数据输入到建立的BP神经网络模型之中进行训练，确定输入层与隐藏层和隐藏层与输出层之间的权值；
(2)算法的实时估计，使用训练完的BP神经网络计算出实际值与滤波值之间的误差，对于EKF的估计值进行补偿从而获得更加准确的结果。
CN202010798618.2A 2020-08-11 2020-08-11 一种复杂环境下基于bp-ekf的uwb-imu定位方法 Active CN111982102B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010798618.2A CN111982102B (zh) 2020-08-11 2020-08-11 一种复杂环境下基于bp-ekf的uwb-imu定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010798618.2A CN111982102B (zh) 2020-08-11 2020-08-11 一种复杂环境下基于bp-ekf的uwb-imu定位方法

Publications (2)

Publication Number Publication Date
CN111982102A true CN111982102A (zh) 2020-11-24
CN111982102B CN111982102B (zh) 2022-09-06

Family

ID=73445384

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010798618.2A Active CN111982102B (zh) 2020-08-11 2020-08-11 一种复杂环境下基于bp-ekf的uwb-imu定位方法

Country Status (1)

Country Link
CN (1) CN111982102B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113608166A (zh) * 2021-08-04 2021-11-05 燕山大学 一种基于多源信息融合的动物行为监测方法
CN113899363A (zh) * 2021-09-29 2022-01-07 北京百度网讯科技有限公司 车辆的定位方法、装置及自动驾驶车辆
CN114166221A (zh) * 2021-12-08 2022-03-11 中国矿业大学 动态复杂矿井环境中辅助运输机器人定位方法及系统
CN117213481A (zh) * 2023-11-09 2023-12-12 深圳易行机器人有限公司 一种用于agv的全局定位方法和系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04363688A (ja) * 1991-03-08 1992-12-16 Mitsubishi Electric Corp 経路予測装置
US20060100781A1 (en) * 1999-11-29 2006-05-11 Ching-Fang Lin Self-contained/interruption-free positioning method and system thereof
CN106793077A (zh) * 2017-01-05 2017-05-31 重庆邮电大学 一种自适应室内动态目标的uwb定位方法及系统
CN106802658A (zh) * 2017-03-21 2017-06-06 厦门大学 一种全自动高精度室内快速定位方法
CN108981694A (zh) * 2018-07-18 2018-12-11 兰州交通大学 基于小波神经网络与ekf的姿态解算方法及系统
CN109029435A (zh) * 2018-06-22 2018-12-18 常州大学 提高惯性-地磁组合动态定姿精度的方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04363688A (ja) * 1991-03-08 1992-12-16 Mitsubishi Electric Corp 経路予測装置
US20060100781A1 (en) * 1999-11-29 2006-05-11 Ching-Fang Lin Self-contained/interruption-free positioning method and system thereof
CN106793077A (zh) * 2017-01-05 2017-05-31 重庆邮电大学 一种自适应室内动态目标的uwb定位方法及系统
CN106802658A (zh) * 2017-03-21 2017-06-06 厦门大学 一种全自动高精度室内快速定位方法
CN109029435A (zh) * 2018-06-22 2018-12-18 常州大学 提高惯性-地磁组合动态定姿精度的方法
CN108981694A (zh) * 2018-07-18 2018-12-11 兰州交通大学 基于小波神经网络与ekf的姿态解算方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
朱建斌等: "基于 EKF 的 UWB/IMU 紧组合导航系统的鲁棒性设计", 《计算机仿真》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113608166A (zh) * 2021-08-04 2021-11-05 燕山大学 一种基于多源信息融合的动物行为监测方法
CN113899363A (zh) * 2021-09-29 2022-01-07 北京百度网讯科技有限公司 车辆的定位方法、装置及自动驾驶车辆
US11953609B2 (en) 2021-09-29 2024-04-09 Beijing Baidu Netcom Science Technology Co., Ltd. Vehicle positioning method, apparatus and autonomous driving vehicle
CN114166221A (zh) * 2021-12-08 2022-03-11 中国矿业大学 动态复杂矿井环境中辅助运输机器人定位方法及系统
CN117213481A (zh) * 2023-11-09 2023-12-12 深圳易行机器人有限公司 一种用于agv的全局定位方法和系统
CN117213481B (zh) * 2023-11-09 2024-01-16 深圳易行机器人有限公司 一种用于agv的全局定位方法和系统

Also Published As

Publication number Publication date
CN111982102B (zh) 2022-09-06

Similar Documents

Publication Publication Date Title
CN111982102B (zh) 一种复杂环境下基于bp-ekf的uwb-imu定位方法
CN105589064B (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN109682375B (zh) 一种基于容错决策树的uwb辅助惯性定位方法
CN107659893B (zh) 一种误差补偿方法、装置、电子设备及可读存储介质
CN113074739A (zh) 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN113706612B (zh) 融合uwb和单目视觉slam的煤矿井下车辆定位方法
CN105357754B (zh) 一种基于无线网络的移动节点组合定位方法
CN111970633A (zh) 基于WiFi、蓝牙和步行者行航位推算融合的室内定位方法
CN113324544B (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN116182873B (zh) 室内定位方法、系统及计算机可读介质
CN110986952A (zh) 一种针对行人的高精度抗干扰室内定位方法
Kasmi et al. Algorithms and position optimization for a decentralized localization platform based on resource-constrained devices
CN114545327A (zh) 一种运动状态信息和uwb融合定位方法及定位系统
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
CN206876184U (zh) 一种基于rssi和惯性导航的室内定位装置
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN116380052A (zh) 一种基于uwb融合imu的室内定位方法
CN116482735A (zh) 一种受限空间内外高精度定位方法
Yuan et al. MIAKF: Motion inertia estimated adaptive Kalman filtering for underground mine tunnel positioning
CN113891251B (zh) 一种基于AGV的惯导-WiFi信号融合定位算法
CN113534226B (zh) 一种基于智能手机场景识别的室内外无缝定位算法
CN115540854A (zh) 一种基于uwb辅助的主动定位方法、设备和介质
Yue et al. Research on fusion and location method of indoor targeted spraying robots based on UWB\IMU

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
TR01 Transfer of patent right

Effective date of registration: 20231102

Address after: Room 501-3, Building 6, East Zone, Airport Business Park, No. 80 Huanhe North Road, Tianjin Pilot Free Trade Zone (Airport Economic Zone), Binhai New Area, Tianjin, 300450

Patentee after: Tianjin Bintou Xinzhi Technology Co.,Ltd.

Patentee after: Tianjin Chengjian University

Address before: Room 501-2, building 6, East Airport Business Park, 80 Huanhe North Road, Tianjin pilot free trade zone (Airport Economic Zone), Binhai New Area, Tianjin

Patentee before: TIANJIN TAIDA BINHAI CLEANING ENERGY GROUP CO.,LTD.

Patentee before: Tianjin Chengjian University

TR01 Transfer of patent right