一种变电站巡检机器人定位导航方法及系统与流程

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

背景技术：

变电站是改变电压等级、电力传输的枢纽，是电力系统中的重要组成部分，对其安全性有很高的要求。因此，变电站的巡检工作必不可少，高质量、高效率的巡检能很大程度地提高变电站运行的可靠性和安全性，有利于维护电力系统的正常运行和稳定。

人工巡检是变电站巡检的传统作业方式，巡检人员有时由于工作劳累、厌烦等因素，很容易造成漏检、错检。根据中国电科院的统计显示，每年由于漏检和错检造成的直接经济损失都要超过26亿元。可见，人工巡检难以保证电力系统的安全可靠。随着科技的发展，使用巡检机器人完成巡检任务，以提高变电站运行的可靠性，成为一种发展趋势。

变电站巡检机器人中定位导航模块尤为重要，定位导航是实现巡检机器人自主运行的关键，其通过传感器根据周围环境建模，并规划行车路径，沿着路径自主行驶，实现变电站巡检任务。目前，变电站巡检机器人的定位导航技术主要有：磁轨迹导航、惯性导航、激光定位导航和gps导航。磁轨迹导航应用较早，需预先在道路上铺设磁条，机器车通过感应磁条信息来寻找路径，编程简单，定位精度高，但是其初期造价高，且后期重新规划路径较为麻烦；惯性导航不适合应用在变电站这类电磁强度高的环境中，因为其主要利用电子罗盘测量地球磁场强度来规划方向路径，会产生较大偏差；激光定位导航虽然定位导航精度相对较高，但是需要设置路标辅助导航，使用环境受到约束；gps导航灵活性高，但需要接受卫星信号，会有信号失真及丢失的情况，影响定位精度。

综上，现有的变电站巡检机器人利用单一传感器实现定位导航，定位精度差，对周围环境进行地图构建的误差也较大。

技术实现要素：

基于此，有必要提供一种变电站巡检机器人定位导航方法及系统，以提高定位精度，减小对周围环境进行地图构建的误差。

为实现上述目的，本发明提供了如下方案：

一种变电站巡检机器人定位导航方法，包括：

获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据；所述位置数据是采用gps获取得到的，所述周围环境点云数据是采用激光雷达获取得到的；

计算第一位姿信息和第二位姿信息；所述第一位姿信息是依据当前时刻的位置数据计算得到的，所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的；

获取上一时刻的校正数据；

当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时，则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差，采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理，得到当前时刻的校正数据；此时，所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差；

当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时，则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差，采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理，得到当前时刻的校正数据；此时，所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差；

依据所述当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

可选的，所述依据所述当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航，具体包括：

当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时，则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航；

当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时，则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

可选的，在所述获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据之后，所述计算第一位姿信息和第二位姿信息之前，还包括：

对所述位置数据和所述周围环境点云数据进行预处理。

可选的，所述位置数据z1＝(px,py)，其中，px为变电站巡检机器人所处位置在直角坐标系下的横坐标，py为变电站巡检机器人所处位置在直角坐标系下的纵坐标；所述周围环境点云数据其中，ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置，表示变电站巡检机器人在极坐标系下的角度，表示变电站巡检机器人在极坐标系下的速度。

本发明还提供了一种变电站巡检机器人定位导航系统，所述系统包括：

第一数据获取模块，用于获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据；所述位置数据是采用gps获取得到的，所述周围环境点云数据是采用激光雷达获取得到的；

位姿计算模块，用于计算第一位姿信息和第二位姿信息；所述第一位姿信息是依据当前时刻的位置数据计算得到的，所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的；

第二数据获取模块，用于获取上一时刻的校正数据；

第一校正模块，用于当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时，则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差，采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理，得到当前时刻的校正数据；此时，所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差；

第二校正模块，用于当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时，则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差，采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理，得到当前时刻的校正数据；此时，所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差；

模型构建模块，用于依据所述当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

可选的，所述模型构建模块，具体包括：

第一构建单元，用于当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时，则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航；

第二构建单元，用于当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时，则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

可选的，所述系统还包括：

