CN112566021B - 一种基于估计偏差平均的四基站uwb平面定位算法 - Google Patents

Abstract

本发明涉及一种基于估计偏差平均的四基站UWB平面定位算法，其包括如下步骤：定位标签向四个基站轮询获取距离数据，检查距离数据的可靠性，计算出多组位置参数和位置参数关于距离数据的梯度的模值，根据梯度模值设置每组位置参数的权重，使最终估计结果表达式对每一个基站的距离数据的偏导相等，该方案抑制测量数据波动对最终定位效果的干扰，提升了定位的准确度。

Description

一种基于估计偏差平均的四基站UWB平面定位算法

技术领域

本发明涉及一种算法，具体涉及一种基于估计偏差平均的四基站UWB平面定位算法，属于UWB定位算法技术领域。

背景技术

移动物体的定位技术是无人车，无人机以及复杂系统集群控制中的关键技术。基于准确的位置信息，可以实现轨迹跟踪、路径规划、集群控制等应用。

目前主流的定位方案根据应用场合可分成两大类，一类适用于室外定位，其中最为成熟的方案是使用卫星定位测量方法的RTK载波相位差分技术，可以在室外得到厘米级的精度。此外，基于激光雷达的SLAM也是一种较为常见的定位技术。另一类适用于室内定位，常见的有机器视觉标签捕捉定位技术、惯导定位技术、超宽带(UWB)定位技术等。

超宽带定位技术的本质是ToF(Time of Flight,飞行时间)测距法，通过测量脉冲信号在基站与标签之间的传输时间，乘以脉冲信号在介质中的传播速度即可得到标签与多个基站之间的直线距离。通过一些几何计算就可以求解出标签与多个基站之间的相对位置。

目前使用双边测距的定位算法一般使用标签到基站的距离基于定位误差对标签位置做最小二乘估计。利用平面上任意3个不共线的基站对标签的距离，可以确定唯一标签相对基站的位置。此函数计为：

(x,y)＝f(d₁,d₂,d₃)

将满足使误差平方

取得最小值的位置

作为本次的估计位置，也就是多个计算出的坐标值的算术平均值。但是这种估计方法存在一个缺陷：由于每个距离值对于标签位置的计算的影响能力不同，当某个距离值的波动对位置的估计产生较大影响时，仍然采用平均值估计将引入较大的估计误差。因此，迫切的需要一种新的方案解决上述技术问题。

发明内容

本发明正是针对现有技术中存在的问题，提供一种基于估计偏差平均的四基站UWB平面定位算法，该方案通过调整四组位置参数估计值的权重，使最终的位置参数估计结果中，每一组的估计偏差相等,进而降低了定位效果在标签靠近某一基站时测量距离扰动对最终估计的影响，提高了四基站定位系统在边缘区域的准确度。

为了实现上述目的，本发明的技术方案如下，一种基于估计偏差平均的四基站UWB平面定位算法，所述方法包括以下步骤：

步骤1：标签向四个基站发送测距请求，获得标签到四个基站的距离值d_i(i＝A,B,C,D)；

步骤2：计算几何误差

如果几何误差E大于误差容限E_threshold，则放弃对本次数据做进一步处理，重新执行步骤1。

步骤3：根据公式：

计算出x_i,i＝A,B,C,D的值并检查所有相邻的距离数据是否符合约束，若发现存在不符合约束的数据，则放弃对本次数据做进一步处理，重新执行步骤1；

步骤4：根据标签到四个基站的距离值d_i(i＝A,B,C,D)，取所有相邻基站的距离数据，根据公式(4)计算出四组位置参数(k_{1_AB},k_{2_AB})、(k_{1_BC},k_{2_BC})、(k_{1_CD},k_{2_CD})、(k_{1_DA},k_{2_DA})；

步骤5：根据标签到四个基站的距离值d_i(i＝A,B,C,D)计算出权重矩阵；

其中

步骤6：计算以图中D'为原点，

为X轴正方向，

为Y轴正方向的平面直角坐标系xoy-1为参考系的位置系数矩阵K:

其中

步骤7：计算基于平面直角坐标系xoy-1的估计位置参数：

乘以定位场地的边长L即可得到标签T在平面直角坐标系xoy-1的位置坐标(x_T,y_T)，其中：

其中，步骤2中，引入几何误差E来衡量当前一组距离数据的可靠性，当可靠性达不到要求时，则将该组距离数据视为不可靠数据并放弃本组数据，重新测量。

