CN101509781A - 基于单目摄像头的步行机器人定位系统 - Google Patents

Abstract

一种基于单目摄像头的步行机器人定位系统，环境模型采用点模型表示，标定离线地图，并建立动作模型及观测模型进行位置更新及路标标定，采用蒙特卡罗粒子滤波技术，用带有权值的粒子集分布表示机器人位姿估计，通过路标匹配处理更新粒子的权值和分布，栅格化场地粒子分布，选择粒子密集度最大的子区域定位机器人所在位置。该系统作为独立平台，其地图标定技术在环境模型上有很大改进，对地面线段的识别有很大的灵活性，在环境变换后只需重新标定，不需重新建模。该模型降低了视觉处理中匹配的复杂度，提高了计算效率。在对信息融合处理技术上对蒙特卡罗定位算法进行扩展，保证了系统的实时性和鲁棒性。

Description

基于单目摄像头的步行机器人定位系统

技术领域

本发明涉及机器人的定位系统，尤其涉及单目摄像头的步行机器人的定位系统。

背景技术

近年来随着传感器等领域的技术进步，智能机器人系统开始应用在服务行业中，开辟了机器人自主服务的新领域。以往服务机器人的研究和开发主要在大学和研究所中进行，而如今越来越受到企业界和商业界的重视。室内服务机器人的出现主要原因有：一是人们想摆脱令人烦恼枯燥的重复性工作，如家务劳动、照料病人等等；二是电器设备成本的下降。由服务机器人代替人完成家务劳动，是一项具有良好应用前景的高科技技术。在机器人系统中，自主导航是一项核心技术，是机器人研究领域的重点和难点问题。在导航过程中，常常面临无法预先知道、不可预测或动态变化的环境。移动机器人感知环境的手段通常是不完备的，传感器给出的数据是不完全、不连续、不可靠的。因此，解决机器人自主导航是一项迫不及待但又十分艰巨的任务。移动机器人导航是指移动机器人按照预先给出的任务命令，根据已知信息做出路径规划，并在行进过程中，不断感知周围的局部环境信息，自主地做出决策，调整自身位姿，引导自身安全行驶或跟踪己知路径达到目标位置。而移动机器人的定位，是机器人自动导航的首要条件。因为只有明确了自身位置，目标点才有意义，也才能在此基础上做出路径规划。

在基于单目摄像头的室内机器人中，定位传感器装置主要为摄像头传感器和里程计传感器，摄像头传感器又称主动式传感器，可灵活获得环境信息，但计算量很大，一般需要改造外部环境或建立比较精确的环境模型，将获取信息与环境模型匹配得到机器人与环境的相对位置，从而获得定位信息。人为路标的建立是机器人定位中惯用的环境识别辅助物体，方便了视觉信息中对环境的提取但通用性不佳。在室内的环境识别中，地线是环境中的另一个主要信息。当前大部分的方法是使用Hough变换从图像中提取直线形式的路面边界，并通过对比探测到的路面信息与机器人内部存储的地图，修正偏差，实现导航。

里程计传感器属于被动式传感器，其在移动的一段时间内可以得到较高定位精度，但增量方式的信息获取同时产生了误差累计。因此，多传感器数据融合的定位系统是解决机器人定位的最终途径。

多传感器的数据融合技术指将多个传感器采集的信息进行合成，形成对环境某一特征的综合描述的一种方法。使用这种方法，可使探测到的环境信息具有冗余性、互补性、实时性和低成本的特点，同时还可以避免摄像机系统中巨大的数据处理量。近年来，大多解决机器人定位的方法都是基于统计或概率估计意义下的方法，如Kalman滤波、最大概率估计、Monte Carlo方法等。其中，Monte Carlo定位是一种相对鲁棒性强的概率化定位方法，它的随机近似技术能够在不需建模的情况下对系统真实后验概率提供合理估计，其收敛问题和计算效率问题一直是解决的重点。

目前大部分的地图标定主要为几何形状的标定，基于摄像头传感器的机器人一般要通过边缘检测、最小二乘法处理边界线条，最终得到物体的几何描述后再进行模式匹配，地图结构复杂且计算量大。

发明内容

