Kalman滤波定位-盘古文库

Kalman滤波定位

来源：火烈鸟

作者：开心麻花

2025-09-18

Kalman滤波定位（精选9篇）

Kalman滤波定位第1篇

国内矿山安全生产要求越来越高,加之井下巷道纵横交错,作业面广,造成井下人员分布分散,实施通信不便。因此,为了保证井下人员安全,建立一套完善的煤矿井下人员安全精确定位系统具有重大的现实意义 [1,2]。目前常用的定位算法主要是基于测距的RSSI(接收信号强度)定位算法。在该定位算法基础上,一般采用最大似然法对接收到的RSSI值进行滤波,滤波结果最终可以看作是对接收到的同一个参考节点的一组RSSI值进行均值滤波。但是定位节点每移动一次,均需要重新对接收到的RSSI值进行均值滤波,滤波不具备连续性、递推性和自适应性。而Kalman滤波则解决了这一问题。Kalman滤波作为一种最优估计理论和方法,由于其具有实时递推、存储量小和简单易行的特点,在工程和应用中受到了重视[3]。本文简单介绍了井下人员定位系统的结构,主要研究基于RSSI的定位算法以及应用Kalman滤波对估算出的定位信息进行优化的问题。

1 井下人员定位系统架构

本文针对基于WiFi的井下定位技术进行研究,整个井下人员定位系统架构如图1所示[4]。在井下需要进行人员跟踪的区域和巷道中布置若干WiFi基站(如图1中的基站1基站n),下井人员携带含WiFi模块的定位终端。当人员进入WiFi基站工作区域时,定位终端根据接收到的基站位置信息和RSSI值,通过经验模型求取定位节点和各个参考节点之间的距离,运用全交点质心定位算法估算出定位节点的位置。以估算出的定位信息为观测值,考虑井下存在的各种干扰噪声,建立Kalman滤波模型,进一步提高井下人员定位的精度,从而得到系统的最优估计。

2 基于RSSI的井下定位算法

2.1 经验模型的建立

设定位节点接收到来自N个参考节点的坐标值与RSSI值,考虑到井下环境、成本、定位精度等因素,井下人员定位系统的经验模型可简化为[5]

$R S S Ι = - (10 n l g d + R) (1)$

式中:n为路径损耗系数;d为定位节点与参考节点间的距离; R为定位节点与参考节点间距离d为1 m时测得的RSSI值。

2.2 全交点质心定位算法

目前,用于井下人员定位的方法一般选用三边定位法[6]。分别以已知位置的3个基站AP为圆心,以3个基站到待测人员距离为半径作圆,如图2所示。

分别计算出定位节点与参考节点之间的距离dn,再用对应的坐标值与距离值列如下方程组:

