CN112698345A - 一种激光雷达的机器人同时定位与建图优化方法 - Google Patents

Abstract

本发明公开了一种激光雷达的机器人同时定位与建图优化方法，包括：获取机器人的观测信息、里程计控制信息、估计自身位姿信息；通过提议分布采样方法获取机器人当前状态下的粒子集；计算粒子集中每个粒子的权重和总权重；计算粒子集的退化程度参数值；当退化程度参数值大于等于预设退化程度阈值时，对粒子集进行重采样，删除粒子集中粒子权重低的粒子，产生新的权重高的粒子建立新粒子集，使新粒子集中粒子数量与重采样之前粒子集总粒子数量相同；根据粒子的观测信息，对地图进行更新。本发明在重采样时使用权重高的粒子生成新的粒子而不是简单的复制，并且保持重采样后的粒子数量保持不变，这样就保持粒子的多样性，提高建图的精度。

Description

一种激光雷达的机器人同时定位与建图优化方法

技术领域

本发明涉及机器人同时定位与建图领域，尤其是涉及一种激光雷达的机器人同时定位与建图优化方法。

背景技术

机器人技术飞速发展，智能化生活近在眼前。未来中国将成为机器人制造和应用大国。目前，机器人不仅应用于工业现场，也广泛应用于生活服务场景。在大多数应用领域，移动机器人都是在未知环境中确定自身的位置，因此移动机器人必须具备环境地图构建和定位的能力，同时定位与建图(simultaneous localization and mapping，SLAM)技术正是为了解决移动机器人同时定位与地图构建的问题而产生的。

目前，主流SLAM技术分为激光雷达SLAM和视觉SLAM。激光雷达SLAM相对于视觉SLAM发展较早，可靠性高技术成熟，建图直观精度高，不存在累计误差，地图可用于路径规划。粒子滤波器能够适用于非线性非高斯系统，很适用于激光雷达SLAM的研究，相应地出现了Rao-Blackwellized粒子滤波器(Rao-Blackwellized Particle Filter，RBPF)方法应用于激光雷达SLAM。

然而，RBPF算法中随着时间的递增，粒子权值方差也随之增大，会不可避免的出现粒子退化现象。为了减少粒子退化现象，需要引入重采样方法，即复制高权值粒子，舍弃低权值粒子，产生多个等权值的粒子。这样虽然改善了粒子退化，但同时造成有效粒子匮乏，增加了计算时间；另一方面一些粒子会被多次复制，这导致粒子多样性的损失。

发明内容

本发明提供了一种激光雷达的机器人同时定位与建图优化方法，以解决现有技术中RBPF-SLAM算法存在粒子退化现象严重，粒子多样性缺失，建图精度降低的技术问题。

本发明提供了一种激光雷达的机器人同时定位与建图优化方法，具体步骤如下：

步骤1：获取机器人的观测信息、里程计控制信息、估计自身位姿信息；

步骤2：通过提议分布采样方法获取机器人当前状态下的粒子集；

步骤3：计算粒子集中每个粒子的权重和总权重；

步骤4：计算粒子集的退化程度参数值；

步骤5：当退化程度参数值大于等于预设退化程度阈值时，执行步骤6；

当退化程度参数值小于预设退化程度阈值时，执行步骤7；

步骤6：对粒子集进行重采样，删除粒子集中粒子权重低的粒子，产生新的权重高的粒子建立新粒子集，使新粒子集中粒子数量与重采样之前粒子集总粒子数量相同；

步骤7：根据粒子集中粒子的轨迹和机器人的观测信息，计算机器人位置概率，对地图进行更新。

进一步地，所述步骤6的具体步骤如下：

步骤61：建立坐标系，其中x轴为粒子集中粒子数量，y轴为权重值，y轴上水平设有数个边界，边界的边界值计算公式如下：

b_j＝j·(S_N+1)/N(1≤j≤N) (4)

其中，b_j为第j个的边界值；j为从1开始的正整数；N为粒子集中粒子总数；S_N为粒子集中粒子的总权重；

步骤62：执行步骤63对粒子集进行遍历，建立新粒子集；

步骤63：计算包括当前粒子之前所有粒子的累计权重，将累计权重放入坐标系中，

步骤64：当当前累计权重与前一累计权重之间不存在边界时，结束当前粒子的遍历；

当当前累计权重与前一累计权重之间存在边界时，执行步骤65-66；

