[发明专利]基于粒子滤波SLAM的地图构建方法及装置

权利要求书

1.基于粒子滤波SLAM的地图构建方法，其特征在于，包括以下步骤：

获取步骤：获取激光传感器数据p(z_t|x_t，m)和里程计数据p(x_t|x_t-1，u_t-1)；

匹配步骤：通过迭代最近点算法对所述激光传感器数据进行扫描匹配；

更新步骤：若扫描匹配失败，采用所述里程计数据更新地图；

若扫描匹配成功，采用所述激光传感器数据更新地图；重采样步骤：计算有效粒子数N_eff，当所述有效粒子数小于粒子总数的一半时，进行自适应重采样补充所述激光传感器数据和所述里程计数据，否则直接返回获取步骤；

其中，所述采用里程计数据更新地图包括以下步骤：

根据所述里程计数据从t-1时刻的粒子集合Y_t-1采样获取t时刻的粒子集合Y_t；

计算t时刻的粒子集合Y_t中的每个粒子的权重；

根据设定的第一阈值对t时刻的粒子集合Y_t中的粒子筛选以生成第一最终粒子集，并使第一最终粒子集的所有粒子具有相同的权重；

根据第一最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图；

其中，所述采用激光传感器数据更新地图包括以下步骤：

将所述激光传感器数据融合至建议分布，并从融合激光传感器数据的建议分布中采样获取t时刻的粒子集合Y_t，其中融合激光传感器数据的建议分布为

计算t时刻的粒子集合Y_t中的每个粒子的权重，其中权重为w_t⁽ⁱ⁾＝p(z_t|m_t-1⁽ⁱ⁾，x_t-1⁽ⁱ⁾，u_t-1)·w_t-1⁽ⁱ⁾；

根据设定的第二阈值对t时刻的粒子集合Y_t中的粒子筛选以生成第二最终粒子集，并使第二最终粒子集的所有粒子具有相同的权重；

根据第二最终粒子集的每个粒子中包含的运动轨迹信息和观测信息生成地图。

2.根据权利要求1所述的基于粒子滤波SLAM的地图构建方法，其特征在于，所述通过迭代最近点算法对所述激光传感器数据进行扫描匹配具体为：设所述激光传感器数据的点集P为P＝{p₁，p₂，...，p_m}和参考点集Q为Q＝{q₁，q₂，...，q_n}，计算所述激光传感器数据的点集P与所述参考点集Q间的变换T＝{R，t}，使存在最优解，其中N为最优解下成功配对的点对数。

3.根据权利要求2所述的基于粒子滤波SLAM的地图构建方法，其特征在于，比较N与设定的第三阈值；若N大于第三阈值，则扫描匹配成功；否则扫描匹配失败。

4.根据权利要求1所述的基于粒子滤波SLAM的地图构建方法，其特征在于，所述重采样步骤中的有效粒子数N_eff为

5.根据权利要求1所述的基于粒子滤波SLAM的地图构建方法，其特征在于，根据粒子中包含的运动轨迹信息和观测信息生成地图具体为：通过占据栅格地图算法生成二维平面栅格图，其中所述二维平面栅格图表示为x_1t为运动轨迹信息，z_1t为观测信息。