一种高定位精度和抗干扰能力的超宽带三维定位算法

技术领域

本发明属于服务器维护领域，具体地说是一种高定位精度和抗干扰能力的 超宽带三维定位算法。

背景技术

现有的超宽带(Ultra Wideband,UWB)三维定位算法有传统三边定位算法、 EKF算法、UKF算法、EPF算法，其中EPF定位算法与UKF、EKF及三边定位算法 相比，EPF定位算法在三个维度上的均方根误差最小，其中X轴和Z轴定位精度 提升较为明显。由于粒子滤波算法的通病，EPF定位算法虽然定位精度有较大提 高，但是EPF定位算法的实时性较差，从而在实际使用中也存在一定的问题。

发明内容

本发明提供一种高定位精度和抗干扰能力的超宽带三维定位算法，用以解 决现有技术中的缺陷。

本发明通过以下技术方案予以实现：

一种高定位精度和抗干扰能力的超宽带三维定位算法，包括如下步骤：

步骤一：通过利用ADS-TWR测距方法得到基站和标签之间的距离；

以ADS-TWR方法测量的距离值作为测量信息，距离模型的测量方程为：

式中：C＝[1 0]，v

式中：

式

步骤二：通过KF算法对ADS-TWR测距数据进行“一次”滤波，经过此步骤 可以对测距数据进行滤波，以减少NLOS现象和噪声造成的测距信息波动，去除 异常数据，为EPF定位算法提供较为准确的测量信息

“二次”滤波的距离测量模型如下所示：

式中：

以下式作为状态方程：

以下式作为非线性观测方程：

步骤三：使用EKF算法更新粒子和权重；