${\begin{cases} (x - a_{1})^{2} + (y - b_{1})^{2} = d_{1}^{2} \\ (x - a_{2})^{2} + (y - b_{2})^{2} = d_{2}^{2} \\ ⋮ \\ (x - a_{n})^{2} + (y - b_{n})^{2} = d_{n}^{2} \end{cases} (2)$

在实际应用中,由于测量误差的存在,可能会导致方程组(2)无解,因此,运用全交点质心法来作为本文的定位算法,将方程组(2)转换成方程组(3)。

${\begin{cases} - 2 a_{1} x - 2 b_{1} y + x^{2} + y^{2} = d_{1}^{2} - a_{1}^{2} - b_{1}^{2} \\ - 2 a_{2} x - 2 b_{2} y + x^{2} + y^{2} = d_{2}^{2} - a_{2}^{2} - b_{2}^{2} \\ ⋮ \\ - 2 a_{n} x - 2 b_{n} y + x^{2} + y^{2} = d_{n}^{2} - a_{n}^{2} - b_{n}^{2} \end{cases} (3)$

即

$[\begin{matrix} - 2 a_{1} & - 2 b_{1} & 1 \\ - 2 a_{2} & - 2 b_{2} & 1 \\ ⋮ \\ - 2 a_{n} & - 2 b_{n} & 1 \end{matrix}] [\begin{matrix} x \\ y \\ x^{2} + y^{2} \end{matrix}] = [\begin{matrix} d_{1}^{2} - a_{1}^{2} - b_{1}^{2} \\ d_{2}^{2} - a_{2}^{2} - b_{2}^{2} \\ ⋮ \\ d_{n}^{2} - a_{n}^{2} - b_{n}^{2} \end{matrix}] (4)$

定义:

$\begin{array}{l} Q = [\begin{matrix} - 2 a_{1} & - 2 b_{1} & 1 \\ - 2 a_{2} & - 2 b_{2} & 1 \\ ⋮ \\ - 2 a_{n} & - 2 b_{n} & 1 \end{matrix}] ‚ θ = [\begin{matrix} x & y & x^{2} + y^{2} \end{matrix}]^{Τ} \\ b = [\begin{matrix} d_{1}^{2} - a_{1}^{2} - b_{1}^{2} & d_{2}^{2} - a_{2}^{2} - b_{2}^{2} & \dots & d_{n}^{2} - a_{n}^{2} - b_{n}^{2} \end{matrix}]^{Τ} \end{array}$

则式(4)可以写为

$Q θ = b (5)$

用最小二乘法求解式(5),得

$\hat{θ}_{L S} = (Q^{Τ} Q)^{- 1} Q^{Τ} b (6)$

向量 $\hat{θ}_{L S}$ 的前两项即为所求定位节点的估计坐标 $(\hat{x}_{L S}, \hat{y}_{L S})$ 。

3 Kalman滤波模型的建立

基于RSSI定位的重点是距离的测量,而RSSI测距容易受到干扰噪声的影响,已不能满足井下人员定位的精度要求。Kalman滤波是高斯过程最优滤波的一种有效算法[7,8],当对象模型足够精确且系统状态和参数不发生突变时,性能较好,因此,将Kalman滤波与RSSI定位算法相结合,利用Kalman滤波功能减小噪声对系统的影响,达到提高定位精度的目的。

3.1 算法描述

不考虑控制作用,设随机线性离散系统的方程为[9]

${\begin{cases} X_{k} = Φ_{k, k - 1} X_{k - 1} + Γ_{k, k - 1} W_{k - 1} \\ Ζ_{k} = Η_{k} X_{k} + V_{k} \end{cases} (7)$

式中:Xk为k时刻的n维状态矢量; Φk,k-1为k-1到k时刻的系统一步转移矩阵;Γk,k-1为系统噪声矩阵;Wk-1为k-1时刻的系统噪声;Zk为k时刻的m维观测矢量;Hk为k时刻的观测矩阵; Vk为k时刻的m维量测噪声。

关于系统过程噪声和观测噪声的统计特性,假定如下:

${\begin{cases} E [W_{k}] = 0, E [W_{k} W_{j}^{Τ}] = Q_{k} δ_{k j} \\ E [V_{k}] = 0, E [V_{k} V_{j}^{Τ}] = R_{k} δ_{k j} \\ E [W_{k} V_{j}^{Τ}] = 0 \end{cases} (8)$

式中:Qk是系统过程噪声Wk的对称非负定方差矩阵;Rk是系统观测噪声Vk的对称正定方差矩阵;δkj是狄拉克(Dirac)δ函数。

3.2 Kalman滤波模型的建立

本文运用Kalman滤波法对井下人员进行跟踪,井下人员的运动可以近似于加速运动,加速度由系统高斯白噪声提供。假设人员的加速度为a,并以v和s分别表示人员的速度和位置,则人员运动方程如下:

${\begin{cases} s_{t + 1} = s_{t} + Τ v_{t} + (Τ^{2} / 2) a_{k} \\ v_{t + 1} = v_{t} + Τ a_{t} (9) \\ a_{t + 1} = w_{t} \end{cases}$

式中:wt为t时刻井下人员运动时受到的扰动噪声;T为人员运动的时间。

3.2.1 构造状态方程

假设在t+1时刻井下人员在x、y轴上的坐标和速度分别为x(t+1)、y(t+1)、 $\dot{x} (t + 1)$ 和 $\dot{y} (t + 1)$ 。wx(t+1)、wy(t+1)是均值为零、方差为σ $_{v}^{2}$ 的平稳高斯噪声w(t+1)在t+1时刻的2个正交分量,它们相互独立,该噪声分别对x、y轴提供随机运动加速度。T为时刻t+1和时刻t的时间间隔。由式(9)可得出井下人员在t+1时刻的坐标位置方程为

${\begin{cases} x (t + 1) = x (t) + Τ \dot{x} (t) + \frac{1}{2} w_{x} (t) Τ^{2} \\ \dot{x} (t + 1) = \dot{x} (t) + Τ w_{x} (t) \\ y (t + 1) = y (t) + Τ \dot{y} (t) + \frac{1}{2} w_{y} (t) Τ^{2} \\ \dot{y} (t + 1) = \dot{y} (t) + Τ w_{y} (t) \end{cases} (10)$

对式(10)合并可得状态方程:

$\begin{array}{l} [\begin{matrix} x (t + 1) \\ \dot{x} (t + 1) \\ y (t + 1) \\ \dot{y} (t + 1) \end{matrix}] = [\begin{matrix} 1 & Τ & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & Τ \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x (t) \\ \dot{x} (t) \\ y (t) \\ \dot{y} (t) \end{matrix}] + \\ [\begin{matrix} Τ^{2} / 2 \\ Τ \\ 0 \\ 0 \end{matrix} \begin{matrix} 0 \\ 0 \\ Τ^{2} / 2 \\ Τ \end{matrix}] [\begin{matrix} w_{x} (t) \\ w_{y} (t) \end{matrix}] (11) \end{array}$

即

3.2.2 构造观测方程

设在t+1时刻观测到的井下人员坐标为(xZ(t+1), yZ(t+1));vx(t+1) 和vy(t+1) 是均值为零、方差为σ $_{w}^{2}$ 的高斯白噪声v(t+1)在t+1时刻的2个正交分量,它们相互独立。则t+1时刻井下人员的观测坐标位置方程为

${\begin{cases} Ζ (t + 1) = [\begin{matrix} x_{Ζ} (t + 1) & y_{Ζ} (t + 1) \end{matrix}]^{Τ} \\ x_{Ζ} (t + 1) = x (t + 1) + v_{x} (t + 1) \\ y_{Ζ} (t + 1) = y (t + 1) + v_{y} (t + 1) \end{cases} (13)$

对式(13)进行合并可得观测方程:

$\begin{array}{l} [\begin{matrix} x_{Ζ} (t + 1) \\ y_{Ζ} (t + 1) \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} x (t + 1) \\ \dot{x} (t + 1) \\ y (t + 1) \\ \dot{y} (t + 1) \end{matrix}] + \\ [\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}] [\begin{matrix} v_{x} (t + 1) \\ v_{y} (t + 1) \end{matrix}] (14) \end{array}$

即

3.3 算法流程

Kalman滤波算法流程如图3所示,从Kalman滤波使用系统状态信息和观测信息的先后次序来看,在一个滤波周期内,可以将Kalman滤波分成时间更新和观测更新2个过程。只要给定初值 $\hat{X}_{0}$ 和P0,根据k时刻的观测值Zk,就可以递推计算得到k时刻的状态估计 $\hat{X}_{k} (k = 1, 2, \dots)$ 。

4 仿真与分析

为了检验Kalman滤波在井下人员跟踪定位中的应用效果,对井下人员的状态进行滤波,具体的仿真结果如图4图7所示。假设井下人员的初始位置坐标为(0,0),x、y轴初始速度分别为V0=2 m/s,Vy=1 m/s,加速度由高斯白噪声随机提供,观测时间间隔T=0.1 s,观测噪声方差为10,状态方程的方差为3。

从图4和图6可以明显地看出,Kalman滤波对井下人员的实时跟踪精度比较高,实时性较好。从

图5和图7可看出,基于Kalman滤波的速度估计不仅可以很好地估计出井下人员在某一时刻的运动速度,并且可以跟踪物体的整个运动过程的速度,可以对定位节点下一时刻的速度进行预测,跟踪精度也比较高。

5 结语

通过建立井下人员的状态方程和观测方程,实现了对基于RSSI定位算法估算出的井下人员位置坐标的滤波处理。通过仿真可以看出,Kalman滤波算法对人员的实时跟踪效果比较精确,提高了系统的实时性和跟踪精度。在运用Kalman滤波算法时,模型建立的好坏对于精度的提高起到关键作用。在实际应用中,要多观察环境因素、多研究定位节点移动的规律等,以便建立尽可能与现实情况相匹配的模型。

Kalman滤波定位第2篇

基于集合Kalman滤波数据同化的热带气旋路径集合预报研究

构建了一个基于集合Kalman滤波数据同化的热带气旋集合预报系统, 通过积云参数化方案和边界层参数化方案的9个不同组合, 采用MM5模式进行了不同时间的`短时预报.对预报结果使用 “镜像法” 得到18个初始成员, 为同化提供初始背景集合.将人造台风作为观测场, 同化后的结果作为集合预报的初值, 通过不同参数组合的MM5模式进行集合预报.对～16个台风个例的分析表明, 初始成员产生方法能够对热带气旋的要素场、中心强度和位置进行合理扰动.同化结果使台风强度得到加强, 结构更接近实际.基于同化的集合路径预报结果要优于未同化的集合预报.使用 “镜像法” 增加集合成员提高了预报准确度, 路径预报误差在48小时和72小时分别低于200 km和250 km.

作者：黄小刚费建芳陆汉城 HUANG Xiao-Gang FEI Jian-Fang LU Han-Cheng 作者单位：解放军理工大学气象学院军事气象系,南京,211101刊名：大气科学 ISTIC PKU英文刊名：CHINESE JOURNAL OF ATMOSPHERIC SCIENCES年，卷(期)：200731(3)分类号：P444关键词：热带气旋集合预报集合kalman滤波数据同化路径预报

Kalman滤波定位第3篇

船舶在深海中的锚泊方式具有很大的限制性, 故研究不借助锚泊的船舶动力定位系统成为船舶动力研究的一个重要课题。船舶动力定位系统在远洋运输中具有较为重要的地位, 它是指船舶在不借助传统的锚泊系统就能抵抗风、浪、流的干扰, 可以使得船舶能够保持在一定位置和方向上。船舶在海洋上的运动主要由风、流、二阶波漂力、推力器造成的低频运动和一阶波浪力造成的高频运动。高频运动仅表现为周期性的振荡而不会导致位置的变动, 为了提高推力器能量的利用效率, 从船舶的位置信号中分离出低频信号进行控制, 如何进行低频信号分离就需要精准的滤波技术, 所以, 滤波在动力系统中就非常重要, 如果滤波不能完全把高频信号滤除, 那么低频信号中掺杂部分未滤除的高频信号, 就会浪费不必要的推力器能量, 使船舶定位精度大打折扣。

2 船舶动力定位系统简介

船舶动力定位系统一般由传感器、控制器和推力器三大系统组成。传感器负责对船舶位置和艏向值进行监测, 与系统设定值进行比较处理后给控制器, 控制器把传感器的船舶位置和艏向角的偏差数据进行运算后输出控制指令, 推力器把控制指令通过推力分配后, 传送到特定的推力器产生精准的力和力矩, 使船舶定位在要求的位置和艏向上, 完成船舶的动力定位。

3 中心微分滤波器设计

船舶动力系统滤波器一般常用Kalman滤波器、时变滤波器、一阶滤波器等, 这些滤波器简单实用, 可以消除大部分波浪干扰, 但是还具有缺陷。标准Kalman滤波是一种高效的递归滤波器, 它能从外界环境噪声和测量噪声中, 估计出系统的状态。

由于标准Kalman滤波解决的是线性问题, 对模型的精确性要求高, 环境噪声必须为白噪声, 而船舶动力系统是一个非线性模型, 船舶在海洋上的风、浪、流的干扰和靶管与海底的摩擦, 艏喷时的反推力都是非线性, 而标准的Kalman滤波不能完全解决这些问题。

在本文中, 我们采用一种改进的Kalman滤波, 采用中心微分Kalman滤波器来进行船舶动力系统过滤高频信号。通过一个确定性的采样方式来解决海洋上的非线性的问题, 而状态分布用高斯随机变量来近似模拟, 可以很好的来描述告诉随机变量的实际均值和方差, 船舶动力系统在海洋上的非线性传播时, 验后均值和方差的精度为二阶泰勒级数展开的结果, 比一阶泰勒级数开展的扩展Kalman精度要高不少, 获得的均值和方差精度都比较理想。

中心微分滤波算法公式如下:

Sigma点:

其中Pw为状态过程噪声协方差。

由于船舶运动方程式十五维的, 所以船舶动力系统滤波器设计需要从纵荡、横荡、艏向三个方向上分别设计滤波器。

纵荡方向上向量模型为:

其中X为横荡方向的位置信息。

横荡方向上向量模型为:

其中Y为横荡方向的位置信息。

艏向方向上向量模型为:

其中Ф为测量的艏摇方向-的艏向角度。

通过中心微分滤波公式和纵荡、横荡、艏向三个向量的模型就可以分别进行噪声过滤, 经过中心微分滤波器后, 就可以估算出船舶的纵向、横向的位置信息和艏向的角度。

经过检验, 本文设计的中心微分滤波器是比较有效的, 船舶动力系统滤波器输出的纵荡方向上的估计误差均值为0.312米, 估计误差标准差为1.471米, 横荡方向上的估计误差均值为0.301米, 估计误差标准差为1.294米, 艏向方向上的估计误差均值为0.297米, 估计误差标准差为1.431米。从纵荡、横荡、艏向三个方向的估计误差均值和标准差可以看出, 本文设计的中心微分滤波器具有较好的干扰滤除效果, 能够准确估算出船舶的低频位置信息和艏摇角度信息, 具有较高的实用性, 该算法具有一定的优越性, 具有较高的精度。

4 结语

本文主要针对船舶动力定位系统中高频信号对定位影响较小, 如何分离出高频信号进行研究, 设计中心微分Kalman滤波器, 从纵荡、横荡、艏向三个方向上进行高频噪声的分离, 从而估算出船舶低频的位置信息和艏向角度, 从而精准的通过推力器进行控制, 使得船舶定位精度大大增加, 经过验证, 该算法具有较高的鲁棒性和适应性。

摘要：船舶在深海作业中受限于锚泊方式, 如何保持准确的定位具有较高的难度。船舶的高频运动仅表现为周期性的振荡而不会导致位置的变动, 从船舶的位置信号中分离出低频信号进行控制, 本文设计的中心微分滤波器, 从纵荡、横荡、艏向三个方向分别设计滤波器进行高频信号过滤, 经过检验具有较高的准确性和精度。

关键词：船舶动力,定位系统,滤波器

参考文献

[1]何黎明.动力定位船舶的非线性观测器设计[J].上海交通大学学报, 2003 (06) .

[2]夏国清.水面舰船动力定位系统智能控制技术研究[D].哈尔滨工程大学, 2001.

[3]王宗义.船舶动力定位的数学模型和滤波方法[J].哈尔滨工程大学学报, 2002 (04) .

Kalman滤波定位第4篇

提出了以二进小波变换为基础的自适应Kalman滤波反褶积(AKFD)新方法，针对该方法的计算复杂程度，提出了一种快速实现方法. 二进小波变换的AKFD抛弃了传统预测反褶积对信号平稳性的假设，克服了提高分辨率而信噪比明显降低的问题，具有很好的抗噪性能. 在小波域进行的AKFD在压制假反射以及提高分辨率方面比时间域的AKFD好，克服了在时域内进行AKFD抬升低频成分的缺陷. 利用二维地震数据的`局部平稳性的假设提出了快速实现方法，通过分段求取自适应预测算子，分别于横向及纵向采用样条插值的方法进行插值，来减少求取自适应预测算子的计算量，达到快速实现的目的. 经过大量实验表明计算速度提高数百倍，仍能保持原来的计算效果.

作者：董恩清刘贵忠张宗平DONG En-qing LIU Gui-zhong ZHANG ZONG-PING 作者单位：西安交通大学电信学院信息与通信工程系, 刊名：地球物理学报 ISTIC SCI PKU英文刊名：Chinese Journal of Geophysics 年，卷(期)：2001 44(2) 分类号：P631 关键词：二进小波变换自适应滤波 Kalman滤波反褶积非平稳信号

★ 基于EXCEL的BP网络法在水质评价中的应用

★ KALMAN滤波技术在北斗无源动态定位中的应用研究

★ 基于RBF神经网络的EMD方法在海平面分析中的应用

★ 动量VLBP神经网络在缺陷接地电路中的应用

★ 基于自适应神经网络的边坡位移预测

★ 基于改进自适应滤波算法的控制系统传感器故障诊断

★ 自适应模糊神经网络预测阳离子标准熵的研究

★ 辐射环境监测优化布点的BP人工神经网络方法

★ 基于人工神经网络的算法及技术应用探讨

Kalman滤波定位第5篇

运动目标跟踪的研究对象是视频序列,或者说是图像序列,是指在整个视频序列中监控运动目标的时空变化,如目标的出现与消失,目标的位置、尺寸和形状等。

由于光照变化、背景干扰、阴影、摄像机的抖动以及运动目标之间遮挡等现象的存在,都给运动目标的正确跟踪带来了极大的挑战。在现实生活中,常需要对繁忙环境中的车辆、人或飞机等等进行实时的观察,以及运动图像编码,交通监测,军事领域等都需要对运动目标进行跟踪。因此就需要对视频图像序列进行处理,序列图像中运动目标的跟踪技术研究目的就是把图像处理,自动控制,信息科学等技术有机的结合起来,形成一种能够从图像信息中快速地检测出运动目标,并对提取目标的位置信息进行实时地跟踪。简单地说就是在下一幅图像中找到目标的确切位置;一种简单的跟踪方法首先要提取被跟踪目标的图像,建立一个模板;然后在下一幅图像中,全图匹配搜索目标图像,找到最佳匹配的位置;这种全图匹配搜索方法有时可以得到较为满意的跟踪结果,但在实际使用中这种全图匹配搜索方法还存在一些问题。一方面是计算复杂度高,实现费事。另一方面是由于目标在下一幅图像中可能由于旋转、光线及运动等原因,目标的图像会有所形变,所以直接利用目标图像像素数据作为匹配模板,往往存在匹配效果不理想的问题[1]。

因此,在模板匹配基础上,采用kalman滤波器原理预测出运动目标下可能出现的位置,缩小目标的搜索范围实现快速跟踪,并对运动目标速度修正,实现目标的准确跟踪。

1 模板匹配法

为了在图像中检测出已知形状的目标物,使用目标的特征模板与图像匹配,在约定的准则下检测出目标物,这种方法称之为模板匹配法。它能检测出图像中直线、曲线、图案等。还可以将一幅图像的一块与另一幅图像匹配,例如从不同的角度摄取的同一景物的两幅图像,通过匹配能识别两幅图像中表示同一目标的那些部分,由此而测得立体视差,目标物的高度和深度等[2]。同样如果取不同时间摄取的两幅图匹配,由此可以得到目标相对运动情况,如天空云层变化,军事事态变化等。如果具有不同的失真的两幅图像匹配,利用匹配可以确定他们的失真度。

2 运动目标的模板匹配

对于图像进行匹配时,利用帧间差分法判断图像中是否存在待检测目标。若图像中不存在目标,不需要进行模板匹配,可转入下一帧进行处理;反之,开始匹配。

采用模板匹配法,从图像的某一固定位置即图像的左上角像素开始进行逐点匹配。这种算法比较简单易懂,但计算量非常大,在操作过程中应用一些计算的技巧外,还可以对图像进行逐行匹配,如先粗后细采样匹配;改变模板大小;去除明显没有目标区域等措施来加快匹配速度。如图1,设模板T叠放在搜索图F上平移,将模板覆盖下的那块搜索区域称为子图Fi,j,i,j为子图Fi,j左上角的像素在F图中的坐标,称为参考点。设模板T的大小为PQ,F的大小为MN,则i, j的取值范围:0 <i<M-P+1,0<j<N-Q+1。

比较T和Fi,j的内容,若两者一致,则T和Fi,j之差为0,所以可以用以下的两种测度之一来衡量T和Fi,j的相似程度:

$D (i, j) = \sum_{m = 1}^{Ρ} \sum_{n = 1}^{Q} [F^{i, j} (m, n) - Τ (m, n)]^{2} (1)$

$D (i, j) = \sum_{m - 1}^{Ρ} \sum_{n - 1}^{Q} ■ F^{i, j} (m, n - Τ (m, n)) ■^{2} (2)$

主要算法是用提取到的模板在图像中进行全面检索,先取出与模板区域相同大小区域,用该区域与模板的匹配,即求得该区域与模板差的平方和,不断循环,直到整个图像的每个区域都被检索,求出两幅图像差为零的区域,由于大多数运动目标形状不是唯一的,因此取得差的平方和最小的点的区域则为目标所在区域。因为模板要在(M-P+1)(N-Q+1)个参考位置上进行匹配,其中除一点以外都是在非匹配点上作无用功,运算速度较慢,达不到实时检测的要求。

2.1 基于直方图的运动目标模板匹配

一般而言,基于图像边缘特征的目标跟踪算法可以取得较好的跟踪效果,但由于图像边缘特征的提取以及利用边缘特征进行模板匹配的计算量非常大,常常使其丧失了实时目标跟踪的意义;基于直方图的目标跟踪的算法最大的优势在于其模板提取与模板更新的快速性。

目标直方图记录的是灰度的出现概率,因而受目标的形状变形影响不大,所以用直方图作为目标的模式,用目标的灰度分布作为匹配的依据,对非刚性运动目标,譬如人体,能取得更好的效果[3]。

按照模板匹配的方法获得运动物体的区域大小并提取出来作为模板,按照模板匹配法中采用的方法进行搜索。先求取模板的直方图H,再求模板覆盖下那块区域搜索区域的子直方图H(Di,j),Di,j为模板覆盖下的那块搜索图[4]。比较H和H(Di,j)中的内容,若两者一致H和H(Di,j)的差为零,因此,可以用下面的测度来衡量H和H(Di,j)的相似程度。

$S (D (i, j)) = \sum_{m = 0}^{255} (Η (D_{i, j}) - Η_{m})^{2} (3)$

虽然基于直方图的运动目标模板匹配方法已经解决了图像边缘特征的提取以及利用边缘特征进行模板匹配的计算量非常大的缺点,但基于直方图的运动目标模板匹配方法的搜索范围比较大,其实时性还是不能很好的解决。

2.2 基于Kalman滤波原理的模板匹配法算法

分析以上直方图运动目标模板匹配方法存在的缺点,提出了基于Kalman滤波器的运动目标模板匹配方法,以解决搜索范围大,实时性不好的问题。

Kalman滤波器是一个线形递归滤波器,基于系统以前的状态序列对下一个状态做最优估计,预测时具有无偏、稳定和最优的特点。Kalman滤波器算法主要包含状态和观测方程,分别如下:

状态方程:xk=Φkk-1xk-1+Wk-1 (4)

观测方程:Yk=HKxK+Vk (5)

其中xk是系统状态的n1维状态向量;Yk是观测到的系统状态的m1维向量;Φkk-1是状态转移矩阵,nn维,从tk-1时刻到tk时刻;Hk是tk时刻观测矩阵,mn维;Wk-1是tk-1时刻状态的随机干扰(白噪声)的随机向量,n1维;Vk是tk时刻的观测噪声向量,m1维。此时设动态噪声Wk和观测噪声Vk为两两互不相关的零均值的正态白噪声序列。若令Qk和Rk为动态噪声Wk和观测噪声Vk的协方差矩阵:

Φk=E{WkW-1k} (6)

若根据Y1,Y2,Y3,Yk去估计xk称为Kalman滤波,若根据Y1,Y2,Y3,Yk去估计xk-1则称为Kalman预测或外推。通过Kalman滤波来更新系统当前状态xk,通过Kalman预测来估计系统未来状态xk-1。

由于系统已确定,则Φkk-1和Hk已知,则Wk-1和Vk满足一定假设,也已知。设Pk是 $\hat{x}_{k}$ 的协方差阵,P′k是xk和 $\hat{x}_{k}$ 的误差协方差阵。得到Kalman滤波器算法的计算公式如下:在t0时刻,用x均值向量初始化 $\hat{x}_{0}$ ,并求P0。

在tk时刻,系统状态预测方程为:

$\hat{x}_{k} = Φ_{k \cdot k - 1} \hat{x}_{k - 1} + Κ_{k} (Y_{k} - Η_{k} Φ_{Κ \cdot Κ - 1} \hat{X}_{Κ - 1}) (7)$

其中:

P′k=Φkk-1pk-1Φ $_{k \cdot k - 1}^{Τ}$ +Qk-1 (8)

Kk=P′kH $_{k}^{Τ}$ (HkP′H $_{k}^{Τ}$ +Rk)-1 (9)

Pk=(1-KkHk)P′k (10)

Kk是增益系数矩阵。

根据Kalman滤波原理则知,对图像中运动目标跟踪时,在已知图像运动方向和速度的前提下,如果预测出运动目标在下一幅图像中的位置,则可以在下一幅图像跟踪中滤出已经出现的历史位置和不可能出现的位置,预测出运动目标可能出现的位置,再在可能出现的位置范围内进行模板匹配,则可以减小搜索的范围,同时对速度进行修正,使跟踪更加准确,减小了每帧图像的运算时间,实现目标的实时跟踪。

2.3 运算结果及分析

设计实现了在固定背景情况下,也就是在摄像机固定的情况进行的运动目标检测和跟踪,这里运用Kalman滤波原理的模板匹配进行运动跟踪,图2为连续三帧原始,图3为连续三帧图像的跟踪结果:

从以上运行结果可以看出,采用常规模板匹配法,基于直方图的模板匹配法,和基于Kalman滤波原理改进模板匹配,这三种算法均能实现序列图像中目标的跟踪。但常规模板匹配法运算最慢,运行为两分钟一帧图像。基于直方图的模板匹配法运行速度为每分钟一帧图像,运行速度提高一倍,但是由于图像与背景灰度相似,使该算法匹配结果不是很理想。基于Kalman滤波原理改进模板匹配,该算法可以将运行速度提高为二十秒一帧图像,并且跟踪更为准确。

3 结束语

对于运动目标的跟踪,由于光照变化、背景干扰、阴影、摄像机的抖动以及运动目标之间遮挡等现象的存在,都给运动目标的正确跟踪带来了极大的挑战。常规模板匹配法进行目标跟踪计算量非常大,运行速度慢,达不到实时效果。而采用基于Kalman滤波原理改进模板匹配,能准确的跟踪运动目标,并使运行速度大大提高,达到很好的实时性。

摘要：应用Kalman滤波原理,对运动目标进行跟踪,缩小目标的搜索范围,实现快速实时跟踪,使跟踪更为准确。理论分析和实验结果表明,该算法与常规的模板匹配法、直方图模板匹配法等算法相比,有效地提高了目标跟踪的速度及跟踪的准确性。该算法对运动目标进行跟踪,运行速度可提高三倍。

关键词：运动目标跟踪,模板匹配,Kalman滤波原理

参考文献

[1]Azarbayejani A,Wren C,Pentland A.Real-time3-d tracking of thehuman body[J].IEEE Transactions on Pattern Analysis and MachineIntelligence,1997,19(7):780-785.

[2]吴立德.计算机视觉[M].上海:复旦大学出版社,1993:216-254.

[3]Wren C,Azarbayejani A,Darrell T,et al.Real-time Tracking of theHuman Body[J].IEEE Trans,1997,19(7):780-785.

Kalman滤波定位第6篇

Kalman滤波的显著特点是对状态空间进行动态估计,即由参数的验前估值和新的观测数据进行状态参数的更新。所以Kalman滤波一般只需存储前一个历元的状态参数估值,无须存储所有历史观测信息,具有很高的计算效率,并可以进行实时估计。应用Kalman滤波法进行卫星运动学定轨,可以充分考虑卫星的运动信息,在确定卫星轨道的同时还可以估计出卫星的运行速度,可以提供短期的预报轨道。当某个历元缺失观测值时,几何法定轨不能提供该历元的轨道信息,而Kalman滤波法可以根据前一历元的状态和速度估算出当前历元的状态信息。

以实时定轨为目标,研究基于Kalman滤波的星载GPS定轨算法,根据GPS观测数据结合简化动力学模型对卫星状态进行最优估计。由于卫星轨道确定是一个非线性问题,可用扩展Kalman滤波进行处理。

1 简化动力学模型

人造卫星在围绕地球的运动过程中受到多种作用力的影响。总的来说,这些作用力可以分为两大类:一类为保守力,另一类是非保守力,也称发散力。保守力包括地球引力、日、月、行星对卫星的引力以及地球的潮汐现象导致的引力场的变化等。对于保守力系统可以使用“位函数”来描述这些作用力。发散力包括大气阻力、地球红外辐射以及卫星姿态控制的动力等。对于发散力则不存在“位函数”,只能直接使用这些力的表达式。在动力学定轨中,大部分保守力能够用比较精确的数学模型表示,而发散力具有较强的随机型,很难用数学模型来描述。为了补偿无法模型化或错误模型的微小摄动力的影响,常引入经验力模型。

除了主要的地球中心引力以外,其余的作用力都相对较小,可以把实际的卫星运动处理成受摄二体问题,即一个简单可积的二体问题加上摄动改正两个部分。因此在利用动力学模型进行卫星定轨时,卫星的受力分析模型主要考虑地球中心引力和其他摄动力这两部分。卫星受摄二体问题所涉及的数学模型是一个相当复杂的非线性动力系统,在所选取的地心直角坐标系中,相应的运动微分方程可写为下列形式:

其中,β是力模型涉及的物理参数和卫星本体的星体参数(如有效面质比),F0是地球作为质点的引力加速度,Fε为各种摄动加速度。

卫星在运动过程中,地球中心引力(即地球相当于一个密度分布均匀的球形天体,等效于质量集中在之心的质点引力)的作用是主要的,其余的作用力都相对较小。图1中给出了不同轨道上不同摄动力的量级,当卫星轨道高度低于780km时,主要受地球中心引力、J2摄动和大气阻力影响;当卫星轨道高度高于780km时,大气阻力可不考虑。实际计算中,将重力势的高阶项和其他的摄动力加入动力学模型,可能会大量增加不必要的计算,现而对实际精度却没有相应的提高,也不利于工程化的实现。因此,在建立卫星动力学模型时采用简单的摄动力模型:JGM-03重力场模型、大气阻力。这种简化的动力学模型称为简化动力学模型,或约化动力学模型。

2 Kalman滤波算法

Kalman滤波是线性最小方差估值器,也叫最优滤波器,在线性系统中,Kalman滤波是最优滤波器,处理高斯模型的系统非常有效。

用如下状态空间模型描述动态线性系统

其中,Xk为k时刻的n维系统状态估计量,是从k时刻到k+1时刻的n×n维状态转移矩阵;Γk+1,k是n×p维系统噪声驱动矩阵;Wk是p维系统噪声矢量;Zk+1是m维观测量;Hk+1是m×n维观测矩阵;Vk+1是m维观测噪声矢量。系统噪声Wk和观测噪声Vk+1被假设成高斯白噪声分布,对所有的k,j有:

式中,Qk和Rk分别为系统噪声协方差阵和观测噪声协方差阵;δkj为克罗尼克δ函数。假定初始状态有下列统计特性:

X0与Wk,Vk都不相关,即:

Kalman滤波过程包括时间更新和测量更新两个部分,对应的5个Kalman核心公式:

公式(6):利用系统的过程模型预测下一时刻的系统状态,是k时刻系统状态量的最优估计值,预测得到k+1时刻的状态量;公式(7):是协方差更新,Pk+1,k是系统状态量对应的状态协方差矩阵;公式(8):计算得到Kalman增益Kk+1;公式(9):根据k+1时刻的状态预测值、观测Zk+1和增益Kk+1计算得到k+1时刻的状态估计值;公式(10):估计得到k+1时刻状态估计值的状态协方差矩阵。

标准Kalman滤波算法除了要求过程噪声和观测噪声是零均值、高斯白噪声序列外,还要求系统的状态方程和测量方程都是线性的。对非线性滤波问题常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题,这就是扩展卡尔曼滤波(EKF)。由于卫星轨道确定是一个非线性过程,所以常常基于动力学模型的统计信息和观测量,用扩展Kalman对卫星运动状态进行最小协方差估计。

3 扩展Kalman滤波算法仿真

在非线性条件下,可以利用非线性函数的局部线性特征,将非线性模型局部化,再利用Kalman滤波算法完成状态估计。扩展Kalman滤波就是基于这种思想,利用雅各比矩阵解决了非线性问题,即将系统的非线性函数的Taylor展开进行一阶线性化阶段。

基于星载GPS接收机的观测值进行扩展Kalman滤波的卫星实时定轨仿真。在J2000坐标系下,使用STK软件模拟的轨道数据进行定轨仿真,如图2所示。用STK软件模拟卫星精密轨道,低轨卫星的轨道根数为:近地点高度470km,离心率0.004,轨道倾角83°,近地点角距、升交点赤经和真近点角都为0。自主定轨采用简化动力学模型进行轨道外推,摄动力模型包括JGM3非球形引力摄动、大气阻力。系统仿真起始时间为2010年1月1日0时。

从图2中可以看到,定轨前由于存在大量噪声,位置速度误差分布范围比较广泛,位置误差分布在0~60m间,速度误差分布在0~0.7m/s之间;图3表示定轨后位置和速度的RMS误差平均值。扩展Kalman定轨收敛后,位置速度误差有了明显减少,位置误差分布减小到0~5m间,速度误差分布减小到0~0.5m/s间。可以看出,扩展Kalman滤波定轨后,卫星位置和速度误差都减小了一个量级。仿真表明,结合简化动力学模型的扩展Kalman滤波定轨对星载GPS的导航解的观测噪声有很好的削弱作用。

摘要：在只考虑重力场作用和大气阻力作用的情况下,建立卫星简化动力学模型。研究基于扩展Kalman滤波的星载GPS定轨,根据GPS观测数据结合简化动力学对卫星状态进行最优估计。在J2000坐标系下,使用STK软件模拟的轨道数据进行定轨仿真。结果表明,该算法能够大幅度提高定轨位置和速度的精度。

关键词：简化动力学,扩展Kalman滤波,定轨,精度

参考文献

[1]谢钢.GPS原理与接收机设计[M].北京:电子工业出版社,2009.

[2]黄小平,王岩.卡尔曼滤波原理及应用[M].北京:电子工业出版社,2015.

一种组合式的Kalman滤波算法第7篇

下一代有线数字电视传送技术的无线传感器网络[1]是下一代有线数字电视传送网络的扩展和延伸,可提供用户在生活、生产中除高速宽带业务需求之外的基于无线传感器的其他综合业务。随着处理业务的增多,数据的传输量和能量消耗也将随之增大,本文在经典最优Kalman滤波理论[2]的基础上,提出一种组合式的数据融合算法对多业务数据进行融合,从而降低整个系统的能耗。

经典Kalman滤波理论对动态系统提出了严格的要求,即当观测几何信息和动力学模型及统计信息可靠时,Kalman滤波性能较好,但是在实践中很难满足这一条件,在使用不精确或错误的模型和噪声统计设计Kalman滤波器时会导致滤波结果失真,甚至使滤波发散,故使得Kalman滤波在实际应用中有一定的局限性。

为了克服Kalman滤波算法的局限性,基于经典Kalman滤波的信息融合估计理论得到了很大的发展。Sage和Husa在文献[3]提出了自适应Kalman滤波算法估计,该算法由互耦的常规Kalman滤波算法和噪声统计估值器组成,可在线互耦估计状态和噪声统计,算法简单且具有良好的性能,但是它不能处理时变噪声统计估计问题,而且精度不高。文献[4]提出了在线性最小方差意义下按矩阵加权、按对角阵加权和按标量加权的3种最优信息融合准则和算法。3种方法的计算复杂度和精度各有其优势和劣势,按矩阵加权方法的融合估计精度较高但计算量大;按标量加权方法融合精度稍有降低计算量最小,适于实时应用;而按对角阵加权方法融合估计准则精度和计算负担介于它们两者之间。本文提出的算法也是基于经典Kalman滤波的融合估计理论而提出的。

1 自适应加权融合算法概述

将n个传感器测量的业务数据进行一致性检验[5]后,对这些测量值进行自适应加权融合估计,融合模型如图1所示,其中W1,W2,,Wn为每个传感器对应的加权因子, $\hat{X}$ 为n个传感器的融合结果。

自适应加权融合算法的优点[6]是可在未知传感器测量数据的任何先验知识的情况下,通过估计各个传感器方差的变化,获取各个传感器的最佳加权因子,得到最优融合结果,可以提高融合的准确度和精度。

设n个传感器的方差分别为σ $_{1}^{2}$ ,σ $_{2}^{2}$ ,,σ $_{n}^{2}$ ;所要估计的真值为X,各传感器的测量值分别为X1,X2,,Xn,它们彼此互相独立,并且是X的无偏估计,各传感器的加权因子分别为W1,W2,,Wn, $\hat{X}$ 为融合后的真值,推导过程见文献[7]。主要过程为

$\hat{X} = \sum_{p = 1}^{n} W_{p} X_{p} (1)$

$\sum_{p = 1}^{n} W_{p} = 1 (2)$

$σ^{2} = \sum_{p = 1}^{n} W_{p}^{2} σ_{p}^{2} (3)$

$W_{p} = \frac{1}{σ_{p}^{2} \sum_{i = 1}^{n} \frac{1}{σ_{i}^{2}}}, (p = 1, 2, \dots, n) (4)$

由上可知,最优加权因子Wp取决于各个传感器的方差σ $_{p}^{2}$ (p=1,2,,n),σ $_{p}^{2}$ 一般未知的,可根据各个传感器所提供的测量值求得,即

σ $_{p}^{2}$ = E[V $_{p}^{2}$ ] = Rpp-Rpq (5)

$\begin{array}{l} R_{p p} = \frac{1}{k} \sum_{i = 1}^{k} X_{p} (i) X_{p} (i) = \frac{1}{k} \sum_{i = 1}^{k - 1} X_{p} (i) X_{p} (i) + \\ X_{p} (k) X_{p} (k) = \frac{k - 1}{k} R_{p p} (k - 1) + \frac{1}{k} X_{p} (k) X_{p} (k) (6) \end{array}$

$R_{p q} = \frac{k - 1}{k} R_{p q} (k - 1) + \frac{1}{k} X_{p} (k) X_{q} (k) (7)$

$\bar{R}_{p} (k) = \frac{1}{n - 1} \sum_{q = 1, q \neq p}^{n} R_{p q} (k) (8)$

式中:Rpp为i时刻Xp的自协方差函数;Rpq为i时刻Xp,Xq的互协方差函数。根据上述公式,可以求出各加权因子,然后计算出融合估计值 $\hat{X}$ 。

2 组合式的Kalman滤波算法

本文提出的算法是将自适应加权融合算法与Kalman滤波算法组合起来,使得观测的数据先进行自适应加权融合算法,将融合的结果作为第二级Kalman滤波算法先验估计值,再进行Kalman滤波,从而得到最优值。新算法的框图如图2所示。

下面是Kalman滤波算法的推导过程[6]:

考虑时变离散系统模型,其状态方程为

X(k+1)=Φ((k+1)/k)X(k)+

Γ((k+1)/k)W(k) (9)

量测方程为

Z(K)=H(k)X(k)+V(k) (10)

式中:X(k+1)为k时刻的系统状态;Φ((k+1)/k)为从k-1到k时刻的一步状态转移矩阵;Γk-1∈Rnr为k时刻的系统噪声矩阵;H(k)∈Rmn为系统量测矩阵; V(k)m为量测噪声向量;Wk-1∈Rr为k时刻的系统噪声; Z(k)∈Rm为系统k时刻的观测。假设,W(k)和V(k)为相互独立且服从正态分布的白色噪声,在采样间隔内,其统计特性为

${\begin{cases} E [W (k)] = E [V (k)] = 0 \\ E [W (k) W^{Τ} (j)] = Q_{k} δ_{k j} \\ E [V (k) V^{Τ} (j)] = R_{k} δ_{k j} \\ E [W (k) V^{Τ} (j)] = 0 \end{cases} (11)$

时间更新方程为

$\begin{array}{l} \hat{X} (k / (k - 1)) = Φ (k / (k - 1)) \hat{X} (k - 1) + \\ Γ (k / (k - 1)) W (k) (12) \end{array}$

P(k/(k-1))=Φ(k/(k-1))P(k-1)

ΦT(k/(k-1))+Q(k) (13)

状态更新方程为

K(k)=Φ((k+1)/k)P(k/(k-1))

HT(k)[H(k)P(k/(k-1))HT(k)+R(k)]-1 (14)

$\begin{array}{l} \hat{X} ((k + 1) / k) = Φ ((k + 1) / k) \hat{X} (k / (k - 1)) + \\ Κ (k) [Ζ (k) - Η (k) \hat{X} (k / (k - 1))] (15) \end{array}$

P((k+1)/k)=Φ((k+1)/k)P(k/(k-1))

ΦT((k+1)/k)-Φ((k+1)/k)P(k/(k-1))

HT(k)[H(k)P(k/(k-1))HT(k)-Rk]-1

H(k)P(k/(k-1))ΦT(k+1/k)+

Γ((k+1)/k)QkΓT((k+1)/k) (16)

算法的运算流程如下:

1) 用式(6)、(7)递推算出采样时刻k的Rpp(k)与Rpq(k);

2) 用式(8)求出时刻k的 $\overset{}{R}_{p} (k)$ ;

3) 用式(5)求出时刻k的σ $_{p}^{2}$ ;

4) 用式(4)求出时刻k各传感器的最优加权因子Wp;

5) 根据式(1)式得出此时刻的估计值 $\hat{X}$ ;

6) 令 $\hat{X}$ 为Kalman滤波器在k时刻的先验估值,即式(12)中的 $\hat{X} (k - 1)$ ,根据式(12)、(13),计算出 $\hat{X} (k / (k - 1))$ 、P(k/(k-1));

