[发明专利]一种基于稀疏位姿调整的移动机器人的Gmapping建图方法

权利要求书

1.一种基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，将图优化过程作为与前端交错运行的单独线程实现对地图构建过程中的优化，包括：

S1：初始化粒子位姿与分布，通过上一时刻第i个粒子的机器人位姿与里程计信息估计该时刻第i个粒子的机器人估计位姿，计算提议分布p；

S2：基于地图信息、机器人估计位姿、观测量，匹配扫描第i个粒子的机器人估计位姿的周边区域；i，j为编号，t为时刻；

若扫描匹配成功则进入S3，计算机器人位姿的极大似然估计值；并判断是否启动线性优化流程，若启动线性优化流程，则执行S3及S3’；否则，仅执行S3；

若扫描匹配失败，则跳过S3、S4，计算机器人位姿，更新第i个粒子的权重；

所述计算机器人位姿的计算公式为：

；

所述更新第i个粒子的权重的计算公式为：

；

S3：计算采样位置的目标分布，计算归一化因子；

所述归一化因子的计算公式为：

；

S4：计算高斯近似，计算第i个粒子的更新位姿；为t时刻各粒子的平均值；

所述高斯近似的计算公式为：

，

；

所述第i个粒子的更新位姿的计算公式为：

；

S5：更新第i个粒子的权重，所述第i个粒子的权重的更新公式为：

；

S6：更新机器人位姿与粒子地图，所述更新机器人位姿的方法为：通过所述第i个粒子的更新位姿与位姿修正量更新机器人位姿，所述更新粒子地图的方法为：通过直接稀疏特征矩阵H、第i个粒子的更新位姿、观测量，及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息；

S7：判断地图创建是否完成，若地图创建完成，则结束流程；

若地图创建未完成，则判断是否进行粒子重采样；

判断是否进行粒子重采样的方法为：

若有效样本量小于阈值T，进行粒子重采样，返回S5；

若有效样本量大于等于阈值T，则不进行粒子重采样，返回S2；

S3’：构建位姿图；所述位姿图构建包括计算机器人极大似然估计值，然后进入S4’；

S4’：通过闭环约束计算机器人位姿修正量；

所述的计算公式为：

，

，

其中，为精度矩阵，λ为梯度下降法到牛顿-欧拉法的转换因子，J为误差雅克比矩阵。

2.根据权利要求1所述的基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，所述提议分布的计算公式为：

；

所述第i个粒子的机器人估计位姿的计算公式为：

，

其中，表示标准位姿合成运算符。

3.根据权利要求1所述的基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，所述极大似然估计值的计算公式为：

。

4.根据权利要求1所述的基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，所述有效样本量的计算公式为：

，其中，为粒子归一化权重。