一种非视距环境下改进的组合定位算法

徐淑萍，杨 帆，苏小会，王 双

(西安工业大学 计算机科学与工程学院，西安 710021)

随着我国对机器人技术的重视程度不断加深，对移动机器人的使用已经逐渐渗透到社会生产生活的方方面面。在移动机器人技术当中，如何提升移动机器人在室内环境里的高精度定位要求已成为目前一个不可忽视的问题[1]。而传统的室内定位技术例如红外线、超声波、WIFI、蓝牙、射频识别等等，已经不能满足移动机器人对于在精度上的要求。文中所使用的超宽带(Ultra Wide Band，UWB)技术，由于使用UWB信号的功率谱密度极低，脉冲宽度极窄的脉冲信号进行数据传输，使得UWB定位技术在传输速度，功率消耗，穿透能力、定位精度相比于其他定位技术效果更好，特别是在视距(Line of Sight，LOS)传输下的定位性能的效果显著。但真实环境下往往非视距(Non-Line of Sight，NLOS)的情况较多，此时会导致定位精度明显下降[2-4]。因此研究在非视距环境下，如何提高定位精度，已然成为了提升室内定位技术的重要挑战之一[5]。

为了抑制NLOS环境对定位精度的影响，国内外已提出多种解决方案。文献[6]通过分析视距以及非视距环境下的UWB信号，通过SVM算法来辨别和减少非视距环境对定位精度的影响，该方法虽可以有效降低非视距误差，但工作量较大；文献[7]将校正因子带入最小二乘法中，通过逐步迭代降低非视距的影响，但室内定位精度较低；文献[8]以惯性导航结合UWB技术共同定位，利用先验阈值对非视距进行判断，然后使用惯性导航的短期高精度结果来判断和消除非视距误差，但硬件成本较高；文献[9]利用残差加权法来减少非视距误差，但由于需要较多的基站参与定位，可实施性低。文献[10]通过最小二乘法以及泰勒级数算法对移动目标进行定位，可解决泰勒级数算法不收敛的问题，但无法消弱非视距误差对定位的影响；文献[11]利用无迹卡尔曼滤波和粒子滤波方法来计算移动目标的位置以及速度，可消弱非视距误差，提升定位精度，但算法复杂，不易实现。

基于上述分析，通过在扩展卡尔曼滤波器中的待估计状态向量中引入非视距误差转换因子判别和消除非视距误差，并在基于Chan和Taylor算法的协同定位算法基础上，融合卡尔曼滤波算法，计算移动机器人的位置，提升整体算法的定位精度。

1 CTK组合定位算法

1.1 Chan-Taylor协同定位算法

针对测量噪声分布为高斯分布与测量误差较小的情况，Chan定位算法的定位性能较好，但在实际多径环境中，信道环境较为复杂，定位精度将变得很差[12]。Talyor定位算法简单实用，但是对于初始坐标的依赖性较强，在信道环境中比较差的情况定位精度会下降，只有当给定的初始值接近真实坐标时才能得到精确的解[13]。由于两种算法都有自己的局限性，为了减小运算的复杂程度并提高运算效率，使用Chan算法作为Taylor级数展开法的初始算法，既能得到一个比较准确的初始估计坐标，又能反映未知节点位置和测量值之间的关系，有利于Taylor算法收敛[14]。同时为了提高算法精度，利用Chan和Taylor级数展开算法的结果进行加权系数的计算，根据合理设置的加权系数，对两种算法的位置估计进行处理，得出目标最终的坐标估计，获得更精确的位置。

首先把基站布置好，相应的基站坐标保存，然后计算TDOA测量值，有了TDOA测量值以后，就可以构建双曲线方程，解该方程时，用上述的Chan算法得到待定位标签的初始值，然后将该初始值作为Taylor算法的起始点，接着计算均方根误差，并判断误差是否超过迭代门限，直到满足迭代门限，使误差最低，即得出待定位标签的位置坐标。

具体的操作步骤[15]如下：

① 利用 Chan 算法解算出待测物体的位置估计值并将其作为初值。

② 利用 Taylor 算法经过多步运算，得出一个定位结果。

