一种基于动态规划的角钢塔攀爬机器人三维路径规划方法
1.本发明涉及电力作业攀爬机器人技术领域，尤其是一种基于动态规划的角钢塔攀爬机器人三维路径规划方法。
背景技术：
2.随着电力市场化进程不断深入，电力攀爬机器人的研制成为一大热点。目前，输电线路角钢塔的维修检测方式仍然是人工检测，这种方法费时费力，甚至威胁到电力工人的生命安全。国内现有的针对输电线路角钢塔攀爬机器人的研究成果较少，且存在结构复杂、稳定性差，自主路径规划算法低效等问题，其中，角钢塔攀爬机器人路径规划算法成为一大研究难点。
3.针对这一问题，本专利提出一种适用于角钢塔攀爬机器人的最优路径规划方法，专利首先针对角钢塔自身结构特点抽象出角钢塔二维有向加权图，利用动态规划算法提高了算法的规划速度。同时提出一种针对角钢塔攀爬机器人的步态规划方法，在确保抓取可靠性以及可重复性的情况下，提高攀爬机器人的攀爬效率。
技术实现要素：
4.为了克服上述现有技术中的缺陷，本发明提供一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，针对角钢塔自身结构特点抽象出角钢塔二维有向加权图，利用动态规划算法提高路径规划速度。
5.为实现上述目的，本发明采用以下技术方案，包括：
6.一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，选取角钢塔上的特征点构建二维有向加权图，确定二维有向加权图中特征点之间的代价，构建二维有向加权图的代价矩阵；利用二维有向加权图和对应的代价矩阵进行路径规划。
7.具体包括以下步骤：
8.s1，根据角钢塔三维模型，选取角钢塔上的节点板构造二维有向加权图，角钢塔上存在连接关系的节点板在二维有向加权图中也相连接，根据节点板之间的连接关系以及节点板的位置，构建代价矩阵w＝(w
i,j
)q×q，q表示角钢塔中的节点板总数量；w
i,j
表示从节点板i到节点板j的代价；下标i、j表示节点板，i＝1,2,3
…
q，j＝1,2,3
…
q，i≠j；
9.代价矩阵w＝(w
i,j
)q×q的构建方式，具体如下所示：
10.若在二维有向加权图中，节点板i与节点板j相连接，则w
i,j
＝l
i,j
，l
i,j
表示节点板i与节点板j之间的欧式距离；
11.若在二维有向加权图中，节点板i与节点板j不相连接，则w
i,j
＝inf，inf表示无穷大；
12.s2，利用二维有向加权图和对应的代价矩阵，对起点节点板e与终点节点板f之间的路径进行路径规划。
13.优选的，步骤s2中，使用动态规划方法对起点节点板e与终点节点板f之间的路径
进行最小路径规划，具体如下所示：
14.假设起点节点板e与终点节点板f之间的路径共需要z个阶段完成，每个阶段均有对应的路径点，形成从起点节点板e到终点节点板f的最小路径；其中，第1个阶段的路径点t1为起点节点板e，即t1＝e，第z阶段的路径点tz为终点节点板f，即tz＝f，
15.s201，进行正向搜索，确定每个阶段的阶段点：
16.从第1阶段的阶段点s1开始，到第z阶段的阶段点sz结束，先依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；
17.第k阶段的最小累计成本值u1(sk)为：
18.u1(sk)＝min(u1(s
k-1
)+d(s
k-1
,sk))
19.其中，u1(s
k-1
)表示第k-1阶段的最小累计成本值；s
k-1
、sk分别为第k-1、k阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e，第z阶段的阶段点sz即为第z个阶段的路径点tz即终点节点板f；
20.s202，进行反向搜索，确定每个阶段的路径点：
21.若进行第k-1阶段与第k阶段之间的反向搜索，则根据二维有向加权图，得到从第k-1阶段到第k阶段的所有连接路径，并结合第k-1阶段的最小累计成本值u1(s
k-1
)，得到第k阶段的前向最优节点，第k阶段的前向最优节点即为第k-1阶段的路径点t
k-1
：
22.t
k-1
＝argmin(u1(s
k-1
)+d(set_s
k-1
,sk))；
23.其中，set_s
k-1
表示与第k阶段的阶段点sk相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u1(s
k-1
)表示阶段s
k-1
的累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；t
k-1
表示第k-1阶段的路径点；
24.其中，k-1≥1，k≤z；
25.s203，得到从起点节点板e到终点节点板f的最小路径t
1,z
为：
26.t
1,z
＝{t1,t2,
…
t
z-1
,tz}
27.得到从起点节点板e到终点节点板f的最小路径代价u
min
(t
1,z
)为：
[0028][0029]
优选的，步骤s2中，使用双向动态规划方法对起点节点板e与终点节点板f之间的路径进行最优规划，得到起点节点板e与终点节点板f之间的最优路径，具体如下所示：
[0030]
假设起点节点板e与终点节点板f之间的路径共需要z个阶段完成，每个阶段均有对应的路径点，形成从起点节点板e到终点节点板f的最小路径；其中，其中，第1个阶段的路径点t1为起点节点板e，即t1＝e，第z阶段的路径点tz为终点节点板f，即tz＝f，
[0031]
所述双向动态规划方法包括顺向动态规划和逆向动态规划；其中，所述顺向动态规划是指：从第1阶段的路径点t1即起点节点板e开始，寻到第z个阶段的路径点tz即终点节点板f结束的最小路径；所述逆向动态规划是指：从第z阶段的路径点tz即终点节点板f开始，寻到第1个阶段的路径点t1即起点节点板e结束的最小路径；
[0032]
双向动态规划的具体过程如下所示：
[0033]
s21，进行顺向动态规划：
[0034]
s211，进行正向搜索，确定每个阶段的阶段点：
[0035]
从第1阶段的阶段点s1开始，到第z阶段的阶段点sz结束，先依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；
[0036]
第k阶段的最小累计成本值u1(sk)为：
[0037]
u1(sk)＝min(u1(s
k-1
)+d(s
k-1
,sk))
[0038]
其中，u1(s
k-1
)表示第k-1阶段的最小累计成本值；s
k-1
、sk分别为第k-1、k阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e，第z阶段的阶段点sz即为第z个阶段的路径点tz即终点节点板f；
[0039]
s212，进行反向搜索，确定每个阶段的路径点：
[0040]
若进行第k-1阶段到第k阶段的反向搜索，则根据二维有向加权图，得到从第k-1阶段到第k阶段的所有连接路径，并结合第k-1阶段的最小累计成本值u1(s
k-1
)，得到第k阶段的前向最优节点，第k阶段的前向最优节点即为第k-1阶段的路径点t
k-1
：
[0041]
t
k-1
＝argmin(u1(s
k-1
)+d(set_s
k-1
,sk))；
[0042]
其中，set_s
k-1
表示与第k阶段的阶段点sk相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u1(s
k-1
)表示阶段s
k-1
的累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；t
k-1
表示第k-1阶段的路径点；
[0043]
s213，得到从第z阶段到第k-1阶段的最小路径t
z,k-1
为：
[0044]
t
z,k-1
＝{tz,...,t
k-1
}；
[0045]
其中，路径是指由各个阶段的路径点构成的集合；
[0046]
从第z阶段到第k-1阶段的最小路径代价u
min
(t
z,k-1
)为：
[0047][0048]
其中，k-1≥1，k≤z；
[0049]
s22，与步骤s21的顺向动态规划同步进行逆向动态规划：
[0050]
s221，进行正向搜索，确定每个阶段的阶段点：
[0051]
从第z阶段的阶段点sz开始，到第1阶段的阶段点s1结束，依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；
[0052]
第u-1阶段的最小累计成本值u2(s
u-1
)为：
[0053]
u2(s
u-1
)＝min(u2(su)+d(su,s
u-1
))
[0054]
其中，u2(su)表示第u阶段的最小累计成本值；su、s
u-1
分别为第u、u-1阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；第z阶段的阶段点sz即为第z个阶段的路径点tz即终点节点板f，从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e；
[0055]
s222，进行反向搜索，确定每个阶段的路径点：
[0056]
若进行第u阶段到第u-1阶段的反向搜索，则根据二维有向加权图，得到从第u阶段到第u-1阶段的所有连接路径，并结合第u阶段的最小累计成本值，得到第u-1阶段的前向最优节点，第u-1阶段的前向最优节点即为第u阶段的路径点tu：
[0057]
tu＝argmin(u2(su)+d(set_su,s
u-1
))；
[0058]
其中，set_su表示与第u-1阶段的阶段点s
u-1
相连的前向节点集合；d(
·
)函数用于
计算两点之间的欧式距离；u2(su)表示第u阶段的最小累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；tu表示第u阶段的路径点；
[0059]
s223，得到从第1阶段到第u阶段的最小路径t
1,u
为：
[0060]
t
1,u
＝{t1,...,tu}；
[0061]
从第1阶段到第u阶段的路径代价u
min
(t
1,u
)为：
[0062][0063]
其中，u≤z，u-1≥1；
[0064]
s23，若从第z阶段到第k-1阶段的最小路径t
z,k-1
与从第1阶段到第u阶段的最小路径t
1,u
中，存在t
k-1
＝tu，则表示顺向动态规划和逆向动态规划到同一路径点，则停止顺向动态规划和逆向动态规划，且得到起点节点板e与终点节点板f之间的最优路径为t
z,k-1
+t
1,u
；否则，继续同步进行顺向动态规划和逆向动态规划；
[0065]
s24，按照步骤s21和s22的方式继续同步进行顺向动态规划和逆向动态规划，并按照步骤s23的方式判断顺向动态规划和逆向动态规划是否到同一路径点，直至到同一路径点，停止顺向动态规划和逆向动态规划，得到节点板e与终点节点板f之间的最优路径；
[0066]
若顺向动态规划和逆向动态规结束后，始终无法到同一路径点，则表示无最优路径，双向动态规划结束。
[0067]
优选的，步骤s1中，二维有向加权图中，根据节点板的高度对角钢塔进行层划分，同一高度的节板点位于同一个层中，角钢塔上相邻的两个层中存在连接关系的节点板在二维有向加权图中相连接，角钢塔上同一层中存在连接关系的节点板在二维有向加权图中相连接。
[0068]
本发明的优点在于：
[0069]
(1)本发明提出一种适用于角钢塔攀爬机器人的最小路径规划方法，首先针对角钢塔自身结构特点抽象出角钢塔的二维有向加权图，基于二维有向加权图利用动态规划算法进行最小路径规划，提高路径规划速度。
[0070]
(2)针对规划空间较大、状态总数增多、规划耗时变长的问题，本发明利用双向动态规划算法把搜索空间重新规划为两个对称区域，在保证全局最优的同时，也提高了最优路径的规划速度，提高攀爬机器人的攀爬效率。
[0071]
(3)本发明依据角钢塔自身结构信息高效完成攀爬机器人路径规划，避免负载相关仪器设备，增加角钢塔攀爬机器人续航能力减少能耗。
附图说明
[0072]
图1为本发明涉及的三维路径规划方法流程图。
[0073]
图2为本实施例利用角钢塔上的节点板所构造的二维有向加权图。
[0074]
图3为本实施例的双向动态规划过程图。
具体实施方式
[0075]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他
实施例，都属于本发明保护的范围。
[0076]
本实施例所涉及的角钢塔模型型号为：变xjj146-1d2sz1-24米。
[0077]
由图1所示，本发明的一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，包括以下步骤：
[0078]
s1，对角钢塔模型进行离散化，针对角钢塔三维模型，利用角钢塔上的节点板构造二维有向加权图，如图2所示，二维有向加权图中，根据节点板的高度对角钢塔进行层划分，划分为m层，同一高度的节板点位于同一个层中，角钢塔上相邻的两个层中存在连接关系的节点板在二维有向加权图中相连接，角钢塔上同一层中存在连接关系的节点板在二维有向加权图中相连接；
[0079]
选中节点板上最下方的一个螺栓的位置点代表该节点板在空间中的位置，根据节点板之间的连接关系以及节点板的位置，构建代价矩阵w＝(w
i,j
)q×q，q表示角钢塔中的节点板总数量；w
i,j
表示从节点板i到节点板j的代价；下表i、j表示节点板，i＝1,2,3
…
q，j＝1,2,3
…
q，i≠j；
[0080]
代价矩阵w＝(w
i,j
)q×q的构建方式，具体如下所示：
[0081]
若在二维有向加权图中，节点板i与节点板j相连接，则w
i,j
＝l
i,j
，l
i,j
表示节点板i的位置与节点板j的位置之间的欧式距离；
[0082]
若在二维有向加权图中，节点板i与节点板j不相连接，则w
i,j
＝inf，inf表示无穷大；
[0083]
本实施例中，按上述方式建立图2的代价矩阵w＝(w
i,j
)
33
×
33
为：
[0084][0085]
s2，使用双向动态规划方法对起点节点板e与终点节点板f之间的路径进行最优规划，得到起点节点板e与终点节点板f之间的最优路径：
[0086]
假设起点节点板e与终点节点板f之间的路径共需要z个阶段完成，其中，第1个阶段的路径点t1为起点节点板e，即t1＝e，第z阶段的路径点tz为终点节点板f，即tz＝f，
[0087]
由图3所示，双向动态规划方法包括顺向动态规划和逆向动态规划；
[0088]
所述顺向动态规划是指：从第1阶段的路径点t1即起点节点板e开始，寻到第z个阶段的路径点tz即终点节点板f结束的最小路径；
[0089]
所述逆向动态规划是指：从第z阶段的路径点tz即终点节点板f开始，寻到第1个阶段的路径点t1即起点节点板e结束的最小路径；
[0090]
所述路径是指由各个阶段的路径点构成的集合。
[0091]
步骤s2中，最优路径的双向动态规划过程，具体如下所示：
[0092]
s21，进行顺向动态规划：
[0093]
s211，进行正向搜索，确定每个阶段的阶段点：
[0094]
从第1阶段的阶段点s1开始，到第z阶段的阶段点sz结束，先依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；
[0095]
第k阶段的最小累计成本值u1(sk)为：
[0096]
u1(sk)＝min(u1(s
k-1
)+d(s
k-1
,sk))
[0097]
其中，u1(s
k-1
)表示第k-1阶段的最小累计成本值；s
k-1
、sk分别为第k-1、k阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e，第z阶段的阶段点sz即为第z个阶段的路径点tz即终点节点板f；
[0098]
s212，进行反向搜索，确定每个阶段的路径点：
[0099]
若进行第k-1阶段到第k阶段的反向搜索，则根据二维有向加权图，得到从第k-1阶段到第k阶段的所有连接路径，并结合第k-1阶段的最小累计成本值u1(s
k-1
)，得到第k阶段的前向最优节点，第k阶段的前向最优节点即为第k-1阶段的路径点t
k-1
：
[0100]
t
k-1
＝argmin(u1(s
k-1
)+d(set_s
k-1
,sk))；
[0101]
其中，set_s
k-1
表示与第k阶段的阶段点sk相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u1(s
k-1
)表示阶段s
k-1
的累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；t
k-1
表示第k-1阶段的路径点；
[0102]
s213，得到从第z阶段到第k-1阶段的最小路径t
z,k-1
为：
[0103]
t
z,k-1
＝{tz,...,t
k-1
}；
[0104]
其中，路径是指由各个阶段的路径点构成的集合；
[0105]
从第z阶段到第k-1阶段的最小路径代价u
min
(t
z,k-1
)为：
[0106][0107]
其中，k-1≥1，k≤z；
[0108]
s22，与步骤s21的顺向动态规划同步进行逆向动态规划：
[0109]
s221，进行正向搜索，确定每个阶段的阶段点：
[0110]
从第z阶段的阶段点sz开始，到第1阶段的阶段点s1结束，依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；
[0111]
第u-1阶段的最小累计成本值u2(s
u-1
)为：
[0112]
u2(s
u-1
)＝min(u2(su)+d(su,s
u-1
))
[0113]
其中，u2(su)表示第u阶段的最小累计成本值；su、s
u-1
分别为第u、u-1阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；第z阶段的阶段点sz即为第z个阶段的路径点tz即终点节点板f，从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e；
[0114]
s222，进行反向搜索，确定每个阶段的路径点：
[0115]
若进行第u阶段到第u-1阶段的反向搜索，则根据二维有向加权图，得到从第u阶段到第u-1阶段的所有连接路径，并结合第u阶段的最小累计成本值，得到第u-1阶段的前向最优节点，第u-1阶段的前向最优节点即为第u阶段的路径点tu：
[0116]
tu＝argmin(u2(su)+d(set_su,s
u-1
))；
[0117]
其中，set_su表示与第u-1阶段的阶段点s
u-1
相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u2(su)表示第u阶段的最小累计成本值；argmin函数用于获取使
得目标函数取最小值时的变量值；tu表示第u阶段的路径点；
[0118]
s223，得到从第1阶段到第u阶段的最小路径t
1u
为：
[0119]
t
1,u
＝{t1,...,tu}；
[0120]
从第1阶段到第u阶段的路径代价u
min
(t
1,u
)为：
[0121][0122]
其中，u≤z，u-1≥1；
[0123]
s23，若从第z阶段到第k-1阶段的最小路径t
z,k-1
与从第1阶段到第u阶段的最小路径t
1,u
中，存在t
k-1
＝tu，则表示顺向动态规划和逆向动态规划到同一路径点，则停止顺向动态规划和逆向动态规划，且得到起点节点板e与终点节点板f之间的最优路径为t
z,k-1
+t
1,u
；否则，继续同步进行顺向动态规划和逆向动态规划；
[0124]
s24，按照步骤s21和s22的方式继续同步进行顺向动态规划和逆向动态规划，并按照步骤s23的方式判断顺向动态规划和逆向动态规划是否到同一路径点，直至到同一路径点，停止顺向动态规划和逆向动态规划，得到节点板e与终点节点板f之间的最优路径；
[0125]
若顺向动态规划和逆向动态规结束后，始终无法到同一路径点，则表示无最优路径，双向动态规划结束。
[0126]
除非特别说明，本发明文本中的以前后顺序出现的各个流程并不必然存在先后的执行顺序。尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
技术特征：
1.一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，其特征在于，选取角钢塔上的特征点构建二维有向加权图，确定二维有向加权图中特征点之间的代价，构建二维有向加权图的代价矩阵；利用二维有向加权图和对应的代价矩阵进行路径规划。2.根据权利要求1所述一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，其特征在于，具体包括以下步骤：s1，根据角钢塔三维模型，选取角钢塔上的节点板构造二维有向加权图，角钢塔上存在连接关系的节点板在二维有向加权图中也相连接，根据节点板之间的连接关系以及节点板的位置，构建代价矩阵w＝(w
i,j
)
q
×
q
，q表示角钢塔中的节点板总数量；w
i,j
表示从节点板i到节点板j的代价；下标i、j表示节点板，i＝1,2,3
…
q，j＝1,2,3
…
q，i≠j；代价矩阵w＝(w
i,j
)
q
×
q
的构建方式，具体如下所示：若在二维有向加权图中，节点板i与节点板j相连接，则w
i,j
＝l
i,j
，l
i,j
表示节点板i与节点板j之间的欧式距离；若在二维有向加权图中，节点板i与节点板j不相连接，则w
i,j
＝inf，inf表示无穷大；s2，利用二维有向加权图和对应的代价矩阵，对起点节点板e与终点节点板f之间的路径进行路径规划。3.根据权利要求2所述一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，其特征在于，步骤s2中，使用动态规划方法对起点节点板e与终点节点板f之间的路径进行最小路径规划，具体如下所示：假设起点节点板e与终点节点板f之间的路径共需要z个阶段完成，每个阶段均有对应的路径点，形成从起点节点板e到终点节点板f的最小路径；其中，第1个阶段的路径点t1为起点节点板e，即t1＝e，第z阶段的路径点t
z
为终点节点板f，即t
z
＝f，s201，进行正向搜索，确定每个阶段的阶段点：从第1阶段的阶段点s1开始，到第z阶段的阶段点s
z
结束，先依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；第k阶段的最小累计成本值u1(s
k
)为：u1(s
k
)＝min(u1(s
k-1
)+d(s
k-1
,s
k
))其中，u1(s
k-1
)表示第k-1阶段的最小累计成本值；s
k-1
、s
k
分别为第k-1、k阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e，第z阶段的阶段点s
z
即为第z个阶段的路径点t
z
即终点节点板f；s202，进行反向搜索，确定每个阶段的路径点：若进行第k-1阶段与第k阶段之间的反向搜索，则根据二维有向加权图，得到从第k-1阶段到第k阶段的所有连接路径，并结合第k-1阶段的最小累计成本值u1(s
k-1
)，得到第k阶段的前向最优节点，第k阶段的前向最优节点即为第k-1阶段的路径点t
k-1
：t
k-1
＝argmin(u1(s
k-1
)+d(set_s
k-1
,s
k
))；其中，set_s
k-1
表示与第k阶段的阶段点s
k
相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u1(s
k-1
)表示阶段s
k-1
的累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；t
k-1
表示第k-1阶段的路径点；
其中，k-1≥1，k≤z；s203，得到从起点节点板e到终点节点板f的最小路径t
1,z
为：t
1,z
＝{t1,t2,
…
t
z-1
,t
z
}得到从起点节点板e到终点节点板f的最小路径代价u
min
(t
1,z
)为：4.根据权利要求2所述的一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，其特征在于，步骤s2中，使用双向动态规划方法对起点节点板e与终点节点板f之间的路径进行最优规划，得到起点节点板e与终点节点板f之间的最优路径，具体如下所示：假设起点节点板e与终点节点板f之间的路径共需要z个阶段完成，每个阶段均有对应的路径点，形成从起点节点板e到终点节点板f的最小路径；其中，其中，第1个阶段的路径点t1为起点节点板e，即t1＝e，第z阶段的路径点t
z
为终点节点板f，即t
z
＝f，所述双向动态规划方法包括顺向动态规划和逆向动态规划；其中，所述顺向动态规划是指：从第1阶段的路径点t1即起点节点板e开始，寻到第z个阶段的路径点t
z
即终点节点板f结束的最小路径；所述逆向动态规划是指：从第z阶段的路径点t
z
即终点节点板f开始，寻到第1个阶段的路径点t1即起点节点板e结束的最小路径；双向动态规划的具体过程如下所示：s21，进行顺向动态规划：s211，进行正向搜索，确定每个阶段的阶段点：从第1阶段的阶段点s1开始，到第z阶段的阶段点s
z
结束，先依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；第k阶段的最小累计成本值u1(s
k
)为：u1(s
k
)＝min(u1(s
k-1
)+d(s
k-1
,s
k
))其中，u1(s
k-1
)表示第k-1阶段的最小累计成本值；s
k-1
、s
k
分别为第k-1、k阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e，第z阶段的阶段点s
z
即为第z个阶段的路径点t
z
即终点节点板f；s212，进行反向搜索，确定每个阶段的路径点：若进行第k-1阶段到第k阶段的反向搜索，则根据二维有向加权图，得到从第k-1阶段到第k阶段的所有连接路径，并结合第k-1阶段的最小累计成本值u1(s
k-1
)，得到第k阶段的前向最优节点，第k阶段的前向最优节点即为第k-1阶段的路径点t
k-1
：t
k-1
＝argmin(u1(s
k-1
)+d(set_s
k-1
,s
k
))；其中，set_s
k-1
表示与第k阶段的阶段点s
k
相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u1(s
k-1
)表示阶段s
k-1
的累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；t
k-1
表示第k-1阶段的路径点；s213，得到从第z阶段到第k-1阶段的最小路径t
z,k-1
为：t
z,k-1
＝{t
z
,...,t
k-1
}；其中，路径是指由各个阶段的路径点构成的集合；
从第z阶段到第k-1阶段的最小路径代价u
min
(t
z,k-1
)为：其中，k-1≥1，k≤z；s22，与步骤s21的顺向动态规划同步进行逆向动态规划：s221，进行正向搜索，确定每个阶段的阶段点：从第z阶段的阶段点s
z
开始，到第1阶段的阶段点s1结束，依次寻下一阶段中与上一阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点，并记录该下一阶段的最小累计成本；第u-1阶段的最小累计成本值u2(s
u-1
)为：u2(s
u-1
)＝min(u2(s
u
)+d(s
u
,s
u-1
))其中，u2(s
u
)表示第u阶段的最小累计成本值；s
u
、s
u-1
分别为第u、u-1阶段的阶段点；d(
·
)函数用于计算两点之间的欧式距离；第z阶段的阶段点s
z
即为第z个阶段的路径点t
z
即终点节点板f，从第1阶段的阶段点s1即为第1阶段的路径点t1即起点节点板e；s222，进行反向搜索，确定每个阶段的路径点：若进行第u阶段到第u-1阶段的反向搜索，则根据二维有向加权图，得到从第u阶段到第u-1阶段的所有连接路径，并结合第u阶段的最小累计成本值，得到第u-1阶段的前向最优节点，第u-1阶段的前向最优节点即为第u阶段的路径点t
u
：t
u
＝argmin(u2(s
u
)+d(set_s
u
,s
u-1
))；其中，set_s
u
表示与第u-1阶段的阶段点s
u-1
相连的前向节点集合；d(
·
)函数用于计算两点之间的欧式距离；u2(s
u
)表示第u阶段的最小累计成本值；argmin函数用于获取使得目标函数取最小值时的变量值；t
u
表示第u阶段的路径点；s223，得到从第1阶段到第u阶段的最小路径t
1,u
为：t
1,u
＝{t1,...,t
u
}；从第1阶段到第u阶段的路径代价u
min
(t
1,u
)为：其中，u≤z，u-1≥1；s23，若从第z阶段到第k-1阶段的最小路径t
z,k-1
与从第1阶段到第u阶段的最小路径t
1,u
中，存在t
k-1
＝t
u
，则表示顺向动态规划和逆向动态规划到同一路径点，则停止顺向动态规划和逆向动态规划，且得到起点节点板e与终点节点板f之间的最优路径为t
z,k-1
+t
1,u
；否则，继续同步进行顺向动态规划和逆向动态规划；s24，按照步骤s21和s22的方式继续同步进行顺向动态规划和逆向动态规划，并按照步骤s23的方式判断顺向动态规划和逆向动态规划是否到同一路径点，直至到同一路径点，停止顺向动态规划和逆向动态规划，得到节点板e与终点节点板f之间的最优路径；若顺向动态规划和逆向动态规结束后，始终无法到同一路径点，则表示无最优路径，双向动态规划结束。5.根据权利要求2～4任意一项所述的一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，其特征在于，步骤s1中，二维有向加权图中，根据节点板的高度对角钢塔进行层划分，同一高度的节板点位于同一个层中，角钢塔上相邻的两个层中存在连接关系的节
点板在二维有向加权图中相连接，角钢塔上同一层中存在连接关系的节点板在二维有向加权图中相连接。
技术总结
本发明公开了一种基于动态规划的角钢塔攀爬机器人三维路径规划方法，涉及电力作业攀爬机器人技术领域，该方法：首先针对角钢塔自身结构特点，利用角钢塔上的节点板构造二维有向加权图，基于二维有向加权图利用动态规划算法进行最小路径规划，提高路径规划速度。本发明针对规划空间较大、状态总数增多、规划耗时变长的问题，利用双向动态规划算法把搜索空间重新规划为两个对称区域，在保证全局最优的同时，也提高了最优路径的规划速度，提高攀爬机器人的攀爬效率。本发明依据角钢塔自身结构信息高效完成攀爬机器人路径规划，避免负载相关仪器设备，增加角钢塔攀爬机器人续航能力减少能耗。能耗。能耗。