步骤65：当当前累计权重与前一累计权重之间只存在一条边界时，获取该边界与前一条边界内所有粒子中权重最大的粒子插入新粒子集；

当当前累计权重与前一累计权重之间存在至少两条边界时，获取当前累计权重与前一累计权重之间存在的突破边界数，以当前粒子生成和所获突破边界数相同数量少一个的新粒子，将新生成的数个粒子和当前粒子插入新粒子集；

步骤66：当当前累计权重跨越倒数第二条边界时，获取包括当前粒子之后的所有粒子中权重最大的粒子插入新粒子集，结束粒子集遍历，完成新粒子集建立。

进一步地，所述步骤65中，当当前累计权重与前一累计权重之间存在至少两条边界时，生成比所获突破边界数相同数量少一个的数个粒子的具体步骤如下：

步骤651：获取当前累计权重与前一累计权重之间存在的突破边界数，通过如下公式获取粒子：

其中，x^*为新粒子；x为当前粒子；N为粒子集中粒子总数；n为突边界数；Δb为相邻两条边间之间的差值；

步骤652：对步骤651获取的粒子分配权重，具体公式如下：

其中，ω^*是新粒子的权重；x为当前粒子；q是提议分布函数。

本发明具有以下优点和有益的效果：

(1)本发明在重采样的过程中分段去除低权重粒子的方法，使得RBPF-SLAM算法无需对大量粒子进行采样，从而解决了粒子耗散的问题。

(2)本发明在重采样时使用权重高的粒子生成新的粒子而不是简单的复制，并且保持重采样后的粒子数量保持不变，这样就保持粒子的多样性，提高建图的精度。

附图说明

通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：

图1是改进激光雷达SLAM建图的流程图；

图2是重采样流程图；

图3是本发明粒子重采样实现图；

图4是本发明粒子滤波中分阶段消除粒子实现图；

图5是本发明粒子滤波中权值较大的粒子图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

移动机器人主体上配有激光雷达传感器、轮式里程计和惯性单元，分别采集激光信息、机器人的角度和加速度等信息，并用USB3.0将采集的信息传输到计算机进行后续处理。

步骤1：获取机器人的观测信息、里程计控制信息、估计自身位姿信息；

移动机器人首先会获得传感器观测信息z_1:t＝z₁,z₂,...,z_t与里程计信息u_1:t＝u₁,u₂,...,u_t,来估计自身位姿信息x_1:t＝x₁,x₂,...,x_t，和地图的地标信息m，位姿信息组成粒子集{x⁽ⁱ⁾}。

步骤2：通过提议分布采样方法获取机器人当前状态下的粒子集；

基于上一状态的位姿信息粒子集

在提议分布

中进行采样，得到当前状态下的粒子集

步骤3：计算粒子集中每个粒子的权重和总权重；

根据重要性采样公式计算当前时刻粒子权值，每个粒子分配权重，计算公式如下：

计算累计权重S_N：

步骤4：计算粒子集的退化程度参数值；

计算粒子退化程度参数N_eff，计算公式如下：

设置有效样本数为N_th＝N/2作为阈值，N是粒子的数目，

是归一化权重，

步骤5：当退化程度参数值小于等于预设退化程度阈值时，执行步骤6；

当退化程度参数值大于预设退化程度阈值时，执行步骤7；

当N_eff≤N_th时，执行步骤6；

当N_eff＞N_th时，执行步骤7；

步骤6：对粒子集进行重采样，删除粒子集中粒子权重低的粒子，产生新的权重高的粒子建立新粒子集，使新粒子集中粒子数量与重采样之前粒子集总粒子数量相同，具体步骤如下：

步骤61：建立坐标系，其中x轴为粒子集中粒子数量，y轴为权重值，y轴上水平设有数个边界，边界的边界值计算公式如下：

b_j＝j·(S_N+1)/N(1≤j≤N) (4)

其中，b_j为第j个的边界值；j为从1开始的正整数；N为粒子集中粒子总数；S_N为粒子集中粒子的总权重；

步骤62：执行步骤63对粒子集进行遍历，建立新粒子集。

步骤63：计算包括当前粒子之前所有粒子的累计权重，将累计权重放入坐标系中，

计算累计权重，

其中

步骤64：当当前累计权重与前一累计权重之间不存在边界时，结束当前粒子的遍历；

当当前累计权重与前一累计权重之间存在边界时，执行步骤65-66；