其中，在步骤4中，使用公式(4)计算出四组位置参数(k_{1_AB},k_{2_AB})、(k_{1_BC},k_{2_BC})、(k_{1_CD},k_{2_CD})、(k_{1_DA},k_{2_DA})，具体如下：

使用相邻的两基站到标签的距离计算可得到四组位置参数：

其中，在步骤5、6中，使用位置参数对距离数据的梯度的模值倒数作为计算权重，使最后估计出的位置参数中每一个距离数据引入的估计偏差相等。

相对于现有技术，本发明具有如下优点，(1)提出了几何误差E的概念，用于衡量距离数据可靠性，当因为某些原因导致距离数据明显偏离正常范围时，可以自动忽略该组数据，防止过大的误差被引入位置估计的计算，提升最终定位的准确性。(2)提出了基于估计偏差平均的位置估计算法，通过调整多组数据的权重，使参与最终位置估计的每一组位置参数都拥有相同的估计偏差，解决了标签在定位区边缘时由于过于靠近某一基站导致测距数据带上过大的估计偏差的缺陷，从而提升位置估计算法的准确度,有效避免传统最小二乘定位产生的误差放大问题。

附图说明

图1为四基站平面定位示意图；

图2为算法2部分参数示意图；

图3为底面为矩形的四棱柱图。

具体实施方式：

为了加深对本发明的理解，下面结合附图对本实施例做详细的说明。

实施例1：参见图1，本发明克服了现有定位技术中存在的上述不足，提出了一种基于估计偏差平均的平面定位算法，实验平台示意图如图1所示A、B、C、D分别为四个基站所在位置，每个基站距离标签T所在平面高度为H四个基站在标签所在平面的投影点正好是一个边长为L的正方形的四个顶点。

首先，需要了解UWB定位模组测距信息的特点：

(1)测距范围在0.3-300米之间，小于0.3m时的数据波动较大。大于300米时由于信号强度的衰减容易失联。

(2)在LOS(Line of sight,直线可视)情况下，距离数据存在±0.1米的随机噪声，噪声幅值与距离几乎无关。

基站往往需要定位多个标签，场地中也可能存在部分障碍，为确保标签与所有基站处于LOS状态下，不被其他标签或障碍物阻挡，因此需要将四个基站的位置抬高至距离标签平面上方垂直高度为H处。需要注意的是，在保证所有标签不被遮挡的情况下，应确保H尽可能小。

由于几何特性，标签到四个基站的距离满足以下公式(具体证明见附录1)：

AT²+CT²＝BT²+DT²  (1)

定义几何误差E表达式：

本实验提出一种算法，记为算法1：当几何误差E大于几何误差容限E_threshold时，可以认定当次获得的距离测量值误差过大，考虑直接放弃当次测量值。

本发明还提出一种算法，记为算法2：可以根据任意两点A,B到第三点T的距离得到第三点与AB的相对位置参数k₁,k₂,各项参数示意图如图2所示：

位置参数k₁,k₂的意义为：

位置参数k₁,k₂可以通过任意两点A,B到第三点T的距离得到。公式如下：

应用此公式需满足L,x_A,x_B能构成三角形的三边且∠TAB,∠TBA均小于等于90度。写成公式约束即为：

根据四基站定位的几何结构，由勾股定理可知：

d_i为标签到各基站的距离长度，x_i为标签到个基站的距离在标签所在平面的投影长度。使用相邻的两基站到标签的距离计算可得到四组位置参数：

对任何一组位置参数分别求取梯度的模(以AB组为例)：

显然不是一个常数，由于测量距离存在噪声扰动，通过上述公式中的

可以得到测量距离摄动对位置参数k₁,k₂的影响。当某一组未知参数的梯度的模特别大时(如标签非常靠近某一个基站)，若将其解算得到的位置参数与其他组平均，将导致最终的位置估计偏差包含较大部分的该组位置参数的估计偏差，从而导致最终估计的偏差变得较大。

为解决上述问题，本发明提供了一种基于估计偏差平均的四基站UWB平面定位算法。通过调整四组位置参数估计值的权重，使最终的位置参数估计结果中，每一组的估计偏差相等。总而降低了定位效果在标签靠近某一基站时测量距离扰动对最终估计的影响。提高了四基站定位系统在边缘区域的准确度。

