CN109814387B - 一种基于动态时延的卡尔曼滤波补偿方法及系统 - Google Patents

Abstract

本发明公开了一种基于动态时延的卡尔曼滤波补偿方法及系统，当数据采集器输出一组数据后，传感器自身会输出一脉冲给控制器作为一组数据获取结束的标志，在控制器获取这一脉冲时使用一个时钟模块记录时刻，在控制器自身完成对于数据的解析后，根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间，两个时刻相减得到时延，将时延除以控制周期后取整便可以得到卡尔曼滤波补偿所需步数，补偿步数将给后续时延期间观测量的拟合估计提供依据，进而对当前时刻状态量进行补偿估计。本发明可以使在控制中所使用的卡尔曼滤波状态估计量更加准确，从而通过提高滤波精度来达到提高控制精度的目标。

Description

一种基于动态时延的卡尔曼滤波补偿方法及系统

技术领域

本发明涉及数据处理领域，更具体地说，涉及一种基于动态时延的卡尔曼滤波补偿方法及系统。

背景技术

常用控制程序中发现：实际控制应用中，会有一些数据传输、处理所产生的时延，该时延会对控制精度产生影响。然而现有的卡尔曼滤波补偿方法中，由于没有涉及到延迟时间的获取，进而对延迟步数处理为某一固定值，这对于控制精度要求较高的对象而言，此类固定的补偿步数无法满足较高精度控制的需求。以线性卡尔曼滤波为例：

首先，建立系统状态空间模型

w_k-1为过程噪声，具体为均值为0、方差为Q_k-1的高斯白噪声，v_k为观测噪声，具体为均值为0、方差为R_k的高斯白噪声，x_k为k时刻的状态值，H为观测矩阵，A为状态转移矩阵，z_k为k时刻的观测值；

依据贝叶斯滤波算法原理，卡尔曼滤波过程可分为时间更新和量测更新两个阶段，即预估和校正阶段，滤波流程如图1所示。

时间更新：实现状态一步预测和协方差一步预测：

其中，

和P_k|k-1为k时刻的状态一步预测值和协方差矩阵，

和P_k-1|k-1为k-1时刻的状态估计值与协方差矩阵，Q_k-1表示k-1时刻状态噪声的方差。

量测更新：依据当前时刻的观测序列，实现状态估计和协方差估计：

其中，K_k表示k时刻的卡尔曼滤波增益；I为单位矩阵。

已有方法中对目标状态进行固定步数的补偿，可以理解为，由于系统时延的存在，实际中无法直接获得当前时刻目标的状态。若假设系统采样周期为T，时延为hT(传统方法假设时延为某个固定值)，当前时刻为k，则实际上系统只能在h+1～k时刻获得1～k-h时刻的状态，由上述滤波模型，通过滤波算法进行h步的预测，即可获得当前时刻目标的状态估计值，由于k-h+1～k时刻系统观测量无法获得，所以需用观测量的估值代替递推过程中的观测量，但是现有的卡尔曼滤波补偿方法中，由于没有涉及到延迟时间的获取，对延迟步数处理为某一固定值，对于控制精度要求较高的对象而言，补偿固定的步数无法满足较高精度控制的需求。

发明内容

本发明要解决的技术问题在于，针对现有的卡尔曼滤波补偿方法中，由于没有涉及到延迟时间的获取，进而对延迟步数处理为某一固定值，对于控制精度要求较高的对象而言，补偿固定的步数无法满足较高控制精度的需求，提供一种基于动态时延的卡尔曼滤波补偿方法及系统。

根据本发明的第一方面，本发明解决其技术问题所采用的基于动态时延的卡尔曼滤波补偿方法包含如下步骤：

S1、利用数据采集器采集任意一组数据后，控制数据采集器自身输出一脉冲给控制器作为一组数据获取结束的标志，控制器记录此时的时刻t_i；

S2、控制器在获得该组数据后，对数据进行解析，在控制器自身完成对于该组数据的解析后，会根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间t_n；其中，控制器会按照固定的控制周期T'控制一受控模块；