本发明的目的在于提供一种基于单目摄像头的步行机器人定位系统，以地图标定技术为基础的针对单目摄像头的室内机器人定位系统。从摄像头传感器和里程计传感器分别得到视觉信息和动作信息，通过该定位系统实时分析，得到机器人当前位姿估计，达到自定位的结果。

为达到以上目的，本发明所采用的解决方案是：

环境模型以点模型为主，标定离线地图。采用蒙特卡罗粒子滤波技术，用带有权值的粒子集分布表示机器人位姿估计。首先针对两类传感器，分别建立动作模型和观测模型。在动作模型下，每个粒子通过机器人的动作累加相对里程计数而更新位置。在观测模型中，根据路标情况主要分为两种，一为人为路标，其特点是数量少，特征明显且唯一，易于识别。二为自然路标，其特点为数量多、不唯一、多呈线状，如地面的白线，或墙体和地面的交线、物体和地面的交线等等。通过路标匹配处理更新粒子的权值和分布。重采样过程中增加粒子检验和新粒子补充的过程。最终通过栅格化场地粒子分布，选择用粒子密集度最大的子区域来计算最终位置，可以滤除其他无用粒子，提高定位精度。

定位系统的构架主要由五个模块组成：离线地图标定模块，动作更新模块，观测更新模块，重采样模块和位置估计模块。

1.离线地图标定模块

地图标定指将环境中的人为路标和自然路标的绝对坐标信息保存在机器人内部文件里，其坐标系与表示机器人位置的世界坐标系一致，定位系统通过探测的路标与该地图比较得到机器人的当前位置。

标定主要是对地面线段的标定用一定间隔的边缘点的标定代替线的标定。分为两种：

(1)线与线的交点。

线与线的交点类型分为L形，T形与X形，将所有交点归类，并将其全局位置保存在表中，机器人视觉信息处理后可分类查表。

(2)线上的点。

1)先将场地上的线简化为一定间隔的点集，点的属性包括其类型和方向：将点按边缘的类型分类(地线，障碍物与地面的线，场地边缘线等)，设为m个种类。点的方向为其所在线段的斜率方向的垂直方向，根据方向的不同分类，设为n个种类。

2)建立m×n个地图，每张地图只保留某个种类且某个方向的点。

3)将场地划分为p×q个区域，每个单位区域用其区域中心点代表。计算出场地上这些中心点所对应的最匹配的地图上的点，并将此映射信息保存。

由于观测的点计算后并不一定在地图中的路标线位置上，但根据点的梯度方向和位置，可以通过分类后的离线地图找到该点最符合的路标线上的位置，用查表匹配代替实时计算，减少计算量，提高了定位实时性。

2.动作更新模块

动作更新模型主要通过机器人的里程计传感器信息对粒子集中的每个粒子的位置进行更新。粒子表示机器人的位置，每个粒子在当前位姿上累加单位时间的机器人位移和噪声误差。为了模拟里程传感器误差以接近真实值，噪声误差为零均值的高斯噪声，其方差取决于当前位置的可信度。

3.观测更新模块

粒子滤波的特点是通过一定数量的粒子点来代表机器人的分布概率。粒子点的权值则关系到下一次更新时该位置粒子数量的增减。观测模型通过观测物体相对机器人的角度和距离更新粒子的权值。其过程为：

(1)路标匹配。将视觉得到的环境信息匹配到先前的离线地图中，主要为图像线扫描得到的地线边缘点的匹配。根据机器人的绝对位置和点对机器人坐标系的相对位置，计算出点在世界坐标系上的绝对位置及其边缘梯度值信息，边缘梯度则为点的方向。先由机器人的位置得到局部地图的范围，已知点的属性后代入局部的离线地图表中查询，根据最小方差匹配出该点的绝对位置估计值。

(2)粒子权值更新。根据路标类型的不同，粒子权值P由一系列权值表示P＝{p₁，...，p_n}，n表示类型的数量。通过路标匹配所有观测到的路标可以得到其唯一的全局坐标。当识别到类型为j的路标时，粒子点针对此类型的权值p^j取决于粒子点位置与地标的距离和夹角与机器人观测到路标的距离和夹角的偏差。建立关于偏差和权值关系的高斯模型函数，其中的协方差参数作为调节不同地标对物体的影响。根据设置不同路标类型的角度和距离的协方差常数来达到灵活调节。

