CN108458715A - 一种基于激光地图的机器人定位初始化方法 - Google Patents

Abstract

本发明公开了一种基于激光地图的机器人定位初始化方法，包括以下步骤：1)获取初始定位初值；2)根据机器人的初始定位初值和定位扫描窗口大小确定所有可能的候选位姿，根据来自机器人的二维激光传感器的激光数据和地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；3)置信度计算：计算每个候选位姿的置信度与置信度权重的乘积作为当前位姿的置信度分值，取置信度分值最高的候选位姿作为初始定位估值；4)对初始定位估值进行非线性优化计算获得最优初始位姿估计。采用本发明的基于激光地图的机器人定位初始化方法，初始定位的准确性很高，从而提高了机器人的定位精度。

Description

一种基于激光地图的机器人定位初始化方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种基于激光地图的机器人定位初始化方法。

背景技术

当今智能电网的建设中利用变电站智能巡检机器人取代人工对变电设备状态进行检测是一种便捷、可靠和经济的技术手段。变电站智能巡检机器人能够实现变电站全天候、全方位、全自主智能巡检和监控，有效降低劳动强度，降低变电站运维成本，提高正常巡检作业和管理的自动化和智能化水平，为智能变电站和无人值守变电站提供创新性的技术检测手段和全方位的安全保障，更快地推进变电站无人值守的进程。

与传统的轨道式巡检机器人不同，利用二维激光定位导航的智能巡检机器人不但可以节省轨道铺设的成本和工作量，而且巡检工作更加灵活，可以根据需要进行巡检路线的规划。巡检机器人利用激光定位的工作过程中，机器人的初始定位会影响到整个巡检过程的效率和准确度。初始定位的不准确不仅会增加机器人迷路的可能性，而且对后续设备检测的位置和角度也会产生不良的影响。因此初始定位方法的设计，是变电站智能巡检机器人开发中的关键技术。

中国专利201610324871.8公开了一种用于变电站巡检机器人激光导航的初始化定位方法，包括以下步骤：1)绘制出二维环境地图，记录下地图上关键节点的位姿信息；2)按照高斯分布以记忆位姿为中心局部播撒粒子；3)对粒子进行重新计算，按照粒子滤波算法中采样与重采样的计算方法，对粒子簇进行实时更新；4)经过短暂的运动，粒子簇会呈现收敛的趋势，最终收敛成一个较小的粒子簇；5)对所有粒子进行加权平均计算，得到一个估计的位姿，即该时刻机器人的定位结果。该方法虽然可以快速精确地确认机器人的初始坐标，但在变电站这样的环境下(一般变电站室外环境稀疏空旷，面积较大，大多数的变电站面积都在1万平方米以上)，此时使用这样的传统的粒子滤波方法进行初始定位会存在以下主要问题：

一、计算量大。粒子滤波的方法主要源自蒙特卡洛思想，需要在地图空间内均匀的撒下粒子，然后对每个粒子位置的可信度进行计算。假定设定机器人巡检定位的位置精度为10厘米*10厘米，那么在一个10000平米的变电站地图内，就需要对100万个初始粒子的位姿进行估算。如果再计算粒子的不同朝向，由此带来的计算量显然是不能容忍的。

二、粒子滤波定位的目标是粒子收敛，确定最终位置。因此需要巡检机器人不断的移动情况下才能实现粒子的定位。在变电站内不确定自己位置的情况下移动，可能会造成机器人的跌落摔跤或者与电力设备的碰撞，风险较大。

发明内容

本发明目的是：针对现有技术的不足，提供一种基于激光地图的机器人定位初始化方法，实现变电站环境内智能巡检机器人高精度的初始定位。

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

1)获得机器人的初始定位初值：

2)根据机器人的初始定位初值设置定位扫描窗口，根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿，根据来自机器人的二维激光传感器的激光数据计算各扫描角度下各激光反射点在地图坐标系中的坐标，并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；

3)计算各可能的候选位姿的置信度，选择置信度分值score最高的位姿作为机器人的初始定位估值；

4)对机器人的初始定位估值进行非线性优化计算获得最优初始位姿估计。

