第2期 李晔，等：水下机器人自适应卡尔曼滤波技术研究 ·45· 列.同时，V(付和E()满足： K(=P(H(R'(. (6) EV(k)]=0.Cov/v(k .v(j)]= 一步预测均方误差： [V(k).V(j)]=Q(k) P(k,k-1)=Φk,k-1)P(k-1)(k,k-1)+ E[E(k)]=0.Cov/E(k)E(j)]= T(k-1)Qk-1)(k-1).7) [E(k).E(j)]R(k)& 估计均方误差： Cov[V(k).E(j)]=E[V(k ET (j)=0.(3) P(=(I-K(H(W)P(k,k-1).8) 式中：Q(材为系统噪声序列方差阵：R(材为量测噪 式4)~式8)为离散型卡尔曼滤波基本方程.只要 声序列方差阵.状态X()的估计哭（可按下述方 给定初值X和B,根据k时刻的量测值Y(材就可 程求解 递推计算得k时刻的状态估计((k=1,2,. 状态一步预测： 式(4)~(7)所描述的算法可用图1来表示，从 究(k,k-1)=Φ(k,k-1)哭(k-1). 4) 图中明显可以看出卡尔曼滤波具有2个计算回路： 状态估计： 增益计算回路和滤波计算回路).其中增益计算回 哭(W=究k,k-1)+K(付Y(材- 路是独立计算回路，而滤波计算回路依赖于增益计 H(X(k,k-1)). (5) 算回路 滤波增益： X=M.iX Pu-IP+ =k+1 K:-P (HPH+R) 文=X+K(Y-H,X） Px =(I-K:Hi)P P. 滤波计算回路 增益计算问路 图1卡尔曼滤波的两个计算回路和两个更新过程 Fig.I Two calculating loops and updating processes of Kalman filter 2 水下机器人运动状态方程和观测方 xe(k) 1 T 2 00 0 xe(k-1) 程的建立 x(k) 0 00 0 x:(k-1 2.1状态方程的建立 x(k) 0 0 1 00 0 x(k-1) 以二维运动为例，从比较全面的反映运动情况 xn(k) 0 0 0 1 T xn(k-1) 的要求考虑，选择状态向量为 x(k) 2 x若(k-1) X=[xe,xa,xt,x:,x,x,e为东向，n为北向. x(k) 00 001 x(k-1小 L00 000 建立水下机器人运动的状态方程： W(k-1) (9 X(W=Φ(k,k-1)X(k-1)+W(k-1), 式中w(k-1)为模型噪声 或 从物理意义上看，水下机器人东向和北向的位 置速度和加速度之间没有必然的联系，因而系统是 解耦的 1994-2008 China Academic Journal Electronic Publishing House.All rights reserved.http://www.cnki.net列. 同时 ,V ( k) 和 E( k) 满足 : E[V ( k) ] = 0 ,Cov[V ( k) ,V ( j) ] = [V ( k) ,V T ( j) ] = Q( k)δkj , E[ E( k) ] = 0 ,Cov[ E( k) , E( j) ] = [ E( k) , E T ( j) ] = R( k)δkj , Cov[V ( k) , E( j) ] = E[V ( k) E T ( j) ] = 0. (3) 式中 :Q( k) 为系统噪声序列方差阵; R( k) 为量测噪 声序列方差阵. 状态 X( k) 的估计 X^ ( k) 可按下述方 程求解. 状态一步预测 : X^ ( k , k - 1) = Φ( k , k - 1) X^ ( k - 1) . (4) 状态估计 : X^ ( k) = X^ ( k , k - 1) + K( k) ( Y( k) - H( k) X^ ( k , k - 1) ) . (5) 滤波增益 : K( k) = P( k) H T ( k) R - 1 ( k) . (6) 一步预测均方误差 : P( k , k - 1) = Φ( k , k - 1) P( k - 1)ΦT ( k , k - 1) + Γ( k - 1) Q( k - 1)ΓT ( k - 1) . (7) 估计均方误差 : P( k) = ( I - K( k) H( k) ) P( k , k - 1) . (8) 式(4) ～式(8) 为离散型卡尔曼滤波基本方程. 只要 给定初值 X^0 和 P0 ,根据 k 时刻的量测值 Y( k) 就可 递推计算得 k 时刻的状态估计 X^ ( k) ( k = 1 ,2 , …) . 式(4) ～(7) 所描述的算法可用图 1 来表示 ,从 图中明显可以看出卡尔曼滤波具有 2 个计算回路 : 增益计算回路和滤波计算回路[5 ] . 其中增益计算回 路是独立计算回路 ,而滤波计算回路依赖于增益计 算回路. 图 1 卡尔曼滤波的两个计算回路和两个更新过程 Fig. 1 Two calculating loop s and updating processes of Kalman filter 2 水下机器人运动状态方程和观测方 程的建立 2. 1 状态方程的建立 以二维运动为例 ,从比较全面的反映运动情况 的要求考虑 ,选择状态向量为 X = [ xe , xn , x·e , x·n , ¨xe , ¨x n ] T ,e 为东向 , n 为北向. 建立水下机器人运动的状态方程 : X( k) = Φ( k , k - 1) X( k - 1) + W( k - 1) , 或 xe ( k) x·e ( k) ¨xe ( k) xn ( k) x·n ( k) ¨x n ( k) = 1 T T 2 2 0 0 0 0 1 T 0 0 0 0 0 1 0 0 0 0 0 0 1 T T 2 2 0 0 0 0 1 T 0 0 0 0 0 1 xe ( k - 1) x·e ( k - 1) ¨xe ( k - 1) xn ( k - 1) x·n ( k - 1) ¨x n ( k - 1) + W( k - 1) . (9) 式中 :W( k - 1) 为模型噪声. 从物理意义上看 ,水下机器人东向和北向的位 置、速度和加速度之间没有必然的联系 ,因而系统是 解耦的. 第 2 期 李 晔 ,等 :水下机器人自适应卡尔曼滤波技术研究 · 54 · © 1994-2008 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net