CN103020427A - 基于红外测距的微机器人粒子滤波定位方法 - Google Patents

Abstract

本发明提供一种基于红外测距的微机器人粒子滤波定位方法，包括步骤：步骤一：根据状态先验分布建立初始状态粒子集；步骤二：采样；步骤三：权值计算；步骤四：状态输出；步骤五：重采样。本发明针对微机器人尺寸小的特点，采用小尺寸的红外传感器作为微型机器人的测距传感器；微机器人的存储能力有限，采用分块存储地图的方法；由于尺寸效应的影响，微机器人无法安装位置传感器，运动模型采用的是计步方法。因此本发明可以实现微机器人在结构化的环境中的自定位，而且定位效率比较高，能够保证一定的定位精度。

Description

基于红外测距的微机器人粒子滤波定位方法

技术领域

本发明涉及的是一种微型机器人定位方法，具体是一种基于红外传感器和粒子滤波估计的微型机器人定位方法。

背景技术

随着MEMS技术及其相关技术的发展，移动微型机器人已经成为机器人学的一个重要发展方向。机器人的微型化为机器人的应用开阔了更为广阔的前景，如微尺度空间内的探测、代替人在微型工厂里工作、危险废弃物的清除，微尺寸管道的检测等，这些应用极大地扩大了机器人的应用范围。为了能完成这些任务，移动微机器人必须能够在工作环境中自定位。所谓的移动机器人自定位是根据先验地图信息，结合当前位姿观测值以及传感器输入信息确定机器人位姿的过程。

定位技术是移动机器人领域中的一项核心技术，几乎所有的任务都要求机器人知道自己在环境中的位置信息。例如机器人到达目的地进行检测，需要机器人知道当前的位置从而规划一条到目标的路径。机器人在执行探索任务时，要求机器人对自身位姿的评估以确定一个集体的地带是否被探测过。从概率的角度来看，定位问题就是移动机器人状态估计问题，基于概率的定位方法具有很高的鲁棒性。当前解决方法主要集中于以贝叶斯滤波为基础理论的概率定位方法，如卡尔曼滤波，多峰值估计法，马尔科夫定位，粒子滤波等。粒子滤波方法应用于移动机器人定位研究，即Monte CarloLocalization(MCL)方法，近十年来受到广泛的关注。

对现有技术的文献检索发现，当前存在很少的微机器人自主定位方法，公开文献有：Buguera等人在SENSORS（传感器杂志）2009年发表的《Sonar Sensor Models and TheirApplication to Mobile Robot》（声纳传感器模型及其在机器人上的应用）和Biber等人在Proceedings of the 2003 IEEE/RJS International Coference on IntelligenceRobots and Systems（智能机器人和***、IEEE2003关于智能机器人和***的会议）发表的《The Normal Distributions Transform:A New Approach to Laser Scan Matching》（正态分布转换：一种新的激光扫描匹配方法）。以上两文所涉及的对象都是相对比较大机器人，所采用的基于声纳传感器和激光传感器粒子滤波的定位方法，声纳传感器和激光传感器尺寸很大故很难应用到尺寸在数十个立方厘米之内的微型机器人上。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于红外测距的微机器人粒子滤波定位方法。

根据本发明的一个方面，提供一种基于红外测距的微机器人粒子滤波定位方法，包括如下步骤：

步骤一：根据状态先验分布建立初始状态粒子集；

步骤二：采样，具体为：根据运动模型，采样得到新粒子集；

步骤三：权值计算，具体为：根据感知模型计算所述新粒子集中粒子的权值，并以所述新粒子集中的粒子、以及所述粒子的权值为元素得到含权重的粒子集；

步骤四：状态输出，具体为：根据所述含权重的粒子集、以及状态后验分布，计算状态；

步骤五：重采样，具体为：根据所述粒子的权值从所述含权重的粒子集中重新抽取粒子。

优选地，所述运动模型采用的是计步方式。

优选地，所述感知模型是采用红外测距扫描提取***状态的粒子特征，进行地图匹配而获得的，地图采用的是栅格地图，其中，基于传感器获得的范围数据和地图信息来预测当前粒子的姿态的概率并获取当前粒子的权值。

优选地，所述栅格地图采用成块存储的方式，存储块的大小由红外测距传感器的最大范围r_max决定，存储块的外接圆的半径为r_max，整个地图由相切的数个存储块组成。

优选地，根据机器人的所述运动模型确定使用的地图从而快速加载块地图进行粒子特征匹配。

我们假设微型机器人在平面内作业，工作在结构化的小于1立方米环境之内。

作为优选的技术方案，在所述步骤二中，微机器人采用计步的方法存储机器人的运动信息。因为微机器人采用步进马达驱动，所以允许其采用计步的方法存储机器人的运动信息，根据运动信息采样得到新粒子集。作为优选的技术方案，在所述步骤三中，采用红外测距扫描提取***状态的粒子特征，红外的中心角要比较小，用多个红外测距传感器以保证其360度的辐射范围。结构化的环境地图采用成块存储，存储块的大小由红外测距传感器的最大范围r_max决定，存储块的外接圆的半径为r_max，整个结构化环境由相切的数个存储块组成。根据机器人的运动信息决定用哪块地图从而快速加载块地图进行粒子特征匹配。

