CN109186609A - 基于KF算法、Chan算法及Taylor算法的UWB定位方法 - Google Patents

Abstract

本发明公开了基于KF算法、Chan算法及Taylor算法的UWB定位方法，包括步骤：步骤1，将待测标签置于待测载体上，并处在4个定位基站的环境下，针对待测标签到各基站的距离进行拟合处理；步骤2，将所述步骤1实时拟合后的距离值作为卡尔曼滤波的量测值进行卡尔曼滤波，得到滤波后的距离值；步骤3，对所述步骤2经卡尔曼滤波后的距离值使用Chan算法得到待测标签估计位置；步骤4，将步骤3中Chan算法解算的待测标签估计位置作为Taylor算法的初值，进行Taylor算法解算得到最终的待测标签估计位置。本发明针对待测标签到各基站的距离建立线性拟合方程，将拟合后的结果进行卡尔曼滤波处理，减少非视距误差，使得测距结果稳定且更接近真值，提高定位精度。

Description

基于KF算法、Chan算法及Taylor算法的UWB定位方法

技术领域

本发明涉及基于KF算法、Chan算法及Taylor算法的UWB定位方法，属于定位导航技术领域。

背景技术

室内精准定位导航作为近些年来民用导航技术发展的主要领域之一已经得到极大的重视和发展，室内精准定位导航服务对于人们的生活越来越重要，特别是环境复杂的室内，如展厅、物品仓库、地下车库、地铁、健身场馆、大型商场、图书馆等场所，目前室内精准定位导航技术处于快速发展时期，各种室内精准定位导航技术仍还处于研究阶段，尚无法满足室内精准导航定位的应用需求。

超宽带(UWB)定位技术由于功耗低、抗多径效果好、安全性高、系统复杂度低，尤其是能提供非常精确的定位精度等优点，而成为未来无线定位技术的热点和首选。目前国际国内对于UWB定位技术的研究正方兴未艾，大多数是只针对视距环境下的定位技术研究，但在视距与非视距交替环境下测距精度易受影响，且利用传统的定位方法定位精度难以达到厘米级，故高精度的UWB定位方法亟待研究。

发明内容

为解决现有技术的不足，本发明的目的在于提供基于KF算法、Chan算法及Taylor算法的UWB定位方法，有效减小了测量噪声，实现基于UWB的高精度定位目标。

基于KF算法、Chan算法及Taylor算法的UWB定位方法，包括步骤：

步骤1，将待测标签置于待测载体上，并处在4个定位基站的环境下，针对待测标签到各基站的距离进行拟合处理；待测标签到各基站的距离为待测标签静态放置时采集的待测标签距离各个定位基站的距离的平均值；

步骤2，将所述步骤1实时拟合后的距离值作为卡尔曼滤波的量测值，将初始时刻的待测标签到各基站的距离作为滤波初值，根据采集的待测标签到每个基站的距离的方差选定噪声阵，进行卡尔曼滤波，得到滤波后的距离值；

步骤3，对所述步骤2经卡尔曼滤波后的距离值使用Chan算法得到待测标签估计位置；

步骤4，将步骤3中Chan算法解算的待测标签估计位置作为Taylor算法的初值，进行Taylor算法解算得到最终的待测标签估计位置。

所述权利要求1中最终的待测标签估计位置的解算如下：

在二维坐标系中，设待测标签位置为[x,y]，四个定位基站BS_i的位置分别为[x_i,y_i]，i＝1,2,3,4；其中以BS₁作为主基站，其余为子基站；各基站到待测标签的距离为r_i，待测标签到BS₁和BS_i的距离差为r_i1＝r_i-r₁；如下式所示：

将上式两边平方为：

r_i ²＝r₁ ²+2r₁r_i1+r_i1 ²＝k_i-(2xx_i+2yy_i)+x²+y² (2)

其中，k_i＝x²+y²；

令i＝1，则式(2)如下：

r₁ ²＝k₁-(2xx₁+2yy₁)+x²+y² (3)

将式(2)减去式(3)，可得：

2r₁r_i1+r_i1 ²＝k_i-k₁-2xx_i1-2yy_i1 (4)

其中，x_i1＝x_i-x₁,y_i1＝y_i-y₁

利用式(4)建立方程组得到：

将式(5)对应记为GZ＝h，利用加权最小二乘法理论得到：

Z＝(G^Tψ^-1G)hψ^-1h (6)

上式中，ψ＝BQB，其中B＝diag(r₂₁,r₃₁,r₄₁)，Q为主基站到各子基站的测量误差协方差阵，计算得到待测标签估计位置

将Chan算法解算的待测标签估计位置作为改进Taylor算法的初值；令位置误差量[δx,δy]，与真实位置关系为：

