一种基于粒子滤波的变电站巡检机器人导航定位方法与流程

1.本发明涉及一种适用于变电站巡检机器人的导航定位方法。


背景技术：

2.长期以来，变电站设备的巡航工作依靠人工检查，存在劳动强度大、工作效率低、检测 手段单一等缺点，而且在检测完毕之后也无法将人工检测的数据第一时间传递到管理信息系 统。而目前推广的无人值守变电站同样需要设备维护和检修，而随着机器人技术的发展，使 用移动机器人对变电站设备进行巡视检修成为可能。变电站巡航机器在一定程度可以实现变 电站的智能巡检，这样不仅提高了巡检效率，同时也有效的降低了巡检人员的作业安全风险。
3.目前变电站智能巡检机器人主要针对变电站的室外设备，变电站的室内电力设备一方面 需要有足够的可靠性，另一方面也需要及时检测以便发现并检修存在潜在隐患的设备，确保 电力系统正常运行。室内设备的实时检测对保证其可靠安全运行有重要意义。采用组合轨道 的机器人系统可方便灵活地对室内设备进行实时检测，了解设备的运行状态，将设备运行状 态及时反馈。
4.而在变电站巡航机器人的巡航过程中，最关键的要素是建立局部环境地图，根据环境地 图和周围环境数据相结合，最终实现自动巡航功能。要想建立局部环境地图，对巡航机器人 的定位必须要精准，定位技术是建立局部环境地图技术的基础所在。
5.就目前而言，所有存在的定位方法都是在已知环境中，放置已知的障碍物，之后给定障 碍物的坐标，规定好机器人的巡航路线，通过障碍物坐标和实时巡航路线的坐标差值来确定 巡航机器人的位置。常用的方法为卡尔曼滤波，计算量很大，而且误差必须服从高斯分布， 所以，国内外的机器人巡航定位算法都是关于粒子滤波的，而使用粒子滤波会导致粒子贫乏 问题，粒子贫乏问题是指在粒子滤波的算法中，随着粒子的不断迭代，很多粒子权重会变小 甚至消失，这样会出现粒子重叠的现象，所以使用较少的粒子解决粒子贫乏问题有重要的意 义。


技术实现要素：