S3、计算时延：Δt＝t_n-t_i；

S4、计算Δt/T'后进行取整处理得到补偿步数N；

S5、利用补偿步数N进行卡尔曼滤波时延期间观测量的拟合估计，进而获得当前时刻状态量的补偿估计。

根据本发明的第二方面，本发明解决其技术问题所采用的基于动态时延的卡尔曼滤波补偿系统包含如下步骤：包含如下模块：

数据采集模器，用于利用数据采集器采集任意一组数据后，控制数据采集器自身输出一脉冲给控制器作为一组数据获取结束的标志，控制器记录此时的时刻t_i；

数据处理模块，控制器在获得该组数据后，对数据进行解析，在控制器自身完成对于该组数据的解析后，会根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间t_n；其中，控制器会按照固定的控制周期T'控制一受控模块；

时延计算模块，用于计算时延：Δt＝t_n-t_i；

补偿步数计算模块，用于计算Δt/T'后进行取整处理得到补偿步数N；

补偿步数处理模块，用于利用补偿步数N进行卡尔曼滤波时后续时延期间观测量的拟合估计，进而对当前时刻状态量进行补偿估计。

实施本发明的基于动态时延的卡尔曼滤波补偿系统，具有以下有益效果：本发明可以使在控制中所使用的卡尔曼滤波状态估计量更加准确，从而通过提高滤波精度来达到提高控制精度的目标。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是卡尔曼滤波流程图；

图2是基于动态时延的卡尔曼滤波补偿方法一实施例的流程图；

图3是基于动态时延的卡尔曼滤波补偿方法的原理图；

图4是图像时滞时序图；

图5是控制周期与图像采集周期关系图；

图6是基于动态时延的卡尔曼滤波补偿系统。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

参考图1，本实施例的基于动态时延的卡尔曼滤波补偿方法包含如下步骤：

S1、利用数据采集器采集任意一组数据后，控制数据采集器自身输出一脉冲给控制器作为一组数据获取结束的标志，控制器记录此时的时刻t_i；数据采集器包括但是不限于：摄像头、声音传感器、红外传感器、浓度传感器、温度传感器、陀螺仪；

S2、控制器在获得该组数据后，对数据进行解析，在控制器自身完成对于该组数据的解析后，会根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间t_n；其中，控制器会按照固定的控制周期T'控制一受控模块；

S3、计算时延：Δt＝t_n-t_i；由于所处理数据组的不同，这一时延应当是动态变化的；

S4、计算Δt/T'后进行取整处理得到补偿步数N；取整处理为下述取整方式中的任意一种：四舍五入、去掉小数位保留整数、向上取整以及向下取整；

S5、利用补偿步数N进行卡尔曼滤波时延期间观测量的拟合估计，进而对当前时刻状态量进行补偿估计。即相较于补偿前：补偿前的某一观测值是k时刻的，现在是把这个观测值当做k-N时刻的数据。

卡尔曼滤波模型中，由于k-h+1～k时刻的观测量无法获得，所以需用观测量的估值

代替卡尔曼滤波递推过程中所需的观测量Z_i(k-h+1≤i≤k)。

对应于前述补偿步数的定义，下面均将h步改为N步。

当k-N+1≤i≤k时，Z_i的估计值

为i时刻目标在空间中各方向上位置的估计值。时延期间，此部分的估计值可以通过拟合的方式进行，考虑采用部分已观测到的数据进行拟合，建立一个多项式，使得每一时刻目标的运动轨迹参照前一时刻目标的运动状态，此处可由传感器返回的数据进行相应时刻影响目标运动轨迹因素信息的提取，并针对这些影响目标运动轨迹的因素设置相应的权系数，寻找这样一组权系数，使得由此建立的多项式拟合残差最小。拟合的数据来源于可获得的历史观测值(即在N+1～k时刻获得的1～k-N时刻观测值)，拟合系数(即前述的一组权系数)需满足估计的观测值(通过多项式拟合获得)与历史观测值在各方向上的拟合残差最小(此部分可定义合适的代价函数，目标为由估计观测值与历史观测值所产生的代价函数最小)，由此获得的拟合系数将再次代入拟合多项式中，这样对于延迟期间的观测量估计值

