CN109682382A - 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 - Google Patents

Abstract

本发明公开了一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其包括提取栅格地图中的地图线特征存储至地图特征数据库中；提取非第二帧激光雷达扫描图的扫描图线特征，采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集位姿初始化；对于第二帧激光雷达扫描图，采用里程计数据、运动模型、激光雷达扫描图和测量模型更新粒子的位姿和权重；对于余下帧激光雷达扫描图，采用激光雷达扫描图计算的估算位姿更新粒子集中部分粒子的初始位姿，之后更新相应粒子的权重，同时采用里程计数据、运动模型、激光雷达扫描图和测量模型更新粒子集中余下部分粒子的位姿和权重，最后将两个更新过位姿和权重的粒子集合并成新的粒子集。

Description

基于自适应蒙特卡洛和特征匹配的全局融合定位方法

技术领域

本发明涉及机器人定位技术领域，具体涉及一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法。

背景技术

移动机器人是指在未知环境或者部分未知环境中，完成环境感知、动态决策与规划、行为控制以及自主移动等功能的装置。对于移动机器人来说，定位就是确定机器人在环境地图中的位姿(包括位置和航向角)，它不仅是移动机器人完成环境地图的基本环节，也是机器人实现自主导航的关键技术。

目前机器人在实现全局定位时，仅仅根据传感器信息来估计自身在环境地图中的全局位置，由于没有初始位姿的先验信息，不能使用单峰分布来表示位姿误差，因此全局定位问题比位姿跟踪更困难，而且包含位姿跟踪问题。此外，全局定位问题也至关重要，它能够为移动机器人提供初始位姿，当机器人遇到跟踪失败或者机器人绑架问题时，能够重新定位机器人。

针对机器人的全局定位，目前有采用基于自适应蒙特卡洛(Adaptive MonteCarlo Localization，AMCL)的全局定位算法，其采用输入栅格地图m，激光雷达的观测信息z_1:t以及里程计的运动信息u_1:t，输出估计位姿其处理的大体思路参考图1，其虽能解决定位失效的问题，但是仍存在如下缺点：

1、定位误差较低，定位误差与随机产生的粒子是否出现在真实位姿的附近有关，AMCL采用的方式是增加粒子数，增加粒子的多样性，另一方面，重采样过程使得粒子的多样性不断减少，当粒子没有出现在真实位姿的附近或者当出现一组不正确的随机粒子的权重大于真实位姿附近的粒子权重时，随着粒子的重采样过程，真实位姿附近的粒子将逐渐耗尽，定位误差也将逐渐增大。

2、粒子数的确定，AMCL使用有限粒子数来估计位姿的后验分布，在对大规模地图进行全局定位中，当粒子数较低时，无法正确的估计位姿的后验分布，当粒子数较高时，将增加定位的时间。

3、定位收敛时间较慢，AMCL需要经过多次的激光雷达的观测信息来更新粒子权重，从而使估算粒子的位姿接近真实位姿。

发明内容

针对现有技术中的上述不足，本发明提供的基于自适应蒙特卡洛和特征匹配的全局融合定位方法解决了现有的自适应蒙特卡洛的全局定位算法存在的缺点。

为了达到上述发明目的，本发明采用的技术方案为：

提供一种基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其包括：

S1、获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；

S2、当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；

S3、当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4；

S4、判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12；

S5、提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征；

S6、计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿；

S7、计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿；

S8、采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χ_t位姿初始化，其中粒子用于评估机器人的位姿，之后返回步骤S4；

S9、根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重；

S10、根据粒子集χ_t中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值；

S11、根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集并对粒子集进行重采样，之后返回步骤S4；

S12、采用步骤S9更新粒子集中粒子的位姿和权重；

S13、根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集中粒子的权重；

S14、采用更新了权重和位姿的粒子集和粒子集合并的新粒子集更新粒子集χ_t，之后返回步骤S10。

进一步地，所述根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集进一步包括：

判断相对差值是否大于零；

若是，则令max＝相对差值；否则令max＝零；

根据粒子集χ_t中的总粒子数量M和max，划分粒子集的粒子总数为M·(1-max)，粒子集的粒子总数为M·max。

进一步地，所述提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中进一步包括：

计算当前激光雷达扫描图中相邻数据点之间的距离；

判断所述距离是否大于预设阈值；

若是，则将两个数据点划分至不同的区域；否则，将两个数据点划分至同一区域；

采用分割-合并算法提取每个区域覆盖的激光雷达扫描图中的扫描图线特征存储至地图特征模板中。

