CN106940185A - 一种基于深度摄像机的移动机器人定位和导航方法 - Google Patents

Abstract

本发明提供了一种基于深度摄像机的移动机器人定位和导航方法，属于机器人导航领域；本发明通过一种快速采样平面滤波算法(FSPF)，从深度图像中采集样本点和对属于3D平面的点集或者与平面不相对应的点进行分类，减少3D点云的数量的同时不被异常值影响；接着基于一个观察模型提出了一个平面滤波点云的定位算法，将平面滤波点向下映射到2D并且对每个2D地图上的点到线分配对应的值；最后，采用完全采样的点云作为自主导航的障碍物避免算法。本发明通过实验性结果展示了室内移动机器人自主导航方法的有效性，并通过将2D激光测距仪和3D深度摄像头进行比较，进一步了验证定位准确性。

Description

一种基于深度摄像机的移动机器人定位和导航方法

技术领域

本发明属于机器人导航领域，尤其涉及一种基于深度摄像机的移动机器人定位和导航方法。

背景技术

对于具有有限计算能力的室内移动机器人，采用深度摄像头用于机器人自主移动主要有以下两方面障碍：首先，深度摄像头通常会生成冗长的数据，这些数据在机器人的实时定位中不能被完全处理；其次，考虑到所使用的内存和计算复杂度是在3D环境下，2D环境中的定位和绘制地图技术(占用栅格)不能很好地去衡量。相反地，将3D环境中的观察物映射到现有的2D地图中这一问题并不简单。

发明内容

本发明的目的在于针对上述技术中存在的缺陷，提出了一种基于深度摄像机的移动机器人定位和导航方法。

为了实现上述目的，本发明提供的技术方案如下：一种基于深度摄像机的移动机器人定位和导航方法，其特征在于包括如下步骤：

步骤1、快速采样平面滤波算法(FSPF)：FSPF将深度图像I作为输入，并创建一个具有n个3D点的列表P、一个对应平面向量的列表R以及一个不与任何平面相对应的异常点列表O，此算法旨在生成一个最多含有n_max个3D点和它们所对应的平面向量的列表。

步骤2、平面滤波点云的定位：采用蒙特卡罗定位(MCL)和矫正的梯度调整(CGR)来实现；

步骤2.1、3D滤波点云P和对应的平面向量I首先被投影到2D来生成一个2D点云P’和对应的规范化的向量R’；

步骤2.2、对于每个P’中的点p_i，找到线l_i(l_i∈L)，使得p_i-x₁方向和从x₁出发的射线与l_i相交；

步骤2.3、如果存在找不到线l_i的点，那么将这些点舍弃，对应的向量估计值r_i与线l_i的差异值比阈值θ_i大的点也会被舍弃；计算来自于(扩展的)线l_i对应的点p_i的垂直距离d_i；

步骤2.4、所有的(非标准化)观察可能性p(y/x)由下式计算得到：

其中，σ是一个距离测量的标准偏差，f:f>1是一个用来折损观察点间相关性的贴现系数；

步骤3、自主导航：通过计算机器人到不同方向角度的开放路径长度来完成机器人自主导航，确保能够成功完成环境中的障碍物检测；

所述步骤3中障碍物检测通常采用来自步骤1中集合P和O中的3D点来实现，对于给定的机器人半径r和期望的前进方向θ_d，开放路径长度d(θ)是一个关于前进方向θ的函数，筛选后的障碍物避免方向θ*由如下计算得到：

其中，是角度θ方向上的单位向量并且坐标系原点和机器人中心点相一致。

本发明具有如下有益效果：

1)本发明提出的快速采样平面滤波算法(FSPF)，通过从深度图像中采集样本点和对属于3D平面的点集(也称作平面滤波点)或者与平面不相对应的点(也称作异常点)进行分类，减少3D点云数量的同时不被异常值影响。

2)本发明基于一个观察模型提出了一个定位算法，此算法能够将平面滤波点向下映射到2D并且对每个2D地图上的点到线分配对应的值，并采用完全采样的点云(由平面滤波点和异常点组成)作为自主导航的障碍物避免算法，成功完成环境中障碍物检测，提高了机器人自主定位和导航的准确性。

附图说明

图1为本发明所述的基于深度摄像机的移动机器人定位和导航方法流程图。

图2为实现本发明采用的Kinect和激光测距仪偏移误差对照图。

具体实施方式

下面结合附图对本发明做进一步的说明。

一种基于深度摄像机的移动机器人定位和导航方法，该方法包括以下步骤：如附图1所示，

步骤1、快速采样平面滤波算法(FSPF)：

步骤1.1、FSPF将深度图像I作为输入，采样三个位置分别记作d₀，d₁，d₂；首先，从图像中的任何一个地方随机选择第一个位置d₀，接着在d₀的附近随机选择邻居大小为η的位置d₁和d₂；

步骤1.2、创建一个具有n个3D点的列表P，步骤1.1中采样的三个位置d₀，d₁，d₂对应的3D坐标能够被计算出来并记作p₀，p₁，p₂，基于点p₀，p₁，p₂的平均深度(z坐标)和环境中的平面最小期望值S能够计算得到具有宽度为w’高度为h’的搜索窗口，从搜索窗口采样额外的l-3局部样本d_j以得到总共l个局部样本；

步骤1.3、由步骤1.2中所述点p₀，p₁，p₂定义的用于从平面中重建3D点p_j的平面拟合误差被计算以用于确定该点是否是异常值，如果在搜索窗口中有超过α_inl个点被归为非异常点，那么所有其他非异常点被加入到列表P中并且相关的向量被加入到列表R中，此算法最多运行m_max次来生成一个最多含有n_max个3D点列表P和它们所对应的平面向量的列表R以及一个不与任何平面相对应的异常点列表O；

