CN107063264A - 一种适用于大规模变电站环境的机器人地图创建方法 - Google Patents

Abstract

本发明公开了一种适用于大规模变电站环境的机器人地图创建方法，利用基于粒子滤波算法进行地图创建，以离散粒子近似机器人位姿的后验概率分布，每个粒子单独维护一幅变电站的地图，针对计算量过大的问题和机器人电量不足的问题，对SLAM算法进行优化，并结合修改基于ROS的rosbag软件包，在变电站现场只录制里程计和激光数据包，过程中机器人电量耗尽可充电后重启，变电站的数据可存为多个数据包，且数据包之间的里程计不必连续，数据包录制完成后通过rosbag回放数据包的方式进行建图。本发明可以实现大规模变电站环境高精度栅格地图的创建，可降低由于定位错误导致建图失败的概率。

Description

一种适用于大规模变电站环境的机器人地图创建方法

技术领域

本发明属于机器人建图技术领域，特别涉及一种适用于大规模变电站环境的机器人地图创建方法。

背景技术

变电站设备巡检为变电站安全运行提供基本保障，而变电站稳定、安全的运行是人民日常生活、工业生产正常进行的重要条件，随着变电站自动化水平提高，变电站设备可靠运行面临更加严峻的考验。由于变电站巡检任务的复杂性，目前，变电站设备巡检主要是工作人员借助一些检测设备进行检查的方式，检查人员的主要工作就是对站内大量的仪表读数进行记录以及对特定区域的温度测量，由于人工巡检诸如操作人员劳动强度、现场环境等不确定因素影响，检查结果可靠性差，漏检和错误检查情况较多，进而可能造成较大经济损失，甚至引起严重安全事故，变电站自主巡检机器人代替人工进行巡检成为智能电网的发展趋势。自主导航是机器人进行自主巡检的关键步骤，而根据移动机器人同时定位与建图(SLAM)方法来创建变电站的环境地图是实现机器人自主导航的关键和前提。

SLAM是指机器人依靠自身传感器在未知环境中从一个未知位置开始移动，利用传感器信息确定自身位置，同时建造增量式地图。SLAM问题的主要难点在于定位和地图之间的依赖性，SLAM问题好比“鸡和蛋”的问题，创建精确的地图依赖于对机器人位置的准确估计，即建图依赖于定位，而对机器人位置的准确估计又来自传感器对外界的认知，即定位依赖于地图。因此“定位”和“建图”作为两个相互耦合，相互依存的问题，很容易受到彼此噪声的干扰，建立大规模的一致地图尤为困难。图1为传统基于标志物的SLAM示意图。三角形为机器人的位姿，多边形为地图标志物，为传感器对这些标志的观测，为机器人里程计的增量。根据，对SLAM问题进行求解，可得到，的估计值。

目前用于解决移动机器人SLAM问题的方法有很多，如卡尔曼滤波算法、扩展卡尔曼滤波算法、无迹卡尔曼滤波算法、粒子滤波算法等，最主要的算法是基于扩展卡尔曼滤波的SLAM算法和基于粒子滤波的SLAM算法。卡尔曼滤波是一种递推滤波器算法，当系统获得新的测量数据后可利用递推公式获得新的系统状态估计，扩展卡尔曼滤波器（EKF）对卡尔曼滤波器进行改进，在非线性系统中利用一阶泰勒公式展开进行近似线性化，然后根据最小方差准则对非线性系统状态方程和测量方程的输出进行估计。扩展卡尔曼滤波只适用于高斯噪声模型，对于非高斯模型，其结果并不理想，在滤波误差和预测误差较小的情况下，通过对系统的非线性进行近似可获得相对满意的结果。基于扩展卡尔曼滤波的SLAM算法将机器人位姿和环境特征坐标向量作为一个整体，构成一个高维的状态空间向量，利用扩展卡尔曼滤波器对状态进行估计，估计过程包括预测和更新两个迭代过程。机器人的运动模型和观测模型一般是非线性的，扩展卡尔曼滤波对状态方程进行一阶泰勒展开，将非线性模型近似线性化。EKF对SLAM问题有两个重要假设：误差为高斯分布，以及工作点附近可以用导数线性化。在这两个假设成立时，EKF可实时的更新机器人的当前位姿与所有路标的均值和协方差矩阵。在路标关联正确的情况下，EKF能够使机器人位姿收敛到稳定的值。然而，由于需要维护所有路标的协方差矩阵，使得算法复杂度随路标数量平方增长。在当时的计算条件下最多只能存储一百多个路标，因此只能对较小范围的地图进行建模。

