一种适用于变电站巡检机器人的路径规划方法与流程

1.本发明涉及电力自动化技术领域，具体的说是一种适用于变电站巡检机器人的路径规划方法。


背景技术：

2.近年来，随着社会经济和自动化技术的不断发展，国内用电需求不断增加，导致对变电站巡视检修的工作量和工作难度不断上升；变电站巡检机器人可以代替人工完成巡检任务，且可以24小时全天候不间断的工作；而路径规划作为变电站巡检机器人的关键技术，需要在线的规划巡检机器人从起始点到目标点间的路径。
3.沿墙路径规划算法优势在于实现简单且可保证算法的全局收敛性；现有的沿墙算法存在只能适用于特定环境，如只包含墙体障碍的环境，或者是不包含动态障碍物的静态场景，并不能完全适用于变电站这样的未知复杂环境；borenstein提出仅利用当前目标方向角与设定阈值比较实现状态切换的方法，容易使得机器人过早的结束沿墙运动，在目标的引力作用下重新陷入极小，且只能应用在只包含静态障碍的环境中；kim提出在机器人与目标距离变小时切换状态，该方法容易过早的结束沿墙运动，且无法针对动态环境。


技术实现要素：

4.针对现有技术中存在的上述不足之处，本发明要解决的技术问题是提供一种适用于变电站巡检机器人的路径规划方法，解决沿墙算法只适用于静态场景的问题。
5.本发明为实现上述目的所采用的技术方案是：一种适用于变电站巡检机器人的路径规划方法，所述机器人设定有两种运行状态：人工势场状态和沿墙避障状态；所述方法为：当所述机器人陷入局部最优位置时，执行沿墙避障状态；当所述机器人未陷入局部最优位置时，执行人工势场状态，在所述人工势场状态下，所述机器人利用改进的人工势场模型进行路径规划；所述机器人在人工势场状态和沿墙避障状态之间切换若干次，直至到达目标位置。
6.所述改进的人工势场模型为：
[0007][0008][0009]vor
＝(v
obs-v)
·eor
[0010]
其中，x是所述机器人当前位置，xg为运动目标位置，ρ是所述机器人到障碍的最短距离，v为所述机器人的速度，vg表示运动目标的速度，v
obs
表示运动障碍物的速度，v
g-v表示运动目标与机器人之间的相对速度，v
obs-v表示运动障碍物与机器人之间的相对速度，k
att
、k
rep
、ρ0均为常系数；e
or
表示从障碍物到机器人的单位向量，v
or
是向量(v
obs-v)与单位向量eor
的内积，即机器人与障碍物在机器人与障碍物之间的直线上的相对速度的投影，u
att
(x)为引力场势能，u
rep
(x)为斥力场势能，m、m_v、k
att_v
、k
rep_v
、n以及n_v均为常系数。
[0011]
所述机器人利用改进的人工势场模型进行路径规划，具体为：
[0012]
机器人所受引力和排斥场的负梯度如下：
[0013][0014][0015][0016]fsum
＝f
att
+f
rep
[0017]
其中，f
att
是目标点对机器人的引力，f
rep
是障碍对机器人的斥力，f
sum
是引力和斥力的矢量和，即机器人所受的合力；f
rep1
为从障碍物距机器人最近的点指向机器人方向的斥力分量，f
rep2
为从机器人指向目标点方向的斥力分量；e
rg
为机器人到目标的单位矢量，e
gr_v
表示机器人到目标相对速度方向的单位矢量；e
or_v
表示障碍物到机器人相对速度方向的单位矢量；k
att
、k
rep
、ρ0均为常系数，x是所述机器人当前位置，xg为运动目标位置，ρ是所述机器人到障碍的最短距离，e
or
表示从障碍物到机器人的单位向量，f
rep_v
表示相对速度给予机器人的额外斥力，m、m_v、k
att_v
、m_v-1、n、n_v以及n_v-1均为常系数；
[0018]
机器人沿着所述负梯度方向运动。
[0019]
所述人工势场避障状态的结束条件为：
[0020]fsum
≈0
[0021]
其中，f
sum
为所述机器人所受的合力。
[0022]
所述沿墙避障状态的结束条件为：
[0023]dlast
＞d
now
[0024]
其中，垂直投影距离d表示路径点与目标的欧氏距离在所述机器人于局部最优位置与运动目标连线上的投影——分为d
last
和d
now
，d
last
为上一个时刻与目标点间的垂直投影距离，d
now
为当前路径点与目标点间的垂直投影距离。
[0025]
本发明具有以下优点及有益效果：
[0026]
1.本发明提出了双层程序结构，定义了势场避障状态和沿墙避障状态两种避障状态；其次，综合考虑了运动障碍以及运动目标的相对位置、相对速度对机器人的影响，改进了人工势场模型，使得移动机器人始终与运动障碍保持相悖的运动趋势，与运动目标保持相同运动趋势。
[0027]
2.仿真以及实际试验结果表明，本发明提出的方法可以在动态场景中完成路径规划，将沿墙算法应用场景扩展到动态场景，具有较好的应用价值。
附图说明
[0028]
图1为本发明的实施例中状态间相互切换关系的流程图；
[0029]
图2为本发明的实施例中单位向量关系图；
[0030]
图3为本发明的实施例中动态场景路径规划图。
具体实施方式
[0031]
下面结合附图及实施例对本发明做进一步的详细说明。
[0032]
为解决沿墙算法只适用于静态场景的问题，本发明首先提出了双层程序结构，定义了势场避障状态(人工势场状态)和沿墙避障状态两种避障状态，状态间的关系如图1所示；双层程序结构直接面向系统的底层，拥有较好的实时性和可靠性；除此之外，便于调整、增加与删除程序结构。
[0033]
本发明的路径规划方法采用多状态程序结构以应对环境中不同的障碍，包含两种状态即人工势场状态和沿墙避障状态。人工势场状态主要针对的是空旷环境(周围没有障碍)或者是只存在柱形障碍的环境，而沿墙避障状态主要针对的是墙体障碍环境。当机器人遇到的场景较为简单时，机器人采用人工势场状态进行路径规划。在人工势场状态下，机器人利用改进的人工势场模型进行路径规划。
[0034]
综合考虑了运动障碍以及运动目标的相对位置、相对速度对机器人的影响，改进了人工势场模型，使得移动机器人始终与运动障碍保持相悖的运动趋势，与运动目标保持相同运动趋势。
[0035]
动态环境的势场模型如下：
[0036][0037][0038]vor
＝(v
obs-v)
·eor
[0039]
其中v为机器人的速度，vg表示运动目标的速度，v
obs
表示运动障碍物的速度，v
g-v表示运动目标与机器人之间的相对速度，v
obs-v表示运动障碍物与机器人之间的相对速度；e
or
表示从障碍物到机器人的单位向量，v
or
是向量(v
obs-v)与单位向量e
or
的内积(机器人与障碍物在机器人与障碍物之间的直线上的相对速度的投影)；对于v
or
＞0，机器人向障碍物移动，因此需要避开障碍物；而对于v
or
≤0，机器人则需要远离障碍物。
[0040]
改进的人工势场模型进行路径规划：在叠加势场的作用下，机器人只能沿着叠加势场的负梯度方向运动，运动过程中受到最终目标点的引力作用而靠近，同时又受到周围障碍的斥力作用而躲避，进而形成起始点与目标点之间的有效路径。机器人所受引力和排斥场的负梯度如下：
[0041][0042]
[0043][0044]fsum
＝f
att
+f
rep
[0045]
其中e
rg
为机器人到目标的单位矢量，e
gr_v
表示机器人到目标相对速度方向的单位矢量；由于f
att
包含e
gr_v
可以保持与目标相同的速度，因此它可以用于跟踪动态目标；e
or_v
表示障碍物到机器人相对速度方向的单位矢量；f
rep_v
加强障碍物对机器人的排斥作用；排斥力与其三个分量之间的关系如图2所示；其中t是移动的目标；作用在机器人上的合力f
sum
是吸引力和排斥力的加和。
[0046]
由于规划环境的多样性，导致机器人在某些位置斥引力大小相等方向相反，合力矢量无法为机器人提供下一步运动的方向和驱动力，导致无法完成路径规划。
[0047]
因此，需要判断机器人在规路径划的途中是否陷入局部极小，当满足切换条件时，即认定机器人陷入局部极小，需要切换为沿墙状态以帮助机器人脱离局部极小位置；在沿墙状态下机器通过沿着障碍物的边缘移动的方式，绕过障碍物并逃离局部最小。
[0048]
双层程序结构的状态以及转换条件如下：
[0049]
人工势场状态的结束条件为：
[0050]fsum
≈0
[0051]
结束条件成立意味着合力大小接近于0，这可用于判断机器人是否已经陷入局部极小。当机器人不受合力影响时，可判定机器人陷入局部最优位置；由于机器人不再受到合力的作用，因此无法判断下一时刻的前进方向和前进角度；
[0052]
沿墙避障状态的结束条件为：
[0053]dlast
＞d
now
[0054]
其中，垂直投影距离d表示路径点与目标的欧氏距离在小车于局部最优解处与目标连线上的投影——可分为d
last
和d
now
，d
last
为上一个时刻与目标点间的垂直投影距离，d
now
为当前路径点与目标点间的垂直投影距离；该结束条件描述了当机器人与目标的垂直距离小于上一时刻时，方可结束沿墙模式。
[0055]
在满足沿墙避障状态的结束条件时，切换回人工势场状态。两种状态循环往复，直至机器人到达目标位置。
[0056]
实施例1：算法实现。将上述机器人受力模型转换为实际环境下移动机器人的速度和前进方向，以实现在实际环境下机器人的路径规划。
[0057]
人工势场状态：
[0058]
每个采样时刻，机器人线速度，角速度和前进的期望方向如下：
[0059][0060]
ω＝ω
max
(θ
res-θ0)
[0061]
θ
exp
＝θ
res
[0062]
其中：v
max
是机器人最大线速度，ω
max
是机器人最大偏转增益常数，f
rep
是障碍物给予机器人的斥力的大小，θ
rep-θ0是斥力方向与机器人前进方向的夹角，θ
res-θ0是合力方向与机器人前进方向的夹角，θ
exp
是机器人期望运动方向，θ
rep
为斥力方向，θ
res
为合力方向。
[0063]
沿墙避障状态：
[0064]
每个采样时刻，机器人线速度，角速度和前进的期望方向如下：
[0065]
v＝v
max
(1-|cos(θ
rep-θ0)|)
[0066]
ω＝ω
max
(θ
res-θ0)+ωd(d
0-ds)
[0067]
θ
exp
＝θ
rep-α
[0068]
其中：d
0-ds是机器人与墙体的实测距离和预设机器人离墙距离的距离差，修正值ωd(d
0-ds)可以让机器人与墙体之间的距离保持在预设值，α为偏转角(理论值为90
°
)；当以速度v靠近障碍并进入沿墙避障状态后，机器人以期望运动方向θ
exp
为参照，沿墙行走并调整自身姿态与墙体平行。
[0069]
实施例2：环境实例
[0070]
图3(a)(b)都是包含动态障碍物以及动态目标的场景，图中带箭头的线是目标轨迹。在图3(a)场景中巡检机器人最大移动速度为v
max
＝0.5m/s，目标最大运动速度为v
obs
＝0.4m/s，巡检机器人从s点开始移动，在依靠本发明所提出的改进的人工势场模型下避开场景中的静态和动态障碍物，并成功追赶上目标，完成了从s到t的动态路径规划；而在图3(b)场景中巡检机器人最大移动速度为v
max
＝0.5m/s，目标最大运动速度为v
obs
＝0.6m/s，巡检机器人从s点出发，首先碰到了墙体障碍，因此巡检机器人先将状态切换成沿墙避障状态，在成功绕开墙体障碍后，巡检机器人继续移动跟踪目标，并将自身状态切换为人工势场避障状态；依靠本文发明的动态避障方法最终到达目标点。
[0071]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