(1)当i＝0时：从先验分布p(x

(2)当i＞0时，使用EKF算法更新粒子：

式中，Q为过程噪声方差，R为测量噪声方差。从重要性函数中获取采样点

权重更新：

归一化权重：

在k-1时刻利用EKF算法，以及最新的观测信息D

如上所述的一种高定位精度和抗干扰能力的超宽带三维定位算法，步骤一 中基站在与标签通信的同时，基站与基站之间也在互相通信，可以获取任意两 个基站之间的距离数据，标签坐标处于静止状态时，可以利用标签坐标测算基 站坐标，将静止的标签作为基站，以防止基站异动对定位数据造成影响。

如上所述的一种高定位精度和抗干扰能力的超宽带三维定位算法，所述的 标签坐标在误差范围内跳动时可认为标签处于静止状态。

如上所述的一种高定位精度和抗干扰能力的超宽带三维定位算法，所述的 不管是在室内还是室外的实际运用中，定位系统中基站的位置难免会受到外力 的作用而导致移位，例如宠物、自然界中的风、鸟类等。这样会导致基站坐标 公中RNt所代表的基站坐标矩阵与真实值出现偏差，自然而然会导致非线性观 测方程中的非线性观测方程出现偏差，由此产生链式反应导致最后的标签定位 精度大打折扣。为了避免基站移动对标签定位精度产生大的影响将基站位置自 动更新算法带入KF-EPF定位算法的迭代，经过多次迭代可以得到基站的精确位 置信息。

利用ADS-TWR测距算法双向通信的特点，基站在与标签通信的同时，基站 与基站之间也在互相通信，可以获取基站与基站之间的距离数据 D

对输出的定位结果X

同时，将

测算图3中3号基站坐标时，需要将标签坐标带入到RN

将

测算图3中4号基站坐标时，需要将标签坐标带入到RN

将

分别将粒子群初始化为2、3、4号基站移动之前的坐标，这样可以减少迭 代次数。经过实验证明，迭代30次即可将位移基站准确定位，本实验采用的定 位系统28ms即可完成一次定位，也就是说一个基站的重新定位只需1秒即可完 成。1秒后取50组数据的平均值作为基站定位的最终坐标。

本发明的优点是：本发明通过将卡尔曼滤波与扩展粒子滤波算法相结合的 定位算法(即KF-EPF算法)，利用卡尔曼滤波对距离数据进行“一次”滤波， 用来处理NLOS条件下的测距异常数据，然后将处理过的距离数据作为扩展卡 尔曼滤波的观测数据进行位置结算，从而提高的定位的准确性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施 例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描 述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出 创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明的KF-EPF算法流程图；

图2是本发明的KF-EPF算法跟踪轨迹图；

图3是本发明的室内实验测试布置图；

图4是发明的KF-EPF算法定位轨迹图；

图5是本发明三维SLAM激光扫描定位轨迹图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明 实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然， 所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中 的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其 他实施例，都属于本发明保护的范围。

室内定位系统实验

室内内定位系统实验的设置如图3所示，图中所标数字代表的器械及功能 依次为：

①为第0号定位基站，其标签所在地面位置为坐标系原点，除了与其他三 个基站和标签进行测距通信外，还要与计算机通过数据线通信。

②、③、④为第1、2、3号定位基站负责与标签、基站间互相进行测距通 信。

⑤为定位标签，将其固定在移动机器人上，确保标签放置在机器人俯视图 的中心位置。

⑥为实时定位与3D扫描设备，其基于先进的3D激光SLAM算法，无需GNSS 等外界辅助定位即可实时输出设备的六自由度位姿及环境3D点云。

⑦为12v DC锂电池，为实时定位与3D扫描设备供电。

⑧为双轮差动移动机器人底盘，通过平板控制软件可以控制机器人的移动 速度与转向等。

⑨为计算机负责运行上位机软件，与第0号基站通讯。

⑩为定位基站支架，用于调整定位标签高度。

静态实验

静态实验中，四个基站分别位于一个2.4m*7.2m的矩形顶点。标签均放置 在地砖顶点(600mm*600mm)并静止不动，以地砖顶点坐标作为标签坐标并假设 其没有误差，为了使实验更加贴近实际使用情况，人为的在实验中添加扰动。 具体方式为：在实验过程中有多人随机在实验场中走动。

为了显示KF-EPF先进性，设置三组对比试验。现有已知UKF与UPF算法在 系统模型的状态方程是线性的情况下定位精度的优势不明显，而且算法较为复 杂，所以就不再进行实物实验。设置KF-EPF、EKF和EPF三组对比试验，验证 KF-EPF算法在定位精度上的优势。

分别将标签放置在不同的位置，待标签在上位机上的位置显示稳定后输出 标签的坐标数据。每种算法静态实验进行三组实验，每组实验将标签放置在不 同的位置。分别将标签放置在(1.2,4.2,1.75)，(1.2,3.0,0.45)，(1.2,1.8, 0.15)坐标上，每个位置收集150组标签位置数据利用MATLAB计算其均方根误 差，结果如表一所示。

表一

由表一可知，KF-EPF定位算法在静态试验中测得数据最接近真实值且均方 根误差最小，因此KF-EPF算法对静止标签的定位精度最高。EPF算法最小误差 为15.57cm，最大误差为18.93cm，KF-EPF算法平均误差在12cm左右，最大误 差不超过15cm。与EPF算法相比定位精度提高3cm左右，满足本文的设计要求。

动态实验

为了验证KF-EPF算法的基站位置自动更新功能，分别将2号和3号基站移 动到(2.4,6.6,1.87)与(0,6.6,1.87)坐标下，模拟定位系统在实际应 用中由于外力作用而导致移位，待基站坐标在固定范围内跳动后分别输出算法 的更新数据，每个基站坐标输出50组数据，分别作平均值与均方根误差，其结 果如表二所示。

表二

由表二可知，KF-EPF算法的基站位置自动更新功能可以较为准确的测算基 站位置，对基站的平均定位误差在15cm以内，均方根误差较小，满足实际应用 需求。

动态实验中，基站位置为：基站0(0,0,1.87)，基站1(2.4,0,1.87)， 基站2(2.4,7.2,1.87)，基站3(0,7.2,1.87)。标签运动轨迹如图4中红 色折线所示，运动高度为1.55m，运动方式是将定位标签放置在移动机器人上， 机器人中心沿着瓷砖缝隙按固定轨迹匀速运动。使用三维激光定位对标签运动 轨迹进行精确跟踪以获得更加精确的定位轨迹，定位轨迹如图5所示。同时将 三维激光定位所获得的标签位置信息作为坐标真实位置信息并假设其没有误 差，用以比较KF-EPF定位算法的定位精度及计算KF-EPF定位算法均方根误差。使用KF-EPF定位算法的UWB定位轨迹如图3所示。显而易见，KF-EPF定位算法 具有良好的定位精度，算法估算的标签轨迹与真实轨迹贴合的十分紧密，只有 很少的路段出现些许偏差。

将上位机输出数据与三维SLAM激光扫描定位获得的真实坐标点做均方根误 差，X、Y、Z轴均方根误差如表三所示。KF-EPF定位算法的动态定位各轴均方 根误差最小，具有最高的定位精度。

表三

本发明采用卡尔曼滤波与扩展粒子滤波算法相结合的定位算法。利用卡尔 曼滤波对距离数据进行“一次”滤波，用来处理NLOS条件下的测距异常数据， 然后将处理过的距离数据作为扩展卡尔曼滤波的观测数据进行位置结算。经过 实验比较KF-EPF算法、EKF算法、EPF算法，发现KF-EPF算法的定位精度最高。 为了验证仿真的正确性进行实物实验，动态实验结果表明KF-EPF算法相较于其 他三种定位算法具有明显的优越性，定位精度在15cm以内，相较于EPF算法17cm 的动态定位误差而言有了较大的提高。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限 制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员 应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其 中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的 本质脱离本发明各实施例技术方案的精神和范围。