第二种主流的SLAM算法是基于粒子滤波（Particle Filter, PF）的SLAM方法。粒子滤波器本质上是一种串行蒙特卡洛方法（Sequential Monte-Carlo, SMC）。与经典的EKF相比，它没有对数据的噪声进行参数化，因此可以处理非高斯的噪声。另一方面，由于粒子数较多，构成了对机器人位置的多个估计，使它对数据关联不是那么敏感。粒子滤波器用离散的随机粒子来表示机器人位姿的后验概率密度函数，适用于非线性非高斯系统的估计问题。由于粒子滤波器的易实现性和非线性，使其成为比基于扩展卡尔曼滤波的SLAM应用更为广泛，但粒子滤波器需要大量样本来近似后验概率密度，粒子数越多，算法复杂度越高。因此，能够有效地减少样本数量是该算法的重点。另外，重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。如何保持粒子的有效性和多样性，克服样本贫化，也是该算法研究重点。

变电站环境规模大、环境复杂，基于粒子滤波的SLAM算法每个粒子单独维护一幅变电站地图，当地图比较大时，机器人建图无法满足实时性要求，尤其是每一次更新地图需要几分钟甚至十几分钟时间，当地图更新完毕，机器人里程计数据和地图更新前相差过大，导致建图发散，此外电站环境过大，机器人受电池电量限制无法一次完成建图，最后，变电站很多地方环境相似度高，SLAM算法容易发生机器人定位错误而导致建图失败。

发明内容

本发明的目的是为了解决现有SLAM算法在大规模环境下建图失败的问题，提供一种适用于大规模变电站环境的机器人地图创建方法。

为此，本发明的技术方案是：一种适用于大规模变电站环境的机器人地图创建方法，所述机器人具有运动控制系统、惯性导航系统、激光扫描传感器，所用软件系统为机器人操作系统ROS；其特征在于：包括以下步骤：

1）在新的未知环境下打开机器人，并在ROS下启动运动控制节点、激光节点和手柄控制节点，运动控制节点会发布里程计数据到/odom主题，激光节点会发布激光数据到/scan主题，手柄控制节点可以根据手柄操作发布速度指令，运动控制节点接收该指令并控制机器人运动；

2）启动rosbag功能包中记录数据的指令，记录发布到/odom主题的里程计消息和发布到/scan主题的激光消息；

3）使用手柄控制机器人在环境中运动以记录里程计数据和激光数据；

4）当机器人由于电量不足或其他原因需要停止当前数据包录制时，将数据包保存；

5）在上一个数据包终止的位置附近开始录制下一个数据包；

6）重复以上步骤，直至已将变电站全部环境至少扫描一遍；

7）回放数据包的方式进行建图：

a1）在ROS下启动建图算法，启动计算里程计增量的节点，该节点接收原始里程计数据主题/odom的消息并计算相对于新建里程计的增量，发布新建里程计主题数据/odom_correct，建图算法会以该里程计作为建图使用的里程计；

a2）通过rosbag工具回放数据包，因为建图时只用到了关键帧的数据，即线性运动距离超过线性阈值linearUpdate或者机器人转动角度超过转动阈值angularUpdate时，才需要使用里程计和激光数据，所以可以以多倍速率回放数据包；

a3）建图算法接收数据包发布到/odom_oorrect主题的里程计数据和发布到/scan主题的激光数据，当接收到第一帧数据时，初始化粒子滤波算法和地图，每个粒子单独维护一幅地图；粒子滤波算法实质是以离散粒子近似机器人的后验概率分布，即用表示系统后验概率密度函数的粒子集合，其中是支持样本集，相应的权值为，且满足，而表示到时刻k系统所有状态的集合，所以时刻k的后验概率密度可近似表示为：，代表狄拉克函数；

