一种融合卡尔曼滤波的室内组合定位算法*

徐淑萍，熊小墩，郭 宇，苏小会，张玉西

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

移动机器人获取自身位置是最基本的要求，如何在室内复杂多变的环境下完成高精度、快速实时定位任务成为一个需要解决的难题[1]。目前，在室内环境下常用的定位技术有Wi-Fi、超宽带(Ultra Wide Band,UWB)、超声波等。其中，基于UWB技术的定位系统，具有测距精度高、发射功率低、通信稳定性好、抗干扰能力强等特点，适用于室内定位[2]。而在室内无线定位方法中，基于测距的到达时间差(The Time Differences of Arrival,TDOA)具有测量简单、成本低、高定位准确度等优点[3]。

在室内定位方法方面，文献[4]使用三边定位法构建UWB定位系统，针对带有噪声干扰的定位数据，采用IMM卡尔曼滤波方法进行平滑处理，显著抑制了噪声带来的干扰。文献[5]提出了一种基于UWB技术的掘进机位姿检测方法，考虑环境变化、测距范围等影响因素的情况下进行测距实验并进行定位精度的分析。由于系统和测量噪声误差的统计特征很难被准确估计，所以常见的卡尔曼滤波效果并不理想[6]。文献[7]采用模糊自适应卡尔曼滤波，虽然可以实现测量噪声方差阵的在线自适应调整，定位精度有所提高，但是实时定位效果却不佳;文献[8]在系统噪声的估计模型中引入了极大似然准则，对模型优化过程中加入滚动时域策略[9]，最后基于二次规划方法对噪声统计的估计值进行获取，但是该方法复杂度较高。

综合上述分析，文中基于TDOA定位方法，并结合陈-泰勒(Chan-Taylor,C-T)协同定位算法，引入权重因子，再融合卡尔曼滤波进行平滑处理，设计一种陈-泰勒-卡尔曼(Chan-Taylor-Kalman,CTK)组合定位算法，减少实际定位过程中因随机误差和环境干扰引起的的测量误差而产生的噪声数据对定位的影响，提升定位的精度和稳定性，并在移动机器人平台上进行动静态实验，相较于C-T协同算法取得了更好的定位效果。

1 TDOA定位算法描述

TDOA即信号到达时间差测距，通过测量两个不同基站接收信号的时间差值，乘以信号的传播速度c得到定位标签到基站的距离差，求解距离差方程组得出定位标签的位置坐标,如图1所示。

图1 TDOA定位原理图

TDOA二维定位为双曲线模型，选定BS1为主基站，BS2、BS3为参考基站，BS1、BS2、BS3接收到信号的时间分别为t1、t2、t3，MS(x,y)为定位标签的位置坐标，固定基站BSi的坐标为(xi,yi)，ri(i=1,2,3)为固定基站到定位标签的距离，ri1为定位标签到第i个基站的距离和到主基站BS1的距离的差值。由定位标签到3个固定基站的时间差可得2个TDOA值，结合ri1=c·(ti-t1)得到距离差的方程组为

(1)

由式(1)得到双曲线的方程组为

(2)

对式(2)的方程组求解可得出定位标签的位置坐标。

1.1 Chan 算法

Chan算法基于TDOA技术，是求解解析表达式双曲线方程组的定位算法[10]，具有非递归性。求解过程中，对式(2) 进行线性化处理，两次利用最小二乘法加权估计结果。Chan定位算法在假定测量噪声分布符合高斯分布并且测量误差较小时，定位性能较好，但在实际多径环境中，信道环境较为复杂，定位精度将变得很差[11]。

1.2 Taylor 算法

Taylor算法是一种需要初始值的递归算法[12]，其利用上一次的计算结果作为本次迭代过程的初始值，通过最小二乘法求出误差解来对迭代值不断的进行修正，直到误差满足设定的阈值为止。该算法最大优点是精度高，但是在解算之前，需要选择合适的初值，初值选取将直接影响定位结果的精度[13]，因此一般会与其他算法协同定位。

2 基于UWB的CTK组合定位算法

2.1 C-T协同定位算法

为了减小运算的复杂程度并提高运算效率，将 Chan算法作为Taylor级数展开法的初始算法，既可以改善在非高斯噪声的干扰下Chan算法精度下降，Taylor算法结果出现不收敛的现象[14]，得到一个比较准确的初始估计坐标，又能反映未知节点位置和测量值之间的关系。所以将Chan算法和Taylor算法进行融合优化为C-T协同定位算法，算法的具体步骤如下：

