CN104914865A - 变电站巡检机器人定位导航系统及方法 - Google Patents

Abstract

本发明公开了变电站巡检机器人定位导航系统及方法，激光测距模块及航位推算模块所得到的信心分别输出至地图创建模块与位姿解算模块，地图创建模块创建表示机器人运行环境的激光路标地图与激光栅格地图并输入地图存储模块进行存储，位姿解算模块利用地图存储模块中的地图数据以及航位推算模块的输出数据实时解算机器人当前位置和姿态并将所得到的位姿数据接入路径规划模块与运动控制模块，路径规划模块生成机器人运行路径数据并输出至运动控制模块，运动控制模块依据机器人当前位置姿态数据及路径规划数据生成运动控制量，之后由电机控制模块执行。本发明保证定位导航实时性的前提下实现巡检机器人变电站内精确定位。

Description

变电站巡检机器人定位导航系统及方法

技术领域

本发明涉及自主移动机器人定位导航领域，特别是涉及一种变电站巡检机器人用定位导航系统及方法。

背景技术

变电站巡检机器人是一种全自主运行的地面移动机器人，其可携带可见光摄像机、红外热像仪、拾音器等传感器对电力设备进行自动巡检，有效降低人工巡检的劳动强度，保障变电站设备安全运行。

在变电站巡检机器人的应用中，精确定位导航是保证其完成巡检任务的关键。目前，变电站巡检机器人定位导航系统一般采用磁轨迹引导加RFID(Radio FrenquencyIdentification，射频识别)标签辅助定位方法实现，该方法依靠安装于机器人底盘前部的磁传感器阵列检测机器人相对于磁轨迹的偏移，据此控制机器人沿磁轨迹运动，同时在停靠点处设置RFID标签，以辅助机器人停靠。该方法可靠性好、抗干扰能力强并具备较高的重复定位精度，但是磁条轨迹的铺设涉及大量现场施工、磁条成本及磁条维护成本、一旦磁轨迹铺设完工则巡检路线不能灵活改变，机器人底盘受限于磁传感器检测距离，使得机器人只能在平地上运动，对于站内砂石、草地等自然地形不能适用。

为解决这一问题，差分GPS、航位推算、惯性及视觉标识等定位导航方式已被引入巡检机器人并在变电站现场进行了测试，但由于变电站室外大规模环境里，电力设备林立且存在强电磁干扰，上述定位导航方法，因受电磁干扰、误差累积及视觉标识受雨雪遮挡等因素影响难以在变电站内可靠工作。从公开的资料可知，基于激光传感器的定位导航表现出了良好的定位精度和对室外变电站环境具有较好适应性。

专利201120115524.7公开了一种变电站巡检机器人用激光定位导航系统，该系统运行前需要预先在机器人运行路线两侧安装反光标识，机器人依据激光传感器检测到的反光标识信息就可以计算得到其在全局坐标系下的位置和姿态。该方法虽然可靠而且精度好，可灵活改变机器人运行路线，但是由于需要在站内设定反光标识，因此不可避免的会带来一定的运维费用。

专利201310686467.1公开了一种巡检机器人无轨化导航方法，其通过从激光传感器扫描数据中提取环境特征，利用SLAM技术构建机器人运行环境地图，实现机器人在站内的定位导航。该方法所构建的环境地图数据存储量小，定位导航实时性好，同时可灵活配置机器人运行区域，无需在变电站内加装辅助设施，极大降低了定位导航系统前期施工和后期运维费用，但该方法需要从激光传感器扫描得到的原始数据中提取环境特征，在这一过程中不可避免的会引入误差，另外变电站内部分区域特征比较稀疏，所需特征可能无法提取，因此以上两方面最终都会影响机器人站内定位导航效果。

文献“基于地图匹配的变电站巡检机器人激光导航系统设计”提出了一种利用激光数据匹配的机器人地图创建及定位导航系统，系统中的地图创建模块通过将里程计与激光扫描数据融合创建变电站环境地图，之后机器人位姿解算利用ICP-EKF算法将激光传感器获取数据与创建的环境地图进行匹配，从而获得机器人全局坐标系下的位置和姿态，但该系统中地图数据以栅格方式存储，为实现所需定位解算精度其必须采用小尺度栅格，在变电站大规模环境下该方式的栅格数据存储量较大，必然造成定位导航过程中的计算量较大。

另外，上述文献与专利201110216379.6公开了一种利用Floyd(弗洛伊德)算法计算所有路径点之间的最短路径的路径规划方法，实现了机器人在站内执行特殊巡检任务时沿最短路径运行，但实际使用中由于变电站内停靠点数量众多，以上路径规划算法只能采用离线方式计算节点间的最短路径，一旦计算并存储完成，每一个新节点的加入都必须对所有节点重新计算和存储，计算效率不高且路径存储量较大，因此上述方法难以满足变电站某些突发情况发生时，需要在线规划机器人运行路径，使机器人迅速到达站内临时设定的停靠点执行检测任务的需求。

总之，现有的技术存在的主要问题为：

1、变电站巡检机器人无法实现定位导航实时性的前提下实现巡检机器人变电站内精确定位；

2、从激光数据提取环境特征容易引入误差对定位导航精度的影响，无法实现通过对当前激光测距数据与地图中激光路标包含的测距数据的匹配，机器人停靠点处的定位精度不高。

3、变电站巡检机器人现有的路径规划计算效率较低，无法满足机器人根据特殊巡检任务在线规划机器人行驶路径的需求。

4、现有的变电站巡检机器人没有建立运行环境地图，没有与原有全局激光路标地图融合，效率低下。

发明内容

为解决现有技术存在的不足，本发明公开了一种基于多分辨率地图的巡检机器人定位导航系统及方法，考虑到变电站巡检机器人执行巡检任务时需要在停靠点进行精确停靠后才能有效的执行巡检任务，而对停靠点间机器人运行过程中的定位精度要求不高，为此，本发明通过构建表示机器人运行环境的激光路标、栅格及拓扑相结合的多分辨率地图，利用基于栅格地图的粗定位与基于激光路标原始数据精细定位相结合定位解算方法，保证定位导航实时性的前提下实现巡检机器人变电站内精确定位；同时，针对变电站机器人巡检路线特点，提出了一种基于拓扑点分类的全局路径规划方法，可有效提高路径规划计算效率并降低存储消耗，满足机器人某些情况下需要在线规划机器人运行路径的需求。

为实现上述目的，本发明的具体方案如下：

