优化的卡尔曼滤波算法在UWB 定位系统中的仿真研究

李 婕，王荣文，杨权启

（1.河南工学院 电气工程与自动化学院，河南 新乡 453003；2.新乡市机械设备运行状态智能监测工程技术研究中心，河南 新乡 453003）

0 引言

在室内定位技术研究中，定位的精度和坐标实时性问题一直都是热点和难点。近几年来，中国北斗与美国GPS 相继研发出了一些定位方法，但这些方法在室内环境中都无法获取较为精准的位置信息，因此研究一种针对室内的高精度定位系统是十分有必要的[1]。

超宽带(Ultra Wide Band，UWB)定位技术是一种采用纳秒级别的非正弦窄脉冲作为介质的无线载波通讯技术，因为其具有对信道衰落不敏感、截获能力低等特点而成为室内定位的优选方法。UWB 定位技术采用定位基站-被定位标签式的定位方法，在被定位标签附近放置3 个以上已知绝对坐标的基站，由每个基站测量出本基站和定位标签的绝对距离，通过几何解算得到标签的坐标[2]。

本文研究的主要内容是利用UWB 技术通过最小二乘法优化的卡尔曼滤波算法来对被定位系统的位置进行跟随和预测。首先通过UWB 技术对目标进行定位，获取被定位目标的平面坐标，并将定位的数据保存起来。该位置坐标是通过传感器直接获得的数据，故存在一定量的噪声和误差，所以需要用滤波算法对该数据进行处理[3]。假定被定位者在一个时间段内作匀速直线运动，由于卡尔曼滤波算法线性回归的特性，会使滤波器输出的坐标滞后于实际坐标。假定采用的传感器速度和控制器计算坐标的速度足够快，可以将被定位者视为在某一时间段内做直线运动。利用最小二乘法对已保存数据做最小误差估计，并对误差数据做二阶拟合，将拟合的结果用卡尔曼滤波算法进行校正，从而得到最优坐标信息。通过试验验证，相比于传统的卡尔曼滤波算法，校正后的算法对坐标位置的跟随具有更好的实时性和快速收敛性[4]。本文将以仿真和实际模型验证改进算法的可行性，并通过对比表明改进算法能有效地提高定位的精度。

1 UWB 技术定位模型

1.1 UWB 定位原理

UWB 定位技术的基础主要是采用ToF 测距方法，即飞行时间测距方法。这种方法通过电磁波在空中飞行的时间解算出发射点和接收点的绝对距离。直接采用这种方法来解算距离会因为各自晶振不同步，导致解算出来的数据精度较低[5]。为了解决这一问题，本文采用每次做完通讯后都发送一个专门用于同步信号的方法，具体通讯时序见图1。

图1 通讯时序图

图1 中，由设备A 发送测距请求信号给设备B，当设备B 接收到测距请求信号后经过处理在t3 时间段对设备A 进行响应反馈。为了使下次测距时两个设备的时钟同步，设备A 在t5 时刻发送一帧数据到设备B 作为下次通讯时的时钟同步信号。TF为信号的飞行时间，Delay为设备B 接收到信号所需要的处理时间。最后由设备A 计算并得出两个设备的绝对距离为：

其中distance为两个设备之间的距离，c为光速常数，t1—t6 为设备A 发送的时间戳。

1.2 定位模型

在获得各个基站与被定位标签距离之后，需要进一步解算才可以得到坐标数据。假设定位基站与被定位标签的绝对距离为d，利用测量学中的空间距离后方交会法可以得出，被定位的标签一定是在以基站为圆心、d为半径的圆上[6]。但在实际中，无法保证基站和被定位者永远在同一平面内，并且被定位者在定位空间中的高度也不一定一致，因此如果采用传统的三点定位法而不加以矫正会向系统引入更多的噪声，从而导致误差增大。为了解决这一问题，本文采用以基站为圆心、以d为半径的空间球体进行位置解算，当定位基站存在三个或三个以上时，就可以确定空间中一条弧线段，该弧线段的上下两个端点坐标就是被定位者可能存在的两个坐标，对得出的两个坐标，可以根据工程实际情况舍去一个坐标，或者直接使用第四个定位基站确定另一个坐标。

当方程式大于等于4 个时，求解上述方程组即可获得被定位标签的空间唯一解。但是实际中存在的距离误差和传输干扰会产生一个距离误差ρ。所以在计算的过程中考虑到ρ的存在可以提前舍去误差外的坐标。

2 卡尔曼滤波算法的定位实现

卡尔曼滤波器是一种基于时序的最佳线性滤波器，它可以根据上一时刻的系统状态和系统推测状态来预测下一时刻的系统真实状态。卡尔曼滤波算法可以简单地由以下5 个方程组成：

（1）状态预测方程：该方程根据状态转移矩阵F把上一时刻的状态转换为当前时刻的状态。

（2）预测协方差矩阵：

（3）计算卡尔曼滤波增益：

（4）输出目标状态：

其中，z为传感器的测量向量，在本系统中z就是通过UWB 技术经过几何计算得到的坐标值。

