CN109443351B

CN109443351B - 一种稀疏环境下的机器人三维激光定位方法

Info

Publication number: CN109443351B
Application number: CN201910002100.0A
Authority: CN
Inventors: 林欢; 李彬; 李栗; 张利平; 王�锋
Original assignee: Yijiahe Technology Co Ltd
Current assignee: Yijiahe Technology Co Ltd
Priority date: 2019-01-02
Filing date: 2019-01-02
Publication date: 2020-08-11
Anticipated expiration: 2039-01-02
Also published as: CN109443351A

Abstract

本发明属于智能机器人技术领域，公开了一种稀疏环境下的机器人三维激光定位方法，包括：限制机器人的三个自由度，将机器人的运动限制在一个水平平面内；采用粒子滤波算法，将来自机器人的三维激光传感器的激光数据结合里程计数据和惯性导航数据计算机器人位姿的初值估计值；定位扫描窗口的步骤；计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计。采用本发明的稀疏环境下的机器人三维激光定位方法，定位相关计算量大大降低，定位效率和定位精度得到明显提高。

Description

一种稀疏环境下的机器人三维激光定位方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种稀疏环境下的机器人三维激光定位方法。

背景技术

在我国电力行业已引入机器人技术，来实现对变电站设备的巡检工作，而采用机器人进行巡检工作面临的就是机器人运行中的定位问题。变电站机器人定位是指机器人通过感知环境信息，经过相关信息处理而确定自身及目标位姿的过程。

现有的智能移动机器人自主定位方法主要有相对定位和绝对定位两种方式，绝对定位就是自主移动机器人利用传感器测量其在空间坐标系中绝对位置信息来实现全局定位的，要求具有较高的定位精度，条件允许的话也用来修正相对定位过程的***误差，以便于达到更高精度的绝对定位标准。相对定位是指机器人知晓其起始位置，只需考虑其运动过程中位移和角度的理论值和测量值之间的偏差即可。在机器人的定位研究中，精确定位问题只靠相对定位是不能很好地解决，当然独自使用绝对定位也是如此。因为这两种定位方法单一使用都存在其或多或少的不足之处。相对定位短时间内精度和稳定性较好，但是测量误差会不断积累，最终导致测量失败。绝对定位测量误差具有时间和空间的独立性，不会随着时间而累积，但容易受到干扰，短时间波动较大。故需要采用组合定位的方式即将相对定位和绝对定位方法结合在一起使用来进行定位，即采用相对定位的方法进行粗定位，再利用绝对定位的方法对定位结果进行校正，从而可以准确定位出机器人的位姿。

变电站室外环境为稀疏环境，具有以下几个特征：

·地势平坦：变电站一般处于地形平坦、表面平整的地区；

·面积较大：大部分变电站面积都在1万平方米以上；

·环境相对静止：变电站内人、车等障碍物出现频次低，环境变化小；

·环境特征点少：缺少特征标志的障碍物。

中国专利CN201810048969.4公开了一种稀疏环境下的机器人实时定位方法，在二维激光传感器的基础上，通过计算机器人下一帧对应时间点的预测位姿初值；根据最新接收的里程计数据和惯性导航数据校正位姿初值，再根据定位扫描窗口确定所有可能的候选位姿；最后计算每个候选位姿的置信度与置信度权重的乘积作为当前位姿的置信度分值，取置信度分值最高的候选位姿作为下一帧数据对应时间点位姿的最优估计。该方法使得定位相关计算量大大降低，定位效率和定位精度得到明显提高。然而该方法是在二维激光传感器的基础上实现机器人的定位，二维激光传感器只能探测到某一高度的环境信息，很难实现对环境的彻底扫描，不能有效描述出空间的三维结构，从而影响机器人的定位精度。

粒子滤波(PF，Particle Filter)的思想基于蒙特卡洛方法，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这些样本被形象的称为“粒子”。当样本数量N趋于无限大时可以逼近任何形式的概率密度分布。

发明内容