(3)粒子集通过重要路标更新分布。对于已有的粒子点，观测模型中的物体测量偏差量不但决定了粒子权重，而且反映了粒子将要移动的方向。将机器人的观测模型中混合扩展卡尔曼滤波算法，将单个粒子看作一个高斯分布模型，通过视觉观测信息改进粒子预测分布，使粒子向似然较高区域移动，减少了粒子退化，并加快收敛。

4.重采样模块

在重要性采样的过程中，通过权值大小决定粒子的分布比率，删除权值较小的粒子，复制权值大的粒子。本发明实现了一种以观测更新后的粒子分布信息为判断依据适时地进行新粒子补充的扩展Monte Carlo定位算法，以节省计算资源并提高定位效率。再第一次粒子集更新基础上，对粒子权值进行检验：计算当前粒子集合的平均权值p并作为阙值，按一定的概率抽取集合中的粒子，比较其权值与p的大小，若小于均值，则删除该粒子，产生新粒子替代。

新粒子的产生通过两种途径：

(1)随机生成新粒子。以机器人当前的位置可信度调整该方法生成的粒子量，用于解决“机器人绑架”问题。“机器人绑架”指：目前的机器人定位采用的是概率估计的方法，在马尔可夫模型下，当前估计值是基于历史估计值和历史观测信息。当机器人被人为的移动或重新放到某一位置时，该机器人的真实位置将与历史位置无关，称之为“绑架”，“机器人绑架”问题则指解决“绑架”事件发生后的机器人定位的问题。

(2)根据历史信息预估粒子位置的方法得到新粒子。机器人在导航过程中检测到路标后根据路标和机器人的相对位置求得机器人的全局位置。任意两个路标位置与机器人的三角关系可以得到机器人位置的唯一值。在所求得的位置值为中心的小范围内产生一定数量的新粒子。这样产生的粒子点成为下一次更新的有效粒子点的概率大大高于随机产生的粒子点，加快MCL蒙特卡罗定位算法(Monte Carlo Localization)收敛速度。但它依赖于历史位置，对于机器人绑架问题是无法得到准确的解，因此，仍需要一些随机粒子的产生，这两种方式产生粒子以一定比率存在。

5.位置估计模块

机器人的最终位置估计过程中选择用粒子密集度最大的子区域来计算最终位置，可以滤除其他无用粒子，提高定位精度。粒子空间为机器人位姿矢量的维数空间，空间范围为矢量的各分量的取值范围。将粒子空间栅格化后，将空间的

倍作为子空间(将矢量中分量分成k等分，i为空间维数)以子空间为单位遍历粒子空间，得到粒子数最多的区域，将其代替整体区域作为机器人位姿最终估计区域。

由于采用了上述方案，本发明具有以下特点：(1)离线地图的标定采用点标定的形式。本系统应用的点标定形式下可在边缘检测后直接对边界点进行匹配，通过每个视觉帧多个检测点来减少噪声误差，降低了地图结构的复杂度，去除了几何拟合的处理，大大减少计算量，提高计算效率。

(2)通常的地图匹配用的是全局地图信息，在点匹配中采用动态地图的匹配方式。由于机器人视觉系统的识别区域有限，通过机器人当前的位置获得机器人局部地图进行匹配，使机器人历史定位信息与已知的全局地图相结合，地图中的自然地标相对唯一，有效避免了误多解的问题。

(3)融合了多传感器信息用于机器人定位，包括被动式传感器(里程计传感器)和主动式传感器(摄像头传感器)。对各传感器建模，通过各模型对各种信息进行融合处理，在模型中嵌套扩展卡尔曼滤波器对采样粒子的分布进行优化处理。

(4)重要性采样过程扩展了传统的蒙特卡罗定位算法。原有的蒙特卡罗定位算法要求采样粒子的数量大，计算量是个严重问题，不适合实时定位。同时容易产生过收敛和粒子匮乏的现象。本系统增加了粒子淘汰和补充的环节：在原有的重采样过程完成后，抽样该粒子集，用粒子的平均权值作为标准值决定是否淘汰该抽样。新粒子的生成是根据视觉系统识别的历史信息预估粒子位置，提高生成理想粒子的几率，加快收敛速度。