6.本发明针对现有变电站巡检机器人巡航定位算法的不足，提出一种适用于变电站巡检机 器人的高精度定位方法。
7.本发明的实现步骤包括：
8.1.1初始化变电站巡检机器人工作条件，包括巡检机器人运动路径、运动初始坐标与终点 坐标、障碍物的位置坐标，通过对巡检机器人车轮光电传感器在一定时间δt内的转动次数n和 光电传感器分辨率p，由公式d＝2πrn/p确定巡航机器人在δt时间内的移动
距离d，巡航机 器人的左右车轮移动距离为d1，d2，由公式可以确定巡航机器人的移动增量δd 以及转角增量δθ，其中r是巡航机器人车轮半径，l为车轮间距，由公式可知巡航机 器人的运动半径r，由以上公式即可求得巡航机器人在下一时刻的位置姿态：
[0009][0010]
上式中的x
t
，y
t
，θ
t
代表巡航机器人在球坐标系下的横坐标，纵坐标以及角度坐标，n(t) 是粒子滤波必须服从的外界高斯分布，如车轮与地面的摩擦等因素。
[0011]
1.2对障碍物坐标进行估计，估计公式为障碍物红外传感器 位于巡航机器人前端，(x
m
，y
m
)为障碍物坐标，(x
s
，y
s
)为传感器的中心坐标，l为传感器和 巡航机器人几何中心的距离，该值可由得到，障碍物的坐标计算公式为： 即可求得ρ
t
为障碍物位置和机器人中心位 置的距离，为巡航机器人传感器检测障碍物时的角度。
[0012]
1.3将巡航机器人在t时刻所处的位置视为一个粒子集合，由步骤(1)中巡航机器人下 一时刻位置计算公式(i)可以对t 1时刻的粒子集合进行更新，并使用融合增强粒子群算 法对巡航机器人所处位置进行优化，确定粒子集的权值w。
[0013]
1.4由步骤(2)巡航机器人位置估计方程以及步骤(3)中巡航机器人位置优化值，使用 kalman滤波算法对机器人位置实际值和预测值进行预测，最终完成机器人的准确定位。
[0014]
进一步的，步骤1.3中粒子集合的权值迭代更新公式为t为当前时刻。
[0015]
进一步的，当上一步骤确定的w
t 1
权值比步骤1.3中预设值小时，系统将返回至步骤1.1 重新开始确定位置，否则巡航机器人定位结束。
[0016]
本发明与现有技术相比具有以下优点
[0017]
本发明使用改进的多粒子群优化算法对由巡航机器人位置构成的粒子群集合进行优化， 并将优化值和观测实际值进行比较，使用kalman滤波算法对其进行融合，对建立
的粒子权 重进行不停的迭代更新，有效的解决了局部粒子退化问题，使巡航机器人定位更加精确，这 对机器人后续自动巡航路线的建立有指导性的意义。
附图说明
[0018]
图1是本发明的实现流程图
具体实施方式
[0019]
下面结合附图对本发明做进一步描述。
[0020]
参照图1，本发明的具体实施步骤如下。
[0021]
步骤1.初始化变电站巡检机器人工作条件，包括巡检机器人运动路径、运动初始坐标与 终点坐标、障碍物的位置坐标，通过对巡检机器人车轮光电传感器在一定时间δt内的转动次 数n和光电传感器分辨率p，由公式d＝2πrn/p确定巡航机器人在δt时间内的移动距离d， 巡航机器人的左右车轮移动距离为d1，d2，由公式可以确定巡航机器人的移 动增量δd以及转角增量δθ，其中r是巡航机器人车轮半径，l为车轮间距，由公式可 知巡航机器人的运动半径r，由以上公式即可求得巡航机器人在下一时刻的位置姿态：
[0022][0023]
上式中的x
t
，y
t
，θ
t
代表巡航机器人在球坐标系下的横坐标，纵坐标以及角度坐标，n(t) 是粒子滤波必须服从的外界高斯分布，如车轮与地面的摩擦等因素。
[0024]
步骤2.对障碍物坐标进行估计，估计公式为障碍物红外传 感器位于巡航机器人前端，(x
m
，y
m
)为障碍物坐标，(x
s
，y
s
)为传感器的中心坐标，l为传感 器和巡航机器人几何中心的距离，该值可由得到，障碍物的坐标计算公 式为：即可求得ρ
t
为障碍物位置和机器人 中心位置的距离，为巡航机器人传感器检测障碍物时的角度。
[0025]
步骤3.将巡航机器人在t时刻所处的位置视为一个粒子集合，由步骤(1)中巡航机
器 人下一时刻位置计算公式(i)可以对t 1时刻的粒子集合进行更新，并使用融合增强粒子 群算法对巡航机器人所处位置进行优化，确定粒子集的权值w。
[0026]
步骤4.由步骤(2)巡航机器人位置估计方程以及步骤(3)中巡航机器人位置优化值， 使用kalman滤波算法对机器人位置实际值和预测值进行预测，最终完成机器人的准确定位。
[0027]
进一步的，步骤1.3中粒子集合的权值迭代更新公式为t为当前时刻。
[0028]
进一步的，当上一步骤确定的w
t 1
权值比步骤1.3中预设值小时，系统将返回至步骤1.1 重新开始确定位置，否则巡航机器人定位结束。
[0029]
实施效果
[0030]
通过实际测试，本发明巡航机器人在建立自动导航路线时，可以更精确的沿着导航路线 移动，若有偏离，能够自动纠正，返回到预设轨道上来，具有较强的实用意义。
[0031]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制， 其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应 为等效的置换方式，都包含在本发明的保护范围之内。

