一种基于多台3D结构光相机拼接的室内导航方法与流程

本发明涉及一种导航方法，具体是一种基于多台3d结构光相机拼接的室内导航方法。

背景技术：

实时定位及导航技术作为机器人智能化的第一步正不断引起行业内的重视，实时定位及导航是指机器人在自身位置不确定的条件下，在完全未知的环境中创建地图，同时利用地图进行自主定位和导航的一种技术，在此过程中，机器人一般通过位姿估计及测距单元来确定自己的位置信息。常见的测距单元包括激光测距、超声波传感器以及图像测距等。

常见的测距单元包括激光测距、超声波传感器以及图像测距等。激光测距是利用激光对目标的距离进行准确测定；超声波传感器采用超声波回波测距原理，运用精确的时差测量技术，检测传感器与目标物之间的距离。

激光测距精度较低且成本高；超声波传感器测距的适用频率范围和灵敏度都过小。

技术实现要素：

本发明的目的在于提供一种基于多台3d结构光相机拼接的室内导航方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种基于多台3d结构光相机拼接的室内导航方法，包括如下步骤：（1）采用栅格地图的方式来表达地图，初始地图设置为空，设机器人的初始位姿为全局坐标系的原点，构建地图过程中，里程计数据通过对机器人运动模型计算机器人位姿预测量，同时采用特征提取技术从不断更新传感器数据中提取观测量，通过设计的观测模型，观测预测量由机器人位姿预测量和可视全局地图计算得到，在获得观测量和观测预测量后，利用特征关联算法来实现特征匹配，匹配特征之间的信息，通过粒子滤波更新公式实现位姿更新和特征更新，未匹配的新特征通过特征增加技术加入全局地图，实现增量地图的构建；（2）使用一组离散的带权粒子模拟机器人位置的后验分布，并重复执行状态预测、更新权重和重采样等步骤完成滤波，实现机器人的定位；（3）采用路径规划单元对路径进行最优设计，路径规划单元包括全局路径规划模块和局部路径规划模块，全局路径规划单元基于静态地图采用a*算法搜索一条从起始状态到目标状态的最优路径或近似最优路径；局部路径规划单元，基于局部障碍物地图采用动态窗口法寻找一段时间内的最优轨迹，选取最优轨迹所对应的速度来驱动机器人运动；（4）通过多台3d结构光相机按特定位置的组合拼接，对机器人前方大于90度及小于6米范围的障碍物探测，实现模拟激光雷达的操作，获取探测范围内物体的坐标，为地图构建提供坐标数据，并实现运动控制同时进行避障操作。

作为本发明再进一步的方案：所述3d结构光相机大于2。

与现有技术相比，本发明的有益效果是：本发明采用多台3d结构光相机拼接模拟激光雷达方法，以及基于概率学的多种滤波算法进行位姿估计，实现了机器人定位与导航。通过3d结构光相机采集数据不容易受到环境的影响，且数据误差小，并具有低成本的优势，能够填补激光雷达的缺点。

附图说明

图1为基于多台3d结构光相机拼接的室内导航方法中多台相机拼接方式。

图2为基于多台3d结构光相机拼接的室内导航方法中相机平行面简单模型。

图3为基于多台3d结构光相机拼接的室内导航方法中相机垂直面简单模型。

