1.一种激光雷达的机器人同时定位与建图优化方法,其特征在于,包括如下步骤:步骤1:获取机器人的观测信息、里程计控制信息、估计自身位姿信息;
步骤2:通过提议分布采样方法获取机器人当前状态下的粒子集;
步骤3:计算粒子集中每个粒子的权重和总权重;
步骤4:计算粒子集的退化程度参数值;
步骤5:当退化程度参数值大于等于预设退化程度阈值时,执行步骤6;
当退化程度参数值小于预设退化程度阈值时,执行步骤7;
步骤6:对粒子集进行重采样,删除粒子集中粒子权重低的粒子,产生新的权重高的粒子建立新粒子集,使新粒子集中粒子数量与重采样之前粒子集总粒子数量相同,具体步骤如下:步骤61:建立坐标系,其中x轴为粒子集中粒子数量,y轴为权重值,y轴上水平设有数个边界,边界的边界值计算公式如下:bj=j·(SN+1)/N(1≤j≤N)
其中,bj为第j个的边界值;j为从1开始的正整数;N为粒子集中粒子总数;SN为粒子集中粒子的总权重;
步骤62:执行步骤63对粒子集进行遍历,建立新粒子集;
步骤63:计算包括当前粒子之前所有粒子的累计权重,将累计权重放入坐标系中,步骤64:当当前累计权重与前一累计权重之间不存在边界时,结束当前粒子的遍历;
当当前累计权重与前一累计权重之间存在边界时,执行步骤65‑66;
步骤65:当当前累计权重与前一累计权重之间只存在一条边界时,获取该边界与前一条边界内所有粒子中权重最大的粒子插入新粒子集;
当当前累计权重与前一累计权重之间存在至少两条边界时,获取当前累计权重与前一累计权重之间存在的突破边界数,以当前粒子生成和所获突破边界数相同数量少一个的新粒子,将新生成的数个粒子和当前粒子插入新粒子集;
步骤66:当当前累计权重跨越倒数第二条边界时,获取包括当前粒子之后的所有粒子中权重最大的粒子插入新粒子集,结束粒子集遍历,完成新粒子集建立;
步骤7:根据粒子集中粒子的轨迹和机器人的观测信息,计算机器人位置概率,对地图进行更新。
2.如权利要求1所述的激光雷达的机器人同时定位与建图优化方法,其特征在于,所述步骤65中,当当前累计权重与前一累计权重之间存在至少两条边界时,生成比所获突破边界数相同数量少一个的数个粒子的具体步骤如下:步骤651:获取当前累计权重与前一累计权重之间存在的突破边界数,通过如下公式获取粒子:*
其中,x 为新粒子;x为当前粒子;N为粒子集中粒子总数;n为突破边界数;Δb为相邻两条边界之间的差值;
步骤652:对步骤651获取的粒子分配权重,具体公式如下:*
其中,ω 是新粒子的权重;x为当前粒子;q是提议分布函数;z1:t是传感器观测信息;u1:t是里程计信息;p()是计算机器人位置概率。