速度自适应约束卡尔曼滤波方法*

周承松，彭　竞，刘文祥，王飞雪

（国防科学技术大学电子科学与工程学院，长沙410073）

速度自适应约束卡尔曼滤波方法*

周承松，彭竞，刘文祥，王飞雪*

（国防科学技术大学电子科学与工程学院，长沙410073）

为解决动态目标定位过程中使用传统卡尔曼滤波算法不能利用先验信息提高定位精度、而使用速度约束卡尔曼滤波算法易发散等问题，论文提出一种基于速度先验信息自适应约束卡尔曼滤波方法。该方法以速度观测值和定速误差统计特性建立变异系数，实时判断速度观测值是否满足约束滤波的要求，从而自适应地在速度约束滤波和传统卡尔曼滤波间选择。实验结果表明，与传统卡尔曼滤波和速度约束滤波相比，本文算法的定位精度提高了cm级至m级，既克服了速度约束卡尔曼滤波易受定速误差影响的缺点，也能利用合理的速度观测值来提高定位精度。

卡尔曼滤波；速度自适应约束；动态定位；变异系数；先验信息

动态目标定位过程中，利用卡尔曼滤波KF （Kalman Filtering）对目标进行状态估计，会受到观测误差和动态模型误差影响导致状态估计精度较差［1-3］，而利用先验信息能够提高状态估计精度［4-8］。

文献［9］提出以道路先验信息作为约束条件的卡尔曼滤波算法，该方法必须实时获取道路信息，因此道路约束卡尔曼滤波算法存在一定局限性［10］。另外，文献［10-12］提出速度约束卡尔曼滤波算法，实验证明，该方法的定位精度易受定速误差的影响。

本论文提出一种速度自适应约束卡尔曼滤波方法，该方法以定速标准差和速度观测值建立变异系数，即自适应因子，在传统卡尔曼滤波和速度约束卡尔曼滤波方法间进行自适应选择。而文献［13］的自适应卡尔曼滤波依据道路信息自适应卡尔曼滤波方法是自适应调整动态模型。实验表明，与传统卡尔曼滤波相比，本文算法能有效提高定位精度，同时避免了道路约束滤波需实时获取道路信息的缺陷，也克服了速度约束滤波易受定速误差影响的不足。

1　卡尔曼滤波

卡尔曼滤波由状态方程和观测方程组成，即

式（1）中，Xk为k时刻的状态向量，φk，k-1为状态转移矩阵，Xk-1为k-1时刻的状态向量，Wk为状态模型噪声；式（2）中，Lk为观测向量，Ak为观测矩阵，ek为观测噪声。

卡尔曼滤波算法的具体流程为：依次计算预测状态向量、预测状态协方差、新息及其协方差、增益矩阵，最终计算得到状态估值和状态估计协方差。

2　速度自适应约束卡尔曼滤波

速度自适应约束卡尔曼滤波，就是在速度约束滤波的基础上，加入了自适应选择传统卡尔曼滤波的过程。因此，首先阐述先验信息约束卡尔曼滤波的原理、速度约束卡尔曼滤波的过程，再介绍自适应因子等内容，进而对本文算法更详尽的描述。

2.1先验信息约束卡尔曼滤波原理

将先验信息约束融入卡尔曼滤波中的方法主要有伪测量法［14］、降维法［15］和投影法［16］等，论文以投影法为例。投影法将卡尔曼无约束状态估计向量通过约束条件函数直接投影到约束面，得到约束状态估计向量X′。为求得X′，需根据先验信息建立状态参数间的关系式，即状态约束方程：

其中，B为状态参数的系数矩阵，D为常数矩阵。再以投影法原则建立目标函数：

其中，W为任意对称的正定权矩阵。

为了同时满足状态约束方程和投影法目标函数，根据式（3）和式（4）构造拉格朗日条件式，即式（5），式中λ为拉格朗日乘数向量。

