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

技术特征：

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

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

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

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

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

2.根据权利要求1所述基于UWB技术的室内行人导航方法，其特征在于：在步骤(1)中，初始化惯性导航系统时，需要对惯性传感器进行标定，扣除三轴加速度计和三轴陀螺仪的零偏值；取静止时刻三轴陀螺仪的输出作为三轴陀螺仪的零偏值；采用三位置加速度计标定法计算三轴加速度计的零偏值，如下式所示：

$\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]=\frac{1}{2}{\left[\begin{array}{ccc}{A}_{1x}& A_{1y}& A_{1z}\\ A_{2x}& A_{2y}& A_{2z}\\ A_{3x}& A_{3y}& A_{3z}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

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

3.根据权利要求1所述基于UWB技术的室内行人导航方法，其特征在于，在步骤(1)中，对UWB基站进行优化配置是指，仅在室内关键节点处配置一定数量的UWB基站设备，保证UWB芯片位于关键节点处时，能够接收到不少于4个的中等强度的基站信号或者UWB芯片定位的RMSE值小于0.2m。

4.根据权利要求1所述基于UWB技术的室内行人导航方法，其特征在于，步骤(2)的步骤如下：

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

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

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

5.根据权利要求4所述基于UWB技术的室内行人导航方法，其特征在于：在步骤(21)中，判断惯性传感器数据是否满足零速判定条件的过程：

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

$\left\{\begin{array}{c}{\mathrm{\ω}}_x=\left|{\mathrm{\ω}}_x^{\max }-{\mathrm{\ω}}_x^{\min }\right|\\ {\mathrm{\ω}}_y=\left|{\mathrm{\ω}}_y^{\max }-{\mathrm{\ω}}_y^{\min }\right|\\ {\mathrm{\ω}}_z=\left|{\mathrm{\ω}}_z^{\max }-{\mathrm{\ω}}_z^{\min }\right|\\ A=\underset{i=0}{\overset{N}{\max }}\left(\sqrt{{a_{xi}}^2+{a_{yi}}^2+{a_{zi}}^2}\right)\end{array}\right.$

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

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

6.根据权利要求4所述基于UWB技术的室内行人导航方法，其特征在于：步骤(22)的具体过程：

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

$\left\{\begin{array}{c}\mathrm{\θ}=arcsin(f_{iby}^b/g)\\ \mathrm{\γ}=-\arctan (f_{ibx}^b/f_{ibz}^b)\end{array}\right.$

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

7.根据权利要求1所述基于UWB技术的室内行人导航方法，其特征在于，步骤(3)的步骤如下：

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

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

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

8.根据权利要求1-7中任意一项所述基于UWB技术的室内行人导航方法，其特征在于：在步骤(3)中，卡尔曼滤波器状态方程与量测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

上式中：

$\left\{\begin{array}{c}{\mathrm{\Φ}}_{k,k-1}=\underset{n=0}{\overset{\mathrm{\∞}}{\Σ}}{\[F\left(t_k\right)T\]}^n/n!\\ {\mathrm{\Γ}}_{k-1}=\{\underset{n=1}{\overset{\mathrm{\∞}}{\Σ}}\[\frac{1}{n!}{\left(F\right(t_k\left)T\right)}^{n-1}\]\}G\left(t_k\right)T\end{array}\right.$

其中，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为离散周期。

9.根据权利要求1-7中任意一项所述基于UWB技术的室内行人导航方法，其特征在于：卡尔曼滤波器的状态量为：

其中，分别为惯导东、北、天三个方向的数学平台误差角，δ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解算位置在东、北、天三个方向的差值。