仪 器 仪 表 学 报第 卷 %& 引 && 言 移动机器人定位就是机器人在作业环境中借助传感器信息估算自身位置和姿态的过程 从概率角度分析 移动机器人定位也是非线性随机动态系统 最优状态估计问题 可由递推贝叶斯预估方法统一描述 然而 非线性系统需要处理无穷维积分运算 甚至不存在解析解 且随着存储量

第 卷 第 期 年 月 仪器仪表学报!"#$%&#%'&& " ( 基于平方根容积机器人蒙特卡罗定位算法研究 朱奇光 张兴家 陈卫东 陈 颖 ( 燕山大学信息科学与工程学院 秦皇岛 ( 燕山大学电气工程学院 秦皇岛 ( 河北省特种光纤与光纤传感重点实验室 秦皇岛 摘 要 针对移动机器人 <&!" 定位中粒子滤, 波存在的粒子退化和粒子多样性匮乏问题 提出基于平方根容积粒子滤波的移动机器人 <&!" 定位算法 新算法采用平方根容积卡尔曼滤波精确设计粒子的重要性函数 将当前观测信息融入重要性采样过程 提高对真实状态后验概率的逼近程度 新算法在 <&!" 定位中直接传播及更新协方差阵的平方根因子 避免协方差阵分解与重构过程 保证协方差阵的对称性及正定性 基于排序的自适应局部重采样仅对部分粒子进行重采样 降低计算代价 增加粒子多样性 进而提高算法估计精度和一致性 实验结果表明 相同粒子条件下 新算法的计算代价比容积 <&!" 定位算法约减少 ; 不同粒子数目约多 新算法 个粒子 的估计精度高于 <&!" 定位算法 个粒子 高于容积 <&!" 定位算法 个粒子 关键词 移动机器人 <&!" 定位 5!& 律 重采样中图分类号 文献标识码 国家标准学科分类代码 ( ( -! > /! >!A!, 6"6 %%: ; 1" &'"$ :,,/ 6";%%; 1" &'"$ :,,/ <$*$)%*: *"*#'% 1" &'"$ :,,/ "!&& 5"&!&!&%",!%6!,!&%",&6&:& &!&%"#"& <&!""%!"!&!!)&%5!&!&%"#"&5!, 5" 5&<&!""%!"!&!"&,(' &!") &!)&%5!& 1!"! #"&, &!%%!&"6, &!&%"&!%#%& &&"!&&5!& #)!&, &&&!%!" % % &!!%""#&& 5!5"&6#!"&!&('!,,& &!"&&! &#!%&#&%!!%!&:!,%&"6!!&,!,,!&, <&!""%!")!& % %!,&&)%"6,%&!, %&%& #&%!!%!&:!,!!&& 6&6!, &),#&#&%!!%!&:(< &!,!&!&!"!)5!,!" 9**!") &"6%,%&&!"&!&!"!&%" %,%&%&!&!"%&"!&&!&%!&!&%",&6 &#&&!&!%%!%6!, %&%6#&!"&!,(:&"&&!& %!, & 5!& <&!""%!"!&!"& &%&!&!"%&#&,!"&,%, 56;S & &!5 #!&%"!, &5 #,&%&!&%"%!, 56(&!&!%%!%6#&,!"& &!&%"&! &!&#&<&!""%!"!&!"&&!&%"!, &!&#&5!& <&!"") %!"!&!"&&!&%"( #$5" 5& <&!""%!"!& %5!& "!" 收稿日期 )*%, -!& ) 基金项目 国家自然科学基金 河北省自然科学基金 河北省普通高等学校青年拔尖人才计划 燕山大学青年教师自主研究计划 项目资助

仪 器 仪 表 学 报第 卷 %& 引 && 言 移动机器人定位就是机器人在作业环境中借助传感器信息估算自身位置和姿态的过程 从概率角度分析 移动机器人定位也是非线性随机动态系统 最优状态估计问题 可由递推贝叶斯预估方法统一描述 然而 非线性系统需要处理无穷维积分运算 甚至不存在解析解 且随着存储量和运算量急剧膨胀 问题最优解在物理上难以实现 因此出现大量次 优的近似非线性滤波方法 其中 粒子滤波用加权粒子集表示任意的概率分布 很好地解决非线性非高斯系统预估问题 ) 成为一种有效 广泛应用的非线 性滤波方法 :(- 等人最早将粒子滤波应用于移动机器人定位研究 即 < &%!""%!"!) & 但基于粒子滤波的 < 算法在理论上仍然存在不足 一是经典 < 利用观测似然评估粒子的重要性 并选择先验概率作为重要性函数 而先验粒子在传播时没有考虑当前观测信息 预测粒子集有可能分布于观测似然函数尾部 导致粒子集退化 二是为避免粒子退化进行的重采样操作会导致粒子多样性匮乏等问题 近年来 为解决 < 存在的问题 学者们做了大 量的研究工作 宋宇等人利用迭代 $! 点 1 精确设计粒子的重要性函数 以迭代更新方式将当前观测信息融入重要性采样过程 使得预测粒子集集中分布于高观测似然区域 但迭代无迹粒子滤波算法只有在 $! 点集设计合理时 才能得到较好的滤波精度 而在高维状态 若参数选择不当将会引起自协方差非正定 导致滤波性能降低甚至发散 1!5!"( ; 等人采用支持向量回归方法结合粒子滤波获得重 要性函数 提高滤波精度 包瑞新等人引入扩展卡尔曼滤波生成粒子滤波重要性概率密度函数 对状态和参数同时进行估计 但扩展卡尔曼滤波需要计算雅克比矩阵 这将引入截断误差 使状态估计性能提高 不大!6((< 等人提出混合迭代粒子滤波算法 即混合无迹卡尔曼滤波和迭代扩张卡尔曼滤波产生粒子的重要性函数 使得粒子分布更加接近后验概 率密度的近似 却以实时性为代价 项学智等人提出自适应的残差系统重采样改进粒子滤波 但并未将当前观测信息融入到重要性采样过程 导致算法性能 提高受限 李庆玲等人则将基于容积卡尔曼 %5!) &!"! #"&1 的粒子滤波应用于移动机器人 < 算法 提高机器人位姿跟踪精度 且结果明显优于 < @%&, < 对于性能与其接近的 @) %&, < 还具有计算效率高 无自由参数 数值稳 定性等优点 但容积 < 算法多次进行协方差矩阵分解与重构过程 增加计算代价 同时未考虑粒子多样性匮乏问题 为解决上述移动机器人 <&!" 定位中存在的问题 在 5!& )< 算法基础上 提出基于平方根容积粒子滤波的移动机器人 < 算法 新算法利用平方根 1 产生粒子的重要性函数 将当前观测信息融入重要性采样过程 使得从重要性函数中得到的采样样本更加逼近从真实后验概率密度采样得到的样本 提高估计精度 再以基于排序的自适应局部重采样方法改进重采样步 增加估计精度 使获得相同精度所需的粒子数目减少 保持粒子多样性 '& 移动机器人平方根 -#5-) 算法 移动机器人 < 算法主要包括初始化 重要性采样 计算归一化权重及重采样 步 其中 重要性函数和重采样方法的选择对于确保 < 算法性能至关重要 因此 新算法从重要性函数和重采样两方面优化 < 算法来提高滤波精度 实现机器人精确的位姿跟踪 为研究基于概率的移动机器人定位问题 不失一般性地定义系统的含噪声状态预测方程和观测方程 % % { % 式中 % 为机器人 时刻的状态 为机器人 时刻的环境观测 和 分别为非线性系统函数和观测函数 为时间段 内的施加控制 为控制噪声 其协方差为 " 为观测噪声 其协方差为 采用平方根 1 设计每一个粒子的重要性函数 其重要性函数表达为高斯密度 %% %% % '%& 高斯权重积分的容积律方法 为实现高斯域下贝叶斯滤波 最为重要的步骤是计算高斯分布的非线性转移概率密度 即计算非线性函数的高斯权重积分 若 % E% 经非线性变换 % 后 分布的数学期望 ; 和方差 # 可由以下 个高斯权重积分计算 ; " % %,% # " % ;% ; %,%!!!&!(' 等人给出 自由度 $%!")*!,!" 5!& 律求解上述高斯权重积分 即容积变换采用等权重的 个 5!& 点的数值积分结果来逼近高斯权

第 期朱奇光等 基于平方根容积机器人蒙特卡罗定位算法研究 重积分 即 " %%,% 槡 % / 式中 5!& 点 为相互正交的点集 表示为 槡 式中 表示 维坐标系下单位圆与坐标轴的交点 代表 维的坐标轴与直角坐标系中单位圆的 个交点 如 维坐标下 5!& 点集表示为 槡 ''&- 粒子状态预测 { } 利用机器人的控制作用 及协方差平方根因子形成机器人状态及其协方差平方根因子的增广矩阵 即 % 式中 % [ ] % [ ] 分别为粒子状态及其协方差平方根因 子的增广矩阵 % 分别为 时刻的粒子状态及其协方差平方根因子 为控制噪声的协方差平方根因子 且 " 增广粒子状态服从高斯分布 % E% % 计算多维高斯分布 % % 5!& 点集 ' 式中 可由式 计算 的 % 通过机器人运动学模型 传播 5!& 点集 L G ' G ; 机器人 时刻状态预测 % 5 " % % % 5,% 式中 % 5 为索引 的粒子状态预测 协方差矩阵平方根因子预测 定义误差矩阵 槡 5 % 5 # 5 式中 为 /* 分解操作符 为 上三角阵 5 表示其协方差矩阵平方根因子预测 '&- 粒子状态更新 计算 5!& 点集 ' 5 5 % 5 利用机器人观测模型传播 5!& 点集 5 ' 5 机器人 时刻观测预测 5 " %% %,% 5 式中 5 表示索引为 的粒子观测预测 机器人观测信息协方差平方根因子预测 需计算机器人状态与观测间的互协方差和观测协方差 定义误差矩阵如下 5 槡 ' 5 槡 5 % 5 得到机器人状态与观测的互协方差 观测信息协方差平方根因子 由误差矩阵 的 /* 分解计算 # # ; 式中 为 的上三角方阵 为观测噪声协方差 的平方根因子 且 利用 1!"! 滤波更新机器人状态 % % 5 5 机器人状态的协方差平方根因子 的 /* 分解计算 即 # 式中 为滤波增益 % 由误差矩阵 为当前时刻 滤波输出 的第 个粒子状态 % 5 为粒子 的状态预测 为当前 时刻获得的含噪观测 为观测噪声协方差 的平方根因子 且 为 的上三角方阵 '& 重要性采样 利用当前观测信息对机器人状态及其协方差平方根因子进行更新 每次更新都以前次更新结果作为初始值 更新完毕后 从机器人的状态分布中采集新一代粒子 即 % E% 进而 根据观测新息及新息协方差计算新粒子的重要性权重

; 仪 器 仪 表 学 报第 卷 : '& 基于排序的自适应局部重采样 { } 5 5 为解决 < 算法存在的粒子退化问题而执行的重采样操作可能从粒子集删除好的粒子 最糟糕的情况是滤波发散 无论什么时候执行重采样 完整的历史位姿将永远失去 这导致代表过去位姿的粒子多样性的缺失 随着时间的推移 不同的粒子数目减少 即引起样本的协方差减少 这最终将导致滤波器不一致 所以 什么 时间 怎样执行重采样显得很重要 &( 等人引进粒子的有效数量 # 来评估当前粒子集在多大程度上逼近真实的后验分布 每当 # 低于预先设定的阈值 # 时执行重采样 以这一重采样原则为基础提出基于排序的自适应局部重采样方法!,!&!&!"!)5!, )!" 9** 9** 思想仅对高权重粒子执行重采样操作 取代低权重粒子 中等权重粒子不进行重采样 9** 方法包括 步 将粒子分为中等 可忽略 主要 类 决定每个粒子被复制的次数 9** 的第 步 每个粒子的权重分别与高阈值! 和低阈值! " 进行比较 认为权值在两个阈值之间的是中等粒子且不被重采样 则被重采样的粒子缓解退化问题 未被重采样的粒子保持粒子的多样性 高于! 和低于! " 的粒子数量分别记为 9 9 " 被重采样的粒子的权重总数计算 9 9 " "! 式中 选择满足条件! =! 或! A! " 的 第 步 权重满足! =! 或! A! " 的粒子的排序由其权重决定 然后 9** 给每个粒子一个选择概率 如果最不合适个体的排序定义为零 最合适个体的排序定义为 然后个体 的选择概率定义 )!!! 式中! 和! 分别是在每一代最坏个体的后代数量和最好个体的预期后代数量 当粒子数量给定时!! 且! 9** 依据选择概率只对部分粒子执行重采样 且能通过控制阈值来保持一定程度的粒子多样性或减低退化 即能进行快速重采样 '& 基于改进粒子滤波的移动机器人 -) 算法 将提出的改进粒子滤波算法集成在经典 < 中 形成基于平方根容积粒子滤波的移动机器人 < 算法 算法的执行流程图如图 所示 图 提出的算法流程图 ( "%!&#&,!"& & 实验与仿真研究 %&/-5 算法性能验证 验证平方根容积粒子滤波!)&%5!&!&) %!"#"&$*9 在非线性非高斯系统中状态估计的性能 并与粒子滤波!&%!"#"&9 扩展粒子滤波 :) &,,!&%!"#"&=9 无迹粒子滤波 %&,!) &%"#"9 容积粒子滤波 %5!&!&%!"#5& 9 算法进行比较 在 <(;( 平台 计算机主频 ( 进行仿真实验 实验所用系统模型和测量模型为 ' $! {! = 式中 ' 是服从 分布的随机变量 观测噪声! 服从高斯分布 实验中粒子数目为 时间步长 ( 算法性能验证使用累加均方根误差的均值和方差进行比较 9=9@99$*9 的状态估计曲线如图 所示 表 为这 种算法运行 次的均方根误差的均值和方差

#第 $ 期 朱奇光 等!基于平方根容积机器人蒙特卡罗定位算法研究 #F!F 图 %#各滤波器的状态估计图 L @ % M*- - :, D : 5C 15 图!#传统 `)N算法定位结果"'&& 个粒子# 表 = 种算法运行 ]] 次的累加均方根误差的均值和方差!$% &!C&-& '')F#, '( / C&( ( "-"% /, F&GHR> L @!N \, 1 5 D, `)N *:"'&&? # @ #= [, ')@% *#, / C-# "'', '* ]] /, -& 算法 均方根误差的均值 均方根误差的方差 WL $("!' &"' "J% awl!'( $F &'( J!! dwl 'F% ($ &'" FJ' )WL &F% (' &&J!$F 6)WL &F& 'F &&J '&( 图 ' 和表 ' 表明 6)WL得到的状态估计值更加准 确&即 6)WL算 法 性 能 明 显 优 于 WL$ awl$ dwl& 略 优 于 )WL' :; 9提出的 HEA算法的实验与仿真研究 验证提出的基 于 平 方 根 )IL的 移 动 机 器 人 `)N 1 -`)N算法定位结果"'& 个粒子# 图 $#)1S L @$ N \, 1 5 1S 1 -`)N *:"'&? # 算法 性 能& `>MN>K仿 真 实 验 中 路 标 地 图 尺 度 为 '&& :^'&& :&仿真设定移动机器人按照规划的 一 系 列目标点行走& 设 定 相 关 参 数 为! 机 器 人 从 全 局 坐 标 " & && # 处开 始& 其 航 向 运 动 速 度 lz! :j &最大驾驶 角角速度 ^Z!& ij &车体轮 间 距 bez$ :& 环 境 观 测, A S,传 感 器& 最 大 观 测 范 围 %& :& 传感器为 最大前向 视 角 'Y& i& 控 制 周 期 %( : &环境路标观测 周期 %&& : % 控 制 噪 声 " & @! :j &! i# & 观 测 噪 声 " & @' :&' i# ' 图! l( 分别为传统 `)N算 法 " '&& 个 粒子# $)1S 1 A `)N算法和新算法" '& 个 粒 子 # 对 移 动机器人 的 定 位 结 果' " 图 中,! - 为 环 境 特 征&, '为目标点&, qqqq- 为机器人真 实 位 置&, '''' - 为 估计位置# ' 图 (#新算法的定位结果"'& 个粒子# L @ ( N \, 1 5 *-?? D # *:"'&?

仪 器 仪 表 学 报第 卷 由以上 幅图对比分析 传统 < 算法随着时间推移 粒子退化及贫化严重最终导致机器人定位失败 ) 5!& )< 算法由于采用 1 将当前观测信息融入粒子滤波的重要性采样过程 在一定程度上解决粒子退化 所以可以实现机器人位姿跟踪 但由于重采样导致的粒子贫化直接威胁到一致性 使得定位精度有限 新算法的估计轨迹与移动机器人的真实轨迹基本重合 原因不仅在于平方根 1 将当前观测信息融入粒子的重要性采样过程 而且确保所得到的协方差阵正定性和对称性 即提高数值稳定性 另外 改进的重采样步在一定程度上解决粒子贫化问题 即双方面增加一致性 最终精确实现移动机器人定位 新算法与 5!& )< 算法的机器人位置的 次仿真的机器人位置均方误差!! <$= 的平均值如图 所示 由图得 新算法的机器人位置的 9; 约为 5!& )< 算法的 原因在于 与 1 相比 平方根 1 确保所得到的协方差阵正定性和对称性 提高了数值稳定性 而且 基于排序的自适应局部重采样方法提高粒子多样性 提高估计的精度和一致性 图 机器人位置的均方误差 ( <$=#5&& 不同粒子条件下新算法和 5!& )< 算法进行机器人位姿跟踪的时间消耗及机器人位置的 9; 分别如表 和图 所示 表 '& 不同算法性能比较 '&(( ( 时间 粒子数目新算法 )< 算法 < 算法 ;( ( (; (; ;( ( ( (; ( ;( ( ( 由表 和图 可知 相同粒子条件下 5!& )< 算法耗时最长 < 算法耗时最短 其中 新算法的耗时少于 5!& )< 算法的原因在于基于排序的自适应局部重采样方法仅对少部分粒子进行重采样操作 减少算法的计算代价 相同粒子条件下新算法的机器人位置 <$= 约为 < 算法的 约为 5!& )< 算法的 即新算法要达到 < 或 5!& )< 算法相同精度时所需的粒子数目更少 由图 分析得 粒子数目的变化使得 < 算法的 <$= 波动较大 其次是 5!& )< 算法 而新算法基本受影响不大 即新算法的性能并不严重依赖于粒子数目 这是由于基于排序的自适应重采样方法保持粒子多样性 使得重采样的粒子渐近地近似来自于真实状态的后验分布的样本 也就是说新算法能更好地避免粒子贫化 因此不需要大量粒子作补偿 图 ; 记录了 个粒子条件下 新算法和 5!) & )< 算法每一次重采样后不同粒子的数目 结果显示新算法的不同粒子数目比 5!& )< 算法约多 原因在于新算法以基于排序的自适应局部重采样改进重采样步 仅对小部分粒子进行重采样 增加了粒子多样性 进一步证明新算法的一致性优于 5!& )< 算法 图 不同粒子数目的算法性能 ( 9#!%#!!"&#,#& 5#!&%" 图 ; 不同粒子的数目 (; 5 #&,&%&!&%"

第 期朱奇光等 基于平方根容积机器人蒙特卡罗定位算法研究 & 结 && 论 为解决移动机器人 < 算法中粒子滤波存在的粒子退化和粒子多样性匮乏问题 提出的基于平方根 5!) & 粒子滤波的移动机器人 < 算法在重要性函数和重采样 步进行改进 采用平方根 1 将当前观测信息融入重要性采样过程 在一定程度上解决粒子退化问题 并确保所得到的协方差阵对称性及正定性 进而提高数值稳定性 且与基于排序的自适应局部重采样方法共同提高算法的一致性 实验仿真重点与 5!& )< 算法进行性能比较 结果表明 提出算法不仅在一定程度上减少计算代价 还提高估计精度和一致性 参考文献 夏凌楠 张波 王营冠 等 ( 基于惯性传感器和视觉里程计的机器人定位 ( 仪器仪表学报 ' > &!"(*5&"%!")!&!"&5!, &!"!,,,) &6 (!"#$%&#%'&& 1*<( :!&!6! % &!&( '#!& $%% ; 林恺 石江宏 邓振淼 等 ( 基于遗传算法粒子滤波的多径跟踪 ( 电子测量技术 '1 $' -=>< &!"(<"&!& &!& &!%5!, &%!"&!&%"#"& ( ="%&%<! &%"6 张凯 单甘霖 ( 基于高斯和与 $1 的非线性非高斯滤波算法 ( 仪器仪表学报 >1 -("! )!! #"&!"&5!,!! #"&!, $1 (!"#$%&#%'&& 郭君斌 郭晓松 雷磊 等 ( 基于改进粒子滤波算法的人眼跟踪方法 ( 仪器仪表学报 ; @? @?$ ='(&!"(=6&!%&, 5!,,!&%"#"&!"& ()!"#$%&#%'&& ;?- @**- *@$(<!"%!"!& # 5" 5&,6!%&(!"# &#%!"'&"%*!% 宋宇 孙富春 李庆玲 等 ( 移动机器人的改进无迹粒子滤波蒙特卡罗定位算 ( 自动化学报 ;);( $? $@ '/ &!"(<5" 5&& %!""%!"!& 5!,, %&,!&%" #"& ( %&!&!&%!$%! ;);( ; 1?@(!&&!%"!&%"#"&& &%& ('===! %"! %6 ; 包瑞新 贾敏 于会龙 等 ( 基于扩展 1!"! 粒子滤波的汽车行驶状态和参数估计 ( 农业机械学报?* '< @ "!&(%"&!&!,!)!& &!&,, &!& 5!, :) &,,!"!!&%"#"& &, (!!%&# &$%&6# %"&!"<!%6 < <=- >=-(65, &!&,!"!!&%"#"& # 5A%&&!% 5" ( 9%,)#&'&!&!"#% & 6!, "%!& );( 项学智 彭宇 韩志英 等 ( 基于改进粒子滤波的空间红外小目标跟踪方法 ( 电子测量与仪器学报 '> 9= > &!"(!% &, #!%'*!"&!&5!,,!&) %"#"&(!"#="%&%<! &!, ') && '/ $?(5!& < <5" 5&& %!""%!"!& 5!, %5!&!&%"#"& ( &"#% '=== ) ( <? *=@='*? /@'T 9 &!"(?)5!, ",)&%!""%!"!& (*) 5&%!, &)$6& ; -@**)= '=($"&!"%!")!&!,!9!&' (*5&%F &!& <!! '=== *$*< ' 1'$(5!& 1!"! ") & ( &!&%&" '===!!%& *'$=' $'$$ @**-(', &%#,!&!)5"!%",!&) %"#"& (*5&% '===!!%&

仪 器 仪 表 学 报第 卷 作者简介 朱奇光 年于燕山大学分别获得学士 硕士和博士学位 现为燕山大学副教授 主要研究方向为智能机器人检测与控制 =)!";;6(,(%, %, ($%( <($%(!, 9(-(,!"#!! @&6!, %&"6(!!%!& #!! @&6(!!% &&,&%&!, %&"#&"&5&( 张兴家 年于河北经贸大学获得学士学位 现为燕山大学硕士研究生 主要研究方向为机器人视觉定位 =)!";;(% :; %, ($%(, #5@&6#=%%!, (! <($%(%!,,!&!! @&6(! )!% &&)5!, 5" 5&"%!"!&( 陈卫东 通讯作者 年于燕山大学分别获得硕士和博士学位 现为燕山大学信息科学与工程学院教授 主要研究方向为智能算法及应用 =)!"(% -,!& ) %, <($%(!, 9(-(,5& #!! @) &6!, %&"6(! # '&&&#'#!& $%%!, =!! @) &6(!!% &&&"&!"&!,!) "%!&(