本发明目的是：针对现有技术的不足，提供一种稀疏环境下的机器人三维激光定位方法。本方法可以满足复杂空间环境下定位的高效性、鲁棒性和准确性要求。

具体地说，本发明是采用以下技术方案实现的，包括：

初值估算的步骤：

限制机器人的三个自由度，将机器人的运动限制在一个水平平面内；

采用粒子滤波算法，将来自机器人的三维激光传感器的激光数据结合里程计数据和惯性导航数据计算机器人位姿的初值估计值；

定位扫描窗口的步骤：

计算定位扫描窗口大小，根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿；

每得到一帧来自机器人的三维激光传感器的激光数据后，计算各扫描角度下各激光反射点在地图坐标系中的坐标；并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；

置信度估算的步骤：

计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计。

进一步而言，所述初值估算的步骤中，限制机器人的三个自由度，将机器人的运动限制在一个平面内的具体方法为：

将机器人位姿中垂直于机器人运动平面的坐标值限制为指定值，绕机器人运动平面中的两个坐标轴旋转的角度限制为指定值。

进一步而言，所述初值估算的步骤中，采用粒子滤波算法，将来自机器人的三维激光传感器的激光数据结合里程计数据和惯性导航数据计算机器人位姿的初值估计值的方法包括：

通过来自机器人的三维激光传感器的两帧激光数据中位姿，确定机器人初始位姿和初始速度；

根据最新接收的惯性导航数据获取机器人的加速度，结合所述初始速度，更新机器人的运动速度，建立机器人的位姿预测方程，根据最新接收的里程计数据建立观测模型；

根据所述机器人的初始位姿，在机器人的运动范围内随机生成若干个均匀分布的粒子，且粒子具有机器人所具有的三个自由度；

根据所述位姿预测方程，更新粒子的状态；

根据所述观测模型和最新接收的里程计数据，计算粒子的权重；

通过计算所述若干个均匀分布的粒子的加权平均值得到机器人位姿的初值估计值；

当有效粒子数小于指定值时，进行重采样。

进一步而言，所述有效粒子数的计算公式为：

其中，N_eff为有效粒子数，ω_i为所述若干个均匀分布的粒子中第i个粒子的权重。

进一步而言，所述初值估算的步骤中，机器人的位姿预测方程为：

P_t ^T＝P^T _t-1+[V_t-1+A_t-1Δt]^T+u_t-1

其中，P_t、P_t-1分别为机器人分别在t时刻、t-1时刻接收连续两帧激光数据对应的位姿，

为t-1时刻机器人的速度，

为t-1时刻机器人在X轴方向的速度、

为t-1时刻机器人在y轴方向的速度，

为t-1时刻机器人角速度，Δt为t时刻与t-1时刻间的时间差，A_t-1为t-1时刻机器人的加速度，u_t-1为t-1时刻的***噪声；

所述观测模型为：

z_t-1＝(x_t-1,y_t-1,θ_t-1)

其中，z_t-1为里程计的观测模型，x_t-1为机器人在t-1时刻接收的里程计数据中x轴方向的数据，y_t-1为机器人在t-1时刻接收的里程计数据中y轴方向的数据，θ_t-1为机器人在t-1 时刻接收的里程计数据中角速度。

进一步而言，所述初值估算的步骤中，计算粒子的权重的具体方法为：

ω_i＝1/d_i

其中，ω_i为第i个粒子的权重，d_i为第i个粒子距离观测模型取值的欧氏距离。

进一步而言，所述置信度估算的步骤中，计算各可能的候选位姿的置信度的具体方法为：

根据每个候选位姿对应的扫描角度的离散扫描数据中各个地图栅格的置信度，计算每个候选位姿的置信度candidate_probability，公式如下：

其中，m为每个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设第n 个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

根据每个候选位姿与位姿的估计值的位姿差来计算每个候选位姿对应的置信度权重 candidate_weight，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值间沿y轴的位移，trans_weigh是位移权重，candiate.rotation是候选位姿与位姿的估计值间旋转角度，rot_weigh是旋转角度权重；

