CN112073909B - 基于uwb/mems组合的uwb基站位置误差补偿方法 - Google Patents

基于uwb/mems组合的uwb基站位置误差补偿方法 Download PDF

Info

Publication number
CN112073909B
CN112073909B CN202010844868.5A CN202010844868A CN112073909B CN 112073909 B CN112073909 B CN 112073909B CN 202010844868 A CN202010844868 A CN 202010844868A CN 112073909 B CN112073909 B CN 112073909B
Authority
CN
China
Prior art keywords
uwb
base station
state
mems
expressed
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
CN202010844868.5A
Other languages
English (en)
Other versions
CN112073909A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010844868.5A priority Critical patent/CN112073909B/zh
Publication of CN112073909A publication Critical patent/CN112073909A/zh
Application granted granted Critical
Publication of CN112073909B publication Critical patent/CN112073909B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/024—Guidance services
    • 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
    • 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

Landscapes

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

Abstract

基于UWB/MEMS组合的UWB基站位置误差补偿方法,涉及组合导航技术领域，针对现有系统中UWB基站位置测量精度有误差，且易受环境影响，很难精确测量的问题，包括：步骤一：收集UWB系统和MEMS系统输出的数据；步骤二：选择系统的状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；步骤三：利用导数无迹卡尔曼滤波器对UWB/MEMS组合导航系统状态空间模型进行状态估计；步骤四：根据导数无迹卡尔曼滤波器输出的状态估计值得到UWB基站的位置误差和MEMS惯导的陀螺仪常值漂移和加速度计零偏，将其反馈到组合系统中，输出姿态、速度和位置信息。

Description

