CN109974712A - 一种基于图优化的变电站巡检机器人建图方法 - Google Patents

Abstract

本发明属于智能机器人技术领域，公开了一种基于图优化的变电站巡检机器人建图方法，包括：通过ICP算法得到机器人当前位姿的估计值；利用连续的指定帧数的激光数据生成子图，后一子图中包含前一子图中后指定帧数的激光点云数据；将子图中的每帧激光数据相应的位姿信息与关键帧进行比对；当二者相似时，将两者进行闭环连接；通过图优化法更新各个子图中的机器人位姿，即可构成最终的建图结果。采用本发明的基于图优化的变电站巡检机器人建图方法，机器人的建图效率大大提高，有效的减少了系统的累积误差。

Description

一种基于图优化的变电站巡检机器人建图方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种基于图优化的变电站巡检机器人建图方法。

背景技术

变电站建图是激光导航变电站机器人运行前的必要环节，主要将变电站的地图尽可能详细和精准的通过数据的形式记录下来。激光导航变电站巡检机器人使用该地图与激光传感器实时获取的周围信息做比对，并通过定位算法确定变电站巡检机器人的位置。所以变电站地图的精度对于激光导航变电站机器人的导航定位至关重要。

由于SLAM(Simultaneous Localization And Mapping，即时定位与地图构建)技术能够很好的解决移动机器人在未知环境中运行时定位导航与地图构建的问题，在巡检机器人变电站建图领域得到广泛应用。SLAM，是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程，解决机器人等在未知环境下运动时的定位与地图构建问题。按照传感器的种类和安装方式不同，SLAM分为激光SLAM和视觉SLAM。相对于视觉SLAM，激光SLAM在理论、技术、产品落地上都相对成熟，是目前最稳定、最主流的定位导航方法。激光SLAM解决问题的方法一般分为基于滤波的方法和基于图优化的方法。

基于滤波的方法主要是利用递归贝叶斯估计原理，在假定从0到t时刻的观测信息以及控制信息已知的条件下，对系统状态(包括机器人当前位姿以及所有地图特征位置)的后验概率进行估计。由于基于滤波器的方法存在线性化及更新效率等问题，这使得它难以应用于大规模环境的地图创建。

基于图优化的方法可以用图的方式作直观描述，所得的图被称作位姿图(posegraph)。图中的节点对应机器人在不同时刻的位置和姿态，而边则描述了位姿与位姿间的空间约束关系。这种约束可以通过里程计或观测信息的配准得到；配准是指同一区域内以不同成像手段所获得的不同图像图形的地理坐标的匹配，目的是对不同帧的激光点云数据进行数据关联，即数据拼接。在图构造好后，对图中节点在姿态空间中所处的位置和姿态位姿(后文简称位姿)进行优化，使其最好地满足边所表示的约束关系，优化的结果即对应机器人的运动轨迹。基于图优化的方法在保证高精度建图的前提下具有较快的建图效率，得到广泛应用。然而在现有技术中存在以下问题：

(1)在激光点云配准过程中，由于激光测距仪所测激光点云数据具有离散的特性，先后两帧激光图像中存在的同一障碍物的点云数据之间没有十分严格的真正匹配，使得激光数据无法有效重叠。

(2)依赖于点云配准的方法来建图，存在较大的累计误差，建立的地图与真实状态差距大。

发明内容

本发明目的是：针对现有技术的不足，提供一种基于图优化的变电站巡检机器人建图方法。本方法可以有效减少建图过程的累积误差。

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

激光点云数据配准的步骤：

通过ICP算法将相邻两帧激光点云数据作对比，分析出二者之间的变换矩阵；

使用相邻两帧激光点云数据中前一帧的激光点云数据及变换矩阵，得到机器人当前位姿的估计值；

瓦片式建图的步骤：

利用连续的激光点云数据生成子图，连续的指定帧数的激光数据生成一张子图，后一子图中包含前一子图中后指定帧数的激光点云数据；

闭环检测的步骤：

将子图中的每帧激光数据相应的激光点云的位姿信息存储于检查表中；

子图生成后接收的下一帧激光数据作为关键帧；