(5)机器人的最终位置估计过程中，通常蒙特卡罗定位算法是通过所有的样本求均值得到最终结果，本系统应用栅格分类粒子集的方式，用小区域的集中样本代替所有样本进行最终估计，将误差较大的样本集合排除，提高了位置的精度。

附图说明

图1是本发明实施例的系统结构框图。

图2是本发明实施例的路标匹配的结构图。

图3是本发明实施例的场地环境图。

图4是本发明实施例的场地点对应最匹配线上点位置的映射图。

图5是本发明实施例的观测模型中观测物、粒子点、机器人实际位置关系图模型图。

图6是本发明实施例的两地标的位置估算模型图。

具体实施方式

以下结合附图所示实施例对本发明作进一步的说明。

1.系统结构

请参阅图1本发明的系统结构框图。机器人的动作帧频率为8ms/帧，视觉帧频率为30ms/帧，由于两者频率不同，系统划分为两个线程进行分别处理：其中动作帧处理线程的输入为单位时间检测的动作增量，线程由动作更新模块和位置估计模块组成，有独立的结果输出。视觉帧处理线程包括地图匹配，视觉更新模块，重采样模块，和位置估计模块，主要通过对环境物体的识别更新样本点的权值和分布。

2.地图建立

请参阅图2-图4，以一个5m×7m的室内足球场地为例，该图例举了地面白线的分布，间隔设为3.5cm将场地上的线简化为一定间隔的点集。根据线的x轴方向和y轴方向将点分类为两个不同集合和

在视觉处理中，边缘象素点有其色彩梯度方向值k_mes，通过公式得到不同线段上点的集合：

其中，k_xy和k_yx分别表示对x轴方向和y轴方向对梯度方向的要求。只有趋近于水平或垂直的梯度的点才能认为是该集合直线上的点。根据不同集合

和

建立两个独立的表。

在每个表中，将场地以间隔2cm为单位划分为250×350个区域，每个单位区域的中心点与表中的已知地标点通过最小方差来匹配，建立映射关系后存入表中，作为离线地图。

图4为两个集合上的点对应最匹配白线上位置的映射图。该图形象的表示了白线边缘点的离线地图的作用：将计算出的场上的点根据梯度的不同对应到白线上的边缘点。因而可以根据地图得到图像扫描到的点的全局位置。

3.机器人模型建立

在全局参考坐标系中，机器人的位姿用三维矢量表示P_robot＝[x，y，h]。其中，x，y均为场地上距中心点的水平距离和垂直距离，h表示机器人身体的朝向。

4.动作更新模块

动作更新模型主要通过机器人的里程计传感器信息得到单位时间的定位增量Δodometry＝(Δx，Δy，Δh)。为了模拟里程传感器误差以接近真实值，加上了零均值的高斯噪声Δerror，其方差取决于当前位置的可信度。

$\mathrm{\Δerror}=\left(\begin{array}{c}0.1d\×\mathrm{random}(-\mathrm{1,1})\\ 0.02d\×\mathrm{random}(-\mathrm{1,1})\\ (0.002d+0.2\mathrm{\α})\×\mathrm{random}(-\mathrm{1,1})\end{array}\right)---\left(8\right)$

式(8)中，d表示单位增量的长度，也就是机器人单位时间内移动的距离，α表示机器人转动的角度增量。

定位系统从机器人动作系统中读入里程计信息，更新粒子状态。更新后的粒子表示如下：

${\mathrm{pose}}_i^{\mathrm{new}}={\mathrm{pose}}_i^{\mathrm{old}}+\mathrm{\Δodometry}+\mathrm{\Δerror}---\left(9\right)$

机器人最终位置为：

$\left[\begin{array}{c}x\\ y\\ h\end{array}\right]=\left[\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{xpose}}_i\×p_i\\ \underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{ypose}}_i\×p_i\\ a\tan 2(\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\sin \left({\mathrm{hpose}}_i\right)\×p_i,\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\cos \left({\mathrm{hpose}}_i\right)\×p_i)\end{array}\right]---\left(10\right)$

