CN111768489B - 一种室内导航地图构建方法和系统 - Google Patents

Abstract

本发明涉及一种室内导航地图构建方法和系统，包括：采集环境深度图和所述深度图对应的RGBD信息和IMU信息；利用VISLAM算法根据所述RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在所述环境特征点地图的运动轨迹；根据所述运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图；对所述3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云；通过对所述地面点云进行离散化生成可行域栅格地图；根据所述可行域栅格地图构建拓扑地图。本发明直接通过图像完成导航地图的构建，避免了现有导航地图信号丢失、定位不准确和易被干扰的情况，更加接近人类的避障方式。

Description

一种室内导航地图构建方法和系统

技术领域

本发明涉及图像处理技术领域，尤其涉及一种室内导航地图构建方法和系统。

背景技术

在我们生活中，常见的定位方式包括：GPS定位，激光定位，超声定位等。其中，最为常用的是GPS定位，主要用于室外，有着诸多限制：室内无法使用、定位精度存在较大误差(2-5m)、易被干扰和劫持；此外，激光定位在无人车、自动驾驶领域有着较多的应用，其定位精度高，然而成本较大，且激光点云数据量较大，对计算性能要求高；超声定位多用于室内移动机器人，其测距短，定位误差大。

发明内容

为了解决上述技术问题，本发明的目的在于提供一种室内导航地图构建方法和系统。

根据本发明的一个方面，提供了一种室内导航地图构建方法，包括：

采集环境深度图和深度图对应的RGBD信息和IMU信息；

利用VISLAM算法根据RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在环境特征点地图的运动轨迹；

根据运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图；

对3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云；

通过对地面点云进行离散化生成可行域栅格地图；

根据可行域栅格地图构建拓扑地图。

利用VISLAM算法根据RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在环境特征点地图的运动轨迹包括：

构建世界坐标系并在世界坐标系中构建环境特征点地图；

定位深度图在环境特征点地图中的位姿；

利用词袋模型检索环境特征点地图中与深度图相似的关键帧；

通过匹配深度图与相似关键帧的特征点获取相机位姿；

根据相机位姿和深度图采集时间生成运动轨迹。

根据运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图包括：

根据图像采集系统在环境特征点地图的运动轨迹获取所有关键帧之间的位姿；

根据关键帧之间的位姿通过图像采集系统坐标系将深度图的图像坐标转换为世界坐标系坐标；

根据深度图图像转换的世界坐标系的图像坐标生成世界坐标系中的3D点云地图。

可选地，在本申请的一种实施方式中，对3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云包括：

提取3D点云地图的点云在世界坐标系中的法向量信息；

根据计算误差设置平行误差夹角；

筛选出法向量与重力方向的夹角小于平行误差夹角的点云作为与地面平行的点云；

对与地面平行的点云进行滤除噪声处理；

将除噪后的点云进行平面提取和聚类并将聚类结果对应点云最多的点云作为地面点云。

可选地，在本申请的一种实施方式中，通过对地面点云进行离散化生成可行域栅格地图包括：

分别计算地面点云在世界坐标系x轴方向的最大坐标和最小坐标；

分别计算地面点云在世界坐标系y轴方向的最大坐标和最小坐标；

设置栅格宽度；

根据栅格宽度对地面点云进行离散并根据x轴方向的最大坐标和最小坐标以及y轴方向的最大坐标和最小坐标生成2D栅格地图；

设置可行点数阈值并将存在点数达到可行点数阈值的栅格作为可行区域；

根据可行区域和2D栅格地图生成可行域栅格地图。

根据可行域栅格地图构建拓扑地图包括：

根据栅格宽度和路线实际宽度计算栅格数量；

除去可行域栅格地图中有且仅有两个连通域的可行区域栅格并将剩下的可行区域栅格作为备选拓扑节点；

根据路线实际宽度和栅格数量清除备选拓扑节点中的合并拓扑节点得到拓扑节点；

通过设定两个拓扑节点为一组对拓扑节点进行随机分组；

判断同组两个拓扑节点的连线是否满足不经过其他拓扑节点且在可行区域内：

是，则将两个拓扑节点进行双向连接；

否，则判定两个拓扑节点不可连接。

方法还包括：

获取当前在环境特征点地图中的位姿；

将当前位姿转换为在拓扑地图中的相应位置信息并进行路线规划。

根据本发明的另一个方面，提供了一种室内导航地图构建系统，包括：