将关键帧的位姿信息与查找表中的激光点云的位姿信息进行比对；当二者相似时，将两者进行闭环连接；当二者不相似时，将当前经里程计数据、IMU数据修正后的激光数据插入到上一子图的最后指定数目的帧数据中，构成新的子图的帧数据，将机器人当前的位置调整为关键帧对应的位置；

图优化的步骤：

通过图的节点表示机器人的位姿，通过连接节点的边来表示机器人位姿间的变换关系；

计算各机器人位姿与经变换矩阵变换后的估计位姿之间的误差；

计算带有若干条边的图的误差目标函数；

通过多次迭代求得使目标函数收敛的机器人位姿，用于更新各个子图中的机器人位姿，即可构成最终的建图结果。

进一步而言，所述通过ICP算法将相邻两帧激光点云数据作对比，分析出二者之间的变换矩阵的方法为；

1)将相邻两帧激光点云数据中，使用前一帧激光点云的点集P中的每个数据点去后一帧激光点云的点集S中搜索与该数据点距离最近的点，并将其保存到第三点集Q中；

2)分别计算点集P、Q的重心位置坐标，进行点集中心化，将点集中的所有数据点扣除重心点坐标后，生成新的点集D、M；

3)分别由新的点集D、M计算协方差矩阵，并求解由该协方差矩阵组成的四元数矩阵的最大特征值及其最大特征向量；计算点集D、M之间的旋转矩阵R；

4)通过点集P、Q的重心点和旋转矩阵确定平移矩阵T；

5)将点集P按照旋转矩阵R和平移矩阵T进行变换，形成新的点集P′，通过点集P′与点集Q计算所有序号相同的对应点间距离平方和，该距离平方和除以数据点总数，得到的值I作为迭代判断数值；

6)迭代过程中，当I小于阈值TH时，ICP算法停止迭代，否则重复步骤1)～5)。

进一步而言，所述阈值TH计算公式如下：

TH＝Speed/F

其中，S为机器人的移动速度，F为激光传感器的采集频率。

进一步而言，所述变换矩阵T′与旋转矩阵R、平移矩阵T之间的关系为：

进一步而言，所述使用相邻两帧激光点云数据中前一帧的激光点云数据及变换矩阵，得到机器人当前位姿的估计值之后，还包括：

利用当前里程计数据和惯性导航单元数据对机器人当前位姿的估计值进行修正。

进一步而言，所述瓦片式建图的步骤中，子图采用栅格地图的形式表示，区别表示每个栅格是空闲还是占据状态。

进一步而言，所述瓦片式建图的步骤中，每建立一张子图，进行一次闭环检测。

进一步而言，所述判断相似的具体方法为：

计算检查表中各子图的每一帧的置信度candidate_probability，计算公式如下：

其中，m为该子图中某一帧激光数据对应的激光点云占据的地图栅格总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

根据每个子图中每个位姿与当前帧相应位姿的位姿差来计算检查表中每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与当前帧相应位姿间沿x轴的位移，y_offset是每个候选位姿与当前帧相应位姿间沿y轴的位移，trans_weight是位移权重，candiate.rotation是每个候选位姿与当前帧相应位姿间旋转角度，rot_weight是旋转角度权重；

将检查表中每个候选位姿的置信度candidate_probability与置信度权重

candidate_weight的乘积作为当前候选位姿的置信度分值score，公式如下：

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿，当该位姿的得分score大于某一阈值时，认为当前关键帧的位姿信息与检查表中某一帧对应的激光点云的位姿信息相似。

进一步而言，所述图优化的步骤中，机器人的位姿被表示成图中的节点，观测信息经过处理后转变为机器人位姿间的约束关系，并通过连接节点的边来表示；图的节点描述机器人位姿，所有节点的状态变量为X＝(x₁,x₂,x₃…,x_n)，其中x₁,x₂,x₃…,x_n分别为各个节点在全局坐标系下的位姿；边则描述机器人位姿之间的变换关系，由通过ICP算法得到的变换矩阵进行约束；节点P_i和节点P_j之间观测方程表示为：