7) 根据式(14)～(16)求出K(k), $\hat{X} ((k + 1) / k)$ ,P((k+1)/k),即可完成后验状态估计。

3 仿真分析

为了验证算法的有效性,对本文的组合式的Kalman滤波算法进行仿真,并与Kalman滤波算法进行比较,选取系统采样点数n=100,对其中50个点进行观察,仿真结果如图3所示。

图3中实线表示Kalman滤波算法,虚线为本文算法。从图中可以看出,经过第一级的自适应加权融合算法,不但数据传输量会显著减少,而且融合后的值比单一传感器所采集的数据更逼近于真实值,提高了融合结果的准确性。图4为本文算法在理论和实际情况下的误差曲线,由此可以验证融合算法的有效性。

新算法先通过自适应算法,对采集到的数据进行第一次处理,再进行Kalman滤波算法,会大大减少数据的处理量,可以提高融合的效率。

4 结束语

本文针对下一代电视传送技术的无线传感器网络中多业务数据的处理问题,基于经典Kalman滤波理论,提出了一种自适应加权融合算法和Kalman滤波算法的组合算法,该算法使测量的数据先进行自适应加权融合算法,将融合的结果作为第二级Kalman滤波算法先验估计值,再进行Kalman滤波,从而得到最优值。通过仿真验证,该算法与自适应融合算法相比,可以提高融合结果的准确性,降低了业务数据的处理量,降低了整个传输系统的能耗。