可以通过历史观测量Z_i由k-N时刻开始递推计算获得，然后由获得的观测量估计，进行卡尔曼滤波N步补偿，进而也获得了当前时刻状态量的补偿估计。

为了进一步加深对本发明的理解，下述结合图3对本发明做进一步说明。在数据采集器(以采集图像为例)采集到图像时向时延计算模块输出一场中断脉冲,将这一脉冲的产生时刻记作t_i,t_i由时延计算模块进行记录，在这一帧图像处理完成后，由数据解析模块向时延计算模块输出一结束脉冲，将这一结束脉冲的产生时刻记作t'_i，t_i与t'_i之间的时间差ΔT便是数据传输处理时延，t_i与t_i+1之间的时间差T即为数据采集周期，其关系如图4所示。

控制器中控制模块通常会按照固定的周期控制伺服器工作，令程序控制周期为T'，程序在t_n时刻进入定时器中断修改控制量，t_n与最近一次图像处理完成时刻t_i'相差时间为Δt'＝t_n-t'_i，所以和对应最近图像采集时刻的时间差为Δt＝t_n-t_i，其关系如图5。

在动态补偿卡尔曼中，需要使用Kalman滤波器补偿的步数N应当是：

同时需注意，在实际应用中需考虑系统噪声为有色的情况。一方面，目标运动一般产生机动，需考虑过程噪声，另一方面，随着越来越多的状态融合算法和新传感器出现，特别是随着传感器采样频率的增加，测量噪声的相关性也不可忽略。对于卡尔曼滤波，有必要考虑有色过程噪声和测量噪声的影响，即针对上述卡尔曼滤波的系统状态空间模型，考虑有色噪声w_k、v_k的一阶马尔科夫模型描述：

w_k＝M_k-1w_k-1+ξ_k-1(3)

这里，M_k-1、

分别为过程噪声和观测噪声的自相关系数，它们可以由视频采样数据分析结合平稳序列谱分解定理获得；ξ_k-1、ζ_k-1是不相关高斯白噪声，其均值为0，方差分别为

若直接将上述一阶马尔科夫模型描述重新代入系统状态空间模型，有色噪声仍然存在，必须通过对过程方程(1)及观测方程(2)重构的方式白化有色噪声，才能继续使用卡尔曼滤波算法的思想。这里过程方程部分将进行扩维处理，考虑系统状态和过程噪声一起作为新过程方程的状态(此过程为过程方程的一次重构)，而观测方程将通过观测差分的形式对其进行重构，并通过对重构后的观测方程进行相应整理，可以获得系统的相关参数并达到噪声分离(整个过程中，观测方程只进行一次重构)，最终将达到一次重构后的新过程方程及新观测方程均达到噪声白化的目标。为使用卡尔曼滤波算法，还需对一次重构后的新过程方程及新观测方程的噪声进行统计特性分析，经过分析，扩维一次重构后的新过程噪声和新观测噪声仍然存在相关性，卡尔曼滤波算法无法使用，这里对过程方程进行二次重构，考虑结合重构后的新观测方程，再次重构过程方程，通过整理可以获得系统的相关参数并达到噪声分离(二次重构后的新过程噪声包含了一次重构后的过程噪声、新观测噪声)，结合已获得的一次重构后的过程方程及新观测方程噪声间的统计特性，获取二次重构后的新过程噪声及新观测噪声满足Kalman滤波噪声数学特性所涉及到的参数，实现噪声相关性消除。然后可以继续使用卡尔曼滤波算法思想，得到鲁棒性更强的卡尔曼滤波器。

上述原理简单描述如下：

Step1:将系统状态和过程噪声一起作为新过程方程的状态,进行一次过程方程重构；