进一步地，计算候选位姿的匹配拟合度时，全局扫描数据点的获取方法为：

根据候选位姿表征的在重合过程中计算出来的从激光雷达坐标系转换为栅格地图坐标系的平移和旋转量，将激光雷达坐标系中的扫描数据点转换为栅格地图坐标系下的全局扫描数据点。

进一步地，每对长度差小于长度阈值的地图线特征和扫描图线特征重合时，存在四个候选位姿及四个相对应的平移和旋转。

进一步地，更新粒子的位姿的计算公式为：

其中，p(x_t|x_t-1,u_t)为运动模型；u_t为里程计数据；x_t-1和x_t分别为t-1时刻和t时刻的机器人在栅格地图坐标系下的位姿；和分别为t-1时刻和t时刻第m个粒子在栅格地图坐标系下的位姿。

进一步地，所述根据机器人的里程计数据和运动模型，更新粒子的位姿进一步包括：

A1、根据里程计数据计算初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2：

其中，里程计数据u_t表示从t-1时刻到t时刻机器人的相对运动；为t时刻的机器人在其内部坐标系下的位姿，为的横坐标，为的纵坐标，为的航向角；为t时刻的机器人在其内部坐标系下的位姿，为的横坐标；为的纵坐标；为的航向角；atan2(.)为反正切函数；

A2、对初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2进行高斯采样：

其中，sample(σ)表示均值为0，偏差为σ的高斯采样，α_i(i＝1,…,4)表示给定的参数；

A3、根据上一时刻的位姿x_t-1＝(x y θ)^T和采样的旋转平移量 更新粒子的位姿x_t＝(x′ y′ θ′)^T：

其中，x′为更新后机器人的横坐标；y′为更新后机器人的纵坐标；θ′为更新后机器人的航向角。

进一步地，粒子的权重的计算公式为：

其中，p(z_t|x_t)为激光雷达的测量模型；x_t为t时刻的机器人在栅格地图坐标系下的位姿；为t时刻第m个粒子在栅格地图坐标系下的位姿；为t时刻第m个粒子的权重；z_t为t时刻激光雷达测量值。

进一步地，所述激光雷达的测量模型为激光雷达测量值的局部测量误差p_hit与随机测量误差p_rand和的累乘，K为激光雷达测量数的总和，其计算步骤为：

B1、计算每个测量值的测量误差q_k：

其中，z_hit为局部测量误差权重参数；z_rand为随机测量误差权重参数；为激光雷达第k个测量数据；m为栅格地图信息；x_t为机器人位姿；p_rand为随机测量误差；p_hit为局部测量误差；z_max为激光雷达最大测量距离；为测量的真实距离；为具有均值为标准偏差σ_hit的正态分布；为归一化因子；z_max为激光雷达的最大测量值；

B2、计算所有测量值的测量误差p(z_t|x_t)：

进一步地，所述估计位姿的计算公式为：

其中，为粒子归一化；为机器人的估计位姿。

本发明的有益效果为：

1、具有较高的定位精度，特征匹配的度量采用全局扫描数据点到栅格地图中最近栅格占据点的距离的累加，与栅格地图特征更贴切，从而使得定位精度更准确。

2、使用粒子数目较少，当蒙特卡洛定位用作于位姿跟踪时，可以用比较少的粒子数完成定位，用作全局定位时，需要比较多的粒子数才能完成定位，由于本方案仅在位姿跟踪时采用蒙特卡洛定位，故使用较少的粒子数来完成位姿跟踪。

3、具有较快的收敛时间，先使用特征匹配完成全局定位，再进行位姿跟踪，使得在全局定位中只用少量的观测信息就能完成收敛，具有较快的收敛时间。

4、具有较低的经济成本，在全局定位时，仅需要传感器上的里程计数据和激光雷达测量数据，无需部署其他任何信标，从而减少***部署成本。

附图说明

图1为现有技术基于自适应蒙特卡洛的全局定位算法的流程图。

图2为本方案的基于自适应蒙特卡洛和特征匹配的全局融合定位方法的流程图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

参考图2，图2示出了基于自适应蒙特卡洛和特征匹配的全局融合定位方法的流程图；如图2所示，该方法S包括步骤S1至步骤S14。

在步骤S1中，获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；步骤S1可以进一步细化为：

接收到栅格地图m后，首先将栅格地图m转换为二值图像，图像中的像素值即为栅格地图中的栅格值，之后采用LSD(Line Segment Detector)算法提取栅格地图的线特征，地图线特征是在全局地图坐标系(栅格地图坐标系)下，通过计算栅格地图中每条线段的斜率k，截距b，起点A坐标(x₁,y₁)，终点B坐标(x₂,y₂)，线段长度len五个参数来描述，地图特征数据库记录栅格地图中所有线特征。

