CN110375730A - 基于imu和uwb融合的室内定位导航系统 - Google Patents

基于imu和uwb融合的室内定位导航系统 Download PDF

Info

Publication number
CN110375730A
CN110375730A CN201910504895.5A CN201910504895A CN110375730A CN 110375730 A CN110375730 A CN 110375730A CN 201910504895 A CN201910504895 A CN 201910504895A CN 110375730 A CN110375730 A CN 110375730A
Authority
CN
China
Prior art keywords
uwb
target
imu
fusion
calculation unit
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
CN201910504895.5A
Other languages
English (en)
Other versions
CN110375730B (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.)
Shenzhen University
Original Assignee
Shenzhen 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 Shenzhen University filed Critical Shenzhen University
Priority to CN201910504895.5A priority Critical patent/CN110375730B/zh
Publication of CN110375730A publication Critical patent/CN110375730A/zh
Application granted granted Critical
Publication of CN110375730B publication Critical patent/CN110375730B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明本发明涉及室内定位导航领域，公开了一种基于IMU和UWB融合的室内定位导航系统。该基于IMU和UWB融合的室内定位导航系统包括IMU传感器、IMU位置计算单元、UWB传感器、UWB位置计算单元、融合位置计算单元，采用融合定位算法，将IMU和UWB相互结合，IMU得到的数据作为卡尔曼滤波的先验信息，UWB得到的数据作为卡尔曼滤波的观测信息，利用各自表现出的优点，可以有效地提高系统的定位导航精度，利用少量的观测站即可实现目标的高精度室内定位导航，实现其在高精度室内定位导航场景需求中的应用。

Description

