[发明专利]变电站巡检机器人定位导航系统及方法

权利要求书

1.变电站巡检机器人定位导航系统的方法，其特征是，包括以下步骤：

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

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

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

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

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

步骤：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)：地图创建初始化，具体包括：

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

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

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

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

步骤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-5)：机器人在进入距离停靠点设定阈值范围之后执行以下子步骤，具体为：

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

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