摘要：针对Kalman滤波算法在估计过程中存在噪声影响和过程信号无法直接观测等问题,提出一种组合式的Kalman滤波算法。首先对观测的数据进行自适应加权融合,然后将融合的结果作为第二级Kalman滤波的先验估计值,进行Kalman滤波。通过自适应算法与Kalman算法的组合算法进行数据融合,可以提高融合的准确度和精度。最后通过仿真证实算法的有效性。

关键词：Kalman,自适应加权,数据融合

参考文献

[1]孙友伟.基于下一代电视传送技术的无线传感器网络[J].电视技术,2010,34(6):54-56.

[2]KALMAN R E.A new approach to linear filtering and prediction prob-lems[EB/OL].[2012-07-10].http://160.78.24.2/Public/Kalman/Kalman1960.pdf.

[3]SAGE A P,HUSA G W.Adaptive filtering with unknown prior statistics[C]//Proc.Joint American Control Conference.Boulder,CO:[s.n.],1969:760-769.

[4]邓自立.最优估计理论及其应用:建模、滤波、信息融合估计[M].哈尔滨:哈尔滨工业大学出版社,2003.

[5]LUNRC.Dynamic multi-sensor data fusion system for intelligent robots[J].IEEE Journal of Robotics and Automation,1988(4):386-396.