将每个候选位姿的置信度candidate_probability与置信度权重 candidate_weight的乘积作为当前位姿的置信度分值score，公式如下：

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿作为最优位姿估计。

进一步而言，所述初值估算的步骤中，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

进一步而言，所述初值估算的步骤中，在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续各扫描角度下各激光反射点在地图坐标系中的坐标的计算。

进一步而言，所述定位扫描窗口的步骤中，对于某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。

本发明的有益效果如下：采用本发明的稀疏环境下的机器人三维激光定位方法，利用粒子滤波算法，结合里程计、惯性导航单元等多传感器融合的数据信息，对机器人的位姿初值进行估算，以此为输入进行基于概率栅格模型的三维激光定位，有效避免因粒子退化问题导致的定位漂移现象。通过限定机器人的z坐标和欧拉角为固定值，解决了定位过程中的z轴漂移问题。整个定位方法在提高定位精度的同时，提高了定位效率。通过本发明提供的一种稀疏环境下的机器人三维激光定位方法，更加直观的描述变电站的环境信息，提供更加精确的定位数据，使变电站巡检机器人在复杂环境定位过程中表现出高效性、鲁棒性、准确性。

附图说明

图1是本发明实施例的定位流程图。

图2是本发明实施例的机器人位姿示意图。

图3是本发明实施例的定位扫描示意图。

具体实施方式

下面结合实施例并参照附图对本发明作进一步详细描述。

实施例：

本发明的一个实施例，介绍了一种稀疏环境下的机器人三维激光定位方法。

本实施例采用的机器人，其硬件***传感器主要包括里程计、惯性导航单元、三维激光传感器，其中里程计可以用来校准位置的预测值，惯性导航单元可以用来校准线速度、角速度，三维激光传感器用来获得激光数据。机器人软件***使用机器人操作***ROS，这是一种常用的机器人软件平台，它能为异质计算机集群提供类似操作***的功能，在ROS***中包括实现定位功能的节点。当然，可以理解的是，本发明方法也可以通过其他机器人的软件***加以实现，本实施例采用ROS***仅作为一种实现方式。

获得各帧激光数据的时间点是计算位姿的时间点。假定机器人在时间点t的位姿是P(t)，前一个计算位姿的时间点为t-1，相应的位姿是P(t-1)，下一个计算位姿的时间点t+1，相应的的位姿是P(t+1)。通过时间点t和t-1之间的位姿差(包括位移和旋转角度)来计算机器人运动的线速度V(x,y)和角速度W。

线速度V(x,y)＝(t和t-1之间的位移)/时间差

角速度W＝(t和t-1之间的旋转角度)/时间差

由于巡检机器人的移动速度较慢，最大约为1米/秒，并且两个时间点之间的时间差很小，一般为ms级别。因此可以使用计算的速度(包括线速度和角速度)来预测机器人在时间点t+1的位姿P(t+1)。由于实际硬件的误差，机器人在时间点t+1的准确位姿与该预测位姿之间会有偏差。通过激光测量数据和预测位姿对应地图数据之间符合的程度来优化P(t+1)，最终获得最接近准确位姿的最优位置。

本实施例中使用的坐标系包括地图坐标系、机器人坐标系、激光传感器坐标系。

地图坐标系是全局坐标系，在地图构建结束以后确定。计算机器人位姿时使用的是地图坐标系。

机器人坐标系是以机器人为原点的坐标系，二维导航中，一般是以机器人的中心点为原点。

激光传感器坐标系是以激光传感器的中心位置为原点的坐标系，激光数据的位姿使用的是激光传感器坐标系。

需要把不同坐标系下面的数据转化到同一个坐标系，才能进行位姿比较和计算。不同坐标系间坐标值的转换可以通过ROS***中TF模块(坐标转换模块)实现。

机器人在时间点t的位姿P(t)可以表示为机器人在地图坐标系中的位置(x,y)以及机器人与x轴间角度r的函数P(x,y,r)。

参照图1，本实施例的稀疏环境下的机器人三维激光实时定位方法，其实现的主要步骤如下：

1、预测位姿初值估算。

