·46· 智能系统学报 第1卷 2.2观测方程的建立 3)量测噪声均值 利用卡尔曼滤波器对系统进行最优估计，需要 建立观测方程，而观测量的选取取决于系统所能给 w=tk-Vk-)+Y因 出的状态信息，为此本文建立了以水下机器人的位 H哭(Hk-1)]. (13) 置、速度为观测量的观测方程，其形式为 4量测噪声方差阵 xe(k) R(1kR(k.V+( xe(k) Yxe(k) [1 00000 H(付P(Wk-1)H(J (14) x(k) Yre(k) 0 10000 式中：E(=Y(材-f(材-H(X(Wk-)为新息 xa(付 Yxn(k) 0 00010 阵 xn(k) Y(k 0000 第3步，交替进行计算，从而构成自适应卡尔曼 xi(付 滤波算法，流程见图2 Lx(k (k) 初始值 X0).P0).R0)0),20),q0).k-0 10 Vre(k) 输出XKK-1) 预测值X双/k-1) Vm(k 预测误差方差矩阵方程 或写成向量的形式： 计算Pkk-1D Y(k HX(k)+V(k 式中：Y(材和V(材分别为k时刻下的观测向量和观 增益方程计算(k) 测噪声向量，H为观测矩阵.在此基础上，也可以将 输入() 计算最优滤波值) 观测方程分解为东向观测方程和北向观测方程 输出《k) 3 自适应卡尔曼滤波 估计送差方程矩阵方程计算P(k) 当噪声统计特性与实际不符时特别是在工程 用自适应算法计 R(k).r(k).(k).q(k) 实际中，外界条件的变化可能使噪声统计特性是时 变的，这都会大大降低滤波的精度，甚至导致滤波发 =k+1 散).时变噪声统计估值器就是在递推滤波的同时， 图2自适应卡尔曼滤波算法 实时对系统噪声和量测噪声的统计特性进行估计 Fig.2 Architecture of adaptive Kalman filer 本文构造了一种指数加权方法的时变噪声统计估值 从统计的观点看，噪声统计估值器是算术平均 器 的，和式中每项的权系数均为1/k,但对时变噪声统 第1步，用标准的卡尔曼滤波方程计算出系统 计而言，应强调新近数据的作用，对于过于陈旧的数 状态的估计量 据应渐渐遗忘.这可用渐消记忆指数加权方法实现， 第2步，计算系统干扰均值q(),系统干扰方 即在和式中每项乘以不同的加权系数，按指数加权 差矩阵Q(付，量测噪声均值（材，量测噪声方差矩 法，选取加权系数月使之满足：A=月b,0<b<1, 阵R(付 1)系统干扰项均值为 ,9=1,这引出月=db,d=1-11-), 9为=太k.)k.)+xW则 i=0,1,,k 中(k,k.1)哭(k。1/k.1 (11) 式中：6称遗忘因子.用女代替原来的权系数 1/k,便得到指数加权渐消记忆的时变噪声统计估值 2)系统干扰项方差阵 器.其递推算法为 Q(-t1(k-vQ(k-V+ 0(=1-d.1q(k-1)+d.1[哭(W)- K(k)E(k)E (k)K (k)+P(k/k) Φ(k,k-1)突(k-1/k-)1.15) 中(k,k-1)P(k-1/k-1)D(Wk-1)1 Q(d=[1-d.)Qk-1)+d.1] (12) K(k)E(k)e (k)KT(k+P(k/k) 1994-2008 China Academic Journal Electronic Publishing House.All rights reserved.http://www.cnki.nei2. 2 观测方程的建立 利用卡尔曼滤波器对系统进行最优估计 ,需要 建立观测方程 ,而观测量的选取取决于系统所能给 出的状态信息 ,为此本文建立了以水下机器人的位 置、速度为观测量的观测方程 ,其形式为 Y xe ( k) Yve ( k) Y xn ( k) Yvn ( k) = 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 xe ( k) x·e ( k) ¨xe ( k) xn ( k) xn ( k) x·n ( k) ¨x n ( k) + V xe ( k) V xn ( k) V ve ( k) V vn ( k) . (10) 或写成向量的形式 : Y( k) = HX ( k) + V ( k) . 式中 :Y( k) 和 V ( k) 分别为 k 时刻下的观测向量和观 测噪声向量 , H 为观测矩阵. 在此基础上 ,也可以将 观测方程分解为东向观测方程和北向观测方程. 3 自适应卡尔曼滤波 当噪声统计特性与实际不符时 ,特别是在工程 实际中 ,外界条件的变化可能使噪声统计特性是时 变的 ,这都会大大降低滤波的精度 ,甚至导致滤波发 散[6 ] . 时变噪声统计估值器就是在递推滤波的同时 , 实时对系统噪声和量测噪声的统计特性进行估计. 本文构造了一种指数加权方法的时变噪声统计估值 器. 第 1 步 ,用标准的卡尔曼滤波方程计算出系统 状态的估计量. 第 2 步 ,计算系统干扰均值 ^q ( k) ,系统干扰方 差矩阵 Q^ ( k) ,量测噪声均值 ^r ( k) ,量测噪声方差矩 阵 R^ ( k) . 1) 系统干扰项均值为 ^q( k) = 1 k ( k - 1) ^q( k - 1) + X^ ( k/ k) - Φ( k , k - 1) X^ ( k - 1/ k - 1) . (11) 2) 系统干扰项方差阵 Q^ ( k) = 1 k [ ( k - 1) Q^ ( k - 1) + K( k)ε( k)εT ( k) K T ( k) + P( k/ k) - Φ( k , k - 1) P( k - 1/ k - 1)ΦT ( k/ k - 1) ]. (12) 3) 量测噪声均值 ^r ( k) = 1 k [ ( k - 1) ^r ( k - 1) + Y( k) - H( k) X^ ( k/ k - 1) ]. (13) 4) 量测噪声方差阵 R^ ( k) = 1 k [ ( k - 1) R^ ( k - 1) +ε( k)εT ( k) - H( k) P( k/ k - 1) H T ( k) ] (14) 式中 :ε( k) = Y( k) - ^r ( k) - H ( k) X( k/ k - 1) 为新息 阵. 第 3 步 ,交替进行计算 ,从而构成自适应卡尔曼 滤波算法 ,流程见图 2. 图 2 自适应卡尔曼滤波算法 Fig. 2 Architecture of adaptive Kalman filer 从统计的观点看 ,噪声统计估值器是算术平均 的 ,和式中每项的权系数均为 1/ k ,但对时变噪声统 计而言 ,应强调新近数据的作用 ,对于过于陈旧的数 据应渐渐遗忘. 这可用渐消记忆指数加权方法实现. 即在和式中每项乘以不同的加权系数 ,按指数加权 法 ,选取加权系数βi 使之满足 : βk =βib , 0 < b < 1 , ∑ k i = 0 βi = 1 ,这引出βi = dkb i , dk = (1 - b) / ( 1 - b k - 1 ) , i = 0 ,1 , …, k. 式中 :b称遗忘因子. 用 1 - dk - 1 k - 1 代替原来的权系数 1/ k ,便得到指数加权渐消记忆的时变噪声统计估值 器. 其递推算法为 ^q( k) = (1 - dk- 1 ) ^q( k - 1) + dk- 1 [ X^ ( k/ k) - Φ( k , k - 1) X^ ( k - 1/ k - 1) ]. (15) Q^ ( k) = [ (1 - dk- 1 ) Q^ ( k - 1) + dk- 1 ] [ K( k)ε( k)εT ( k) K T ( k) + P( k/ k) - · 64 · 智 能 系 统 学 报 第 1 卷 © 1994-2008 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net