将式(7)代入式(2)，整理得到式(8)：

其中δx²+δy²≈0，则式(8)简化为：

其中，表示各基站距离待测标签估计位置的距离；

由式(9)构建出误差方程组G₁Z₁＝h₁，整理成如下形式：

利用加权最小二乘法理论得：

其中，ψ₁为测量误差协方差阵，ψ₁＝diag(Δr_i-Δr_a)，

其中，Δr_i为待测标签到基站i的实测距离与估计距离之差，Δr_a为待测标签到各基站的实测距离与估计距离之差的平均值；

迭代更新过程如下：

其中，为第m此迭代解算之后的结果，连续迭代直到位置误差量满足Δd≤TH，其中TH为门限值，则停止迭代，此时即为最终的待测标签估计位置。

有益效果：

本发明首先进行数据预处理，针对待测标签到各基站的距离建立线性拟合方程，将拟合后的结果进行卡尔曼滤波处理，减少非视距误差，使得测距结果稳定且更接近真值，提高定位精度；在以上技术的基础上，提出一种改进的Chan/Taylor算法进行定位解算，实现了UWB的厘米级的定位精度。本发明在具有UWB定位条件的环境下，适用于多种导航载体，为载体提供高精度的定位结果，具有极高的工程应用和商业价值。

附图说明

图1是本发明UWB定位算法流程图。

图2是本发明与传统方法的误差对比图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

如图1所示，本发明基于KF算法、Chan算法及Taylor算法的UWB定位方法包括如下步骤：

步骤1，将待测标签置于待测载体上，并处在4个定位基站的环境下，针对待测标签到各基站的距离建立拟合方程，并进行拟合处理；其中拟合方法为线性拟合方法，待测标签到各基站的距离为待测标签静态放置时采集的待测标签距离各个定位基站的距离的平均值；

步骤2，采用卡尔曼滤波模型将所述步骤1拟合后的结果进行滤波处理：

KF(卡尔曼滤波)具有高效率，可实时估计且具有递推性的优点，该方法由前一时刻的估计值结合当前时刻的量测值递推得到当前时刻的估计值。将所述步骤1实时拟合后的距离值作为卡尔曼滤波的量测值，设定合理的滤波初值及噪声阵，得到实时估计的状态量作为滤波后的距离值，可减少非视距误差，提高测距精度；在本发明中，合理的滤波初值为初始时刻的距离值；噪声阵根据标签测距的方差选定；所述标签测距是指采集的待测标签到每个基站的距离；

步骤3，对所述步骤2经滤波处理后的距离值使用Chan算法得到初步估计解。在Chan算法的基础上加入Taylor迭代算法思想，将Chan算法解算的位置量作为改进Taylor迭代算法的初值，利用距离关系式，建立误差方程组，再使用加权最小二乘算法得到误差估计值，进而得到优化结果。利用位置误差量判定是否停止迭代，最终得到高精度的定位结果。

在二维坐标系中，设待测标签位置为[x,y]，四个定位基站BS_i的位置分别为[x_i,y_i](i＝1,2,3,4)，其中以BS₁作为主基站，其余为子基站。各基站到待测标签的距离为r_i(i＝1,2,3,4)，待测标签到BS₁和BS_i的距离差为r_i1＝r_i-r₁(i＝1，2,3,4)，

如下式所示：

将上式两边平方为：

r_i ²＝r₁ ²+2r₁r_i1+r_i1 ²＝k_i-(2xx_i+2yy_i)+x²+y² (2)

其中，k_i＝x²+y²。

对式(2)令i＝1则：

r₁ ²＝k₁-(2xx₁+2yy₁)+x²+y² (3)

将式(2)减去式(3)，可得：

2r₁r_i1+r_i1 ²＝k_i-k₁-2xx_i1-2yy_i1 (4)

其中，x_i1＝x_i-x₁,y_i1＝y_i-y₁

利用式(4)建立方程组，由于本发明可直接测得待测标签到基站的距离，

故未知量仅待测标签的位置[x,y]，进一步整理成如下形式：

将式(5)对应记为GZ＝h，利用加权最小二乘法理论可得：

Z＝(G^Tψ^-1G)hψ^-1h (6)

上式中，ψ＝BQB，其中B＝diag(r₂₁,r₃₁,r₄₁)，Q为主基站到各子基站的测量误差协方差阵，计算得到待测标签估计位置

步骤4，将步骤3中Chan算法解算的待测标签估计位置作为改进Taylor算法的初值，并构建误差方程组，进行多次迭代计算；令位置误差量[δx,δy]，与真实位置关系为：

将式(7)代入距离关系式(2)，整理为式(8)：

其中δx²+δy²≈0，则式(8)简化为：