① 利用UWB基站的分布位置和TDOA测量值构建TDOA方程，然后利用Chan算法对TDOA中的定位方程组进行求解，解算出标签的位置估计值。

② 若①中得到的初始值均方根误差满足应用，则直接作为最终的标签位置估计值。

③ 当①若中得到的初始值均方根误差不满足应用时，则利用Taylor 算法对Chan算法解算出的估计值进行迭代修正，得出一个优化的定位结果，然后判断其误差是否满足设定的阈值。

④ 若③中的误差不满足设定的阈值，则转回到Taylor算法中继续迭代，直到满足设定的阈值为止。

⑤ 当③中的误差满足设定的阈值时，则输出其定位结果，作为最终的标签位置估计值。

2.2 C-T协同加权定位算法

虽然C-T协同定位算法相对于Taylor算法能更收敛，但是仍可能出现定位结果发散而不收敛的情况，所以通过把C-T协同算法的结果进行加权的计算，根据合理设置的加权系数，得出目标最终的坐标估计，获得更精确的位置，算法的具体步骤如下：

① 利用UWB基站的分布位置和TDOA测量值构建TDOA方程，然后利用Chan算法对TDOA中的定位方程组进行求解，解算出标签的位置估计值。

② 若①中得到的初始值均方根误差满足应用，则直接计算其加权系数获取最终标签定位估计值。

③ 当①中得到的初始值均方根误差不满足应用时，则利用Taylor 算法对Chan算法解算出的估计值进行迭代修正，得出一个优化的定位结果，然后判断其误差是否满足设定的阈值。

④ 若③中的误差不满足设定的阈值，则转回到Taylor算法中继续迭代，直到满足设定的阈值为止。

⑤ 当③中的误差满足设定的阈值时，则输出其定位结果，并计算其加权系数，获取最终标签定位估计值。

为了计算加权系数，定义定位结果与测量值之间的残差为

(3)

如果对同一组TDOA测量值分别采用k种定位算法，则第k种算法的加权系数Rk的计算方法[15]为

(4)

(5)

2.3 改进Kalman滤波剔除异常值

卡尔曼滤波算法分两个过程：预测和更新。预测阶段卡尔曼滤波算法利用前个状态的估计做出对当前状态的估计[16-17]。然后更新阶段利用当前状态的观测值优化预测值，进而得到最佳估计新值。卡尔曼滤波的状态方程为

xk=Fxk-1+wk-1。

(6)

观测方程为

zk=Hxk-1+vk。

(7)

状态预测为

(8)

状态估计值为

(9)

滤波增益为

(10)

协方差为

(11)

更新协方差为

Pk|k=(I-KkHk)Pk|k-1。

(12)

当系统在k时刻进行更新时，异常的测量值会影响最优估计的结果，但若在式(9)状态估计时，进行修正计算的残差，然后再最优估计，则会减轻异常值的影响。其中Δ(k)为残差，D(k)为方差矩阵，表示为

(13)

D(k)=E[Δ(k)Δ(k)T]

=HP(k|k-1)HT+R。

(14)

由3σ准则和式(13)、式(14)可以得出异常值的一个判定公式，表示为

(15)

2.4 融合卡尔曼滤波的CTK组合定位算法

由于在实际定位中会有随机误差和环境干扰引起的测量误差，因这些误差产生的噪声数据会使定位产生波动，影响定位的稳定性和精度，而卡尔曼滤波能较好的抑制这些噪声，故进一步优化算法将C-T协同定位算法与卡尔曼滤波相结合构成CTK组合定位算法，以改善定位的稳定性同时提升定位的精度。该算法将上述C-T协同加权定位算法输出的估计位置坐标作为改进卡尔曼滤波的观测状态，对移动机器人标签坐标位置进行二次估计，即对初步优化估计坐标进行滤波处理，算法的具体步骤如下：

① 利用UWB基站的分布位置和TDOA测量值构建TDOA方程，然后利用Chan算法对TDOA中的定位方程组进行求解，解算出标签的位置估计值。

② 若①中得到的初始值均方根误差满足应用，则计算其加权系数获取标签定位估计值并作为改进Kalman滤波的观测状态，然后对标签的位置进行二次估计得到最终的定位结果。

③ 当①中得到的初始值均方根误差不满足应用时，则利用Taylor 算法对Chan算法解算出的估计值进行迭代修正，得出一个优化的定位结果，然后判断其误差是否满足设定的阈值。

④ 若③中的误差不满足设定的阈值，则转回到Taylor算法中继续迭代，直到满足设定的阈值为止。

⑤ 当③中的误差满足设定的阈值时，则输出其定位结果，并计算其加权系数，获取标签定位估计值。

