·346. 智能系统学报 第8卷 所以得到离散形式的系统方程为 X(k+1)=A(k)X(k)+u(k)+W(k), Z(k)=H(k)X(k)+V(k). (5) 对于系统方程(5)，系统初始时刻的状态定义 为Z(0)=X(0),X(0)=X(0),对于采样次数k=1, 2,…,N,使用式(6)进行最优递推滤波，可得系统状 态最优估计值。 X(k)=A(k)X(k-1)+u(k-1), 2468102146 P-(k)=A(k)P(k-1)A(k)"+, 图2领航者方程为式(7)时，7个智能体滤波后的速度 K(k)=P-(kH(k)(H(k)P-(k)H(k)+R)-, 一致性 X(k)=X(k)+K()(Z(k)-H()()), Fig.2 Consensus on velocity of 7 agents with Kalman P(k)=(I-K()H(k)P(k).(6) filter when the leader equation is Equ.(7) 式中：X(k)为k时刻使用系统状态方程对状态的 卡尔曼滤波 预测，X(k-1)为k-1时刻状态的最优估计值， 3.0m 无滤波 P(k)为k时刻对误差协方差的预测，P(k-1)为上 2.5 州 一时刻计算得到的误差协方差，X(k)为k时刻状态 n2.0 的最优估计值，P(k)为k时刻更新得到的误差协方 E1.5 差，K(k)为k时刻滤波器增益. 1.0H 3数值仿真 0.5 由于多AUV编队拓扑为动态拓扑，所以在 46810121416 t/s MATLAB仿真时，对于每一次采样都重新计算拉普 拉斯矩阵L(k)、系统矩阵A(k)和观测矩阵H(k). (a)原图 将相互有通信联系的智能体在拓扑矩阵中相应矩阵 元素置为1，失去联系的智能体其相应矩阵元素置 2.6 卡尔曼滤波 无滤波 为0，以表示时变矩阵。 设虚拟领航者方程为 2.4 xo(t)=[2(m/s)2(rad)2(rad)]T.(7) 取输出矩阵H=Ix,:W、V的协方差分别为Q= 2Ix7,R=2Ix仿真步长△T=0.1s.数值仿真结果如 图1~3所示. (b)局部放大图 图3领航者方程为式(7)时，滤波前后智能体1的速度 对比 Fig.3 Consensus on velocity of agent-1 with filtering Vs without filtering when the leader equation is Equ.(7) 46810立1416 设虚领航都方程为 图1领航者方程为式(7)时，7个智能体无滤波时的速 xo(t)=sin(@t)I3x3, (8) 度一致性 b。为常数矩阵，L(t)为时变拉普拉斯矩阵.则系统 Fig.I Consensus on velocity of 7 agents without filte- 方程的解为 ring when the leader equation is Equ.(7) X(t)=e(t)+boxo(t)d +W(t).所以得到离散形式的系统方程为 Ｘ（ｋ ＋ １） ＝ Ａ（ｋ）Ｘ（ｋ） ＋ ｕ（ｋ） ＋ Ｗ（ｋ）， Ｚ（ｋ） ＝ Ｈ（ｋ）Ｘ（ｋ） ＋ Ｖ（ｋ）． （５） 对于系统方程（５），系统初始时刻的状态定义 为 Ｚ（０）＝ Ｘ（０），Ｘ ＾ （０）＝ Ｘ（０），对于采样次数 ｋ ＝ １， ２，…，Ｎ，使用式（６）进行最优递推滤波，可得系统状 态最优估计值． Ｘ ＾ － （ｋ） ＝ Ａ（ｋ）Ｘ ＾ （ｋ － １） ＋ ｕ（ｋ － １）， Ｐ － （ｋ） ＝ Ａ（ｋ）Ｐ（ｋ － １）Ａ（ｋ） Ｔ ＋ Ｑ， Ｋ（ｋ） ＝ Ｐ － （ｋ）Ｈ（ｋ） Ｔ （Ｈ（ｋ）Ｐ － （ｋ）Ｈ（ｋ） Ｔ ＋ Ｒ） －１ ， Ｘ ＾ （ｋ） ＝ Ｘ ＾ － （ｋ） ＋ Ｋ（ｋ）（Ｚ（ｋ） － Ｈ（ｋ）Ｘ ＾ － （ｋ））， Ｐ（ｋ） ＝ （Ｉ － Ｋ（ｋ）Ｈ（ｋ））Ｐ － （ｋ）． （６） 式中：Ｘ ＾ － （ｋ）为 ｋ 时刻使用系统状态方程对状态的 预测，Ｘ ＾ （ ｋ － １） 为 ｋ － １ 时刻状态的最优估计值， Ｐ － （ｋ）为 ｋ 时刻对误差协方差的预测，Ｐ（ ｋ－１）为上 一时刻计算得到的误差协方差，Ｘ ＾ （ｋ）为 ｋ 时刻状态 的最优估计值，Ｐ（ｋ）为 ｋ 时刻更新得到的误差协方 差，Ｋ（ｋ）为 ｋ 时刻滤波器增益． ３ 数值仿真 由于多 ＡＵＶ 编队拓扑为动态拓扑， 所以在 ＭＡＴＬＡＢ 仿真时，对于每一次采样都重新计算拉普 拉斯矩阵 Ｌ（ｋ）、系统矩阵 Ａ（ ｋ）和观测矩阵 Ｈ（ ｋ）． 将相互有通信联系的智能体在拓扑矩阵中相应矩阵 元素置为 １，失去联系的智能体其相应矩阵元素置 为 ０，以表示时变矩阵． 设虚拟领航者方程为 ｘ０（ｔ） ＝ ［２（ｍ ／ ｓ） ２（ｒａｄ） ２（ｒａｄ）］ Ｔ ． （７） 取输出矩阵 Ｈ＝ Ｉ７×７ ；Ｗ、Ｖ 的协方差分别为 Ｑ ＝ ２Ｉ７×７ ，Ｒ＝ ２Ｉ７×７ ．仿真步长 ΔＴ ＝ ０．１ｓ．数值仿真结果如 图 １～３ 所示． 图 １ 领航者方程为式（７）时，７ 个智能体无滤波时的速 度一致性 Ｆｉｇ．１ Ｃｏｎｓｅｎｓｕｓ ｏｎ ｖｅｌｏｃｉｔｙ ｏｆ ７ ａｇｅｎｔｓ ｗｉｔｈｏｕｔ ｆｉｌｔｅ⁃ ｒｉｎｇ ｗｈｅｎ ｔｈｅ ｌｅａｄｅｒ ｅｑｕａｔｉｏｎ ｉｓ Ｅｑｕ．（７） 图 ２ 领航者方程为式（７）时，７ 个智能体滤波后的速度 一致性 Ｆｉｇ．２ Ｃｏｎｓｅｎｓｕｓ ｏｎ ｖｅｌｏｃｉｔｙ ｏｆ ７ ａｇｅｎｔｓ ｗｉｔｈ Ｋａｌｍａｎ ｆｉｌｔｅｒ ｗｈｅｎ ｔｈｅ ｌｅａｄｅｒ ｅｑｕａｔｉｏｎ ｉｓ Ｅｑｕ．（７） （ａ）原图 （ｂ）局部放大图 图 ３ 领航者方程为式（７）时，滤波前后智能体 １ 的速度 对比 Ｆｉｇ．３ Ｃｏｎｓｅｎｓｕｓ ｏｎ ｖｅｌｏｃｉｔｙ ｏｆ ａｇｅｎｔ⁃１ ｗｉｔｈ ｆｉｌｔｅｒｉｎｇ Ｖｓ ｗｉｔｈｏｕｔ ｆｉｌｔｅｒｉｎｇ ｗｈｅｎ ｔｈｅ ｌｅａｄｅｒ ｅｑｕａｔｉｏｎ ｉｓ Ｅｑｕ．（７） 设虚领航都方程为 ｘ０（ｔ） ＝ ｓｉｎ（ωｔ）Ｉ３×３ ， （８） ｂ０ 为常数矩阵， Ｌ（ｔ）为时变拉普拉斯矩阵．则系统 方程的解为 Ｘ（ｔ） ＝ ｅ （Ｌ（（ｔ） －（Ｉ（）（ｔ－ｔ０ ）Ｘ（ｔ ０ ） ＋ ∫ ｔ ｔ０ ｂ０ ｘ０（ｔ）ｄτ ＋ Ｗ（ｔ）． ·３４６· 智 能 系 统 学 报 第 ８ 卷