CN105333879A - 同步定位与地图构建方法 - Google Patents

Abstract

本发明公开了一种同步定位与地图构建方法，该方法包括以下步骤：S1，初始化系统状态：当t＝0时，根据机器人运动模型先验概率p(x₀)选取N个粒子，记为，i＝1,2,…,N，N为正整数，每个粒子对应的权重为；S2，计算优化的混合提议分布；S3，在该提议分布中采样粒子；S4，计算并更新权重；S5，当有效粒子数N_eff小于预先设定的阈值N_th时，进行重采样；S6，更新地图，返回步骤S2。本发明能够保持粒子多样性，在不同环境下能在线创建高精度的2-D栅格地图。

Description

同步定位与地图构建方法

技术领域

本发明涉及一种移动机器人导航领域，特别是涉及一种同步定位与地图构建方法。

背景技术

自主移动机器人的同步定位与地图构建(simultaneouslocalizationandmapping，SLAM)问题可描述为：在未知环境中，移动机器人通过机载传感器(如里程计、视觉传感器、超声波及激光等)来感知环境信息，逐步构建周围环境地图，同时运用此地图对其位置和姿态进行估计。该问题一直是移动机器人研究领域的热点和难点，被认为是能否真正实现机器人自主导航的关键问题，具有广阔的应用前景。

现有解决SLAM问题采用的方法有粒子滤波器和扩展卡尔曼滤波器等方法，但不能很好地表示提议分布，随着重采样的反复运用，使得粒子退化严重，降低粒子多样性，所得到的2-D栅格地图的精度不高，同时存在复杂度高、执行效率低等问题。

发明内容

本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种同步定位与地图构建方法。

为了实现本发明的上述目的，本发明提供了一种同步定位与地图构建方法，包括以下步骤：

S1，初始化系统状态：

当t＝0时，根据机器人运动模型先验概率p(x₀)选取N个粒子，记为i＝1,2,…,N，所述N为正整数，每个粒子对应的权重为

S2，计算优化的混合提议分布；

S3，在该提议分布中采样粒子；

S4，计算并更新权重；

S5，计算有效粒子数N_eff，当有效粒子数N_eff小于预先设定的阈值N_th时，进行重采样；否则直接执行步骤S6；

S6，更新地图，返回步骤S2。

以退火参数调控运动模型和观测模型在混合提议分布中的比重，使得改进后的提议分布更加接近真实状态；融入当前最新激光传感器观测信息，使得重要性权重的方差较小，减少所需粒子数，并保持粒子多样性，在不同环境下能在线创建高精度的2-D栅格地图。

在本发明的一种优选实施方式中，混合提议分布的计算方法为：

利用退火参数α来优化控制里程计运动模型和观测模型之间的比例，此时，混合提议分布的表示形式为：

$q_{improvedHydrid}=p{\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_{t-1})}^{\mathrm{\α}}\·p{\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}^{1-\mathrm{\α}},$

其中α为退火参数，取值范围[0,1]；表示第i个粒子在t时刻的状态由该粒子在t-1时刻的状态和t-1时刻的里程计的控制输入u决定；表示在t时刻的观测信息由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定。

融入当前最新激光测距仪的观测信息，以退火参数调控结合里程计运动模型和观测模型在混合提议分布中的比重，使提议分布更加接近真实分布，有效地降低粒子数量，并保持粒子多样性，大大减小计算复杂度。

在本发明的一种更加优选实施方式中，当提议分布由里程计运动模型起主导作用时，设置退火参数α为0.6；当融入激光测距仪的观测模型更接近真实状态分布时，则增加观测模型的比率，设置α为0.02。

通过退火参数优化控制两者在提议分布中的比重，当激光传感器的观测信息的精度明显高于里程计运动模型时，使得尽可能多的粒子分布在观测信息高似然的可行区域，使粒子权重不会出现明显差别，保持粒子多样性。

在本发明的一种优选实施方式中，权重的计算方法为：

