输电线路巡检机器人同步定位与地图构建方法及装置与流程

1.本发明属于输电线路巡检技术领域，具体涉及输电线路巡检机器人同步定位与地图构建方法及装置。


背景技术：

2.输电线路巡检就是沿着线路详细巡视线路上各设备的运行情况，及时发现设备存在的缺陷和故障，并详细记录，以便作为线路检修的依据。
3.架空输电线路传统巡检方式即人工巡检方式是巡检人员沿线逐塔巡视或者检测，必要时还要登塔巡检。这种工作方式效率低、劳动强度大，且存在设备漏检和存在巡视死角等问题。近年来，随着机器人技术的不断发展，输电线路的巡检逐渐由巡检机器人巡检代替了人工巡检。
4.目前，巡检机器人主要依靠预先输入巡线目标，然后根据gps或者北斗导航系统进行定位，实现自动化巡检。然而，由于输电线路一般比较长，部分输电线路段的导航信号并不好，机器人获取的位置信息可能偏差较大甚至会丢失实时位置信息，导致实际巡检路线出现偏差。


技术实现要素：

5.有鉴于此，本发明旨在解决现有巡检机器人在导航信号不好的输电线路段无法准确的对巡线目标进行巡检的问题。
6.为了解决上述技术问题，本发明提供以下技术方案：
7.第一方面，本发明提供了一种输电线路巡检机器人同步定位与地图构建方法，包括如下步骤：
8.根据导航信号获取第一位置信息，在预置巡检地图中生成基于第一位置信息的第一二维坐标并记录当前时刻的位姿，记为初始位姿；
9.根据第一二维坐标判断巡检机器人的当前位置与预置巡检地图中预设巡检路线的偏差距离是否大于第一预设距离阈值，若是，则继续执行后续步骤；
10.向周围环境发射测量信号，接收从周围环境中的各特征点反射回的测量数据；
11.从测量数据中获取观测数据，利用误差判定公式去除各特征点中的伪数据点，获得除杂后的特征点集；
12.以当前位置为坐标原点，对除杂后的特征点集利用栅格地图创建算法生成局部栅格地图；
13.基于融合非线性最小二乘和随机梯度下降优化算法对局部栅格地图进行优化，生成全局一致性地图；
14.基于初始位姿和观测数据生成下一时刻对位姿进行预估的第一特征向量；
15.利用扩展卡尔曼滤波器生成下一时刻对位姿进行预估的第二特征向量；
16.将第一特征向量与第二特征向量组成特征对，判断特征对的马氏距离是否满足第
二预设距离阈值，若是，则根据特征对对初始位姿进行更新，得到巡检机器人在下一时刻的预估位姿。
17.进一步的，巡检机器人预置有激光测距传感器，巡检机器人通过激光测距传感器向周围环境发射测量信号。
18.进一步的，从测量数据中获取观测数据，利用误差判定公式去除各特征点中的伪数据点，获得除杂后的特征点集具体包括：
19.从测量数据中取出连续的n帧观测数据；
20.基于n帧观测数据，利用误差判定公式去除伪数据点，以便生成除杂后的特征点集。
21.进一步的，利用误差判定公式去除伪数据点具体包括：
22.根据第一计算公式计算n帧观测数据间的有效偏差，第一计算公式如下；
[0023][0024]
式中，u
n
表示连续n帧观测数据间的有效偏差，l
n
表示激光测距传感器与特征点之间的距离，t表示激光测距传感器对周围环境扫描的时刻，a表示n帧观测数据间的关联权值，a∈(0，1)；
[0025]
根据第二计算公式判断与n帧观测数据对应的特征点是否为伪数据点，若是，则舍弃与n帧观测数据对应的特征点，第二计算公式如下：
[0026]
f(n 1)＝f(n) (1 n)2/g(u
n
)
[0027]
式中，f(n)为累积函数，用以判断n帧观测数据对应的特征点是否为伪数据点，若f(n)＞f，则认为n帧观测数据对应的特征点为伪数据点，f(n)的初值设置为0，f为预设累积阈值。
[0028]
进一步的，伪数据点还包括：
[0029]
处在运动状态的特征点和大于预设观测距离的特征点。
[0030]
第二方面，本发明提供了一种输电线路巡检机器人同步定位与地图构建装置，包括：
[0031]
位置模块，用于根据导航信号获取第一位置信息，在预置巡检地图中生成基于第一位置信息的第一二维坐标并记录当前时刻的位姿，记为初始位姿；
[0032]
巡检路线跟踪模块，用于根据第一二维坐标判断巡检机器人的当前位置与预置巡检地图中预设巡检路线的偏差距离是否大于第一预设距离阈值，若是，则进入测量模块；
[0033]
测量模块，用于向周围环境发射测量信号，接收从周围环境中的各特征点反射回的测量数据；
[0034]
地图生成模块，用于从测量数据中获取观测数据，利用误差判定公式去除各特征点中的伪数据点，获得除杂后的特征点集；
[0035]
以当前位置为坐标原点，对除杂后的特征点集利用栅格地图创建算法生成局部栅格地图；
[0036]
基于融合非线性最小二乘和随机梯度下降优化算法对局部栅格地图进行优化，生成全局一致性地图；
[0037]
位姿更新模块，用于基于初始位姿和观测数据生成下一时刻对位姿进行预估的第一特征向量；
[0038]
利用扩展卡尔曼滤波器生成下一时刻对位姿进行预估的第二特征向量；
[0039]
将第一特征向量与第二特征向量组成特征对，判断特征对的马氏距离是否满足第二预设距离阈值，若是，则根据特征对对初始位姿进行更新，得到巡检机器人在下一时刻的预估位姿。
[0040]
进一步的，测量模块为激光测距传感器。
[0041]
进一步的，地图生成模块包括：
[0042]
观测数据提取模块，用于从测量数据中取出连续的n帧观测数据；
[0043]
除杂模块，用于基于n帧观测数据，利用误差判定公式去除伪数据点，以便生成除杂后的特征点集。
[0044]
进一步的，除杂模块具体包括：
[0045]
第一计算模块，用于根据第一计算公式计算n帧观测数据间的有效偏差，第一计算公式如下；
[0046][0047]
式中，u
n
表示连续n帧观测数据间的有效偏差，l
n
表示激光测距传感器与特征点之间的距离，t表示激光测距传感器对周围环境扫描的时刻，a表示n帧观测数据间的关联权值，a∈(0，1)；
[0048]
第二计算模块，用于根据第二计算公式判断与n帧观测数据对应的特征点是否为伪数据点，若是，则舍弃与n帧观测数据对应的特征点，第二计算公式如下：
[0049]
f(n 1)＝f(n) (1 n)2/g(u
n
)
[0050]
式中，f(n)为累积函数，用以判断n帧观测数据对应的特征点是否为伪数据点，若f(n)＞f，则认为n帧观测数据对应的特征点为伪数据点，f(n)的初值设置为0，f为预设累积阈值。
[0051]
第三方面，本发明提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如第一方面的输电线路巡检机器人同步定位与地图构建方法。
[0052]
综上，本发明提供了一种输电线路巡检机器人同步定位与地图构建方法及装置，通过在导航信号不好的区域进行地图构建，即通过向周围环境发射测量信号，去除掉反射回来的特征点中的伪数据点之后，利用栅格地图生成算法先生成局部栅格地图，然后对该地图进行进一步优化，从而完成地图构建，然后再分别根据观测数据和扩展卡尔曼滤波器生成两组对位姿进行预估的特征向量，然后当两组向量之间的相似性较高时，则根据该特征向量的参数对初始位置进行更新，该方法可以使巡检机器人即使接收不到导航信号或者导航信号的误差较大时，也能够自主完成对于巡线目标的巡检。本发明解决了现有巡检机器人在导航信号不好的输电线路段无法准确的对巡线目标进行巡检的问题。
附图说明
[0053]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。
[0054]
图1为本发明实施例提供的输电线路巡检机器人同步定位与地图构建方法的流程示意图。
具体实施方式
[0055]
为使得本发明的目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0056]
请参阅图1，本实施例提供了一种输电线路巡检机器人同步定位与地图构建方法，包括如下步骤：
[0057]
s100：根据导航信号获取第一位置信息，在预置巡检地图中生成基于第一位置信息的第一二维坐标并记录当前时刻的位姿，记为初始位姿。
[0058]
可以理解的是，导航信号可以由设置在巡检机器人上的导航模块通过gps或北斗导航系统获取。预置巡检地图为预先输入在巡检机器人中用于巡检的地图，并且同时为巡检机器人设置了预设巡检路线，巡检机器人沿着预设巡检路线在预置巡检地图中巡检即可完成对于巡线目标的自动巡检。
[0059]
s200：根据第一二维坐标判断巡检机器人的当前位置与预置巡检地图中预设巡检路线的偏差距离是否大于第一预设距离阈值，若是，则继续执行后续步骤。
[0060]
需要说明的是，当导航信号较差时，可能会引导巡检机器人偏离巡检路线从而产生偏差距离，另外还可能是导航信号产生了较大误差，但是根据导航信号显示仍然在预设巡检路线上，而巡检机器人的实际位置却已经产生了偏差。由于导航信号较差产生的误差是不断变化的，故此时巡检机器人可以根据预设巡检路线返回一段距离，同时不断获取导航信号发送的位置，根据位置变化规律判断此时导航信号是否较差，进一步根据该导航信号提供的位置信息判断是否偏离了路线。
[0061]
s300：向周围环境发射测量信号，接收从周围环境中的各特征点反射回的测量数据。
[0062]
需要说明的是，巡检机器人内置了激光测距传感器，采用激光测距传感器对巡检器机人的四周环境进行扫描。接收的测量数据包括扫描角度以及该角度对应的反射点(即特征点)的距离。
[0063]
s400：从测量数据中获取观测数据，利用误差判定公式去除各特征点中的伪数据点，获得除杂后的特征点集。
[0064]
需要说明的是，伪数据点包括噪声点、误测点以及动态障碍物生成的数据点。本实施例中s400具体包括从测量数据中取出连续的n帧观测数据；基于n帧观测数据，利用误差
判定公式去除伪数据点，以便生成除杂后的特征点集。本实施例中取n＝3，该数值有利提高运算速度，有利于降低减少运算器的占用度。
[0065]
另外，由于特征点集为包括了周围环境中的所有特征点，故需要判断进行除杂时输入的特征点集与全部特征点集之间的欧拉距离，若该欧拉距离小于预设阈值时，则根据巡检机器人位置更新输入点集，否则就利用误差判定公式去除伪数据点。具体步骤如下：
[0066]
1)确定输入点集q，计算输入点集q与特征点集p之间的欧拉距离e，计算公式如下：
[0067][0068]
式中，k
i
为输入点i相对应的参考点，x,y为点的坐标。
[0069]
2)更新输入点集q
[0070]
q
i
'＝t {r(q
i
‑
c) c}
[0071]
其中t为齐次坐标更换变换的平移向量，r为齐次坐标变更的旋转向量，c为巡检机器人(也即发射测量信号的激光传感器)的位置(r的旋转中心)，默认c为零。
[0072]
3)当欧拉距离e大于预设阈值e(阈值e根据激光传感器的性能设定)时，则根据第一计算公式计算n帧观测数据间的有效偏差，第一计算公式如下；
[0073][0074]
式中，u
n
表示连续n帧观测数据间的有效偏差，l
n
表示激光测距传感器与特征点之间的距离，t表示激光测距传感器对周围环境扫描的时刻，a表示n帧观测数据间的关联权值，a∈(0，1)；
[0075]
根据第二计算公式判断与n帧观测数据对应的特征点是否为伪数据点，若是，则舍弃与n帧观测数据对应的特征点，第二计算公式如下：
[0076]
f(n 1)＝f(n) (1 n)2/g(u
n
)
[0077]
式中，f(n)为累积函数，用以判断n帧观测数据对应的特征点是否为伪数据点，为高斯平滑误差函数且g(n)～n(0,1)分布，若f(n)＞f，则认为n帧观测数据对应的特征点为伪数据点，f(n)的初值设置为0，f为预设累积阈值。
[0078]
s500：以当前位置为坐标原点，对除杂后的特征点集利用栅格地图创建算法生成局部栅格地图。
[0079]
可以理解的是，通过栅格地图创建算法将外部环境分割成n个相互连接的单元格，通过计算每个扫描角度的反射点与传感器之间的距离来判断对应的单元格是否被占用。
[0080]
在本实施例中，可以采用基于rbpf的栅格地图创建算法，具体如下：
[0081]
s501：根据巡检机器人所处的环境，对周围环境采样，并预测第i个粒子的位姿且表示的为粒子k
‑
1时刻的位姿，u
k
‑1为经过更新后的里程计数据，操作符为位姿合成运算符,粒子用于对巡检机器人的位姿进行预测。
[0082]
s502：根据地图和预测位姿执行上述步骤s400中的除杂过程。
[0083]
s503：对位姿估计值在一定的区域随机采样，采样生成的点通过储存在巡检机
器人控制器中的程序计算，算出该粒子对应建议分布的均值和方差。
[0084]
s504：依据步骤s503中计算出的建议分布的高斯估计来更新k时刻第i个粒子的位姿以及权重
[0085]
s505：此时根据更新后的位姿k时刻的观测数据o
k
更新第i个粒子的地图m
‘
(i)
。在粒子迭代更新中由于粒子贫乏的影响，算法执行重采样步骤，重采样是通过粒子的有效数目有选择性的执行，通过选用权重较大的粒子进行重采样，权重较大的粒子更加能够代表机器人的真实位姿，最后相应的地图即被设为输出地图。
[0086]
s600：基于融合非线性最小二乘和随机梯度下降优化算法对局部栅格地图进行优化，生成全局一致性地图。
[0087]
需要说明的是，优化算法的具体步骤如下：
[0088]
s601：初始化状态空间数目num x＝0,当巡检机器人进入到新的局部栅格地图中时，即获得新的相对转化结构x
new
＝(t
new
∑
new
),则令num x＝1 num x。
[0089]
s602：更新估计m矩阵，通过局部栅格地图中每一个特征点位姿的相对置信度来构成对角矩阵，以对m矩阵进行估计。
[0090]
s603：根据梯度优化算法计算梯度下降的方向向量δx,判定||x||的值是否小于给定的约束误差值a，当满足时迭代结束，否则继续向下执行。
[0091]
s604：计算学习速率λ，通过公式λ＝λ/(1 λ),对学习效率进行迭代，实验中λ的初值设置为λ＝1/3,得到新的状态空间变量x
i
,并令x
i
＝x
i
‑1 δx。
[0092]
s605：判定相邻时刻局部地图间的限制代价f(x)的差值是否满足给定的误差b，满足则迭代结束，不满足则继续迭代。
[0093]
需要说明的是，限制代价f(x)＝(jδx
‑
r)
t
σ
‑1(jδx
‑
r)，即f(x)是以雅克比矩阵之间的约束给出，参数r为限制代价误差，δx为寻找的最小代价(即与真实状态估计之间的偏差)，σ限制代价误差的方差，则σ
‑1为对角信息矩阵约束。
[0094]
s606：迭代次数加1，转到步骤s602中，不断进行新的迭代求解，直到限制代价满足误差要求，则结束优化算法，迭代结束。
[0095]
s700：基于初始位姿和观测数据生成下一时刻对位姿进行预估的第一特征向量。
[0096]
需要说明的是，基于初始位姿和观测数据生成下一时刻对位姿进行预估的第一特征向量即是巡检机器人建立观测数据模型，根据观测的数据对位姿进行预估，生成对应的第一特征向量，具体步骤为现有熟知技术，在此不再赘述。
[0097]
s800：利用扩展卡尔曼滤波器生成下一时刻对位姿进行预估的第二特征向量。
[0098]
可以理解的是，利用扩展卡尔曼滤波器对位姿进行预估，生成对应的第二特征向量也可以采用现有熟知技术，在此不再赘述。
[0099]
s900：将第一特征向量与第二特征向量组成特征对，判断特征对的马氏距离是否满足第二预设距离阈值，若是，则根据特征对对初始位姿进行更新，得到巡检机器人在下一时刻的预估位姿。
[0100]
需要说明的是，步骤s700和步骤s800分别采用两种方法对位姿进行预估，分别生成两个对应的特征向量，通过马氏距离准则判断两特征向量之间的相似度即可选出最优匹配的特征对，最终根据相应的特征向量对巡检机器人的位姿进行更新。
[0101]
进一步的，在本实施例中，还可以在待需检的输电线路上设定导航地标模块，并将这些导航地标模块的轮廓信息和坐标信息储存到巡检机器人的数据库中，这些导航地标模块用于机器人定位过程中对自身位姿误差进行调整消除。导航地标模块的形状为具有同心结构的环状/矩形图案的标牌，导航地标模块的材质为光洁度和反射性良好的玻璃。
[0102]
本实施例提供了一种输电线路巡检机器人同步定位与地图构建方法，通过在导航信号不好的区域进行地图构建，即通过向周围环境发射测量信号，去除掉反射回来的特征点中的伪数据点之后，利用栅格地图生成算法先生成局部栅格地图，然后对该地图进行进一步优化，从而完成地图构建，然后再分别根据观测数据和扩展卡尔曼滤波器生成两组对位姿进行预估的特征向量，然后当两组向量之间的相似性较高时，则根据该特征向量的参数对初始位置进行更新，该方法可以使巡检机器人即使接收不到导航信号或者导航信号的误差较大时，也能够自主完成对于巡线目标的巡检。
[0103]
以上是对本发明提供的一种输电线路巡检机器人同步定位与地图构建方法的实施例进行详细的描述，以下将对本发明提供的一种输电线路巡检机器人同步定位与地图构建装置的实施例进行详细的描述。
[0104]
本实施例提供了一种输电线路巡检机器人同步定位与地图构建装置，包括：
[0105]
位置模块，用于根据导航信号获取第一位置信息，在预置巡检地图中生成基于第一位置信息的第一二维坐标并记录当前时刻的位姿，记为初始位姿。
[0106]
巡检路线跟踪模块，用于根据第一二维坐标判断巡检机器人的当前位置与预置巡检地图中预设巡检路线的偏差距离是否大于第一预设距离阈值，若是，则进入测量模块。
[0107]
测量模块，用于向周围环境发射测量信号，接收从周围环境中的各特征点反射回的测量数据。
[0108]
需要说明的是，测量模块为激光测距传感器。
[0109]
地图生成模块，用于从测量数据中获取观测数据，利用误差判定公式去除各特征点中的伪数据点，获得除杂后的特征点集；以当前位置为坐标原点，对除杂后的特征点集利用栅格地图创建算法生成局部栅格地图；基于融合非线性最小二乘和随机梯度下降优化算法对局部栅格地图进行优化，生成全局一致性地图。
[0110]
需要说明的是，地图生成模块具体包括：
[0111]
观测数据提取模块，用于从测量数据中取出连续的n帧观测数据；
[0112]
除杂模块，用于基于n帧观测数据，利用误差判定公式去除伪数据点，以便生成除杂后的特征点集。
[0113]
进一步的，除杂模块具体包括：
[0114]
第一计算模块，用于根据第一计算公式计算n帧观测数据间的有效偏差，第一计算公式如下；
[0115][0116]
式中，u
n
表示连续n帧观测数据间的有效偏差，l
n
表示激光测距传感器与特征点之间的距离，t表示激光测距传感器对周围环境扫描的时刻，a表示n帧观测数据间的关联权值，a∈(0，1)；
[0117]
第二计算模块，用于根据第二计算公式判断与n帧观测数据对应的特征点是否为
伪数据点，若是，则舍弃与n帧观测数据对应的特征点，第二计算公式如下：
[0118]
f(n 1)＝f(n) (1 n)2/g(u
n
)
[0119]
式中，f(n)为累积函数，用以判断n帧观测数据对应的特征点是否为伪数据点，u表示连续数据有效偏差的阈值，若f(n)＞f，则认为n帧观测数据对应的特征点为伪数据点，f(n)的初值设置为0，f为预设累积阈值。
[0120]
位姿更新模块，用于基于初始位姿和观测数据生成下一时刻对位姿进行预估的第一特征向量；利用扩展卡尔曼滤波器生成下一时刻对位姿进行预估的第二特征向量；将第一特征向量与第二特征向量组成特征对，判断特征对的马氏距离是否满足第二预设距离阈值，若是，则根据特征对对初始位姿进行更新，得到巡检机器人在下一时刻的预估位姿。
[0121]
本实施例提供了一种输电线路巡检机器人同步定位与地图构建装置，本装置能够使得巡检机器人在导航信号不好的区域进行地图构建，即通过向周围环境发射测量信号，去除掉反射回来的特征点中的伪数据点之后，利用栅格地图生成算法先生成局部栅格地图，然后对该地图进行进一步优化，从而完成地图构建，然后再分别根据观测数据和扩展卡尔曼滤波器生成两组对位姿进行预估的特征向量，然后当两组向量之间的相似性较高时，则根据该特征向量的参数对初始位置进行更新，该方法可以使巡检机器人即使接收不到导航信号或者导航信号的误差较大时，也能够自主完成对于巡线目标的巡检。
[0122]
以上是对本发明提供的一种输电线路巡检机器人同步定位与地图构建装置的实施例进行详细的描述，以下将对本发明提供的一种计算机可读存储介质的实施例进行详细的描述。
[0123]
本实施例提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如前述实施例所述的输电线路巡检机器人同步定位与地图构建方法。
[0124]
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。