[发明专利]一种室内移动定位与制图方法

摘要

本发明公开了一种室内移动定位与制图方法，主要步骤包括按要求输入采集的点云数据；点云初步处理，具体包含点云滤波、点云坐标变换及非线性插值和下采样；利用粒子滤波的方法进行轨迹的姿态预测；子地图生成、标靶添加以及约束计算，具体包含动态子地图的生成、虚拟标靶和全局标靶添加、局部捆集和全局约束计算；利用图优化进行全局优化，减小逐帧匹配造成的累积误差；更新结果，包含更新子地图数据、轨迹数据和根据轨迹生成的全局地图数据；保存结果数据。本发明不依赖GPS和IMU，数据处理速度快，易于并行处理；制图精度高，实现厘米级精度。

主权项

一种室内移动定位与制图方法，其特征在于，包括以下步骤：步骤1：激光点云数据获取；步骤2：点云数据预处理；步骤3：将输入的极坐标系下的点云数据转换到笛卡尔坐标系下；步骤4：点云插值及下采样；步骤5：粒子滤波预测姿态；步骤6：子地图生成；具体实现包括以下子步骤：步骤6.1：设第一个落入地图中的点云位置为地图的起始位置start；步骤6.2：当有一个新的点云newPoint落入地图中时，由于点云的连续性，其位置一定在start的8‑邻域中，则在该位置申请一小块内存，用于存储newPoint的8‑邻域的点，n小块内存组成的地图即为子地图miniMap，且任意两个miniMap需要有重叠；步骤6.3：重复步骤6.2，生成最终地图finalMap，且finalMap是不规则地图的外接矩形，占用内存最小；步骤7：虚拟标靶及局部捆集约束计算；首先计算两个子地图相交部分概率最大的几个特征点，将这些特征点设置为虚拟标靶，其次对每一个虚拟标靶分别与这两个子地图包含的点云的轨迹计算约束，也就是在某个轨迹坐标系中计算标靶的姿态，即为局部捆集约束；步骤8：全局标靶添加及约束计算；步骤9：利用图优化进行全局优化；步骤10：根据步骤9得到的优化结果，更新轨迹和子地图数据；步骤11：将每一帧点云数据按照轨迹姿态进行累加，生成全局地图；步骤12：输出地图。