[6]李战明,陈若珠,张保梅.同类多传感器自适应加权估计的数据级融合算法研究[J].兰州理工大学学报,2006(8):78-82.

Kalman滤波定位第8篇

北斗卫星导航系统采用主动式双向测距实现二维导航,且三维定位精度约几十米,授时精度约100 ns[1]。北斗系统为了统一时间,需要一种手段将基准时间信号传递给用户以实现时间同步,这就是时间的传递,也叫做授时。北斗单向授时用户利用北斗卫星的出站信号广播2颗卫星的位置、速度和电波传播修正以及中心站至导航卫星的精确上行传播时延值等参数,通过计算机对采集的数据进行统计处理,根据处理结果对本地时钟进行改正,实现北斗时间同步[2]。由于北斗用户单向授时精度要求高,而影响单向授时精度的因素较多,下面通过对北斗单向授时原理进行了分析,结合北斗用户设备单向授时精度,针对卫星广播参数的上行时延值连续波动影响授时精度问题,提出了基于修正Kalman滤波算法能够彻底解决用户单向授时异常问题。

1 单向授时原理

中心站每分钟以一个超帧的形式经卫星向用户发送出站信号,在每一超帧的广播信息中发播授时信息,授时信息包括时刻、闰秒、时差、卫星位置、卫星速度、上行时延和电波传播修正模型参数等。用户接收中心站播发的授时信息,同时用户机的时间间隔计数器以本地钟秒信号1 pps作开门脉冲,以调制解调器检测出的第n帧询问信号的参考时标作关门脉冲测得ΔT′,如图1所示。