为求得当Ω最小时的X′，令Ω对X′和λ的一阶偏导数等于零，即式（6）和式（7），再解得约束状态估计量X′，即式（8）。

2.2速度约束卡尔曼滤波

2.2.1速度约束卡尔曼滤波模型

车载接收机的测速精度约为0.2 m/s，而测距码的动态单点定位精度约为10 m［17］。因此，利用速度观测值间的某种关系提高定位精度，在理论上是可行的。由于车辆行驶速度、行驶范围受到车辆性能、交通法规和道路宽度等的限制，假设车辆在较短时间间隔Δt内的行驶轨迹是直线，平均速度为接收机在Δt内始末时刻速度观测值的平均值［13］。基于该假设，在Δt内车辆的位置值满足空间直线关系，如式（9），速度值满足式（10）。

式（9）和式（10）中，（u，v，w）为直线方向向量，（xk，yk，zk）为k时刻的位置值，）为k时刻和k-1时刻的速度均值。

根据xyz方向间的状态约束方程式（11）和式（12），可构建形如式（3）的状态约束方程，再参照式（8）就可求得速度约束滤波的结果。

2.2.2速度约束卡尔曼滤波条件

速度约束卡尔曼滤波利用xyz方向速度观测值两两间的状态约束方程进行滤波。分析式（11）和式（12）可得，要保证状态约束方程的准确性，速度观测值间的比值必须要尽可能准确，以接近真实速度间的比值，这就是速度约束卡尔曼滤波能提高滤波精度的前提条件。

由于存在定速误差，各历元的速度观测值间比值不一定都接近真实速度间的比值。当不满足速度约束滤波条件时，速度约束滤波结果将出现不可估量的偏差。因此，速度约束滤波存在易受定速误差影响的缺点。

2.3速度自适应约束卡尔曼滤波

速度自适应约束卡尔曼滤波是在速度约束滤波的基础上，通过速度比值的变异系数确定状态约束方程的准确程度，在约束方程不准确时选择传统卡尔曼滤波，在约束方程准确时选择速度约束滤波。实现这一思想的核心是构造一个自适应因子，在传统卡尔曼滤波和速度约束滤波间进行自适应选择。

2.3.1自适应因子

式（13）中，0为速度误差均值，σ为速度分量误差的标准差。在实际动态定位中，多普勒定速误差约为0.2 m/s，则速度分量的标准差σ约为0.115 5 m/s［17］。

要使得定速误差对速度分量间的比值影响较小以便保证状态约束方程的正确性，速度观测值间的比值则要尽可能接近真实速度间的比值，以xy方向为例，即要满足

在寻求满足式（14）的条件时，由于无法获知定速误差的真实值（εvxk，εvyk），则考虑从定速误差的统计特性着手，即式（13）。首先，利用速度分量误差的标准差σ构建集合U：

然后分析集合U，若集合内五个值的离散程度越大，说明定速误差对速度分量间的比值影响越大；若离散程度小，则说明定速误差对其速度分量间比值影响小。例如，当xy方向的真实速度值均为20 m/s，比值为1时，考虑定速误差的影响后，的比值是接近1的，即满足式（14），另外，集合中的五个值也都接近于1，此时这五个值的离散程度较小，定速误差对速度分量间比值影响较小；但当xy方向的真实速度值均为0.1 m/s，比值为1时，考虑定速误差的影响后，的比值是难以保证接近1的，即不能保证满足式（14），此时，集合中的5个值也都不一定接近于1，离散程度较大，定速误差对速度分量间比值影响也较大。故以集合中五个值的离散程度来衡量定速误差对速度分量间比值的影响是可行的。

衡量离散程度的指标主要有极差、平均差、标准差、方差和变异系数等，而标准差最为常用。但集合U中的元素Ui在各历元处的平均值是不一样的，若利用标准差衡量离散程度显得不合理，而采用标准差与平均值的比值（变异系数）来表示离散程度则更为合理［18］。以xy方向为例，其变异系数cvxy的求取公式为：