预处理模块，用于对所述位置数据和所述周围环境点云数据进行预处理。

可选的，所述位置数据z1＝(px,py)，其中，px为变电站巡检机器人所处位置在直角坐标系下的横坐标，py为变电站巡检机器人所处位置在直角坐标系下的纵坐标；所述周围环境点云数据其中，ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置，表示变电站巡检机器人在极坐标系下的角度，表示变电站巡检机器人在极坐标系下的速度。

与现有技术相比，本发明的有益效果是：

本发明提出了一种变电站巡检机器人定位导航方法及系统。该方法包括：获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据；计算第一位姿信息和第二位姿信息；获取上一时刻的校正数据；依据上一时刻的校正数据，采用卡尔曼滤波算法对当前时刻的第一位姿信息或第二位姿信息进行滤波处理，得到当前时刻的校正数据；依据当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。本发明在现有的采用激光雷达获取的点云数据进行定位的基础上，加入了gps数据，通过两组数组之间相互交替更新位姿信息的方式，使二者融合，提高了定位精度，减小了对周围环境进行地图构建的误差。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1一种变电站巡检机器人定位导航方法的流程图；

图2为本发明实施例3一种变电站巡检机器人定位导航系统的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

术语解释：

变电站巡检机器人：巡检机器人系统根据预先设定的参数，自主启动完成例行巡检任务或运维人员远程遥控机器人完成巡检任务。变电站巡检机器人的整个平台是以基于激光雷达的同步定位和地图创建技术(lidar-basedsimultancouslocalizationandmapping，slam)、导航和ros框架为基础，其中主要的硬件模块有：rplidara1激光雷达、树莓派3b+、stm32、双路电机驱动模块、减速电机和降压/稳压直流电源模块。

卡尔曼滤波：一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

实施例1：

图1为本发明实施例一种变电站巡检机器人定位导航方法的流程图。

参见图1，实施例的变电站巡检机器人定位导航方法，包括：

步骤s1：获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据。

所述位置数据是采用gps获取得到的，所述周围环境点云数据是采用激光雷达获取得到的。所述位置数据z1＝(px,py)，其中，px为变电站巡检机器人所处位置在直角坐标系下的横坐标，py为变电站巡检机器人所处位置在直角坐标系下的纵坐标；所述周围环境点云数据其中，ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置，表示变电站巡检机器人在极坐标系下的角度，表示变电站巡检机器人在极坐标系下的速度。

gps定位的原理是把卫星某一时刻的瞬时位置作为初始值，卫星将有本身信息的无线电波发送给用户终端，终端通过接收机精确测量信号从卫星到终端的时间，计算出它们的距离，卫星到接收机的交点即为接收机所在位置。本实施例中变电站巡检机器人中的gps采用at6558的atgm332d，其具有出色的定位导航功能，冷启动捕获灵敏度为-148dbm，跟踪灵敏度为-162dbm，定位精度为2.5米(cep50)，首次定位时间为32秒，能够满足系统需要。

激光雷达基于tof原理设计，根据激光发射和遇到障碍物返回经过的时间按照激光在空气中的传播速度折算得到当前位置到障碍物的距离。该原理结构简单、扫描频率高、测量距离精确度高、数据信息丰富以及受光线影响小。本实施例中变电站巡检机器人中的激光雷达采用的是由slamtec公司开发的rplidara1激光雷达，该雷达成本低，利用三角测距原理对二维平面6m半径范围内进行激光测距扫描，生成平面点云地图信息，在5.5hz的扫描频率下，测距分辨率可达当前距离值的0.2％。

步骤s2：计算第一位姿信息和第二位姿信息。

所述第一位姿信息是依据当前时刻的位置数据计算得到的，所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的。

步骤s3：获取上一时刻的校正数据。

步骤s4：当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时，则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差，采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理，得到当前时刻的校正数据。

此时，所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差。

步骤s5：当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时，则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差，采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理，得到当前时刻的校正数据。

此时，所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差。

步骤s6：依据所述当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

所述步骤s6，具体包括：

当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时，则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航；

当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时，则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