则用户钟差ΔT与ΔT′的关系为:

ΔT=ΔT′-τ-nΔt, (1)

τ=t设备单+tCS+tSU (2)

式中,t设备单为指信号在单向传播过程中所经设备的总时延;tCS为上行时延, 即中心站至卫星的距离时延值;tSU为下行时延,包括卫星至用户的距离时延即真空传播时延tSU′以及该路径上由对流层和电离层折射产生的传播附加时延 $t (θ, φ)_{}$ ,计算方法如下:

卫星至用户的真空传播时延为:

$τ_{S U^{'}} = \sqrt{(x_{s} - x_{u})^{2} + (y_{s} - y_{u})^{2} + (z_{s} - z_{u})^{2} / c}$ 。 (3)

卫星至用户的电波传播附加时延为 $t (θ, φ)_{}$ ,根据单向授时电波修正的数学模型:

$t (θ, φ) = \sum_{m 1, m 2 = 2}^{m 1 + m 2 Μ} a_{m 1 m 2} (θ - θ_{0})^{m 1} (φ - φ_{0})^{m 2} \cos^{m 1} (\frac{φ + φ_{0}}{2})$ 。 (4)

由于 $(x_{s}, y_{s}, z_{s})$ 与 $(x_{u}, y_{u}, z_{u})$ 已知,则卫星至用户的真空传播时延τSU′为真值;ΔT′为定时终端计数器测量时差值(假设为准确值),nΔt为常数,用户钟差ΔT仅与tCS和 $t (θ, φ)_{}$ 有关。而传播附加时延 $t (θ, φ)_{}$ 误差约为20 ns[3]。

因此,若单向授时用户时间跳数大于100 ns,由式(1)～式(4)可知,用户钟差ΔT仅与tCS相关。

2 修正卡尔曼滤波算法

根据卡尔曼滤波算法原理可知,当在测量序列中出现野值时, 必将影响新息序列的原有性质,导致滤波器估计不准,滤波精度下降。基于卡尔曼滤波器新息序列的特性,这里采用对卡尔曼滤波新息序列进行修正,提高测量野值鲁棒性:在判断存在测量野值的情况下,用一个活化函数加权于新息序列,通过在线修正新息序列,使修正的新息序列能够保持原有的性质,从而达到消除测量野值对滤波器估计结果的不利影响,提高估值的准确性[4,5]。

包含测量野值的离散Kalman滤波模型为:

xk=Axk-1+Buk+ωk。 (5)

yk=Cxk+Duk+ρk+vk。 (6)

式中,ρk为测量野值;yk∈Rm。上述离散系统的卡尔曼滤波递推公式为:

$\hat{x}_{k | k} = \hat{x}_{k | k - 1} + Κ_{k} e_{k}$ 。 (7)

式中,Kk为当前时刻滤波器增益矩阵; $e_{k} = y_{k} - \hat{y}_{k}$ 为传感器测量值与卡尔曼滤波器输出估计值之差即新息,ek可以看作是yk与 $\hat{y}$ k的线性组合,容易得到,在滤波结果趋于稳定时, 新息序列服从多维正态分布 $e_{k} ~ Ν (0, σ_{k}^{2})$ ,其中σ $_{k}^{2}$ =CPx,k|k-1CT+Rk,Px,k|k-1为状态变量当前时刻估计协方差。

新息序列的统计特性直接反映出测量参数的统计特性,因此可以通过对新息序列的分析判断当前系统测量参数中是否存在野值。由于新息序列为高维向量,直接对其进行分析比较复杂,因此构造新息序列的标量变量为:

$r_{e k} = e_{k}^{Τ} (σ_{k}^{2})^{- 1} e_{k}$ 。 (9)

容易得出,变量rek服从自由度为m的分布,即有 $r_{e k} ~ λ^{2} (m)$ 。rek反映了新息序列的统计特性,而新息序列的统计特性又可以反映出测量值的统计特性[6]。当测量参数中存在野值时,rek特性也随之发生改变。在判断是否存在测量野值后,通过对卡尔曼滤波算法进行修正,提高算法对于测量野值的鲁棒性,减小野值对估计结果的影响。

$\hat{x}_{k | k} = \hat{x}_{k | k - 1} + Κ_{k} Φ_{k} (r_{k}) e_{k}$ 。 (10)

式中, $Φ_{k} (r_{k})$ 为活化函数,用于消除测量参数中野值对于滤波器估计结果影响,通过活化函数对新息序列进行修正,保证新息序列特性不变,消除野值对于滤波器估计值的影响,保证上行时延估值的准确性,算法仿真如图2所示,图2(a)为仿真信号图,图2(b)为加入修正前后卡尔曼滤波算法仿真图。仿真结果表明,针对脉冲野值信号,修正的卡尔曼滤波算法平滑野值,效果明显。

3 实例分析

提取2010年4月19日中心站广播星历数据(包含上行时延和附加时延值)和北斗单向定时终端设备的数据分析,验证单向授时精度异常与上行时延值波动较大有关。

由于中心站广播上行时延频度为1次/min,而中心站卫星中心站的单向时延测量值τ更新频度为1次/5 s,利用每分钟内多个单向时延测量值,采用线性Kalman滤波算法进行滤波,估计并给出下一整分钟上行时延值的插值,即为该时刻所广播的上行时延值,而当某时刻中心站测量的单向时延测量值τ发生大的跳变时,若仍采用线性Kalman滤波算法估算上行时延值,其值会变化较大而无法使用,如图3(a)所示,直接影响用户单向授时精度。为了解决问题,提出了基于修正的卡尔曼滤波算法估计上行时延值的方法,通过对卡尔曼滤波算法进行修正,提高算法对于测量野值的鲁棒性,减小野值对估计结果的影响,上行时延值经过卡尔曼滤波后结果如图3(b)所示。

4 结束语

通过对北斗卫星导航系统广播上行时延值的分析,确定其变化直接影响了用户单向授时精度。采用修正卡尔曼滤波算法估计上行时延值,结果显示,在存在测量野值的情况下,线性Kalman滤波估计出现了较大的偏差,而修正卡尔曼滤波算法保证了较高的估计精度,从而确保了北斗用户单向授时精度。 

摘要：北斗卫星导航系统中北斗单向授时用户的授时精度与其上行时延值变化较大有直接关系。经过分析发现,在出现连续较大的野值情况下,线性Kalman滤波算法不能正确估计上行时延值,从而影响北斗用户正常使用。因此,对算法进行了相应修改并提出了基于修正的卡尔曼滤波算法,完成上行时延值的估计方法,同时对该算法进行了仿真,实验结果表明了该算法的正确性。

关键词：北斗导航,广播,单向授时,卡尔曼滤波,估计

参考文献

[1]张继荣.基于SDH的时间传递方法研究[D].北京:中国科学院研究生院,2008:12-15.

[2]谭述森.卫星导航定位工程(第2版)[M].北京:国防工业出版社,2010.

[3]谭述森.广义RDSS全球定位报告系统[M].北京:国防工业出版社,2011.

[4]张鹏,黄金泉.基于双重卡尔曼滤波器的发动机故障诊断[J].航空动力学报,2008(5):10-14.

[5]张帆,卢峥.自适应抗野值Kalman滤波[J].电机与控制学报,2007,11(2):188-190.

Kalman滤波定位第9篇

车道保持系统通过车载传感器获取道路信息，判断车辆行驶状态，对转向系统进行主动调节，控制车辆转向运动，降低传统汽车驾驶对驾驶员身体状态和驾驶技术的过分依赖，达到减轻驾驶疲劳，提高行车安全的目标，正日益受到人们的重视[1,2]。特别在车辆横向控制方面，近几年来，国内外许多学者进行了深入研究。Shinichiro Horiuchi和Kei Sunada根据广义预测理论(GPC)设计了一种基于理想虚拟驾驶员概念的新型驾驶员辅助系统。虚拟驾驶员控制的车辆前轮转角与实际驾驶员控制的车辆前轮转角差值对比，计算出驾驶辅助系统所需的转向力矩，进而帮助驾驶员操纵汽车行驶[3]。Minoiu等设计了一种转向控制器，当驾驶员精力不集中时发挥作用[4]。Mizushima等同时设计了车道保持控制器和车辆稳定控制器，并根据两种控制器对驾驶员操纵意图的权重估计设计了估计器[5]。日本大学的学者采用最优控制理论为两轮摩托车设计了驾驶员辅助车道保持系统，并利用驾驶员模型来评价设计的车道保持辅助系统[6]。在国内，吉林大学王荣本教授利用视觉传感器，对采集的道路信息实行图像处理，从实际道路中辨识出车道标识线，对其进行跟踪，实现车辆车道跟踪的功能，并进行了实车实验验证了其有效性[7]。

在此基础上，轮胎达到附着极限时，车道保持算法能否维持车辆的行驶稳定性需要加以深入研究，而这与轮胎的非线性特性密切相关。为此，本文选取车道偏差和方向偏差两个控制变量，首先建立了被控对象车辆模型、非线性轮胎模型和“预瞄跟随”驾驶员模型，随后，利用线性参数变化及最优控制理论设计车道保持系统实现对车道的跟踪与保持，维持车辆横摆运动的稳定性，保证轮胎侧向附着力不超过路面所能提供的附着极限，通过驾驶员模型在回路仿真的方式验证所设计车道保持系统。最后利用模型车实验，对比仿真结果，评价所设计的车道保持辅助控制系统的有效性。

1 车辆动力学模型

1.1 车辆模型

本文采用两自由度单轨车辆操纵动力学模型(如图1所示)，车辆具有侧向运动自由度和横摆运动自由度，结合非线性轮胎模型，借助计算机仿真软件Carsim与Matlab/Simulink建立联合仿真模型，研究车道保持驾驶辅助系统，在急促双移线工况下的性能。

车辆在弯道上行驶时，受到轮胎给地面的侧向力，产生侧向运动，动力学方程如下:

其中，ωr为车辆横摆角速度，IZ为车辆绕z轴的转动惯量，其中Ff、Fr为式(3)计算到的前、后轮胎侧向力，a、b为汽车前后轴到轴心的距离，前后轮侧偏角由下式计算:

1.2 非线性轮胎模型

由于线性轮胎模型无法表征轮胎接近极限工况下的车道保持系统的特性，因此，本文采用非线性刷子轮胎模型[8]，来描述轮胎非线性侧向力特性，具体公式如下:

其中ρy=θyσy，αsl=tan-1(1/θy)，θy=Cα/(3μFz)，σy=tan(α)，σ为轮胎侧偏角，μ为附着系数，Fz为轮胎垂向力，Cα为轮胎侧偏刚度。

1.3 驾驶员模型

根据郭孔辉院士提出的“预瞄跟随”驾驶员模型[9]，按照“最小误差原则”，预瞄时间T后，车辆预期横向位置坐标为:

最优侧向加速度为:

考虑反映与执行延时，驾驶员方向盘角输入为:

其中Kh为转向角增益，τr为驾驶员反映延时，τh为驾驶员操作延时。

2 车道保持控制器设计

驾驶员感知环境状态与车辆行驶状态，经判断对车辆施加一个控制转角δRM。同时传感器测量车辆的运动状态，基于测量值与驾驶员控制信号利用Kalman滤波算法估计车辆状态行驶状态x。所得到的车辆行驶状态估计值，按照设计时各项状态参数的要求，运用最优控制理论得到一个附加控制转角δLK，最终得到的控制转角为

即驾驶辅助系统控制转角。车道保持系统结构框图如图2所示。

2.1 车道保持控制器设计

车辆动力学模型取系统状态变量X=(ωTr，β，，ΔY)，ωr为横摆角速度，β为质心侧偏角，方向偏差，ΔY为侧向位置偏差，系统输入u为前轮转向角δf。根据线性2自由度单轨车辆模型，系统状态方程和输出方程如下，其中u=δf:

其中:

控制目标函数:

引入加权系数可写成如下形式:

轨迹切线方向与X正向夹角:

方向偏差与侧向位置偏差:

将上式代入式(12)得:

根据式(14)可得到具体的Q,R,N:

其中qi需要根据不同车速，进行增益调度设计。针对非线性车辆动力学模型，设计中采用线性参数变化调整方式[10,11,12]，选定一些稳定操作点，同时确定稳定操作点附近的稳定工作区间，选择vx作为因变量，进行多次试验，则得到的稳定的加权因子qi(vx)由vx确定，车道保持控制器在对应工作区间内稳定，按照不同的车速范围，调节qi的取值。利用LPV的方式就能将非线性系统以传统线性系统理论的方式来处理。

求解具有二次型目标函数的系统的最优反馈，可以解黎卡提方程，见式(11)。A、B为状态矩阵，Q、R为权矩阵，P为黎卡提方程的解，其数值解可以运用MATLAB工具包中的“LQR”函数来求解。

求解式(11)，可以得到反馈增益KLQR，辅助控制器的输出控制量为:

式中:Δx=x-x0;x0为理想行驶状态，。Y*为规划车道，为规划车道切线方向与全局坐标系X轴正向夹角。

2.2 Kalman滤波器实现状态估计

运用车载传感器可以获得横摆角速度ωr，侧向偏差ΔY，与方向偏差。基于这些测量值，利用Kalman滤波器能够实现x的状态估计。

对原系统式(8)在输入点引入过程噪声w，其状态方程可变为如下形式:

其中，w=δdist,G=B。

带有传感器噪声v的测量方程可由如下表示:

计算Kalman增益，假设过程扰动的协方差设定为102，横摆角速度ωr，质心侧偏角β，导航角，侧向偏距ΔY的噪声都设定为0.012。通过Matlab/Simulink软件中的“Kalman”函数就能得到优化后的估计值。结合式(12)得到带有状态估计的辅助系统控制量。

3 驾驶员在环仿真分析与评价

依照图2结构搭建仿真模型，选定不同车速，按照ISO/TC22/TR3888双移线实验标准，仿真模拟双移线道路试验。为了体现控制器具有一定的抗扰能力，过程中加入周期为5秒，脉宽0.1秒，幅值为1弧度的脉冲，模拟驾驶员的误操作产生的前轮转角扰动。采用Carsim软件中某型车辆参数如下所示:m=1 529.98;L=2.77622;a=1.13906;b=1.63716;Cαf=-46 560.5;Cαr=-24 955.5;Iz=4 607.47。驾驶员反应延迟和动作延迟时间为τr+τh=0.3秒。仿真结果如图3所示。

从图中可以看出在90 km/h高速工况下，对于没有车道保持辅助系统的车辆，单纯依靠驾驶员(预瞄跟随驾驶员模型)已不能控制车辆完成急促双移线试验，而带有车道保持(LK)驾驶辅助系统的车辆则能完成双移线试验，而对于中、低速行驶工况(60 km/h和30 km/h)，带有LK驾驶辅助系统车辆的行驶轨迹也更接近理想双移线行驶车道，其侧向偏差和方向偏差也较小，如图4所示。同时就横摆角速度和质心侧偏角两个动力学参数而言，也优于单独的“预瞄跟随”驾驶员模型控制。

此外，由于加入了Kalman滤波器对状态量进行优化估计，控制系统对噪声具有一定抗扰能力。

从轮胎侧向力仿真结果(图5)可以看出，所设计的驾驶辅助系统能通过调整车辆转向特性，合理地减小轮胎侧向力，在车辆高速行驶时能在一定程度上避免轮胎侧向力达到附着极限，提高车辆行驶安全性与操纵稳定性。

4 结语

针对非线性轮胎力单轨车辆模型，基于质心侧偏角估计的车道保持控制驾驶辅助系统仿真结果表明:

1)与传统汽车驾驶“人车路”闭环系统相比较，再加入所设计的车道保持控制系统之后，能在不同车速下有效帮助驾驶员提高车辆车道跟踪性能，同时能够改善车辆运动参数，如横摆角速度、质心侧偏角的跟踪精度。

2)所设计的车道保持控制系统能对影响前轮转角的人为扰动起到一定的抑制作用。