上述技术方案进一步而言，所述步骤4)中，对机器人的初始定位估值进行非线性优化计算获得最优初始位姿估计的方法是使用Ceres-Solver库来计算最优初始位姿估计，包括以下步骤：

将机器人的初始定位估值作为观测值，使用权重表示不同参数对初始定位结果的不同影响，根据置信度、位移和旋转角度构建如以下三个公式所示的三个代价函数；非线性优化采用双三次插值法，求解每个可能的候选位姿中残差residual最小的，作为最优初始位姿估计；

probab_residual＝probab_weight*(1-pose_probability_{pose_estimated})

其中，probab_residual为候选位姿的置信度残差，probab_weight为候选位姿的计算置信度残差时的权重，pose_probability_{pose_estimated}为候选位姿的置信度；

trans_residual＝trans_weight*Δtranslation

其中，trans_residual为位移残差，trans_weight为位移权重，Δtranslation为可能的候选位姿相对初始定位估值的位移，即Δtranslation＝translation_{pose_estimated}-translation_{pose_observation}；其中，translation_{pose_estimated}为可能的候选位姿的位移，translation_{pose_observation}为初始定位估值的位移；

rot_residual＝rot_weight*Δrotation

其中，rot_residual为旋转角度残差，rot_weight为旋转角度权重，Δrotation为可能的候选位姿相对初始定位估值的旋转角度，即Δrotation＝rotatation_{pose_estimated}-rotation_{pose_observation}；其中，rotatation_{pose_estimated}为可能的候选位姿的角度，rotation_{pose_observation}为初始定位估值的角度；

残差residual与候选位姿的信度残差probab_residual、位移残差trans_residual和旋转角度残差rot_residual间计算公式如下：

residual

＝(probab_weight*probab_residual ²+trans_weight*trans_residual ²+rot_weight*rot_residual ²)/3

上述技术方案进一步而言，所述步骤1)中，通过以下任一方式获得机器人的初始定位初值：

通过保存的地图文件获取机器人的初始定位初值；

通过订阅机器人起始位姿获得机器人的初始定位初值。

上述技术方案进一步而言，所述步骤2)中，根据来自机器人的二维激光传感器的激光数据计算各扫描角度下各激光反射点在地图坐标系中的坐标的具体方法为：

每得到一帧来自机器人的二维激光传感器的激光数据后，对该激光数据进行处理得到各激光反射点在激光传感器坐标系中的坐标；

根据机器人的初始定位初值、各激光反射点在激光传感器坐标系中的坐标、扫描角度大小和扫描角度步长计算各扫描角度下各激光反射点在地图坐标系中的坐标。

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

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

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

上述技术方案进一步而言，所述步骤3)中，计算各可能的候选位姿的置信度的计算方法为：

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

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

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

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

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

score＝pose_probability_{pose_estimated}*candidate_weight。

上述技术方案进一步而言，trans_weight为10，rot_weight为10，probab_weight按以下方式计算，取值范围约为0.3～0.5：

本发明的有益效果如下：采用本发明的基于激光地图的机器人定位初始化方法，初始定位的准确性很高，从而提高了机器人的定位精度。同时，初始定位的高度准确性降低了机器人迷路的发生频次，提高了设备巡检的效率和检测数据的可靠性。

附图说明

图1是本发明实施例的二维环境地图示意图，黑色部分表示有电力设备或者障碍物的位置。

图2是本发明实施例的初始定位流程图。

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

具体实施方式

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

实施例1：

本发明的一个实施例，介绍了一种基于激光地图的机器人定位初始化方法。

本实施例采用的机器人，其硬件系统传感器主要包括二维激光传感器，用来获得激光数据。机器人软件系统使用机器人操作系统ROS，这是一种常用的机器人软件平台，它能为异质计算机集群提供类似操作系统的功能，在ROS系统中包括实现定位功能的节点。当然，可以理解的是，本发明方法也可以通过其他机器人的软件系统加以实现，本实施例采用ROS系统仅作为一种实现方式。

本实施例的基于激光地图的机器人定位初始化方法，其相关情况如下：

(1)要求在机器人开始巡检工作前，已经构建了变电站的二维环境地图。