基于IMU和UWB融合的室内定位导航系统
技术领域
本发明涉及室内定位导航领域，尤其涉及一种基于IMU和UWB融合的室内定位导航系统。
背景技术
惯性导航系统(INS，Inertial Navigation System)基于运动学和牛顿经典力学，核心部分是利用惯性测量单元(IMU，Inertial Measurement Unit)，包括一系列传感器和微型控制器单元(MCU，Micro Controller Unit)。通过惯性传感器来获取载体的姿态信息和运动特性，如加速度、角速度和角度信息等，无需任何参考基站节点，可以通过数学积分直接计算载体的坐标位置。由于其成本低、精度高、环境影响小等特点，在飞行器、车辆、行人等移动物体定位场景中得到了广泛的应用，但其累积误差随时间增加而增大，因此对传感器误差的分析与校正是提高惯性导航性能的关键之一。
近年来，各研究机构对基于UWB的室内定位技术进行了多方面的研究，包括信道模型、多径分量估计和定位精度的理论下限。超宽带(Ultra-Wide Band,UWB)信号是一种利用纳秒非正弦窄脉冲信号传输数据的通信技术。基于UWB的测距技术由于其脉冲间隔短，具有时间分辨率高的特征，因此可以达到厘米级别的测距精度。此外，UWB信号对多径效应具有良好的鲁棒性和穿透能力，在障碍物密集的室内无线定位场景中具有很大的优势。由于UWB技术在测距中的优异性能，它主要应用于基于测距的室内定位算法中。非测距定位算法定位精度不高，观测站需要集中部署。同样，测距或角度定位算法也取决于传感器的测量精度。
但是，在室内定位时，当室内的GNSS信号较弱或没有GNSS信号的环境下，目标无法完成定位导航功能或定位导航功能受到限制，不能实现对目标的精确定位导航。
发明内容
本发明主要目的在于，提供一种基于IMU和UWB融合的室内定位导航系统，旨在解决现有技术中室内的GNSS信号较弱或没有GNSS信号的环境下，目标无法完成定位导航功能或定位导航功能受到限制的技术问题。
本发明是通过如下技术方案实现的：
一种基于IMU和UWB融合的室内定位导航系统，包括IMU传感器、IMU位置计算单元、UWB传感器、UWB位置计算单元、融合位置计算单元；
所述IMU传感器安装于目标上，用于检测目标的三轴加速度和三轴角速度，并将所述三轴加速度和三轴角速度发送给所述IMU位置计算单元；
所述IMU位置计算单元根据所述三轴加速度和三轴角速度对目标进行定位，得到目标的第一定位数据；
所述UWB传感器包括固定在目标上的未知节点和固定在空间已知位置上的四个基准基站节点，用于检测所述未知节点与所述四个基准基站节点之间的相对位置关系数据，并将所述相对位置关系数据发送到所述UWB位置计算单元；
所述UWB位置计算单元根据所述相对位置关系对所述目标进行定位，得到目标的第二定位数据；
所述融合位置计算单元对所述IMU位置计算单元和所述UWB位置计算单元的数据进行卡尔曼滤波融合，得到最终的目标定位数据。
进一步地，所述卡尔曼滤波融合为基于观测距离的扩展卡尔曼滤波融合，包括：
初始化:
初始化系统状态向量X0＝E[X0]，初始化系统状态协方差矩阵P(0)＝var[X(0)]；
迭代：
For t＝1:k，In为n×n单位矩阵；
对状态做一步预测：
状态协方差矩阵的一步预测：
P(k+1|k)＝F(k+1|k)P(k|k)FT(k+1|k)+GQGT；
求卡尔曼滤波增益矩阵：
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R]-1；
状态更新：
协方差矩阵更新：
P(k+1|k+1)＝[In-K(k+1)H(k+1)]P(k+1|k)。
进一步地，所述卡尔曼滤波融合为基于距离和角度的无迹卡尔曼滤波融合，包括：
获得一组Sigma点集及其对应权值：
计算2n+1个Sigma点集的一步预测：
计算系统状态量的一步预测及协方差矩阵：
根据一步预测值，再次使用UT变换，产生新的Sigma点集：
将预测产生的Sigma点集带入观测方程得到预测的观测量；
Z(i)(k+1|k)＝h(X(i)(k+1|k))；
将得到的Sigma点集的观测预测值通过加权求和得到系统预测的值及协方差：
计算Kalman增益矩阵：
计算系统的状态更新和协方差更新：
进一步地，所述系统还包括单个静态观测基站，所述UWB传感器用于检测目标与观测基站之间的距离，所述IMU传感器用于检测目标与观测基站之间的角度；所述融合位置计算单元通过下述方式基于单个静态观测基站对目标进行二维精确定位：
设目标在二维平面内以近似均匀的加速度移动，时间k的状态向量包含位置、速度和加速度信息，用下式表示：
X(k)＝[xx(k) xy(k) vx(k) vy(k) ax(k) ay(k)]T
两个方向的运动都有加性系统噪声W(k)，目标的运动状态方程如下式所示：
X(k+1)＝FX(k)+W(k)
其中F表示状态转换矩阵，W(k)表示0的平均值如下式所示：
方差为Q过程噪声如下式所示：
UWB传感器检测目标与观测基站之间的距离为d，IMU传感器检测目标与观测基站之间的方位角为
观测噪声V(k)是一个均值为0，方差为R的加性噪声，笛卡尔坐标系下的目标观测方程如下式所示：
均方根误差(RMSE)如下式所示：
进一步地，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据、所述IMU位置计算单元计算的速度差数据和加速度数据进行AUAM过滤处理，包括：
初始化：初始化UWB传感器采样周期T＝0.02s，初始化IMU传感器采样周期t＝0.01s；
基于非对称双边双向测距法完成目标到基站距离测量：
d＝[d1(k)d2(k)…dn(k)]；
根据三边测量算法计算目标最小二乘位置：
X(k)＝(HTH)-1HTb；
根据目标位置计算差分速度：
将IMU加速度坐标转换：
更新目标位置：
进一步地，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据和速度差数据进行AUM过滤处理，包括：
初始化：初始化UWB传感器采样周期T＝0.02s；
基于非对称双边双向测距法完成目标到基站距离测量：
d＝[d1(k)d2(k)…dn(k)]；
根据三边测量算法计算目标最小二乘位置：
X(k)＝(HTH)-1HTb；
根据目标位置计算差分速度：
更新目标位置：
X(k+1)＝X(k)+V(k)T。
进一步地，非对称双边双向测距法的步骤包括：
设备A向设备B发送一个轮询消息包并记录发送时间；
设备B接收轮询消息包并记录接收时间；
设备B等待先前设置的延迟处理时间，将响应消息包发送到设备A并记录发送时间；
设备A接收响应消息包并记录接收时间，从而完成一个范围任务；
当设备A接收到设备B发送的响应消息包后，设备A将最终消息包发送到设备B；
设备B接收到最终消息包并记录接收时间。
与现有技术相比，本发明提供的基于IMU和UWB融合的室内定位导航系统，采用融合定位算法，将IMU和UWB相互结合，IMU得到的数据作为卡尔曼滤波的先验信息，UWB得到的数据作为卡尔曼滤波的观测信息，利用各自表现出的优点，可以有效地提高系统的定位导航精度，利用少量的观测站即可实现目标的高精度室内定位导航，实现其在高精度室内定位导航场景需求中的应用。
附图说明
图1是本发明实施例基于IMU和UWB融合的室内定位导航系统的架构示意图；
图2是捷联惯导系统原理示意图；
图3是目标空间角度位置确定原理示意图；
图4是基于单个静态观测站观测距离和角度定位示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步详细说明。
如图1所示，本发明实施例提供的基于IMU和UWB融合的室内定位导航系统，包括IMU传感器1、IMU位置计算单元2、UWB传感器3、UWB位置计算单元4、融合位置计算单元5。其中：
IMU传感器1安装于目标上，用于检测目标的三轴加速度和三轴角速度，并将三轴加速度和三轴角速度发送给IMU位置计算单元2；
IMU位置计算单元2根据三轴加速度和三轴角速度对目标进行定位，得到目标的第一定位数据；
UWB传感器3包括固定在目标上的未知节点和固定在空间已知位置上的四个基准基站节点，用于检测未知节点与四个基准基站节点之间的相对位置关系数据，并将相对位置关系数据发送到UWB位置计算单元4；
UWB位置计算单元4根据相对位置关系对目标进行定位，得到目标的第二定位数据；
融合位置计算单元5对IMU位置计算单元2和UWB位置计算单元4的数据进行卡尔曼滤波融合，得到最终的目标定位数据。
理论上，求解未知节点的位置至少需要三个基准基站节点，但在实际中，由于数据冗余的存在，通常需要部署四个基准基站节点才能使结果更加准确。本发明结合UWB传感器3和IMU传感器1，获得了鲁棒性和最优定位性能。MATLAB仿真结果表明，IMU传感器1提供的先验信息可以显著抑制UWB传感器3在非视线条件下的观测误差，与仅使用UWB传感器3或IMU传感器1相比，集成系统的定位精度有了显著提高。此外，该算法具有较高的计算效率，可以在一般嵌入式设备上实现实时计算。最后，提出了两种随机运动近似模型算法，并进行了实验。实验结果表明，该算法在实际的室内定位导航场景中具有一定的鲁棒性和连续跟踪能力。
基于IMU的定位导航处理算法：
捷联惯导系统(SINS)是基于一系列精确定义的参考坐标系实时计算系统姿态矩阵，转换不同坐标系之间的比例测量值，计算速度和位置，实现惯导定位功能。参考坐标系及其变换是捷联惯导算法的基础，姿态更新解是该算法的核心。捷联惯导系统原理如图2所示。
为了准确、完整地描述载体的空间运动状态，需要选择合适的参考坐标系。假设由载体体轴确定的坐标系为B系，惯性导航系统采用的导航坐标系为N系，则由B系到N系的坐标变换矩阵为载体姿态矩阵。系统中陀螺仪、加速度等测量仪器的坐标属于载体坐标。加速度、速度和位置的最终输出属于导航坐标系。以导航坐标系为参考坐标系，采用欧拉角法将导航坐标系转换为载体坐标系。载体的航向角以表示，俯仰角以表示，横滚角以表示。当坐标系以欧拉角旋转时，由于旋转顺序不同，矩阵的乘积不能互换。变换矩阵等于由基本旋转确定的变换矩阵的连续乘法。连续乘法的序列按基本旋转的顺序从右向左排列。以导航坐标系为参考坐标系，采用欧拉角法将导航坐标系转换为载体坐标系。目标空间角度位置的确定如图3所示。
导航坐标系到载体坐标系的坐标变换矩阵如式(1)所示：
其中s表示sin，c表示cos。相反，从载体坐标系到导航坐标系，由于直角坐标系之间的变换矩阵是单位正交矩阵，如果保持坐标系n到坐标系b中的坐标系，作为等效旋转坐标系中的直角坐标系。然后根据单位正交矩阵的性质如式(6)所示:
姿态更新是指根据惯性器件的输出实时计算矩阵。在力学中，欧拉角常用于确定运动坐标系与参考坐标系之间的角位置关系。载体的航向角、俯仰角和横滚角基本上是一组欧拉角,这些欧拉角描述了载体坐标系和导航坐标系之间的角位置关系。如果姿态率为ω(相对于导航坐标系N的载体坐标系B的角速度)，则载体坐标系B中ω的分量如式(8)所示:
所以我们可以得到欧拉角微分方程如式(9)所示：
姿态率由以下公式确定如式(10)所示:
公式中，是用陀螺仪组合测量载体的角速度，是根据载体的经纬度和速度计算得出的。
载体系统中的加速度值是用惯性测量单元中的三轴正交加速度传感器测量的如式(11)所示:
坐标变换矩阵通过姿态更新计算，通过坐标变换得到导航坐标系中的加速度如式(12)所示:
将重力矢量从导航坐标系加速度中去除，即可得到导航坐标系中载体的加速度如式(13)所示:
当采样间隔较短时，载体受到恒定的力并作均匀的线性运动。通过用牛顿第二定律代替动量守恒方程，载体的速度变化等于载体在导航坐标系中相对于时间Δt的瞬时加速度的积分如式(14)所示:
载体在导航坐标系中的加速度积分得到如下速度如式(15)所示:
然后根据速度积分计算载体的位移变化如式(16)所示:
最后载体在导航坐标中的位置如式(17)所示:
基于UWB的定位导航处理算法：
基于无线信号的定位方法很多，一般分为测距和测角定位算法和非测距定位算法。在定位过程中，如果用节点之间的距离信息或信号到达的角度信息来计算位置，这就称为测距定位算法。如果使用节点间的连接和多跳路由信息来估计距离，而不直接测量角度和距离信息，则称为非测距定位算法。基于非测距的常用定位算法包括质心算法、dv-hop算法、三角测量中的近似点定位算法等。基于测距的定位算法一般分为两步。第一步是测量距离或角度信息，第二步是使用测量的距离或角度信息计算坐标。基于UWB测距的定位算法如下。
针对传统测距算法的不足，本发明提出了一种改进的增强型的非对称双边双向测距(EADS-TWR)优化算法，这种非对称双边双向测距法的步骤包括：
设备A向设备B发送一个轮询消息包并记录发送时间；
设备B接收轮询消息包并记录接收时间；
设备B等待先前设置的延迟处理时间，将响应消息包发送到设备A并记录发送时间；
设备A接收响应消息包并记录接收时间，从而完成一个范围任务；
当设备A接收到设备B发送的响应消息包后，设备A将最终消息包发送到设备B；
设备B接收到最终消息包并记录接收时间。
设备A和设备B传输过程中信号的往返时间如式(19)所示:
信号往返时间的真实值如式(20)所示:
在XY二维平面上，一个点可以用三个圆来表示。在XYZ三维空间中，一个点可以由四个圆来标识。多边测量是根据目标的距离确定目标位置的过程。以三个基站的多边测量为例，在该多边测量中，目标应该位于以每个基站为中心的三个圆的交点。只要三个基站不在一条直线上，三边测量的结果是唯一的。然而，在实际测量中存在着不可避免的误差，这就导致了这三个圆不能相交于一点。
假设未知目标节点位置位于(x,y)，d'i是未知节点到第i个基站(xi,yi)的测量距离，1<＝i<＝n，其中n是基站总个数，di是未知节点到第i个基站的实际距离如式(21)所示:
因此测量距离与实际距离的差值可以表示为ρi＝d'i-di。目前，已有一些方法用于处理测距噪声，其中最小二乘法通过最小化来确定(x,y)。具体来说，每一个距离度量确定一个关于未知节点位置的方程式如式(22)所示:
然后所有方程减去第一个方程，新方程以矩阵形式表示，并通过简化得到如式(23)所示:
Hx＝b (23)
其中，
最后方程的最小方差解如式(25)所示:
x＝(HTH)-1HTb (25)
基于IMU和UWB融合的室内定位导航算法：
在惯性导航系统中，由于误差积分，陀螺和加速度容易发散。UWB可以在室内定位系统中提供厘米级的测距和定位精度，但无线信号容易受到非视线传播的影响。因此，通过一种或多种数学方法对IMU和UWB传感器3的数据进行优化，可以有效地提高目标的定位精度。
线性卡尔曼滤波器可以在线性高斯模型条件下对目标状态进行最优估计。然而，实际系统中总是存在不同程度的非线性。典型的非线性函数关系包括平方关系、对数关系、指数关系、三角函数关系等。一些非线性系统可以近似为线性系统，但为了精确地估计系统的状态，大多数系统不能只用线性微分方程来描述，如飞机的飞行状态、导弹制导系统、卫星导航系统等，非线性因素是不可忽略的，因此必须建立非线性系统滤波算法。本发明提出了两种非线性卡尔曼滤波算法，包括扩展卡尔曼滤波和无迹卡尔曼滤波。基于此，本发明中的卡尔曼滤波融合可以是基于观测距离的扩展卡尔曼滤波融合或者基于距离和角度的无迹卡尔曼滤波融合。
在二维平面中，目标的位置可以通过至少三个观察距离来确定。在三维空间中，至少需要四个观测距离来定位目标。基于观测距离的扩展卡尔曼滤波融合(EKF)，包括：
初始化:
初始化系统状态向量X0＝E[X0]，初始化系统状态协方差矩阵P(0)＝var[X(0)]；
迭代：
For t＝1:k，In为n×n单位矩阵；
对状态做一步预测：
状态协方差矩阵的一步预测：
P(k+1|k)＝F(k+1|k)P(k|k)FT(k+1|k)+GQGT；
求卡尔曼滤波增益矩阵：
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R]-1；
状态更新：
协方差矩阵更新：
P(k+1|k+1)＝[In-K(k+1)H(k+1)]P(k+1|k)。
在二维平面中，假设目标在k时刻的状态向量包含位置、速度和加速度信息，用式(26)表示：
X(k)＝[xx(k) xy(k) vx(k) vy(k) ax(k) ay(k)]T
(26)
在这个过程中加速度可能受到空气阻力、摩擦等外部因素的干扰，因此引入过程噪声与实际情况相一致，然后根据均匀加速度直线运动方程得到式(27)：
如果上述方程以矩阵形式表示，则状态方程如式(28)所示：
X(k+1)＝FX(k)+GW(k) (28)
其中F表示状态转换矩阵，W(k)表示均值为0，方差为Q过程噪声，G表示噪声驱动矩阵如式(29)和式(30)所示：
公式中G的表达式不是唯一的。有时也可以根据需要采用其他形式。由于UWB传感器3检测到目标与基站之间的距离，因此观测方程如式(31)所示：
公式中H(k)是观测矩阵，V(k)是观测噪声矩阵，[d1(k)d2(k)…dn(k)]T表示目标与超宽带基站之间的测距矩阵如式(32)所示：
由于观测方程32是非线性的，需要对其进行线性化。对它进行一阶泰勒展开，H(k)是线性化观测矩阵，即雅可比矩阵。具体的偏导数结果如方程33所示。在视距条件下，测距噪声服从高斯分布，均值为0，方差为σd。在非视线条件下，测距噪声仍服从高斯分布，但其均值不为零，方差增大，R对应于噪声方差矩阵如式(35)所示：
由于EKF算法将非线性系统方程或观测方程进行泰勒展开，保留了一阶近似项，不可避免地引入了线性化误差。如果线性化假设不成立，滤波器的性能将下降，滤波器将发散。另外，状态方程和观测方程的雅可比矩阵不易计算，增加了算法的计算复杂度。基于距离和角度的无迹卡尔曼滤波融合(UKF)摒弃了非线性函数线性化的传统做法，采用了卡尔曼线性滤波框架。对于一步预测方程，采用无迹变换处理均值和方差的非线性传递。UKF算法近似非线性函数的概率密度分布，使用一组确定性样本强制状态的后验概率密度，而不是近似非线性函数，而不采用雅可比矩阵的导数。UKF不忽略高阶项，对非线性分布的统计具有较高的计算精度，有效地克服了EKF滤波器估计精度低、稳定性差的缺点。基于距离和角度的无迹卡尔曼滤波融合包括：
获得一组Sigma点集及其对应权值：
计算2n+1个Sigma点集的一步预测：
X(i)(k+1|k)＝f[k,X(i)(k|k)],i＝1,2,…,2n+1；
计算系统状态量的一步预测及协方差矩阵：
根据一步预测值，再次使用UT变换，产生新的Sigma点集：
将预测产生的Sigma点集带入观测方程得到预测的观测量；
Z(i)(k+1|k)＝h(X(i)(k+1|k))；
将得到的Sigma点集的观测预测值通过加权求和得到系统预测的值及协方差：
计算Kalman增益矩阵：
计算系统的状态更新和协方差更新：
系统还包括单个静态观测基站，UWB传感器3用于检测目标与观测基站之间的距离，IMU传感器1用于检测目标与观测基站之间的角度，基于单个静态观测基站的二维精确定位的原理如图4所示。融合位置计算单元5通过下述方式基于单个静态观测基站对目标进行二维精确定位：
设目标在二维平面内以近似均匀的加速度移动，时间k的状态向量包含位置、速度和加速度信息，用下式表示：
X(k)＝[xx(k) xy(k) vx(k) vy(k) ax(k) ay(k)]T
两个方向的运动都有加性系统噪声W(k)，目标的运动状态方程如下式所示：
X(k+1)＝FX(k)+W(k)
其中F表示状态转换矩阵，W(k)表示0的平均值如下式所示：
方差为Q过程噪声如下式所示：
UWB传感器3检测目标与观测基站之间的距离为d，IMU传感器1检测目标与观测基站之间的方位角为
观测噪声V(k)是一个均值为0，方差为R的加性噪声，笛卡尔坐标系下的目标观测方程如下式所示：
均方根误差(RMSE)如下式所示：
基于IMU和UWB融合的匀速运动和匀加速运动的近似模型算法：
由于目标的运动通常是随机的，因此很难建立精确的运动模型。因此，我们提出了两种近似运动模型，一种是将目标的随机运动作为短时间内的近似匀速运动(AUAM)模型，即：
融合位置计算单元5结合UWB位置计算单元4计算的定位数据、IMU位置计算单元2计算的速度差数据和加速度数据进行AUAM过滤处理，包括：
初始化：初始化UWB传感器3采样周期T＝0.02s，初始化IMU传感器1采样周期t＝0.01s；
基于非对称双边双向测距法完成目标到基站距离测量：
d＝[d1(k) d2(k)…dn(k)]；
根据三边测量算法计算目标最小二乘位置：
X(k)＝(HTH)-1HTb；
根据目标位置计算差分速度：
将IMU加速度坐标转换：
更新目标位置：
另一种是将目标的随机运动作为短时间内的近似匀速运动(AUM)模型，即融合位置计算单元5结合UWB位置计算单元4计算的定位数据和速度差数据进行AUM过滤处理，包括：
初始化：初始化UWB传感器3采样周期T＝0.02s；
基于非对称双边双向测距法完成目标到基站距离测量：
d＝[d1(k) d2(k)…dn(k)]；
根据三边测量算法计算目标最小二乘位置：
X(k)＝(HTH)-1HTb；
根据目标位置计算差分速度：
更新目标位置：
X(k+1)＝X(k)+V(k)T。
上述实施例仅为优选实施例，并不用以限制本发明的保护范围，在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于IMU和UWB融合的室内定位导航系统，其特征在于，包括IMU传感器、IMU位置计算单元、UWB传感器、UWB位置计算单元、融合位置计算单元；
所述IMU传感器安装于目标上，用于检测目标的三轴加速度和三轴角速度，并将所述三轴加速度和三轴角速度发送给所述IMU位置计算单元；
所述IMU位置计算单元根据所述三轴加速度和三轴角速度对目标进行定位，得到目标的第一定位数据；
所述UWB传感器包括固定在目标上的未知节点和固定在空间已知位置上的四个基准基站节点，用于检测所述未知节点与所述四个基准基站节点之间的相对位置关系数据，并将所述相对位置关系数据发送到所述UWB位置计算单元；
所述UWB位置计算单元根据所述相对位置关系对所述目标进行定位，得到目标的第二定位数据；
所述融合位置计算单元对所述IMU位置计算单元和所述UWB位置计算单元的数据进行卡尔曼滤波融合，得到最终的目标定位数据。
2.如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述卡尔曼滤波融合为基于观测距离的扩展卡尔曼滤波融合，包括：
初始化:
初始化系统状态向量X0＝E[X0]，初始化系统状态协方差矩阵P(0)＝var[X(0)]；
迭代：
For t＝1:k，In为n×n单位矩阵；
对状态做一步预测：
状态协方差矩阵的一步预测：
P(k+1|k)＝F(k+1|k)P(k|k)FT(k+1|k)+GQGT；
求卡尔曼滤波增益矩阵：
K(k+1)＝P(k+1|k)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R]-1；
状态更新：
协方差矩阵更新：
P(k+1|k+1)＝[In-K(k+1)H(k+1)]P(k+1|k)。
3.如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述卡尔曼滤波融合为基于距离和角度的无迹卡尔曼滤波融合，包括：
获得一组Sigma点集及其对应权值：
计算2n+1个Sigma点集的一步预测：
X(i)(k+1|k)＝f[k,X(i)(k|k)],i＝1,2,…,2n+1；
计算系统状态量的一步预测及协方差矩阵：
根据一步预测值，再次使用UT变换，产生新的Sigma点集：
将预测产生的Sigma点集带入观测方程得到预测的观测量；
Z(i)(k+1|k)＝h(X(i)(k+1|k))；
将得到的Sigma点集的观测预测值通过加权求和得到系统预测的值及协方差：
计算Kalman增益矩阵：
计算系统的状态更新和协方差更新：
4.如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，还包括单个静态观测基站，所述UWB传感器用于检测目标与观测基站之间的距离，所述IMU传感器用于检测目标与观测基站之间的角度；所述融合位置计算单元通过下述方式基于单个静态观测基站对目标进行二维精确定位：
设目标在二维平面内以近似均匀的加速度移动，时间k的状态向量包含位置、速度和加速度信息，用下式表示：
X(k)＝[xx(k) xy(k) υx(k) υy(k) ax(k) ay(k)]T
两个方向的运动都有加性系统噪声W(k)，目标的运动状态方程如下式所示：
X(k+1)＝FX(k)+W(k)
其中F表示状态转换矩阵，W(k)表示0的平均值如下式所示：
方差为Q过程噪声如下式所示：
UWB传感器检测目标与观测基站之间的距离为d，IMU传感器检测目标与观测基站之间的方位角为
观测噪声V(k)是一个均值为0，方差为R的加性噪声，笛卡尔坐标系下的目标观测方程如下式所示：
均方根误差(RMSE)如下式所示：
5.如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据、所述IMU位置计算单元计算的速度差数据和加速度数据进行AUAM过滤处理，包括：
初始化：初始化UWB传感器采样周期T＝0.02s，初始化IMU传感器采样周期t＝0.01s；
基于非对称双边双向测距法完成目标到基站距离测量：
d＝[d1(k) d2(k) … dn(k)]；
根据三边测量算法计算目标最小二乘位置：
X(k)＝(HTH)-1HTb；
根据目标位置计算差分速度：
将IMU加速度坐标转换：
更新目标位置：
6.如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据和速度差数据进行AUM过滤处理，包括：
初始化：初始化UWB传感器采样周期T＝0.02s；
基于非对称双边双向测距法完成目标到基站距离测量：
d＝[d1(k) d2(k) … dn(k)]；
根据三边测量算法计算目标最小二乘位置：
X(k)＝(HTH)-1HTb；
根据目标位置计算差分速度：
更新目标位置：
X(k+1)＝X(k)+V(k)T。
7.如权利要求6所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，非对称双边双向测距法的步骤包括：
设备A向设备B发送一个轮询消息包并记录发送时间；
设备B接收轮询消息包并记录接收时间；
设备B等待先前设置的延迟处理时间，将响应消息包发送到设备A并记录发送时间；
设备A接收响应消息包并记录接收时间，从而完成一个范围任务；
当设备A接收到设备B发送的响应消息包后，设备A将最终消息包发送到设备B；
设备B接收到最终消息包并记录接收时间。
CN201910504895.5A 2019-06-12 2019-06-12 基于imu和uwb融合的室内定位导航系统 Expired - Fee Related CN110375730B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910504895.5A CN110375730B (zh) 2019-06-12 2019-06-12 基于imu和uwb融合的室内定位导航系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910504895.5A CN110375730B (zh) 2019-06-12 2019-06-12 基于imu和uwb融合的室内定位导航系统