5.观测更新模块

(1)粒子权值更新

设粒子点与地标距离为d_i，夹角为α_i，观测距离为d_mes，观测夹角为α_mes，其通过以下高斯函数求得权值：

${p_{i\left(\mathrm{\α}\right)}}^j=\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\σ}}_{\mathrm{\α}}^j}}\exp [-\frac{{({\mathrm{\α}}_i-{\mathrm{\α}}_{\mathrm{mes}})}^2}{2{\mathrm{\σ}}_{\mathrm{\α}}^{j2}}]---\left(11\right)$

${p_{i\left(d\right)}}^j=\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\σ}}_d^j}}\exp [-\frac{{(d_i-d_{\mathrm{mes}})}^2}{2{\mathrm{\σ}}_d^{j2}}]---\left(12\right)$

${p_i}^j({\mathrm{\α}}_{\mathrm{mes}},{\mathrm{\α}}_i,d_{\mathrm{mes}},d_i,{\mathrm{\σ}}_{\mathrm{\α}}^j,{\mathrm{\σ}}_d^j)={p_{i\left(\mathrm{\α}\right)}}^j{{\mathrm{gp}}_{i\left(d\right)}}^j---\left(13\right)$

式(11)(12)(13)中

和

分别为角度和距离的协方差常数，根据调试经验进行选取。为了区别不同地标对物体的影响，定义参数 $\∂=\{{\∂}^1\·\·\·{\∂}^n\}.$ 粒子最终的权值为：

$P=\underset{j=1}{\overset{n}{\mathrm{\Π}}}\left(p^j{g\∂}^j\right)---\left(14\right)$

可以通过设定不同的

值来加强和削弱不同类型的物体对定位的影响。

(2)粒子位姿更新

请参阅图5，该图为观测更新模型中观测物、粒子点、机器人实际位置关系图。将每个粒子看作是以三维高斯分布的位姿模型。当前粒子的位姿对于二维空间，为笛卡尔位置坐标及航向角，用X＝(x，y，θ)表示。

1)粒子位置预测。设置位置预测

，产生相应的协方差

P(k+1|k)＝P(k|k)+Q(k)  (15)

式中Q(k)为噪声协方差，且判断其为对角矩阵。为协方差P(0|0)设定估计值，实际上估计的准确性不重要，随着循环的运行，算法会收敛到一个更准确的值。

2)地标实际观测值。通过视觉系统测得地标的实际位置z(k+1)。

3)测量预测。用估计地标的预测观测值：

$\hat{z}(k+1)=h\left(\hat{X}(k+1|k)\right)---\left(16\right)$

其中h(X(k))为测量函数。

4)预测值和实际值的匹配。利用

与z(k+1)进行匹配。

5)估计更新。根据P(k+1|k)及测量噪声协方差R(k+1)，计算权重：

W(k+1)＝P(k+1|k)H(k+1)^T×

[H(k+1)P(k+1|k)H(k+1)^T+R(k+1)]^-1  (17)

根据

与z(k+1)之间差的加权值修正状态估计，

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+W(k+1)v(k+1)---\left(18\right)$

其中 $v(k+1)=z(k+1)-H(k+1)\hat{X}(k+1|k),$ 反映预测量

与z(k+1)之间的匹配程度。并且计算相应的协方差P(k+1|k+1)：

P(k+1|k+1)＝[I-W(k+1)H(k+1)]P(k+1|k)  (19)

(I为单位阵)，估计与实际测量匹配则停止，否则转第一步。

6.重采样中的新粒子估计

在视觉更新时，存储n个不同类型地标相对机器人坐标的最新的历史位置，设为k_i＝{x_i，y_i，h_i}，i＝1，2..n，由于可信的地标类型有限，加上历史信息随着累加误差变大，一般n小于4。当动作更新时，该位置也应得到更新：

$k_i^{\mathrm{new}}=k_i^{\mathrm{old}}+\mathrm{\Δodometry}---\left(20\right)$

以保持地标与机器人的相对关系与实际一致。而粒子点的产生为：

因此通过两个地标位置与机器人的三角关系可以计算出机器人可能存在的位置。