基于UWB/MEMS组合的UWB基站位置误差补偿方法
技术领域
本发明涉及组合导航技术领域，具体为基于UWB/MEMS组合的UWB基站位置误差补偿方法。
背景技术
在现代化生活中，人们对定位和导航的需求愈加强烈。现如今，全球导航卫星系统(Global Navigation Satellite System，GNSS)飞速发展，其与惯性导航系统(InertialNavigation System，INS)组成的导航系统在精度和可靠性等方面更是得到了极大的提高。然而，在室内环境下，GNSS信号极易受遮挡，使得用户无法通过GNSS实现正常的导航定位。同时，人类绝大部分的活动发生于室内环境下，高精度室内定位服务可以为用户提供路线导航、商品导购、资源引导等网络化服务，从而提升用户在生活服务、安全保障等方面的体验。因此，高精度的室内定位技术亟待发展。
现如今，室内定位技术普遍采用无线定位，应用较广泛的室内定位技术主要有无线局域网(Wireless Local Area Networks，WLAN)、射频识别(Radio FrequenceIdentification， RFID)、超宽带(Ultra-wide Band，UWB)、蓝牙等，其中，UWB具有超大的带宽、良好的抗多径干扰能力、超高的时间分辨率等优点，更适合用于提供室内高精度定位服务。然而，UWB定位精度易受非视距环境的影响。因此，许多学者提出将两种或两种以上的传感器进行融合来弥补单一传感器导航系统的缺点。随着微电子机械系统(Micro ElectroMechanical System，MEMS)技术的兴起和发展，使得传统惯性导航设备在体积、成本等方面得到有效降低。基于UWB/MEMS的组合导航系统能够充分利用各自的优势，提高系统定位精度和稳定性，具有广阔的发展前景。
UWB和MEMS组合常用的方式有松组合和紧组合。松组合是位置组合，其模型简单，易于实现，但组合精度易受NLOS环境影响。紧组合是伪距组合，其不需要解算UWB标签的实时位置信息，对环境的适应性更强，精度更高。无论是哪种组合方式，其一般都是假设UWB基站位置是精确已知的，但在实际系统中其位置测量精度有误差，且易受环境影响，很难精确测量。
发明内容
本发明的目的是：针对现有系统中UWB基站位置测量精度有误差，且易受环境影响，很难精确测量的问题，提出一种基于UWB/MEMS组合的UWB基站位置误差补偿方法。
本发明为了解决上述技术问题采取的技术方案是：
基于UWB/MEMS组合的UWB基站位置误差补偿方法，包括：
步骤一：收集UWB系统和MEMS系统输出的数据；
步骤二：选择系统的状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；
步骤三：利用导数无迹卡尔曼滤波器对UWB/MEMS组合导航系统状态空间模型进行状态估计；
步骤四：根据导数无迹卡尔曼滤波器输出的状态估计值得到UWB基站的位置误差和 MEMS惯导的陀螺仪常值漂移和加速度计零偏，将其反馈到组合系统中，输出姿态、速度和位置信息。
进一步的，所述UWB/MEMS组合导航系统状态空间模型的系统状态方程表示为：
Figure RE-GDA0002782209150000021
即
Xk+1＝FkXk+wk
其中，wk为k时刻组合系统过程噪声，wk的协方差矩阵为Qk，
所述系统状态方程由两部分组成，第一部分为惯导系统的误差方程，惯导系统的状态方程表示为：
XI,k+1＝FI,kXI,k+wI,k
其中，XI,k为15维的惯导解算信息误差，即
Figure RE-GDA0002782209150000023
其中，φk＝[φk,x φk,y φk,z]T为k时刻惯导解算的姿态误差，ΔVk＝[ΔVk,x ΔVk,y ΔVk,z]T为k时刻惯导解算的地理坐标系下的速度误差，ΔPk＝[ΔLk Δλk Δhk]T为k时刻惯导解算的经纬高误差，εk＝[εk,x εk,y εk,z]T为k时刻陀螺常值漂移，
Figure RE-GDA0002782209150000022
为k时刻加速度计零偏，wI,k为k时刻惯导系统过程噪声，且满足E[wI,k]＝0，FI,k为k时刻惯导系统状态转移矩阵；
第二部分为UWB基站位置误差，其状态方程表示为：
XU,k+1＝FU,kXU,k
其中，XU,k为k时刻n个UWB基站在地心地固坐标系下的位置测量误差，表示为
Figure RE-GDA0002782209150000031
其中第i个基站在地心地固坐标系下的位置误差表示为
Figure RE-GDA0002782209150000032
分别为第i个基站的位置在地心地固坐标系x， y，z方向上的误差，FU,k为状态转移矩阵，表示为FU,k＝I3n×3n。
进一步的，所述UWB/MEMS组合导航系统状态空间模型的系统量测方程表示为：
Zk＝h(X'k)+Vk
其中，Zk为系统量测输入，h(X'k)为系统非线性量测函数，Vk为系统量测噪声，其噪声协方差矩阵为Rk，h(X'k)和Vk分别表示为：
Figure RE-GDA0002782209150000033
Vk＝-2ρUi,kε+ε2
其中，X'k包含惯导在地心地固坐标系下的位置误差Δxk、Δyk和各个基站的位置误差，其中Δxk、Δyk与状态量Xk中惯导在地理坐标系下的位置误差ΔLk、Δλk、Δhk的关系表示为：
Δxk＝-(Rn+hk)cosλk sin LkΔLk-(Rn+hk)cos Lk sinλkΔλk+cos Lk cosλkΔhk
Δyk＝-(Rn+hk)sinλk sin LkΔLk+(Rn+hk)cos Lk cosλkΔλk+cos Lk sinλkΔhk
其中，e为地球离心率，Rn为地球子午圈半径。
进一步的，所述步骤三的具体步骤为：
步骤三一：给定系统状态估计值
Figure RE-GDA0002782209150000034
及系统状态估计值协方差
Figure RE-GDA0002782209150000035
步骤三二：由k-1时刻系统状态估计值
Figure RE-GDA0002782209150000036
及状态估计协方差
Figure RE-GDA0002782209150000037
计算k时刻系统状态一步预测值及状态一步预测协方差，k时刻系统状态一步预测值
Figure RE-GDA0002782209150000038
及状态一步预测协方差表示
Figure RE-GDA0002782209150000039
为：
Figure RE-GDA00027822091500000310
Figure RE-GDA00027822091500000311
步骤三三：根据k时刻系统状态一步预测值
Figure RE-GDA0002782209150000041
及状态一步预测协方差
Figure RE-GDA0002782209150000042
选择一组加权Sigma点，选择的Sigma点表示为：
Figure RE-GDA0002782209150000043
步骤三四：将Sigma点经非线性量测方程h(·)进行变换，经过非线性量测方程h(·)变换后的Sigma点表示为：
γi,k/k-1＝h(ξi,k/k-1),i＝0,1,...,2n
量测预测值及其协方差表示为：
Figure RE-GDA0002782209150000044
Figure RE-GDA0002782209150000045
状态预测值与量测预测值间的互协方差表示为：
Figure RE-GDA0002782209150000046
其中，ωi为权值，其表示为
Figure RE-GDA0002782209150000047
步骤三五：确定卡尔曼滤波增益矩阵，表示为：
Figure RE-GDA0002782209150000048
步骤三六：更新状态估计值
Figure RE-GDA0002782209150000049
及其协方差
Figure RE-GDA00027822091500000410
表示为：
Figure RE-GDA00027822091500000411
Figure RE-GDA00027822091500000412
进一步的，所述步骤四中基站的位置误差表示为：
Figure RE-GDA00027822091500000413
Figure RE-GDA0002782209150000051
为k时刻滤波器估计的第i个基站的位置误差。
进一步的，所述基站的位置表示为：
Figure RE-GDA0002782209150000052
其中，
Figure RE-GDA0002782209150000053
为k+1时刻测量的第i个基站的位置，
Figure RE-GDA0002782209150000054
为k+1时刻补偿后的第i个基站的位置。
进一步的，所述步骤四中陀螺仪常值漂移和加速度计零偏表示为：
Figure RE-GDA0002782209150000055
Figure RE-GDA0002782209150000056
其中，Gk+1＝[xG,k+1,yG,k+1,zG,k+1]为k+1时刻陀螺仪的测量值，Ak+1＝[xA,k+1,yA,k+1,zA,k+1]为 k+1时刻加速度计的测量值，
Figure RE-GDA0002782209150000057
为k+1时刻陀螺仪补偿后的输出值，
Figure RE-GDA0002782209150000058
为k+1时刻加速度计补偿后的输出值。
本发明的有益效果是：
(1)本发明根据UWB和MEMS惯导定位原理，建立了UWB/MEMS紧组合数学模型，该模型比常规的UWB/MEMS松组合模型定位精度更高，对环境适应能力更强。
(2)本发明将UWB基站位置误差引入系统的状态变量中，通过信息融合滤波算法进行预估和补偿，减小基站位置误差对组合系统定位精度的影响，测量精确。
(3)本发明根据组合系统状态方程和量测方程不同的数学特性，使用导数无迹卡尔曼作为系统数据融合滤波器，避免了状态方程的冗余计算，从而在保证系统定位精度的同时减小组合滤波的计算量。
附图说明
图1为本发明基于UWB/MEMS组合的UWB基站位置误差补偿方法原理图；
图2为本发明设置的仿真运动轨迹；
图3为本发明基站误差补偿算法与UWB、MEMS、UWB/MEMS松组合、UWB/MEMS 紧组合算法解算的载体轨迹对比图；
图4(a)为姿态角误差对比图1；
图4(b)为姿态角误差对比图2；
图4(c)为姿态角误差对比图3；
图5(a)为速度误差对比图1；
图5(b)为速度误差对比图2；
图6(a)为位置误差对比图1；
图6(b)为位置误差对比图2；
图7(a)为姿态角误差对比图1；
图7(b)为姿态角误差对比图2；
图7(c)为姿态角误差对比图3；
图8(a)为速度误差对比图1；
图8(b)为速度误差对比图2；
图9(a)为位置误差对比图1；
图9(b)为位置误差对比图2；
图10(a)为位置误差对比图1；
图10(b)为位置误差对比图2；
图10(c)为位置误差对比图3；
图11(a)为基站存在误差情况下本发明基站误差补偿算法估计出的基站A1的位置误差图1；
图11(b)为基站存在误差情况下本发明基站误差补偿算法估计出的基站A1的位置误差图2；
图12(a)为基站存在误差情况下本发明基站误差补偿算法估计出的基站A1的位置误差图3；
图12(b)为基站存在误差情况下本发明基站误差补偿算法估计出的基站A1的位置误差图4。
具体实施方式
具体实施方式一：参照图1具体说明本实施方式，本发明为基于UWB/MEMS组合的UWB基站位置误差补偿方法，流程图如图1所示，包括以下几个步骤：
(1)收集UWB系统和MEMS系统输出的数据；
(2)选择状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；
(3)UWB/MEMS组合系统滤波初始化；
(4)时间更新。由k-1时刻状态估计值及协方差计算k时刻系统状态预测值及协方差；
(5)Sigma点选择。根据状态预测值及其协方差选择一组加权Sigma点；
(6)量测更新。利用经过非线性量测方程变换后的Sigma点进行量测更新，得到k时刻的状态估计值及其协方差，完成对系统的状态估计；
(7)将滤波输出的误差信息反馈补偿到UWB系统和MEMS系统，输出高精度姿态、速度、位置信息。
本发明基于UWB/MEMS组合的UWB基站位置误差补偿方法还包括：
建立基站位置误差补偿的UWB/MEMS紧组合系统模型
(1)系统状态方程：
Figure RE-GDA0002782209150000071
即
Xk+1＝FkXk+wk
其中，wk为k时刻组合系统过程噪声，其协方差矩阵为Qk。
系统状态方程由两部分组成，第一部分是惯导系统的误差方程，与松组合模式一样，其状态方程为：
XI,k+1＝FI,kXI,k+wI,k
其中，XI,k为15维的惯导解算信息误差，其可写成
Figure RE-GDA0002782209150000075
其中，φk＝[φk,x φk,y φk,z]T为k时刻惯导解算的姿态误差，ΔVk＝[ΔVk,x ΔVk,y ΔVk,z]T为k 时刻惯导解算的地理坐标系下的速度误差，ΔPk＝[ΔLk Δλk Δhk]T为k时刻惯导解算的经纬高误差，εk＝[εk,x εk,y εk,z]T为k时刻陀螺常值漂移，
Figure RE-GDA0002782209150000072
为k时刻加速度计零偏。wI,k为k时刻惯导系统过程噪声，且满足E[wI,k]＝0。FI,k为k时刻惯导系统状态转移矩阵。
第二部分为UWB基站位置误差，其状态方程可以表示为：
XU,k+1＝FU,kXU,k
其中，XU,k为k时刻n个UWB基站在地心地固坐标系下的位置测量误差，其可写成
Figure RE-GDA0002782209150000073
其中第i个基站在地心地固坐标系下的位置误差可以表示为
Figure RE-GDA0002782209150000074
FU,k为状态转移矩阵，其可以表示为FU,k＝I3n×3n。
(2)系统量测数学模型推导：
在地心地固坐标系中，假设k时刻载体的真实位置为(xk,yk)，惯导解算得到的载体的位置为(xI,k,yI,k)，第i个基站Si的真实位置为
Figure RE-GDA0002782209150000081
测量得到的基站Si的位置为
Figure RE-GDA0002782209150000082
(此处简写均省略Z轴)，因此由MEMS惯导解算所得到的载体到基站Si之间的伪距ρIi,k为：
Figure RE-GDA0002782209150000083
载体到基站S的真实距离ρ为
Figure RE-GDA0002782209150000084
两式相减，得
Figure RE-GDA0002782209150000085
由于惯导解算的位置(xI,k,yI,k)存在误差，其与真实值(xk,yk)之间的关系为：
xk＝xI,k-Δxk
yk＝yI,k-Δyk
其中，Δxk和Δyk为k时刻MEMS惯导解算的载体位置误差。同样，假设
Figure RE-GDA0002782209150000086
和
Figure RE-GDA0002782209150000087
为k时刻基站Si位置测量值
Figure RE-GDA0002782209150000088
与真实值
Figure RE-GDA0002782209150000089
之间的偏差，则两者之间的关系可以表示为：
Figure RE-GDA00027822091500000810
Figure RE-GDA00027822091500000811
整理可得
Figure RE-GDA00027822091500000812
假设k时刻UWB测量的载体到基站Si的距离为ρUi,k，其与载体到基站Si的真实距离ρi,k之间的关系可表示为：
ρUi,k＝ρi,k+ε
其中，ε为UWB测量噪声。整理可得系统量测模型：
Figure RE-GDA0002782209150000091
(3)系统量测方程：
Zk＝h(X'k)+Vk
其中，Zk为系统量测输入，h(X'k)为系统非线性量测函数，Vk为系统量测噪声，其噪声协方差矩阵为Rk，h(X'k)和Vk分别可以表示为：
Figure RE-GDA0002782209150000092
其中，X'k包含惯导在地心地固坐标系下的位置误差Δxk、Δyk和各个基站的位置误差，其中Δxk、Δyk与状态量Xk中惯导在地理坐标系下的位置误差ΔLk、Δλk、Δhk的关系可以表示为：
Δxk＝-(Rn+hk)cosλk sinLkΔLk-(Rn+hk)cos Lk sinλkΔλk+cos Lk cosλkΔhk
Δyk＝-(Rn+hk)sinλk sinLkΔLk+(Rn+hk)cos Lk cosλkΔλk+cos Lk sinλkΔhk
其中，e为地球离心率，Rn为地球子午圈半径。
2.导数无迹卡尔曼滤波(Derivative Unscented Kalman Filter，DUKF)算法
根据系统状态方程和量测方程可以看出，系统的状态方程是线性的，而量测方程是非线性的，传统的卡尔曼滤波器(KF,kalman filter)将无法满足系统滤波要求。因此，本文针对系统状态方程和量测方程不同的数学特性，以及系统维数高这一特点，采用导数无迹卡尔曼(DUKF,derivative unscented kalman filter)作为系统信息融合滤波器，即对系统的状态方程使用传统的KF，而对非线性的量测方程使用UKF进行数据融合。
假设非线性离散时间系统为：
Figure RE-GDA0002782209150000093
其中，xk∈Rn和zk∈Rm分别为k时刻统的状态向量和量测向量，Φk/k-1为离散型状态转移矩阵，h(·)为描述量测模型的非线性函数，wk和vk为互不相关的零均值高斯白噪声，其方差为：
Figure RE-GDA0002782209150000101
根据DUKF滤波算法，进行系统状态更新和量测更新得到载体的导航信息，具体步骤包括：
(1)给定状态估计值
Figure RE-GDA0002782209150000102
及其协方差
Figure RE-GDA0002782209150000103
(2)时间更新。由于系统状态方程为线性，状态预测值
Figure RE-GDA0002782209150000104
及其协方差
Figure RE-GDA0002782209150000105
的可表示为：
Figure RE-GDA0002782209150000106
Figure RE-GDA0002782209150000107
(3)Sigma点选择。根据状态预测值及其协方差选择一组加权Sigma点，选择的Sigma 点为：
Figure RE-GDA0002782209150000108
(4)量测更新。经过非线性量测方程h(·)变换后的Sigma点为：
γi,k/k-1＝h(ξi,k/k-1),i＝0,1,...,2n
计算量测预测值及其协方差：
Figure RE-GDA0002782209150000109
Figure RE-GDA00027822091500001010
计算状态预测值与量测预测值间的互协方差：
Figure RE-GDA00027822091500001011
其中，ωi为权值，其可表述为
Figure RE-GDA00027822091500001012
(5)确定卡尔曼滤波增益矩阵：
Figure RE-GDA0002782209150000111
(6)更新状态估计值
Figure RE-GDA0002782209150000112
及其协方差
Figure RE-GDA0002782209150000113
Figure RE-GDA0002782209150000114
Figure RE-GDA0002782209150000115
从DUKF的算法步骤可以看出，在时间更新过程中，它与线性KF具有相同的形式，可以简洁有效地计算状态预测值及其协方差，避免了标准UKF因UT变换导致的额外计算；在量测更新过程中，DUKF选择一组加权Sigma点捕获状态预测值及其方差的信息，然后根据所选择的Sigma点和经过非线性量测方程变换后的Sigma点的统计信息，更新状态估计值及其协方差，继承了标准UKF处理非线性滤波问题的优良特性。
本发明提出了一种基站位置误差补偿的UWB/MEMS紧组合算法，该方法将基站位置误差引入系统的状态方程中，并通过信息融合滤波算法进行预估和补偿，从而克服传统UWB/MEMS组合导航系统需要高精度基站位置信息的缺点。并且，本发明根据组合系统状态方程和量测方程不同的数学特性，使用导数无迹卡尔曼作为组合系统数据融合滤波器，避免了状态方程的冗余计算，减小了组合滤波的计算量。
本发明针对现有UWB/MEMS组合算法需要高精度基站位置信息的问题，将UWB基站位置误差引入系统的状态变量中，通过信息融合滤波算法进行预估和补偿，减小基站位置误差对组合系统定位精度的影响。同时，本发明根据组合系统状态方程和量测方程不同的数学特性，使用导数无迹卡尔曼作为系统数据融合滤波器，避免了状态方程的冗余计算，从而在保证系统定位精度的同时减小组合滤波的计算量。所以本发明利用MATLAB仿真软件进行仿真实验，在基站位置无误差和存在误差的情况下，将本发明的基站误差算法与现有的UWB/MEMS松组合算法和紧组合算法进行比较。
本发明实施例用来解释本发明，而不是对本发明进行限制，在发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。
下面结合具体事例进行仿真分析：
传感器参数设置为：陀螺仪的常值漂移和随机噪声分别为10°/h和5°/h，加速度计的常值漂移和随机噪声分别为500ug和70ug，陀螺仪和加速度计的输出频率均为100Hz；UWB标签测距的随机漂移为0.3m，UWB系统的输出频率为10Hz。UWB基站含误差时，设置各基站位置存在1m的偏差。载体运动设置为：假设物体M在一个二维平面上做圆周运动，水平方向(x轴)上做匀加速直线运动，垂直方向(y轴)上也做匀加速直线运动，运动速度为1m/s，运动两周，总的运动时长为280s。4个基站布置在运动轨迹四周，图2 位载体运动轨迹和基站位置分布。
图3为本发明基站误差补偿算法与UWB、MEMS、UWB/MEMS松组合、UWB/MEMS 紧组合算法解算的载体轨迹对比图，从图中可以看出，MEMS解算轨迹较为平滑稳定，但是随着时间推移惯导误差逐渐累加导致定位结果偏离参考轨迹。与MEMS惯导对比，UWB 定位精度较高，但是UWB测距存在噪声扰动，导致UWB定位轨迹有较大的波动。相较于 MEMS和UWB，UWB/MEMS组合系统继承了MEMS惯导和UWB的优点，因此解算结果与参考轨迹更为接近，稳定性更好。
图4-图6为基站无误差情况下本发明基站误差补偿算法与UWB/MEMS松组合、 UWB/MEMS紧组合算法滤波输出误差对比图，从误差图中可以看出，UWB/MEMS紧组合和本发明所提算法的解算结果误差轨迹基本相同，两者输出的姿态、速度和位置信息比松组合更快趋近理论参考值，收敛性更好。如图4(a)、图4(b)、图4(c)、图5(a)、图5 (b)、图6(a)和图6(b)所示。
表1为基站无误差情况下本发明基站误差补偿算法与UWB/MEMS松组合、 UWB/MEMS紧组合算法滤波解算的姿态、速度、位置的均方根误差，从表1中可以看出， UWB/MEMS紧组合定位系统输出的姿态、速度、位置信息精度最高，而本文所提算法解算精度和紧组合也较为接近。
表1
Figure RE-GDA0002782209150000121
图7-图9为基站存在误差情况下本发明基站误差补偿算法与UWB/MEMS松组合、UWB/MEMS紧组合算法滤波输出误差对比图，从误差图中可以看出，当由于测量导致各基站位置存在1m的偏差时，如果不对基站的位置误差进行估计和补偿，UWB/MEMS松组合和紧组合解算输出的载体纬度误差均在0.6m左右，精度误差在1.5m左右。而本发明所提的算法可以对各基站的位置误差进行准确估计，从图中可以看出，基站位置误差补偿后的载体纬度误差为0.1m，经度误差为0.12m。因此，本发明所提算法能准确估计基站位置误差并进行补偿，能有效提高载体的定位精度。如图7(a)、图7(b)、图7(c)、图8(a)、图8(b)、图9(a)和图9(b)所示。
图11为基站存在误差情况下本发明基站误差补偿算法估计出的基站A1的位置误差，从图中可以看出，本发明所提算法对基站位置误差能够进行很好的预估，然后反馈回组合系统进行误差补偿，达到提高组合定位精度的效果。如图11(a)和图11(b)所示。
表2为基站存在误差情况下本发明基站误差补偿算法与UWB/MEMS松组合、 UWB/MEMS紧组合算法滤波解算的姿态、速度、位置的均方根误差。从表2中可以看出，基站误差补偿算法在位置精度上有很大的提高。
表2
Figure RE-GDA0002782209150000131
表3
Figure RE-GDA0002782209150000132
综上，本发明基于UWB/MEMS组合的UWB基站位置误差补偿方法，定位精度高，且系统鲁棒性强，方法简单，易于工程实现。
需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。

Claims (3)

1.基于UWB/MEMS组合的UWB基站位置误差补偿方法，其特征在于包括：
步骤一：收集UWB系统和MEMS系统输出的数据；
步骤二：选择系统的状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；
步骤三：利用导数无迹卡尔曼滤波器对UWB/MEMS组合导航系统状态空间模型进行状态估计；
步骤四：根据导数无迹卡尔曼滤波器输出的状态估计值得到UWB基站的位置误差和MEMS惯导的陀螺仪常值漂移和加速度计零偏，将其反馈到组合系统中，输出姿态、速度和位置信息；
所述UWB/MEMS组合导航系统状态空间模型的系统状态方程表示为：
Figure FDA0003594494080000011
即
Xk+1＝FkXk+wk
其中，wk为k时刻组合系统过程噪声，wk的协方差矩阵为Qk，
所述系统状态方程由两部分组成，第一部分为惯导系统的误差方程，惯导系统的状态方程表示为：
XI,k+1＝FI,kXI,k+wI,k
其中，XI,k为15维的惯导解算信息误差，即
Figure FDA0003594494080000012
其中，φk＝[φk,x φk,y φk,z]T为k时刻惯导解算的姿态误差，ΔVk＝[ΔVk,x ΔVk,y ΔVk,z]T为k时刻惯导解算的地理坐标系下的速度误差，ΔPk＝[ΔLk Δλk Δhk]T为k时刻惯导解算的经纬高误差，εk＝[εk,x εk,y εk,z]T为k时刻陀螺常值漂移，
Figure FDA0003594494080000013
为k时刻加速度计零偏，wI,k为k时刻惯导系统过程噪声，且满足E[wI,k]＝0，FI,k为k时刻惯导系统状态转移矩阵；
第二部分为UWB基站位置误差，其状态方程表示为：
XU,k+1＝FU,kXU,k
其中，XU,k为k时刻n个UWB基站在地心地固坐标系下的位置测量误差，表示为
Figure FDA0003594494080000014
其中第i个基站在地心地固坐标系下的位置误差表示为
Figure FDA0003594494080000021
Figure FDA0003594494080000022
分别为第i个基站的位置在地心地固坐标系x，y，z方向上的误差，FU,k为状态转移矩阵，表示为FU,k＝I3n×3n；
所述UWB/MEMS组合导航系统状态空间模型的系统量测方程表示为：
Zk＝h(X'k)+Vk
其中，Zk为系统量测输入，h(X'k)为系统非线性量测函数，Vk为系统量测噪声，其噪声协方差矩阵为Rk，h(X'k)和Vk分别表示为：
Figure FDA0003594494080000023
Vk＝-2ρUi,kε+ε2
其中，X'k包含惯导在地心地固坐标系下的位置误差Δxk、Δyk和各个基站的位置误差，ε为UWB测量噪声，ρUi,k为k时刻UWB测量的载体到基站Si的距离，其中Δxk、Δyk与状态量Xk中惯导在地理坐标系下的位置误差ΔLk、Δλk、Δhk的关系表示为：
Δxk＝-(Rn+hk)cosλksinLkΔLk-(Rn+hk)cosLksinλkΔλk+cosLkcosλkΔhk
Δyk＝-(Rn+hk)sinλksinLkΔLk+(Rn+hk)cosLkcosλkΔλk+cosLksinλkΔhk
其中，e为地球离心率，Rn为地球子午圈半径；
所述步骤三的具体步骤为：
步骤三一：给定系统状态估计值
Figure FDA0003594494080000024
及系统状态估计值协方差
Figure FDA0003594494080000025
步骤三二：由k-1时刻系统状态估计值
Figure FDA0003594494080000026
及状态估计协方差
Figure FDA0003594494080000027
计算k时刻系统状态一步预测值及状态一步预测协方差，k时刻系统状态一步预测值
Figure FDA0003594494080000028
及状态一步预测协方差表示
Figure FDA0003594494080000029
为：
Figure FDA00035944940800000210
Figure FDA00035944940800000211
其中，Φk/k-1为离散型状态转移矩阵，Qk为k时刻组合系统协方差矩阵；
步骤三三：根据k时刻系统状态一步预测值
Figure FDA00035944940800000212
及状态一步预测协方差
Figure FDA00035944940800000213
选择一组加权Sigma点，选择的Sigma点表示为：
Figure FDA0003594494080000031
步骤三四：将Sigma点经非线性量测方程h(·)进行变换，经过非线性量测方程h(·)变换后的Sigma点表示为：
γi,k/k-1＝h(ξi,k/k-1),i＝0,1,...,2n
量测预测值及其协方差表示为：
Figure FDA0003594494080000032
Figure FDA0003594494080000033
状态预测值与量测预测值间的互协方差表示为：
Figure FDA0003594494080000034
其中，ωi为权值，其表示为
Figure FDA0003594494080000035
步骤三五：确定卡尔曼滤波增益矩阵，表示为：
Figure FDA0003594494080000036
步骤三六：更新状态估计值
Figure FDA0003594494080000037
及其协方差
Figure FDA0003594494080000038
表示为：
Figure FDA0003594494080000039
Figure FDA00035944940800000310
所述步骤四中基站的位置误差表示为：
Figure FDA00035944940800000311
Figure FDA00035944940800000312
为k时刻滤波器估计的第i个基站的位置误差。
2.根据权利要求1所述的基于UWB/MEMS组合的UWB基站位置误差补偿方法，其特征在于所述基站的位置表示为：
Figure FDA0003594494080000041
其中，
Figure FDA0003594494080000042
为k+1时刻测量的第i个基站的位置，
Figure FDA0003594494080000043
为k+1时刻补偿后的第i个基站的位置。
3.根据权利要求2所述的基于UWB/MEMS组合的UWB基站位置误差补偿方法，其特征在于所述步骤四中陀螺仪常值漂移和加速度计零偏表示为：
Figure FDA0003594494080000044
Figure FDA0003594494080000045
其中，Gk+1＝[xG,k+1,yG,k+1,zG,k+1]为k+1时刻陀螺仪的测量值，Ak+1＝[xA,k+1,yA,k+1,zA,k+1]为k+1时刻加速度计的测量值，
Figure FDA0003594494080000046
为k+1时刻陀螺仪补偿后的输出值，
Figure FDA0003594494080000047
为k+1时刻加速度计补偿后的输出值。
CN202010844868.5A 2020-08-20 2020-08-20 基于uwb/mems组合的uwb基站位置误差补偿方法 Active CN112073909B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010844868.5A CN112073909B (zh) 2020-08-20 2020-08-20 基于uwb/mems组合的uwb基站位置误差补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010844868.5A CN112073909B (zh) 2020-08-20 2020-08-20 基于uwb/mems组合的uwb基站位置误差补偿方法

Publications (2)

Publication Number Publication Date
CN112073909A CN112073909A (zh) 2020-12-11
CN112073909B true CN112073909B (zh) 2022-05-24

Family

ID=73662558

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010844868.5A Active CN112073909B (zh) 2020-08-20 2020-08-20 基于uwb/mems组合的uwb基站位置误差补偿方法

Country Status (1)

Country Link
CN (1) CN112073909B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112822632B (zh) * 2021-02-06 2022-04-12 湖南科天健光电技术有限公司 动态姿态位置补偿方法、系统、电子设备及介质
CN113091770B (zh) * 2021-04-02 2023-12-15 上海有个机器人有限公司 惯性测量传感器的零偏的补偿方法
CN113120713B (zh) * 2021-04-28 2023-09-08 上海有个机器人有限公司 电梯定位方法
CN113514797B (zh) * 2021-07-09 2023-08-08 中国人民解放军战略支援部队信息工程大学 一种uwb基站的自动标定方法
CN113709662B (zh) * 2021-08-05 2023-12-01 北京理工大学重庆创新中心 一种基于超宽带的自主式三维反演定位方法
CN114501300A (zh) * 2021-12-16 2022-05-13 江苏集萃未来城市应用技术研究所有限公司 一种基于空间环境误差模型的分布式定位算法
CN114710748B (zh) * 2022-02-24 2024-05-14 智己汽车科技有限公司 Uwb和ble组合定位控制方法及设备
CN115186715B (zh) * 2022-07-20 2023-07-28 哈尔滨工业大学 一种基于状态空间模型的机电定位系统贝叶斯辨识方法
CN115339488B (zh) * 2022-08-19 2023-08-22 中国人民解放军国防科技大学 基于ldv、uwb、mems组合的列车定位终端
CN116448106B (zh) * 2023-05-24 2024-05-03 中铁第四勘察设计院集团有限公司 一种基于uwb/sins组合系统的狭长环境定位方法及装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108844543A (zh) * 2018-06-08 2018-11-20 南通大学 基于uwb定位及航位推算的室内agv导航控制方法
CN109916410A (zh) * 2019-03-25 2019-06-21 南京理工大学 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016183812A1 (zh) * 2015-05-20 2016-11-24 北京诺亦腾科技有限公司 一种混合运动捕捉系统及方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108844543A (zh) * 2018-06-08 2018-11-20 南通大学 基于uwb定位及航位推算的室内agv导航控制方法
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN109916410A (zh) * 2019-03-25 2019-06-21 南京理工大学 一种基于改进平方根无迹卡尔曼滤波的室内定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
UWB/INS联合室内定位方法研究;李月;《中国优秀硕士学位论文全文数据库》;20190831;全文 *
UWB/MEMS室内定位技术研究;朱彩杰;《中国优秀硕士学位论文全文数据库》;20180630;参见第2章，第5章 *

Also Published As

Publication number Publication date
CN112073909A (zh) 2020-12-11

Similar Documents

Publication Publication Date Title
CN112073909B (zh) 基于uwb/mems组合的uwb基站位置误差补偿方法
CN111896008A (zh) 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN113074739B (zh) 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法
KR102034527B1 (ko) 와이-파이 위치와 센서정보 결합을 통한 단말 위치 필터링 시스템 및 그 방법
AU2010304861B2 (en) Improvements in or relating to radio navigation
Shin et al. An unscented Kalman filter for in-motion alignment of low-cost IMUs
Chiang et al. Assessment for INS/GNSS/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a GNSS-degraded environment
Zhao et al. Localization of indoor mobile robot using minimum variance unbiased FIR filter
CN112747747B (zh) 一种改进的uwb/imu融合室内行人定位方法
Ibrahim et al. Inertial measurement unit based indoor localization for construction applications
CN112639504B (zh) 使用电子测距设备进行地理定位优化的方法
CN104869637A (zh) 用户站定位方法及装置
Tao et al. Precise displacement estimation from time-differenced carrier phase to improve PDR performance
Vana et al. Benefits of motion constraining for robust, low-cost, dual-frequency GNSS PPP+ MEMS IMU navigation
CN112639503B (zh) 使用电子测距设备进行地理定位的方法
CN116482735A (zh) 一种受限空间内外高精度定位方法
Lategahn et al. Robust pedestrian localization in indoor environments with an IMU aided TDoA system
Zhang et al. Three-dimension indoor positioning algorithms using an integrated RFID/INS system in multi-storey buildings
US20150097724A1 (en) Method and apparatus of gnss receiver heading determination
CN113267188A (zh) 基于v2x通信的车辆协同定位方法及系统
Gong et al. Application of improved robust adaptive algorithm in UWB/MEMS positioning system
Gao et al. Evaluation on Nonholonomic Constraints and Rauch–Tung–Striebel Filter‐Enhanced UWB/INS Integration
CN117148406B (zh) 一种室内外无缝弹性融合定位方法、系统、介质及设备
Li Multi Sensor-Multi Mode Integration for Indoor Navigation with Smartphones
CN115327597A (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