Publications (2)

Publication Number Publication Date
CN110375730A true CN110375730A (zh) 2019-10-25
CN110375730B CN110375730B (zh) 2021-07-27

Family

ID=68250112

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910504895.5A Expired - Fee Related CN110375730B (zh) 2019-06-12 2019-06-12 基于imu和uwb融合的室内定位导航系统

Country Status (1)

Country Link
CN (1) CN110375730B (zh)

Cited By (30)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110764506A (zh) * 2019-11-05 2020-02-07 广东博智林机器人有限公司 移动机器人的航向角融合方法、装置和移动机器人
CN111121754A (zh) * 2019-12-31 2020-05-08 深圳市优必选科技股份有限公司 移动机器人定位导航方法、装置、移动机器人及存储介质
CN111208472A (zh) * 2020-01-06 2020-05-29 江苏大学 一种温室大范围高精度定位系统与方法
CN111795695A (zh) * 2020-05-15 2020-10-20 北京百度网讯科技有限公司 位置信息确定方法、装置及设备
CN112034479A (zh) * 2020-06-15 2020-12-04 煤炭科学技术研究院有限公司 一种应用于煤矿井下智能巡检无人机的定位方法及系统
CN112325878A (zh) * 2020-10-30 2021-02-05 南京航空航天大学 基于ukf与空中无人机节点辅助的地面载体组合导航方法
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法
CN112595320A (zh) * 2020-11-23 2021-04-02 北京联合大学 基于ros的室内智能轮椅高精定位自主导航方法及系统
CN112595327A (zh) * 2020-12-25 2021-04-02 燕山大学 一种基于uwb室内定位系统的定位方法及装置
CN112683268A (zh) * 2020-12-08 2021-04-20 中国铁建重工集团股份有限公司 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及系统
CN112747747A (zh) * 2021-01-20 2021-05-04 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN112957033A (zh) * 2021-02-01 2021-06-15 山东大学 人机协作中人体实时室内定位与运动姿态捕捉方法与系统
CN113108791A (zh) * 2021-03-05 2021-07-13 深圳大学 一种导航定位方法及导航定位设备
CN113120713A (zh) * 2021-04-28 2021-07-16 上海有个机器人有限公司 电梯定位方法
CN113438615A (zh) * 2020-03-04 2021-09-24 北京京东乾石科技有限公司 定位方法和装置
CN113570798A (zh) * 2021-08-04 2021-10-29 吉林建筑大学 一种无线火灾探测器及火灾人员定位方法
CN113640738A (zh) * 2021-06-24 2021-11-12 西南科技大学 一种结合imu与单组uwb的旋转式目标定位方法
CN113900061A (zh) * 2021-05-31 2022-01-07 深圳市易艾得尔智慧科技有限公司 基于uwb无线定位与imu融合的导航定位系统及方法
CN113923591A (zh) * 2021-10-08 2022-01-11 西安电子科技大学 提高超宽带测距精度的滤波函数智能优化方法及系统
CN114111802A (zh) * 2021-12-21 2022-03-01 中国有色金属长沙勘察设计研究院有限公司 一种行人航迹推算辅助uwb的定位方法
CN114509069A (zh) * 2022-01-25 2022-05-17 南昌大学 基于蓝牙aoa和imu融合的室内导航定位系统
CN114623823A (zh) * 2022-05-16 2022-06-14 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置
CN114973036A (zh) * 2022-06-04 2022-08-30 西北工业大学 基于gnss/惯导/无线基站融合的无人机三维定位方法
CN116321418A (zh) * 2023-03-02 2023-06-23 中国人民解放军国防科技大学 一种基于节点构型优选地集群无人机融合估计定位方法
CN117031521A (zh) * 2023-10-08 2023-11-10 山东大学 一种室内外无缝环境下的弹性融合定位方法及系统
CN117119586A (zh) * 2023-08-29 2023-11-24 长春理工大学 一种基于uwb和imu室内定位融合算法
CN117490689A (zh) * 2023-11-06 2024-02-02 河南理工大学 一种二维超宽带与惯性导航融合定位方法及系统
CN113900061B (zh) * 2021-05-31 2024-10-22 深圳鹏鲲智科技术有限公司 基于uwb无线定位与imu融合的导航定位系统及方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109655060A (zh) * 2019-02-19 2019-04-19 济南大学 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109655060A (zh) * 2019-02-19 2019-04-19 济南大学 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
DAQUAN FENG等: "Kalman-Filter-Based Integration of IMU and UWB for High-Accuracy Indoor Positioning and Navigation", 《IEEE INTERNET OF THINGS JOURNAL》 *
WANG GUOSHENG等: "UWB and IMU System Fusion for Indoor Navigation", 《PROCEEDINGS OF THE 37TH CHINESE CONTROL CONFERENCE》 *
王春琦等: "基于UWB的增强非对称双边双向测距算法研究", 《南昌航空大学学报：自然科学版》 *
聂杨: "智能割草机器人的关键技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
谢永强: "基于GPS/INS组合导航的轨迹补偿方法研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *

Cited By (42)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110764506A (zh) * 2019-11-05 2020-02-07 广东博智林机器人有限公司 移动机器人的航向角融合方法、装置和移动机器人
CN111121754A (zh) * 2019-12-31 2020-05-08 深圳市优必选科技股份有限公司 移动机器人定位导航方法、装置、移动机器人及存储介质
CN111208472A (zh) * 2020-01-06 2020-05-29 江苏大学 一种温室大范围高精度定位系统与方法
CN113438615A (zh) * 2020-03-04 2021-09-24 北京京东乾石科技有限公司 定位方法和装置
CN111795695A (zh) * 2020-05-15 2020-10-20 北京百度网讯科技有限公司 位置信息确定方法、装置及设备
CN111795695B (zh) * 2020-05-15 2022-06-03 阿波罗智联(北京)科技有限公司 位置信息确定方法、装置及设备
CN112034479A (zh) * 2020-06-15 2020-12-04 煤炭科学技术研究院有限公司 一种应用于煤矿井下智能巡检无人机的定位方法及系统
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112325878A (zh) * 2020-10-30 2021-02-05 南京航空航天大学 基于ukf与空中无人机节点辅助的地面载体组合导航方法
CN112595320A (zh) * 2020-11-23 2021-04-02 北京联合大学 基于ros的室内智能轮椅高精定位自主导航方法及系统
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法
CN112683268A (zh) * 2020-12-08 2021-04-20 中国铁建重工集团股份有限公司 一种基于扩展卡尔曼滤波的巷道实时定位导航方法及系统
CN112595327A (zh) * 2020-12-25 2021-04-02 燕山大学 一种基于uwb室内定位系统的定位方法及装置
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN112833876B (zh) * 2020-12-30 2022-02-11 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
CN112747747A (zh) * 2021-01-20 2021-05-04 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN112747747B (zh) * 2021-01-20 2022-10-11 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN112957033A (zh) * 2021-02-01 2021-06-15 山东大学 人机协作中人体实时室内定位与运动姿态捕捉方法与系统
CN113108791B (zh) * 2021-03-05 2023-08-04 深圳大学 一种导航定位方法及导航定位设备
CN113108791A (zh) * 2021-03-05 2021-07-13 深圳大学 一种导航定位方法及导航定位设备
CN113120713A (zh) * 2021-04-28 2021-07-16 上海有个机器人有限公司 电梯定位方法
CN113120713B (zh) * 2021-04-28 2023-09-08 上海有个机器人有限公司 电梯定位方法
CN113900061B (zh) * 2021-05-31 2024-10-22 深圳鹏鲲智科技术有限公司 基于uwb无线定位与imu融合的导航定位系统及方法
CN113900061A (zh) * 2021-05-31 2022-01-07 深圳市易艾得尔智慧科技有限公司 基于uwb无线定位与imu融合的导航定位系统及方法
CN113640738A (zh) * 2021-06-24 2021-11-12 西南科技大学 一种结合imu与单组uwb的旋转式目标定位方法
CN113570798A (zh) * 2021-08-04 2021-10-29 吉林建筑大学 一种无线火灾探测器及火灾人员定位方法
CN113923591A (zh) * 2021-10-08 2022-01-11 西安电子科技大学 提高超宽带测距精度的滤波函数智能优化方法及系统
CN114111802A (zh) * 2021-12-21 2022-03-01 中国有色金属长沙勘察设计研究院有限公司 一种行人航迹推算辅助uwb的定位方法
CN114509069B (zh) * 2022-01-25 2023-11-28 南昌大学 基于蓝牙aoa和imu融合的室内导航定位系统
CN114509069A (zh) * 2022-01-25 2022-05-17 南昌大学 基于蓝牙aoa和imu融合的室内导航定位系统
CN114623823B (zh) * 2022-05-16 2022-09-13 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置
CN114623823A (zh) * 2022-05-16 2022-06-14 青岛慧拓智能机器有限公司 一种融合里程计的uwb多模态定位系统、方法及装置
CN114973036A (zh) * 2022-06-04 2022-08-30 西北工业大学 基于gnss/惯导/无线基站融合的无人机三维定位方法
CN114973036B (zh) * 2022-06-04 2024-08-23 西北工业大学 基于gnss/惯导/无线基站融合的无人机三维定位方法
CN116321418A (zh) * 2023-03-02 2023-06-23 中国人民解放军国防科技大学 一种基于节点构型优选地集群无人机融合估计定位方法
CN116321418B (zh) * 2023-03-02 2024-01-02 中国人民解放军国防科技大学 一种基于节点构型优选的集群无人机融合估计定位方法
CN117119586B (zh) * 2023-08-29 2024-05-24 长春理工大学 一种基于uwb和imu室内定位融合方法
CN117119586A (zh) * 2023-08-29 2023-11-24 长春理工大学 一种基于uwb和imu室内定位融合算法
CN117031521B (zh) * 2023-10-08 2024-01-30 山东大学 一种室内外无缝环境下的弹性融合定位方法及系统
CN117031521A (zh) * 2023-10-08 2023-11-10 山东大学 一种室内外无缝环境下的弹性融合定位方法及系统
CN117490689A (zh) * 2023-11-06 2024-02-02 河南理工大学 一种二维超宽带与惯性导航融合定位方法及系统
CN117490689B (zh) * 2023-11-06 2024-08-13 河南理工大学 一种二维超宽带与惯性导航融合定位方法及系统