同理，求出yz方向的变异系数cvyz。以变异系数（cvxy，cvyz）作为自适应因子，当cvxy、cvyz都小于阈值d时，则认为定速误差对速度分量间的比值影响较小，满足速度约束滤波条件，选择速度约束滤波；反之，则选择传统卡尔曼滤波。

2.3.2阈值的确定

变异系数用来衡量序列离散程度，变异系数不超过某阈值则认为该数据稳定合格，这在工业制造、医学等行业有广泛应用。在白细胞过氧化物传感器研制中，制备固定细胞膜的重现性变异系数为5.5%［19］；在机器发动机使用寿命可靠性研究中设为30%～40%［20］；在学生考试成绩的稳定评估中设为20%［21］；在GBT 27404—2008实验室质量控制规范中，根据成分浓度的不同其变异系数门限变动在1.3%～43%区间。综上所述，本文实验变异系数的阈值d取0～90%内不同值，分析不同阈值对实验结果的影响。

3　实验及分析

3.1实验数据

实验中，跑车作起步、匀速、加减速、转弯等复杂的运动，共6个路段，真实轨迹如图1所示。

图1　车辆真实轨迹

各观测历元的参考坐标和参考速度已知，行驶时间为920 s，车载接收机采样间隔为1 s。在参考坐标和参考速度上加入服从正态分布的随机误差，模拟得到接收机的坐标观测值和速度观测值。

3.2实验方案

采用匀速运动模型，状态向量为Xk，以接收机输出坐标和速度为观测向量，建立状态方程和观测方程。

实验中，分别在不同定速误差的情况下，分析比较卡尔曼滤波算法、速度约束卡尔曼滤波算法和本文算法的差异。本文以位置偏差的标准差SD（Standard Deviation）和均方根RMS（Root Mean Square）作为滤波精度指标，SD越大则滤波结果越离散，RMS越大则滤波精度越差。

位置偏差的求解过程为：先求出各历元滤波结果与参考坐标的偏差（dxk，dyk，dzk）；再求出位置偏差dk，即

位置偏差的标准差SD的求取公式为：

位置偏差的均方根RMS的求取公式为：

3.3实验及分析

分析不同的定速误差、不同的阈值对卡尔曼滤波算法、速度约束卡尔曼滤波算法和本文算法的影响及差异。

3.3.1定速误差

定速误差即为加入参考速度上的随机误差，其服从高斯分布。在阈值为20%的前提下，实验选取均值为0，速度分量误差标准差σ分别为0.015 5 m/s至1.115 5 m/s等间隔的12个值，共12组定速误差序列。

为简要说明，给出当 σ1=0.115 5 m/s、σ2= 0.615 5 m/s时3种算法的位置偏差图，如图2，其位置偏差标准差SD和均方根值RMS如表1。

图2　位置偏差图

表1　位置偏差的SD和RMS　单位：m

另外，在12个不同σ的情况下，统计3种算法的位置偏差标准差SD和均方根值RMS，结果如图3、图4所示。

图3　位置偏差标准差（SD）

图4　位置偏差均方根（RMS）

结合表1、图2，重点分析图3和图4，整体上观察，随着定速误差的增大，三种算法的滤波位置偏差标准差和均方根也出现增大的现象，即定位精度呈下降趋势，其中，速度约束滤波受定速误差的影响最大，而本文算法基本不受影响，传统卡尔曼滤波受的影响最小。

当速度分量误差标准差σ在0～0.15 m/s时，此时的速度观测值相对准确，能够满足速度约束滤波的要求，故速度约束滤波的精度最高。而本文算法以变异系数不超过阈值的原则对速度观测值进行自适应筛选，而该原则不可能完全准确，排除了部分原本能用来约束滤波的速度观测值，因此其滤波精度比速度约束滤波法要差；但与传统卡尔曼滤波相比，其滤波精度要优。

