CN111948602A - 基于改进Taylor级数的二维UWB室内定位方法 - Google Patents

Abstract

本发明公开了一种基于改进Taylor级数的二维UWB室内定位方法，属于位置跟踪技术领域，包括如下步骤：采用TOA测距算法获取标签与多个基站之间的相对距离；并对相对距离进行卡尔曼滤波，得到滤波距离；根据标签与各个基站之间的滤波距离列写超定方程组，利用最小二乘法进行求解，得到标签的定位初值，即Taylor级数算法的初值；列写定位函数，并进行Taylor级数展开，将标签的定位初值代入Taylor级数展开式中列写超定方程组，利用最小二乘法得到定位误差，通过定位初值和定位误差得到标签最终的定位坐标。本发明提出的定位方法定位精度高，有良好的抗干扰效果，且有效提高了定位效率。

Description

基于改进Taylor级数的二维UWB室内定位方法

技术领域

本发明属于位置跟踪技术领域，尤其涉及一种基于改进Taylor级数的二维UWB室内定位方法。

背景技术

超宽带(UWB)定位技术是指在定位空间内布置好n(n≥3)个坐标已知的锚点，锚点与待定位的标签之间进行相互通信，获取两者之间的距离参数，距离获取方案一般有TOA(到达时间)、TDOA(到达时间差)、AOA(到达角度)、RSS(接收信号强度)等，本发明所述的定位算法根据距离参数获取标签的坐标。

Taylor级数是一种基于TOA测距模型的定位方法，Taylor级数相比其他定位算法，如三角质心、最小二乘法，在非视距环境下定位精度更高。但是Taylor级数解算坐标时需要先验信息，在初值精度不高的情况下，Taylor级数易陷入局部最优导致算法收敛性和定位精度降低。

发明内容

发明目的：针对现有技术中Taylor级数解算坐标时初值精度的问题，本发明公开了一种基于改进Taylor级数的二维UWB室内定位方法，利用最小二乘法计算出先验信息并代入到Taylor级数算法中，定位精度高，有良好的抗干扰效果，且有效提高了定位效率。

技术方案：本发明采用如下技术方案：一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，包括如下步骤：

S1、在多基站的定位环境内，待定位的标签与基站实时通信，采用TOA测距算法获取标签与各个基站之间的相对距离；

S2、将初始时刻的标签到各个基站的相对距离作为滤波初值，将所有相对距离的方差作为选定噪声阵，对滤波初值进行卡尔曼滤波，得到滤波距离；

S3、根据至少一个时刻下标签与各个基站之间的滤波距离列写超定方程组，利用最小二乘法进行求解，得到标签的定位初值，即Ta训or级数算法的初值；其中，若根据一个时刻下的滤波距离建立一个超定方程组，则该超定方程组的解作为标签的定位初值，若根据一个以上时刻下的滤波距离建立一个以上超定方程组，则解算出一个以上的解，计算每个解的权值，并根据权值对解进行筛选，根据筛选后剩下的解及其权值得到标签的定位初值；

S4、根据标签与各个基站之间的滤波距离列写定位函数，并对定位函数进行Taylor级数展开，将标签的定位初值代入Taylor级数展开式中，在定位函数的误差最小时建立超定方程组，利用最小二乘法得到定位误差；

S5、若定位误差符合要求，则通过定位初值和定位误差得到标签；若定位误差不符合要求，则通过原有的定位初值和定位误差得到标签新的定位初值，重复步骤S4和S5，直到定位误差符合要求。

优选地，步骤S3中，超定方程组为：

其中，(x_i，y_i，z_i)(i＝1，2…n)，n≥3表示各个基站的三维坐标，(x，y，z_E)表示标签的三维坐标，d′_i(i＝1，2…n)表示标签与各个基站之间的滤波距离；

将该超定方程组改写为矩阵形式为AX＝B，其中

优选地，当建立N(N≥2)个超定方程组时，解算超定方程组得到N(N≥2)个解

每个解对应的权值为：

优选地，当存在N(N≥2)个解

时，取阈值p，若p_i≤p，保留权值p_i对应的解，若p_i＞p，删去权值p_i对应的解，得到筛选后的解为

其对应的权值分别为p′_i(i＝1，2，…，t)，标签的定位初值为

优选地，阈值p为：

优选地，步骤S4中，定位函数为：

定位函数通过Taylor级数展开为：

其中：

其中，(x_i，y_i，z_i)(i＝1，2…n)，n≥3表示各个基站的三维坐标；(x，y，z_E)表示标签的三维坐标，且

(x₀，y₀)表示标签的定位初值，δ_x和δ_y表示定位误差；d_i(i＝1，2…n)表示标签与各个基站之间的滤波距离；ε_i表示定位函数的误差。

