一种改进的自适应卡尔曼滤波算法

许亚朝，何秋生，王少江，成 熊

（太原科技大学电子信息工程学院，太原030024）



一种改进的自适应卡尔曼滤波算法

许亚朝，何秋生，王少江，成 熊

（太原科技大学电子信息工程学院，太原030024）

摘 要：针对多传感器数据融合时传统卡尔曼滤波算法极易引起滤波发散，降低滤波精度和系统实时性的问题，研究一种改进的自适应滤波算法对多传感器数据进行融合，得到更为准确的信息数据。该算法在简化的Sage - Husa滤波基础上引入滤波收敛性判据，抑制滤波发散并提高滤波精度和稳定性。同时结合强跟踪滤波思想调整增益矩阵，使滤波器具有强跟踪滤波的特性，提高改进的滤波算法对不确定系统模型的鲁棒性以及对突变状态的滤波处理能力。将改进算法与传统卡尔曼滤波算法进行仿真比较。仿真结果表明，在系统模型参数失配或实变噪声未知情况下，改进的自适应滤波算法有更好的鲁棒性，并且在系统状态突变时仍有较好的滤波效果，明显提高了滤波精度和实时性。

关键词：卡尔曼滤波；Sage - Husa自适应滤波；强跟踪滤波

两轮自平衡车系统是高度不稳定的非线性系统，在保持车体平衡过程中，控制器通过传感器检测到的姿态角度信号控制电机产生控制力矩来控制两个车轮的速度和方向，以此控制车体平衡。因此，两轮自平衡车的稳定性很大程度上取决于姿态检测的准确性，然而实际应用中加速度计动态过程采集数据误差大，陀螺仪易受温度和噪声干扰导致漂移，单独使用单一传感器误差较大，使测量数据不准确。目前大多采用多个传感器测量并将测量数据进行融合的方法进行平衡车最优姿态角的估计［1］。在传感器数据融合中，标准卡尔曼滤波算法要求准确的系统模型和噪声统计特性，在实际应用中很难达到，最终可能导致滤波发散。Sage-Husa自适应卡尔曼滤波算法可以实时估计噪声统计特性，但会降低滤波实时性，增加算法的复杂程度，实际应用中效果并不理想。文献［2］提出一种对Sage-Husa自适应卡尔曼滤波算法的改进方法，但是其公式复杂，计算量大，不易在实际应用中实现。文献［3］提出一种简化的Sage-Husa算法，只对对滤波影响大的测量噪声协方差阵进行在线估计，但每个采样周期都对进行估计，未能降低算法的复杂度，无法保证滤波的实时性。文献［4］结合协方差匹配技术对Sage-Husa算法进行了改进，有效防止了滤波发散并提高了实时性，但由于此方法对系统模型不确定性的鲁棒性差，并且在系统稳定后对突变状态失去跟踪能力所以在实际应用中效果并非特别理想。

本文设计一种改进的滤波算法。对传统Kalman滤波算法进行优化改进，在简化的Sage-Husa自适应滤波算法基础上，引入滤波收敛性判据和强跟踪滤波思想，提高滤波收敛性和稳定性。在载有陀螺仪和加速度计惯性测量单元的两轮自平衡小车上采集实验数据，并在Matlab环境下对改进的滤波算法进行仿真测试。

1 基于改进的自适应卡尔曼滤波算法的数据融合

1.1 时变线性离散系统模型的建立

采用改进的卡尔曼滤波对加速度计和陀螺仪采集的数据进行融合，以陀螺仪测量角速度作为输入量，加速度计测量值作为观测量，通过滤波算法的不断迭代准确地估计平衡车的实时姿态角度。

首先建立系统的状态方程和测量方程，设线性离散系统数学模型如下：

状态方程：

测量方程：