当速度分量误差标准差大于0.15 m/s时，速度约束滤波的精度急剧变差；传统卡尔曼滤波的精度缓慢变差，但最为稳定。而本文算法的滤波精度明显优于速度约束滤波；与传统卡尔曼滤波相比，其SD值要优，即滤波结果的离散程度较小，但在速度分量标准差为0.20 m/s～0.40 m/s、0.72 m/s～1.11 m/s区间时，其RMS略差。

综合比较，本文算法既有传统卡尔曼滤波的稳健性，又有速度约束滤波的优点，虽然在速度分量误差标准差在0.2 m/s以上时，其滤波精度偶尔会比传统卡尔曼滤波略差，但实际中的速度分量误差标准差一般低于0.2 m/s，因此，实际定位过程中采用本文算法较为合适。

3.3.2阈值

在速度分量误差标准差为0.115 5 m/s时，分析阈值d对本文算法的影响。阈值是变异系数的门限，在该门限以内则进行速度约束滤波，否则进行传统卡尔曼滤波。在不同阈值的情况下，本文算法的位置误差标准差SD和均方根RMS如图5所示。

图5　阈值对滤波精度影响图

分析图5，本文算法的滤波精度随阈值的增加有先好再差的现象，呈凹形趋势，其原因是：阈值越小，对速度观测值的要求越严格，过于严格则会剔除原本能用来约束滤波的速度观测值，导致滤波精度变差，阈值为0时本文算法就相当于传统卡尔曼滤波；而阈值越大，对速度观测值要求越松，当要求过松时，会导致原本不能用来约束滤波的速度观测值却能满足要求，故滤波精度也会变差，阈值无穷大时本文算法相当于速度约束滤波。因此，选取合适的阈值是关键，本实验中合适的阈值区间是20%～60%。

4　总结

本文算法是基于速度观测值的自适应约束滤波，根据速度观测值和定速误差的统计特性建立变异系数，以变异系数与阈值作比较，实时判断速度观测值是否满足约束滤波条件，从而在传统卡尔曼滤波和速度约束滤波中进行自适应选择，达到合理利用速度先验信息提高定位精度的目的。

实验证明，本文算法不仅克服了速度约束滤波易受定速误差影响的缺点，也比传统卡尔曼滤波提高了滤波精度，既有传统卡尔曼滤波的稳健性，又有速度约束滤波的优点，是两者的有效结合，相比之下其定位精度提高的范围在cm～m。

［1］Rousseeuw P J，Driessen K V.A Fast Algo Rithm for the Minimum Covariance Deter Minant Estimator［J］.Technometrics，1999，41（3）：212-223.

［2］杨元喜，任夏，许艳.自适应抗差滤波理论及应用的主要进展［J］.导航定位学报，2013，1（1）：9-15.

［3］杨元喜，曾安敏，景一帆.函数模型和随机模型双约束的GNSS数据融合及其性质［J］.武汉大学学报（信息科学版），2014，39（2）：127-128.

［4］严浙平，郝悦，王千一，等.基于变维卡尔曼滤波的UUV目标运动优化估计方法［J］.传感技术学报，2014，27（12）：1637-1642.

［5］蔡斌，梁甸农，董臻，等.基于先验信息的分布式星载单基线SAR/ATI方法［J］.国防科学技术大学学报，2008，30（4）：76-81.

［6］丁海强，齐光快，庄华亮，等.基于负约束条件下最大似然估计的无线传感网络定位算法［J］.传感技术学报，2014，27（11）：1545-1550.

［7］任晓栋，冯祖仁，苏承平.利用姿态约束的并联机器人运动学标定方法［J］.西安交通大学学报，2008，42（12）：1445-1449.

［8］苑艳华，李四海，南江.基于卡尔曼滤波器的航姿系统测姿算法研究［J］.传感技术学报，2011，24（12）：1718-1722.