变电站巡检机器人定位导航系统，包括：激光测距模块所采集的机器人周边障碍物相对于激光传感器扫描中心的距离信息及航位推算模块所得到的当前机器人在全局坐标系下的位置姿态估计分别输出至地图创建模块与位姿解算模块，地图创建模块根据得到的信息创建表示机器人运行环境的激光路标地图与激光栅格地图并输入地图存储模块进行存储，所述位姿解算模块利用地图存储模块中的地图数据以及航位推算模块的输出数据实时解算机器人当前位置和姿态并将所得到的位姿数据接入路径规划模块与运动控制模块，路径规划模块生成机器人运行路径数据并输出至运动控制模块，运动控制模块依据机器人当前位置姿态数据及路径规划数据生成运动控制量，之后由电机控制模块执行。

所述激光测距模块，由固定安装于机器人本体上的激光测距传感器构成。

所述航位推算模块，具体用于将运动控制模块上发的机器人驱动电机轴编码器脉冲数据转换为设定计算周期内驱动轮移动距离，经机器人航位推算模型得到当前机器人在全局坐标系下的位置姿态估计。

所述地图创建模块，该模块融合激光测距数据和里程计数据，利用SLAM(SimultaneousLocalization and Mapping，同时定位与地图构建)技术创建表示机器人运行环境的激光路标地图与激光栅格地图。

所述地图存储模块，该模块基于实时数据库实现，可实时进行地图数据的存储及查询操作，同时机器人巡检任务生成过程中也需要与该模块进行数据交互，以确定巡检任务起点和终点信息。

所述位姿解算模块，利用地图存储模块中的地图数据，以及航位推算模块的输出，利用基于栅格地图的粗定位与基于激光原始数据精细定位相结合方法，实时解算机器人当前位置和姿态。

所述路径规划模块，该模块负责生成机器人激光路表拓扑地图与全局路径规划用拓扑地图，依据巡检任务规划机器人巡检运行路线。

所述运动控制模块：可根据机器人定位模块输出机器人位姿数据及路径规划模块输出路径数据实现机器人对设定路径的跟踪，所生成的控制量由电机驱动模块执行。

所述电机驱动模块：接收运动控制模块的电机控制量，依据电机轴编码器信号闭环控制控制巡检机器人驱动轮伺服电机，实现机器人在变电站内运行。

基于多分辨率地图的变电站巡检机器人定位导航的方法，包括以下步骤：

步骤一：利用激光测距模块采集的机器人周边障碍物相对于激光传感器扫描中心的距离信息，利用航位推算模块得到的当前机器人在全局坐标系下的位置姿态估计，将激光测距模块及航位推算模块的结果分别输出至地图创建模块与位姿解算模块；

步骤二：地图创建模块根据得到的信息创建表示机器人运行环境的激光路标地图与激光栅格地图并输入地图存储模块进行存储；

步骤三：位姿解算模块利用地图存储模块中的地图数据以及航位推算模块的输出数据实时解算机器人当前位置和姿态并将所得到的位姿数据接入路径规划模块与运动控制模块；

步骤四：路径规划模块生成机器人激光路表拓扑地图与全局路径规划用拓扑地图，依据巡检任务规划机器人巡检运行路线，路径规划得到的路径数据输入运动控制模块执行。

所述步骤二中，地图创建模块创建表示机器人运行环境的激光路标地图与激光栅格地图，具体包括以下步骤：

步骤：2-1)：地图创建初始化；

步骤:2-2)：获取k至k+1时刻过程中机器人位置姿态变化量，经航位推算模块计算得到机器人全局坐标系下的位置姿态估计值；

步骤：2-3)：在k+1时刻，获取激光测距模块数据，与地图存储模块中距机器人当前位置最近的已有激光路标包含的激光测距数据匹配，得到机器人相对于该路标的距离和夹角的观测向量；

步骤：2-4)：利用扩展卡尔曼滤波方法将航位推算输出机器人位置姿态估计值与匹配得到的路标观测值融合，从而得到机器人当前位置姿态的精确估计；

步骤：2-5)：计算当前机器人位置与路标位置的距离，并与设定的相邻路标间隔距离阈值比较，再执行下子步骤：

2-5-1)如果小于等于预先设定的阈值，机器人返回步骤2-2)执行；

2-5-2)如果大于设定阈值，则读取激光测距数据和机器人当前位置姿态，形成新的激光路标并进行存储，之后再返回步骤2-2)执行；

步骤：2-6)：重复执行步骤2-2)至2-5)，直至建立完整的机器人运行环境电子地图；

步骤：2-7)：对上述步骤生成地图中的激光路标数据进行进一步数据处理，路标点相关激光扫描数据可栅格化后形成栅格地图。

所述步骤：2-1)：地图创建初始化，具体包括：

2-1-1)建立全局坐标系，设定地图创建起始点处机器人全局坐标系下的初始位置和姿态，设定地图中相邻路标间隔距离阈值；

2-1-2)读取起始点处激光测距模块测距数据；

2-1-3)将激光测距数据与机器人初始位置姿态数据组合后形成地图创建初始点激光路标，并输入地图存储模块进行存储。

所述步骤三，具体包括以下步骤：

步骤3-1)：定位解算初始化；

步骤3-2)：k时刻查询地图存储模块数据库存储的距离机器人当前位置最近的路标，并载入该路标的激光测距数据以及路标相关区域的栅格地图；

步骤3-3)：读取k至k+1时刻过程中机器人位置姿态变化量，经航位推算模块计算得到机器人全局坐标系下的位置姿态估计值；

步骤3-4)：基于栅格地图数据，结合航位推算数据和激光测距数据，利用粒子滤波算法估计机器人在全局坐标系下的位置和姿态，实现基于栅格地图的粗定位，定位精度由设定栅格尺寸决定；

步骤3-5)：如果机器人需要在该区域内设定停靠点进行精确停靠，则机器人在进入距离停靠点设定阈值范围之后执行以下子步骤，否则执行步骤3-6)；

步骤3-6)：计算机器人与当前路标之间的距离，并与地图分段加载阈值比较；

步骤3-7)：重复执行步骤3-2)至3-6)直至整个定位任务完成。

所述步骤3-1)中，定位解算初始化，包含以下子步骤：

3-1-1)设定机器人定位开始时的初始位置和姿态；

3-1-2)设定地图分段加载阈值以及停靠点停靠精度阈值。

所述步骤3-5)：机器人在进入距离停靠点设定阈值范围之后执行以下子步骤，具体为：

3-5-1)读取激光测距模块数据，与当前所用激光路标所包含的激光测距数据匹配，得到机器人相对于该路标的位置姿态变化量的观测值；

