CN114485639A - 一种用于室内导航的uwb定位漂移校正方法 - Google Patents

Abstract

本发明公开了一种用于室内导航的UWB定位漂移校正方法，首先建立室内三维坐标系，将UWB定位基站安装于固定位置，产生一组足够数量的、带有影响力权值的粒子来近似地描述初始系统状态；然后根据运动模型进行系统状态递推；接着根据UWB获取的距离观测值与手机传感器获取的运动数据观测值对系统状态进行修正；最后将各个状态粒子的状态量进行合并，得出最终的位置估计值。本发明克服了现有UWB定位易出现定位漂移的缺点，有效提高了定位精度。

Description

一种用于室内导航的UWB定位漂移校正方法

技术领域

本发明属于室内导航领域，尤其涉及一种用于室内导航的UWB定位漂移校正方法。

背景技术

目前，GPS定位系统已被广泛应用于室外导航领域，但其在室内却不能发挥作用，原因主要在于：1)信号问题，室内场景有建筑物遮挡，GPS卫星信号快速衰减，甚至可能搜索不到；2)精度问题，在室外导航场景下，几米的误差通常也可以接受，而对于室内导航，精度要求往往更高。

在室内导航场景中，大多使用UWB(超宽带)定位技术。UWB定位技术是一种采用纳秒级别的非正弦窄脉冲作为介质的无线载波通讯技术，因为其具有复杂性低、传输速率高、穿透能力强等特点而成为室内定位的优选方案。UWB定位技术采用基站-标签式的定位方法，其中基站相当于天上的卫星，安装于固定的参考点，标签作为待定位点，安装于待定位的载体，如行人。利用UWB技术可以获得各个基站与标签之间的距离，然后再以基站位置为球心，基站到标签的距离为半径画球，列出方程解算求出各个球面的公共交点，即为标签坐标。

在理想情况下，以基站位置为球心，基站到标签的距离为半径画球，各个球面会交于标签位置，但实际环境错综复杂，障碍物会阻挡UWB信号传播造成延迟，以及设备测量时的精度有限，基站与标签间的距离始终会存在误差，使得各个球面无法相交于一个点，对定位精度造成很大影响，出现定位漂移的情况。为了解决这一问题，传统的方法通常是在求解球面公共交点时，采取方法估计最优解，比如最小二乘法、三角形质心法等，但都具有计算量大、精度不高等缺点。

发明内容

本发明的目的在于针对现有技术中UWB方法易出现定位漂移的不足，提供一种用于室内导航的UWB定位漂移校正方法。

本发明的目的是通过以下技术方案来实现的：一种用于室内导航的UWB定位漂移校正方法，包括以下步骤：

S1、状态初始化：建立室内空间xyz三维坐标系，将UWB定位基站安装于空间中的固定位置，然后产生一组足够数量的、带有影响力权值的粒子来近似地描述初始系统状态，所述系统状态包括[x y z s θ l]，分别表示行人的x坐标、y坐标、z坐标、行走步数、行走方向、每一步的步长；

S2、状态递推：根据运动模型进行系统状态递推，递推方程为如下形式：

其中，[x_k y_k z_k s_k θ_k l_k]^T代表系统第k时刻的状态，[x_k-1 y_k-1 z_k-1 s_k-1 θ_k-1l_k-1]^T代表系统第k-1时刻的状态，[noise_x，k-1 noise_y，k-1 noise_z，k-1 nosie_s，k-1 noise_θ，k-1noise_l，k-1]^T代表系统第k-1时刻的各类预测噪声；

S3、状态修正：根据UWB获取的距离观测值[D_1，k D_2，k D_3，k...]与手机传感器获取的运动数据观测值[S_k Θ_k]对系统状态进行修正，其中D_i，k表示第i个定位基站在k时刻观测到的距离值，S_k表示手机传感器在k时刻观测到的行人行走步数，Θ_k表示手机传感器在k时刻观测到的行人行走方向；

S4、合并估计：将k时刻各个状态粒子的状态量进行合并，得出k时刻最终的行人位置估计值

进一步地，步骤S1中所述的产生一组足够数量的、具有权值的粒子来近似地描述初始系统状态，包括以下步骤：

S1.1、行人开始行走一小段时间后，获取UWB的距离观测值[D_1，0 D_2，0 D_3，0...]与手机传感器的运动数据观测值[S₀ Θ₀]，其中D_i，0表示第i个定位基站在初始时刻观测到的标签距离值，S₀表示手机传感器在初始时刻观测到的行人行走步数，Θ₀表示手机传感器在初始时刻观测到的行人行走方向；