z_j＝x_iT_i,j；

其中，x_i为节点P_i相应的机器人在全局坐标系下的位姿，Z_j为节点P_j的观测值，即x_i经变换矩阵变换后的估计位姿，T_i,j为节点P_i与P_j之间的变换关系，；

机器人位姿x_j与经变换矩阵变换后的估计位姿z_j之间存在误差e(z_j,x_j)，使用下式表示：

e(z_j,x_j)＝x_iT_i,j-x_j；

其中T_i,j为节点P_i与P_j之间的变换关系，x_i为节点P_i相应的机器人在全局坐标系下的位姿，x_j为节点P_j相应的机器人在全局坐标系下的位姿；

带有若干条边的图的误差目标函数E(x)为：

其中，n为边的数目，Ω_k为协方差矩阵的逆；

假设x_k增加一个增量Δx，通过多次迭代最终求得满足目标函数E(x)的收敛值，用求得的Δx更新状态变量X，X中的各个节点的位姿即可构成最终的建图结果。

本发明的有益效果如下：采用本发明的基于图优化的变电站巡检机器人建图方法，采用ICP算法对激光点云数据进行配准，从而获得障碍物完整表面的激光点云数据；之后采用瓦片式的建图方法分块建图，相邻地图块之前有重叠区域，这样可以预加载下一块地图，防止跨越地图时的位姿跳变；通过将已完成子图的各帧激光点云数据的位姿信息存储到查找表中，新的激光点数据的位姿信息与查找表中的数据进行比对，根据二者之间的相似性完成闭环检测；最后利用图优化的方法矫正机器人的位姿信息，消除理论值与实际值之间的差距，使得建立的地图更加接近实际情况。经测试，采取本发明的技术方案后，机器人的建图效率大大提高，通过瓦片式分块建图、闭环检测、图优化等技术手段，有效的减少了系统的累积误差。

附图说明

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

图2是本发明实施例的ICP算法流程图。

图3是本发明实施例的搜索临近点示意图。

图4是本发明实施例的闭环检测流程图。

图5是本发明实施例的关键帧闭环连接原理图。

图6是本发明实施例的图优化求解流程图。

具体实施方式

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

实施例：

本发明的一个实施例，介绍了一种基于图优化的变电站巡检机器人建图方法。

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

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

地图坐标系是全局坐标系。计算机器人位姿时使用的是地图坐标系。

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

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

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

参照图1，本实施例的基于图优化的变电站巡检机器人建图方法，其实现的主要步骤分别在前端和后端完成。其中，前端完成激光点云数据的获取和处理、数据配准、瓦片式建图；后端通过高计算性能的计算机进行闭环检测、图优化。具体步骤如下：

1、激光点云数据的获取和处理

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

2)将激光数据进行处理得到激光点云数据，对激光点云数据进行过滤，即去掉激光点云数据中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点。计算有效激光点云在激光传感器坐标系中的位姿。具体如下：

每得到一帧来自机器人的二维激光传感器的激光数据laserscan后，ROS系统对激光数据进行处理得到激光点云数据pointcloud，该激光点云数据pointcloud是每帧激光数据所包含的laserscan.size()个激光数据点信息的统称，反映了各激光反射点在激光传感器坐标系中的坐标。不同激光传感器的帧率、扫描角度、每帧包含的数据点数量不同，ROS系统在定位过程中使用激光点云数据来进行处理和计算。

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

2、激光点云数据配准

ROS系统通过ICP算法(即迭代就近点法)将相邻两帧激光点云数据作对比，分析出二者之间的变换矩阵T。具体原理为：设相邻两帧激光点云数据中，前一帧激光点云的点集为S＝{S1,S2,...,Sn}，后一帧激光点云的点集为P＝{P1,P2,...,Pn}，ICP算法假设待匹配的两个点集S、P中扫描数据是完全重叠的，但由于激光测距仪所测数据离散的特性，在两个点集S、P中与同一个点相应的扫描数据之间没有十分严格的真正匹配。为了增加有效点的重叠部分，并且缩短计算时间，选取激光测距仪扫描一轮所测数据的正前方指定角度(例如115～135度)范围的数据点作为待匹配数据的点集P。下文结合图2，对ICP算法进行详细说明：