3-5-2)利用扩展卡尔曼滤波方法对航位推算模块输出的机器人位置姿态估计值与激光测距模块得到的机器人位置姿态观测值进行融合，得到机器人当前位置姿态的精确估计，从而保证机器人停靠点的停靠精度。

所述步骤3-6)：计算机器人与当前路标之间的距离，并与地图分段加载阈值比较，之后执行以下子步骤：

3-6-1)如果得到的距离小于等于设定的阈值，则返回步骤3-3)执行；

3-6-2)如果距离大于设定阈值，则表示机器人已经远离当前路标，需要新的路标辅助机器人定位，此时返回步骤3-2)执行。

所述步骤四中，具体包括以下步骤：

步骤4-1)：激光路标拓扑地图建立；

步骤4-2)：激光路标拓扑点按路径分类，首先将巡检路线划分为首尾连接的巡检路径集合，之后将激光路标拓扑地图中的路标节点按照巡检路径划分，得到每条巡检路径包含的拓扑点集合；

步骤4-3)：建立以每条路径起点和终点对应拓扑点为节点，以整条巡检路径长度为权值的有向图数据结构，形成路径规划拓扑地图，并输入地图存储模块以临接矩阵方式存储。

步骤4-4)：当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置姿态和目标点位置姿态在，并在寻找激光路标拓扑地图中对应的拓扑点，判定这两点所属步骤4-2)建立的每条路径包含拓扑点的集合；

步骤4-5)：判定以上两拓扑点是否属于同一拓扑点集，若为同一点集，则表示机器人当前位置和目标点属于同一路径，此时可以直接判定这两点的相对位置关系，并输出路径规划数据至运动控制器执行，若不为同一点集则执行步骤4-6)；

步骤4-6)：将步骤4-4)选择的拓扑点插入路径规划拓扑地图相关路径起点和终点对应的拓扑点之间，并以其与所属路径起点和终点距离更新其与以上两点间拓扑地图权值；

步骤4-7)：利用路径规划算法计算机器人运行最优路径，将机器人所需运行路径拓扑点输出至运动控制器执行。

所述步骤4-1)：激光路标拓扑地图建立,包含以下子步骤：

4-1-1)将每个激光路标抽象为有向图数据结构的拓扑节点，以节点间的相对关系建立有向图数据结构，形成激光路标拓扑地图；

4-1--2)将生成的地图输入地图存储模块进行存储。

本发明的有益效果：

(1)基于变电站巡检机器人执行巡检任务时需要在停靠点进行精确停靠，而对停靠点间机器人运行过程中的定位精度要求不高的特点，构建了表示机器人运行环境的激光路标、栅格及拓扑相结合的多分辨率地图，利用基于栅格地图的粗定位与基于激光路标原始数据精细定位相结合定位解算方法，保证定位导航实时性的前提下实现巡检机器人变电站内精确定位。

(2)有效避免了从激光数据提取环境特征引入误差对定位导航精度的影响，通过对当前激光测距数据与地图中激光路标包含的测距数据的匹配，保证了机器人停靠点处的定位精度。

(3)基于变电站机器人巡检路线特点，提出了基于拓扑点分类的全局路径规划方法，通过对激光路标拓扑地图路径点的分类，建立路径规划用拓扑地图，有效提高了路径规划计算效率并降低了存储消耗，满足机器人根据特殊巡检任务在线规划机器人行驶路径的需求。

(4)由于利用了SLAM技术建立机器人运行环境地图，当变电站环境出现变化需要更新地图时，则只需导航机器人进入需更新地图的区域再次建图，新的地图数据则会在地图创建过程中与原有全局激光路标地图融合，从而提高了建图效率，方便后续工程化应用。

附图说明

图1，定位导航系统组成结构示意图；

图2，定位导航坐标系示意图；

图3，地图创建模块执行流程图；

图4，位姿解算模块执行流程图；

图5，全局路径规划执行流程图；

图6，构建的多分辨率地图示意图。

具体实施方式：

下面结合附图对本发明进行详细说明：

如图1所示：一种变电站巡检机器人定位导航系统，包括：激光测距模块、航位推算模块、地图创建模块、地图存储模块、定位解算模块、路径规划模块、运动控制模块、电机驱动模块。

各模块功能及数据连接关系如下：

激光测距模块：由固定安装于机器人本体上的激光测距传感器构成，用于测量机器人周边障碍物相对于激光传感器扫描中心的距离信息，所获取的数据接入地图创建模块、位姿解算模块及路径规划模块。

航位推算模块：将运动控制模块上发的机器人驱动电机轴编码器脉冲数据转换为设定计算周期内驱动轮移动距离和姿态变化，经机器人航位推算模型得到当前机器人在全局坐标系下的位置姿态估计，之后输出至地图创建模块与位姿解算模块。

地图创建模块：该模块融合激光测距数据和里程计数据，利用SLAM(SimultaneousLocalization and Mapping，同时定位与地图构建)技术创建表示机器人运行环境的激光路标地图与激光栅格地图，最后输入地图存储模块进行存储。

地图存储模块：该模块基于实时数据库实现，可实时进行地图数据的存储及查询操作，同时机器人巡检任务生成过程中也需要与该模块进行数据交互，以确定巡检任务起点和终点信息。

定位解算模块：利用地图存储模块中的地图数据，以及航位推算模块的输出，利用基于栅格地图的粗定位与基于激光原始数据精细定位相结合方法，实时解算机器人当前位置和姿态，所得到的位姿数据接入路径规划模块与运动控制模块。

路径规划模块：该模块负责生成机器人激光路标拓扑地图与全局路径规划用拓扑地图，依据巡检任务规划机器人巡检运行路线，路径规划得到的路径数据输入运动控制模块执行。运动控制模块：可根据机器人定位模块输出机器人位姿数据及路径规划模块输出路径数据实现机器人对设定路径的跟踪，所生成的控制量由电机驱动模块执行。

电机驱动模块：接收运动控制模块的电机控制量，依据电机轴编码器信号闭环控制控制巡检机器人驱动轮伺服电机，实现机器人在变电站内运行。

上述定位导航系统中：激光测距模块中激光传感器为二维平面激光传感器，扫描范围为180°或270°的扇形区域，其固定安装于机器人本体前部S点(如图2所示)，扫描范围中心线与机器人朝向一致；

航位推算模块中机器人航位推算模型为以激光传感器扫描中心S表示的机器人位置和姿态。为得到具体的航位推算模型，如图2建立全局坐标系XOY和传感器坐标系X_SO_SY_S,如下:

●全局坐标系XOY:原点可根据定位导航任务需求设定于机器人运行环境的任意位置，一旦设定就不能改变。

●传感器坐标系X_SO_SY_S：坐标原点位于激光传感器扫描中心点S，横轴为扫描范围中心线并与机器人朝向一致，纵轴与横轴垂直并符合右手法则。

依据以上坐标系间的相对位置关系，从k时刻到k+1时刻经一步航位推算输出的机器人全局坐标系下的位置姿态(x_k+1,y_k+1,θ_k+1)可按下式计算：

x_k+1＝x_k+Δs_k,k+1cosθ_k-Δθ_k,k+1[a sinθ_k+b cosθ_k]

y_k+1＝y_k+Δs_k,k+1sinθ_k+Δθ_k,k+1[a cosθ_k-b sinθ_k]

θ_k+1＝θ_k+Δθ_k,k+1

式中：

Δs_k,k+1和Δθ_k,k+1为k至k+1时刻过程中机器人运行的距离和姿态变化；

a和b表示激光传感器中心S相对于机器人轮轴连线中心C的横向和纵向偏移。

以上航位推算模型计算过程已经考虑了激光扫描中心S与机器人轮轴连线AB中心点C的相对关系，激光传感器测距数据可以更为方便地换至全局坐标系XOY进行后续处理。已知任意时刻机器人全局坐标系XOY下的位置姿态(x,y,θ)则，激光传感器检测到的障碍物上的一点P转换至全局坐标系XOY的转换关系如下：

式中：

x_P，y_P为任意时刻光传感器检测到的障碍物上的一点P转换至全局坐标系下的坐标；

r_P为点P点距离激光传感器中心的距离；

为点P与激光传感器中心连线相对于传感器坐标系X_SO_SY_S中X轴的夹角。

地图创建模块包含一种构建机器人运行环境的激光路标地图与激光栅格地图的方法，包括以下步骤(如图3所示)：

步骤1：地图创建初始化，包含以下子步骤：

(1)建立全局坐标系，设定地图创建起始点处机器人全局坐标系下的初始位置和姿态X₀＝[x₀,y₀,θ₀]^T，设定地图中相邻路标间隔距离阈值

(2)读取起始点处激光测距模块测距数据，测距数据包含扫描范围内等间隔测量的一系列测距数据，可记为其中r...为距离测量值，N为数据点数；

(3)将激光测距数据与机器人初始位置姿态数据组合后形成地图创建初始点激光路标并输入地图存储模块进行存储。

步骤:2：获取k至k+1时刻过程中机器人位置姿态变化量Δs_k,k+1和Δθ_k,k+1，经航位推算模块计算得到k+1时刻机器人全局坐标系下的位置姿态估计值

步骤3：在k+1时刻，获取激光测距模块数据与地图存储模块中距机器人当前位置最近的已有激光路标)包含的激光测距数据匹配，得到机器人相对于该路标的距离和夹角的观测向量

步骤4：利用扩展卡尔曼滤波方法将航位推算输出机器人位置姿态估计值与匹配得到的路标观测值融合，从而得到机器人当前位置姿态的精确估计

步骤5：计算当前机器人位置与路标位置的距离并与设定的相邻路标间隔距离阈值比较，再执行下子步骤：

(1)如果小于等于预先设定的阈值，即机器人返回步骤2执行；

(2)如果大于设定阈值，即则读取激光测距数据L^k+1和机器人当前位置姿态形成新的激光路标并进行存储，之后再返回步骤2执行；

步骤6：重复执行步骤2至5，直至创建完整的机器人运行环境电子地图。

步骤7：对上述步骤生成的全局地图中的激光路标数据进行进一步数据处理，路标点相关激光扫描数据可栅格化后形成栅格地图。

对于上述地图创建步骤中，步骤4中，激光测距数据与存储路标中测距数据匹配，现将激光传感器输出测距数据按前述公式转换至全局坐标系，再利用ICP(Iterative ClosestPoint，迭代最近点)算法完成，该算法的评估函数如下：

$E(R_{\mathrm{\α}},T)=\underset{i=\mathrm{1,2},\·\·\·N}{\min }\left(\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|(R_{\mathrm{\α}}{r}_j+T)-{r_i}^M\left|\right|}^2\right)$

式中：

E(R_α,T)为经旋转平移后，匹配点集间的距离误差最小值函数；

r_i ^M和r_j分别是激光路标数据和当前激光传感器测距数据包含的测距数据点

R_α为当前激光测距点集相对于激光路标中激光测距点集的旋转变换矩阵，α为旋转角度，而T为点集平移变换向量。

以上通过最小化E(R_α,T),得到激光扫描点与激光路标点间的旋转和平移关系，进而得到机器人相对于该路标的距离和夹角的观测值具体的ICP算法原理和计算方法可参考文献“Paul J.Bed,Neil D.McKay.A Method for Registration of 3-D Shapes,IEEETRANSACTIONS ON PARERN ANALYSIS AND MACHINE INTELLIGENCE,VOL.14,NO.2,FEBRUARY1992”。

步骤5中，扩展卡尔曼滤波计算从由k时刻估计k+1时刻系统状态所需的机器人状态估计方程为:

$\begin{array}{c}{X}_{k+1}^{M,k}=F(X_k^{M,k},u_{k,k+1},w_k)=\left[\begin{array}{c}f(X_k^k,u_k)\\ X_k^{M_0,k}\\ \·\\ \·\\ \·\\ X_k^{M_N,k}\end{array}\right]+w_k\\ =\left[\begin{array}{c}{x}_{k+1}^k\\ y_{k+1}^k\\ {\mathrm{\θ}}_{k+1}^k\\ x_{k+1}^{M_0,k}\\ y_{k+1}^{M_0,k}\\ \·\\ \·\\ \·\\ x_{k+1}^{M_N,k}\\ x_{k+1}^{M_N,k}\end{array}\right]=\left[\begin{array}{c}{x}_k^k+{\mathrm{\Δs}}_{k,k+1}\cos {\mathrm{\θ}}_k^k-{\mathrm{\Δ\θ}}_{k,k+1}[a\sin {\mathrm{\θ}}_k^k+b\cos {\mathrm{\θ}}_k^k]\\ y_k^k+{\mathrm{\Δs}}_{k.k+1}\sin {\mathrm{\θ}}_k^k+{\mathrm{\Δ\θ}}_{k,k+1}[a\cos {\mathrm{\θ}}_k^k-b\sin {\mathrm{\θ}}_k^k\\ {\mathrm{\θ}}_k^k+{\mathrm{\Δ\θ}}_{k,k+1}\\ x_k^{M_0,k}\\ y_k^{M_0,k}\\ \·\\ \·\\ \·\\ x_k^{M_N,k}\\ x_k^{M_N,k}\end{array}\right]+w_k\end{array}$

式中:

和为k时刻与k+1时刻的系统状态，系统状态信息由机器人位置姿态估计值及全局地图中存储的激光路标的位置信息的估计值组成；

F(·)为机器人状态估计函数，f(·)为航位推算函数，其中u_k,k+1包含k至k+1时刻机器人行走距离及姿态变化量Δs_k,k+1和Δθ_k,k+1；

w_k为高斯分布系统噪声，扩展卡尔曼滤波中系统误差传播协方差矩阵计算所需系统状态雅克比矩阵如下所示：

$\frac{\∂F}{{\∂X}_{k+1}^{M,k}}=\left[\begin{array}{cccccccc}1& 0& -{\mathrm{\Δs}}_{k,k+1}\sin {\mathrm{\θ}}_k^k-{\mathrm{\Δ\θ}}_{k,k+1}[a\cos {\mathrm{\θ}}_k^k-b\sin {\mathrm{\θ}}_k^k]& 0& 0& \·\·\·& 0& 0\\ 0& 1& {\mathrm{\Δs}}_{k,k+1}\cos {\mathrm{\θ}}_k^k-{\mathrm{\Δ\θ}}_{k,k+1}[a\sin {\mathrm{\θ}}_k^k+b\cos {\mathrm{\θ}}_k^k]& 0& 0& \·\·\·& 0& 0\\ 0& 0& 1& 0& 0& \·\·\·& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

$\frac{\∂F}{{\∂u}_{k,k+1}}=\left[\begin{array}{cc}\cos {\mathrm{\θ}}_k^k& -a\sin {\mathrm{\θ}}_k^k-b\cos {\mathrm{\θ}}_k^k\\ \sin {\mathrm{\θ}}_k^k& a\cos {\mathrm{\θ}}_k^k-b\sin {\mathrm{\θ}}_k^k\\ 0& 1\\ 0& 0\\ 0& 0\\ \·& \·\\ \·& \·\\ \·& \·\\ 0& 0\\ 0& 0\end{array}\right]$

进一步，扩展卡尔曼滤波计算过程中k+1时刻对系统状态中参与定位计算第i个路标的观测方程如下:

$Z_{k+1}^{M_i}=H(X_{k+1}^{M_i,k},v_{k+1})=\left[\begin{array}{c}{d}_{k+1}^{M_i}\\ {\mathrm{\α}}_{k+1}^{M_i}\end{array}\right]=\left[\begin{array}{c}\sqrt{{(x_{k+1}^{M_i,k}-x_{k+1}^k)}^2-{(y_{k+1}^{M_i,k}-y_{k+1}^k)}^2}\\ {\tan }^{-1}\left(\frac{y_{k+1}^{M_i,k}-y_{k+1}^k}{x_{k+1}^{M_i,k}-x_{k+1}^k}\right)-{\mathrm{\θ}}_{k+1}^k\end{array}\right]+v_{k+1}$

式中：

H(·)为与当前参与计算的第i个路标相关的机器人状态观测函数。

v_k+1为高斯分布观测噪声，在扩展卡尔曼滤波过程中，观测误差传播的协方差矩阵计算所需观测方程的雅克比矩阵如下：

$\frac{\∂H}{{\∂H}_{k+1}^{M_i,k}}=\left[\begin{array}{ccccccccccc}\frac{x_{k+1}^k-x_{k+1}^{M_i,k}}{r}& \frac{y_{k+1}^k-y_{k+1}^{M_i,k}}{r}& 0& 0& \·\·\·& 0& \frac{x_{k+1}^{M_i,k}-x_{k+1}^k}{r}& \frac{y_{k+1}^{M_i,k}-y_{k+1}^k}{r}& 0& \·\·\·& 0\\ \frac{y_{k+1}^{M_i,k}-y_{k+1}^k}{r^2}& \frac{x_{k+1}^k-x_{k+1}^{M_i,k}}{r^2}& -1& 0& \·\·\·& 0& \frac{y_{k+1}^k-y_{k+1}^{M_i,k}}{r^2}& \frac{x_{k+1}^{M_i,k}-x_{k+1}^k}{r^2}& 0& \·\·\·& 0\end{array}\right]$

其中： $r=\sqrt{{(x_{k+1}^{M_i,k}-x_{k+1}^k)}^2+{(y_{k+1}^{M_i,k}-y_{k+1}^k)}^2}$

具体的扩展卡尔曼滤波过程可参见相关文献，扩展卡尔曼滤波后即可得到对当前机器人位置姿态的k+1时刻的最优估计即可作为下一个地图创建计算周期机器人状态估计的输入。

步骤7中，激光栅格地图建立过程为将全局地图按照机器人导航运行过程所允许定位精度，设定矩形栅格尺寸，并将全局地图以栅格形式划分，之后将存储的路标数据投影至栅格范围内，同时规定每个栅格以1或0表示是否存在障碍物，将存在障碍物的栅格设为1，不存在障碍物的栅格设为0，从而完成全局地图从激光路标地图到激光栅格地图的转换。

基于以上构建的巡检机器人运行环境地图，定位解算模块利用基于栅格地图的粗定位与基于激光路标原始数据精细定位相结合方法，保证定位导航实时性的前提下实现巡检机器人变电站内精确定位，该方法包括以下步骤(如图4所示)：

步骤1：定位解算初始化，包含以下子步骤：

(1)设定机器人定位开始时的初始位置和姿态X₀＝[x₀,y₀,θ₀]^T；

(2)设定地图分段加载阈值以及停靠点停靠精度阈值

步骤2：k时刻查询地图存储模块数据库存储的距离机器人当前位置最近的路标M_i＝(X_i,L_i)，并载入该路标的激光测距数据以及路标相关区域的栅格地图；

步骤3：读取k至k+1时刻过程中机器人位置姿态变化量Δs_k,k+1和Δθ_k,k+1，经航位推算模块计算得到机器人全局坐标系下的位置姿态估计值

步骤4：基于栅格地图数据，结合航位推算数据和激光测距数据，利用粒子滤波算法估计机器人在全局坐标系下的位置和姿态，实现基于栅格地图的粗定位，定位精度由设定栅格尺寸决定；

步骤5：如果机器人需要在该区域内设定停靠点进行精确停靠，则机器人在进入距离停靠点设定阈值范围之后执行以下子步骤，否则执行步骤6：

(1)读取激光测距模块数据与当前所用激光路标i所包含的激光测距数据匹配，得到机器人相对于该路标的位置姿态变化量的观测值 $Z_{k+1}^{M_i}={[x_{k+1}^{M_i},y_{k+1}^{M_i},{\mathrm{\θ}}_{k+1}^{M_i}]}^T;$

(2)利用扩展卡尔曼滤波方法对航位推算模块输出的机器人位置姿态估计值与激光测距模块得到的机器人位置姿态观测值进行融合，得到机器人当前位置姿态的精确估计，从而保证机器人停靠点的停靠精度。

步骤6：计算机器人与当前路标之间的距离并与地图路标加载阈值比较之后执行以下子步骤：

(1)如果得到的距离小于等于设定的阈值，即则返回步骤3执行；

(2)如果距离大于设定阈值，即则表示机器人已经远离当前路标，需要新的路标辅助机器人定位，此时返回步骤2执行。

步骤7：重复执行步骤2至6直至整个定位任务完成。

对于上述定位解算步骤中步骤4中选择粒子滤波算法基于栅格地图进行粗定位，主要因为粒子滤波在大规模环境下表现出较高的计算效率，因此比较适用于变电站大规模环境下的机器人定位解算，从而保证机器人定位导航的实时性，算法的具体实现可参考文献“MichaelMontemerlo,Sebastian Thrun，etc.FastSLAM 2.0:An Improved Particle FilteringAlgorithm for Simultaneous Localization and Mapping that Provably Converges.In Proc.of the Int.Conf.on Artificial Intelligence,2003”。

步骤5(1)所述匹配方法与前述匹配方法一致。

步骤5(2)中精确定位用扩展卡尔曼滤波状态方程和观测方程如下：

$\begin{array}{c}{X}_{k+1}^k=f(X_k^k,u_k,w_k)\\ =\left[\begin{array}{c}{x}_{k+1}^k\\ y_{k+1}^k\\ {\mathrm{\θ}}_{k+1}^k\end{array},\right]=\left[\begin{array}{c}{x}_k^k+{\mathrm{\Δs}}_{k,k+1}\cos {\mathrm{\θ}}_k^k-{\mathrm{\Δ\θ}}_{k,k+1}[\mathrm{asi\; n}{\mathrm{\θ}}_k^k+b\cos {\mathrm{\θ}}_k^k]\\ y_k^k+{\mathrm{\Δs}}_{k,k+1}\sin {\mathrm{\θ}}_k+{\mathrm{\Δ\θ}}_{k,k+1}[\mathrm{ac\; os}{\mathrm{\θ}}_k-b\sin {\mathrm{\θ}}_k]\\ {\mathrm{\θ}}_k^k+{\mathrm{\Δ\θ}}_{k,k+1}\end{array}\right]+w_k\end{array}$

$Z_{k+1}^{M_i}=h(X_{k+1}^k,v_{k+1})=\left[\begin{array}{c}{x}_{k+1}^{M_i}\\ y_{k+1}^{M_i}\\ {\mathrm{\θ}}_{k+1}^{M_i}\end{array}\right]=\left[\begin{array}{c}{x}_{k+1}^k\\ y_{k+1}^k\\ {\mathrm{\θ}}_{k+1}^k\end{array}\right]+v_{k+1}$

式中：

和为k时刻与k+1时刻机器人位置姿态估计值；

f(·)为航位推算函数，其中u_k,k+1包含k至k+1时刻机器人行走距离及姿态变化量Δs_k,k+1和Δθ_k,k+1；

w_k和v_k+1为高斯分布系统噪声和观测噪声，扩展卡尔曼滤波中协方差矩阵计算所需系统状态和观测方程的雅克比矩阵如下所示：

$\frac{\∂f}{{\∂X}_{k+1}^k}=\left[\begin{array}{ccc}1& 0& -{\mathrm{\Δs}}_{k,k+1}\sin {\mathrm{\θ}}_k^k-{\mathrm{\Δ\θ}}_{k,k+1}[a\cos {\mathrm{\θ}}_k^k-b\sin {\mathrm{\θ}}_k^k]\\ 0& 1& {\mathrm{\Δs}}_{k,k+1}\cos {\mathrm{\θ}}_k^k-{\mathrm{\Δ\θ}}_{k,k+1}[a\sin {\mathrm{\θ}}_k^k+b\cos {\mathrm{\θ}}_k^k]\\ 0& 0& 1\end{array}\right]$

$\frac{\∂f}{{\∂u}_{k,k+1}}=\left[\begin{array}{cc}{\cos \mathrm{\θ}}_k^k& -a\sin {\mathrm{\θ}}_k^k-b\cos {\mathrm{\θ}}_k^k\\ \sin {\mathrm{\θ}}_k^k& a\cos {\mathrm{\θ}}_k^k-b\sin {\mathrm{\θ}}_k^k\\ 0& 1\end{array}\right],\frac{\∂h}{{\∂X}_{k+1}^k}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

建立上述方程后，之后可利用扩展卡尔曼滤波算法对机器人全局坐标系下位置和姿态的精确估计，进而实现机器人精确停靠。

由于变电站内巡检路径均为直线路径，设备区外机器人停靠点较少，而设备区内机器人停靠点则均在同一直线路径上，基于巡检路径这一特点，路径规划模块包含了一种基于拓扑点分类的全局路径规划方法，该方法包括以下步骤(如图5所示)：

步骤1：激光路标拓扑地图建立,包含以下子步骤：

(1)将每个激光路标抽象为有向图数据结构的拓扑节点，以节点间的相对关系建立有向图数据结构，形成激光路标拓扑地图G^L＝(V^L,A^L)，拓扑节点内存储激光路标全局坐标系下的位置，各节点间的连接关系依据地图创建过程中的路标前后关系确定。上述V^L为有向图数据结构包含的拓扑点集，假设有n个点，则而A^L则表示有向图数据结构两拓扑点之间的弧集；

(2)将生成的地图输入地图存储模块进行存储。，以上形成的有向图数据结构以临接矩阵方式进行存储，临接矩阵中A^L[i][j]表示弧的权值，若弧不存在则置A^L[i][j]为∞，其中i,j＝1,2,…,n；。

步骤2：激光路标拓扑点按路径分类，首先将巡检路线划分为首尾连接的巡检路径集合，之后将激光路标拓扑地图G^L中的路标节点按照巡检路径划分，得到每条巡检路径包含的拓扑点集合；

步骤3：建立以每条路径起点和终点对应拓扑点为节点，以整条巡检路径长度为权值的有向图数据结构G^P＝(V^P,A^P)，形成路径规划拓扑地图，并输入地图存储模块以临接矩阵方式存储，临接矩阵中A^P[i][j]表示弧的权值，若弧不存在则置A^P[i][j]为∞，假设V^P包含m个节点，则i,j＝1,2,…,m。

步骤4：当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置姿态和目标点位置姿态在，并在寻找激光路标拓扑地图G^L中对应的拓扑点和判定这两点所属步骤2建立的每条路径包含拓扑点的集合；

步骤5：判定以上两拓扑点和是否属于同一拓扑点集，若为同一点集，则表示机器人当前位置和目标点属于同一直线路径，此时可以直接判定这两点的相对位置关系，并输出路径规划数据至运动控制器执行，若不为同一点集则执行步骤6；

步骤6：将步骤3选择的拓扑点和插入路径规划拓扑地图有向图数据结构G^P中对应路径起点和终点对应的拓扑点之间，对应拓扑点为和并以其与所属路径起点和终点距离更新其与以上两点间拓扑地图权值；

步骤7：利用路径规划算法计算机器人运行最优路径，将机器人所需运行路径拓扑点输出至运动控制器执行。

对于上述全局路径规划步骤的步骤7中路径规划方法为为Dijkstra(迪杰斯特拉)算法，具体执行步骤如下：

(1)定义S^P为已找到从机器人当前位置拓扑点出发的最短路径的终点集合，它的初始状态为空集。那么从出发到有向图上其余各顶点可能达到的最短路径长度D^P[s]的初始值为：

D^P[s]＝A^P[s][t]s,t＝1,2,…，m+2

(2)选择使得

$D^P\left[t\right]=\min \left\{D^P\right[s\left]\right|v_s^P\∈V^P-S^P\}$

就是当前求得的一条从v_s出发的最短路径的终点，之后令S^P＝S^P∪{t}，从而将拓扑点t存入最短路径终点集合；

(3)修改从出发到集合V^P-S^P上任意一个顶点可达的最短路径长度D^P[k]。如果

D^P[t]+A^P[t][k]＜D^P[k]

则修改D^P[k]为

D^P[k]＝D^P[t]+A^P[t][k]

(4)重复操作(2)(3)，由此求得从到有向图上其余各拓扑点的最短路径，之后依据目标点信息则可从S^P中选择机器人从当前点至该点最短路径所包含的拓扑点集合，从而完成巡检机器人路径规划。

图6为由本定位导航系统生成多分辨率地图的层次结构示意图，图中最底层为激光路标地图，图中十字点为激光路标位置，十字点周边的曲线表示路标中存储的该路标区域激光测距数据；中间两层分别为激光栅格地图和激光路标拓扑地图，最上层为拓扑点分类后的路径规划拓扑地图。图6中可以看出，激光路标拓扑地图包含拓扑点58个，而路径规划拓扑地图则只包含10个路径点，两者比较，经拓扑点分类后可显著降低参与路径规划计算的拓扑点数量，进而提高路径规划计算效率和对存储空间的消耗，满足机器人根据特殊巡检任务在线规划机器人行驶路径的需求。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (18)

1.变电站巡检机器人定位导航系统，其特征是，包括：激光测距模块所采集的机器人周边障碍物相对于激光传感器扫描中心的距离信息及航位推算模块所得到的当前机器人在全局坐标系下的位置姿态估计分别输出至地图创建模块与位姿解算模块，地图创建模块根据得到的信息创建表示机器人运行环境的激光路标地图与激光栅格地图并输入地图存储模块进行存储，所述位姿解算模块利用地图存储模块中的地图数据以及航位推算模块的输出数据实时解算机器人当前位置和姿态并将所得到的位姿数据接入路径规划模块与运动控制模块，路径规划模块生成机器人运行路径数据并输出至运动控制模块，运动控制模块依据机器人当前位置姿态数据及路径规划数据生成运动控制量，之后由电机控制模块执行。

2.如权利要求1所述的变电站巡检机器人定位导航系统，其特征是，所述激光测距模块，由固定安装于机器人本体上的激光测距传感器构成。

3.如权利要求1所述的变电站巡检机器人定位导航系统，其特征是，所述航位推算模块，具体用于将运动控制模块上发的机器人驱动电机轴编码器脉冲数据转换为设定计算周期内驱动轮移动距离，经机器人航位推算模型得到当前机器人在全局坐标系下的位置姿态估计。

4.如权利要求1所述的变电站巡检机器人定位导航系统，其特征是，所述地图创建模块，该模块融合激光测距数据和里程计数据，利用SLAM技术创建表示机器人运行环境的激光路标地图与激光栅格地图。

5.如权利要求1所述的变电站巡检机器人定位导航系统，其特征是，所述地图存储模块，该模块基于实时数据库实现，可实时进行地图数据的存储及查询操作，同时机器人巡检任务生成过程中也需要与该模块进行数据交互，以确定巡检任务起点和终点信息。

6.如权利要求5所述的变电站巡检机器人定位导航系统，其特征是，所述位姿解算模块，利用地图存储模块中的地图数据，以及航位推算模块的输出，利用基于栅格地图的粗定位与基于激光原始数据精细定位相结合方法，实时解算机器人当前位置和姿态。

7.如权利要求1所述的变电站巡检机器人定位导航系统，其特征是，所述路径规划模块，该模块负责生成机器人激光路表拓扑地图与全局路径规划用拓扑地图，依据巡检任务规划机器人巡检运行路线。

8.如权利要求3所述的变电站巡检机器人定位导航系统，其特征是，所述运动控制模块：可根据机器人定位模块输出机器人位姿数据及路径规划模块输出路径数据实现机器人对设定路径的跟踪，所生成的控制量由电机驱动模块执行。

9.如权利要求1所述的变电站巡检机器人定位导航系统，其特征是，所述电机驱动模块：接收运动控制模块的电机控制量，依据电机轴编码器信号闭环控制控制巡检机器人驱动轮伺服电机，实现机器人在变电站内运行。

10.应用如权利要求1-9任一所述的变电站巡检机器人定位导航系统的方法，其特征是，包括以下步骤：

步骤一：利用激光测距模块采集的机器人周边障碍物相对于激光传感器扫描中心的距离信息，利用航位推算模块得到的当前机器人在全局坐标系下的位置姿态估计，将激光测距模块及航位推算模块的结果分别输出至地图创建模块与位姿解算模块；

步骤二：地图创建模块根据得到的信息创建表示机器人运行环境的激光路标地图与激光栅格地图并输入地图存储模块进行存储；

步骤三：位姿解算模块利用地图存储模块中的地图数据以及航位推算模块的输出数据实时解算机器人当前位置和姿态并将所得到的位姿数据接入路径规划模块与运动控制模块；

步骤四：路径规划模块生成机器人激光路表拓扑地图与全局路径规划用拓扑地图，依据巡检任务规划机器人巡检运行路线，路径规划得到的路径数据输入运动控制模块执行。

11.如权利要求10所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤二中，地图创建模块创建表示机器人运行环境的激光路标地图与激光栅格地图，具体包括以下步骤：

步骤：2-1)：地图创建初始化；

步骤:2-2)：获取k至k+1时刻过程中机器人位置姿态变化量，经航位推算模块计算得到机器人全局坐标系下的位置姿态估计值；