S1.2、以基站坐标为球心，基站到标签的距离[D_1，0 D_2，0 D_3，0...]为半径画球，列出方程，求出各个球面的公共交点[X₀ Y₀ Z₀]作为初始时刻的行人坐标；

S1.3、将50cm作为行人的步长初始值L₀；

S1.4、根据所述1.1)得到[S₀ Θ₀]、所述1.2)得到[X₀ Y₀ Z₀]、所述1.3)得到L0后，将[X₀ Y₀ Z₀ S₀ Θ₀ L₀]作为系统状态初始值，引入可能存在的观测误差后，假设初始时刻系统状态满足多维正态分布：

其中σ₀ ²、σ₁ ²、σ₂ ²、σ₃ ²、σ₄ ²、σ₅ ²分别表示系统初始时刻对行人x坐标、y坐标、z坐标、行走步数、行走方向、步长的观测误差。

根据如上的概率分布，在matlab中，随机产生初始粒子集合：

其中，N为提前指定的粒子数量，

为第j个粒子在初始时刻的影响力权重，

为第j个粒子在初始时刻的系统状态，Z表示整数集合。

进一步地，步骤S2中所述的根据运动模型进行系统状态递推，具体做法是对每个粒子执行递推方程更新粒子状态。

进一步地，步骤S3中所述的根据UWB获取的距离观测值[D_1，k D_2，k D_3，k…]与手机传感器获取的运动数据观测值[S_k Θ_k]对系统状态进行修正，具体做法是，假设各个观测相互独立，则根据观测方程：

更新粒子权值的公式为：

f_S～N(0，σ_s ²)

f_θ～N(0，σ_θ ²)

其中，[x_k y_k z_k s_k θ_k l_k]为k时刻利用传感器观测到的系统状态，

为第j个粒子在k时刻的预测系统状态，在实际情况下，k时刻利用传感器观测到的系统状态与通过运动方程预测的系统状态往往存在误差，所以需要根据观测值，对每一个粒子j在k时刻更新影响力权重，误差越大，权重越小，M为基站数量，(Loc_i，x Loc_i，y Loc_i，z)为第i个基站在三维空间中的位置坐标，σ_s为手机传感器测量行人步数时误差，σ_θ为手机传感器测量行人行走方向时误差，f_D，i、f_S、f_θ分别表示基站测量距离、手机传感器测量行人步数、手机传感器测量行人行走方向时的观测噪声。

进一步地，步骤S4中所述的将k时刻各个状态粒子的状态量进行合并，得出k时刻最终的行人位置估计值

具体的计算公式为：

其中，N为粒子总数，

为k时刻第j个粒子的影响力权重，

分别为k时刻第j个粒子的x坐标、y坐标、z坐标状态量。

相比于现有技术，本发明具有如下的有益效果：

1、结合运动模型与UWB设备、手机传感器的观测结果对行人的位置进行预测，提高精度，减少定位漂移。运动模型不依赖外部环境信息，且计算简单，能连续快速地提供行人的实时位置，但存在累计误差，运行时间越长，累计误差越大；UWB设备与手机传感器的观测值依赖外部环境，存在观测误差，但不随时间累计，每次误差相对独立；将两者结合，起到互补作用，能有效降低总体误差；

2、采用手机传感器采集运动数据，成本低、实时性好、操作方便；

3、避免了传统UWB方法求球面公共交点的复杂计算和可能出现的无解情况，传统的UWB方法需要列出方程解算求球面的公共交点作为行人的位置坐标，求解交点的方程计算量大，而且可能出现无解或者局部最优解的情况，而本发明的位置坐标由运动模型预测而来，计算十分简单；

4、充分运用到所有定位基站的距离观测量，并设置不同的影响力权重。在UWB定位方法中，不同的基站节点对标签节点的定位有不同的影响力，与未知节点通信质量越好的基站节点，影响力越大，传统UWB定位方法通常做法是选取离标签近的4个节点用于定位计算，这种做法既丢失了一部分距离观测数据，又没有对筛选出来的4个节点进行影响力区分。本发明不仅运用到所有的距离观测量，并通过对观测值设置不同的方差，间接实现对观测值设置不同的影响力权重。

附图说明

图1是本发明的流程图；

图2是本发明基站与标签设置示意图。

具体实施方式

下面结合附图对本发明作进一步描述。

本发明一种用于室内导航的UWB定位漂移校正方法，建立运动模型对行人的位置进行递推预测，并结合UWB获取的距离观测值与手机传感器获取的运动数据观测值，不断对位置预测值进行修正，有效解决了定位漂移问题，提高定位精度。

如图1所示，包括以下步骤：