⑥ 将⑤中获取的定位估计值作为改进Kalman滤波的观测状态，然后对标签的位置进行二次估计得到最终的定位结果。

3 实验验证与分析

3.1 定位性能评价指标

文中采用均方根误差(Root Mean Square Error,RMSE)作为定位性能的评价指标。

RMSE表示为标签的估计值和真实值偏差的平方与观测数n的比值的平方根[20]，其值越小，表明估计值越靠近真实值，表示为

(16)

式中:n为观测次数;x,y为机器人的实际位置坐标;xi,yi为第i次估计的标签位置坐标。

3.2 静态定位分析

实验采用一标签四基站的方式，其中UWB标签T0固定在移动机器人身上，用笔记本电脑远程控制机器人运动。四个UWB基站分别为A0、A1、A2和A3，其中以主基站A0的坐标(0,0)为原点建立坐标系，A1、A2、A3的坐标为各自布置的距离。各基站固定在距离地面1.5 m高的相同三脚架的顶端，基站和标签分别配备充电宝供电，其中主基站A0通过USB串口和笔记本电脑连接通信。测试场景如图2所示的10 m×10 m大小的室内视距环境，不考虑行人的干扰。

图2 场景布置示意图Fig.2 Layout of the test site

为分析算法在静态环境下的定位精度，在测试区域选取趋近于正方形的16个点用作移动机器人的定位分析，记为T1-T16，测量数据见表1。

表1 基站静态测量数据Tab.1 Static measurement data of base station 单位：m

由于UWB测距定位数据是实时刷新的，表1中的数据为该定位点上截取的一小段时间内测量结果的平均值。为了方便观察，将其中部分实时刷新的100次数据记录下来，分别计算其定位均方根误差，并用三维图进行表示，如图3所示。

通过图3和图2中基站的分布位置可以明显看出，由于基站的安放位置靠近墙边，靠近墙体的定位点精度会受到墙体的影响导致定位的RMSE增大，越靠近四个基站的中心位置，定位的误差越小。同时对比图3(a)和图3(b)可以看出，同样是靠近墙体定位受到干扰的情况下CTK组合定位相比于C-T协同定位的均方根误差更小，即定位的精度更高。

图3 两种算法定位均方根误差三维图Fig.3 Three dimensional maps of the root mean square error of the two algorithms

对表1中16个点的定位结果进行分析，比较CTK组合算法和C-T协同算法的定位效果，如图4所示。从图4(a)中可以观察出CTK组合定位算法的位置估计值相比于C-T协同算法更靠近实际值，定位结果更加准确。从图4(b)可以观察到，CTK组合算法在绝大多数的定位RMSE都要明显低于C-T协同算法。

图4 C-T和CTK两种算法定位结果对比Fig.8 Comparison of the positioning results obtained by C-T and CTK algorithms

结合表1中两种算法的平均RMSE表明：静态情况下CTK组合算法相较于C-T协同算法定位RMSE下降了约13.2%，进一步降低了噪声对定位的影响，提高了定位的精度。

3.3 动态定位分析

图2为实验环境的视距场地，机器人从坐标(1，5)匀速直线运动到坐标(11，5)，速度设置为v=0.5 m·s-1，如图5所示。

图5 两种算法动态定位结果Fig.5 Dynamic positioning results obtained by two algorithms

由图5(a)可以看出，C-T协同算法定位的坐标点比较分散，没有集中靠近在真实运动轨迹附近，而CTK组合算法的定位轨迹与真实运动轨迹则比较吻合，整体靠近在真实运动轨迹附近。从图5(b)中两种算法定位轨迹的均方根误差中可以观察到CTK组合算法定位RMSE相对比较平稳，波动幅度较小，大部分集中在9 cm，而C-T协同算法虽然有小部分RMSE接近于0，但整体波动幅度相对较大，定位性能不稳定。

综上所述，动态环境下CTK组合算法相比于C-T协同算法的定位性能更稳定，整体来看定位的RMSE更低，定位精度更高，更好的抑制了噪声的干扰影响。

4 结 论

通过对UWB定位算法进行分析和优化，在C-T协同算法的基础上，引入权重因子，再融合卡尔曼滤波算法进行平滑处理，提出一种抑制定位误差的CTK组合算法。实验结果表明CTK组合算法相比于C-T协同算法静态环境下的定位均方根误差为7.54 cm，下降了约13.2%，提升了定位的精度；动态环境下的定位均方根误差波动幅度更小，集中在9 cm，定位性能更稳定。在室内对高精度定位有要求的场景下，将具有较高的应用性和实用价值。