步骤：2-3)：在k+1时刻，获取激光测距模块数据，与地图存储模块中距机器人当前位置最近的已有激光路标包含的激光测距数据匹配，得到机器人相对于该路标的距离和夹角的观测向量；

步骤：2-4)：利用扩展卡尔曼滤波方法将航位推算输出机器人位置姿态估计值与匹配得到的路标观测值融合，从而得到机器人当前位置姿态的精确估计；

步骤：2-5)：计算当前机器人位置与路标位置的距离，并与设定的相邻路标间隔距离阈值比较，再执行下子步骤：

2-5-1)如果小于等于预先设定的阈值，机器人返回步骤2-2)执行；

2-5-2)如果大于设定阈值，则读取激光测距数据和机器人当前位置姿态，形成新的激光路标并进行存储，之后再返回步骤2-2)执行；

步骤：2-6)：重复执行步骤2-2)至2-5)，直至建立完整的机器人运行环境电子地图；

步骤：2-7)：对上述步骤生成地图中的激光路标数据进行进一步数据处理，路标点相关激光扫描数据可栅格化后形成栅格地图。

12.如权利要求11所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤：2-1)：地图创建初始化，具体包括：

2-1-1)建立全局坐标系，设定地图创建起始点处机器人全局坐标系下的初始位置和姿态，设定地图中相邻路标间隔距离阈值；