参见图6，在图中定位问题的2D地平面投影被描述。<0>代表全局参考坐标系，矢量P∈R^2×1代表了机器人的在全局参考坐标系下的位置和朝向

。<1>是机器人本体下的局部坐标系，L₁∈R^2×1和L₂∈R^2×1是两路标关于<0>坐标系下的位置，而Z₁∈R^2×1和Z₂∈R^2×1代表了同样的两个路标相对于<1>坐标系下的位置。则姿态的估算问题就是有计算P和

组成。通过视觉系统的测量可以得到¹Z₁和¹Z₂，而矢量⁰L₁和⁰L₂也已经通过地平面距离的先验知识所知道(^jZ表示矢量Z在参考平面<j>下的投影)。

式(22)为<0>和<1>坐标变换的旋转矩阵，可以使用如下的公式：

⁰L₂＝⁰P+⁰R₁ ¹Z₂  (23)

⁰L₁＝⁰P+⁰R₁ ¹Z₁  (24)

$\&DoubleRightArrow;(L_2-L_1)_0=R_1^{\mathrm{}}{}_0(Z_2-Z_1)_1---\left(25\right)$

在以上假设下，唯一不知道的就是在式(22)中的

解决公式(22)中的

和是通过以下公式：

            (26)

            (27)

在以上公式里，ΔL_x≡⁰(L₂-L₁)_x，ΔL_y≡⁰(L₂-L₁)_y，ΔZ_x≡¹(Z₂-Z₁)_x，ΔZ_y≡¹(Z₂-Z₁)_y都是已知的。只要L₁和L₂是两个不同的路标，既只要L₁≠L₂，则等式(26)，(27)总是可以有意义的。机器人的朝向角

可以通过

计算得到。然后利用得到的

和

通过公式(24)或(25)计算机器人的绝对位置⁰p。当多于两个路标可见时候，把所有路标两两不同组合应用上面的方法计算，再求平均就得到机器人姿态的预测位置。将该位置附近产生粒子点成为下一次更新的有效粒子点。

7.位置估计模块

先栅格化粒子分布空间，按机器人位姿矢量[x，y，h]各自分量的取值范围划分为K×K×K的栅格空间，每个子空间为一个cell单位。选择的子区域空间为

的cell空间，记录每个cell的粒子数目。再用

的空间遍历得到粒子数最多的子区域。

在子区域中的m个粒子点的分布被看作是机器人当前位姿的密度分布。求m个点的位姿矢量平均值：

$P_{\mathrm{robot}}=\left[\begin{array}{c}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{x}_i\\ \underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{y}_i\\ a\tan 2(\underset{i}{\overset{m}{\mathrm{\&Sigma;}}}\sin \left(h_i\right),\underset{i}{\overset{m}{\mathrm{\&Sigma;}}}\cos \left(h_i\right))\end{array}\right]$           (28)

P_robot则作为机器人当前位置。计算P_robot的可信度p，表示估计的机器人位姿的可信程度。

$\stackrel{\&OverBar;}{p}=\frac{m}{\mathrm{TotalNumber}}g\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{p}_i---\left(29\right)$

其中pi表示在子区域的粒子点i的权重。当定位稳定时，粒子较为集中，m值较大，最终的p值也较大；当定位不稳定时，粒子发散，m值较小，最终的p值也较小。因此，该值可以提供相应信息给决策系统。

上述的对实施例的描述是为便于该技术领域的普通技术人员能理解和应用本发明。熟悉本领域技术的人员显然可以容易地对这些实施例做出各种修改，并把在此说明的一般原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于这里的实施例，本领域技术人员根据本发明的提示，对于本发明做出的改进和修改都应该在本发明的保护范围之内。

Claims (10)

1.一种基于单目摄像头的步行机器人定位系统，其特征在于：环境模型采用点模型表示，标定离线地图，并建立动作模型及观测模型进行位置更新及路标标定，采用蒙特卡罗粒子滤波技术，用带有权值的粒子集分布表示机器人位姿估计，通过路标匹配处理更新粒子的权值和分布，栅格化场地粒子分布，选择粒子密集度最大的子区域定位机器人所在位置。

2.根据权利要求1所述的基于单目摄像头的步行机器人定位系统，其特征在于：其包括离线地图标定模块、动作更新模块、观测更新模块、重采样模块和位置估计模块。