a4）采样阶段：首先利用机器人运动模型

即里程计数据计算每个粒子下一时刻的位置，是t-1时刻第i个粒子所维护的地图；为了实现上述在高似然区域采样的目的，我们对每个粒子周围一定范围的位置进行迭代，在这些位置中选出机器人在此位置时能使当前激光和地图匹配最好的位置，作为该粒子下一步的采样结果；

a5）粒子群优化步骤：采样完毕通过每个粒子的激光匹配得分判断采样是否符合要求，在变电站某些复杂或相似性高的地方，一旦定位错误就会导致整个地图建图失败，所以当定位结果不准确时，进入粒子群优化步骤。在粒子群优化算法中机器人速度和位姿的更新方程为：和。其中和表示学习因子，和为对角线矩阵，对角线上元素为正的标准正态分布随机数，和分别表示机器人位姿预测的局部最优解和全局最优解，通过粒子群优化过程，机器人位姿由变为，此外，利用激光匹配的得分作为适应度函数，判断粒子群算法对机器人位姿预测的优化程度；

a6）粒子权重，根据各粒子位姿利用当前激光数据与地图进行匹配，根据匹配得分计算每个粒子的权重；

a7）重采样：引入有效粒子系数，只有当有效系数低于给定阈值时才会进行重采样。当采样符合真实分布时，各粒子权重相似，当采样分布与真实分布偏差越大，粒子的权重方差会越大，因此有效系数可以作为采样粒子是否逼近真实分布的有效测度；当时进行重采样，当时跳过重采样步骤；

a8）更新局部地图并将机器人各个粒子位姿和对应的激光数据记录到路径树；为避免频繁更新全局地图，可将全局地图更新时间间隔map_update_interval设成一个较大的值；

a9）在上述机器人进行定位过程和记录激光数据到路径树的过程中，建图算法发布暂停数据包的消息到/map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待记录数据完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题，rosbag包恢复数据播放；

a10）若当前回放的数据的时间戳与上次更新全局地图的时间差超过设定的全局地图更新时间阈值map_update_interval，建图算法会发布暂停数据包的消息到/map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待地图更新完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题，rosbag包恢复数据播放，更新全局地图是算法会确定当前匹配最好的粒子，然后根据路径树回溯该粒子从起始时刻到当前时刻所有的机器人位姿和对应的激光数据，更新全局地图；

a11）在任意时刻手动发布需要更新地图的请求到/map_cmd_sub主题，建图算法会订阅该主题，当接收到更新地图的请求时，建图算法会发布暂停数据包的消息到/map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待地图更新完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题，rosbag包恢复数据播放；

a12）建图完毕后将地图保存。

进一步地，所述的粒子群优化具体步骤为：

b1）将优化后的粒子集中的样本作为各粒子的局部最优解，其中激光匹配得分最高的作为全局最优解；

b2）通过速度更新方程和位置更新方程：

和对粒子集进行更新，得到优化后的粒子集；

b3）通过激光匹配得分计算，表示粒子位姿通过激光数据和地图进行匹配的得分，代表意义是估计粒子和正确位姿的似然性；

b4）若，则返回b）继续进行优化，是提前设置好的一个固定阈值；

b5）若，则优化完毕，将优化后的粒子集作为新的样本。

本发明利用基于粒子滤波算法进行地图创建，以离散粒子近似机器人位姿的后验概率分布，每个粒子单独维护一幅变电站的地图，针对计算量过大的问题和机器人电量不足的问题，对SLAM算法进行优化，并结合修改基于ROS的rosbag软件包，在变电站现场只录制里程计和激光数据包，过程中机器人电量耗尽可充电后重启，变电站的数据可存为多个数据包，且数据包之间的里程计不必连续，数据包录制完成后通过rosbag回放数据包的方式进行建图。