$w_t^{\left(i\right)}=w_{t-1}^{\left(i\right)}p{\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_{t-1})}^{1-\mathrm{\α}}p{\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}^{\mathrm{\α}},$

其中表示第i个粒子在t时刻的状态由该粒子在t-1时刻的状态和t-1时刻的里程计的控制输入u决定；表示在t时刻的观测信息由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定；表示第i个粒子在t-1时刻的权重。

在本发明的一种优选实施方式中，有效粒子数N_eff的计算方法为：

$N_{eff}=\frac{1}{\underset{i=1}{\overset{N}{\Σ}}{\left({\tilde{w}}^{\left(i\right)}\right)}^2},$

其中N为粒子数目个数，为归一化重要性权重，即为第i个粒子在t时刻的权重。

有效粒子数N_eff保证了足够数量的粒子数来近似目标分布，呈现真实的后验概率，防止粒子退化，保持粒子多样性。

在本发明的一种优选实施方式中，重采样的计算方法包括以下步骤：

a.设置高阈值权重和低阈值权重所述H为大于L的正数，根据粒子权重与门限值的大小关系将粒子分成两部分：

I部分为高权重粒子域和低权重粒子域：

$w_t^{\left(i\right)}>w_H$ or $w_t^{\left(i\right)}<w_L,$

II部分为中等权重粒子域：

$w_L\&le;w_t^{\left(i\right)}\&le;w_H,$

其中N_HL＝N_H+N_L表示高权重粒子域与低权重粒子域的粒子数量之和，N-N_HL表示中等权重粒子域的粒子数量，表示第i个粒子在t时刻的状态，表示第i个粒子在t时刻的权重；

b.决定第i个粒子被复制的次数nⁱ，对于II部分中的粒子，直接复制；对I部分的粒子，根据权重由高到低给粒子排序，依次设定粒子的序号为1、2、3、……N_HL，设置每个粒子被选择的概率：

$p\left(i\right)=\frac{{\mathrm{\&alpha;}}_{rank}+\&lsqb;rank\left(i\right)/(N_{HL}-1)\&rsqb;({\mathrm{\&beta;}}_{rank}-{\mathrm{\&alpha;}}_{rank})}{N_{HL}},$

其中rank(i)为某一粒子排序后的序号，α_rank和β_rank为系数，当粒子数确定时，α_rank＝2-β_rank，1≤β_rank≤2，取值为：