［9］左峻疆，鲍远律，刘艳.一种带约束条件的GPS动态滤波新方法［J］.计算机仿真，2004，21（12）：80-83.

［10］左峻疆.带约束条件的GPS定位滤波方法研究［D］.合肥：中国科学技术大学，2005：41-42.

［11］付强文，秦永元，李四海.速度约束辅助车载捷联惯导系统零速校正算法［J］.系统工程与电子技术，2013，35（8）：1723-1728.

［12］伍小洁，房建成.一种带速度观测量的GPS动态定位自适应卡尔曼滤波新算法［J］.中国惯性技术学报，2000，8（3）：28-32.

［13］刘友文，刘经南，朱敦尧.附有道路信息约束的自适应卡尔曼滤波在车载导航中的应用［J］.武汉大学学报（信息科学版），2008，33（8）：828-830.

［14］Alouani A T，Blair W D.Use of a Kinematic Constraint in Track-ing Constant Speed，Ma Neuvering Targets［J］.IEEE Transactions on Automatic Control，1993，38（7）：1107-1111.

［15］Wen W，Durrant H F.Model Based Multi Sensor Data Fusion［C］// IEEE International Conference on Robotics and Automation，1992：1720-1726.

［16］Simon D，Chia T L.Kalman Filtering with State Equality Constraints［J］.IEEE Transactions on Aerospace and Electronic Systems，2002，38（1）：128-136.

［17］谢钢.GPS原理与接收机设计［M］.北京：电子工业出版社，2009：120-121.

［18］董满才，芮筱亭.射弹落点变异系数研究［J］.弹道学报，2005，17（4）：27-31.

［19］那晓琳，张宏绪.白细胞过氧化物传感器的研制［J］.传感技术学报，1995，8（2）：39-43.

［20］高连华，孙伟.变异系数在可靠性中的应用［J］.装甲兵工程学院学报，2004，18（4）：5-8.

［21］吴媚，顾赛赛.变异系数的统计推断及其应用［J］.铜仁学院学报，2010，12（1）：139-141.

周承松（1991-），男，江西吉安人，武汉大学本科毕业，现为国防科大研究生，从事卫星导航、控制理论、滤波相关工作，1025158324@qq.com；

王飞雪（1971-），男，福建长汀人，IEEE高级会员，教授，博士生导师，担任某国防项目的副总工程师，第9届全国青联委员，主要从事卫星导航领域的研究，whunudter@whu.edu.cn。

A Kalman Filtering Method with Adaptive Constraints of Velocity Information*

ZHOU Chengsong，PENG Jing，LIU Wenxiang，WANG Feixue*
（Electronic Science and Engineering School，National University of Defense Technology，Changsha 410073，China）

In dynamic positioning，the positioning accuracy could be improved through the constraints of priori information，but the Kalman filtering with constraints of priori velocity information diverges easily owing to velocity measure errors.Thus this paper puts forward a new Kalman filtering method based on adaptive constraints of priori velocity information.It takes the velocity observations and the statistical characteristics of velocity measure errors to build the coefficient of variation，and adaptively determines whether the current velocity observations reach the requirements of restraining the filtering or not，then chooses the appropriate algorithm from the traditional Kalman filtering without constraints and the Kalman filtering with constraints of priori velocity information.The experiment results show that，the positioning accuracy of the new method is centimeter to meter better than the traditional Kalman filtering and the Kalman filtering with constraints of priori velocity information.It not only overcomes the shortcoming of the Kalman filtering with constraints of priori velocity information which is affected easily by velocity measure errors，but also improves the positioning accuracy by the appropriate velocity observations.

Kalman filtering；adaptive velocity constraints；dynamic positioning；coefficient of variation；priori information

P228.42

A

1004-1699（2016）08-1180-06

EEACC：2575；7230；763010.3969/j.issn.1004-1699.2016.08.010

项目来源：国家自然科学基金项目（61403413）

2016-04-11修改日期：2016-06-05