建图过程中实际上算法只用到了关键帧的激光和里程计数据，其他数据没用，而算法中消耗时间的主要是两个过程，一是将关键帧的数据记录到机器人的路径树中，二是机器人更新出当前地图的时候，所以在建图过程中以实际数倍的速率回放数据包，SLAM算法中当需要记录激光数据或更新地图时，算法会发布暂停数据包的消息到map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待记录数据完毕或地图更新完成，再发布恢复数据包播放的消息到map_cmd_pub主题，rosbag包恢复数据播放。针对由于数据包之间数据的不连续，以算法中新建里程计代替原始数据包中的里程计数据，原始数据包中的里程计数据用来计算里程计的增量，将该增量不断累加到新建里程计中，当切换数据包时，由于数据的不连续，可对增量进行重置，从而保证在新建里程计中数据的连续性。

针对在相似性较高区域，由于定位错误导致建图失败的问题，本发明将粒子群优化算法引入SLAM算法中。粒子群优化算法（Particle Swaim Optimization, PSO）是基于群体智能的一种进化计算方法，粒子群优化算法的基本思想是通过群体中个体之间的协作和信息共享来寻找最优解。将粒子群优化算法引入SLAM问题中，就是在利用粒子滤波估计机器人轨迹时，利用激光匹配的得分作为适应度函数，若匹配的分较低，则进入粒子群优化步骤，在预测阶段考虑个体粒子和群体粒子的共同影响，使预测分布更接近机器人系统状态的真实分布，利用粒子群优化算法中的速度与位置方程更新机器人位姿，当机器人位姿达到要求，再进行后续的建图操作。

本发明所述的/odom主题是里程计主题，/scan主题是激光主题， /odom_correct主题是新建里程计主题，/map_cmd_pub主题是暂停/恢复数据包主题，Rosbag功能包是ROS提供的功能包，主要包含了对数据包的一些列操作， linearUpdate是指线性更新阈值：当机器人线性运动超过该阈值时需记录里程计和激光数据；angularUpdate是指角度更新阈值：当机器人转动角度超过该阈值时需记录里程计和激光数据。

本发明可以实现大规模变电站环境高精度栅格地图的创建，由于在处理数据时暂停了数据包的播放，因此不会出现地图发散的情况，在某些相似性较高的地方，当定位效果不好时，由于引进了粒子群优化算法，可在粒子滤波的预测阶段对机器人位姿进行优化，使粒子分布较好的覆盖机器人位姿的后验概率分布，因此可降低由于定位错误导致建图失败的概率。

附图说明

以下结合附图和本发明的实施方式来作进一步详细说明

图1为传统基于标志物的SLAM示意图。

具体实施方式

本实施例所用平台机器人须具有运动控制系统、惯性导航系统、激光扫描传感器，而所用软件系统为机器人操作系统(ROS)，包括以下步骤：

1）在新的未知环境下打开机器人，并在ROS下启动运动控制节点、激光节点和手柄控制节点，运动控制节点会发布里程计数据到/odom主题（里程计主题），激光节点会发布激光数据到/scan主题（激光主题），手柄控制节点可以根据手柄操作发布速度指令，运动控制节点接收该指令并控制机器人运动；

2）启动rosbag功能包中记录数据的指令，记录发布到/odom主题（里程计主题）的里程计消息和发布到/scan主题（激光主题）的激光消息；Rosbag功能包是ROS提供的功能包，主要包含了对数据包的一些列操作；

3）使用手柄控制机器人在环境中运动以记录里程计数据和激光数据；

4）当机器人由于电量不足或其他原因需要停止当前数据包录制时，将数据包保存；

5）在上一个数据包终止的位置附近开始录制下一个数据包；

6）重复以上步骤，直至已将变电站全部环境至少扫描一遍；

7）回放数据包的方式进行建图：

a1）在ROS下启动建图算法，启动计算里程计增量的节点，该节点接收原始里程计数据主题/odom（里程计主题）的消息并计算相对于新建里程计的增量，发布新建里程计主题数据/odom_correct（新建里程计主题），建图算法会以该里程计作为建图使用的里程计。