图4为基于多台3d结构光相机拼接的室内导航方法的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1～4，本发明实施例中，一种基于多台3d结构光相机拼接的室内导航方法，包括如下步骤：（1）采用栅格地图的方式来表达地图，初始地图设置为空，设机器人的初始位姿为全局坐标系的原点，构建地图过程中，里程计数据通过对机器人运动模型计算机器人位姿预测量，同时采用特征提取技术从不断更新传感器数据中提取观测量，通过设计的观测模型，观测预测量由机器人位姿预测量和可视全局地图计算得到，在获得观测量和观测预测量后，利用特征关联算法来实现特征匹配，匹配特征之间的信息，通过粒子滤波更新公式实现位姿更新和特征更新，未匹配的新特征通过特征增加技术加入全局地图，实现增量地图的构建；（2）使用一组离散的带权粒子模拟机器人位置的后验分布，并重复执行状态预测、更新权重和重采样等步骤完成滤波，实现机器人的定位；（3）采用路径规划单元对路径进行最优设计，路径规划单元包括全局路径规划模块和局部路径规划模块，全局路径规划单元基于静态地图采用a*算法搜索一条从起始状态到目标状态的最优路径或近似最优路径；局部路径规划单元，基于局部障碍物地图采用动态窗口法寻找一段时间内的最优轨迹，选取最优轨迹所对应的速度来驱动机器人运动；（4）通过多台3d结构光相机按特定位置的组合拼接，对机器人前方大于90度及小于6米范围的障碍物探测，实现模拟激光雷达的操作，获取探测范围内物体的坐标，为地图构建提供坐标数据，并实现运动控制同时进行避障操作。

所述3d结构光相机大于2。

通过多台3d结构光相机按特定位置的组合拼接，可以对机器人前方大于90度及小于6米范围的障碍物探测，实现模拟激光雷达的操作。如图1所示为多台3d结构光相机拼接方式，四台3d结构光相机按照一定的角度固定在机械固定装置上，通过棋盘格标定方法获得各个相机基于中心点的旋转分量r和平移分量t，利用r和t将多台相机的深度数据映射到同一坐标系中拼接，去除两台相机重叠部分，最终得到一个探测范围更大激光雷达数据。如图1所示，单台深度数据水平探测范围为58.4°,去除其余相邻相机重叠部分，其真正的水平探测范围为45°。

所述3d结构光相机水平方向测距方式，其水平方向视角范围为到(不同的3d结构光相机是不一样，这里的3d结构光相机为29.2°)。对3d结构光相机的深度图像用简单模型描述，如图2所示，模型平面为与地面平行的平面，坐标原点为3d结构光相机中心，其对应像素平面的宽度，相机获取到的障碍物离成像平面的深度值，通过转换后得到障碍物离光心距离的激光量程值。转换方式如下：对于深度图像中第行第列的某一点深度值，3d结构光相机水平方向扫描范围，其角度步长的计算公式为，因此该像素点对应相机坐标系x轴方向的夹角，故转换成激光点数据就可以根据公式得到。

所述3d结构光相机垂直方向测距，对3d结构光相机的深度图像用简单模型描述，如图3所示，模型平面为与地面垂直的平面，坐标原点为3d结构光相机中心，其垂直方向视角范围为到(不同的3d结构光相机是不一样，这里的3d结构光相机为22.5°)。对应像素平面的高度，可以根据应用对垂直方向扫描的高度不同设置不同的垂直方向扫描范围，例如设置为-22.5°到5°。

如上所述扫描3d结构光相机一行所有像素进行水平方向测距计算，即可建立起当前局部环境，模拟实现二位激光雷达功能。也可以根据实际需要，取3d结构光相机若干行并对其求最小深度值作为激光点数据的深度值进行计算，即可达到近似三维雷达效果的二维激光雷达数据。

基于多台3d结构光相机拼接的室内导航方法流程如下：

1.启动机器人定位导航功能。

2.系统建立初始地图，并设机器人当前位置为全局坐标系的原点。

3.多台3d结构光相机按特定位置的组合拼接，获取机器人前方大于90度及小于6米范围的物体的坐标，为地图构建提供坐标数据。

4.基于静态地图采用a*算法，计算出一条从起始状态到目标状态的最优路径或近似最优路径。

5.系统通过获取里程计信息对机器人进行位姿估计，并提取周围环境特征信息与先验地图进行特征匹配，采用mlc定位方法对机器人进行实时定位。

6.在局部运动中，利用多台3d结构光相机拼接模拟激光雷达方法对机器人探测范围的障碍物进行识别。系统进行局部路径规划，采用动态窗口法寻找一段时间内的最优轨迹，选取最优轨迹所对应的速度来实现机器人运动控制同时进行避障操作。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。