即将系统状态和过程噪声一起作为新过程方程的状态，结合系统原过程方程(1)的形式及过程噪声的一阶马尔科夫模型描述形式(3)，按照卡尔曼滤波过程方程的形式进行整理得到过程方程的一次重构形式。

这里将进行系统扩维，即将过程噪声w_k作为系统状态的一部分，扩维后的参数采用上标a与原参数区分。新过程方程如下：

标记

由公式(5)，新过程方程如下：

Step2:通过观测差分，进行一次观测方程重构，根据重构后的观测方程获得系统的相关参数并达到噪声分离，其中获得的观测噪声记为

由上，可以得到观测方程(2)如下：

由(7)可得

观测方程通过如下观测差分的形式进行重构：

标记

得到观测方程的一次重构形式如下：

获得系统的相关参数并达到噪声分离指得到公式(9)的各部分参数表达式，整个过程是一些公式及参数的代入整理，整理是按照常规卡尔曼滤波的观测方程的形式进行整理的。

Step3:根据ξ_k、

的统计特性，得到它们之间的相关性；

即

其中，

为ξ_k的方差，

为

的方差，S_k为ξ_k与

的互协方差，δ_kj为克罗内克函数。对于重构后的过程方程(6)及观测方程(9)，常规卡尔曼滤波需达到

(即ξ_k、

不相关)才可以使用，由上述ξ_k、

的统计特性分析，它们仍然存在相关性。

Step4:结合Step2新得到的观测方程，对一次重构后的过程方程进行二次重构，获得系统的相关参数并达到噪声分离，其中获得的新过程噪声记为

包含了ξ_k及

即

这里，r_k为噪声相关性消除所涉及的参数；一次重构后的过程方程为公式(6)，新观测方程为公式(9)，二次重构后的过程方程即公式(10)是在一次重构后的过程方程(6)基础上引入了一个零项(该零项由新观测方程(9)获得)；然后按照常规卡尔曼滤波的过程方程形式进行整理得上式(10)。根据式(3)(4)可得，新观测方程(9)及新过程方程(10)的噪声

均为高斯白噪声。

Step5:结合Step 3的噪声统计特性分析，获取二次重构后的新过程噪声

及新观测噪声

满足Kalman滤波噪声数学特性所涉及到的参数，实现噪声相关性消除；

即要使用卡尔曼滤波算法，需达到新过程方程(10)的过程噪声

与新观测方程(9)的观测噪声

的统计相关性为0(

不相关)，即

结合Step 3的噪声ξ_k、

统计特性分析，通过展开(

包含了ξ_k、

)并解上式(11)可以得到噪声相关性消除所涉及的参数r_k。

Step6:利用噪声相关性消除后的数据进行新卡尔曼滤波器的构建。

即达到了卡尔曼滤波算法的要求(即过程噪声

和观测噪声

为不相关高斯白噪声)，可基于卡尔曼滤波算法的基本思想推导得到鲁棒性更强的卡尔曼滤波器。

参考图6，本实施例的基于动态时延的卡尔曼滤波补偿系统包含如下模块：数据采集模器31、数据处理模块32、时延计算模块33、补偿步数计算模块34、补偿步数处理模块35。

数据采集模器31用于利用数据采集器采集任意一组数据后，控制数据采集器自身输出一脉冲给控制器作为一组数据获取结束的标志，控制器记录此时的时刻t_i；数据采集器包括但是不限于：摄像头、声音传感器、红外传感器、浓度传感器、温度传感器、陀螺仪；

数据处理模块32用于控制器在获得该组数据后，对数据进行解析，在控制器自身完成对于该组数据的解析后，会根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间t_n；其中，控制器会按照固定的控制周期T'控制一受控模块；

时延计算模块33用于计算时延：Δt＝t_n-t_i；

补偿步数计算模块34用于计算Δt/T'后进行取整处理得到补偿步数N，T'为控制周期；取整处理为下述取整方式中的任意一种：四舍五入、去掉小数位保留整数、向上取整以及向下取整；