与现有技术相比，本发明采用基于红外测距的微机器人粒子滤波定位方法在结构化的环境中实现微机器人的自定位。针对微机器人尺寸小的特点，采用小尺寸的红外传感器作为微型机器人的测距传感器，感知模型采用的红外测距地图匹配方式比较适合尺寸小的微型机器人；微机器人的存储能力有限，采用分块存储地图的方法，通过结构化的全局地图成块读取方式，节省了存储空间，便于微机器人的快速加载匹配，从而加速定位过程；由于尺寸效应的影响，微机器人无法安装位置传感器，采用计步的方法存储机器人的运动信息比较适合微型机器人。因此本发明可以实现微机器人在结构化的环境中的自定位，而且定位效率比较高,能够保证一定的定位精度。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

设X为被估计量，其先验分布为p(X)，Z_1:k为X的k个观测值，其条件概率密度函数为p(Z_1:k|X)，则可利用贝叶斯公式得到后验概率密度函数

贝叶斯公式：

$p\left(X\right|Z_{1:k})=\frac{p\left(Z_{1:k}\right|X)p\left(X\right)}{p\left(Z_{1:k}\right)}$

步骤一：初始化：对于k＝0，

根据状态先验分布p(X₀)建立初始状态粒子集

其中

N为粒子的总数；

步骤二：对于k＝1,2,...

采样：从状态转移概率（也称运动模型）

采样得到新粒子集

运动模型所采用的是：因为机器人没有位置传感器，所以在本实施例中优选地采用开环的方法，计步的方法，每个时间步，假设从上一步的状态运动到此状态，没有外部的干扰，则

$\mathrm{\Δ\θ}=\frac{\mathrm{\Δ}{s}_r-\mathrm{\Δ}{s}_l}{2R},$ $\mathrm{\Δd}=\frac{\mathrm{\Δ}{s}_r+\mathrm{\Δ}{s}_l}{2},$ $\mathrm{\Δx}=\mathrm{\Δd}\·\cos (\mathrm{\θ}+\frac{\mathrm{\Δ\θ}}{2}),$ $\mathrm{\Δy}=\mathrm{\Δd}\·\cos (\mathrm{\θ}+\frac{\mathrm{\Δ\θ}}{2}),$ θ(k)＝θ(k-1)+Δθ

其中：

Δs_r为在一个时间步里右轮平移的距离，Δs_l为在一个时间步里左轮平移的距离；

Δd为在一个时间步里在局部框架下，位移变化量，Δθ为在一个时间步里在局部框架下，角度的变化；

Δx为在一个时间步里在全局框架下，位移变化量，Δy为在一个时间步里在全局框架下，角度的变化；

θ(k)为上一时刻的方向角，θ(k-1)为此时刻的方向角；

R为两轮间距的一半。

步骤三：对于k＝1,2,...

权值计算：采用红外测距扫描提取***状态的粒子特征，红外的中心角要比较小，用多个红外测距传感器以保证其360度的辐射范围。结构化的环境地图采用成块存储，存储块的大小由红外测距传感器的最大范围r_max决定，块的外接圆的半径为r_max，整个结构化环境由相切的数个块组成。根据机器人的运动信息决定用哪块地图从而快速加载块地图进行粒子特征匹配，从而得到根据式

（也称感知模型）计算粒子

的权

并进行归一化

步骤四：根据含权重的粒子集状态后验分布p(X_k|Z_1:k)，状态估计为 ${\hat{X}}_k=E\left(X_k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^{\left(i\right)}{\tilde{X}}_k^{\left(i\right)}$

步骤五：根据粒子的权从粒子集重新抽取N个粒子

并令 $w_k^{\left(i\right)}=\frac{1}{N},$ 则建立新的粒子集 ${\{X_k^{\left(i\right)},\frac{1}{N}\}}_{i=1}^N.$

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (5)

1.一种基于红外测距的微机器人粒子滤波定位方法，其特征在于，包括如下步骤：

步骤一：根据状态先验分布建立初始状态粒子集；

步骤二：采样，具体为：根据运动模型，采样得到新粒子集；

步骤三：权值计算，具体为：根据感知模型计算所述新粒子集中粒子的权值，并以所述新粒子集中的粒子、以及所述粒子的权值为元素得到含权重的粒子集；

步骤四：状态输出，具体为：根据所述含权重的粒子集、以及状态后验分布，计算状态；

步骤五：重采样，具体为：根据所述粒子的权值从所述含权重的粒子集中重新抽取粒子。

2.根据权利要求1所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，所述运动模型采用的是计步方式。

3.根据权利要求1所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，所述感知模型是采用红外测距扫描提取***状态的粒子特征，进行地图匹配而获得的，地图采用的是栅格地图，其中，基于传感器获得的范围数据和地图信息来预测当前粒子的姿态的概率并获取当前粒子的权值。

4.根据权利要求3所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，所述栅格地图采用成块存储的方式，存储块的大小由红外测距传感器的最大范围r_max决定，存储块的外接圆的半径为r_max，整个地图由相切的数个存储块组成。

5.根据权利要求4所述的基于红外测距的微机器人粒子滤波定位方法，其特征在于，根据机器人的所述运动模型确定使用的地图从而快速加载块地图进行粒子特征匹配。