3.根据权利要求2所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的地图标定模块标定离线地图，其将环境中的人为路标和自然路标的绝对坐标信息保存在机器人内部文件里，对地面线段的标定用一定间隔的边缘点的标定代替线的标定，其坐标系与表示机器人位置的世界坐标系一致，定位系统通过探测的路标与该地图比较得到机器人的当前位置。

4.根据权利要求2所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的动作更新模型对应里程计传感器建立，其通过机器人的里程计传感器信息对粒子集中的每个粒子的位置进行更新，每个粒子在当前位姿上累加单位时间的机器人位移和噪声误差，噪声误差为零均值的高斯噪声，其方差取决于当前位置的可信度。

5.根据权利要求2所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的观测更新模型对应摄像头传感器建立，其通过观测物体相对机器人的角度和距离更新粒子的权值，并将单个粒子看作一个高斯分布模型，通过视觉观测信息改进粒子预测分布，在模型中嵌套扩展卡尔曼滤波器对采样粒子的分布进行优化处理，更新粒子集。

6.根据权利要求2所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的重采样模块定义了以观测更新后的粒子分布信息为判断依据适时地进行新粒子补充的扩展蒙特卡罗定位算法，其在第一次粒子集更新基础上，对粒子权值进行检验：计算当前粒子集合的平均权值p并作为阙值，按一定的概率抽取集合中的粒子，比较其权值与p的大小，若小于均值，则删除该粒子，产生新粒子替代。

7.根据权利要求2所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的位置估计模块确定机器人的最终位置，其将粒子空间栅格化后，将空间的

倍作为子空间以子空间为单位遍历粒子空间，得到粒子数最多的区域，将其代替整体区域作为机器人位姿最终估计区域，其中将矢量中分量分成k等分，i为空间维数。

8.根据权利要求3所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的地面线段的标定包括线与线的交点、线上的点，线与线的交点类型分为L形，T形与X形，将所有交点归类，并将其全局位置保存在表中，机器人视觉信息处理后可分类查表；线上的点则将场地上的线简化为一定间隔的点集，点的属性包括其类型和方向，将点按边缘的类型分为m类，按方向类型分为n类，建立m×n个地图，每张地图只保留某个种类且某个方向的点，将场地划分为p×q个区域，每个单位区域用其区域中心点代表，计算出场地上这些中心点所对应的最匹配的地图上的点，并将此映射信息保存。

9.根据权利要求5所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的粒子集更新包括以下步骤：

1)路标匹配，将视觉得到的环境信息匹配到离线地图中，根据机器人的绝对位置和点对机器人坐标系的相对位置，计算出点在世界坐标系上的绝对位置及其边缘梯度值信息，边缘梯度则为点的方向，由机器人的位置得到局部地图的范围，已知点的属性后代入局部的离线地图表中查询，根据最小方差匹配出该点的绝对位置估计值。

2)粒子权值更新，根据路标类型的不同，粒子权值P由一系列权值表示P＝{p1，...，p_n}，n表示类型的数量，通过路标匹配所有观测到的路标得到其唯一的全局坐标；当识别到类型为j的路标时，粒子点针对此类型的权值p^j取决于粒子点位置与地标的距离和夹角与机器人观测到路标的距离和夹角的偏差，建立关于偏差和权值关系的高斯模型函数，其中的协方差参数作为调节不同地标对物体的影响。

3)粒子集通过重要路标更新分布，将机器人的观测模型中混合扩展卡尔曼滤波算法，将单个粒子看作一个高斯分布模型，通过视觉观测信息改进粒子预测分布，使粒子向似然较高区域移动，减少了粒子退化，并加快收敛。

10、根据权利要求6所述的基于单目摄像头的步行机器人定位系统，其特征在于：所述的产生新粒子的形式有：

1)随机生成新粒子，以机器人当前的位置可信度调整该方法生成的粒子量。

2)根据历史信息预估粒子位置的方法得到新粒子，机器人在导航过程中检测到路标后根据路标和机器人的相对位置求得机器人的全局位置，在机器人位置为中心的小范围内产生一定数量的新粒子。

Images