补偿步数处理模块35用于利用补偿步数N进行卡尔曼滤波时后续时延期间观测量的拟合估计，进而对当前时刻状态量进行补偿估计。

补偿步数处理模块35中，卡尔曼滤波包括：

依次重构单元，用于将系统状态和过程噪声一起作为新过程方程的状态,进行一次过程方程重构，重构后得过程噪声为ξ_k-1；

噪声分离单元，用于通过观测差分，进行一次观测方程重构，根据重构后的观测方程获得系统的相关参数并达到噪声分离，其中获得的得观测噪声记为

相关性获取单元，用于根据ξ_k、

的统计特性，得到它们之间的相关性；

二次重构及噪声分离单元，用于结合噪声分离单元新得到的观测方程，对一次重构后的过程方程进行二次重构，获得系统的相关参数并达到噪声分离，其中获得的新过程噪声记为

包含了ξ_k及

相关性消除单元，用于结合相关性获取单元的噪声统计特性分析，获取二次重构后的新过程噪声

及新观测噪声

满足Kalman滤波噪声数学特性所涉及到的参数，实现噪声相关性消除；

卡尔曼滤波单元，用于利用噪声相关性消除后的数据进行新卡尔曼滤波器的构建。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (6)

1.一种基于动态时延的卡尔曼滤波补偿方法，其特征在于，包含如下步骤：

S1、利用数据采集器采集任意一组数据后，控制数据采集器自身输出一脉冲给控制器作为一组数据获取结束的标志，控制器记录此时的时刻t_i；

S2、控制器在获得该组数据后，对数据进行解析，在控制器自身完成对于该组数据的解析后，会根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间t_n；其中，控制器会按照固定的控制周期T'控制一受控模块；

S3、计算时延：Δt＝t_n-t_i；

S4、计算Δt/T'后进行取整处理得到补偿步数N；

S5、利用补偿步数N进行卡尔曼滤波时后续时延期间观测量的拟合估计，进而对当前时刻状态量进行补偿估计；

步骤S5中，卡尔曼滤波包括：

Step1:将系统状态和过程噪声一起作为新过程方程的状态,进行一次过程方程重构，重构后得过程噪声为ξ_k-1；其中，新过程方程如下：

其中，

H_c＝[H 0]

w_k为过程噪声，x_k为k时刻的状态值，H为观测矩阵，A为状态转移矩阵，z_k为k时刻的观测值，M_k-1的过程噪声的自相关系数，上标a起到参数区分的作用；

Step2:通过观测差分，进行一次观测方程重构，根据重构后的观测方程获得系统的相关参数并达到噪声分离，其中获得的观测噪声记为

其中，重构前的观测方程为：

z_k为k时刻的观测值，v_k为观测噪声；重构前的观测方程通过如下观测差分的形式进行重构：

得到观测方程的一次重构形式如下：

其中，

为观测噪声的自相关系数，ζ_k是均值为0高斯白噪声；

Step3:根据ξ_k、

的统计特性，得到它们之间的相关性，即：

其中，

分别为ξ_k、ζ_k的方差，

为

的方差，S_k为ξ_k与

的互协方差，δ_kj为克罗内克函数；

Step4:结合Step2新得到的观测方程，对一次重构后的过程方程进行二次重构，获得系统的相关参数并达到噪声分离，其中获得的新过程噪声记为

包含了ξ_k及

二次重构后的过程方程为：

r_k为噪声相关性消除所涉及的参数；

Step5:结合Step3的噪声统计特性分析，获取二次重构后的新过程噪声

及新观测噪声

满足Kalman滤波噪声数学特性所涉及到的参数，实现噪声相关性消除；即要使用卡尔曼滤波算法，需达到二次重构后的过程方程的过程噪声

与一次重构后的观测方程的观测噪声

的统计相关性为0，即

结合Step3的噪声ξ_k、

统计特性分析，通过展开并解上式得到噪声相关性消除所涉及的参数r_k；

Step6:利用噪声相关性消除后的数据进行新卡尔曼滤波器的构建。