式（1），（2）中，为k时刻系统的状态向量，U（k）是k时刻输入信号，Z（k）为k时刻的测量向量，A，B，H分别为系统状态转移矩阵、输入控制矩阵和测量矩阵，v（k）和W（k）分别是测量噪声和过程噪声。

在两轮平衡车姿态检测系统中，车体的角度和角速度成导数关系，将车体角度θ作为状态变量，将陀螺仪的常值偏差作为另一状态变量。陀螺仪倾角测量方程：

式（3）中，θ（k）为车体角度，ω（k）是陀螺仪测得的角速度，v′（k）是陀螺仪测量噪声，b（k）是陀螺仪常值偏差，dt是测量周期。由式（1）～式（3）可得：

1.2 基于Sage-Husa改进的自适应滤波算法的设计

基于Sage-Husa改进的自适应卡尔曼滤波算法为：

①状态预测：

②新息序列更新：

③状态预测协方差更新：

通过次优渐消因子λ（k），实时调整状态预测协方差阵和增益阵［5］，对历史数据进行渐消，降低旧数据对当前滤波值的影响程度，使其具备强跟踪滤波的优良特性。

④测量噪声协方差估计：

在Sage-Husa滤波算法中，测量噪声协方差估计为：

式（9）中存在负因子，可能会使R（k）的估计值逐渐失去正定性，使滤波增益大于1，最终导致滤波发散。为预防测量噪声估计失去正定性，修正为式（8）进行改进，保证当前估计值R（k）正定就可以保证滤波增益值小于1，抑制了滤波发散的可能性。

⑤滤波增益矩阵更新：

⑥姿态角状态估计方程：

⑦状态估计协方差：由式（10）和（12）可看出滤波增益和系统状态估计协方差与测量噪声协方差R（k）有关，R（k）对滤波影响更大，而Q（k）对滤波影响较小。所以本文基于简化的Sage-Husa算法，仅对测量噪声协方差R（k）进行估计，以减小计算量，提高实时性。

⑧滤波收敛性判据：

在滤波过程中，由滤波收敛性判据判断当前滤波是否收敛，若满足条件说明当前滤波收敛，不需要更新R（k），令；若不满足则说明此时系统模型已不适应当前状态，则需要由式（8）估计R（k）的新值。dk=（1 0＜b＜1是遗忘因子。

⑨系统噪声估计：

系统噪声主要由陀螺仪内部机理决定，噪声参数相对稳定。只需如式（14）所示估计系统的噪声协方差，式（14）中q-acce和q-gyro是加速度计和陀螺仪测量值的协方差，表示滤波器对传感器的信任程度，本文通过实验不断修正其取值大小，以取得较好的滤波效果。

⑪自适应渐消因子：

式（17）中β≥1是为使状态估计值更平滑引入的一个弱化因子。式（19）中ρ为遗忘因子［6］。

当模型不确定时Kalman滤波的鲁棒性较差，使滤波过程中状态估计不准。而且，在系统达到平稳状态时，对突变状态的滤波效果无法满足要求［7］。系统稳定时，增益矩阵K（k）趋于极小值，若系统状态发生突变，预报残差V（k）增大，由式（10）可知，增益矩阵K（k）不会随V（k）增大而增大，此时滤波器不能对突变状态进行跟踪处理。为克服这一缺陷，通过实时调整状态预测协方差和相应的增益阵，使滤波器具有强跟踪滤波的特性，对简化的Sage-Husa算法进行改进。

强跟踪滤波器在线选择一个适当的滤波增益阵使：

即不同时刻的残差序列正交。正交性原理表示在模型参数失配情况下，需在线调整增益矩阵K（k），使预报残差时刻具有类似于高斯白噪声的性质［8-9］。V0为残差的协方差。

将式（10）带入（20）中得到：

即：

将式（7）带入（22）中，简化得：

由（23）有（17），（18）可求解λ（k）.

2 仿真分析