2-1-2)读取起始点处激光测距模块测距数据；

2-1-3)将激光测距数据与机器人初始位置姿态数据组合后形成地图创建初始点激光路标，并输入地图存储模块进行存储。

13.如权利要求10所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤三，具体包括以下步骤：

步骤3-1)：定位解算初始化；

步骤3-2)：k时刻查询地图存储模块数据库存储的距离机器人当前位置最近的路标，并载入该路标的激光测距数据以及路标相关区域的栅格地图；

步骤3-3)：读取k至k+1时刻过程中机器人位置姿态变化量，经航位推算模块计算得到机器人全局坐标系下的位置姿态估计值；

步骤3-4)：基于栅格地图数据，结合航位推算数据和激光测距数据，利用粒子滤波算法估计机器人在全局坐标系下的位置和姿态，实现基于栅格地图的粗定位，定位精度由设定栅格尺寸决定；

步骤3-5)：如果机器人需要在该区域内设定停靠点进行精确停靠，则机器人在进入距离停靠点设定阈值范围之后执行以下子步骤，否则执行步骤3-6)；

步骤3-6)：计算机器人与当前路标之间的距离，并与地图分段加载阈值比较；

步骤3-7)：重复执行步骤3-2)至3-6)直至整个定位任务完成。

14.如权利要求13所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤3-1)中，定位解算初始化，包含以下子步骤：