Also Published As

Publication number Publication date
CN110375730B (zh) 2021-07-27

Similar Documents

Publication Publication Date Title
CN110375730B (zh) 基于imu和uwb融合的室内定位导航系统
US7821453B2 (en) Distributed iterative multimodal sensor fusion method for improved collaborative localization and navigation
Li et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments
CN110554359B (zh) 一种融合长基线与单信标定位的海底飞行节点定位方法
CN105589064A (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN107289932B (zh) 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
CN114509069B (zh) 基于蓝牙aoa和imu融合的室内导航定位系统
CN113382354B (zh) 一种基于因子图的无线定位非视距信号判别方法
Guosheng et al. UWB and IMU system fusion for indoor navigation
Yu et al. Combining Zigbee and inertial sensors for quadrotor UAV indoor localization
EP3146356A2 (en) Direct geolocation from tdoa, fdoa, and agl
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
CN108981703B (zh) 火星着陆器联合位置估计方法
CN113076634B (zh) 一种多机协同无源定位方法、装置及系统
CN110471096A (zh) 一种分布式海底飞行节点群体定位方法
CN110728026B (zh) 一种基于角速度量测的末端弹道目标被动跟踪方法
Elfadil et al. Indoor navigation algorithm for mobile robot using wireless sensor networks
Lategahn et al. Robust pedestrian localization in indoor environments with an IMU aided TDoA system
CN116482735A (zh) 一种受限空间内外高精度定位方法
CN108332749B (zh) 一种室内动态追踪定位方法
WO2020248154A1 (zh) 基于imu和uwb融合的室内定位导航系统
CN114705223A (zh) 多移动智能体在目标跟踪中的惯导误差补偿方法及系统
Yao et al. Tightly coupled indoor positioning using UWB/mmWave radar/IMU
Deneault et al. Tracking ground targets with measurements obtained from a single monocular camera mounted on an unmanned aerial vehicle
RU2264598C1 (ru) Способ определения координат летательного аппарата

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210727