(2)实时性的要求低。与机器人巡检过程中的实时定位的主要区别是巡检工作过程中的定位方法的要求在毫秒级别内实现。但是初始定位允许具有足够的时间进行，可以进行更大范围的搜索和优化计算。

(3)每次工作完成后，自动保存机器人的位姿，作为下次巡检工作开始的位姿估计。

(4)异常情况(例如机器人丢失定位)下，使用业务控制平台系统或者人工来控制机器人，并进行初始定位初值的设置。

参照图1，本实施例中的地图以及机器人的导航运动假定在一个二维平面以内，坐标系包括地图坐标系、机器人坐标系、激光传感器坐标系。

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

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

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

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

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

参照图2，本实施例的基于激光地图的机器人定位初始化方法，其实现的主要步骤如下：

1、初值估算：

根据不同的情况，获得机器人的初始定位初值P_S(x₀，y₀，r₀)。一般可通过以下两种方式获得：

(1)通过保存的地图文件获取初始定位初值。地图文件保存在机器人的存储模块中，比如Flash或者硬盘。地图文件格式为.pgm，读取地图文件就可以得到初始定位初值P_S(x₀，y₀，r₀)。正常情况下，通过保存的地图文件获取。

(2)通过订阅机器人起始位姿获得初始定位初值P_S(x₀，y₀，r₀)。机器人的位置出现异常，比如被搬动后，通过订阅的机器人起始位姿获取初始定位初值P_S(x₀，y₀，r₀)。

2、定位窗口扫描：

根据机器人的初始定位初值设置定位扫描窗口。定位窗口扫描时使用位移扫描参数linear_search_window和角度扫描参数，其中角度扫描参数包括扫描角度大小angular_search_window和扫描角度步长angular_step。位移扫描参数用于限定定位扫描窗口的位移范围为以P_S(x₀，y₀，r₀)为中心，上下左右各偏离linear_search_window大小的正方形。角度扫描参数用于限定定位扫描窗口的角度范围为以P_S(x₀，y₀，r₀)为中心角度，上下各偏离angular_search_window大小的角度。这里角度范围并不是机器人实际的激光扫描角度，而是定位初始化计算过程中使用的扫描角度。

由于实时性要求不高，本实施例设定linear_search_window为1米，地图分辨率为0.1米*0.1米，angular_search_window为360度，angular_step为1度。则定位扫描窗口是以P_S(x₀，y₀，r₀)为中心上下左右各1米的正方形范围，精度为0.1米*0.1米，扫描角度范围360度，角度步长为1度。

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

例如，参照图2，机器人当前初始位姿P_S(10，10，30)其中(10，10)为当前初始位姿在地图坐标系中的位置，30表示当前初始位姿与水平向右方向的x轴成30度角。linear_search_window＝1米，angular_search_window＝360，angular_step＝1，地图分辨率(地图栅格大小)为0.1米*0.1米。以P_S(10，10，30)为中心计算各栅格上各扫描角度的位姿时，因为在地图坐标系中每个可能位置为(10±0.1*x，10±0.1*y)，其中x≤10，y≤10，那么可能位置有(2*1/0.1+1)²＝441个，即441个栅格位置；每个位置上可能角度包括(360/1)＝360个朝向。最终可能的位姿就是441*360个位姿。

如果扫描角度是10度，扫描步长0.5，那么就是以当前朝向30为中心线，向左向右各10度，共(2*10/0.5+1)＝41个可能的朝向。

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

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

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

3、置信度计算：

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

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

根据每个候选位姿possible_candidate与P_S(x₀，y₀，r₀)的位姿差来计算每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是候选位姿与P_S(x₀，y₀，r₀)间沿x轴的位移，y_offset是候选位姿与P_S(x₀，y₀，r₀)间沿y轴的位移，trans_weight是位移权重，candiate.rotation是候选位姿与P_S(x₀，y₀，r₀)间旋转角度，ror_weight是旋转角度权重。

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

score＝pose_probability_{pose_estimated}*candidate_weight (3)

取置信度分值score最高的候选位姿作为初始定位估值P_E(x₀，y₀，r₀)。