2.根据权利要求1所述的基于动态时延的卡尔曼滤波补偿方法，其特征在于，数据采集器包括：摄像头、声音传感器、红外传感器、浓度传感器、温度传感器、陀螺仪。

3.根据权利要求1所述的基于动态时延的卡尔曼滤波补偿方法，其特征在于，取整处理为下述取整方式中的任意一种：四舍五入、去掉小数位保留整数、向上取整以及向下取整。

4.一种基于动态时延的卡尔曼滤波补偿系统，其特征在于，包含如下模块：

数据采集模器，用于利用数据采集器采集任意一组数据后，控制数据采集器自身输出一脉冲给控制器作为一组数据获取结束的标志，控制器记录此时的时刻t_i；

数据处理模块，控制器在获得该组数据后，对数据进行解析，在控制器自身完成对于该组数据的解析后，会根据解析后的数据进入定时器中断修改控制量，记录进入定时器的时间t_n；其中，控制器会按照固定的控制周期T'控制一受控模块；

时延计算模块，用于计算时延：Δt＝t_n-t_i；

补偿步数计算模块，用于计算Δt/T'后进行取整处理得到补偿步数N；

补偿步数处理模块，用于利用补偿步数N进行卡尔曼滤波时后续时延期间观测量的拟合估计，进而对当前时刻状态量进行补偿估计；

补偿步数处理模块中，卡尔曼滤波包括：

依次重构单元，用于将系统状态和过程噪声一起作为新过程方程的状态,进行一次过程方程重构，重构后得过程噪声为ξ_k-1；其中，新过程方程如下：

其中，

H_c＝[H 0]

w_k为过程噪声，x_k为k时刻的状态值，H为观测矩阵，A为状态转移矩阵，z_k为k时刻的观测值，M_k-1的过程噪声的自相关系数，上标a起到参数区分的作用；

噪声分离单元，用于通过观测差分，进行一次观测方程重构，根据重构后的观测方程获得系统的相关参数并达到噪声分离，其中获得的观测噪声记为

其中，重构前的观测方程为：

z_k为k时刻的观测值，v_k为观测噪声；重构前的观测方程通过如下观测差分的形式进行重构：

得到观测方程的一次重构形式如下：

其中，

为观测噪声的自相关系数，ζ_k是均值为0高斯白噪声；

相关性获取单元，用于根据ξ_k、

的统计特性，得到它们之间的相关性，即：

其中，

分别为ξ_k、ζ_k的方差，

为

的方差，S_k为ξ_k与

的互协方差，δ_kj为克罗内克函数；

二次重构及噪声分离单元，用于结合噪声分离单元新得到的观测方程，对一次重构后的过程方程进行二次重构，获得系统的相关参数并达到噪声分离，其中获得的新过程噪声记为

包含了ξ_k及

二次重构后的过程方程为：

r_k为噪声相关性消除所涉及的参数；

相关性消除单元，用于结合相关性获取单元的噪声统计特性分析，获取二次重构后的新过程噪声

及新观测噪声

满足Kalman滤波噪声数学特性所涉及到的参数，实现噪声相关性消除；即要使用卡尔曼滤波算法，需达到二次重构后的过程方程的过程噪声

与一次重构后的观测方程的观测噪声

的统计相关性为0，即

结合相关性获取单元的噪声ξ_k、

统计特性分析，通过展开并解上式得到噪声相关性消除所涉及的参数r_k；

卡尔曼滤波单元，用于利用噪声相关性消除后的数据进行新卡尔曼滤波器的构建。

5.根据权利要求4所述的基于动态时延的卡尔曼滤波补偿系统，其特征在于，数据采集器包括：摄像头、声音传感器、红外传感器、浓度传感器、温度传感器、陀螺仪。

6.根据权利要求4所述的基于动态时延的卡尔曼滤波补偿系统，其特征在于，取整处理为下述取整方式中的任意一种：四舍五入、去掉小数位保留整数、向上取整以及向下取整。

Images