优选地，步骤S4中，超定方程组为：

h＝ξδ

定位误差δ的最优估计为：

其中，

优选地，步骤S5中，定位误差应符合的要求为：

μ为预设的误差区间的平均值。

优选地，步骤S5中，标签最终的二维定位坐标为

其中，(x₀，y₀)为步骤S4中代入的标签的定位初值，

和

为符合要求的定位误差。

优选地，步骤S2中，卡尔曼滤波的状态转移参数和观测参数均为单位矩阵，预测协方差矩阵的初始值为0.127，初始时刻的最优状态估计值等于输入的第一个量测值。

有益效果：本发明具有如下有益效果：

1、本发明利用最小二乘法计算出先验信息并代入到Taylor级数算法中，定位精度高，有良好的抗干扰效果，且有效提高了定位效率；

2、本发明利用最小二乘法解算不同数量的超定方程组，对超定方程组的解进行了筛选，同时还提出了两种定位初值的计算方法，进一步提高了定位精度。

附图说明

图1是本发明基于改进Taylor级数的二维UWB室内定位方法的流程简图；

图2是本发明基于改进Taylor级数的二维UWB室内定位方法的流程示意图；

图3是本发明基于改进Taylor级数的二维UWB室内定位方法的结构示意图；

图4是根据最小二乘算法得出的标签的散点和坐标变化图，其中图4中的(a)为散点图，(b)为坐标变化图；

图5是根据本发明中改进定位算法得出的标签的散点和坐标变化图，其中图5中的(a)为散点图，(b)为坐标变化图；

图6是以最小二乘算法的解的算术平均根为初值得出的标签的散点图和坐标变化图，其中图6中的(a)为散点图，(b)为坐标变化图；

图7是起(1，2.4)移至终点(11，2.4)的定位结果图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

本发明公开了一种基于改进Taylor级数的二维UWB室内定位方法，旨在解决由于初值精度不高，会使Taylor级数算法收敛性和精度降低的问题，如图1和图2所示，包括如下步骤：

S1、如图3所示，在多基站的定位空间内，待定位的标签与基站实时通信，采用TOA测距算法获取标签与各个基站之间的相对距离，设标签(x，y，z)到各个基站的相对距离分别为d_i(i＝1，2…n)，则：

其中，c表示光速；

表示定位空间内第i个基站收到标签发送的信号的时间戳；t_t表示标签发送信号的时间戳；n表示基站的数量。

S2、将初始时刻的标签到各个基站的相对距离d_i(i＝1，2…n)作为滤波初值，将相对距离的方差作为选定噪声阵，对滤波初值进行卡尔曼滤波，将滤波后得到的距离d′_i(i＝1，2…n)作为本发明中定位方法的输入。

卡尔曼滤波的具体算法如下：

(1)标签到各个基站的距离是每隔毫秒级时间上传至服务器的，因此可以认为每个时刻标签与各个基站之间的距离基本不变，因此在k时刻的状态可以用以下公式预测：

其中，

表示根据k-1时刻状态预测得到的k时刻状态；

表示k-1时刻下的最优状态估计；A表示状态转移参数；w_k表示状态噪声。

(2)结合当前时刻的距离量测值实现最优状态估计，在k时刻的量测值可以由下式所示的量测方程来表示：

z_k＝Hx_k+V_k

其中，z_k表示k时刻的距离量测值；H表示观测参数；V_k表示量测噪声；x_k表示k时刻的状态。

在现场定位环境中，状态噪声w_k和量测噪声V_k都不是均值为0、方差为1的高斯白噪声，其均值、方差需要根据实验所得到的参数来确定。对于状态噪声w_k来说，其均值和方差由仿真时的滤波效果来确定，初始值分别暂定为0和0.0001；对于量测噪声V_k来说，均值和方差在本发明中设定为0.209和0.083。

(3)引入预测协方差矩阵

对

的估计如下：

其中，

表示由k-1时刻预测得到的k时刻协方差矩阵；

表示k-1时刻的更新协方差矩阵；Q_k表示状态协方差矩阵(即状态噪声w_k的方差)。

(4)为权衡预测协方差

和量测协方差矩阵R_k(即量测噪声V_k的方差)的大小，引入滤波增益矩阵K_k，对K_k的计算可由下式来表示。

(5)完成滤波增益矩阵K_k的计算后，即开始对k时刻的状态做最优估计，该值

由如下公式得到：

(6)完成k时刻的最优状态估计后需要计算该时刻的更新协方差矩阵

由下式

其中，I为单位矩阵。