仿真数据在载有陀螺仪和加速度计惯性测量单元的两轮自平衡小车上采集的。将实验测量数据导入到Matlab中对改进的滤波算法进行仿真测试，设置参数初始值如下表。

表1 滤波算法初始参数设置Tab.1 Filter initial parameters settings

图1是车体直立静止状态下应用改进后的自适应滤波算法得到的姿态角度输出曲线。车体直立静止时，加速度计测得的零位倾角值存在很大干扰噪声。应用改进的滤波方法后输出倾角曲线更平滑，滤除了原始输出数据的干扰噪声，使倾角误差明显减小，控制在0.5°之内。滤波处理之后的数据经控制单元解算输出，控制电机输出转矩，使车体稳定性更好。经滤波处理之后的输出倾角比原始输出稳定而有效，可满足静止状态下车体姿态检测的精度，说明了改进后的滤波方法在应用于平衡车姿态检测的可行性。

图2是在姿态检测过程中人为加入突变数据，模拟小车在行走过程中由于噪声干扰引起的抖动。由图可以看出在2 s和4 s处的数据发生突变，加速度计测量数据发生较大变化，而经滤波处理的输出曲线很好的滤除由于突变数据引起的输出震荡，更好地保证了平衡车在行驶过程的稳定性。

图1 车身直立静止时应用改进滤波算法后的倾角输出曲线Fig.1 The improved filtering algorithm output with the car in vertically stationary

图2 动态条件下改进的滤波算法倾角输出曲线Fig.2 The improved filtering algorithm output under the dynamic condition

图3是采用卡尔曼滤波方法对测量数据处理后的倾角曲线。标准卡尔曼滤波要求准确的系统模型和噪声统计特性，当系统模型不准确或者噪声统计特性发生改变时，滤波效果就会变差。在实际情况下模型和噪声统计特性并不能保证非常精确，可能导致滤波发散，这种方法在实际应用中局限性很大。当改变测量噪声的协方差，滤波后的输出倾角误差较大，并可能引起滤波发散，如图4所示。

如图5所示，Sage-Husa滤波算法的滤波效果较好，但是Sage-Husa滤波对初值选取要求高，易引起滤波发散。并且由于其需要不断估计过程噪声和测量噪声的协方差阵，增加了计算量，容易积累过程误差，在实际应用中长时间运行会降低姿态角度检测的精度和实时性。当测量噪声和系统噪声协方差初值选取不当，或实际运行中对时变噪声的估计R（k）取负时，而使滤波增益大于1，导致滤波发散，如图6所示，滤波发散使姿态角度输出远离真实值。

图3 系统模型和噪声统计特性已知时卡尔曼滤波倾角输出Fig.3 Kalman filter output with system model and the known noise statistical characteristics

图4 系统模型和噪声统计特性变化后卡尔曼滤波倾角输出Fig.4 Kalman filter output with system model and the changing noise statistical properties

图5 Sage-Husa滤波收敛时倾角输出Fig.5 Sage-Husa filtering output with convergence

图6 Sage-Husa滤波发散时倾角输出Fig.6 Sage-Husa filtering output with divergence

如图7，系统运行过程中，在2至3 s内突然改变加速度计测量倾角值，模拟实际情况中由路况影响使车体角度突然改变。改进后的滤波算法在车身角度突然改变时能比较准确的实时跟踪车体实际角度，而其他方法瞬间失去跟踪能力，导致姿态角度估计不准，最终使控制器得不到正确的倾角信息而使车身不稳甚至倾倒。

综上仿真结果表明，改进的滤波算法对加速度计的零位偏差有较好的修正能力，能够明显抑制动态过程中的干扰噪声。改进滤波方法具有较强收敛性，并且在系统模型和噪声统计特性不精确情况下具有较强鲁棒性，同时在车身角度发生突变时能比较快速准确的实时跟踪车体实际角度。

