一种基于UWB技术的室内行人导航方法与流程

本发明属于行人导航技术领域，特别涉及了一种基于UWB技术的室内行人导航方法。

背景技术：

基于卫星定位的行人导航系统已被广泛应用于定位领域，由于卫星信号容易被建筑物阻隔，产生多路径、非视距、时变性等问题，因此卫星导航接收机很难实现室内的高精度定位应用。美国Draper实验室于20世纪末提出将惯性测量器件安装在人体的足部实现人体定位功能，其中应用零速修正技术较好地解决了低成本惯性导航系统误差发散快的问题。

由于室内航向角很难精确获得，因此行人惯性导航系统中的航向角误差已成为惯性导航系统误差的主要来源之一。当前解决航向角发散问题的方法主要有：磁航向辅助算法、陀螺旋转修正算法等。磁航向辅助算法主要运用于室外，室内磁场环境复杂，很难应用该算法；陀螺旋转修正算法多用于机器，行人无法满足算法所需的陀螺匀速旋转的条件。对室内行人惯性导航系统航向进行修正的重要性日益突出。

UWB(超宽带无线技术)是当前国内外定位领域研究热点之一。在室内定位过程中，UWB具有厘米级的定位精度、良好的抗多路径性能、较低的发射功率及一定的穿透能力，与WiFi、ZigBee、RFID等室内定位技术相比优势显著。目前，国内外研究者对惯导/UWB组合的研究主要集中在UWB与IMU组合实现无人机的室内定位，UWB与PDR/SLU(Pedestrian Dead Reckoning/Step Length Update)组合实现行人导航。但是需要指出PDR/SLU对行人的步长、步数精度要求较高，这限制了最终的导航精度。

技术实现要素：

为了解决上述背景技术提出的技术问题，本发明旨在提供一种基于UWB技术的室内行人导航方法，在未知行人精确步长的情况下，使用基于零速修正的惯性导航系统抑制惯性导航定位误差随时间发散，依靠优化配置下的UWB与惯性导航结果进行卡尔曼滤波，实现低成本、高精度的室内导航。

为了实现上述技术目的，本发明的技术方案为：

一种基于UWB技术的室内行人导航方法，包括以下步骤：

(1)将惯性传感器和UWB芯片安装在行人的足部，对UWB基站进行优化配置，初始化惯性导航系统，获得初始位置信息和目的地信息；

(2)获取惯性传感器的三轴加速度计与三轴陀螺仪数据，由惯性导航系统进行实时导航解算，同时采用零速修正算法进行误差修正；

(3)判断行人是否位于室内关键节点处，综合UWB芯片的定位信息与惯性导航系统的解算信息，进行卡尔曼滤波，获得定位结果；

(4)判断是否到达目的地，在到达目的地后结束导航。

进一步地，在步骤(1)中，初始化惯性导航系统时，需要对惯性传感器进行标定，扣除三轴加速度计和三轴陀螺仪的零偏值；取静止时刻三轴陀螺仪的输出作为三轴陀螺仪的零偏值；采用三位置加速度计标定法计算三轴加速度计的零偏值，如下式所示：

上式中，b_x、b_y、b_z分别为三轴加速度计的零偏值，A_ix、A_iy、A_iz分别为第i个位置的三轴加速度计的输出均值，i＝1,2,3，m_i表示第i个位置的三轴加速度计输出的平方和，g表示当地重力加速度。

进一步地，在步骤(1)中，对UWB基站进行优化配置是指，仅在室内关键节点处配置一定数量的UWB基站设备，保证UWB芯片位于关键节点处时，能够接收到不少于4个的中等强度的基站信号或者UWB芯片定位的RMSE值小于0.2m。

进一步地，步骤(2)的步骤如下：

(21)判断惯性传感器数据是否满足零速判定条件，若满足，则进入步骤(22)，若不满足，则进入步骤(23)；

(22)采用零速修正算法修正速度，并重新计算姿态角以修正惯性导航解算结果；

(23)采用捷联惯性导航算法得到导航结果。

进一步地，在步骤(21)中，判断惯性传感器数据是否满足零速判定条件的过程：

根据行走状态设置滑动窗口宽度N的大小，定义相关变量：

上式中，ω_x、ω_y、ω_z分别为滑动窗口范围内陀螺仪各轴输出量的极差，max表示极大值，min表示极小值，A为滑动窗口范围内三轴加速度计输出矢量的最大幅值；

当ω_x、ω_y、ω_z、A中至少有1个变量的值处于预设的阀值范围内，则认为当前设于足部的惯性测量单元处于零速状态，需要进行零速修正。

进一步地，步骤(22)的具体过程：

进行零速修正时，将速度置零，同时利用下式重新计算姿态角：

上式中，θ、γ分别为俯仰、横滚角度，分别为三轴加速度计输出，g为当地重力加速度值。

进一步地，步骤(3)的步骤如下：

(31)判断UWB芯片接收到中等强度的基站信号是否不少于4个或者UWB芯片定位的RMSE值是否小于0.2m，若是，则说明行人处于关键节点处，此时进入步骤(32)，若否，则进入步骤(33)；

