基于粒子群优化卡尔曼滤波的水下机器人信号处理

第1期
船
海
工
程
第 39 卷
估计值 x ^k 。卡尔曼滤波算法的计算回路见图 1。
在的位置 , 整个搜索过程结束。最后输出的就是 gBest , 即算法找到的全局最优解。粒子群优化算 法的框架见图 2。
图1
卡尔曼滤波的两个计算回路
图 1 表明卡尔曼滤波具有两个计算回路: 增 益计算回路和滤波计算回路, 增益计算回路是独 立计算回路, 而滤波计算回路依赖于增益计算回 路。 1. 2 粒子群优化算法 以往卡尔曼滤波器系数是在假设噪声特性的
i= 1 m
法通过适应值来衡量粒子的优劣, 然后各个粒子 就追随当 前 的 最 优 粒 子 在 解 空 间中 搜 索。令 PSO 初始化为一群随机粒子 , 在每一次迭代中粒 子通过跟踪两个 ∀ 极值# 来更新自己: 一个是粒子 本身所找到的最优解称为 个体极值点 ( 用 gBest 表示其位置) ; 另一个是整个粒子群目前找到的最 优解 , 称作全局极 值点 ( 用 p Best 表示 其位置 ) 。 粒子在找到这两个极值后 , 就根据下面两个公式 更新自己的速度 v t + 1 和位置 ! t+ 1 . v t+ 1 = ∀ v t + c1 r and 1 ( p Best t - ! t)+
由于海洋深处工作环境的复杂性、 不可预测 性, 机器人在实际工作中不可避免地要受到各种 随机噪声的影响, 因此水下机器人的信号处理技 术就显得尤为重要。 卡尔 曼 滤波 作 为 一 种 有 效 的信 号 处 理 方 法 , 在量测噪声甚至存在轻微非线性的情况下 仍然非常有效, 但标准的卡尔曼滤波方法必须清 楚地知道系统噪声和量测噪声的统计特性。由于 相关传感器受各种因素影响波动很大 , 噪声的统 计特性不易获得, 从而限制了卡尔曼滤波方法的 应用。对水下机器人来说 , 水下环境更为复杂多 变, 干扰信号的统计特性更难预先获取 , 这必然影 响卡尔曼滤波在水下机器人传感器信号处理中的 应用。为此, 在普通卡尔曼滤波器基础上引入粒 子群优化算法, 探索水下机器人信号处理新的滤 波方法, 以提高水下机器人传感器测量数据处理 精度。
表 1 卡尔曼改进前后均 方误差表 对比项 K P KP SO P 对比项 K P 采样点 k= 1 2 2 k= 2 k= 3 k= 4 k= 5 k= 6 k= 7 k= 8 k= 9 k = 10 0. 748 5 0. 634 9 0. 619 2 0. 616 9 0. 616 6 0. 616 6 0. 616 6 0. 616 6 0. 616 6 0. 659 5 0. 402 6 0. 297 4 0. 242 3 0. 209 8 0. 189 2 0. 175 5 0. 166 2 0. 159 8 采样点
( 8) ( 9)
式中 : v !
粒子的速度; 粒子当前的位置, 为了防止粒子远离 搜索空间, 粒子每一维的速度和搜索 范围一般都会钳制在一个范围内;
rand1 、 rand2 介于( 0, 1) 之间的随机数; c1 、 c2 学习因子 , 通常 c1 = c2 = 2 ; ∀ 100 加权系数, 取值在 0 . 1~ 0. 9 之间。 粒子通过不断学习更新, 最终到达最优解所
由图 5 可见, 卡尔曼滤波后数据仍受噪声影 响较大, 粒子群优化卡尔曼滤波器输出的波动较 小, 对噪声的影响有较好的抑制作用。改进前后 均方误差比较见图 6。
图6
改进前后均方误差比较
由图 6 可见, 由于初始化的原因, 常规卡尔曼 滤波与粒子群优化卡 尔曼滤波的均 方误差均为 2. 0, 在一段时间后 , 均方误差将保持在一个固定
验室的 OU T LAND1000 机器人进行实验及数据 采集。OUT L AND1000 为一开放式 水下机器人 实验平台, 由水下实验载体部分、 传感器部分、 推 进器部分、 通信系统组成。 1) 传感器部分。深度计﹑罗经及云台, 可以 加装 1 个声呐系统。 2) 4 个推进器。其中尾部 2 个推进器实现转 向和直推功能 , 1 个侧推实现机器人横移功能 , 1 个垂直方向推进器实 控制转换器连接( RS- 232 接口 ) , OUT LAND1000 控制转换器与机器人水下载体部分通过电缆连接 ( RS- 485 接口) , 通过串行通信方式采集机器人的 水下传感器信号, 再通过串行通信方式发送水面控 制信号给水下机器人, 验证相关算法。 OU T L AND1000 通信系统示意见图 3。
] = Rk ]= 0
Cov [ wk , 式中 : Qk Rk
] = E [ wk
k, j = 0 , 1, 2 , 动态噪声协方差阵, 量测噪声协方差阵,
k
( 3)
1
1. 1
模型
原理 卡尔曼滤波是根据前一个估计值和最近一个
Qk = Var [ wk ] = E [ wk wk ] ; Rk = Var [ 随机向量。 状态 x k 的估计 x ^ k 可按卡尔曼一步递推公式 和卡尔曼滤波器基本方程求解。 递推公式 增益方程 x ^ k = Ak x ^ k- 1 + H k ( yk - BkAk x ^ k- 1 ) Hk = P! k B k ( Bk P! k Bk + Rk )
第 39 卷 第 1 期 2010 年 2 月
船 海工程 SH IP & OCEA N ENG IN EERI NG
V ol. 39 N o. 1 Feb. 2010
DOI: 10. 3963/ j. issn. 1671 7953. 2010. 01. 031
基于粒子群优化卡尔曼滤波的水下机器人信号处理
图 4 自动定向回 路结构图
的值 , 可以看出 , 基于粒子群优化算法的卡尔曼滤 波器的均方误差明显小于常规卡尔曼滤波器, 说 明前者对噪声有更好的抑制作用。 101
自动定向控制回路是机器人自动保持给定的
第1期
船
海
工
程
第 39 卷
为清楚表示 , 将均方误差值列于表 1, 由于数
据较多, 只取代表性数据。
( 4) ( 5) ( 6) ( 7)
一步预测均方误差
P! k = Ak P k - 1 Ak + Qk
估计均方误差阵 P k = ( I- Hk B k ) P! k
当已知增益矩阵 Hk , 利用前一个 xk- 1 的估计值 x ^ k- 1 和当前的量测值 yk , 就可以求得当前状态 xk 的 99
[ 1 4]
信号模型。离散时间系统状态方程定义为: 式中 : xk Ak, k- 1 wk xk = Ak, k - 1 x k - 1 + wk - 1 ( 1) 一组状态变量组成的多维状态矢量 ; k - 1 到 k 时刻的一步转移矩阵; 系统激励噪声序列。
如果用 y k 表示量测或观察到的信号矢量序 列, 则其与状态变量 x k 的关系可以写成 : 式中 : y k
- 1
] = E[
k
k
]。
系统的初始状态 x0 设定为与 wk 、 k 独立的
观察数据来估计信号的当前值。用状态方程和递 推方法进行估计 , 其解是以估计值 ( 常常是状态变 量的估计值) 的形式给出的。 由状态方程和量测方程得到卡尔曼滤波器的
收稿日期 : 2009 04 01 修回日期 : 2009 07 08 作者简介 : 王翠翠 ( 1984 ) , 女 , 硕士生。 研究方向 : 水下机器人信号处理技术。 E mail: cuicui0219@ hotmail. co m
t) c 2 r and 2 ( gBestt - ! ! t+ 1 = ! t + v t+ 1
^ ∃ [| x
k
|- | Xk | ]
2
( 10)
式中 : m 离散频率点的最大采样点数。 1. 3 用 PSO 算法设计卡尔曼滤波器的步骤 1) 初始化 PSO 算法的参数 , 包括群体大小、 参数维数及加权因子 , 等。 2) 在参数区间内随机初始化群体中各粒子 的位置与速度。 3) 利用式( 4 ) ~ ( 7 ) 及式 ( 10 ) 计算粒子的适 应度。 4) 根据式( 8) 和( 9 ) 更新粒子的位置与速度。 5) 重新计算粒子适应度。 6) 判断是否更新粒子的个体极值 p Best 以 及粒子群的全局极值 gBest 。 7) 重复 4) 至 6 ) , 直到满足精度要求或达到 预先设定的迭代次数。 8) 输出 gB est, 得到 卡 尔曼 滤 波器 的 系数 Ak、 Bk 、 Rk 、 Qk , k = 1, 2 , , n。
王翠翠, 朱大奇 , 刘 静
( 上海海事大学 水下机器人与智能系统实验室 , 上海 200135) 摘 要 : 在卡尔曼滤波的基础上 , 引入粒子群优化算法 , 对卡尔曼 滤波方法 进行改进 , 提出基 于粒子群 优
化的卡尔曼滤波器模型 , 从而提高水下机器人测 量数据的精度 , 降低 系统噪声和 量测噪声所 带来的误 差。水 池仿真试验结果表明改进的滤波方法有效、 实用 。 关键词 : 卡尔曼滤波器 ; 粒子群算法 ; 水下机器人 ; 信号处理 中图分类号 : T P18, T P274 文献标志码 : A 文章编号 : 1671 7953( 2010) 01 0099 04
1 与 # 之差作控制 出角度 , 若只用传感器输出值 ∃ 输入 , 势必受到噪声的影响 , 经过卡尔曼滤波后,
除去噪声 , 用 ∃ 2 代替 ∃ 1 进行控制, 从而降低了系 统噪声和量测噪声对控制系统的影响 , 进而提高 机器人控制性能的稳定性和精确性。 2. 2 实验数据结果分析 以 OU T LAND1000 水下机器人定向航行的 实验数据为基础, 讨论改进滤波算法在水下机器 人信号处理中的应用 效果。该实验 是一定向控 制, 设定期望航向 为 150%, 机器人根 据罗经的方 向输出信号, 依据图 4 的控制算法进行状态控制。 为了比较常规卡尔曼滤波器和粒子群优化卡尔曼 滤波器的性能 , 在罗经实际输出数据的基础上, 人 为加上白噪声干扰信号 , 再分别应用常规卡尔曼 滤波器和粒子群优化卡尔曼滤波器进行处理, 试 验所得传感器数据及滤波后数据见图 5。