$\left\{\begin{array}{ccc}{\mathrm{\&alpha;}}_{rank}=0.5,& {\mathrm{\&beta;}}_{rank}=1.5& rank\left(i\right)\&le;\frac{1}{2}{N}_{HL}\\ {\mathrm{\&alpha;}}_{rank}=0,& {\mathrm{\&beta;}}_{rank}=2& rank\left(i\right)>\frac{1}{2}{N}_{HL}\end{array}\right.,$

如果第i个粒子的概率小于其被选择的概率，即p(i)^*＝(1/N_HL)＜p(i)，则该粒子被舍弃；如果第i个粒子的概率大于或者等于其被选择的概率，即p(i)^*＝(1/N_HL)≥p(i)，则该粒子被复制，被复制的次数n_i由以下决定：

表示舍余取整，

如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，则依次增加由最高到低权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权重的粒子总数；如果粒子被复制的总次数大于采样前的高权重和低权重的粒子总数，则依次减少由最低到高权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权重的粒子总数；

复制后高权重的各粒子的权重为为高权重粒子的权重，n_i*为对应各粒子增加或者减少后实际被复制的次数；其余粒子权重保持不变。

改进的方法能减少运行时间，降低计算复杂度，增加算法的执行效率；同时通过控制权值的门限值增加粒子多样性，缓解粒子退化，使得所建地图更精确。

在本发明的一种更加优选实施方式中，H为2，L为0.5。

在本发明的一种优选实施方式中，更新地图的计算方法为：

根据描述机器人轨迹的粒子的位姿和历史观测信息z_1:t来更新计算对应的地图的后验概率其中为第i粒子在[1,t]时间段的状态，z_1:t为[1,t]时间段的观测信息，为第i粒子的地图信息由第i粒子在[1,t]时间段的状态和[1,t]时间段的观测信息决定。

综上所述，由于采用了上述技术方案，本发明的有益效果是：在相同环境下使用相同粒子数，改进方法提高机器人建立2-D栅格地图的精度，增加移动机器人SLAM的鲁棒性，保持粒子多样性，防止粒子退化，减少计算复杂度，减少重采样时间。

附图说明

图1是本发明的流程示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

本发明的基本思路为：通过机器人装有的HOKUYOURG-04LX激光测距仪的观测信息z_1:t＝z₁,…,z_t和它的里程计控制信息u_1:t-1＝u₁,…u_t-1来估算它可能的位姿的信息x_1:t＝x₁,…,x_t，即后验概率p(x_1:t|z_1:t,u_1:t-1)，并用它来计算基于地图m和位姿x_1:t的联合后验概率分布：

p(x_1:t,m|z_1:t,u_1:t-1)＝p(m|x_1:t,z_1:t)·p(x_1:t|z_1:t,u_1:t-1)，

由上式可知，根据已知的x_1:t和z_1:t可以有效计算基于地图的后验概率p(m|x_1:t,z_1:t)，该算法采用粒子滤波器来估算机器人的位姿的后验概率p(x_1:t|z_1:t,u_1:t-1)，每一个粒子代表一条可能的路径；卡尔曼滤波算法来更新地图信息。

本发明提供了一种同步定位与地图构建方法，如图1所示，包括以下步骤：

第一步，初始化系统状态：

当t＝0时，根据机器人运动模型先验概率p(x₀)选取N个粒子，记为i＝1,2,…,N，N为正整数，每个粒子对应的权重为

第二步，计算优化的混合提议分布。在本实施方式中，混合提议分布的计算方法为：

利用退火参数α来优化控制里程计运动模型和观测模型之间的比例，此时，混合提议分布的表示形式为：

$q_{improvedHydrid}=p{\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_{t-1})}^{\mathrm{\&alpha;}}\&CenterDot;p{\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}^{1-\mathrm{\&alpha;}},$

其中α为退火参数，取值范围[0,1]；表示第i个粒子在t时刻的状态由该粒子在t-1时刻的状态和t-1时刻的里程计的控制输入u决定，叫做里程计运动模型；表示在t时刻的观测信息由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定，叫做观测模型。在本发明更加优选实施方式中，当提议分布由里程计运动模型起主导作用时，设置退火参数α为0.6；当融入激光测距仪的观测模型更接近真实状态分布时，则增加观测模型的比率，设置α为0.02。

第三步，在该提议分布中采样粒子；从粒子集合中产生下一代粒子集合

第四步，计算并更新权重。在本实施方式中，权重的计算方法为：

$w_t^{\left(i\right)}=w_{t-1}^{\left(i\right)}p{\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_{t-1})}^{1-\mathrm{\&alpha;}}p{\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}^{\mathrm{\&alpha;}},$

其中表示第i个粒子在t时刻的状态由该粒子在t-1时刻的状态和t-1时刻的里程计的控制输入u决定；表示在t时刻的观测信息由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定；表示第i个粒子在t-1时刻的权重。该公式的推导过程为：

其中η＝1/p(z_t|z_1:t-1,u_1:t-1)为贝叶斯定律中的归一化因子,对所有粒子都是相等的；为目标分布，表示第i个粒子在[1,t]时间段的状态由[1,t]时间段的观测值z和[1,t-1]时间段的里程计的控制输入u决定；为提议分布，即预测分布，表示第i个粒子在[1,t]时间段的状态由[1,t]时间段的观测值z和[1,t-1]时间段的里程计的控制输入u决定；表示t时刻的观测信息由[1,t-1]时间段的观测信息z和第i个粒子在[1,t]时间段的状态决定；表示第i个粒子在t时刻的状态由该粒子在t-1时刻的状态和t-1时刻的里程计的控制输入u决定，叫做里程计运动模型；表示在t时刻的观测信息由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定，叫做观测模型；表示第i个粒子在t-1时刻的权重。