技术特征：
1.一种基于粒子滤波的变电站巡检机器人导航定位方法，其特征步骤在于：(1)初始化变电站巡检机器人工作条件，包括巡检机器人运动路径、运动初始坐标与终点坐标、障碍物的位置坐标，通过对巡检机器人车轮光电传感器在一定时间δt内的转动次数n和光电传感器分辨率p，由公式d＝2πrn/p确定巡航机器人在δt时间内的移动距离d，巡航机器人的左右车轮移动距离为d1，d2，由公式可以确定巡航机器人的移动增量δd以及转角增量δθ，其中r是巡航机器人车轮半径，l为车轮间距，由公式可知巡航机器人的运动半径r，由以上公式即可求得巡航机器人在下一时刻的位置姿态：上式中的x
t
，y
t
，θ
t
代表巡航机器人在球坐标系下的横坐标，纵坐标以及角度坐标，n(t)是粒子滤波必须服从的外界高斯分布，如车轮与地面的摩擦等因素。(2)对障碍物坐标进行估计，估计公式为障碍物红外传感器位于巡航机器人前端，(x
m
，y
m
)为障碍物坐标，(x
s
，y
s
)为传感器的中心坐标，l为传感器和巡航机器人几何中心的距离，该值可由得到，障碍物的坐标计算公式为：即可求得ρ
t
为障碍物位置和机器人中心位置的距离，为巡航机器人传感器检测障碍物时的角度。(3)将巡航机器人在t时刻所处的位置视为一个粒子集合，由步骤(1)中巡航机器人下一时刻位置计算公式(i)可以对t 1时刻的粒子集合进行更新，并使用融合增强粒子群算法对巡航机器人所处位置进行优化，确定粒子集的权值w。(4)由步骤(2)巡航机器人位置估计方程以及步骤(3)中巡航机器人位置优化值，使用kalman滤波算法对机器人位置实际值和预测值进行预测，最终完成机器人的准确定位。2.根据权利要求1所述的一种基于粒子滤波的变电站巡检机器人导航定位方法，其特征在于：所述步骤(3)中粒子集合的权值迭代更新公式为t为当前时刻。3.根据权利要求1、2所述的一种基于粒子滤波的变电站巡检机器人导航定位方法，其特征在于：当确定的w
t 1
权值比步骤(3)小时，系统将返回至步骤(1)重新开始确定位置，否
则巡航机器人定位结束。
技术总结
本发明公开了一种基于粒子滤波的变电站巡检机器人导航定位方法，针对现有变电站巡检机器人巡航定位算法的不足，提出一种适用于变电站巡检机器人的高精度定位方法。具体为使用改进的多粒子群优化算法对由巡航机器人位置构成的粒子群集合进行优化，并将优化值和观测实际值进行比较，使用Kalman滤波算法对其进行融合，对建立的粒子权重进行不停的迭代更新，有效的解决了局部粒子退化问题，使巡航机器人定位更加精确，这对机器人后续自动巡航路线的建立有指导性的意义。而在实际测试中，巡航机器人在建立自动导航路线时，可以更精确的沿着导航路线移动，若有偏离，能够自动纠正，返回到预设轨道上来，具有较强的实用意义。具有较强的实用意义。


技术研发人员：王喆 张桓闻 薛倩楠 夏震 瑚成健 高国梁 张维军 成昱嘉 豆河伟 李昱伟 刘全龙 白洁 高伟 申佳 宋贝 傅亦甲 亓婷 叶通 杨小军 栾剑钊 蒋浩 韩阳 张涛 刘云 张锋 刘磊 王梦琳 乔琦琦 杨拯 李东海 孟凯
受保护的技术使用者：国网陕西省电力公司榆林供电公司
技术研发日：2019.12.30
技术公布日：2021/7/15