在步骤S2中，当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；在步骤S3中，当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4。

在步骤S4中，判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12。

在步骤S5中，提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征。

在本发明的一个实施例中，所述提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中进一步包括：

计算当前激光雷达扫描图中相邻数据点之间的距离；

判断所述距离是否大于预设阈值；

若是，则将两个数据点划分至不同的区域；否则，将两个数据点划分至同一区域；

采用分割-合并算法提取每个区域覆盖的激光雷达扫描图中的扫描图线特征存储至地图特征模板中。

采用上述分成若干区域的方式进行扫描图线特征的提取，可以避免分开合并算法引入错误的线特征。

在步骤S6中，计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿。

下面以一个具体的实例对候选位姿的计算进行说明：

以地图特征模板中一条扫描图线特征l_M中的线段长度len为标准，在地图特征数据库中寻找长度差小于阈值的地图线特征l_D，通过平移和旋转激光雷达坐标系，使得扫描图线特征l_M和地图线特征l_D能够重合，而将激光雷达坐标系中原点通过上述的平移和旋转后即为候选位姿。

其中每对长度差小于长度阈值的地图线特征和扫描图线特征重合时，存在四个候选位姿及四个相对应的平移和旋转。下面继续以扫描图线特征l_M和地图线特征l_D为例对四个候选位姿及四个相对应的平移和旋转进行说明：

由于扫描图线特征l_M和地图线特征l_D的长度不完全相等，则存在四种重合情况，即其中，A_M，B_M为扫描图线特征l_M的起点和终点，A_D，B_D为地图线特征l_D的起点和终点，表示l_M和l_D重合时，起点A_M与起点A_D重合，表示l_M和l_D重合时，起点A_M与终点B_D重合，表示l_M和l_D重合时，终点B_M与起点A_D重合，表示l_M和l_D重合时，终点B_M与终点B_D重合。因此，每对线特征l_M和l_D重合时，会有四个候选位姿以及四个相应的平移和旋转。

在步骤S7中，计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿。

其中，选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离d_i的计算公式为：

匹配拟合度的计算公式为：

估算位姿为所有候选位姿集中最小匹配拟合度的候选位姿：

其中，(x′,y′)为栅格地图m中占据点的位置，{(x′,y′)}为所有占据点位置的集合，d_i为全局扫描数据点i到最近栅格占据点的距离；为候选位姿集中的候选位姿，为估算位姿。

实施时，本方案优选计算候选位姿的匹配拟合度时，全局扫描数据点的获取方法为：

根据候选位姿表征的在重合过程中计算出来的从激光雷达坐标系转换为栅格地图坐标系的平移和旋转量，将激光雷达坐标系中的扫描数据点转换为栅格地图坐标系下的全局扫描数据点。

在步骤S8中，采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χ_t位姿初始化，之后返回步骤S4，其中粒子用于评估机器人的位姿；

在步骤S9中，根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重。

在本发明的一个实施例中，更新粒子的位姿的计算公式为：

其中，p(x_t|x_t-1,u_t)为运动模型；u_t为里程计数据；x_t-1和x_t分别为t-1时刻和t时刻的机器人在栅格地图坐标系下的位姿；和分别为t-1时刻和t时刻第m个粒子在栅格地图坐标系下的位姿。

实施时，本方案优选所述根据机器人的里程计数据和运动模型，更新粒子的位姿进一步包括：

A1、根据里程计数据计算初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2：

其中，里程计数据u_t表示从t-1时刻到t时刻机器人的相对运动；为t时刻的机器人在其内部坐标系下的位姿，为的横坐标，为的纵坐标，为的航向角；为t时刻的机器人在其内部坐标系下的位姿，为的横坐标；为的纵坐标；为的航向角；atan2(.)为反正切函数；

A2、对初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2进行高斯采样：

其中，sample(σ)表示均值为0，偏差为σ的高斯采样，α_i(i＝1,…,4)表示给定的参数；

A3、根据上一时刻的位姿x_t-1＝(x y θ)^T和采样的旋转平移量 更新粒子的位姿x_t＝(x′ y′ θ′)^T：

其中，x′为更新后机器人的横坐标；y′为更新后机器人的纵坐标；θ′为更新后机器人的航向角。

在本发明的一个实施例中，，粒子的权重的计算公式为：

