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

权利要求书

1.一种室内移动定位与制图方法，其特征在于，包括以下步骤：

步骤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：输出地图。

2.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤1中，首先激光水平固定放置，标定不同激光的相对位置，然后同步获取水平方向和竖直方向激光数据。

3.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤2中，设定阈值，剔除距离大于阀值的点云数据，以保证输入点云数据的精度。

4.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤4的具体实现包括以下子步骤：

步骤4.1：对于第i-1，i，i+1个点云数据d[i-1]，d[i]和d[i+1]，根据步骤3得到对应的位置坐标为(x_i-1,y_i-1)，(x_i,y_i)和(x_i+1,y_i+1)；

步骤4.2：判断i-1与i之间是否产生了突变；如果min(d_i-1,i,d_i,i+1)<d_i-1,i+1，则认为点云是连续的，未发生突变；则对这两点点云之间进行插值，插值个数为其中d_i-1,i表示i-1与i之间的距离，d_i,i+1表示i与i+1之间的距离，d_i-1,i+1表示i-1与i+1之间的距离；

步骤4.3：对插值后的点云，如果有某个(x_i,y_i)＝(x_j,y_j)，则丢掉其中一个点云，进而达到下采样的结果。

5.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤5的具体实现包括以下子步骤：

步骤5.1：将当前帧点云数据变换到候选位置所对应的姿态下；

步骤5.2：根据已生成的地图，统计每个点云数据是障碍物的概率，对概率求和；

步骤5.3：概率最大的位置则认为是当前的最优位置，也是下一个候选位置；

步骤5.4：重复步骤5.1-步骤5.3，直到最优位置不再变化或变化范围小于预设阀值，则结束。

6.根据权利要求1所述的室内移动定位与制图方法，其特征在于，步骤8的具体实现包括以下子步骤：

步骤8.1：设点A为子地图miniMap_i的一个特征点，点B为子地图miniMap_j的对应于A的特征点，则(A,B)记为子地图i和j的一对全局标靶；

步骤8.2：分别计算A与子地图miniMap_j，B与子地图miniMap_i中所有轨迹的对应关系，形成全局约束。

7.根据权利要求1所述的室内移动定位与制图方法，其特征在于：步骤9中，在点云逐帧匹配的过程中，误差被逐渐累积，根据步骤7和步骤8中计算的约束，利用图优化算法解决大规模地图生成过程中的累积误差问题。