3-1-1)设定机器人定位开始时的初始位置和姿态；

3-1-2)设定地图分段加载阈值以及停靠点停靠精度阈值。

15.如权利要求13所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤3-5)：机器人在进入距离停靠点设定阈值范围之后执行以下子步骤，具体为：

3-5-1)读取激光测距模块数据，与当前所用激光路标所包含的激光测距数据匹配，得到机器人相对于该路标的位置姿态变化量的观测值；

3-5-2)利用扩展卡尔曼滤波方法对航位推算模块输出的机器人位置姿态估计值与激光测距模块得到的机器人位置姿态观测值进行融合，得到机器人当前位置姿态的精确估计，从而保证机器人停靠点的停靠精度。

16.如权利要求13所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤3-6)：计算机器人与当前路标之间的距离，并与地图分段加载阈值比较，之后执行以下子步骤：

3-6-1)如果得到的距离小于等于设定的阈值，则返回步骤3-3)执行；

3-6-2)如果距离大于设定阈值，则表示机器人已经远离当前路标，需要新的路标辅助机器人定位，此时返回步骤3-2)执行。

17.如权利要求10所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤四中，具体包括以下步骤：

步骤4-1)：激光路标拓扑地图建立；

步骤4-2)：激光路标拓扑点按路径分类，首先将巡检路线划分为首尾连接的巡检路径集合，之后将激光路标拓扑地图中的路标节点按照巡检路径划分，得到每条巡检路径包含的拓扑点集合；

步骤4-3)：建立以每条路径起点和终点对应拓扑点为节点，以整条巡检路径长度为权值的有向图数据结构，形成路径规划拓扑地图，并输入地图存储模块以临接矩阵方式存储。

步骤4-4)：当需要进行最短路径规划时，依据巡检任务给定的机器人当前位置姿态和目标点位置姿态在，并在寻找激光路标拓扑地图中对应的拓扑点，判定这两点所属步骤4-2)建立的每条路径包含拓扑点的集合；

步骤4-5)：判定以上两拓扑点是否属于同一拓扑点集，若为同一点集，则表示机器人当前位置和目标点属于同一路径，此时可以直接判定这两点的相对位置关系，并输出路径规划数据至运动控制器执行，若不为同一点集则执行步骤4-6)；

步骤4-6)：将步骤4-4)选择的拓扑点插入路径规划拓扑地图相关路径起点和终点对应的拓扑点之间，并以其与所属路径起点和终点距离更新其与以上两点间拓扑地图权值；

步骤4-7)：利用路径规划算法计算机器人运行最优路径，将机器人所需运行路径拓扑点输出至运动控制器执行。

18.如权利要求17所述的变电站巡检机器人定位导航系统的方法，其特征是，所述步骤4-1)：激光路标拓扑地图建立,包含以下子步骤：

4-1-1)将每个激光路标抽象为有向图数据结构的拓扑节点，以节点间的相对关系建立有向图数据结构，形成激光路标拓扑地图；

4-1--2)将生成的地图输入地图存储模块进行存储。