其中，p(z_t|x_t)为激光雷达的测量模型；x_t为t时刻的机器人在栅格地图坐标系下的位姿；为t时刻第m个粒子在栅格地图坐标系下的位姿；为t时刻第m个粒子的权重；z_t为t时刻激光雷达测量值。

实施时，本方案优选所述激光雷达的测量模型为激光雷达测量值的局部测量误差p_hit与随机测量误差p_rand和的累乘，K为激光雷达测量数的总和，其计算步骤为：

B1、计算每个测量值的测量误差q_k：

其中，z_hit为局部测量误差权重参数；z_rand为随机测量误差权重参数；为激光雷达第k个测量数据；m为栅格地图信息；x_t为机器人位姿；p_rand为随机测量误差；p_hit为局部测量误差；z_max为激光雷达最大测量距离；为测量的真实距离；为具有均值为标准偏差σ_hit的正态分布；为归一化因子；z_max为激光雷达的最大测量值；

B2、计算所有测量值的测量误差p(z_t|x_t)：

在步骤S10中，根据粒子集χ_t中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值。

其中，所述估计位姿的计算公式为：

其中，为粒子归一化；为机器人的估计位姿。

实施时，本方案优选粒子权重的平均值w_avg，长期均值w_slow，短期均值w_fast以及长期均值与短期均值的相对差值w_diff的计算公式分别为：

w_slo_w＝w_slo_w+α_slo_w(w_avg-w_slo_w)

w_fast＝w_fast+α_fast(w_avg-w_fast)

其中，M为粒子集χ_t的总数，0≤α_fast＜＜α_slow，α_fast，α_slow分别为短期均值和长期均值的衰减率。

在步骤S11中，根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集并对粒子集进行重采样，之后返回步骤S4。

在本发明的一个实施例中，所述根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集进一步包括：

判断相对差值是否大于零；

若是，则令max＝相对差值；否则令max＝零；

根据粒子集χ_t中的总粒子数量M和max，划分粒子集的粒子总数为M·(1-max)，粒子集的粒子总数为M·max。

在步骤S12中，采用步骤S9更新粒子集中粒子的位姿和权重；

在步骤S13中，根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集中粒子的权重；

在步骤S14中，采用更新了权重和位姿的粒子集和粒子集合并的新粒子集更新粒子集χ_t，之后返回步骤S10。

综上所述，本方案采用MCL与特征匹配相联合进行机器人全局定位，首先先使用特征匹配完成全局定位，再使用MCL完成位姿跟踪，通过该种方式具有定位精准、采用粒子数量少、收敛时间快和经济成本低等优点。

Claims (10)

1.基于自适应蒙特卡洛和特征匹配的全局融合定位方法，其特征在于，包括：

S1、获取机器人在检测环境行走时生成的栅格地图，并提取栅格地图中的地图线特征存储至地图特征数据库中；

S2、当不存在机器人的激光雷达扫描图和里程计数据输入时，退出全局融合定位；

S3、当存在机器人的激光雷达扫描图和里程计数据输入时，进入步骤S4；

S4、判断当前激光雷达扫描图是否为第一帧、第二帧或余下帧，若是第一帧进入步骤S5，若是第二帧进入步骤S9，若是余下帧进入步骤S12；

S5、提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中，查找地图特征数据库与地图特征模板中长度差小于长度阈值的地图线特征和扫描图线特征；

S6、计算长度差小于长度阈值的地图线特征和扫描图线特征重合时，激光雷达坐标系与栅格地图坐标系的转换关系，并利用转换关系平移和旋转激光雷达坐标系中原点计算得到候选位姿；

S7、计算候选位姿上的所有全局扫描数据点到栅格地图中最近栅格占据点的距离的累加作为候选位姿的匹配拟合度，并将匹配拟合度最小的候选位姿作为估算位姿；

S8、采用第一帧激光雷达扫描图计算的估算位姿对自适应蒙特卡洛的粒子集χ_t位姿初始化，其中粒子用于评估机器人的位姿，之后返回步骤S4；

S9、根据机器人的里程计数据和运动模型，更新粒子的位姿；根据激光雷达扫描图和测量模型，计算粒子的权重；

S10、根据粒子集χ_t中粒子的权重和位姿，计算粒子位姿的期望作为机器人的估计位姿，之后计算粒子权重的平均值、长期均值、短期均值及长期均值与短期均值的相对差值；

S11、根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集并对粒子集进行重采样，之后返回步骤S4；

S12、采用步骤S9更新粒子集中粒子的位姿和权重；

S13、根据当前激光雷达扫描图执行步骤S5至步骤S7，并采用得到的估算位姿更新粒子集中粒子的位姿，之后采用当前激光雷达扫描图和测量模型，计算粒子集中粒子的权重；