作为一种可选的实施方式，在所述步骤s1之后，所述步骤s2之前，还包括：对所述位置数据和所述周围环境点云数据进行预处理。

本实施例的变电站巡检机器人定位导航方法，采用gps和激光雷达混合导航的思路，利用卡尔曼滤波，进行融合数据，使其优势互补，提高了定位导航精度。

实施例2：

变电站巡检机器人在工作过程中，通过在运动过程中反复观测到的环境特征，重新定位自己的位姿，继而根据自身位置增量式地构建地图，便达到了同时定位和构建地图的作用。但由于此过程中有随机误差，传感器对机器车运动的判断也有不确定性，同时存在变电站的电磁干扰等外界因素的影响，使得在每次构建地图时，加到前一次地图上的增量有误差，随着迭代次数的增多，误差越来越大，从而降低了地图构建以及定位导航的精度。本实施利用卡尔曼滤波算法对迭代增加的误差进行补偿，以提高定位精度。

卡尔曼滤波是基于最小方差经过递推计算进行最优估计的滤波算法，在组合导航系统中，用卡尔曼滤波估计各种系统状态误差，再根据获得的状态误差估计值来进行反馈校正，得到导航参数的估计值，实现组合导航的目的。卡尔曼滤波算法采用递推式，只需要储存当前时刻的信息和上一时刻的状态滤波估值，就可以计算出当前误差状态的最优估计，而不需要存储之前的量测信息和状态滤波估值。

标准卡尔曼滤波只适用于线性系统，对于非线性系统的滤波方法，可采用扩展卡尔曼滤波方法。扩展卡尔曼滤波方法是将卡尔曼滤波的式子进行适当的变化，将非线性系统进行线性化处理，泰勒展开的方式只取一阶项。

下面对本实施例变电站巡检机器人定位方法的具体过程进行描述。

变电站巡检机器人采用激光雷达和gps两种传感器进行定位导航，能够充分发挥激光雷达以及gps各自的优点，即激光雷达精度高，gps较为灵活不受环境约束；又能够去除各自的缺点，即去除激光雷达需预先标识反射板，使用时对环境要求高，gps信号易受到干扰甚至丢失，从而导致误差增大，精度降低的缺点。在本实施例中，使用卡尔曼滤波进行激光雷达与gps的数据融合。卡尔曼滤波以导航系统输出参数作为状态变量，经过计算，得到导航参数的最优估计值。具体为：

1)获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据。所述位置数据是采用gps获取得到的，所述周围环境点云数据是采用激光雷达获取得到的。

所述位置数据z1＝(px,py)，其中，px为变电站巡检机器人所处位置在直角坐标系下的横坐标，py为变电站巡检机器人所处位置在直角坐标系下的纵坐标；所述周围环境点云数据其中，ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置，表示变电站巡检机器人在极坐标系下的角度，表示变电站巡检机器人在极坐标系下的速度。

2)计算第一位姿信息和第二位姿信息。所述第一位姿信息是依据当前时刻的位置数据计算得到的，所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的。具体的，第一位姿信息

第二位姿信息为

3)获取上一时刻的校正数据。

4)当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时，则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差，采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理，得到当前时刻的校正数据。

此时，所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差。具体为：

x1'＝x1+k1y1

p1'＝(1-k1h1)p1

其中，x1′表示当前时刻的环境状态滤波估计值，p1′表示当前时刻的环境估计误差协方差，其中

y1＝z1-h1x1

s1＝h1ph1^t+r1

k1＝p1h1^ts1^-1，

h1表示第二位姿信息对应的状态空间的到测量空间之间的映射，r1表示第二位姿信息对应的测量值的不确定度，k1表示第二位姿信息对应的卡尔曼增益，p1表示上一时刻的位置估计误差协方差。其中，

5)当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时，则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差，采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理，得到当前时刻的校正数据。

此时，所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差。具体为：

x2'＝x2+k2y2

p2'＝(1-k2h2j)p2，

其中，x2′表示当前时刻的位置状态滤波估计值，p2′表示当前时刻的位置估计误差协方差

y2＝z2-f(x)