步骤2、平面滤波点云的定位：采用蒙特卡罗定位(MCL)和矫正的梯度调整(CGR)来实现；

步骤2.1、3D滤波点云P和对应的平面向量I首先被投影到2D来生成一个2D点云P’和对应的规范化的向量R’；对应的地面平面检测点在这一步中并不需要考虑。假设机器人x的位置用x＝{x₁，x₂}来表示，其中x₁是机器人的2D位置，x₂是它的方向角度，可观察的场景线列表L可以用解析光线投射计算得到；

步骤2.2、对于每个P’中的点p_i，找到线l_i(l_i∈L)，使得p_i-x₁方向和从x₁出发的射线与l_i相交；

步骤2.3、如果存在找不到线l_i的点，那么将这些点舍弃，对应的向量估计值r_i与线l_i的差异值比阈值θ_i大的点也会被舍弃；计算来自于(扩展的)线l_i对应的点p_i的垂直距离d_i；

步骤2.4、所有的(非标准化)观察可能性p(y/x)由下式计算得到：

其中，σ是一个距离测量的标准偏差，f:f>1是一个用来折损观察点间相关性的贴现系数；

步骤3、自主导航：通过计算机器人到不同方向角度的开放路径长度来完成机器人自主导航，确保能够成功完成环境中的障碍物检测；

所述障碍物检测通常采用来自步骤1中集合P和O中的3D点来实现，对于给定的机器人半径r和期望的前进方向θ_d，开放路径长度d(θ)是一个关于前进方向θ的函数，筛选后的障碍物避免方向θ*由如下计算得到：

其中，是角度θ方向上的单位向量并且坐标系原点和机器人中心点相一致。

本发明实验的实现是基于传统全向建立的室内移动机器人，该机器人备有微软公司旗下的Kinect摄像头传感器。为了与此摄像头传感器形成对比，本发明同时也采用了Hokuyo URG-04LX 2D激光测距扫描仪并采用MCL和CGR的点云传感模型来实现其定位功能。

图2显示当机器人行走374m后，Kinect和激光测距感应器在定位方面的相对应的平均误差。即使Kinect在定位方面并没有激光测距感应器那么准确，但是对于室内移动机器人导航方面来说其结果已经相当精确了，其平均偏移误差比20cm要小同时其平均角度误差大概在1.5°左右。值得注意的是，在成本方面Kinect要比激光测距仪便宜。因此对于机器人研究的预算方面而言Kinect性价比更高。

为了验证深度摄像头在定位和导航解决方案方面的稳健性，还设置了一系列随机导航点用于机器人的导航并将这些点分布到地图上。其中，路径的总长度超过了4.1km。在整个实验过程中，只有Kinect传感器被用于定位和障碍物避免。机器人能够成功地被导航到所有的导航点，但是定位在三处被重设置，这些地方都是地图中Kinect传感器在一段时间内于开放地区中不能检测到墙的位置。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内的所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于深度摄像机的移动机器人定位和导航方法，其特征在于，包括如下步骤：

步骤1、快速采样平面滤波算法(FSPF)：FSPF将深度图像I作为输入，并创建一个具有n个3D点的列表P、一个对应平面向量的列表R以及一个不与任何平面相对应的异常点列表O，此算法旨在生成一个最多含有n_max个3D点和它们所对应的平面向量的列表。

步骤2、平面滤波点云的定位：采用蒙特卡罗定位(MCL)和矫正的梯度调整(CGR)来实现；

步骤2.1、3D滤波点云P和对应的平面向量I首先被投影到2D来生成一个2D点云P’和对应的规范化的向量R’；

步骤2.2、对于每个P’中的点p_i，找到线l_i(l_i∈L)，使得p_i-x₁方向和从x₁出发的射线与l_i相交；

步骤2.3、如果存在找不到线l_i的点，那么将这些点舍弃，对应的向量估计值r_i与线l_i的差异值比阈值θ_i大的点也会被舍弃；计算来自于(扩展的)线l_i对应的点p_i的垂直距离d_i；

步骤2.4、所有的(非标准化)观察可能性p(y/x)由下式计算得到：

其中，σ是一个距离测量的标准偏差，f:f>1是一个用来折损观察点间相关性的贴现系数；

步骤3、自主导航：通过计算机器人到不同方向角度的开放路径长度来完成机器人自主导航，确保能够成功完成环境中的障碍物检测。

2.根据权利要求1所述的基于深度摄像机的移动机器人定位和导航方法，其特征在于：所述步骤3中障碍物检测通常采用来自步骤1中集合P和O中的3D点来实现，对于给定的机器人半径r和期望的前进方向θ_d，开放路径长度d(θ)是一个关于前进方向θ的函数，筛选后的障碍物避免方向θ*由如下计算得到：

$d\left(\mathrm{\θ}\right)=\underset{p\∈P\∪O}{\min }\left(\max \right(0,\left|\right|p\·\widehat{\mathrm{\θ}}\left|\right|-r\left)\right)$

$\mathrm{\θ}*=\arg \underset{\mathrm{\θ}}{\max }\left(d\right(\mathrm{\θ}\left)\cos \right(\mathrm{\θ}-{\mathrm{\θ}}_d\left)\right)$

其中，是角度θ方向上的单位向量并且坐标系原点和机器人中心点相一致。