(32)获取UWB定位结果，将惯性导航系统解算结果与UWB定位结果进行卡尔曼滤波，修正位置信息；

(33)采用惯性导航系统解算结果作为定位结果。

进一步地，在步骤(3)中，卡尔曼滤波器状态方程与量测方程为：

上式中：

其中，X_k、X_k-1分别为k、k-1时刻的状态量，Z_k为k时刻的量测量，Φ_k,k-1为状态转移矩阵，Γ_k-1为k-1时刻的系统噪声转移矩阵，H_k为k时刻的量测矩阵，W_k-1为k-1时刻的系统噪声、V_k为k时刻的量测噪声，F(t_k)为系统矩阵，G(t_k)为系统噪声矩阵，T为离散周期。

进一步地，卡尔曼滤波器的状态量为：

其中，分别为惯导东、北、天三个方向的数学平台误差角，δv_E,δv_N,δv_U分别为惯导东、北、天三个方向的速度误差，δL,δλ,δh分别为惯导东、北、天三个方向的位置误差，δK_gx,δK_gy,δK_gz,δK_ax,δK_ay,δK_az分别为陀螺仪X、Y、Z轴和加速度计X、Y、Z轴的标定因数误差；

卡尔曼滤波的量测量为：

Z＝[δγ,δθ,δψ,δv_E,δv_N,δv_U,δL,δλ,δh]^T

其中，δγ,δθ,δψ分别为当前时刻惯性导航解算的横滚角、俯仰角、航向角与零速时刻加速度计计算的横滚角、俯仰角、航向角的差值，δv_E,δv_N,δv_U分别为当前时刻惯性导航解算东北天三个方向的速度与零速差值，δL,δλ,δh分别为当前时刻惯性导航解算位置与UWB解算位置在东、北、天三个方向的差值。

采用上述技术方案带来的有益效果：

本发明相对现有的UWB与PDR/SLU组合算法而言，使用了基于零速修正的惯性导航系统，不需要获知行人的精确步长、步数，能较好的抑制惯性导航定位误差随时间发散；依靠UWB定位结果与惯性导航结果进行卡尔曼滤波，修正惯性导航系统的航向与位置，同时在UWB受干扰定位出现偏差时，利用惯性导航保证定位的准确性；优化配置UWB基站设备，实现了低成本高精度的室内导航。

附图说明

图1是本发明的方法流程图；

图2是TOA定位方法的几何模型图；

图3是卡尔曼滤波递推回路图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明提供的一种基于UWB技术的室内行人导航方法，主要应用于行人室内的导航定位。具体实施中还可以根据本发明核心思想用于弱卫星信号环境中的行人位置监控追踪。

如图1所示，该方法包括如下步骤：

步骤1：将惯性传感器和UWB芯片安装在行人的足部，对UWB基站进行优化配置，初始化惯性导航系统，获得初始位置信息和目的地信息。

在UWB基站设备安装时对其进行优化配置，即仅在关键节点处配置一定数量UWB基站设备，要求保证关键节点处UWB芯片接收到中等强度信号基站不少于4个或UWB定位RMSE值小于0.2m。所述关键节点为拐角、楼梯口等行人行走过程中会发生较大航向变化的位置区域。

惯性传感器与UWB芯片为安在足部的MEMS/UWB组合器件。由于低成本惯性器件稳定性、重复性差，每次使用前对其进行标定，扣除三轴陀螺仪和三轴加速度计的零偏误差，可以大幅提升传感器的精度。采用陀螺仪取静止时刻的输出作为零偏，但加速度计无法仅由某一位置的输出求出零偏，故使用一种三位置加速度计标定法，通过传感器在三个不同位置的静态数据，求出加速度计的零偏值。该方法简单实用，适用于安装完成后的现场标定方法。算法如下式所示：

上式中，b_x、b_y、b_z分别为三轴加速度计的零偏值，A_ix、A_iy、A_iz分别为第i个位置的三轴加速度计的输出均值，i＝1,2,3，m_i表示第i个位置的三轴加速度计输出的平方和，g表示当地重力加速度。

从解算终端获取初始位置与目的地在预设地图上的位置信息，开始导航。当然在其他优选实施例中，惯性系统的初始化及初始位置的获取也可由其他简易的方式来实现，本发明对此不作特别限定。

步骤2：获取惯性传感器的三轴加速度计与三轴陀螺仪数据，由惯性导航系统进行实时导航解算，同时采用零速修正算法进行误差修正。

首先判断惯性传感器数据是否满足零速判定条件，本发明采取了以陀螺仪输出为主、加速度计输出为辅的滑动窗口判断方法，对足部的零速时刻进行判定。基于不同的行走状态将滑动窗口宽度设置为变量N，该值的大小与行走状态有关，下式定义了相关变量：