其中，表示各基站距离待测标签估计位置的距离；

由式(9)构建出误差方程组G₁Z₁＝h₁，进一步整理成如下形式：

鉴于传统Taylor算法展开式忽略二阶以上的分量，产生一定的误差，可知本文所提出的误差方程组与传统Taylor算法的误差方程组相比误差更小。

利用加权最小二乘法理论可得：

Z₁＝(G₁ ^Tψ₁ ^-1G₁)h₁ψ₁ ^-1h₁ (11)

其中，ψ₁为测量误差协方差阵，为提高解算精度，本文令ψ₁＝diag(Δr_i-Δr_a)，式中

其中，Δr_i为待测标签到基站i的实测距离与估计距离之差，Δr_a为待测标签到各基站的实测距离与估计距离之差的平均值；

迭代更新过程如下：

其中，为第m此迭代解算之后的结果，连续迭代直到位置误差量满足Δd≤TH，其中TH为门限值，则停止迭代，此时的即为最终的待测标签估计位置。

如图2所示，采用本发明提出的改进的定位方法所得的定位误差如点虚线所示，采用传统定位方法的定位误差如三角实线所示，从误差曲线的角度可知改进后的定位方法优于传统方法，且平均定位精度约3.5厘米。

以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护。

Claims (2)

1.基于KF算法、Chan算法及Taylor算法的UWB定位方法，其特征在于：包括步骤：

步骤1，将待测标签置于待测载体上，并处在4个定位基站的环境下，针对待测标签到各基站的距离进行拟合处理；待测标签到各基站的距离为待测标签静态放置时采集的待测标签距离各个定位基站的距离的平均值；

步骤2，将所述步骤1实时拟合后的距离值作为卡尔曼滤波的量测值，将初始时刻的待测标签到各基站的距离作为滤波初值，根据采集的待测标签到每个基站的距离的方差选定噪声阵，进行卡尔曼滤波，得到滤波后的距离值；

步骤3，对所述步骤2经卡尔曼滤波后的距离值使用Chan算法得到待测标签估计位置；

步骤4，将步骤3中Chan算法解算的待测标签估计位置作为Taylor算法的初值，进行Taylor算法解算得到最终的待测标签估计位置。

2.根据权利要求1所述的基于KF算法、Chan算法及Taylor算法的UWB定位方法，其特征在于：所述权利要求1中最终的待测标签估计位置的解算如下：

在二维坐标系中，设待测标签位置为[x,y]，四个定位基站BS_i的位置分别为[x_i,y_i]，i＝1,2,3,4；其中以BS₁作为主基站，其余为子基站；各基站到待测标签的距离为r_i，待测标签到BS₁和BS_i的距离差为r_i1＝r_i-r₁；如下式所示：

将上式两边平方为：

r_i ²＝r₁ ²+2r₁r_i1+r_i1 ²＝k_i-(2xx_i+2yy_i)+x²+y² (2)

其中，k_i＝x²+y²；

令i＝1，则式(2)如下：

r₁ ²＝k₁-(2xx₁+2yy₁)+x²+y² (3)

将式(2)减去式(3)，可得：

2r₁r_i1+r_i1 ²＝k_i-k₁-2xx_i1-2yy_i1 (4)

其中，x_i1＝x_i-x₁,y_i1＝y_i-y₁

利用式(4)建立方程组得到：

将式(5)对应记为GZ＝h，利用加权最小二乘法理论得到：

Z＝(G^Tψ^-1G)hψ^-1h (6)

上式中，ψ＝BQB，其中B＝diag(r₂₁,r₃₁,r₄₁)，Q为主基站到各子基站的测量误差协方差阵，计算得到待测标签估计位置

将Chan算法解算的待测标签估计位置作为改进Taylor算法的初值；令位置误差量[δx,δy]，与真实位置关系为：

将式(7)代入式(2)，整理得到式(8)：

其中δx²+δy²≈0，则式(8)简化为：

其中，表示各基站距离待测标签估计位置的距离；

由式(9)构建出误差方程组G₁Z₁＝h₁，整理成如下形式：

利用加权最小二乘法理论得：

Z₁＝(G₁ ^Tψ₁ ^-1G₁)h₁ψ₁ ^-1h₁ (11)

其中，ψ₁为测量误差协方差阵，ψ₁＝diag(Δr_i-Δr_a)，

其中，Δr_i为待测标签到基站i的实测距离与估计距离之差，Δr_a为待测标签到各基站的实测距离与估计距离之差的平均值；

迭代更新过程如下：

其中，为第m此迭代解算之后的结果，连续迭代直到位置误差量满足Δd≤TH，其中TH为门限值，则停止迭代，此时即为最终的待测标签估计位置。