WO2020248154A1

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

Info

Publication number: WO2020248154A1
Application number: PCT/CN2019/090855
Authority: WO
Inventors: 冯大权; 王春琦; 何春龙; 王凯源; 龙立泰; 赖礼锋; 周健; 刘梦悦
Original assignee: 深圳大学
Priority date: 2019-06-12
Filing date: 2019-06-12
Publication date: 2020-12-17

Abstract

一种基于IMU和UWB融合的室内定位导航系统。该基于IMU和UWB融合的室内定位导航系统包括IMU传感器（1）、IMU位置计算单元（2）、UWB传感器（3）、UWB位置计算单元（4）、融合位置计算单元（5），采用融合定位算法，将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位置计算单元的数据进行卡尔曼滤波融合，得到最终的目标定位数据。

进一步地，所述卡尔曼滤波融合为基于观测距离的扩展卡尔曼滤波融合，包括：

初始化:

初始化系统状态向量X ₀＝E[X ₀]，初始化系统状态协方差矩阵P(0)＝var[X(0)]；

迭代：

For t＝1:k，I _n为n×n单位矩阵；

对状态做一步预测：

状态协方差矩阵的一步预测：

P(k+1|k)＝F(k+1|k)P(k|k)F ^T(k+1|k)+GQG ^T；

求卡尔曼滤波增益矩阵：

K(k+1)＝P(k+1|k)H ^T(k+1)[H(k+1)P(k+1|k)H ^T(k+1)+R] ^-1；

状态更新：

协方差矩阵更新：

P(k+1|k+1)＝[I _n-K(k+1)H(k+1)]P(k+1|k)。

进一步地，所述卡尔曼滤波融合为基于距离和角度的无迹卡尔曼滤波融合，包括：

获得一组Sigma点集及其对应权值：

计算2n+1个Sigma点集的一步预测：

X ⁽ⁱ⁾(k+1|k)＝f[k,X ⁽ⁱ⁾(k|k)],i＝1,2,...,2n+1；

计算系统状态量的一步预测及协方差矩阵：

根据一步预测值，再次使用UT变换，产生新的Sigma点集：

将预测产生的Sigma点集带入观测方程得到预测的观测量；

Z ⁽ⁱ⁾(k+1|k)＝h(X ⁽ⁱ⁾(k+1|k))；

将得到的Sigma点集的观测预测值通过加权求和得到系统预测的值及协方差：

计算Kalman增益矩阵：

计算系统的状态更新和协方差更新：

进一步地，所述系统还包括单个静态观测基站，所述UWB传感器用于检测目标与观测基站之间的距离，所述IMU传感器用于检测目标与观测基站之间的角度；所述融合位置计算单元通过下述方式基于单个静态观测基站对目标进行二维精确定位：

设目标在二维平面内以近似均匀的加速度移动，时间k的状态向量包含位置、速度和加速度信息，用下式表示：