上式中，ω_x、ω_y、ω_z分别为滑动窗口范围内各轴陀螺仪输出量的极差，max表示极大值，min表示极小值，A为滑动窗口范围内三轴加速度计输出矢量的最大幅值。当ω_x、ω_y、ω_z、A中至少有1个变量的值处于预设的阈值范围内，则认为当前设于足部惯性测量单元处于零速状态，进行零速修正。

当惯性器件处于零速修正时刻，将其速度置零，同时利用下式使用加速度计输出信息重新计算该器件的横滚角与俯仰角：

其中，θ、γ分别为俯仰、横滚角度，分别为三轴加速度计输出，g为当地的重力加速度值。

当惯性器件未处于零速时刻时，采用传统捷联解算方法，递推得到自身的位置与姿态。

步骤3：判断行人是否位于室内关键节点处，综合UWB芯片的定位信息与惯性导航系统的解算信息，进行卡尔曼滤波，获得定位结果。

在UWB基站设备安装时对其进行优化配置，即仅在关键节点处配置一定数量UWB基站设备，要求保证关键节点处UWB芯片接收到中等强度信号基站不少于4个或UWB定位RMSE值小于0.2m。所述关键节点为拐角、楼梯口等行人行走过程中会发生较大航向变化的位置区域。

首先检测UWB芯片接收到的信号是否满足预设要求(中等强度信号基站不少于4个或UWB定位RMSE值小于0.2m)。

当信号满足要求时，获取UWB定位结果，本发明采用TOA方法：精确测量定位芯片发出的无线脉冲信号到达各基站所经过的时间，乘以光速即获得芯片与各基站之间的精确距离，从而解算出较高精度的芯片位置。如图2所示，各基站BS_i的坐标是(x_i,y_i,z_i)，定位芯片MS的坐标是(x,y,z)。定位芯片MS与各基站BS_i之间的距离r_i都可以确定一个以基站BS_i为球心，r_i为半径的球体。这三个球的交点就是定位芯片MS的位置。大部分情况下，由于r₁、r₂、r₃的测量值存在误差，三球并不会正好相交于一点，因此需要N个(N≥4)基站才能完成定位，如下式所示：

上式通过平方和求差可得下式：

N个定位基站可得N-1个类似上式的方程，写成如下式所示的矩阵形式：

MI＝b

其中，

I＝[x,y,z]^T

根据基站的数据，获得M和b的值，利用最小二乘法，即可求出定位芯片MS的坐标I。当然在其他优选实施例中，UWB芯片可以通过TDOA/AOA/RSSI的方法进行定位，本发明对此不作特别限定。

进一步地，将惯性导航系统解算结果与UWB定位结果进行卡尔曼滤波，修正位置信息。

具体地，卡尔曼滤波器状态方程与量测方程为：

上式中：

其中，X_k为状态量，Φ_k,k-1为状态转移矩阵，Γ_k-1为系统噪声转移矩阵，H_k为量测矩阵，W_k-1和V_k分别表示系统噪声和量测噪声，F(t_k)为系统矩阵，G(t_k)为系统噪声矩阵，T为离散周期。

其中：卡尔曼滤波要求系统噪声和量测噪声是互不相关的白噪声序列，即满足协方差阵：

cov{W_k,W_j^T}＝Q_kδ_kj

cov{V_k,V_j^T}＝R_kδ_kj

其中，Q_k,R_k分别称为系统噪声方差矩阵和量测噪声方差矩阵，它们分别是已知值的非负定阵和正定阵。δ_kj为Kronecker函数，即仅当k＝j的时刻为1。卡尔曼滤波依据前一时刻的滤波估计值和当前时刻的量测量进行当前状态的最优估计，递推过程中存在两个回路，如图3所示。图3中，表示状态一步预测值，表示状态估计值，P_k,k-1表示状态一步预测均方误差，P_k,k表示量测更新后的估计值均方误差，K_k为滤波增益。以上两个回路循环递推得到状态的估计值。

卡尔曼滤波器的状态量为：

其中，分别为惯导东北天三个方向的数学平台误差角，δv_E,δv_N,δv_U分别为惯导东北天三个方向的速度误差，δL,δλ,δh分别为惯导东北天三个方向的位置误差，δK_gx,δK_gy,δK_gz,δK_ax,δK_ay,δK_az分别为陀螺X,Y,Z轴和加表X,Y,Z轴的标定因数误差。状态量初值需要按照实际情况自主设置。

卡尔曼滤波的量测量为：

Z＝[δγ,δθ,δψ,δv_E,δv_N,δv_U,δL,δλ,δh]^T

其中，δγ,δθ,δψ分别为当前时刻惯导解算的横滚角、俯仰角、航向角与零速时刻加速度计计算的横滚角、俯仰角、航向角的差值，δv_E,δv_N,δv_U分别为当前时刻惯导解算东北天三个方向的速度与零速差值，δL,δλ,δh分别为当前时刻惯导解算位置与UWB解算位置在东北天三个方向的差值。

当信号不满足要求时，使用惯性导航系统解算结果为定位结果。

步骤4：判断是否到达目的地，如是，则结束导航，如否，则返回步骤2。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。