2-1)对于点集P中的每个数据点Pi，搜索点集S中与该点距离最近的点Sj。

如图3所示，两点间的欧式距离为其中，j为点集S中的点的序号，i为点集P中的点的序号，x_sj为点集S中序号为j的点在x轴的坐标，x_pi为点集P中序号为i的点在x轴的坐标，y_sj为点集S中序号为j的点在y轴的坐标，y_pi点集P中序号为i的点在y轴的坐标。通过遍历点集S中的点，计算两点间欧式距离，找到距离点集P中第i个点最近的点，将其保存到点集Q中，记为q_i。

2-2)分别计算点集P、Q的重心位置坐标，进行点集中心化，将点集中的所有数据点扣除重心位置坐标后，生成新的点集D、M。计算两个点集P、Q的重心d、m的坐标 的公式如下：

其中，k为点集中的当前点序号，n为点集中点的总数。

2-3)分别由新的点集D、M计算协方差矩阵，并求解由它组成的一个四元数矩阵的最大特征值及其最大特征向量。新点集D、M的协方差矩阵为：

其中，S_xx为点集D的x维度与点集M的x维度的协方差，S_xy为为点集D的x维度与点集M的y维度的协方差，S_xz为点集D的x维度与点集M的z维度的协方差，S_yx为点集D的y维度与点集M的x维度的协方差，S_yy为点集D的y维度与点集M的y维度的协方差，S_yz为点集D的y维度与点集M的z维度的协方差，S_zx为点集D的z维度与点集M的x维度的协方差，S_zy为点集D的z维度与点集M的y维度的协方差，S_zz为点集D的z维度与点集M的z维度的协方差，x_Dk、y_Dk分别为点集D中第k个x、y的坐标值，x_Mk、y_Mk分别为点集M中第k个x、y的坐标值。

通过SVD(Singular Value Decomposition，奇异值分解)方法获得协方差矩阵的U、S和V，U和V分别是协方差矩阵的奇异向量，S是协方差矩阵的奇异值，公式如下：

[U,S,V]＝SVD(Cov)

根据U和V，可以利用下式计算点集D、M之间的旋转矩阵R。

R＝VU^T

2-4)在旋转矩阵R被确定后，由于平移矩阵T仅仅是两个点集的重心差异，可以通过点集P、Q的重心点和旋转矩阵确定。

T＝m-(R×d)

其中，m、d分别是点集P、Q的重心坐标向量。

2-5)将点集P按照旋转矩阵R和平移矩阵T变换后形成新的点集P′，通过点集P′与点集Q计算所有序号相同的对应点间距离平方和，该距离平方和除以数据点总数，得到的值I作为迭代判断数值。

式中，n为数据点集总数，为点集Q中序号为j的点的坐标向量，为点集P′中序号为j的点的坐标向量。

2-6)迭代过程中，当数值I小于阈值TH时，ICP算法就停止迭代，否则重复步骤2-1)～2-5)。阈值TH计算公式如下：

TH＝Speed/F

其中，Speed为机器人的移动速度，F为激光传感器的采集频率。

3、利用相邻两帧激光点云的变换关系矩阵和上一帧激光点云数据，对机器人的当前位姿进行估算，并利用里程计数据，惯性导航单元(IMU)数据进行修正。

4、建立子图

ROS系统中的建图模块根据变换矩阵T′将相邻两帧激光数据进行整合，利用连续的激光数据生成子图。具体如下：

采用瓦片式的建图方法，利用连续的激光数据生成子图，子图的形式由栅格地图形式表示，栅格的大小决定地图的分辨率，每个栅格用s＝0或s＝1表示该格点是空闲还是占据状态。对于每一个能观察到的栅格，灰色部分表示激光末端点所落在的栅格，即检测到存在障碍物，表示被占据的状态，白色表示空闲状态。每连续的20帧激光数据生成一张子图，第二张子图包含上一张子图的后10帧激光数据。例如，将第t帧到t+20帧激光数据进行配准，形成子图A1，第t+10帧到t+30帧激光数据进行配准，形成子图A2，第t+20帧到t+40帧激光数据进行配准，形成子图A3，以此类推。