技术特征：
1.一种适用于变电站巡检机器人的路径规划方法，其特征在于，所述机器人设定有两种运行状态：人工势场状态和沿墙避障状态；所述方法为：当所述机器人陷入局部最优位置时，执行沿墙避障状态；当所述机器人未陷入局部最优位置时，执行人工势场状态，在所述人工势场状态下，所述机器人利用改进的人工势场模型进行路径规划；所述机器人在人工势场状态和沿墙避障状态之间切换若干次，直至到达目标位置。2.根据权利要求1所述的一种适用于变电站巡检机器人的路径规划方法，其特征在于，所述改进的人工势场模型为：所述改进的人工势场模型为：v
or
＝(v
obs-v)
·
e
or
其中，x是所述机器人当前位置，x
g
为运动目标位置，ρ是所述机器人到障碍的最短距离，v为所述机器人的速度，v
g
表示运动目标的速度，v
obs
表示运动障碍物的速度，v
g-v表示运动目标与机器人之间的相对速度，v
obs-v表示运动障碍物与机器人之间的相对速度，k
att
、k
rep
、ρ0均为常系数；e
or
表示从障碍物到机器人的单位向量，v
or
是向量(v
obs-v)与单位向量e
or
的内积，即机器人与障碍物在机器人与障碍物之间的直线上的相对速度的投影，u
att
(x)为引力场势能，u
rep
(x)为斥力场势能，m、m_v、k
att_v
、k
rep_v
、n以及n_v均为常系数。3.根据权利要求1所述的一种适用于变电站巡检机器人的路径规划方法，其特征在于，所述机器人利用改进的人工势场模型进行路径规划，具体为：机器人所受引力和排斥场的负梯度如下：机器人所受引力和排斥场的负梯度如下：机器人所受引力和排斥场的负梯度如下：f
sum
＝f
att
+f
rep
其中，f
att
是目标点对机器人的引力，f
rep
是障碍对机器人的斥力，f
sum
是引力和斥力的矢量和，即机器人所受的合力；f
rep1
为从障碍物距机器人最近的点指向机器人方向的斥力分量，f
rep2
为从机器人指向目标点方向的斥力分量；e
rg
为机器人到目标的单位矢量，e
gr_v
表示机器人到目标相对速度方向的单位矢量；e
or_v
表示障碍物到机器人相对速度方向的单位矢量；k
att
、k
rep
、ρ0均为常系数，是所述机器人当前位置，x
g
为运动目标位置，ρ是所述机器人
到障碍的最短距离，e
or
表示从障碍物到机器人的单位向量，f
rep_v
表示相对速度给予机器人的额外斥力，m、m_v、k
att_v
、m_v-1、n、n_v以及n_v-1均为常系数；机器人沿着所述负梯度方向运动。4.根据权利要求1所述的一种适用于变电站巡检机器人的路径规划方法，其特征在于，所述人工势场避障状态的结束条件为：f
sum
≈0其中，f
sum
为所述机器人所受的合力。5.根据权利要求1所述的一种适用于变电站巡检机器人的路径规划方法，其特征在于，所述沿墙避障状态的结束条件为：d
last
＞d
now
其中，垂直投影距离d表示路径点与目标的欧氏距离在所述机器人于局部最优位置与运动目标连线上的投影——分为d
last
和d
now
，d
last
为上一个时刻与目标点间的垂直投影距离，d
now
为当前路径点与目标点间的垂直投影距离。

技术总结
本发明公开了一种适用于变电站巡检机器人的路径规划方法，所述机器人设定两种运行状态，包括人工势场状态和沿墙避障状态；当机器人陷入局部最优解时，选择沿墙避障状态；当机器人未陷入局部最优解时，选择人工势场状态，机器人利用改进的人工势场进行路径规划；改进后的人工势场模型综合考虑了运动障碍以及运动目标的相对位置、相对速度对机器人的影响，使得移动机器人始终与运动障碍保持相悖的运动趋势，与运动目标保持相同运动趋势；所提出的本发明提出的方法可以在动态场景中完成路径规划，将沿墙算法应用场景扩展到动态场景，在变电站巡检机器人具有较好的应用价值。在变电站巡检机器人具有较好的应用价值。在变电站巡检机器人具有较好的应用价值。


技术研发人员：徐胜
受保护的技术使用者：南京国电南自电网自动化有限公司
技术研发日：2023.03.29
技术公布日：2023/7/7