a2）通过rosbag工具回放数据包，因为建图时只用到了关键帧的数据，即线性运动距离超过线性阈值linearUpdate或者机器人转动角度超过转动阈值angularUpdate时，才需要使用里程计和激光数据，所以可以以多倍速率回放数据包。

a3）建图算法接收数据包发布到/odom_oorrect主题（新建里程计主题）的里程计数据和发布到/scan主题（激光主题）的激光数据，当接收到第一帧数据时，初始化粒子滤波算法和地图，每个粒子单独维护一幅地图；粒子滤波算法实质是以离散粒子近似机器人的后验概率分布，即用表示系统后验概率密度函数的粒子集合，其中是支持样本集，相应的权值为，且满足，而表示到时刻k系统所有状态的集合，所以时刻k的后验概率密度可近似表示为：，代表狄拉克函数。

a4）采样阶段：传统基于粒子滤波的SLAM算法以机器人运动模型为建议分布来近似机器人真实后验概率分布进行采样，但这种方法需要大量的粒子才能较好的表示机器人的真实分布，受限于粒子滤波SLAM计算量的因素，过多粒子无法满足实时性要求，有文献证明最优的建议分布是，当机器人搭载激光传感器时，观测模型的概率分布程尖峰形式，此时在中观测模型占据主导作用，而在观测模型的概率分布区域，运动模型的概率分布可近似为一个常数，此时机器人的的建议分布

只有此处的N代表正态分布；

因此采样阶段首先利用机器人运动模型，即里程计数据计算每个粒子下一时刻的位置，为了实现上述在高似然区域采样的目的，我们对每个粒子周围一定范围的位置进行迭代，在这些位置中选出机器人在此位置时能使当前激光和地图匹配最好的位置，作为该粒子下一步的采样结果。采样步骤在高似然区域进行采样可大大减少所需粒子数，从而大大减小计算量。

a5）粒子群优化步骤：采样完毕通过每个粒子的激光匹配得分判断采样是否符合要求，在变电站某些复杂或相似性高的地方，一旦定位错误就会导致整个地图建图失败，所以当定位结果不准确时，进入粒子群优化步骤。在粒子群优化算法中机器人速度和位姿的更新方程为：和。其中和表示学习因子，和为对角线矩阵，对角线上元素为正的标准正态分布随机数，和分别表示机器人位姿预测的局部最优解和全局最优解，通过粒子群优化过程，机器人位姿由变为，此外，利用激光匹配的得分作为适应度函数，判断粒子群算法对机器人位姿预测的优化程度。

a6）粒子权重，根据各粒子位姿利用当前激光数据与地图进行匹配，根据匹配得分计算每个粒子的权重。

a7）重采样：在重采样阶段，会删除权重较小的粒子，复制权重较大的粒子由于只有少数粒子能较好的代表机器人的真实位置，所以重采样步骤是必须的，但过于频繁的重采样会导致粒子匮乏问题，因此引入有效粒子系数，只有当有效系数低于给定阈值时才会进行重采样。当采样符合真实分布时，各粒子权重相似，当采样分布与真实分布偏差越大，粒子的权重方差会越大，因此有效系数可以作为采样粒子是否逼近真实分布的有效测度；当时进行重采样，当时跳过重采样步骤，N为粒子数。

a8）更新局部地图并将机器人各个粒子位姿和对应的激光数据记录到路径树，此时只更新局部地图是对于每个粒子来说的，只更新各粒子所维护地图中当前激光扫描到障碍物的位置附近的地图，之所以更新障碍物附近的地图是为了增量式扩大地图进而为后续机器人定位时进行激光匹配做准备，没有更新全部地图是因为更新全部地图需要消耗大量时间，机器人实现定位并不需要全局地图，所以没必要频繁更新全局地图，可将全局地图更新时间间隔map_update_interval设成一个较大的值。