信息采集单元，配置用于采集环境深度图和深度图对应的RGBD信息和IMU信息；

轨迹计算单元，配置用于利用VISLAM算法根据RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在环境特征点地图的运动轨迹；

图像转换单元，配置用于根据运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图；

三维压缩单元，配置用于对3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云；

栅格生成单元，配置用于通过对地面点云进行离散化生成可行域栅格地图；

拓扑构建单元，配置用于根据可行域栅格地图构建拓扑地图。

轨迹计算单元包括：

地图构建模块，配置用于构建世界坐标系并在世界坐标系中构建环境特征点地图；

图像分析模块，配置用于定位深度图在环境特征点地图中的位姿；

相似检索模块，配置用于利用词袋模型检索环境特征点地图中与深度图相似的关键帧；

位姿获取模块，配置用于通过匹配深度图与相似关键帧的特征点获取相机位姿；

轨迹生成模块，配置用于根据相机位姿和深度图采集时间生成运动轨迹。

图像转换单元包括：

关联获取模块，配置用于根据图像采集系统在环境特征点地图的运动轨迹获取所有关键帧之间的位姿；

坐标转换模块，配置用于根据关键帧之间的位姿通过图像采集系统坐标系将深度图的图像坐标转换为世界坐标系坐标；

三维生成模块，配置用于根据深度图图像转换的世界坐标系的图像坐标生成世界坐标系中的3D点云地图。

三维压缩单元包括：

向量提取模块，配置用于提取3D点云地图的点云在世界坐标系中的法向量信息；

误差设置模块，配置用于根据计算误差设置平行误差夹角；

点云筛选模块，配置用于筛选出法向量与重力方向的夹角小于平行误差夹角的点云作为与地面平行的点云；

噪声滤除模块，配置用于对与地面平行的点云进行滤除噪声处理；

提取聚类模块，配置用于将除噪后的点云进行平面提取和聚类并将聚类结果对应点云最多的点云作为地面点云。

栅格生成单元包括：

横向坐标计算模块，配置用于分别计算地面点云在世界坐标系x轴方向的最大坐标和最小坐标；

纵向坐标计算模块，配置用于分别计算地面点云在世界坐标系y轴方向的最大坐标和最小坐标；

栅格宽度设置模块，配置用于设置栅格宽度；

二维栅格生成模块，配置用于根据栅格宽度对地面点云进行离散并根据x轴方向的最大坐标和最小坐标以及y轴方向的最大坐标和最小坐标生成2D栅格地图；

可行栅格查找模块，配置用于设置可行点数阈值并将存在点数达到可行点数阈值的栅格作为可行区域；

栅格地图生成模块，配置用于根据可行区域和2D栅格地图生成可行域栅格地图。

拓扑构建单元包括：

数量计算模块，配置用于根据栅格宽度和路线实际宽度计算栅格数量；

备选查找模块，配置用于除去可行域栅格地图中有且仅有两个连通域的可行区域栅格并将剩下的可行区域栅格作为备选拓扑节点；

节点确定模块，配置用于根据路线实际宽度和栅格数量清除备选拓扑节点中的合并拓扑节点得到拓扑节点；

节点分组模块，配置用于通过设定两个拓扑节点为一组对拓扑节点进行随机分组；

连接判断模块，配置用于判断同组两个拓扑节点的连线是否满足不经过其他拓扑节点且在可行区域内；

双向连接模块，配置用于将两个拓扑节点进行双向连接；

否定连接模块，配置用于判定两个拓扑节点不可连接。

系统还包括：

位姿获取单元，配置用于获取当前在环境特征点地图中的位姿；

路线规划单元，配置用于将当前位姿转换为在拓扑地图中的相应位置信息并进行路线规划。

另外，本发明还提供一种设备，包括：

一个或多个处理器；

存储器，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上任一项所述的方法。

根据本发明的另一个方面，提供了一种存储有计算机程序的计算机可读存储介质，该程序被处理器执行时实现如上任一项所述的方法。

与现有技术相比，本发明具有以下有益效果：

1、本发明示例的一种室内导航地图构建方法，通过采集环境深度图并获取RGBD信息和IMU信息构建环境特征点地图并计算图像采集装置的运动轨迹，再根据运功轨迹和图像采集装置自带坐标系将深度图转换为世界坐标系的3D点云地图，对3D点云地图进行三维压缩、离散等处理构建可行域栅格地图，通过可行域栅格地图构建拓扑地图即可完成室内导航地图的构建。本发明直接通过图像完成导航地图的构建，避免了现有导航地图信号丢失、定位不准确和易被干扰的情况，更加接近人类的避障方式。