S14、采用更新了权重和位姿的粒子集和粒子集合并的新粒子集更新粒子集χ_t，之后返回步骤S10。

2.根据权利要求1所述的全局融合定位方法，其特征在于，所述根据相对差值与零的大小关系，将粒子集χ_t划分成粒子集和粒子集进一步包括：

判断相对差值是否大于零；

若是，则令max＝相对差值；否则令max＝零；

根据粒子集χ_t中的总粒子数量M和max，划分粒子集的粒子总数为M·(1-max)，粒子集的粒子总数为M·max。

3.根据权利要求1所述的全局融合定位方法，其特征在于，所述提取激光雷达扫描图中的扫描图线特征存储至地图特征模板中进一步包括：

计算当前激光雷达扫描图中相邻数据点之间的距离；

判断所述距离是否大于预设阈值；

若是，则将两个数据点划分至不同的区域；否则，将两个数据点划分至同一区域；

采用分割-合并算法提取每个区域覆盖的激光雷达扫描图中的扫描图线特征存储至地图特征模板中。

4.根据权利要求1所述的全局融合定位方法，其特征在于，计算候选位姿的匹配拟合度时，全局扫描数据点的获取方法为：

根据候选位姿表征的在重合过程中计算出来的从激光雷达坐标系转换为栅格地图坐标系的平移和旋转量，将激光雷达坐标系中的扫描数据点转换为栅格地图坐标系下的全局扫描数据点。

5.根据权利要求1所述的全局融合定位方法，其特征在于，每对长度差小于长度阈值的地图线特征和扫描图线特征重合时，存在四个候选位姿及四个相对应的平移和旋转。

6.根据权利要求1所述的全局融合定位方法，其特征在于，更新粒子的位姿的计算公式为：

其中，p(x_t|x_t-1,u_t)为运动模型；u_t为里程计数据；x_t-1和x_t分别为t-1时刻和t时刻的机器人在栅格地图坐标系下的位姿；和分别为t-1时刻和t时刻第m个粒子在栅格地图坐标系下的位姿。

7.根据权利要求6所述的全局融合定位方法，其特征在于，所述根据机器人的里程计数据和运动模型，更新粒子的位姿进一步包括：

A1、根据里程计数据计算初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2：

其中，里程计数据u_t表示从t-1时刻到t时刻机器人的相对运动；为t时刻的机器人在其内部坐标系下的位姿，为的横坐标，为的纵坐标，为的航向角；为t时刻的机器人在其内部坐标系下的位姿，为的横坐标；为的纵坐标；为的航向角；atan2(.)为反正切函数；

A2、对初次旋转δ_rot1、平移δ_trans和第二次旋转δ_rot2进行高斯采样：

其中，sample(σ)表示均值为0，偏差为σ的高斯采样，α_i(i＝1,…,4)表示给定的参数；

A3、根据上一时刻的位姿x_t-1＝(x y θ)^T和采样的旋转平移量 更新粒子的位姿x_t＝(x′ y′ θ′)^T：

其中，x′为更新后机器人的横坐标；y′为更新后机器人的纵坐标；θ′为更新后机器人的航向角。

8.根据权利要求1所述的全局融合定位方法，其特征在于，粒子的权重的计算公式为：

其中，p(z_t|x_t)为激光雷达的测量模型；x_t为t时刻的机器人在栅格地图坐标系下的位姿；为t时刻第m个粒子在栅格地图坐标系下的位姿；为t时刻第m个粒子的权重；z_t为t时刻激光雷达测量值。

9.根据权利要求8所述的全局融合定位方法，其特征在于，所述激光雷达的测量模型为激光雷达测量值的局部测量误差p_hit与随机测量误差p_rand和的累乘，K为激光雷达测量数的总和，其计算步骤为：

B1、计算每个测量值的测量误差q_k：

其中，z_hit为局部测量误差权重参数；z_rand为随机测量误差权重参数；为激光雷达第k个测量数据；m为栅格地图信息；x_t为机器人位姿；p_rand为随机测量误差；p_hit为局部测量误差；z_max为激光雷达最大测量距离；为测量的真实距离；为具有均值为标准偏差σ_hit的正态分布；为归一化因子；z_max为激光雷达的最大测量值；

B2、计算所有测量值的测量误差p(z_t|x_t)：

10.根据权利要求6-9任一所述的全局融合定位方法，其特征在于，所述估计位姿的计算公式为：

其中，为粒子归一化；为机器人的估计位姿。