（5）优化协方差矩阵：

根据UWB 室内定位系统的特性，假设定位基站有n个，故可以获取的坐标就为f(n)个。令基站位置坐标为(i可取值1,2,3,…,f(n)，目标的坐标为(x,y)。故被定位标签到基站的距离d为：

系统状态空间模型为：

其中公式(9),(10)分别为状态方程和测量方程。X(k)为状态向量，A(k,k-1)为系统矩阵，W(k-1)为过程噪声向量，Z(k)为观测向量，C(k)为测量矩阵，V(k)为测量噪声向量。

由式(8)即可以构建一个关于x和y的方程

令状态变量X的初始值为式(17)，其中和为获取UWB 定位的坐标初始值：

该系统状态转移矩阵为：

过程噪声方差矩阵为：

最后带入式(3)—(7)即可计算出当前目标的真实状态。综上所述，卡尔曼滤波算法的工作顺序和递归原理可以由图2 简易地表达出来。

图2 卡尔曼滤波算法流程图

传统的卡尔曼滤波算法虽然会根据卡尔曼增益不断地调整系统观测和系统预测两个数据的权值，使得最终值逐渐逼近系统的真实值，但是在位置跟随等需要结果快速收敛的场合，传统的卡尔曼滤波算法会导致位置更新滞后，具有较大的稳态误差[7]。故本文针对UWB 室内定位与跟随状态停滞问题，在传统的卡尔曼滤波算法中加入最小二乘法对其进行优化。

3 优化的卡尔曼滤波算法

优化的卡尔曼滤波算法是通过最小二乘法与卡尔曼滤波算法相结合的计算方法。最小二乘法也叫最小平方法，它可以通过最小化误差来寻找与数据匹配的最佳函数[8]。它在工程中通常会用于传感器的标定和后续数据的预测，其原理就是通过已知数据和理想结果的误差平方和，并使其最小化，从而得到该组数据的回归函数，然后使用该函数对系统进行预测。

令t为最小二乘法的拟合长度，系统输入为，系统输出为，i=(1,2,3…n)。所以总误差的平方和为：

当最小二乘法为二阶拟合时，上述误差可以写为：

此时上式中不同的a，b对应不同的。所以当：

根据g(x)就可以对系统t+1 时刻的状态进行预测。在最小二乘法和卡尔曼滤波算法的融合中，将t+1 时刻的系统状态作为卡尔曼滤波算法中系统预测向量的，此时系统预测值的误差协方差为：

系统通过式(3),(27),(28),(4)—(7)即可计算或预测目标的状态值。综上，优化的卡尔曼滤波算法工作顺序和递归原理可由图3 简化得出。

图3 优化的卡尔曼滤波算法流程图

由上图可得优化的卡尔曼滤波算法，系统预测值不再使用t-1 时刻卡尔曼滤波算法输出值取而代之的是对传感器观测的历史数据拟合后再预测的值。由于上文中提到了在时间内，假定系统的真实状态是在做匀速直线运动，故可以通过最小二乘法对历史数据进行二次拟合后得到作为卡尔曼滤波算法的系统预测值，最小二乘法拟合的误差作为卡尔曼滤波算法的预测方差。所以不同于传统的卡尔曼滤波算法中系统预测值的是上一时刻的递归。通过最小二乘法拟合预测的数据更具有预测的参考价值，并且可以有效地抑制在几何解算过程中所出现的误差[10]。

4 仿真结果与对比

4.1 传统卡尔曼滤波算法与优化卡尔曼滤波算法的比较

通过MATLAB 对本文提出的采用最小二乘法优化的卡尔曼滤波算法进行仿真验证与经典算法作对比。假定一个20×200 的单位场地，目标在场地内从（0,0）点到（200,20）点做匀速直线运动。因在工程实践中Z轴坐标一般是为了求解平面坐标的辅助解，所以为方便计算不考虑Z轴的变化。采样周期为1，如图4 所示。

图4 目标实际运动轨迹

从 UWB 定位设备中获取的位置坐标精度为10cm 即1dm，UWB 定位设备作为系统观测数据的来源，故Q取单位1。

在MATLAB 中对原始信号加入1db 的高斯噪声来模拟UWB 定位设备的数据，这里为了观察更直观，加入噪声后的定位曲线的横坐标为被定位者的X坐标，纵坐标为被定位者的Y坐标，如图5 所示。

图5 UWB 定位曲线

在实际应用中，由于传感器本身器件特性都会存在一定量的噪声，并且这些噪声都遵循高斯分布，因此我们通过MATLAB 对假定的真实状态的X,Y坐标分别加入高斯白噪声，即可模拟出真实传感器测量的曲线情况。

图6 中，星号虚线的曲线代表使用未优化的卡尔曼滤波算法做滤波所得到的状态曲线。实线曲线代表使用最小二乘法优化的卡尔曼滤波算法所得到的状态曲线。由对比图不难看出，使用最小二乘法优化的卡尔曼滤波算法的性能优于原始的卡尔曼滤波算法。

图6 最小二乘法优化的卡尔曼滤波算法与传统卡尔曼滤波算法的对比

4.2 粒子滤波算法与优化的卡尔曼滤波算法的比较

粒子滤波算法是一种以贝叶斯推理和重要性采样为基本框架的算法，粒子滤波结构实际上就是加一层重要性采样思想的蒙特卡洛方法[11]。该思想的基本结构是用一组样本来近似表示系统的概率分布，然后使用这样的近似表达来估计非线性系统的实际状态，因此利用这个思想，粒子滤波算法可以处理任意分布形式分布的概率，这一点是优于传统卡尔曼滤波算法只能处理线性高斯分布的问题。但是由于粒子滤波算法的特性，其相比于优化的卡尔曼滤波算法还是缺少一定的快速性和准确性[12]。

对比图7 和图8 不难看出，粒子滤波算法误差在0—0.3 范围内，优化的卡尔曼滤波算法最开始误差达到了0.6，但是收敛之后，误差均在0.1 以下。虽然粒子滤波算法可以将具有高斯白噪声的信号与系统真实状态的误差控制在一定范围内，但是优化的卡尔曼滤波算法对噪声的抑制是优于传统的粒子滤波算法的。

为了更直观地看出优化的卡尔曼滤波算法在室内定位系统中相对于其他算法和传统的卡尔曼滤波算法的优势，表1 列出了三种算法在同样的环境下误差绝对值的均值。

由表1 可见，在UWB 室内定位系统中，优化的卡尔曼滤波算法明显优于传统的卡尔曼滤波算法和粒子滤波算法。所以测试表明，本文提出的采用最小二乘法优化的卡尔曼滤波算法针对UWB 室内定位系统，可以使定位精度和性能有明显的提升。

图7 粒子滤波算法误差曲线

图8 最小二乘法优化的卡尔曼滤波算法误差曲线

表1 传统卡尔曼滤波算法、优化卡尔曼滤波算法和粒子滤波算法的数据对比

5 试验数据与分析

在实际获取数据的过程中，采用DWM1000 超宽带定位模块，该模块具有“基站”和“标签”两种工作模式。在测试中，使用STM32 系列单片机作为主控制器，由于使用空间三个球体定位计算量过大，因此用四个模块作为基站，一个模块作为被定位的标签，采用平面三点定位的方式，利用另一个基站（其余三个基站不在同一平面）作空间补偿。由于UWB 测距的特性，距离数据最终只在基站上产生，因此在每个基站上还要额外添加一个无线通讯装置，用来保证距离数据不会混淆并可进行高速的数据传输。各个基站与主控的通讯采用无线MODBUS 通讯协议，由于该通讯协议的灵活性和简便性，后期测试中可以随机添加或者减少一定量的定位基站和被定位者，方便测试。

测试中被定位者以小车代替，小车的真实坐标从小车的驱动轮的编码器做积分运算得出。测试过程中小车从相对坐标点（100,10）匀速走向坐标点（200,20）。在被定位者（小车）匀速直线运动中，每间隔50ms 分别对小车实际坐标、UWB传感器计算出的原始坐标和经过优化的卡尔曼滤波算法计算得到的坐标各采集一次并上传到计算机，最后对上传的数据收集、分类后导入MATLAB并绘制曲线如图9 所示。

图9 中，original 曲线为被定位者真实的坐标，由于被定位者在测试中以小车代替，小车的速度和行进角度都是由内部控制算法控制。由于算法和传感器本身带有误差，所以被定位者（小车）行进曲线是一个含有一定噪声的曲线。UWB 曲线为传感器原始数据，least squares-Kalman 曲线为滤波后的数据。由图9 可以看出，优化的曲线明显比原始数据曲线平滑很多，并且在位置更新上并没有出现滞后的现象。然后再将原始数据和滤波后的数据取X轴数据求出误差，误差比较如图10 所示。

图10 中original 曲线为原始数据误差曲线，Kalman-Min 曲线为滤波后的数据误差曲线。由图可知，由最小二乘法优化的卡尔曼滤波算法可以有效抑制系统的噪声，滤波算法的误差有明显降低，且收敛速度快。

图9 测试数据曲线

图10 测试误差曲线

6 结束语

本文介绍了一种不同于传统的目标跟随算法的复合式滤波与预测算法，针对定位和动态位置跟踪系统的特性，通过最小二乘法特有的数据误差最小化的理念与卡尔曼滤波算法相结合，利用两种算法的优势对UWB 室内定位技术产生的过程噪声所导致的定位跟随误差做进一步处理，相对于传统的室内坐标跟随滤波算法，该算法可有效提高UWB 室内定位技术的精度和目标位置跟随的实时性和快速性。

通过测试，把小车作为被定位对象，采用四个DWM1000 超宽带测距（定位）模块作为定位基站，在被定位者以匀速直线运动的环境下，对本文提出的算法进行验证。通过试验得出的数据证明，本文提出的采用最小二乘法优化的卡尔曼滤波算法与传统的目标跟随算法相比，在保证准确性的同时具有较好跟随性和快速性。