专利名称：基于红外测距的微机器人粒子滤波定位方法
技术领域：
本发明涉及的是一种微型机器人定位方法，具体是一种基于红外传感器和粒子滤波估计的微型机器人定位方法。
背景技术：
随着MEMS技术及其相关技术的发展，移动微型机器人已经成为机器人学的一个重要发展方向。机器人的微型化为机器人的应用开阔了更为广阔的前景，如微尺度空间内的探测、代替人在微型工厂里工作、危险废弃物的清除，微尺寸管道的检测等，这些应用极大地扩大了机器人的应用范围。为了能完成这些任务，移动微机器人必须能够在工作环境中自定位。所谓的移动机器人自定位是根据先验地图信息，结合当前位姿观测值以及传感器输入信息确定机器人位姿的过程。定位技术是移动机器人领域中的一项核心技术，几乎所有的任务都要求机器人知道自己在环境中的位置信息。例如机器人到达目的地进行检测，需要机器人知道当前的位置从而规划一条到目标的路径。机器人在执行探索任务时，要求机器人对自身位姿的评估以确定一个集体的地带是否被探测过。从概率的角度来看，定位问题就是移动机器人状态估计问题，基于概率的定位方法具有很高的鲁棒性。当前解决方法主要集中于以贝叶斯滤波为基础理论的概率定位方法，如卡尔曼滤波，多峰值估计法，马尔科夫定位，粒子滤波等。粒子滤波方法应用于移动机器人定位研究，即Monte CarloLocalization(MCL)方法,近十年来受到广泛的关注。对现有技术的文献检索发现，当前存在很少的微机器人自主定位方法，公开文献有Buguera等人在SENSORS (传感器杂志)2009年发表的《Sonar Sensor Modelsand TheirApplication to Mobile Robot))(声纳传感器模型及其在机器人上的应用)和 Biber 等人在 Proceedings of the 2003 IEEE/RJS International Coference onIntelligenceRobots and Systems (智能机器人和系统、IEEE2003关于智能机器人和系统的会议)发表的《The Normal Distributions Transform:A New Approach to Laser ScanMatching》(正态分布转换一种新的激光扫描匹配方法)。以上两文所涉及的对象都是相对比较大机器人，所采用的基于声纳传感器和激光传感器粒子滤波的定位方法，声纳传感器和激光传感器尺寸很大故很难应用到尺寸在数十个立方厘米之内的微型机器人上。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于红外测距的微机器人粒子滤波定位方法。根据本发明的一个方面，提供一种基于红外测距的微机器人粒子滤波定位方法，包括如下步骤步骤一根据状态先验分布建立初始状态粒子集；步骤二 采样，具体为根据运动模型，采样得到新粒子集；
步骤三权值计算，具体为根据感知模型计算所述新粒子集中粒子的权值，并以所述新粒子集中的粒子、以及所述粒子的权值为元素得到含权重的粒子集；步骤四状态输出，具体为根据所述含权重的粒子集、以及状态后验分布，计算状态；步骤五重采样，具体为根据所述粒子的权值从所述含权重的粒子集中重新抽取粒子。优选地，所述运动模型采用的是计步方式。优选地，所述感知模型是采用红外测距扫描提取系统状态的粒子特征，进行地图匹配而获得的，地图采用的是栅格地图，其中，基于传感器获得的范围数据和地图信息来预测当前粒子的姿态的概率并获取当前粒子的权值。优选地，所述栅格地图采用成块存储的方式，存储块的大小由红外测距传感器的最大范围rmax决定，存储块的外接圆的半径为rmax，整个地图由相切的数个存储块组成。优选地，根据机器人的所述运动模型确定使用的地图从而快速加载块地图进行粒子特征匹配。我们假设微型机器人在平面内作业，工作在结构化的小于I立方米环境之内。作为优选的技术方案，在所述步骤二中，微机器人采用计步的方法存储机器人的运动信息。因为微机器人采用步进马达驱动，所以允许其采用计步的方法存储机器人的运动信息，根据运动信息采样得到新粒子集。作为优选的技术方案，在所述步骤三中，采用红外测距扫描提取系统状态的粒子特征，红外的中心角要比较小，用多个红外测距传感器以保证其360度的辐射范围。结构化的环境地图采用成块存储，存储块的大小由红外测距传感器的最大范围rmax决定，存储块的外接圆的半径为rmax，整个结构化环境由相切的数个存储块组成。根据机器人的运动信息决定用哪块地图从而快速加载块地图进行粒子特征匹配。与现有技术相比，本发明采用基于红外测距的微机器人粒子滤波定位方法在结构化的环境中实现微机器人的自定位。针对微机器人尺寸小的特点，采用小尺寸的红外传感器作为微型机器人的测距传感器，感知模型采用的红外测距地图匹配方式比较适合尺寸小的微型机器人；微机器人的存储能力有限，采用分块存储地图的方法，通过结构化的全局地图成块读取方式，节省了存储空间，便于微机器人的快速加载匹配，从而加速定位过程；由于尺寸效应的影响，微机器人无法安装位置传感器，采用计步的方法存储机器人的运动信息比较适合微型机器人。因此本发明可以实现微机器人在结构化的环境中的自定位，而且定位效率比较高，能够保证一定的定位精度。
具体实施例方式下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。设X为被估计量，其先验分布为P (X)，Z1:k为X的k个观测值，其条件概率密度函数为P (z1:k IX)，则可利用贝叶斯公式得到后验概率密度函数
贝叶斯公式
权利要求
1.一种基于红外测距的微机器人粒子滤波定位方法，其特征在于，包括如下步骤 步骤一根据状态先验分布建立初始状态粒子集； 步骤二 采样，具体为根据运动模型，采样得到新粒子集； 步骤三权值计算，具体为根据感知模型计算所述新粒子集中粒子的权值，并以所述新粒子集中的粒子、以及所述粒子的权值为元素得到含权重的粒子集； 步骤四状态输出，具体为根据所述含权重的粒子集、以及状态后验分布，计算状态； 步骤五重采样，具体为根据所述粒子的权值从所述含权重的粒子集中重新抽取粒子。
2.根据权利要求1所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，所述运动模型采用的是计步方式。
3.根据权利要求1所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，所述感知模型是采用红外测距扫描提取系统状态的粒子特征，进行地图匹配而获得的，地图采用的是栅格地图，其中，基于传感器获得的范围数据和地图信息来预测当前粒子的姿态的概率并获取当前粒子的权值。
4.根据权利要求3所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，所述栅格地图采用成块存储的方式，存储块的大小由红外测距传感器的最大范围rmax决定，存储块的外接圆的半径为rmax，整个地图由相切的数个存储块组成。
5.根据权利要求4所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，根据机器人的所述运动模型确定使用的地图从而快速加载块地图进行粒子特征匹配。
全文摘要
本发明提供一种基于红外测距的微机器人粒子滤波定位方法，包括步骤步骤一根据状态先验分布建立初始状态粒子集；步骤二采样；步骤三权值计算；步骤四状态输出；步骤五重采样。本发明针对微机器人尺寸小的特点，采用小尺寸的红外传感器作为微型机器人的测距传感器；微机器人的存储能力有限，采用分块存储地图的方法；由于尺寸效应的影响，微机器人无法安装位置传感器，运动模型采用的是计步方法。因此本发明可以实现微机器人在结构化的环境中的自定位，而且定位效率比较高，能够保证一定的定位精度。
文档编号G06F19/00GK103020427SQ20121048459
公开日2013年4月3日 申请日期2012年11月23日 优先权日2012年11月23日
发明者陈佳品, 毛玲, 张大伟, 李振波, 唐晓宁 申请人:上海交通大学