(7)利用上述过程对定位系统的测距值进行滤波时，需要设定相关参数的初值。其中，参数A和H都为单位矩阵；初始时刻的最优估计值

为输入的第一个量测值z₁；预测协方差矩阵的初始值

在本发明中设定为0.127。完成卡尔曼滤波的一轮滤波后，将

和

作为下一轮更新的输入值，即可以对k+1时刻的状态进行最优状态估计。

S3、根据标签与各个基站之间的经过滤波后的距离列写超定方程组，利用最小二乘法进行求解，得到标签的定位初值，即Taylor级数算法的初值。

假设定位空间内存在n(n≥3)个基站，其中任意3个基站不位于同一条直线上，各个基站的三维坐标分别为(x_i，y_i，z_i)(i＝1，2…n)，各个基站到标签(x，y，z)的距离分别为d_i(i＝1，2…n)，滤波后得到的距离分别为d′_i(i＝1，2…n)，由于标签高度的不同对标签与基站之间映射到xoy平面的距离的影响极小，所以本发明中将标签的高度设为z_E，其中标签的高度z_E为已知量，因此可以构建以下超定方程组：

用矩阵形式AX＝B对该超定方程组进行表示，其中：

由于在定位空间内建立基站时，其中任意3个基站不位于同一条直线上，因此A^TA为非奇异矩阵，该超定方程组有唯一解

若仅采用一个时刻下标签与各个基站之间的经过滤波后的距离列写超定方程组，则只能建立一个超定方程组，该超定方程组的解即为Taylor级数算法的初值(x₀，y₀)；

若采用N(N≥2)个时刻下标签与各个基站之间的经过滤波后的距离列写超定方程组，则可以建立N(N≥2)个超定方程组，解算出N(N≥2)个解

为这N(N≥2)个解

分配权值p_i：

从上式中可以看出，当权值p_i越小时，第i个解对应的坐标与其余解对应的坐标之间的误差越小，则该解也更接近真实值，因此设置一个阈值p，当p_i≤p时，说明该权值对应的解距离真实值较近，保留该解；当p_i＞p时，说明该权值对应的解距离真实值较远，删除该解。一般的，取

假设在N个解中完成筛选后还剩下t(t≥1)个解，即

其对应的权值分别为p′_i(i＝1，2，…，t)。由于权值p′_i与其对应解的误差呈正比，即p′_i越小时其对应解的可信度就越高，所以将p′_i的倒数设置为其对应解的权重，通过最小二乘法的解及其权重确定Taylor级数算法的初值(x₀，y₀)：

S4、根据标签与各个基站之间的经过滤波后的距离列写定位函数，并对定位函数进行Taylor级数展开，将标签的定位初值代入Taylor级数展开式中列写超定方程组，利用最小二乘法得到定位误差，对标签实现二维定位。

构建误差方程

其中δ_x和δ_y为定位误差。

根据距离公式建立定位函数

k_i为定位标签与各个基站之间映射到xoy平面上的距离，将该函数在(x₀，y₀)处进行Taylor级数展开得到：

其中，

其中，k_i|0(i＝1，2…n)表示标签的定位初值与各个基站之间在二维平面上的距离。

展开式可以改写为

对所有基站建立误差向量ε＝h-ξδ，其中

当误差向量ε最小即取0时，建立超定方程组h＝ξδ，当ξ^Tξ为非奇异矩阵时，根据最小二乘原理，该超定方程组有唯一解，即最优估计为

否则当ξ^Tξ为奇异矩阵时，该超定方程组有多解，但此时可认为无解，即无法得到定位误差，对标签实现二维定位。

但是，在定位空间内建立基站时，其中任意3个基站不位于同一条直线上，因此可以保证ξ^Tξ为非奇异矩阵，即该超定方程组有唯一解。

根据预设的门限值μ，μ取值越小则定位精度越高，但同时会增加算法的迭代次数，导致算法收敛性降低，为了让系统定位误差达到0.1～0.3m，所以μ取误差区间的平均值，设为0.2m，判断

是否成立。若成立则推出标签的最终的二维坐标为

若不成立，则以

为新的标签的定位初值，继续重复上述步骤S4直至不等式成立，得到标签的最终的二维坐标。

进行仿真分析，将3个基站布置在定位空间为10.5m×13m×4.5m的房间内，基站坐标分别为A1(0.35，0.15，3.9)、A2(10.1，0.25，3.5)、A3(10.05，6.8，3.85)，将待定位的标签固定或做直线运动，持续观测不同算法下的基站与标签的相对距离以及定位坐标的误差。

