[发明专利]基于已知占据栅格地图的持续激光SLAM构图定位方法

权利要求书

1.一种基于已知占据栅格地图的持续激光SLAM构图定位方法，其特征在于包括如下步骤：

S1：加载预先构建好的已知地图文件，所述已知地图文件为占据栅格地图文件；

S2：在已知地图范围内，基于蒙特卡洛粒子滤波方法，实现移动机器人全局粗定位，输出全局粗定位模式下移动机器人状态向量；包括：

S21：粒子群状态初始化：基于步骤S1加载的已知地图，在地图范围内初始化粒子群，随机生成粒子状态并均一化粒子权重其中，i＝1～n，n为粒子总数，取经验值10000-50000，x为粒子横坐标，y为粒子纵坐标，θ为粒子方位角；

S22：设定移动机器人的线速度和角速度，发送移动机器人运动指令，随机驱动移动机器人在已知地图范围内行走，记录运动时间间隔Δt，计算Δt时间间隔内机器人的位移信息，更新每个粒子的状态向量形成更新后的粒子群；

S23：根据激光传感器测量结果和更新后每个粒子的状态向量X_i’，结合载入的已知地图，计算粒子后验概率P(X_i|m)，其中m表示激光传感器测量结果，更新粒子权重W_i；

S24：在更新后的粒子群中根据粒子权重W_i进行重要性重采样，采样次数为n，权重高的粒子被采样的概率将高于权重低的粒子，采样后均一化粒子权重使即完成粒子群的重要性重采样；

S25：匹配度计算：统计重要性重采样后状态向量集中的粒子数，计算状态向量集中的粒子数占总粒子群的比重；匹配度的衡量标准为：比重越大，匹配度越高；

S26：阈值判断：判断当前匹配度是否高于设定阈值，若是，则计算粒子群的加权平均值，作为全局粗定位模式下状态向量X输出；若否，则重复进行步骤S22-S26，直到匹配度高于设定阈值为止；

S3：以步骤S2输出的全局粗定位模式下状态向量进行激光和地图匹配，通过计算激光扫描点最小重投影误差，求解移动机器人状态向量，实现精细定位；具体为：

以步骤S2输出的全局粗定位模式下状态向量X作为初始值，基于高斯-牛顿迭代法，求解精细定位模式下状态向量X^※，使式(1)激光扫描重投影误差最小：

上式(1)中，j表示激光雷达末端测量点标号，S_j(X)表示移动机器人状态向量为X、标号为j的激光测量点坐标值，M(S_j(X))表示地图中上述坐标值对应的栅格占据概率；

S4：根据步骤S1的已知地图初始化占据栅格地图，以步骤S3求得的精细定位结果的移动机器人状态向量作为SLAM初始状态，进行持续激光SLAM构图定位。

2.根据权利要求1所述的基于已知占据栅格地图的持续激光SLAM构图定位方法，其特征在于步骤S4实现步骤为：基于步骤S1的已知占据栅格地图以及步骤S3求解的精定位状态向量X^※作为SLAM初始状态，进行持续激光SLAM构图定位。

3.根据权利要求1或2所述的基于已知占据栅格地图的持续激光SLAM构图定位方法，其特征在于步骤S1中向移动机器人本体或上位机或远程控制端加载预先构建好的已知地图文件。