步骤65：当当前累计权重与前一累计权重之间只存在一条边界时，获取该边界与前一条边界内所有粒子中权重最大的粒子插入新粒子集,如图4所示，累计权重S_i到达什么位置就将粒子xⁱ分配到什么位置，黑色圆形面积的大小代表粒子权重的大小；

当当前累计权重与前一累计权重之间存在至少两条边界时如图5所示，获取当前累计权重与前一累计权重之间存在的突破边界数，以当前粒子生成比所获突破边界数相同数量少一个的数个粒子，将新生成的粒子插入新粒子集，具体步骤如下：

步骤651：获取当前累计权重与前一累计权重之间存在的突破边界数，通过如下公式获取粒子：

其中，x^*为新粒子；x为当前粒子；N为粒子集中粒子总数；n为突边界数；Δb为相邻两条边间之间的差值；

步骤652：对步骤651获取的粒子分配权重，具体公式如下：

其中，ω^*是新粒子的权重；x为当前粒子；q是提议分布函数。

步骤66：当当前累计权重跨越倒数第二条边界时，获取包括当前粒子之后的所有粒子中权重最大的粒子插入新粒子集，结束粒子集遍历，完成新粒子集建立，重采样结束。

步骤7：根据粒子集中粒子的轨迹和机器人的观测信息，计算机器人位置概率，对地图进行更新。

对于每个粒子，根据粒子的轨迹

和历史观测信息z_1:t，里程计信息u_1:t计算机器人位置概率

计算公式如下：

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下作出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (3)

1.一种激光雷达的机器人同时定位与建图优化方法，其特征在于，包括如下步骤：

步骤1：获取机器人的观测信息、里程计控制信息、估计自身位姿信息；

步骤2：通过提议分布采样方法获取机器人当前状态下的粒子集；

步骤3：计算粒子集中每个粒子的权重和总权重；

步骤4：计算粒子集的退化程度参数值；

步骤5：当退化程度参数值大于等于预设退化程度阈值时，执行步骤6；

当退化程度参数值小于预设退化程度阈值时，执行步骤7；

步骤6：对粒子集进行重采样，删除粒子集中粒子权重低的粒子，产生新的权重高的粒子建立新粒子集，使新粒子集中粒子数量与重采样之前粒子集总粒子数量相同；

步骤7：根据粒子集中粒子的轨迹和机器人的观测信息，计算机器人位置概率，对地图进行更新。

2.如权利要求1所述的激光雷达的机器人同时定位与建图优化方法，其特征在于，所述步骤6的具体步骤如下：

步骤61：建立坐标系，其中x轴为粒子集中粒子数量，y轴为权重值，y轴上水平设有数个边界，边界的边界值计算公式如下：

b_j＝j·(S_N+1)/N(1≤j≤N)

其中，b_j为第j个的边界值；j为从1开始的正整数；N为粒子集中粒子总数；S_N为粒子集中粒子的总权重；

步骤62：执行步骤63对粒子集进行遍历，建立新粒子集；

步骤63：计算包括当前粒子之前所有粒子的累计权重，将累计权重放入坐标系中，

步骤64：当当前累计权重与前一累计权重之间不存在边界时，结束当前粒子的遍历；

当当前累计权重与前一累计权重之间存在边界时，执行步骤65-66；

步骤65：当当前累计权重与前一累计权重之间只存在一条边界时，获取该边界与前一条边界内所有粒子中权重最大的粒子插入新粒子集；

当当前累计权重与前一累计权重之间存在至少两条边界时，获取当前累计权重与前一累计权重之间存在的突破边界数，以当前粒子生成和所获突破边界数相同数量少一个的新粒子，将新生成的数个粒子和当前粒子插入新粒子集；

步骤66：当当前累计权重跨越倒数第二条边界时，获取包括当前粒子之后的所有粒子中权重最大的粒子插入新粒子集，结束粒子集遍历，完成新粒子集建立。

3.如权利要求2所述的激光雷达的机器人同时定位与建图优化方法，其特征在于，所述步骤65中，当当前累计权重与前一累计权重之间存在至少两条边界时，生成比所获突破边界数相同数量少一个的数个粒子的具体步骤如下：

步骤651：获取当前累计权重与前一累计权重之间存在的突破边界数，通过如下公式获取粒子：

其中，x^*为新粒子；x为当前粒子；N为粒子集中粒子总数；n为突边界数；Δb为相邻两条边间之间的差值；

步骤652：对步骤651获取的粒子分配权重，具体公式如下：

其中，ω^*是新粒子的权重；x为当前粒子；q是提议分布函数。

Images