3)根据仿真实验，车道保持驾驶辅助系统在车辆转向时能有效防止轮胎侧向附着力超过路面附着极限，减轻了驾驶员的工作负荷，同时，在一定程度上提高了车辆行驶的安全性。

摘要：以非线性车辆动力学模型作为系统被控对象,利用Matlab/Simulink软件设计一种基于Kalman滤波算法的车道保持驾驶辅助系统。运用Kalman算法估计车辆行驶状态信息,并利用“预瞄—跟随”驾驶员模型—车辆模型—控制器所组成的驾驶员模型在回路仿真的方式对所设计系统进行验证。结果显示所设计的车道保持辅助系统能有效提高车辆路径跟踪能力。

关键词：车道保持,刷子轮胎模型,驾驶辅助系统,驾驶员模型,Matlab/Simulink

参考文献

[1]Pongsathorn R,Masao N,Motoki S.Lane keeping control strategy with direct yaw moment control input by considering dynamics of electric vehicle[J].Vehicle System Dynamics(s0042-3114),2006(44):192201.

[2]丁海涛,郭孔辉,李飞,等.基于加速度反馈的任意道路和车速跟随控制驾驶员模型[J].机械工程学报,2010,46(10):116120.

[3]Shinichiro horiuchi,Kei Sunada.Synthesis of driver assistance system for lane-following using generalized predictive control[C]//AVEC’98,1998.