5、闭环检测

ROS系统中的建图模块将子图中的激光点云的位姿信息存储于检查表中，每当步骤4中建立一张子图，进行一次闭环检测。首先提取子图完成后的下一帧激光数据作为关键帧，并将子图中的激光点云的位姿信息存储于检查表中，最后利用关键帧的位姿信息与查找表中的数据作比对，当相似时，将两者进行闭环连接。如图5所示，节点1～10为由上述步骤1～4所建立的各个相连接的子图(子图1～子图9)。当建立子图9时，判断子图9中的关键帧的位姿信息与查找表中子图1的位姿信息相似，则说明子图1与子图9间在地图上是相关联的、相匹配的，求出二者之间的位姿变换矩阵T′，以便在后面进行图优化时，将该变换矩阵T′作为约束条件放入优化模型。由此，在子图2与子图9之间建立连接关系，即进行闭环连接，表明子图2与子图9在地图上位置相邻。同样的，当建立子图6时，判断子图6中的关键帧的位姿信息与查找表中子图4的位姿信息相似，则求出二者之间的位姿变换矩阵T′，在子图4与子图6之间建立连接关系。如图5所示，连续编号的子图间利用激光帧间位姿进行匹配，从而在子图间建立了连接；而闭环检测是在相关联的不连续编号的子图间间建立连接，以减少这两个子图间的激光帧的累积误差。通过子图间关键帧的位姿信息的相似性进行闭环检测，有效减少匹配候选集的数量，提高检测速度。具体方法如下：

5-1)每一张子图完成后，将子图中每帧激光数据对应的激光点云的位姿信息存储于队列H中；

5-2)接收到新的一帧激光数据后，将该激光数据的激光点云的位姿信息与队列H中的信息进行比对；当当前激光点云的位姿信息与队列H中某一帧激光数据的激光点云的位姿信息相似时，将当前激光数据与队列H中查找到的该帧激光数据进行闭环连接，否则转入步骤3)。

其中，当前激光点云的位姿信息与队列H中某一帧激光数据对应的激光点云的位姿信息是否相似的判别方法如下：

计算队列H中机器人各子图的每一帧的置信度candidate_probability，计算公式如下：

其中，m为某个子图中某一帧激光数据对应的激光点云占据的地图栅格总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格的置信度是probability_xnyn。

根据每个子图中机器人的每个位姿与当前帧对应位姿的位姿差来计算队列H中每个闭环候选位姿对应的置信度权重candidate_weight，公式如下:

其中，x_offset是机器人每个位姿与当前位姿间沿x轴的位移差，y_offset是机器人每个位姿与当前位姿沿y轴的位移差，trans_weight是位移权重，candiate.rotation机器人每个位姿与当前位姿的旋转角度差，rot_weight是旋转角度权重。

队列H中每个闭环候选位姿的置信度candidate_probability与置信度权值candidate_weight的乘积作为当前闭环候选位姿的置信度分值，公式如下：

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿，当该位姿的得分score大于某一阈值时，认为当前激光点云的位姿信息与队列H中某历史激光点云的位姿信息相似。

3)以上一张子图的后10帧数据为基础(作为新的子图中的10帧数据)，将当前经里程计数据、IMU数据修正后的激光数据插入到新的子图的帧数据中，构成新的子图的20帧数据。将机器人当前的位置调整为关键帧对应的位置。

6、图优化

为了减小建图过程中的累积误差，ROS系统中的建图节点采用图优化的方法对机器人的位姿进行优化。如图6所示，在图优化的过程中，机器人的位姿被表示成图中的节点，观测信息经过处理后转变为机器人位姿间的约束关系，并通过连接节点的边来表示。图的每个节点描述了机器人位姿，所有节点的状态变量为X＝(x₁,x₂,x₃…,x_n)，其中x₁,x₂,x₃…,x_n分别为各个节点在全局坐标系下的位姿；边则描述了机器人位姿之间的变换关系边由通过ICP算法得到的变换关系进行约束。机器人经过运动T_i,j由P_i运动到P_j，节点P_i和节点P_j之间观测方程可以表示为：