第五步，计算有效粒子数N_eff，当有效粒子数N_eff小于预先设定的阈值N_th时，进行重采样，否则直接执行第六步；。在本实施方式中，有效粒子数N_eff的计算方法为：

$N_{eff}=\frac{1}{\underset{i=1}{\overset{N}{\&Sigma;}}{\left({\tilde{w}}^{\left(i\right)}\right)}^2},$

其中N为粒子数目个数，为归一化重要性权重，即为第i个粒子在t时刻的权重。另外，在本实施方式中，重采样的计算方法包括以下步骤：

a.设置高阈值权重和低阈值权重H为大于L的正数，优选的，H为2，L为0.5。根据粒子权重与门限值的大小关系将粒子分成两部分：

I部分为高权重粒子域和低权重粒子域：

$w_t^{\left(i\right)}>w_H$ or $w_t^{\left(i\right)}<w_L,$

II部分为中等权重粒子域：

$w_L\&le;w_t^{\left(i\right)}\&le;w_H,$

其中N_HL＝N_H+N_L表示高权重粒子域与低权重粒子域的粒子数量之和，N-N_HL表示中等权重粒子域的粒子数量，表示第i个粒子在t时刻的状态，表示第i个粒子在t时刻的权重；

b.决定第i个粒子被复制的次数n_i，对于II部分中的粒子，直接复制；对I部分的粒子，根据权重由高到低给粒子排序，依次设定粒子的序号为1、2、3、……N_HL，设置每个粒子被选择的概率：

$p\left(i\right)=\frac{{\mathrm{\&alpha;}}_{rank}+\&lsqb;rank\left(i\right)/(N_{HL}-1)\&rsqb;({\mathrm{\&beta;}}_{rank}-{\mathrm{\&alpha;}}_{rank})}{N_{HL}},$

其中rank(i)为某一粒子排序后的序号，α_rank和β_rank为系数，当粒子数确定时，α_rank＝2-β_rank，1≤β_rank≤2，取值为：