按照本发明提供的技术方案，提出一种基于估计偏差平均的四基站UWB平面定位算法。所述平面定位算法主要包括如下步骤：

步骤1：标签向四个基站发送测距请求，获得标签到四个基站的距离值d_i(i＝A,B,C,D)。

步骤2：计算几何误差

如果几何误差E大于误差容限E_threshold，则放弃对本次数据做进一步处理。重新执行步骤1。

步骤3：根据公式(6)计算出x_i(i＝A,B,C,D)的值并检查所有相邻的距离数据是否符合约束，若发现存在不符合约束的数据，则不再放弃对本次数据做进一步处理，重新执行步骤1。

步骤4：根据x_i(i＝A,B,C,D)使用公式(4)计算出四组位置参数(k_{1_AB},k_{2_AB})、(k_{1_BC},k_{2_BC})、(k_{1_CD},k_{2_CD})、(k_{1_DA},k_{2_DA})

步骤5：根据标签到四个基站的距离值d_i(i＝A,B,C,D)计算出权重矩阵:

其中

步骤6：计算以图中D'为原点，

为X轴正方向，

为Y轴正方向的平面直角坐标系xoy-1为参考系的位置系数矩阵K:

其中

步骤7：计算基于平面直角坐标系xoy-1的估计位置参数

乘以定位场地的边长L即可得到标签T在平面直角坐标系xoy-1的位置坐标(x_T,y_T)其中：

附录1

证明：

在底面为矩形的四棱柱(如图3)中

因为AE²+CE²-BE²-DE²

＝(AH²+EH²)+(CH²+EH²)-(BH²+EH²)-(DH²+EH²)

＝AH²+CH²+BH²-DH²

＝(AF²+AG²)+(BF²+GD²)-(BF²+AG²)-(GD²+AF²)

＝0

所以AD²+CE²＝BE²+DE²。

需要说明的是上述实施例，并非用来限定本发明的保护范围，在上述技术方案的基础上所作出的等同变换或替代均落入本发明权利要求所保护的范围。

Claims (3)

1.一种基于估计偏差平均的四基站UWB平面定位方法，其特征在于，所述定位方法包括以下步骤：

步骤1：标签向四个基站发送测距请求，获得标签到四个基站的距离值d_i(i＝A,B,C,D)；

步骤2：计算几何误差

如果几何误差E大于误差容限E_threshold，则放弃对本次数据做进一步处理，重新执行步骤1；

步骤3：根据公式：

计算出x_i,i＝A,B,C,D的值并检查所有相邻的距离数据是否符合约束，若发现存在不符合约束的数据，则放弃对本次数据做进一步处理，重新执行步骤1；

步骤4：根据标签到四个基站的距离值d_i(i＝A,B,C,D)，取所有相邻基站的距离数据，根据公式(4)计算出四组位置参数(k_{1_AB},k_{2_AB})、(k_{1_BC},k_{2_BC})、(k_{1_CD},k_{2_CD})、(k_{1_DA},k_{2_DA})；

步骤5：根据标签到四个基站的距离值d_i(i＝A,B,C,D)计算出权重矩阵；

其中

步骤6：计算以D'为原点，

为X轴正方向，

为Y轴正方向的平面直角坐标系xoy-1为参考系的位置系数矩阵K:

其中

步骤7：计算基于平面直角坐标系xoy-1的估计位置参数:

乘以定位场地的边长L即可得到标签T在平面直角坐标系xoy-1的位置坐标(x_T,y_T)，其中：

其中H为四个基站距离标签所在平面的高度，

表示哈密度算子；

在步骤4中，使用公式(4)计算出四组位置参数(k_{1_AB},k_{2_AB})、(k_{1_BC},k_{2_BC})、(k_{1_CD},k_{2_CD})、(k_{1_DA},k_{2_DA})，

其中公式(4)如下：

使用相邻的两基站到标签的距离计算可得到四组位置参数：

2.根据权利要求1所述的基于估计偏差平均的四基站UWB平面定位方法，其特征在于，

步骤2中，引入几何误差E来衡量当前一组距离数据的可靠性，当可靠性达不到要求时，则将该组距离数据视为不可靠数据并放弃本组数据，重新测量。

3.根据权利要求2所述的基于估计偏差平均的四基站UWB平面定位方法，其特征在于，

在步骤5、6中，使用位置参数对距离数据的梯度的模值倒数作为计算权重，使最后估计出的位置参数中每一个距离数据引入的估计偏差相等。

Images