2、本发明示例的一种室内导航地图构建系统，基于传统的RGBD inertial SLAM技术，在运动轨迹上进行智能化的节点提取，建立拓扑地图，并融合深度图，提取地面可行域，得到可行域地图，将其栅格化，更快速、方便完成地图存储，辅助于导航，实现低成本、易扩展减少人工操作的导航地图生成。

3、本发明中示例的设备，通过处理器执行室内导航地图构建方法，能够更好地达成业务时效，提高服务质量。

4、本发明中示例的可读存储介质，储存有被处理器执行时实现的所述室内导航地图构建方法，便于中转车辆排序系统的使用及推广。

附图说明

图1为本发明流程图。

图2为本发明的方法的一种可行域栅格地图。

图3为本发明拓扑点密集的一种可行域栅格地图。。

其中，1、拓扑节点；2、合并拓扑节点。

具体实施方式

为了更好的了解本发明的技术方案，下面结合具体实施例、说明书附图对本发明作进一步说明。

实施例1：

本实施例提供一种室内导航地图构建装置，包括：

信息采集单元，配置用于采集环境深度图和所述深度图对应的RGBD信息和IMU信息；

轨迹计算单元，包括地图构建模块，配置用于构建世界坐标系并在所述世界坐标系中构建环境特征点地图；图像分析模块，配置用于定位深度图在所述环境特征点地图中的位姿；相似检索模块，配置用于利用词袋模型检索所述环境特征点地图中与所述深度图相似的关键帧；位姿获取模块，配置用于通过匹配所述深度图与相似关键帧的特征点获取相机位姿；轨迹生成模块，配置用于根据所述相机位姿和所述深度图采集时间生成所述运动轨迹；

图像转换单元，包括关联获取模块，配置用于根据所述图像采集系统在所述环境特征点地图的运动轨迹获取所有关键帧之间的位姿；坐标转换模块，配置用于根据所述关键帧之间的位姿通过图像采集系统坐标系将深度图的图像坐标转换为世界坐标系坐标；三维生成模块，配置用于根据深度图图像转换的世界坐标系的图像坐标生成世界坐标系中的3D点云地图；

三维压缩单元，包括向量提取模块，配置用于提取3D点云地图的点云在世界坐标系中的法向量信息；误差设置模块，配置用于根据计算误差设置平行误差夹角；点云筛选模块，配置用于筛选出法向量与重力方向的夹角小于所述平行误差夹角的点云作为与地面平行的点云；噪声滤除模块，配置用于对所述与地面平行的点云进行滤除噪声处理；提取聚类模块，配置用于将除噪后的点云进行平面提取和聚类并将聚类结果对应点云最多的点云作为地面点云；

栅格生成单元，包括横向坐标计算模块，配置用于分别计算所述地面点云在世界坐标系x轴方向的最大坐标和最小坐标；纵向坐标计算模块，配置用于分别计算所述地面点云在世界坐标系y轴方向的最大坐标和最小坐标；栅格宽度设置模块，配置用于设置栅格宽度；二维栅格生成模块，配置用于根据栅格宽度对地面点云进行离散并根据所述x轴方向的最大坐标和最小坐标以及y轴方向的最大坐标和最小坐标生成2D栅格地图；可行栅格查找模块，配置用于设置可行点数阈值并将存在点数达到所述可行点数阈值的栅格作为可行区域；栅格地图生成模块，配置用于根据所述可行区域和2D栅格地图生成可行域栅格地图；

拓扑构建单元，包括数量计算模块，配置用于根据栅格宽度和路线实际宽度计算栅格数量；备选查找模块，配置用于除去可行域栅格地图中有且仅有两个连通域的可行区域栅格并将剩下的可行区域栅格作为备选拓扑节点；节点确定模块，配置用于根据路线实际宽度和栅格数量清除备选拓扑节点中的合并拓扑节点得到拓扑节点；节点分组模块，配置用于通过设定两个拓扑节点为一组对拓扑节点进行随机分组；连接判断模块，配置用于判断同组两个拓扑节点的连线是否满足不经过其他拓扑节点且在可行区域内；双向连接模块，配置用于将两个拓扑节点进行双向连接；否定连接模块，配置用于判定两个拓扑节点不可连接配置。