1)限制机器人的三个自由度。

由于变电站一般处于平原地区，地形平坦，极少出现大幅度的上下坡情况，且变电站的表面较为平整，因此巡检机器人的运动处于一个平面内，在三维环境中不需要z轴方向的移动和旋转。同时，为了解决机器人三维激光定位过程中出现的z轴漂移问题，本实施例中限制了机器人绕x轴旋转、绕y轴旋转、沿z轴移动这三个自由度。即将机器人位姿中z坐标的值限制为z＝z₀，机器人的位姿中绕x轴旋转的欧拉角限制为α＝α₀，绕y轴旋转的欧拉角限制为β＝β₀。此时，机器人在三维空间中的运动范围为平面z＝z₀，即只需要在二维平面 z＝z₀中对机器人进行定位即可，如图2所示。其中，P(t)表示机器人在t时刻的位姿。

2)变电站巡检机器人接收巡检任务，按照指定路线行驶，在行驶过程中以固定采样频率记录三维激光数据，里程计数据，惯性导航单元(IMU)数据。

3)每得到一帧来自机器人的三维激光传感器的激光数据laserscan后，ROS***对激光数据进行处理得到激光点云数据pointcloud，该激光点云数据pointcloud是每帧激光数据所包含的laserscan.size()个激光数据点信息的统称，反映了各激光反射点在激光传感器坐标系中的坐标。不同激光传感器的帧率、扫描角度、每帧包含的数据点数量不同，ROS***在定位过程中使用激光点云数据来进行处理和计算。针对稀疏环境的特点，可以对激光点云数据 pointcloud进行过滤，即去掉激光点云数据pointcloud中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点，可以提高定位估计的置信度。由于对每一帧激光数据对应的激光点云数据pointcloud进行过滤的结果不同，每一帧激光数据对应的有效点的值不同。例如，在laserscan.size()为1141的传感器上，有效点约为几百个。对于激光数据laserscan，要进行帧率的检测，即在每得到一帧激光数据后，检查激光数据laserscan的帧率是否小于阈值，如果小于阈值则上报告警并等待接收到下一帧激光数据后重新进行相应处理。例如，如果定义的帧率是25Hz，而得到相邻两帧激光数据的时间大于40ms，则激光数据laserscan不符合要求，可能激光传感器出现过热等故障，需要上报告警并等待下一帧激光数据。

4)利用粒子滤波算法，结合里程计、IMU等多传感器融合的数据信息，对机器人的位姿初值进行估算。

在机器人实时定位过程中，有一些粒子可能偏离目标太远。这些粒子是我们想要的***状态的可能性非常小，因此取得的权值非常小。如果只根据粒子传播来改变粒子的状态的话，偏离的粒子永远是偏离的，很难再回到目标周围。这样，偏离的粒子可能越积越多，从而导致定位出现偏差。本实施例中，使用粒子滤波进行多传感器融合的初始位姿估算。

根据IMU数据建立机器人的位姿预测方程，利用里程计数据建立观测模型，位姿预测方程用于更新粒子的状态，观测模型用于计算粒子的权重，最后通过计算粒子群的加权平均值得到机器人位姿的初值估算值。

1-4-1)通过来自机器人的三维激光传感器的两帧激光数据中位姿，确定机器人初始位姿 P₀(x₀,y₀,θ₀)以及初始速度

1-4-2)根据最新接收的惯性导航数据获取机器人的加速度

其中

为t时刻机器人在X轴方向的加速度，

为t时刻机器人在y轴方向的加速度，

为t时刻机器人角加速度。结合初始速度

更新机器人的运动速度，建立机器人的位姿预测方程。假设t时刻机器人的速度为

其中，

为t时刻机器人在X轴方向的速度、

为t时刻机器人在y轴方向的速度，

为t时刻机器人角速度，则机器人的位姿预测方程可以表示为：

P_t ^T＝P^T _t-1+[V_t-1+A_t-1Δt]^T+u_t-1

为t-1时刻机器人的速度，

为t-1时刻机器人在x轴方向的速度、

