基于随机有限集的SLAM算法.pdf

第 卷 第 期系统工程与电子技术 年 月 文章编号 收稿日期 修回日期 基金项目 国家自然科学基金 黑龙江省博士后科研启动金 资助课题 作者简介 杜航原 男 博士研究生 主要研究方向为导航 制导与控制及同步定位与地图创建 基于随机有限集的 算法 杜航原 赵玉新 杨永鹏 韩庆楠 哈尔滨工程大学自动化学院 黑龙江 哈尔滨 摘 要 提出一种基于随机有限集的同步定位与地图创建算法 该算法利用随机有限集对环境地图和传感器 观测信息建模 建立联合目标状态变量的随机有限集 依据 估计框架 利用概率假设密度滤波的粒子滤 波实现对机器人位姿和环境地图进行同时估计 新算法避免了数据关联过程 并能更加自然有效地表达同步定 位与地图创建 问题中多特征 多观测特性及多种传感器信息 在 仿真实验中 利用 算法和新算法进行对比 实验结果验证了新算法的优越性 关键词 同步定位与地图创建 随机有限集 估计 概率假设密度滤波 粒子滤波 中图分类号 文献标志码 引 言 同 步 定 位 与 地 图 创 建 是指处于未知环境中的机器人从某一未 知起点出发 在行驶过程中通过传感器感知外部环境 构建 描述环境的地图 同时利用地图对机器人进行定位的过 程 是机器人实现真正自主的关键问题 已经成 为机器人领域的研究热点 近年来 基于 估计的 算法大量涌现 这种方法使用联合状态向量 也就是 随机向量的序列形式表示地图特征和机器人位姿信息 对 联合状态后验概率密度进行递归估计 其中比较典型的方 法主要有基于卡尔曼滤波 模型的随机 地图法 和基于序贯蒙特卡罗方法的 算 法 此外还出现了一些混合算法 以上算法都使用向量序列形式表示特征 机器人联合 状态 这种表示方式能完成对地图特征和机器人位姿的同 时估计 并且具有较为严谨的数学结构 但这类算法存在 个问题 第一 需要判断不同时刻对环境的观测是否来源 于同一 特 征 也 就 是 数 据 关 联 问 题 数 据 关 联 是 当 前 问题的一大难题 在实际应用中 数据关联的正确率 对算法的最终效果具有很大影响 同时数据关联过程带来 了巨大的计算负担 第二 为了对观测到的特征数目进行 估计需要进行特征管理 第三 如果环境中存在杂波 这种 模型无法有效表达全部传感器信息 如视场 检测概率及虚 警等 近年来 随机有限集 理论逐渐 第 期 杜航原等 基于随机有限集的 算法 被用于多目标跟踪领域 将随机变量的统计理论推广 到随机集的统计理论 它通过将传感器观测信息和多目标 状态表示成随机有限集 对全体多目标联合后验概率密度 分布进行最优 递推估计 避免了数据关联过程 能 解决复杂环境下数目变化的多目标跟踪问题 本文使用 理论对 问题进行建模 将各时刻 机器人位姿 传感器观测信息以及环境地图表示为随机有 限集 能更好地描述观测噪声 视场 检测概率及虚警等多 种传感器信息 依据 估计框架利用概率假设密度 滤波对联合后验分布进行递归估计 实现机器人位姿和地 图状态的同时估计 问题的 模型 在 时刻进行如下定义 表示机器人位姿变量 表示机器人控制变量 表 示环境地图中的第 个特征 表示由传感器获得的观测 信息 其中可能包含多个特征的观测 是由各时刻机器人位姿向量组成的序列 表示行 驶轨迹 表示机器人控制变量 序列 表示机器人获取的历史 观测信息 表示由特征组成的环境地图 地 图中特征的数目 是未知的 根据 问题的定义 时刻机器人位姿和环境特 征状态的联合后验概率密度可以表示为 如果已知 时刻的后验分布概率 和 时刻的控制输入量 以及传感器观测信息 那么 时刻后验概率密度可以利用 递归估计 获得 假设 时刻的观测模型为 在此认为一 旦机器人的位姿和环境地图确定后各环境特征间的观测是 独立的 假设机器人运动模型为 机器人的 状态服从马尔可夫过程 即 时刻状态 只与上一时刻状 态 和当前控制变量 有关 而与观测信息独立 利用 理论对联合后验概率密度进行递归估计 包括以下两个过程 预测 更新 在传统的 递归估计中 式 表示包含地图状 态 和机器人位姿 的联合状态向量的后验概率密度 该向量中每个分量的顺序一经确定不再改变 而观测信息 也是由 视场 中特征的多个观测值组成的向量 在使用 更新前需要将两个向量中的分量 即特征及其观 测值 进行对应 这就是数据关联问题 目前 关于数据关 联虽然已经有了大量研究成果 但尚未出现一种兼顾 精度和计算效率的方法 而且 为了估计观测信息中包含 的特征数需要进行特征管理 这一过程可能包含在数据关 联中 或者使用网格填充法实现 此外 由于基于特征向 量的观测模型忽略了观测的不确定性 漏检以及杂波引起 的虚警 算法实际效果受到很大影响 基于 的 算法 基于 理论的 模型 理论最早于 年由 提出 经过 近年来的不断完善 在越来越多的领域受到广泛关注 在 问题中 随着环境特征在传感器视场中的出现 消 失 地图中的特征数目和特征状态是随时间变化的 在观测 噪声 检测概率以及杂波的影响下 传感器获得的观测信息 有可能是随机变化的 因而 使用 可以更自然 有效 地对传感器观测信息和地图状态进行描述 利用 理论可将 时刻地图状态表示为 时刻地图的状态集 由两部分构成 表 示上一时刻的地图特征状态在 时刻仍存在的状态 表示 时刻新加入地图的特征对应的 假设这些 随机集之间是相互独立的 如果环境保持静止 式 也可 以写为 利用 理论可将 时刻观测信息表示为 式中 是源于环境特征 的随机集 是源于 虚假观测的随机集 由于传感器具有一定的检测概率 中包含的元素数目可能与 时刻实际出现在传感器视场中 的特征数不一致 这里 假设 和 是相互独 立的 相比状态向量序列的表示形式 这种基于 的观 测信息模型能真实反映观测噪声 视场 漏检以及虚警等多 种传感器信息 基于 的 递归估计 特征 产生的 可以视为 即对于 任意 以概率 满足 系统工程与电子技术第 卷 以概率 满足 其中 表示机器人位姿为 时传感器观测到一个特征 位于位置 的概率 根据 统计理论 给定机器人位姿 和地图状态集 传感器产生观测集合 的概率密度 即多目标联合似然函数表示为 式中 为观测随机集 关于机器人位姿 和 地图状态 的条件概率密度 其中蕴含了观测信息与机器 人位姿间的关系以及观测不确定性和噪声 为产生 虚假观测随机集 的概率密度 通常这个密度由先验知识 获得 基于 理论的 后验概率密度 递 归估计可以改写为 式中 表示近似 测度 相比式 和式 的 递归估计 式 和式 最大的不同在于地图状 态 和观测信息 表示的是 将这种新的 算法 称为 由于联合似然函数 的计算 量随着加入地图的特征数目增加呈指数上升 严重影响算 法实时性 所以必须寻求 估计的次优解 概率假设密度滤波 估计 概率假设密度 滤波在每次迭代时利用容易求取的后验密度函数的期望代 替多目标随机集合的后验概率密度 可有效降低计算 负担 假设随机有限集 的概率分布函数为 对应的 概率密度函数为 则 的 函数 定义为 同时满足 由以上两式可以看出 在区域 上的积分也就是 有限集 中落入该区域内的元素个数的期望 因此 集合 中包含的元素数目为 这些元素 的状态矢量对应于 的 个局部极大值点 这样 通 过对地图状态随机集 的 求积分获得地图特征数目 同时将 的 个局部极大值矢量作为对环境地图 中特征的状态估计 以上模型实现了对地图状态的估计 为了同时获取机 器人位姿状态 对 时刻机器人位姿采样 个粒子 将每 个地图特征和某一机器人位姿粒子组成的联合状态变量记 作 全部 个联合状态 构成了联合状态 如果已知机器人的确切路径 每个环境特征状态估计之间 是相互独立的 因此通过对 进行 滤波迭代估计 从而实现机器人位姿和地图状态的同时估计 假设对机器 人位姿的采样是 点过程 环境地图状态服从 分布 那么联合状态变量 对应的随机集也是服从 分布的 由此 联合状态随机集的 滤波迭 代公式表示为 式中 表示联合状态由 到 的转移概率 密度函数 表示对于环境特征观测的联合似然函 数 表示对于联合状态 在某一机器人位姿采样下 观测到对应环境特征的概率 和 分别表示 时刻新进 入传感器视场的特征产生的随机集和虚假观测产生的随机 集对应的 由于环境是保持静止的 式 可以写成 利用粒子滤波实现 的 滤波估计 式 式 的 滤波迭代过程中存在难以求 解的积分运算 粒子滤波本质上是一种蒙特卡罗采样方法 可用于实现 滤波迭代 时 初始化一组粒子集 来表示 当 时 每一时刻 及联合状态的估计如下 预测 烅 烄 烆 式中 为新生粒子数 重要性采样函数 可取为联合状态的状态转移密度函数 为新特征出现在传感器视场中的概率密 度函数 如果缺乏先验信息 可以取为均匀分布 计算粒子权重 第 期 杜航原等 基于随机有限集的 算法 烅 烄 烆 更新 时刻传感器获得观测信息随机集 对每个 计算 对 更新权重 式中 为每一时刻传感器产生 虚假观测的期望 在此假设其服从 分布 重采样 计算联合状态随机集中的元素数目 对粒子集 进行重采样 得到新的粒子 集 状态提取 计算特征 位姿联合状态变量数目 其中 为生成联合状态变量时生成的机器人位姿采样 点数 通过聚类将粒子集分为 个集合 对任意 满足 每个 的聚类中心为 则 为 联合状态的估计值 仿 真 仿真参数设置 仿真是在如图 所示的二维平面矩形区域中进行的 该区域中包含 个特征 路标 点 用 表示 机器人由 点出发 沿预定路线按顺时针方向行驶至终点 机 器人在行驶过程中通过传感器不断对环境特征进行观测 获取 角 度 和 距 离 信 息 仿 真 选 取 估 计 精 度 较 高 的 算 法 以 下 简 称 与 本 文 的 算法进行对比 仿真参数设定如下 机器人行 驶速度为 最大舵角为 线速度误差为 角速度误差为 传感器的视场为前向 距离 的区域 检测概率为 传感器的测距和测角误差分别 为 和 设环境中的杂波在视场区域内服从均值为 的 分布 在这样的参数设定可以较好地对杂 波环境中传感器噪声强度较大时算法的估计性能进行比 较 为避免实验随机性造成的影响 对两种算法都进行 次独立重复实验 将统计数据作为实验结果 两种算法 中粒子滤波的粒子数都取 图 仿真区域 仿真结果及分析 图 为使用两种算法得到的各时刻观测信息包含的特 征数目均值与实际出现在视场中的特征数目的比较情况 由图 可知 算法估计得到的特征数目存在较 大的误差 误差的最大值达到 这主要是由于 以随机向量序列形式表示联合状态 只能对传感器观测噪 声进行表达 特征数目的估计过程由数据关联完成 而环境 中存在的杂波以及观测的不确定性会对数据关联过程造成 较大影响 所以特征估计数目误差较大 获得 的特征估计数目与真实特征数目非常接近 这是由于利用 理论对 问题建模能有效表达视场 噪声 漏检 及虚警等多种传感器信息 并且避免了数据关联过程 其估 计误差主要来源于联合目标状态提取时的取整运算 图 特征数目估计情况对比 利用 次独立重复实验获得的实验数据计算每一时 刻机器人定位和环境地图估计的均方根误差 系统工程与电子技术第 卷 使用两种 算法获得的每一 时刻机器人定位的 和环境地图状态估计 分 别如图 和图 所示 由图可以看出 使用 获得的机器人定位误 差和环境地图状态估计误差较大 其定位误差最大达到 特征位置估计误差最大达到 而使用 获得的机器人定位误差和环境地图状态估计误差都 保持在较低水平 造成这一结果的原因与特征数目估计类 似 随机特征序列的建模方式忽略了观测的不确定性 较大 的传感器观测噪声以及环境中的杂波降低了数据关联的正 确率 而 的状态估计以数据关联为前提和基础 由于避免了数据关联 利用 理论建立涵盖 了环境特征和传感器全部观测信息的联合状态 在模 型建立上更加准确 因此虽然两种算法最终都通过估计精 度较高的粒子滤波进行实现 但是 具有更好的 算法性能 结 论 本文以随机有限集理论为基础 建立了 问题的 模型 不同于传统 算法的随机向量序列形式 本文将环境地图和