[4]Minoiu N,Netto M,Mammar S,et al.A switched optimized approach for road-departure avoidance:implementation results[C]//IEEE intelligent vehicle symposium,Istanbul,Turkey,2007.

[5]Mizushima T,Raksincharoensak P,Nagai M.Direct Yaw-moment Control adapted to driver behavior recognition[C]//SICE-ICASE international Joint Conference,Bexco,Busan,Korea,2006.

[6]Katagiri N,Marumo Y.,Tsunashima H.Design and evaluation of lane-keeping assistance system for motorcycles[C]//AVEC’08,2008.

[7]王荣本.基于边界的车道标识线识别和跟踪方法研究[J].计算机工程,2006,32(18).

[8]Pacejka H B.Tyre brush model[M]//Tyre and Vehicle Dynamics.2nd ed.Oxford,U.K.:Elsevier,2005:93 134.

[9]Guo Konghui,Guan H.Modeling of driver/vehicle directional control system[J].Vehicle System Dynamics(s0042-3114),1993,22:141184.

[10]Shamma J S.Analysis and design of gain scheduled control systems[D].Massachusetts Institute of Technology,1988.

[11]Liu S,He X.Gain-scheduled control design using the loop transfer recovery method for vapor compression systems[C]//Proceedings of the American Control Conference,1997:3326 3330.