为t-1时刻机器人在y轴方向的速度，

为t-1时刻机器人角速度，Δt为t时刻与t-1时刻间的时间差，A_t-1为t-1时刻机器人的加速度，u_t-1为t-1时刻的***噪声。

根据最新接收的里程计数据建立如下观测模型：

z_t-1＝(x_t-1,y_t-1,θ_t-1)

以机器人的位姿预测方程为输入，进行基于概率栅格模型的三维激光定位，可以有效避免因粒子退化问题导致的定位漂移现象。

1-4-3)生成粒子。根据预先确定机器人的初始位姿P₀＝(x₀,y₀,θ₀)，初始时刻在机器人的运动范围内随机生成N个均匀分布的粒子，且粒子具有

i＝1,2,3…N，三个特征。

1-4-4)利用机器人运动模型预测状态。根据1-4-2)中求取的机器人的运动位姿预测方程，更新1-4-2)中每一个粒子的状态。由于控制***存在噪声，需加入合理的噪声。

1-4-5)更新粒子权值。使用里程计得到的观测值更新粒子的权重，随着观测值的依次到达，为每个粒子计算相应的权重。该权重值代表了预测的位姿取每个粒子时获得观测模型的概率。对所有粒子都进行这样的评价。越接近观测值的粒子，获得的权重越高。权重的计算方式如下：

ω_i＝1/d_i

1-4-6)计算状态变量估计值。通过粒子群的加权平均值计算状态变量的估计值

即机器人接收第k帧激光数据时机器人位姿的估计值pose_estimated。

1-4-7)重采样。为了解决计算过程中的权值退化问题，采用有效粒子数N_eff衡量粒子的退化程度：

其中ω_i为第i个粒子的权重。有效粒子数N_eff越小，表明权重退化越严重。

当N_eff的值小于某一阈值N_th时，进行重采样；否则，返回1-4-4)。重采样的具体方法为：根据粒子的权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，舍弃一小部分权重小的粒子，代之以权重较大的粒子。

2、定位窗口扫描。

根据机器人位姿的估计值pose_estimated设置定位扫描窗口。如图3所示，定位窗口扫描时使用位移扫描参数linear_search_window和角度扫描参数，其中角度扫描参数包括扫描角度大小angular_search_window和扫描角度步长angular_step。位移扫描参数用于限定定位扫描窗口的位移范围为以位姿的估计值pose_estimated为中心，上下左右各偏离linear_search_window大小的正方形。角度扫描参数用于限定定位扫描窗口的角度范围为以位姿的估计值pose_estimated为中心角度，上下各偏离angular_search_window大小的角度。

定位扫描窗口大小确定定位扫描窗口内各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿possible_candidates。

候选位姿是机器人可能的位置和朝向，也即激光传感器可能的位置和朝向。当激光传感器放置在不同位置和朝向即机器人采用不同的候选位姿时，同一激光点云数据映射到地图坐标系中会得到不同的一套坐标。在各套地图坐标中选择候选位姿置信度最高的那套坐标作为激光点云数据在地图坐标系的坐标。

确定各候选位姿对应的激光点云数据在地图坐标系中坐标的方法为：根据机器人的定位扫描窗口(通过位姿的估计值pose_estimated、角度扫描参数和位移扫描参数确定)和激光点云数据pointcloud计算各扫描角度的激光点云数据rotatedcloud，这个rotatedcloud反映了各激光反射点在地图坐标系中的坐标。根据位姿的估计值pose_estimated和各扫描角度的激光点云数据rotatedcloud计算各扫描角度下各激光反射点对应的地图栅格位置(即计算每个地图栅格在地图坐标系中的坐标)，作为各扫描角度的离散扫描数据discretizescans。对某个扫描角度的离散扫描数据discretizescans而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。

3、置信度计算。

根据每个候选位姿possible_candidate对应的各个地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，计算每个候选位姿的置信度 candidate_probability，公式如下：

其中，m为某个候选位姿对应的扫描角度的离散扫描数据discretizescan中的地图栅格的总数，设其中第n个栅格的地图坐标为(x_n,y_n)，该栅格置信度是