z_j＝x_iT_i,j，

其中，Z_j为节点P_j的观测值，T_i,j为节点P_i与P_j之间的变换关系，x_i为节点P_i在全局坐标系下的位姿。

在优化过程中，每个节点都有一个估计值，那么由一条边连接的两个顶点各有一个估计值，它们与这条边的约束(即两个顶点估计值间的变换关系)之间会有一个误差。也就是说，理想情况下观测方程为：

z_j＝x_iT_i,j (1)

实际上机器人位姿与经变换后的估计位姿之间存在误差：

e(z_j,x_j)＝x_iT_i,j-x_j (2)

由于每条边都产生了误差，假设建立的是一个带有n条边的图，其目标函数可以写成:

其中，信息矩阵Ω_k是协方差矩阵的逆，是一个对称矩阵。它的每个元素Ω_i,j用作误差的系数，对e(z_i,x_i)、e(z_j,x_j)的误差相关性进行预计。最简单的是把Ω_k设成对角矩阵，对角阵元素的大小表明对各元素相应误差的重视程度。采用其他协方差矩阵也可以。

x_k增加一个增量Δx，误差值则从e(z_k,x_k)变为e(z_k,x_k+Δx)，对误差项进行一阶展开：

J_k是e(z_k,x_k)关于x_k的导数，矩阵形式下为雅可比阵。对于第K条边的目标函数项，进一步展开有：

将与Δx无关的项整理成常数项C_k，Δx的一次项系数整理成b_k，Δx二次项系数写成H_k。

将公式(5)代入(3)中，重写目标函数：

其中x_k发生增量后，目标函数E(x)项的改变值为：

ΔE(x)＝2bΔx+Δx^THΔx (7)

为了使ΔE(x)取得最小值，令ΔE(x)对Δx的导数为零，有

通过多次迭代最终求得满足目标函数的收敛值，用求得的Δx更新状态变量X，X中的各个节点的位姿即可构成最终的建图结果。

采用图优化的方法可以对整个运动过程中机器人位姿进行优化，有效的降低了系统的积累误差。

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

Claims (9)

1.一种基于图优化的变电站巡检机器人建图方法，其特征在于，包括：

激光点云数据配准的步骤：

通过ICP算法将相邻两帧激光点云数据作对比，分析出二者之间的变换矩阵；

使用相邻两帧激光点云数据中前一帧的激光点云数据及变换矩阵，得到机器人当前位姿的估计值；

瓦片式建图的步骤：

利用连续的激光点云数据生成子图，连续的指定帧数的激光数据生成一张子图，后一子图中包含前一子图中后指定帧数的激光点云数据；

闭环检测的步骤：

将子图中的每帧激光数据相应的激光点云的位姿信息存储于检查表中；

子图生成后接收的下一帧激光数据作为关键帧；

将关键帧的位姿信息与查找表中的激光点云的位姿信息进行比对；当二者相似时，将两者进行闭环连接；当二者不相似时，将当前经里程计数据、IMU数据修正后的激光数据插入到上一子图的最后指定数目的帧数据中，构成新的子图的帧数据，将机器人当前的位置调整为关键帧对应的位置；

图优化的步骤：

通过图的节点表示机器人的位姿，通过连接节点的边来表示机器人位姿间的变换关系；

计算各机器人位姿与经变换矩阵变换后的估计位姿之间的误差；

计算带有若干条边的图的误差目标函数；

通过多次迭代求得使目标函数收敛的机器人位姿，用于更新各个子图中的机器人位姿，即可构成最终的建图结果。

2.根据权利要求1所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述通过ICP算法将相邻两帧激光点云数据作对比，分析出二者之间的变换矩阵的方法为；

1)将相邻两帧激光点云数据中，使用前一帧激光点云的点集P中的每个数据点去后一帧激光点云的点集S中搜索与该数据点距离最近的点，并将其保存到第三点集Q中；