S1、状态初始化：建立室内空间xyz三维坐标系，将UWB定位基站安装于空间中的固定位置，然后产生一组足够数量的、带有影响力权值的粒子来近似地描述初始系统状态。所述系统状态包括[x y z s θ l]，分别表示行人的x坐标、y坐标、z坐标、行走步数、行走方向、每一步的步长。包括以下步骤：

S1.1、行人开始行走一段时间后，获取UWB的距离观测值[D_1，0 D_2，0 D_3，0…]与手机传感器的运动数据观测值[S₀ Θ₀]，其中D_i，0表示第i个定位基站在初始时刻观测到的标签距离值，S₀表示手机传感器在初始时刻观测到的行人行走步数，Θ₀表示手机传感器在初始时刻观测到的行人行走方向。

S1.2、如图2所示，以基站坐标为球心，基站到标签的距离[D_1，0 D_2，0D_3，0...]为半径画球，列出方程求出各个球面的公共交点[X₀ Y₀ Z₀]作为初始时刻的行人坐标。

S1.3、将50cm作为行人的步长初始值L₀。

S1.4、根据步骤S1.1得到的[S₀ Θ₀]、步骤S1.2得到的[X₀ Y₀ Z₀]、步骤S1.3得到的L₀，将[X₀ Y₀ Z₀ S₀ Θ₀ L₀]作为系统状态初始值。引入可能存在的观测误差后，假设初始时刻系统状态满足多维正态分布：

其中σ₀ ²、σ₁ ²、σ₂ ²、σ₃ ²、σ₄ ²、σ₅ ²分别表示系统初始时刻对行人x坐标、y坐标、z坐标、行走步数、行走方向、步长的观测误差。

根据如上的概率分布，在matlab中，随机产生初始粒子集合：

其中，N为提前指定的粒子数量，

为第j个粒子在初始时刻的影响力权重，

为第j个粒子在初始时刻的系统状态，Z表示整数集合。

S2、状态递推：根据运动模型进行系统状态递推，对每个粒子执行递推方程更新粒子状态。递推方程为如下形式：

其中，[x_k y_k z_k s_kθ_k l_k]^T代表系统第k时刻的状态，[x_k-1 y_k-1 z_k-1 s_k-1 θ_k-1 l_k-1]^T代表系统第k-1时刻的状态，[noise_x，k-1 noise_y，k-1 noise_z，k-1 nosie_s，k-1 noise_θ，k-1noise_l，k-1]^T代表系统第k-1时刻的各类预测噪声。

S3、状态修正：根据UWB获取的距离观测值[D_1，k D_2，k D_3，k…]与手机传感器获取的运动数据观测值[S_k Θ_k]对系统状态进行修正，其中D_i.k表示第i个定位基站在k时刻观测到的距离值，S_k表示手机传感器在k时刻观测到的行人行走步数，Θ_k表示手机传感器在k时刻观测到的行人行走方向。具体做法是：

假设各个观测相互独立，则观测方程为：

其中，[x_k y_k z_k s_k θ_k l_k]为k时刻利用传感器观测到的系统状态，(Loc_i，x Loc_i，yLoc_i，z)为第i个基站在三维空间中的位置坐标。

而在实际情况下，k时刻利用传感器观测到的系统状态，与通过运动方程预测的系统状态往往存在误差，所以需要根据观测值，对每一个粒子j在k时刻更新影响力权重，误差越大，权重越小。更新粒子权值的公式为：

f_S～N(0，σ_s ²)

f_θ～N(0，σ_θ ²)

其中，[x_k y_k z_k s_k θ_k l_k]为k时刻利用传感器观测到的系统状态，

为第j个粒子在k时刻的预测系统状态，M为基站数量，σ_s为手机传感器测量行人步数时的误差，σ_θ为手机传感器测量行人行走方向时的误差，f_D，i、f_S、f_θ分别表示基站测量距离、手机传感器测量行人步数、手机传感器测量行人行走方向时的观测噪声，符合正态分布。

为k时刻第j个粒子的影响力权重。

S4、合并估计：将k时刻各个状态粒子的状态量进行合并，得出k时刻最终的行人位置估计值

具体的计算公式为：

其中，N为粒子总数，

分别为k时刻第j个粒子的x坐标、y坐标、z坐标状态量。

本发明并不限于上述实施方式，采用与本发明上述实施方式相同或近似的方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，均在本发明专利的保护范围之内。

Claims (6)

1.一种用于室内导航的UWB定位漂移校正方法，其特征在于，包括以下步骤：

S1、状态初始化：建立室内空间xyz三维坐标系，将UWB定位基站安装于空间中的固定位置，然后产生一组足够数量的、带有影响力权值的粒子来近似地描述初始系统状态。所述系统状态包括[x y z s θ l]，分别表示行人的x坐标、y坐标、z坐标、行走步数s、行走方向θ、每一步的步长l等。