将待定位的标签放置在测试点位T1(5.7，3.4)处，观测标签到各个基站之间的距离，并分别利用最小二乘法和本发明所述方法解算标签的二维坐标，结果如图4和图5所示。

图4为最小二乘法直接解算得到的标签的坐标散点图以及坐标变化曲线图，图5为本发明所述方法解算得到的标签的坐标散点图以及坐标变化曲线图。通过比较发现，本发明的解算得到的坐标误差明显小于最小二乘法，说明了Taylor级数算法的有效性。

将最小二乘法解算得到的二维坐标直接求算术平均根，将该算术平方根作为Taylor级数算法的初值对标签进行二次定位，迭代次数设置为10，结果如图6所示，图6中分别为解算得到的标签的坐标散点图以及坐标变化曲线图。通过比较图5与图6发现，本发明中将最小二乘法解算得到的二维坐标直接作为Taylor级数算法的初值，最终解算得到的标签的坐标误差明显较小。

将标签从起点(1，2.4)沿着y＝2.4移至终点(11，2.4)，利用本发明方法所述方法解算该过程中标签坐标，结果如图7所示。从图7中可以看出，解算得到的标签坐标与标签的示意坐标之间的误差小于0.15m，满足0.1～0.3m的系统定位误差。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，包括如下步骤：

S1、在多基站的定位环境内，待定位的标签与基站实时通信，采用TOA测距算法获取标签与各个基站之间的相对距离；

S2、将初始时刻的标签到各个基站的相对距离作为滤波初值，将所有相对距离的方差作为选定噪声阵，对滤波初值进行卡尔曼滤波，得到滤波距离；

S3、根据至少一个时刻下标签与各个基站之间的滤波距离列写超定方程组，利用最小二乘法进行求解，得到标签的定位初值，即Taylor级数算法的初值；其中，若根据一个时刻下的滤波距离建立一个超定方程组，则该超定方程组的解作为标签的定位初值，若根据一个以上时刻下的滤波距离建立一个以上超定方程组，则解算出一个以上的解，计算每个解的权值，并根据权值对解进行筛选，根据筛选后剩下的解及其权值得到标签的定位初值；

S4、根据标签与各个基站之间的滤波距离列写定位函数，并对定位函数进行Taylor级数展开，将标签的定位初值代入Taylor级数展开式中，在定位函数的误差最小时建立超定方程组，利用最小二乘法得到定位误差；

S5、若定位误差符合要求，则通过定位初值和定位误差得到标签；若定位误差不符合要求，则通过原有的定位初值和定位误差得到标签新的定位初值，重复步骤S4和S5，直到定位误差符合要求。

2.根据权利要求1所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，步骤S3中，超定方程组为：

其中，(x_i，y_i，z_i)(i＝1，2…n)，n≥3表示各个基站的三维坐标，(x，y，z_E)表示标签的三维坐标，d′_i(i＝1，2…n)表示标签与各个基站之间的滤波距离；

将该超定方程组改写为矩阵形式为AX＝B，其中

3.根据权利要求2所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，当建立N(N≥2)个超定方程组时，解算超定方程组得到N(N≥2)个解

每个解对应的权值为：

4.根据权利要求3所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，当存在N(N≥2)个解

时，取阈值p，若p_i≤p，保留权值p_i对应的解，若p_i＞p，删去权值p_i对应的解，得到筛选后的解为

其对应的权值分别为p′_i(i＝1，2，…，t)，标签的定位初值为

5.根据权利要求4所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，阈值p为：

6.根据权利要求1所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，步骤S4中，定位函数为：

定位函数通过Taylor级数展开为：

其中：

其中，(x_i，y_i，z_i)(i＝1，2…n)，n≥3表示各个基站的三维坐标；(x，y，z_E)表示标签的三维坐标，且

(x₀，y₀)表示标签的定位初值，δ_x和δ_y表示定位误差；d_i(i＝1，2…n)表示标签与各个基站之间的滤波距离；ε_i表示定位函数的误差。

7.根据权利要求6所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，步骤S4中，超定方程组为：

h＝ξδ

定位误差δ的最优估计为：

其中，

8.根据权利要求1所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，步骤S5中，定位误差应符合的要求为：

μ为预设的误差区间的平均值。

9.根据权利要求1所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，步骤S5中，标签最终的二维定位坐标为

其中，(x₀，y₀)为步骤S4中代入的标签的定位初值，

和

为符合要求的定位误差。

10.根据权利要求1所述的一种基于改进Taylor级数的二维UWB室内定位方法，其特征在于，步骤S2中，卡尔曼滤波的状态转移参数和观测参数均为单位矩阵，预测协方差矩阵的初始值为0.127，初始时刻的最优状态估计值等于输入的第一个量测值。

Images