4、对P_E(x₀，y₀，r₀)进行非线性优化计算获得最优初始位姿估计P₀(x₀，y₀，r₀)：

理论上，对于机器人在地图中的任何一个位姿，通过地图数据，可以计算出该位姿和对应的置信度pose_probability_{pose_estimated}。

本实施例使用Ceres-Solver库来计算最优初始位姿估计P₀(x₀，y₀，r₀)。将P_E(x₀，y₀，r₀)作为观测值pose_observation，使用权重表示不同参数对初始定位结果的不同影响，根据置信度、位移和旋转角度构建如公式(4)、(5)和(7)所示的三个代价函数。非线性优化采用双三次插值法，

求解每个可能的候选位姿中残差residual最小的，作为最优初始位姿估计；

probab_residual＝probab_weight*(1-pose_probability_{pose_estimated}) (4)

其中，probab_residual为可能的候选位姿P₀(x₀，y₀，r₀)的置信度残差，probab_weight为可能的候选位姿P₀(x₀，y₀，r₀)的计算置信度残差时的权重，pose_probability_{pose_estimated}为可能的候选位姿P₀(x₀，y₀，r₀)的置信度；

trans_residual＝trans_weight*Δtranslation (5)

其中，trans_residual为位移残差，trans_weight为位移权重，Δtranslation为可能的候选位姿P₀(x₀，y₀，r₀)相对初始定位估值P_E(x₀，y₀，r₀)的位移，

其中，translation_{pose_estimated}为可能的候选位姿P₀(x₀，y₀，r₀)的位移，为初始定位估值P_E(x₀，y₀，r₀)的位移；

rot_residual＝rot_weight*Δrotation (7)

其中，rot_residual为旋转角度残差，rot_weight为旋转角度权重，Δrotation为可能的候选位姿P₀(x₀，y₀，r₀)相对初始定位估值P_E(x₀，y₀，r₀)的旋转角度，

Δrotation＝rotatation_{pose_estimated}-rotation_{pose_observation} (8)

其中，rotation_{pose_estimated}为可能的候选位姿P₀(x₀，y₀，r₀)的角度，为初始定位估值P_E(x₀，y_0，r₀)的角度；

残差residual与置信度残差probab_residual、位移残差trans_residual和旋转角度残差rot_residual间计算方法为公式(9)。

residual

＝(probab_weight*probab_residual ²+trans_weight*trans_residual ²+rot_weight*rot_residual ²)/3

(9)

本实施例中，可能的候选位姿P₀(x₀，y₀，r₀)的位移权重trans_weight为10，可能的候选位姿P₀(x₀，y₀，r₀)的旋转角度权重rot_weight为10，probab_weight定义如下，取值范围约为0.3～0.5。

下面具体描述一下ROS系统中定位的工作流程：

1)定位节点启动后，从本地的硬盘或者flash文件读取二维环境地图数据。

2)定位节点订阅/initialpose和/scan主题。/initialpose是机器人开始工作的位姿，/scan是激光传感器的数据。

3)定位节点获得当前起始位姿的初始定位初值P_S(x₀，y₀，r₀)。

一般使用上次工作结束时保存在地图文件中的位姿。当机器人的位置出现异常时，根据发布的/initialpose来更新。

4)根据P_S(x₀，y₀，r₀)、二维环境地图数据以及通过/scan主题获得的激光数据，执行以下步骤进行初始定位估计：

4-1)根据P_S(x₀，y₀，r₀)设置定位扫描窗口。

4-2)按照角度扫描步长，计算角度扫描窗口内每个角度激光点云数据rotatedcloud。

4-3)根据P_S(x₀，y₀，r₀)和定位扫描窗口参数(位移扫描参数、角度扫描参数)计算所有可能的位姿候选值possible_candidates。

4-4)根据rotatedcloud以及当前候选位姿pose_estimated的(x，y)值，计算rotatedcloud中每个点对应的栅格地图坐标，并删除重复的点，保存在离散扫描数据discretizescans中。

4-5)计算每个位姿候选值的置信度，计算其与每个位姿候选值的置信度权重的乘积作为当前位姿的分值，选取分值最高的位姿作为P_E(x₀，y₀，r₀)。