$\left\{\begin{array}{ccc}{\mathrm{\&alpha;}}_{rank}=0.5,& {\mathrm{\&beta;}}_{rank}=1.5& rank\left(i\right)\&le;\frac{1}{2}{N}_{HL}\\ {\mathrm{\&alpha;}}_{rank}=0,& {\mathrm{\&beta;}}_{rank}=2& rank\left(i\right)>\frac{1}{2}{N}_{HL}\end{array}\right.,$

如果第i个粒子的概率小于其被选择的概率，即p(i)^*＝(1/N_HL)＜p(i)，则该粒子被舍弃；如果第i个粒子的概率大于或者等于其被选择的概率，即p(i)^*＝(1/N_HL)≥p(i)，则该粒子被复制，被复制的次数n_i由以下决定：

表示舍余取整，

如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，则依次增加由最高到低权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权重的粒子总数；如果粒子被复制的总次数大于采样前的高权重和低权重的粒子总数，则依次减少由最低到高权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权重的粒子总数；在本实施方式中，依次增加由最高到低权重粒子被复制的次数，应当理解为：如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，首先再复制一次最高(第一高)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止复制；如果粒子被复制的总次数还小于采样前的高权重和低权重的粒子总数，再复制一次次高(第二高)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止复制；如果粒子被复制的总次数仍然小于采样前的高权重和低权重的粒子总数，再复制一次次次高(第三高)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止复制；如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，……，如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，再复制一次次低(第二低)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止复制；如果粒子被复制的总次数还小于采样前的高权重和低权重的粒子总数，再复制一次最低(第一低)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止复制；如果粒子被复制的总次数仍然小于采样前的高权重和低权重的粒子总数，返回到最高权重粒子，按照上述规则继续。每次复制的次数可以不是一次，可以每次复制的次数为2次、3次等。依次减少由最低到高权重粒子被复制的次数，应当理解为：如果粒子被复制的总次数大于采样前的高权重和低权重的粒子总数，首先删除一次最低(第一低)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止删除；如果粒子被复制的总次数还大于采样前的高权重和低权重的粒子总数，如果还存在最低(第一低)权重粒子，则再删除一次最低(第一低)权重粒子，如果不存在最低(第一低)权重粒子，则删除一次次低(第二低)权重粒子，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止删除；……。即是说，从最低到高，首先删除完最低(第一低)的，再删除次低(第二低)的，再按照次序依次删除，如果粒子被复制的总次数等于采样前的高权重和低权重的粒子总数，则停止删除。

复制后高权重的各粒子的权重为为高权重粒子的权重，n_i*为对应各粒子增加或者减少后实际被复制的次数；其余粒子权重保持不变。

第六步，更新地图，返回第二步。在本实施方式中，更新地图的计算方法为：

根据描述机器人轨迹的粒子的位姿和历史观测信息z_1:t来更新计算对应的地图的后验概率其中为第i粒子在[1,t]时间段的状态，z_1:t为[1,t]时间段的观测信息，为第i粒子的地图信息由第i粒子在[1,t]时间段的状态和[1,t]时间段的观测信息决定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (8)

1.一种同步定位与地图构建方法，其特征在于，包括以下步骤：

S1，初始化系统状态：

当t＝0时，根据机器人运动模型先验概率p(x₀)选取N个粒子，记为i＝1,2,···,N，所述N为正整数，每个粒子对应的权重为

S2，计算优化的混合提议分布；

S3，在该提议分布中采样粒子；

S4，计算并更新权重；

S5，计算有效粒子数N_eff，当有效粒子数N_eff小于预先设定的阈值N_th时，进行重采样；否则直接执行步骤S6；

S6，更新地图，返回步骤S2。

2.根据权利要求1所述的同步定位与地图构建方法，其特征在于，混合提议分布的计算方法为：

利用退火参数α来优化控制里程计运动模型和观测模型之间的比例，此时，混合提议分布的表示形式为：

$q_{improvedHydrid}=p{\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_{t-1})}^{\mathrm{\&alpha;}}\&CenterDot;p{\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}^{1-\mathrm{\&alpha;}},$

其中α为退火参数，取值范围[0,1]；表示第i个粒子在t时刻的状态，由该粒子在t-1时刻的状态和t-1[1,t-1]时刻的里程计的控制输入u决定；表示在t时刻的观测信息，由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定。

3.根据权利要求2所述的同步定位与地图构建方法，其特征在于，当提议分布由里程计运动模型起主导作用时，设置退火参数α为0.6；当融入激光测距仪的观测模型更接近真实状态分布时，则增加观测模型的比率，设置α为0.02。

4.根据权利要求1所述的同步定位与地图构建方法，其特征在于，权重的计算方法为：

$w_t^{\left(i\right)}=w_{t-1}^{\left(i\right)}p{\left(x_t^{\left(i\right)}\right|x_{t-1}^{\left(i\right)},u_{t-1})}^{1-\mathrm{\&alpha;}}p{\left(z_t\right|m_{t-1}^{\left(i\right)},x_t^{\left(i\right)})}^{\mathrm{\&alpha;}},$

其中表示第i个粒子在t时刻的状态，由该粒子在t-1时间段的状态和t-1时刻的里程计的控制输入u决定；表示在t时刻的观测信息，由第i个粒子在t-1时刻的携带的地图信息和该粒子在t时刻的状态决定；表示第i个粒子在t-1时刻的权重。

5.根据权利要求1所述的同步定位与地图构建方法，其特征在于，有效粒子数N_eff的计算方法为：