③ 由②处理后的位置估计往往精度高于第一步的初值，但为处理Taylor 算法结果不收敛的情形，计算得到基于残差的加权系数。

④ 将此加权系数与①②得到的位置估计结果相结合，得出Chan-Taylor(简称C-T)协同定位算法的最终位置估计。

1.2 融合Kalman的CTK组合定位算法

C-T协同定位算法不仅稳定性好，而且定位精度比单一的定位算法要高，但也有随机性误差存在。卡尔曼滤波能较好的抑制随机性误差，虽然C-T协同定位算法引入了加权系数，但在不同环境下还是不可避免的存在异常值或野值或离群簇，导致定位算法结算的位置离实际位置偏离较大。本文将C-T协同定位优化算法与Kalman滤波相结合构成C-T-Kalman(简称CTK)组合定位算法，在室内定位中，C-T协同定位算法的输出坐标作为Kalman滤波的初始值，输出移动定位标签的估计位置。

首先C-T协同定位算法对移动机器人标签初次估计坐标位置，引入加权系数；其次按加权系数计算初次位置坐标得到精度更高的估计位置坐标；最后，将输出初步优化的估计坐标位置作为卡尔曼滤波的观测状态，对移动机器人标签坐标位置进行二次估计，即对初步优化估计坐标进行滤波处理。

2 改进的CTK组合定位算法

室内定位环境中，由于障碍物导致信号的非视距传播路径，进而造成的UWB定位估计误差是导致定位偏差的主要来源之一[16]。首先，非视距环境引发信号传播的多径效应使得信号到达接收节点的直达分量被错误判别；其次，由于UWB信号传播过程中穿过障碍物时使得信号到达时间附带时延，因此非视距误差不可避免会受到一定的影响，进而产生基站 (Base Station，BS)与移动台(Mobile Station，MS)也就是本文所说的移动标签的测量距离明显大于真实距离，导致UWB定位估计值大于真实值。鉴于上述非视距的影响，若实际定位中直接采用信号在非视距环境中得到的UWB定位估计值进行标签位置估计，那么估计值会大大偏离真实值，因此对非视距传播路径进行鉴别分类，再通过CTK算法处理相关数据可以提高室内定位性能[17]。

2.1 非视距误差鉴别

(1)

式(1)中,当假设检验H0成立，信号在NLOS环境传播;当假设检验H1成立，信号在NLOS环境传播。确定测量误差来自NLOS环境影响后，用卡尔曼滤波对测量数据进行重构，将重构后的数据代入定位算法，计算出待测目标的位置坐标。

为了更好的说明传统定位算法(Chan算法，Taylor算法)，C-T算法以及CTK算法在定位精度上的差别，利用Matlab对这四种算法在理想的高斯噪声环境下，只考虑基站设备自身的测量误差，分别测量当TDOA测量值误差为10 ns,20 ns,30 ns,40 ns,50 ns，标准差分别为3 m,6 m,9 m,12 m,15 m时这四种算法对目标信号的位置估计值，设定参与定位的基站为4个，用基站来定位机器人标签发射源。主基站设在A号监测点并用均方根误差来评价仿真结果，如图1所示。

图1 四种算法定位误差仿真结果

由图1可知，C-T算法的均方根误差值比Chan算法、Taylor级数定位算法的低，其定位性能优于传统定位算法，但是CTK算法比C-T算法定位性更高。可见CTK提高了经典TDOA算法的定位精度，是一种可行有效的新型定位算法。

2.2 引入转化因子误差模型

在非视距环境下误差模型可表示为

rm(tk)=Lm(tk)+vm(tk)+nm(tk),

(2)

式中:rm(tk)是tk时刻在NLOS环境中BSm基站与待测目标的距离值，Lm(tk)表示tk时刻在视距环境中基站BSm与待测目标在tk时刻的真实距离值，vm(tk)为基站的系统测量误差，nm(tk)是NLOS环境下在tk时刻产生的测量误差。

如果直接利用较大非视距误差的非高斯测量值对移动台进行定位计算，会导致移动台的预测位置与真实距离的误差增加[16]。为减少这种情况带来的误差，文中引入非视距转换因子r(k)，利用式(3)将非视距误差转化为视距误差，如下所示。