2)分别计算点集P、Q的重心位置坐标，进行点集中心化，将点集中的所有数据点扣除重心点坐标后，生成新的点集D、M；

3)分别由新的点集D、M计算协方差矩阵，并求解由该协方差矩阵组成的四元数矩阵的最大特征值及其最大特征向量；计算点集D、M之间的旋转矩阵R；

4)通过点集P、Q的重心点和旋转矩阵确定平移矩阵T；

5)将点集P按照旋转矩阵R和平移矩阵T进行变换，形成新的点集P′，通过点集P′与点集Q计算所有序号相同的对应点间距离平方和，该距离平方和除以数据点总数，得到的值I作为迭代判断数值；

6)迭代过程中，当I小于阈值TH时，ICP算法停止迭代，否则重复步骤1)～5)。

3.根据权利要求2所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述阈值TH计算公式如下：

TH＝Speed/F

其中，Speed为机器人的移动速度，F为激光传感器的采集频率。

4.根据权利要求2所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述变换矩阵T′与旋转矩阵R、平移矩阵T之间的关系为：

5.根据权利要求2所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述使用相邻两帧激光点云数据中前一帧的激光点云数据及变换矩阵，得到机器人当前位姿的估计值之后，还包括：

利用当前里程计数据和惯性导航单元数据对机器人当前位姿的估计值进行修正。

6.根据权利要求1所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述瓦片式建图的步骤中，子图采用栅格地图的形式表示，区别表示每个栅格是空闲还是占据状态。

7.根据权利要求1所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述瓦片式建图的步骤中，每建立一张子图，进行一次闭环检测。

8.根据权利要求1所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述判断相似的具体方法为：

计算检查表中各子图的每一帧的置信度candidate_probability，计算公式如下：

其中，m为该子图中某一帧激光数据对应的激光点云占据的地图栅格总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

根据每个子图中每个位姿与当前帧相应位姿的位姿差来计算检查表中每个候选位姿对应的置信度权重candidate_weight，公式如下：

其中，x_offset是每个候选位姿与当前帧相应位姿间沿x轴的位移，y_offset是每个候选位姿与当前帧相应位姿间沿y轴的位移，trans_weight是位移权重，candiate.rotation是每个候选位姿与当前帧相应位姿间旋转角度，rot_weight是旋转角度权重；

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

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿，当该位姿的得分score大于某一阈值时，认为当前关键帧的位姿信息与检查表中某一帧对应的激光点云的位姿信息相似。

9.根据权利要求1所述的基于图优化的变电站巡检机器人建图方法，其特征在于，所述图优化的步骤中，机器人的位姿被表示成图中的节点，观测信息经过处理后转变为机器人位姿间的约束关系，并通过连接节点的边来表示；图的节点描述机器人位姿，所有节点的状态变量为X＝(x₁,x₂,x₃…,x_n)，其中x₁,x₂,x₃…,x_n分别为各个节点在全局坐标系下的位姿；边则描述机器人位姿之间的变换关系，由通过ICP算法得到的变换矩阵进行约束；节点P_i和节点P_j之间观测方程表示为：

z_j＝x_iT_i,j；

其中，x_i为节点P_i相应的机器人在全局坐标系下的位姿，Z_j为节点P_j的观测值，即x_i经变换矩阵变换后的估计位姿，T_i,j为节点P_i与P_j之间的变换关系，；

机器人位姿x_j与经变换矩阵变换后的估计位姿z_j之间存在误差e(z_j,x_j)，使用下式表示：

e(z_j,x_j)＝x_iT_i,j-x_j；

其中T_i,j为节点P_i与P_j之间的变换关系，x_i为节点P_i相应的机器人在全局坐标系下的位姿，x_j为节点P_j相应的机器人在全局坐标系下的位姿；

带有若干条边的图的误差目标函数E(x)为：

其中，n为边的数目，Ω_k为协方差矩阵的逆；

假设x_k增加一个增量Δx，通过多次迭代最终求得满足目标函数E(x)的收敛值，用求得的Δx更新状态变量X，X中的各个节点的位姿即可构成最终的建图结果。