S2、状态递推：根据运动模型进行系统状态递推，对每个粒子执行如下递推方程，更新粒子状态：

其中，[x_k y_k z_k s_k θ_k l_k]^T代表系统第k时刻的状态，[x_k-1 y_k-1 z_k-1 s_k-1 θ_k-1 l_k-1]^T代表系统第k-1时刻的状态，[noise_x,k-1 noise_y,k-1 noise_z,k-1 nosie_s,k-1 noise_θ,k-1noise_l,k-1]^T代表系统第k-1时刻的各类预测噪声。

S3、状态修正：根据UWB获取的距离观测值[D_1,k D_2,k D_3,k …]与手机传感器获取的运动数据观测值[S_k Θ_k]，对系统状态进行修正。其中，D_i,k表示第i个定位基站在k时刻观测到的距离值，S_k表示手机传感器在k时刻观测到的行人行走步数，Θ_k表示手机传感器在k时刻观测到的行人行走方向。

S4、合并估计：将k时刻各个状态粒子的状态量进行合并，得出k时刻最终的行人位置估计值

2.根据权利要求1所述用于室内导航的UWB定位漂移校正方法，其特征在于，步骤S1中，产生一组足够数量的、具有权值的粒子来近似地描述初始系统状态，包括以下步骤：

S1.1、行人开始行走，获取UWB的距离观测值[D_1,0 D_2,0 D_3,0 …]与手机传感器的运动数据观测值[S₀ Θ₀]。其中，D_i,0表示第i个定位基站在初始时刻观测到的标签距离值，S₀表示手机传感器在初始时刻观测到的行人行走步数，Θ₀表示手机传感器在初始时刻观测到的行人行走方向。

S1.2、以基站坐标为球心，基站到标签的距离[D_1,0 D_2,0 D_3,0 …]为半径画球，列出方程，求出各个球面的公共交点[X₀ Y₀ Z₀]作为初始时刻的行人坐标。

S1.3、设置行人的步长初始值L₀。

S1.4、根据所述S1.1得到[S₀ Θ₀]、所述S1.2得到[X₀ Y₀ Z₀]、所述S1.3得到L0后，将[X₀Y₀ Z₀ S₀ Θ₀ L₀]作为系统状态初始值，引入可能存在的观测误差后，初始时刻系统状态满足多维正态分布：

其中，σ₀ ²、σ₁ ²、σ₂ ²、σ₃ ²、σ₄ ²、σ₅ ²分别表示系统初始时刻对行人x坐标、y坐标、z坐标、行走步数、行走方向、步长的观测误差。

根据所述多维正态分布，随机产生初始粒子集合：

其中，j＝1～N，N为粒子数量，

为第j个粒子在初始时刻的影响力权重，

为第j个粒子在初始时刻的系统状态。

3.根据权利要求2所述用于室内导航的UWB定位漂移校正方法，其特征在于，步骤S1.3中，设置行人的步长初始值L₀＝50cm。

4.根据权利要求2所述用于室内导航的UWB定位漂移校正方法，其特征在于，步骤S1.4中，在matlab中随机产生初始粒子集合。

5.根据权利要求1所述用于室内导航的UWB定位漂移校正方法，其特征在于，步骤S3中，根据UWB获取的距离观测值[D_1,k D_2,k D_3,k …]与手机传感器获取的运动数据观测值[S_kΘ_k]，对系统状态进行修正，具体是：

各个观测相互独立，观测方程如下：

其中，(Loc_i,x Loc_i,y Loc_i,z)为第i个基站在三维空间中的位置坐标，[x_k y_k z_k s_k θ_kl_k]为k时刻利用传感器观测到的系统状态。

根据观测值，对每一个粒子j在k时刻更新影响力权重：

f_S～N(0,σ_s ²)

f_θ～N(0,σ_θ ²)

其中，

为k时刻第j个粒子的影响力权重，

为第j个粒子在k时刻的预测系统状态，M为基站数量，_s为手机传感器测量行人步数时的误差，_θ为手机传感器测量行人行走方向时的误差。f_D,i、f_s、f_θ分别表示基站测量距离、手机传感器测量行人步数、手机传感器测量行人行走方向时的观测噪声。

6.根据权利要求1所述用于室内导航的UWB定位漂移校正方法，其特征在于，步骤S4中，将k时刻各个状态粒子的状态量进行合并，得出k时刻最终的行人位置估计值

具体的计算公式为：

其中，N为粒子总数，

分别为k时刻第j个粒子的x坐标、y坐标、z坐标状态量。

Images