a9）在上述机器人进行定位过程和记录激光数据到路径树的过程中，由于使用的激光分辨率高、范围大，所以需要的时间较长，为防止在此过程中数据包中的数据丢失，建图算法发布暂停数据包的消息到/map_cmd_pub主题（暂停/恢复数据包主题），rosbag包接收到该主题消息会暂停数据包的播放，等待记录数据完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题（暂停/恢复数据包主题），rosbag包恢复数据播放。

a10）若当前回放的数据的时间戳与上次更新全局地图的时间差超过设定的全局地图更新时间阈值map_update_interval，建图算法会发布暂停数据包的消息到/map_cmd_pub主题（暂停/恢复数据包主题），rosbag包接收到该主题消息会暂停数据包的播放，等待地图更新完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题（暂停/恢复数据包主题），rosbag包恢复数据播放，更新全局地图是算法会确定当前匹配最好的粒子，然后根据路径树回溯该粒子从起始时刻到当前时刻所有的机器人位姿和对应的激光数据，更新全局地图。

a11）在上述步骤已说明为避免频繁更新全局地图我们将全局地图更新时间间隔map_update_interval设成一个较大的值，但建图过程中或者数据包播放完毕时并没有达到全局地图更新条件导致建图不完整，所以可在任意时刻手动发布需要更新地图的请求到/map_cmd_sub主题（暂停/恢复数据包主题），建图算法会订阅该主题，当接收到更新地图的请求时，建图算法会发布暂停数据包的消息到/map_cmd_pub主题（暂停/恢复数据包主题），rosbag包接收到该主题消息会暂停数据包的播放，等待地图更新完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题（暂停/恢复数据包主题），rosbag包恢复数据播放。

a12）建图完毕后将地图保存。

本实施例之前所述的粒子群优化的具体步骤为：

b1）将优化后的粒子集中的样本作为各粒子的局部最优解，其中激光匹配得分最高的作为全局最优解；

b2）通过速度更新方程和位置更新方程：

和对粒子集进行更新，得到优化后的粒子集；

b3）通过激光匹配得分计算，表示粒子位姿通过激光数据和地图进行匹配的得分，代表意义是估计粒子和正确位姿的似然性；但并没有公式可直接表示其定义，若必须加公式，可用激光观测数据的后验概率表示，即

b4）若，则返回b）继续进行优化，是提前设置好的一个固定阈值；

b5）若，则优化完毕，将优化后的粒子集作为新的样本。

本实施例所述的方法所创建的栅格地图精度高且无地图发散问题，可用于机器人的自主导航，使机器人对变电站设备进行自主巡检。

Claims (2)

1.一种适用于大规模变电站环境的机器人地图创建方法，所述机器人具有运动控制系统、惯性导航系统、激光扫描传感器，所用软件系统为机器人操作系统ROS；其特征在于：包括以下步骤：

1）在新的未知环境下打开机器人，并在ROS下启动运动控制节点、激光节点和手柄控制节点，运动控制节点会发布里程计数据到/odom主题，激光节点会发布激光数据到/scan主题，手柄控制节点可以根据手柄操作发布速度指令，运动控制节点接收该指令并控制机器人运动；

2）启动rosbag功能包中记录数据的指令，记录发布到/odom主题的里程计消息和发布到/scan主题的激光消息；

3）使用手柄控制机器人在环境中运动以记录里程计数据和激光数据；

4）当机器人由于电量不足或其他原因需要停止当前数据包录制时，将数据包保存；

5）在上一个数据包终止的位置附近开始录制下一个数据包；

6）重复以上步骤，直至已将变电站全部环境至少扫描一遍；

7）回放数据包的方式进行建图：

a1）在ROS下启动建图算法，启动计算里程计增量的节点，该节点接收原始里程计数据主题/odom的消息并计算相对于新建里程计的增量，发布新建里程计主题数据/odom_correct，建图算法会以该里程计作为建图使用的里程计；

a2）通过rosbag工具回放数据包，因为建图时只用到了关键帧的数据，即线性运动距离超过线性阈值linearUpdate或者机器人转动角度超过转动阈值angularUpdate时，才需要使用里程计和激光数据，所以可以以多倍速率回放数据包；