Lm(tk)+nm(tk)=[1+r(k)]Lm(tk),

(3)

转换因子r(k)等于非视距误差nm(tk)所占真实距离的比率Lm(tk)。综合式(2)与式(3)可以得出引入非视距因子的误差模型公式为

rm(tk)=[1+r(k)]Lm(tk)+nm(tk)。

(4)

(5)

由式(5)可知，当r(k)=0时，表示tk时刻的测量值为视距环境下的测量值；当r(k)>0时，则表示tk时刻的测量值为非视距环境下的测量值。

2.3 抗非视距误差Kalman滤波

由于在非视距环境下，UWB信号会在穿过障碍物时会附带时延而导致测量值产生误差，使得Kalman算法性能下降[18-20]。为了消弱非视距误差对位置预测的影响，将上一小节提出的非视距转换因子r(k)引入到扩展Kalman滤波的待估计状态向量中来改进其迭代过程。

新的状态矢量表示为

(6)

其中[x(k)y(k)]T为水平和垂直方向的移动台坐标，可以得到新的测量模型为

(7)

其中，改进的扩展Kalman滤波的状态转换和测量模型可用下面(可微)函数表示

(8)

(9)

2.4 非视距CTK组合定位算法流程

为了抑制非视距误差，文中结合CTK算法给出了一种适用于室内非视距环境的改进组合定位算法，算法实现步骤如下。

① 设置定位环境参数包括定位区域、基站部署、视距与非视距设置，误差模型；

② 引入非视距转换因子进行误差识别，当为视距路径时保存测距值转到④，当为非视距路径时转到③；

③ 利用改进卡尔曼滤波器优化处理非视距路径的测距值，通过多次测距将非视距误差转化近似服从正态分布；

④ 将测距值整理并建立TDOA方程；

⑤ Chan算法解析TDOA方程并获得标签位置的第一次估计值；

⑥ 将估计值代入Taylor算法中获得第二次估计值；

⑦ 计算第二次与第一次位置估计值的RMSE；

⑧ 判断RMSE是否超过预设的门限值，若超过门限值，则跳转至④；若没有超过门限值，则输出位置估计值；

⑨ 根据此时位置估计值计算加权系数；

⑩ 根据加权系数计算最终标签位置估计值；

3 定位实验与分析

3.1 视距环境下定位分析

实验场地设定为4号教学楼的一块6 m×10 m的场地，如图2所示。假设噪声服从高斯分布，测定移动机器人在运动时的定位数据并进行分析。

图2 机器人定位部署环境

本实验选用人进行代替移动机器人。假设人是类似机器人移动，为了减少由于人身体对定位标签的遮挡，让人在测量期间一直手持定位标签并举过头顶。机器人在地面移动，而人手持定位标签，定位标签在大约离地2米的空中移动，对此所造成的轻微影响本实验不考虑在内。任意选取四个基站中的其中一个作为定位标签，其他三个为固定基站。固定基站的坐标分别为(0,0)，(5,5)，(10,0)，在规定的路线移动运行一段时间，并且保持周边环境不变，例如没有人员走动，选取前25秒对收集的位置坐标数据使用Matlab仿真分析。首先视距环境下测试，之后在非视距环境下测试，测试数据图如图3所示。如图3(a)所示，其中CDF也称为累积分布函数，为概率密度函数的积分，用来说明待测标签定位误差的概率分布。累积分布函数图用来描述定位误差满足门限之下定位点数量的概率分布情况，从图中可以清晰的看到不同算法的定位效果。视距环境下的改进前后的CTK组合算法对移动台的移动轨迹的测量差距较小，更趋近于真实轨迹；如图3(b)所示。当位置误差为60 mm时，虽然改进后的CTK组合定位算法较改进前的CDF相差不大，定位误差相差也比较小，但整体上还是略优于改进前的CTK算法。实验结果表明在不考虑轻微误差时的视距环境下，两种组合算法都适用。

图3 视距环境下CTK改进前后定位结果对比图

如图4所示，改进前后的CTK算法在测定视距环境下移动台与三个固定基站之间的距离时，在测定第二个基站的定位值与真实值有轻微的误差，其他两个基站的测量值与真实值也基本吻合。同样表明改进前后的CTK定位算法在视距环境下的定位误差都较小，精度较高，都适用于对视距环境下移动机器人的测量。