此外，系统还包括：

位姿获取单元，配置用于获取当前在环境特征点地图中的位姿；

路线规划单元，配置用于将当前位姿转换为在拓扑地图中的相应位置信息并进行路线规划。

本实施例的一种室内导航地图构建方法，包括以下步骤：

S1、采集环境深度图和所述深度图对应的RGBD信息和IMU信息。

本实施例采用RGBD相机作为图像采集装置，将相机静止放置几秒后(用于重力等初始化)，手持相机遍历待应用的室内场景，采集环境深度图和所述深度图对应的RGBD信息和IMU信息(惯性测量单元数据)，并且离线标定相机内参(f_x,f_y,c_x,c_y)，离线标定相机内参数用于建立相机坐标系。

S2、利用VISLAM算法根据所述RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在所述环境特征点地图的运动轨迹。

通过VISLAM算法，融合RGBD信息和IMU信息，以初始位置为原点，重力反方向为z轴，任意yaw角作为世界坐标系，建立环境特征点地图，该地图中存储了相机经过场景的特征点信息和词袋向量。

定位当前采集图像在所述环境特征点地图中的位姿，并记录下采集时刻t。通过词袋模型在地图中检索与当前采集图像相似的关键帧，通过关键帧和当前t时刻图像进行特征点匹配，估计当前相机的6DoF位姿当定位到t时刻相机相对于地图的位姿/>后续采集到的t+1时刻图像采用相同方法计算相机位姿，根据不同时刻相机位姿计算相机在环境特征点地图中的运动轨迹。

S3、根据所述运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图。

根据相机运动轨迹采集所有关键帧间的位姿(关键帧不能太稀疏，需要有共视关系)将深度图转换为点云，对应深度图D_t中的图像坐标转换为世界坐标系中的3D点/>其中改点在相机坐标系中为/>

在世界坐标系中从而将所有深度图结合该时刻对应姿态进行融合，得到一个在世界坐标系中的3D点云地图。

S4、对所述3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云。

由于点云计算量较大，同一个位置可能被多个3D点描述，将3D点云进行三维压缩，得到既保留了结构又减少点云数量的点云地图。

提取3D点云地图中的点云在世界坐标系中的法向量信息Ω_n＝{n₁,n₂,…,n_N′}，n_i＝(xi,yi,zi)三维度。

根据z轴方向法向量进行滤波，由于地面与重力方向垂直，则对应地面的点云对应法向量与重力方向平行，所以法向量与重力方向平行的点云是需要过滤出来用于后续导航地图构建的。考虑到计算的误差，需要设置平行误差夹角，若点对应法向量满足：

则将点作为可用点。得到和地面平行的所有点云后，需要滤除一些障碍物、楼顶等噪声，对剩下的点云进行平面提取和聚类，取聚类结果对应点云最多的点云作为地面点云。

S5、通过对所述地面点云进行离散化生成可行域栅格地图。

计算地面点云在x和y方向的最大最小坐标：

设置栅格宽度b_w，将地面点云以宽度为b_w栅格进行离散，得到一张m*n的2D栅格地图M，其中：

设置可行点数阈值，若落在栅格Pi,_j中的点数达到可行点数阈值，则认为该栅格可通行(可通行表达式Mi,_j＝1)，视为可行区域。根据可行区域和2D栅格地图生成可行域栅格地图。

S6、根据所述可行域栅格地图构建拓扑地图。

根据栅格宽度和路线实际宽度计算栅格数量，由于室内过道宽度不会大于3m(大厅等会大于3m但是可以将其作为拓扑节点)，对应的栅格个数N_b＝3/b_w。

对栅格进行连通性con_i,j计算，对应如图2所示，除去可行域栅格地图中有且仅有两个连通域的可行区域栅格并将剩下的可行区域栅格作为备选拓扑节点，即备选拓扑节点的连通域数量不为2(con_i,j≠2)。

若栅格个数过多，则可能出现较小范围内重复出现多个拓扑节点的问题，为了避免这种情况，如图3所示，因此需要对备选拓扑节点进行滤除。将所有备选拓扑节点进行遍历，若当前备选拓扑节点半径3m内有其他备选拓扑节点，将半径内的备选拓扑节点删除，剩下的备选拓扑节点作为构建导航地图的拓扑节点。