取值范围为0.1～0.9。

根据每个候选位姿possible_candidate与位姿的估计值pose_estimated的位姿差来计算每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值pose_estimated间沿x轴的位移，y_offset 是每个候选位姿与位姿的估计值pose_estimated间沿y轴的位移，trans_weigh是位移权重， candiate.rotation是候选位姿与位姿的估计值pose_estimated间旋转角度，rot_weight是旋转角度权重；

将每个候选位姿的置信度candidate_probability与置信度权重candidate_weight的乘积作为当前位姿的置信度分值，公式如下，

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿更新位姿的估计值pose_estimated，作为最优位姿估计，即作为P(t+1)，定位结束。

虽然本发明已以较佳实施例公开如上，但实施例并不是用来限定本发明的。在不脱离本发明之精神和范围内，所做的任何等效变化或润饰，同样属于本发明之保护范围。因此本发明的保护范围应当以本申请的权利要求所界定的内容为标准。

Claims

1.一种稀疏环境下的机器人三维激光定位方法，其特征在于，包括：

初值估算的步骤：

定位扫描窗口的步骤：

置信度估算的步骤：

计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计；

所述初值估算的步骤中，采用粒子滤波算法，将来自机器人的三维激光传感器的激光数据结合里程计数据和惯性导航数据计算机器人位姿的初值估计值的方法包括：

根据所述位姿预测方程，更新粒子的状态；

当有效粒子数小于指定值时，进行重采样。

2.根据权利要求1所述的一种稀疏环境下的机器人三维激光定位方法，其特征在于，所述初值估算的步骤中，限制机器人的三个自由度，将机器人的运动限制在一个平面内的具体方法为：

3.根据权利要求1所述的一种稀疏环境下的机器人三维激光定位方法，其特征在于，所述有效粒子数的计算公式为：

4.根据权利要求1所述的一种稀疏环境下的机器人三维激光定位方法，其特征在于，所述初值估算的步骤中，机器人的位姿预测方程为：

P_t ^T＝P^T _t-1+[V_t-1+A_t-1Δt]^T+u_t-1

为t-1时刻机器人的速度，

为t-1时刻机器人在x轴方向的速度、

为t-1时刻机器人在y轴方向的速度，

所述观测模型为：

z_t-1＝(x_t-1,y_t-1,θ_t-1)

其中，z_t-1为里程计的观测模型，x_t-1为机器人在t-1时刻接收的里程计数据中x轴方向的数据，y_t-1为机器人在t-1时刻接收的里程计数据中y轴方向的数据，θ_t-1为机器人在t-1时刻接收的里程计数据中角速度。

5.根据权利要求1所述的一种稀疏环境下的机器人三维激光定位方法，其特征在于，所述初值估算的步骤中，计算粒子的权重的具体方法为：

ω_i＝1/d_i

6.根据权利要求1所述的一种稀疏环境下的机器人三维激光定位方法，其特征在于，所述置信度估算的步骤中，计算各可能的候选位姿的置信度的具体方法为：

其中，m为每个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

根据每个候选位姿与位姿的估计值的位姿差来计算每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值间沿y轴的位移，trans_weight是位移权重，candiate.rotation是候选位姿与位姿的估计值间旋转角度，rot_weig是旋转角度权重；

将每个候选位姿的置信度candidate_probability与置信度权重candidate_weight的乘积作为当前位姿的置信度分值score，公式如下：

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿作为最优位姿估计。

7.根据权利要求1～6任一所述的稀疏环境下的机器人三维激光定位方法，其特征在于，所述初值估算的步骤中，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

8.根据权利要求7所述的稀疏环境下的机器人三维激光定位方法，其特征在于，所述初值估算的步骤中，在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续各扫描角度下各激光反射点在地图坐标系中的坐标的计算。

9.根据权利要求1～6任一所述的稀疏环境下的机器人三维激光定位方法，其特征在于，所述定位扫描窗口的步骤中，对于某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。