图7 突变状态下改进的滤波算法与其他滤波算法输出比较Fig.7 Output comparison between improved filtering algorithm and others in mutation condition

3 结论

本文针对传统卡尔曼滤波算法在对多传感器数据融合过程中的局限性，改进了一种自适应强跟踪卡尔曼滤波算法。文中分析并比较了卡尔曼滤波算法和Sage-Husa滤波算法的不足和相同条件下改进滤波方法的优越性。仿真结果表明，改进的滤波算法能有效的修正加速度计零位偏差，并能明显抑制动态过程中的噪声干扰实现平滑滤波，在状态突变时能快速准确的实时跟踪车体实际角度，比传统卡尔曼类滤波方法有较强的稳定性和鲁棒性。

参考文献：

［1］ 杨凌霄，李晓阳.基于卡尔曼滤波的两轮自平衡车姿态检测方法［J］.计算机仿真，2014（31）：406-409.

［2］ 周晶，黄显高.一种改进的Sage-Husa自适应滤波算法［J］.仪器仪表用户，2009（16）：71-73.

［3］ 沈云峰，朱海，莫军.简化的Sage-Husa自适应滤波算法在组合导航中的应用及仿真［J］.青岛大学学报，2001（16）：44-47.

［4］ 鲁平，赵龙，陈哲.改进的Sage-Husa自适应滤波及其应用［J］.系统仿真学报，2007，19（15）：3503-3505.

［5］ 宋康宁，丛爽，邓科，等.自适应强跟踪卡尔曼滤波在陀螺稳定平台中的应用［J］.中国科学技术大学学报，2015（7）：401-406.

［6］ 张欣，白越，赵常均，等.多旋翼姿态解算中的改进自适应扩展Kalman算法［J］.光学精密工程，2014，22（12）：3384-3390.

［7］ 郭泽，缪玲娟，赵洪松.一种改进的强跟踪UKF算法及其在SINS大方位失准角初始对准中的应用［J］.航空学报，2014，35（1）：203-214.

［8］ ZHANG Z T，ZHANG J S.A novel strong tracking finite-difference extended Kalman filter for nonlinear eye tracking［J］.Science in China，2009，52（4）：688-694.

［9］ 范文兵，刘春风，张素贞.一种强跟踪扩展卡尔曼滤波器的改进算法［J］.控制与决策，2006，21（1）：73-76.

（School of Electronic and Information Engineering，Taiyuan University of Science & Technology，Taiyuan 030024，China）

An Improved Adaptive Kalman Filtering Algorithm

XU Ya-zhao，HE Qiu-sheng，WANG Shao-jiang，CHENG Xiong

Key words：kalman filter，sage-husa adaptive filter，strong tracking filter

Abstract：Traditional Kalman filtering algorithm easily leads filter to diverge and reduces the filtering accuracy and system real-time performance when multi-sensor is mixed with data.This paper puts forward an improved adaptive filtering algorithm for multi-sensor mixing data，which gets more accurate information data.The algorithm restrains filtering divergence and improves filtering accuracy and stability with introducing filtering convergence criterion to the simplified Sage-Husa filter.And the algorithm adjusts gain matrix with strong tracking filter，thus making the filter have strong tracking performance and improving the filter′s robustness for uncertain system model and processing capacity for mutation status.Simulation comparison between the improved algorithm and the traditional Kalman filtering algorithm shows that the improved adaptive filtering algorithm has better robustness and filtering effectiveness with system model parameters mismatched and the real variable noise unknown.And it also shows that the filtering accuracy and real-time performance have been improved obviously.

中图分类号：TP212.9

文献标志码：A

doi：10.3969/ j.issn.1673 -2057.2016.03.001

文章编号：1673 -2057（2016）03 -0163 -06

收稿日期：2015-08-22

基金项目：山西省自然科学基金（2013011035-2）

作者简介：许亚朝（1990 -），男，硕士研究生，主要研究方向为智能控制理论研究。