对任意两个拓扑节点进行连接，若两拓扑节点间连线未曾经过任一拓扑节点并且都为可行区域，则将两拓扑节点进行双向连接，从而完成拓扑地图构建。

S7、获取当前在环境特征点地图中的位姿；将当前位姿转换为在拓扑地图中的相应位置信息并进行路线规划。

获取t时刻在3D离线地图中的位姿将其转换为在拓扑地图上的相应位置，对应位置为：

预设的目标点在拓扑地图上坐标为tarⁱ，在拓扑地图上进行路径搜索。

b)搜索得到的路径点集合为拓扑地图上的2D点投影到图像平面上进行显示。2D拓扑点到3D点之间转换为：

通过相机内参和t时刻位姿，将3D点转到图像平面：

判断像素点是否在图像平面内，若在则将i和i+1进行连线，并显示。

本实施例的一个或多个处理器；

存储器，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上任一项所述的室内导航地图构建方法。

本实施例的一种存储有计算机程序的计算机可读存储介质，该程序被处理器执行时实现如上任一项所述的室内导航地图构建方法，便于室内导航地图构建系统的使用及推广

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能。

Claims (12)

1.一种室内导航地图构建方法，其特征在于，包括：

采集环境深度图和所述深度图对应的RGBD信息和IMU信息；

利用VISLAM算法根据所述RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在所述环境特征点地图的运动轨迹；

根据所述运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图；

对所述3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云；

通过对所述地面点云进行离散化生成可行域栅格地图；

根据所述可行域栅格地图构建拓扑地图；

其中，对所述3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云包括：

提取3D点云地图的点云在世界坐标系中的法向量信息；

根据计算误差设置平行误差夹角；

筛选出法向量与重力方向的夹角小于所述平行误差夹角的点云作为与地面平行的点云；

对所述与地面平行的点云进行滤除噪声处理；

将除噪后的点云进行平面提取和聚类并将聚类结果对应点云最多的点云作为地面点云。

2.根据权利要求1所述的方法，其特征在于，所述利用VISLAM算法根据所述RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在所述环境特征点地图的运动轨迹包括：

构建世界坐标系并在所述世界坐标系中构建环境特征点地图；

定位深度图在所述环境特征点地图中的位姿；

利用词袋模型检索所述环境特征点地图中与所述深度图相似的关键帧；

通过匹配所述深度图与相似关键帧的特征点获取相机位姿；

根据所述相机位姿和所述深度图采集时间生成所述运动轨迹。

3.根据权利要求1所述的方法，其特征在于，所述根据所述运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图包括：

根据所述图像采集系统在所述环境特征点地图的运动轨迹获取所有关键帧之间的位姿；

根据所述关键帧之间的位姿通过图像采集系统坐标系将深度图的图像坐标转换为世界坐标系坐标；

根据深度图图像转换的世界坐标系的图像坐标生成世界坐标系中的3D点云地图。

4.根据权利要求1所述的方法，其特征在于，所述通过对所述地面点云进行离散化生成可行域栅格地图包括：

分别计算所述地面点云在世界坐标系x轴方向的最大坐标和最小坐标；

分别计算所述地面点云在世界坐标系y轴方向的最大坐标和最小坐标；

设置栅格宽度；

根据栅格宽度对地面点云进行离散并根据所述x轴方向的最大坐标和最小坐标以及y轴方向的最大坐标和最小坐标生成2D栅格地图；

设置可行点数阈值并将存在点数达到所述可行点数阈值的栅格作为可行区域；

根据所述可行区域和2D栅格地图生成可行域栅格地图。

5.根据权利要求1所述的方法，其特征在于，所述根据所述可行域栅格地图构建拓扑地图包括：

根据栅格宽度和路线实际宽度计算栅格数量；

除去可行域栅格地图中有且仅有两个连通域的可行区域栅格并将剩下的可行区域栅格作为备选拓扑节点；

根据路线实际宽度和栅格数量清除备选拓扑节点中的合并拓扑节点得到拓扑节点；

通过设定两个拓扑节点为一组对拓扑节点进行随机分组；

判断同组两个拓扑节点的连线是否满足不经过其他拓扑节点且在可行区域内：

是，则将两个拓扑节点进行双向连接；

否，则判定两个拓扑节点不可连接。

6.根据权利要求1所述的方法，其特征在于，所述方法还包括：

获取当前在环境特征点地图中的位姿；

将当前位姿转换为在拓扑地图中的相应位置信息并进行路线规划。