图4 视距环境下改进前后的CTK算法测定移动台与基站的距离值对比图

3.2 非视距环境下定位分析

实验场地设定为学院实验室的一块6 m×10 m的场地，如图5所示。假设噪声服从高斯分布，测定移动机器人在运动时的定位数据并进行分析。任意选取四个基站中的其中一个作为定位标签，其他三个为固定基站，固定基站的坐标分别为(0,0)，(5,5)，(10,0)。在非视距环境下的规定的路线移动运行一段时间，并且保持周边环境不变，例如没有人员走动，选取前25秒对收集的位置坐标数据使用Matlab仿真分析，其结果如图6所示。

图5 非视距环境下CTK改进前后定位结果对比图

图6 非视距环境下改进前后的CTK算法测定移动台与基站的距离值对比图

如图5(a)所示，在非视距环境下改进前后的CTK组合算法定位轨迹相差较大，改进后的CTK组合定位算法更加贴近于真实值；如图5(b)所示，以位置误差为100 mm时为例，改进后的CTK组合算法相较于改进前的CDF值有了明显的提升，也说明改进后的CTK算法的定位精度有了明显的提升，因此表明在非视距情况下，改进后的CTK算法在定位精度上有了一定的提升。如图6所示，改进前的CTK组合算法测量的移动机器人与第一、第二、第三基站的距离误差明显较大，而改进后的N-CTK组合算法机器人与第一、第二、第三基站的距离误差相对较小，定位轨迹较重合，充分说明在非视距环境下，改进后的CTK算法的性能要优于改进前的CTK算法。

3.3 机器人方型轨迹分析

选取室内环境6 m×7 m的范围内，采用四个基站节点一个标机器人签节点进行搭建，移动机器人从坐标(1 m,1 m)开始，一次移动到(5 m,1 m)，(5 m,5 m)，(1 m,5 m)这三个坐标点，并往返两次进行测试。设定移动机器人速度为0.5 m·s-1，可知移动机器人起始位置会有一个滞后差的距离。测距、数据传输以及算法处理所耗费的时间大约为100 ms，可知机器人在非视距环境移动状态下的滞后距离约为0.05 m。在更为复杂的环境下可能会造成更大的滞后差，CTK改进前后定位方型轨迹和CDF图如图7～8所示。其中图7的横纵坐标分别为选取的室内环境的长和宽，(Y距离，X距离)为机器人相对于选取的室内环境位置坐标。

图7 CTK改进前后定位方型轨迹

文中用的是普通方法绘制CDF图，而其他方法绘制的CDF图，可能会和文中的实验结果有差异，呈现的精度也可能发生细微的改变。取一次定位轨迹数据分析，如图8所示。可以看出，改进CTK联合定位算法大约95%的定位结果误差在0.04 m左右，CTK定位算法大约82%的定位结果误差在0.04 m左右，因此改进CTK联合算法定位结果更加准确，定位效果提升了大约13%左右。另外本实验采集的数据有限，所有实验结果图都是基于本实验数据量为某一定值的前提下得出的，当采集数据量增多或减少时，实验结果可能会和本文结果有偏差，文中的研究为提高室内定位精度提供了一种方法。

图8 CTK改进前后定位方型轨迹CDF图

4 结 论

基于UWB定位技术，在室内非视距环境下进行系统定位，为消除非视距误差，引入一个非视距误差转化因子，加入到扩展卡尔曼滤波待估计的状态量中。通过与CTK组合定位算法联合，提出一种改进的CTK组合算法，最后在视距环境和非视距环境下进行动态定位实验，并利用Matlab工具对文中算法定位结果能进行了仿真分析，使用定位性能评价指标对本文算法定位结果进行了优越性分析。分析结果表明，文中算法针对非视距环境在约95%的定位结果中的误差仅为0.04m左右，相比于CTK算法，定位精度效果提升了13%左右，定位结果更加准确，能够有效消除视距与非视距环境下的使用UWB技术进行测距时产生的非视距误差，从而提高了UWB 定位的精度和可靠性，满足室内移动机器人的定位要求。