$N_{eff}=\frac{1}{\underset{i=1}{\overset{N}{\&Sigma;}}{\left({\tilde{w}}^{\left(i\right)}\right)}^2},$

其中N为粒子数目个数，为归一化重要性权重，即 为第i个粒子在t时刻的权重。

6.根据权利要求1所述的同步定位与地图构建方法，其特征在于，重采样的计算方法包括以下步骤：

a.设置高阈值权重和低阈值权重所述H为大于L的正数，根据粒子权重与门限值的大小关系将粒子分成两部分：

I部分为高权重粒子域和低权重粒子域：

$\begin{array}{ccc}{\{x_t^{\left(i\right)},w_t^{\left(i\right)}\}}_{i=1}^{N_{HL}},w_t^{\left(i\right)}>w_H& or& w_t^{\left(i\right)}<w_L\end{array},$

II部分为中等权重粒子域：

${\{x_t^{\left(i\right)},w_t^{\left(i\right)}\}}_{i=1}^{N-N_{HL}},w_L\&le;w_t^{\left(i\right)}\&le;w_H,$

其中N_HL＝N_H+N_L表示高权重粒子域与低权重粒子域的粒子数量之和，N-N_HL表示中等权重粒子域的粒子数量，表示第i个粒子在t时刻的状态，表示第i个粒子在t时刻的权重；

b.决定第i个粒子被复制的次数n_i，对于II部分中的粒子，直接复制；对I部分的粒子，根据权重由高到低给粒子排序，依次设定粒子的序号为1、2、3、……N_HL，设置每个粒子被选择的概率：

$p\left(i\right)=\frac{{\mathrm{\&alpha;}}_{rank}+\&lsqb;rank\left(i\right)/(N_{HL}-1)\&rsqb;({\mathrm{\&beta;}}_{rank}-{\mathrm{\&alpha;}}_{rank})}{N_{HL}},$

其中rank(i)为某一粒子排序后的序号，α_rank和β_rank为系数，当粒子数确定时，α_rank＝2-β_rank，1≤β_rank≤2，取值为：

$\left\{\begin{array}{ccc}{\mathrm{\&alpha;}}_{rank}=0.5,& {\mathrm{\&beta;}}_{rank}=1.5& rank\left(i\right)\&le;\frac{1}{2}{N}_{HL}\\ {\mathrm{\&alpha;}}_{rank}=0,& {\mathrm{\&beta;}}_{rank}=2& rank\left(i\right)>\frac{1}{2}{N}_{HL}\end{array}\right.,$

如果第i个粒子的概率小于其被选择的概率，即p(i)^*＝(1/N_HL)＜p(i)，则该粒子被舍弃；如果第i个粒子的概率大于或者等于其被选择的概率，即p(i)^*＝(1/N_HL)≥p(i)，则该粒子被复制，被复制的次数n_i由以下决定：

表示舍余取整，

如果粒子被复制的总次数小于采样前的高权重和低权重的粒子总数，则依次增加由最高到低权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权重的粒子总数；如果粒子被复制的总次数大于采样前的高权重和低权重的粒子总数，则依次减少由最低到高权重粒子被复制的次数，使得粒子被复制的总次数等于采样前的高权重和低权重的粒子总数；

复制后高权重的各粒子的权重为 为高权重粒子的权重，n_i*为对应各粒子增加或者减少后实际被复制的次数；其余粒子权重保持不变。

7.根据权利要求6所述的同步定位与地图构建方法，其特征在于，H为2，L为0.5。

8.根据权利要求1所述的同步定位与地图构建方法，其特征在于，更新地图的计算方法为：

根据描述机器人轨迹的粒子的位姿和历史观测信息z_1:t来更新计算对应的地图的后验概率其中为第i粒子在[1,t]时间段的状态，z_1:t为[1,t]时间段的观测信息，为第i粒子的地图信息由第i粒子在[1,t]时间段的状态和[1,t]时间段的观测信息决定。