X(k)＝[x _x(k) x _y(k) v _x(k) v _y(k) a _x(k) a _y(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＝[d ₁(k) d ₂(k) ... d _n(k)]；

根据三边测量算法计算目标最小二乘位置：

X(k)＝(H ^TH) ^-1H ^Tb；

根据目标位置计算差分速度：

将IMU加速度坐标转换：

更新目标位置：

进一步地，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据和速度差数据进行AUM过滤处理，包括：

初始化：初始化UWB传感器采样周期T＝0.02s；

基于非对称双边双向测距法完成目标到基站距离测量：

d＝[d ₁(k) d ₂(k) ... d _n(k)]；

根据三边测量算法计算目标最小二乘位置：

X(k)＝(H ^TH) ^-1H ^Tb；

根据目标位置计算差分速度：

更新目标位置：

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并记录发送时间；

设备B接收到最终消息包并记录接收时间。

设备A和设备B传输过程中信号的往返时间如式(19)所示:

信号往返时间的真实值如式(20)所示:

在XY二维平面上，一个点可以用三个圆来表示。在XYZ三维空间中，一个点可以由四个圆来标识。多边测量是根据目标的距离确定目标位置的过程。以三个基站的多边测量为例，在该多边测量中，目标应该位于以每个基站为中心的三个圆的交点。只要三个基站不在一条直线上，三边测量的结果是唯一的。然而，在实际测量中存在着不可避免的误差，这就导致了这三个圆不能相交于一点。

假设未知目标节点位置位于(x,y)，d’ _i是未知节点到第i个基站(x _i,y _i)的测量距离，1<＝i<＝n，其中n是基站总个数，d _i是未知节点到第i个基站的实际距离如式(21)所示:

因此测量距离与实际距离的差值可以表示为ρ _i＝d’ _i-d _i。目前，已有一些方法用于处理测距噪声，其中最小二乘法通过最小化

来确定(x,y)。具体来说，每一个距离度量确定一个关于未知节点位置的方程式如式(22)所示:

然后所有方程减去第一个方程，新方程以矩阵形式表示，并通过简化得到如式(23)所示:

Hx＝b (23)

其中，

最后方程的最小方差解如式(25)所示:

x＝(H ^TH) ^-1H ^Tb (25)

基于IMU和UWB融合的室内定位导航算法：

在惯性导航系统中，由于误差积分，陀螺和加速度容易发散。UWB可以在室内定位系统中提供厘米级的测距和定位精度，但无线信号容易受到非视线传播的影响。因此，通过一种或多种数学方法对IMU和UWB传感器3的数据进行优化，可以有效地提高目标的定位精度。

线性卡尔曼滤波器可以在线性高斯模型条件下对目标状态进行最优估计。然而，实际系统中总是存在不同程度的非线性。典型的非线性函数关系包括平方关系、对数关系、指数关系、三角函数关系等。一些非线性系统可以近似为线性系统，但为了精确地估计系统的状态，大多数系统不能只用线性微分方程来描述，如飞机的飞行状态、导弹制导系统、卫星导航系统等，非线性因素是不可忽略的，因此必须建立非线性系统滤波算法。本发明提出了两种非线性卡尔曼滤波算法，包括扩展卡尔曼滤波和无迹卡尔曼滤波。基于此，本发明中的卡尔曼滤波融合可以是基于观测距离的扩展卡尔曼滤波融合或者基于距离和角度的无迹卡尔曼滤波融合。

在二维平面中，目标的位置可以通过至少三个观察距离来确定。在三维空间中，至少需要四个观测距离来定位目标。基于观测距离的扩展卡尔曼滤波融合(EKF)，包括：

初始化:

初始化系统状态向量X ₀＝E[X ₀]，初始化系统状态协方差矩阵 P(0)＝var[X(0)]；

迭代：

For t＝1:k，I _n为n×n单位矩阵；

对状态做一步预测：

状态协方差矩阵的一步预测：

P(k+1|k)＝F(k+1|k)P(k|k)F ^T(k+1|k)+GQG ^T；

求卡尔曼滤波增益矩阵：

K(k+1)＝P(k+1|k)H ^T(k+1)[H(k+1)P(k+1|k)H ^T(k+1)+R] ^-1；

状态更新：

协方差矩阵更新：

P(k+1|k+1)＝[I _n-K(k+1)H(k+1)]P(k+1|k)。

在二维平面中，假设目标在k时刻的状态向量包含位置、速度和加速度信息，用式(26)表示：

X(k)＝[x _x(k) x _y(k) v _x(k) v _y(k) a _x(k) a _y(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)是观测噪声矩阵，[d ₁(k) d ₂(k) ... d _n(k)] ^T表示目标与超宽带基站之间的测距矩阵如式(32)所示：

由于观测方程32是非线性的，需要对其进行线性化。对它进行一阶泰勒展开，H(k)是线性化观测矩阵，即雅可比矩阵。具体的偏导数结果如方程33所示。在视距条件下，测距噪声服从高斯分布，均值为0，方差为σ _d。在非视线条件下，测距噪声仍服从高斯分布，但其均值不为零，方差增大，R对应于噪声方差矩阵如式(35)所示：

由于EKF算法将非线性系统方程或观测方程进行泰勒展开，保留了一阶近似项，不可避免地引入了线性化误差。如果线性化假设不成立，滤波器的性能将下降，滤波器将发散。另外，状态方程和观测方程的雅可比矩阵不易计算，增加了算法的计算复杂度。基于距离和角度的无迹卡尔曼滤波融合(UKF)摒弃了非线性函数线性化的传统做法，采用了卡尔曼线性滤波框架。对于一步预测方程，采用无迹变换处理均值和方差的非线性传递。UKF算法近似非线性函数的概率密度分布，使用一组确定性样本强制状态的后验概率密度，而不是近似非线性函数，而不采用雅可比矩阵的导数。UKF不忽略高阶项，对非线性分布的统计具有较高的计算精度，有效地克服了EKF滤波器估计精度低、稳定性差的缺点。基于距离和角度的无迹卡尔曼滤波融合包括：

获得一组Sigma点集及其对应权值：

计算2n+1个Sigma点集的一步预测：

X ⁽ⁱ⁾(k+1|k)＝f[k,X ⁽ⁱ⁾(k|k)],i＝1,2,...,2n+1；

计算系统状态量的一步预测及协方差矩阵：

根据一步预测值，再次使用UT变换，产生新的Sigma点集：

将预测产生的Sigma点集带入观测方程得到预测的观测量；

Z ⁽ⁱ⁾(k+1|k)＝h(X ⁽ⁱ⁾(k+1|k))；

计算Kalman增益矩阵：

计算系统的状态更新和协方差更新：

系统还包括单个静态观测基站，UWB传感器3用于检测目标与观测基站之间的距离，IMU传感器1用于检测目标与观测基站之间的角度，基于单个静态观测基站的二维精确定位的原理如图4所示。融合位置计算单元5通过下述方式基于单个静态观测基站对目标进行二维精确定位：

X(k)＝[x _x(k) x _y(k) v _x(k) v _y(k) a _x(k) a _y(k)] ^T

X(k+1)＝FX(k)+W(k)

其中F表示状态转换矩阵，W(k)表示0的平均值如下式所示：

方差为Q过程噪声如下式所示：

UWB传感器3检测目标与观测基站之间的距离为d，IMU传感器1检测目标与观测基站之间的方位角为

均方根误差(RMSE)如下式所示：

基于IMU和UWB融合的匀速运动和匀加速运动的近似模型算法：

由于目标的运动通常是随机的，因此很难建立精确的运动模型。因此，我们提出了两种近似运动模型，一种是将目标的随机运动作为短时间内的近似匀速运动(AUAM)模型，即：

融合位置计算单元5结合UWB位置计算单元4计算的定位数据、IMU位置计算单元2计算的速度差数据和加速度数据进行AUAM过滤处理，包括：

初始化：初始化UWB传感器3采样周期T＝0.02s，初始化IMU传感器1采样周期t＝0.01s；

基于非对称双边双向测距法完成目标到基站距离测量：

d＝[d ₁(k) d ₂(k) ... d _n(k)]；

根据三边测量算法计算目标最小二乘位置：

X(k)＝(H ^TH) ^-1H ^Tb；

根据目标位置计算差分速度：

将IMU加速度坐标转换：

更新目标位置：

另一种是将目标的随机运动作为短时间内的近似匀速运动(AUM)模型，即融合位置计算单元5结合UWB位置计算单元4计算的定位数据和速度差数据进行AUM过滤处理，包括：

初始化：初始化UWB传感器3采样周期T＝0.02s；

基于非对称双边双向测距法完成目标到基站距离测量：

d＝[d ₁(k) d ₂(k) ... d _n(k)]；

根据三边测量算法计算目标最小二乘位置：

X(k)＝(H ^TH) ^-1H ^Tb；

根据目标位置计算差分速度：

更新目标位置：

X(k+1)＝X(k)+V(k)T。

上述实施例仅为优选实施例，并不用以限制本发明的保护范围，在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

一种基于IMU和UWB融合的室内定位导航系统，其特征在于，包括IMU传感器、IMU位置计算单元、UWB传感器、UWB位置计算单元、融合位置计算单元；

所述IMU传感器安装于目标上，用于检测目标的三轴加速度和三轴角速度，并将所述三轴加速度和三轴角速度发送给所述IMU位置计算单元；

所述IMU位置计算单元根据所述三轴加速度和三轴角速度对目标进行定位，得到目标的第一定位数据；

所述UWB传感器包括固定在目标上的未知节点和固定在空间已知位置上的四个基准基站节点，用于检测所述未知节点与所述四个基准基站节点之间的相对位置关系数据，并将所述相对位置关系数据发送到所述UWB位置计算单元；

所述UWB位置计算单元根据所述相对位置关系对所述目标进行定位，得到目标的第二定位数据；

所述融合位置计算单元对所述IMU位置计算单元和所述UWB位置计算单元的数据进行卡尔曼滤波融合，得到最终的目标定位数据。
如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述卡尔曼滤波融合为基于观测距离的扩展卡尔曼滤波融合，包括：

初始化:

初始化系统状态向量X ₀＝E[X ₀]，初始化系统状态协方差矩阵 P(0)＝var[X (0)]；

迭代：

For t＝1:k，I _n为n×n单位矩阵；

对状态做一步预测：

状态协方差矩阵的一步预测：

P(k+1|k)＝F(k+1|k)P(k|k)F ^T(k+1|k)+GQG ^T；

求卡尔曼滤波增益矩阵：

K(k+1)＝P(k+1|k)H ^T(k+1)[H(k+1)P(k+1|k)H ^T(k+1)+R] ^-1；

状态更新：

协方差矩阵更新：

P(k+1|k+1)＝[I _n-K(k+1)H(k+1)]P(k+1|k)。
如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述卡尔曼滤波融合为基于距离和角度的无迹卡尔曼滤波融合，包括：

获得一组Sigma点集及其对应权值：

计算2n+1个Sigma点集的一步预测：

X ⁽ⁱ⁾(k+1|k)＝f[k,X ⁽ⁱ⁾(k|k)],i＝1,2,…,2n+1；

计算系统状态量的一步预测及协方差矩阵：

根据一步预测值，再次使用UT变换，产生新的Sigma点集：

将预测产生的Sigma点集带入观测方程得到预测的观测量；

Z ⁽ⁱ⁾(k+1|k)＝h(X ⁽ⁱ⁾(k+1|k))；

将得到的Sigma点集的观测预测值通过加权求和得到系统预测的值及协方差：

计算Kalman增益矩阵：

计算系统的状态更新和协方差更新：
如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，还包括单个静态观测基站，所述UWB传感器用于检测目标与观测基站之间的距离，所述IMU传感器用于检测目标与观测基站之间的角度；所述融合位置计算单元通过下述方式基于单个静态观测基站对目标进行二维精确定位：

设目标在二维平面内以近似均匀的加速度移动，时间k的状态向量包含位置、速度和加速度信息，用下式表示：

X(k)＝[x _x(k) x _y(k) v _x(k) v _y(k) a _x(k) a _y(k)] ^T

两个方向的运动都有加性系统噪声W(k)，目标的运动状态方程如下式所示：

X(k+1)＝FX(k)+W(k)

其中F表示状态转换矩阵，W(k)表示0的平均值如下式所示：

方差为Q过程噪声如下式所示：

UWB传感器检测目标与观测基站之间的距离为d，IMU传感器检测目标与观测基站之间的方位角为

观测噪声V(k)是一个均值为0，方差为R的加性噪声，笛卡尔坐标系下的目标观测方程如下式所示：

均方根误差(RMSE)如下式所示：
如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据、所述IMU位置计算单元计算的速度差数据和加速度数据进行AUAM过滤处理，包括：

初始化：初始化UWB传感器采样周期T＝0.02s，初始化IMU传感器采样周期t＝0.01s；

基于非对称双边双向测距法完成目标到基站距离测量：

d＝[d ₁(k) d ₂(k) … d _n(k)]；

根据三边测量算法计算目标最小二乘位置：

X(k)＝(H ^TH) ^-1H ^Tb；

根据目标位置计算差分速度：

将IMU加速度坐标转换：

更新目标位置：
如权利要求1所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，所述融合位置计算单元结合所述UWB位置计算单元计算的定位数据和速度差数据进行AUM过滤处理，包括：

初始化：初始化UWB传感器采样周期T＝0.02s；

基于非对称双边双向测距法完成目标到基站距离测量：

d＝[d ₁(k) d ₂(k) … d _n(k)]；

根据三边测量算法计算目标最小二乘位置：

X(k)＝(H ^TH) ^-1H ^Tb；

根据目标位置计算差分速度：

更新目标位置：

X(k+1)＝X(k)+V(k)T。
如权利要求6所述的基于IMU和UWB融合的室内定位导航系统，其特征在于，非对称双边双向测距法的步骤包括：

设备A向设备B发送一个轮询消息包并记录发送时间；

设备B接收轮询消息包并记录接收时间；

设备B等待先前设置的延迟处理时间，将响应消息包发送到设备A并记录发送时间；

设备A接收响应消息包并记录接收时间，从而完成一个范围任务；

当设备A接收到设备B发送的响应消息包后，设备A将最终消息包发送到设备B；

设备B接收到最终消息包并记录接收时间。