a3）建图算法接收数据包发布到/odom_oorrect主题的里程计数据和发布到/scan主题的激光数据，当接收到第一帧数据时，初始化粒子滤波算法和地图，每个粒子单独维护一幅地图；粒子滤波算法实质是以离散粒子近似机器人的后验概率分布，即用表示系统后验概率密度函数的粒子集合，其中是支持样本集，相应的权值为，且满足，而表示到时刻k系统所有状态的集合，所以时刻k的后验概率密度可近似表示为：，代表狄拉克函数；

a4）采样阶段：首先利用机器人运动模型

即里程计数据计算每个粒子下一时刻的位置，是t-1时刻第i个粒子所维护的地图；为了实现上述在高似然区域采样的目的，我们对每个粒子周围一定范围的位置进行迭代，在这些位置中选出机器人在此位置时能使当前激光和地图匹配最好的位置，作为该粒子下一步的采样结果；

a5）粒子群优化步骤：采样完毕通过每个粒子的激光匹配得分判断采样是否符合要求，在变电站某些复杂或相似性高的地方，一旦定位错误就会导致整个地图建图失败，所以当定位结果不准确时，进入粒子群优化步骤；

在粒子群优化算法中机器人速度和位姿的更新方程为：

和；

其中和表示学习因子，和为对角线矩阵，对角线上元素为正的标准正态分布随机数，和分别表示机器人位姿预测的局部最优解和全局最优解，通过粒子群优化过程，机器人位姿由变为，此外，利用激光匹配的得分作为适应度函数，判断粒子群算法对机器人位姿预测的优化程度；

a6）粒子权重，根据各粒子位姿利用当前激光数据与地图进行匹配，根据匹配得分计算每个粒子的权重；

a7）重采样：引入有效粒子系数，只有当有效系数低于给定阈值时才会进行重采样；

当采样符合真实分布时，各粒子权重相似，当采样分布与真实分布偏差越大，粒子的权重方差会越大，因此有效系数可以作为采样粒子是否逼近真实分布的有效测度；当时进行重采样，当时跳过重采样步骤；

a8）更新局部地图并将机器人各个粒子位姿和对应的激光数据记录到路径树；为避免频繁更新全局地图，可将全局地图更新时间间隔map_update_interval设成一个较大的值；

a9）在上述机器人进行定位过程和记录激光数据到路径树的过程中，建图算法发布暂停数据包的消息到/map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待记录数据完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题，rosbag包恢复数据播放；

a10）若当前回放的数据的时间戳与上次更新全局地图的时间差超过设定的全局地图更新时间阈值map_update_interval，建图算法会发布暂停数据包的消息到/map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待地图更新完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题，rosbag包恢复数据播放，更新全局地图是算法会确定当前匹配最好的粒子，然后根据路径树回溯该粒子从起始时刻到当前时刻所有的机器人位姿和对应的激光数据，更新全局地图；

a11）在任意时刻手动发布需要更新地图的请求到/map_cmd_sub主题，建图算法会订阅该主题，当接收到更新地图的请求时，建图算法会发布暂停数据包的消息到/map_cmd_pub主题，rosbag包接收到该主题消息会暂停数据包的播放，等待地图更新完毕，再发布恢复数据包播放的消息到/map_cmd_pub主题，rosbag包恢复数据播放；

a12）建图完毕后将地图保存。

2.如权利要求1所述的一种适用于大规模变电站环境的机器人地图创建方法，其特征在于：进一步地，所述的粒子群优化具体步骤为：

b1）将优化后的粒子集中的样本作为各粒子的局部最优解，其中激光匹配得分最高的作为全局最优解；

b2）通过速度更新方程和位置更新方程：

和对粒子集进行更新，得到优化后的粒子集；

b3）通过激光匹配得分计算，表示粒子位姿通过激光数据和地图进行匹配的得分，代表意义是估计粒子和正确位姿的似然性；

b4）若，则返回b）继续进行优化，是提前设置好的一个固定阈值；

b5）若，则优化完毕，将优化后的粒子集作为新的样本。