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

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

背景技术：

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

技术实现要素：

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

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

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

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

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

步骤2.2、对于每个p’中的点pi，找到线li(li∈l)，使得pi-x1方向和从x1出发的射线与li相交；

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

步骤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作为输入，采样三个位置分别记作d0，d1，d2；首先，从图像中的任何一个地方随机选择第一个位置d0，接着在d0的附近随机选择邻居大小为η的位置d1和d2；

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

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

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

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

步骤2.2、对于每个p’中的点pi，找到线li(li∈l)，使得pi-x1方向和从x1出发的射线与li相交；

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

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

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

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

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

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

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

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

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

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