其中，h2j表示第一位姿信息对应的状态空间的到测量空间之间的映射，r2表示第一位姿信息对应的测量值的不确定度，k2表示第一位姿信息对应的卡尔曼增益，p2表示上一时刻的环境估计误差协方差。

其中，状态空间到测量空间的非线性映射f(x)如下

非线性映射线性化后的jacobian矩阵hj，

测量值的不确定度

6)依据所述当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。具体的，

当所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差时，则依据所述当前时刻的环境状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

当所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差时，则依据所述当前时刻的位置状态滤波估计值构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

本实施例中，在收到第一个测量值后，对机器人状态x进行初始化。计算得到位姿信息后，预测下一位置

位姿预测状态方程：

x'＝fx

p'＝fpf+q

其中，x表示状态滤波估计值，p表示估计误差协方差，f是系统的控制函数，表示系统的改变，这里将问题简单化，只考虑线性系统。其中，

p表明了系统状态的不确定性程度，用x的协方差表示，

q表明了x'＝fx未能刻画的其他外界干扰。本实施例使用线性模型，因此，加速度变成了干扰项。x'＝fx中未衡量的额外项目为v，

其中，ax，ay分别为x轴方向的加速度，y轴方向的加速度，σ为标准差，v服从高斯分布n(0,q)。

其中，σax表示x轴方向的加速度的标准差，σay表示y轴方向的加速度的标准差。

实施例3：

本发明还提供了一种变电站巡检机器人定位导航系统，图2为本发明实施例一种变电站巡检机器人定位导航系统的结构示意图。

参见图2，实施例的变电站巡检机器人定位导航系统包括：

第一数据获取模块201，用于获取变电站巡检机器人当前时刻的位置数据和周围环境点云数据；所述位置数据是采用gps获取得到的，所述周围环境点云数据是采用激光雷达获取得到的。

所述位置数据z1＝(px,py)，其中，px为变电站巡检机器人所处位置在直角坐标系下的横坐标，py为变电站巡检机器人所处位置在直角坐标系下的纵坐标；所述周围环境点云数据其中，ρ表示变电站巡检机器人所处位置距离极坐标系原点的位置，表示变电站巡检机器人在极坐标系下的角度，表示变电站巡检机器人在极坐标系下的速度。

位姿计算模块202，用于计算第一位姿信息和第二位姿信息；所述第一位姿信息是依据当前时刻的位置数据计算得到的，所述第二位姿信息是依据当前时刻的周围环境点云数据计算得到的。

第二数据获取模块203，用于获取上一时刻的校正数据。

第一校正模块204，用于当所述上一时刻的校正数据包括上一时刻的位置状态滤波估计值和上一时刻的位置估计误差协方差时，则依据所述上一时刻的位置状态滤波估计值和所述上一时刻的位置估计误差协方差，采用卡尔曼滤波算法对所述第二位姿信息进行滤波处理，得到当前时刻的校正数据；此时，所述当前时刻的校正数据包括当前时刻的环境状态滤波估计值和当前时刻的环境估计误差协方差。

第二校正模块205，用于当所述上一时刻的校正数据包括上一时刻的环境状态滤波估计值和上一时刻的环境估计误差协方差时，则依据所述上一时刻的环境状态滤波估计值和所述上一时刻的环境估计误差协方差，采用卡尔曼滤波算法对所述第一位姿信息进行滤波处理，得到当前时刻的校正数据；此时，所述当前时刻的校正数据包括当前时刻的位置状态滤波估计值和当前时刻的位置估计误差协方差。

模型构建模块206，用于依据所述当前时刻的校正数据，构建当前时刻变电站巡检机器人所处环境的地图模型，以实现变电站巡检机器人的定位导航。

作为一种可选的实施方式，所述变电站巡检机器人定位导航系统还包括：预处理模块，用于对所述位置数据和所述周围环境点云数据进行预处理。

本实施例的变电站巡检机器人定位导航系统，采用gps和激光雷达混合导航的思路，利用卡尔曼滤波，进行融合数据，使其优势互补，提高了定位导航精度。

对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。