4-6)对P_E(x₀，y₀，r₀)进行非线性优化计算，获得最终的初始位姿估计P₀(x₀，y₀，r₀)。

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

Claims (9)

1.一种基于激光地图的机器人定位初始化方法，其特征在于，包括以下步骤：

1)获得机器人的初始定位初值：

2)根据机器人的初始定位初值设置定位扫描窗口，根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿，根据来自机器人的二维激光传感器的激光数据计算各扫描角度下各激光反射点在地图坐标系中的坐标，并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；

3)计算各可能的候选位姿的置信度，选择置信度分值score最高的位姿作为机器人的初始定位估值；

4)对机器人的初始定位估值进行非线性优化计算获得最优初始位姿估计。

2.根据权利要求1所述的基于激光地图的机器人定位初始化方法，其特征在于，所述步骤4)中，对机器人的初始定位估值进行非线性优化计算获得最优初始位姿估计的方法是使用Ceres-Solver库来计算最优初始位姿估计，包括以下步骤：

将机器人的初始定位估值作为观测值，使用权重表示不同参数对初始定位结果的不同影响，根据置信度、位移和旋转角度构建如以下三个公式所示的三个代价函数；非线性优化采用双三次插值法，求解每个可能的候选位姿中残差residual最小的，作为最优初始位姿估计；

probab_residual＝probab_weight*(1-pose_probability_{pose_estimated})

其中，probab_residual为候选位姿的置信度残差，probab_weight为候选位姿的计算置信度残差时的权重，pose_probability_{pose_estimated}为候选位姿的置信度；

trans_residual＝trans_weight*Δtranslation

其中，trans_residual为位移残差，trans_weight为位移权重，Δtranslation为可能的候选位姿相对初始定位估值的位移，即Δtranslation＝translation_{pose_estimated}-translation_{pose_observation}；其中，translation_{pose_estimated}为可能的候选位姿的位移，translation_{pose_observation}为初始定位估值的位移；

rot_residual＝rot_weight*Δrotation

其中，rot_residual为旋转角度残差，rot_weight为旋转角度权重，Δrotation为可能的候选位姿相对初始定位估值的旋转角度，即Δrotation＝rotatation_{pose_estimated}-rotation_{pose_observation}；其中，rotatation_{pose_estimated}为可能的候选位姿的角度，rotation_{pose_observation}为初始定位估值的角度；

残差residual与候选位姿的信度残差probab_residual、位移残差trans_residual和旋转角度残差rot_residual间计算公式如下：

residual

＝(probab_weight*probab_residual ²+trans_weight*trans_residual ²+rot_weight*rot_residual ²)/3。

3.根据权利要求1～2任一所述的基于激光地图的机器人定位初始化方法，其特征在于，所述步骤1)中，通过以下任一方式获得机器人的初始定位初值：

通过保存的地图文件获取机器人的初始定位初值；

通过订阅机器人起始位姿获得机器人的初始定位初值。

4.根据权利要求1～2任一所述的基于激光地图的机器人定位初始化方法，其特征在于，所述步骤2)中，根据来自机器人的二维激光传感器的激光数据计算各扫描角度下各激光反射点在地图坐标系中的坐标的具体方法为：

每得到一帧来自机器人的二维激光传感器的激光数据后，对该激光数据进行处理得到各激光反射点在激光传感器坐标系中的坐标；

根据机器人的初始定位初值、各激光反射点在激光传感器坐标系中的坐标、扫描角度大小和扫描角度步长计算各扫描角度下各激光反射点在地图坐标系中的坐标。

5.根据权利要求4所述的基于激光地图的机器人定位初始化方法，其特征在于，所述步骤2)中，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

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

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

8.根据权利要求2所述的基于激光地图的机器人定位初始化方法，其特征在于，所述步骤3)中，计算各可能的候选位姿的置信度的计算方法为：

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

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

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

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

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

score＝pose_probability_{pose_estimated}*candidate_weight。

9.根据权利要求8所述的基于激光地图的机器人定位初始化方法，其特征在于，trans_weight为10，rot_weight为10，probab_weight按如下方式计算，取值范围约为0.3～0.5：