7.一种室内导航地图构建系统，其特征在于，包括：

信息采集单元，配置用于采集环境深度图和所述深度图对应的RGBD信息和IMU信息；

轨迹计算单元，配置用于利用VISLAM算法根据所述RGBD信息和IMU信息构建环境特征点地图并计算图像采集系统在所述环境特征点地图的运动轨迹；

图像转换单元，配置用于根据所述运动轨迹和图像采集系统坐标系将深度图转换为世界坐标系中的3D点云地图；

三维压缩单元，配置用于对所述3D点云地图在实际坐标系的法向量方向进行三维点压缩获取地面点云；

栅格生成单元，配置用于通过对所述地面点云进行离散化生成可行域栅格地图；

拓扑构建单元，配置用于根据所述可行域栅格地图构建拓扑地图；

其中，所述三维压缩单元包括：

向量提取模块，配置用于提取3D点云地图的点云在世界坐标系中的法向量信息；

误差设置模块，配置用于根据计算误差设置平行误差夹角；

点云筛选模块，配置用于筛选出法向量与重力方向的夹角小于所述平行误差夹角的点云作为与地面平行的点云；

噪声滤除模块，配置用于对所述与地面平行的点云进行滤除噪声处理；

提取聚类模块，配置用于将除噪后的点云进行平面提取和聚类并将聚类结果对应点云最多的点云作为地面点云。

8.根据权利要求7所述的系统，其特征在于，所述轨迹计算单元包括：

地图构建模块，配置用于构建世界坐标系并在所述世界坐标系中构建环境特征点地图；

图像分析模块，配置用于定位深度图在所述环境特征点地图中的位姿；

相似检索模块，配置用于利用词袋模型检索所述环境特征点地图中与所述深度图相似的关键帧；

位姿获取模块，配置用于通过匹配所述深度图与相似关键帧的特征点获取相机位姿；

轨迹生成模块，配置用于根据所述相机位姿和所述深度图采集时间生成所述运动轨迹。

9.根据权利要求7所述的系统，其特征在于，所述图像转换单元包括：

关联获取模块，配置用于根据所述图像采集系统在所述环境特征点地图的运动轨迹获取所有关键帧之间的位姿；

坐标转换模块，配置用于根据所述关键帧之间的位姿通过图像采集系统坐标系将深度图的图像坐标转换为世界坐标系坐标；

三维生成模块，配置用于根据深度图图像转换的世界坐标系的图像坐标生成世界坐标系中的3D点云地图。

10.根据权利要求7所述的系统，其特征在于，所述栅格生成单元包括：

横向坐标计算模块，配置用于分别计算所述地面点云在世界坐标系x轴方向的最大坐标和最小坐标；

纵向坐标计算模块，配置用于分别计算所述地面点云在世界坐标系y轴方向的最大坐标和最小坐标；

栅格宽度设置模块，配置用于设置栅格宽度；

二维栅格生成模块，配置用于根据栅格宽度对地面点云进行离散并根据所述x轴方向的最大坐标和最小坐标以及y轴方向的最大坐标和最小坐标生成2D栅格地图；

可行栅格查找模块，配置用于设置可行点数阈值并将存在点数达到所述可行点数阈值的栅格作为可行区域；

栅格地图生成模块，配置用于根据所述可行区域和2D栅格地图生成可行域栅格地图。

11.根据权利要求7所述的系统，其特征在于，所述拓扑构建单元包括：

数量计算模块，配置用于根据栅格宽度和路线实际宽度计算栅格数量；

备选查找模块，配置用于除去可行域栅格地图中有且仅有两个连通域的可行区域栅格并将剩下的可行区域栅格作为备选拓扑节点；

节点确定模块，配置用于根据路线实际宽度和栅格数量清除备选拓扑节点中的合并拓扑节点得到拓扑节点；

节点分组模块，配置用于通过设定两个拓扑节点为一组对拓扑节点进行随机分组；

连接判断模块，配置用于判断同组两个拓扑节点的连线是否满足不经过其他拓扑节点且在可行区域内；

双向连接模块，配置用于将两个拓扑节点进行双向连接；

否定连接模块，配置用于判定两个拓扑节点不可连接。

12.根据权利要求7所述的系统，其特征在于，所述系统还包括：

位姿获取单元，配置用于获取当前在环境特征点地图中的位姿；

路线规划单元，配置用于将当前位姿转换为在拓扑地图中的相应位置信息并进行路线规划。