(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210862282.0 (22)申请日 2022.07.21 (71)申请人 国网安徽省电力有限公司 地址 230022 安徽省合肥市包河区黄山路9 号 申请人 合肥工业大 学　 中国能源建 设集团安徽省电力设计 院有限公司 (72)发明人 张金锋　都海波　谢枫　程智余　 张天忠　刘军　葛展展　胡正　 许水清　罗义华　刘勇　何辉　 惠金花　梅一　徐宁　刘雁生　 常帅　黄杰　刘鑫　孟宪乔　吴睿　 周贺　(74)专利代理 机构 合肥和瑞知识产权代理事务 所(普通合伙) 34118 专利代理师 王挺 (51)Int.Cl. G05D 1/02(2020.01) G06T 17/00(2006.01) (54)发明名称 一种基于动态规划的角钢塔攀爬机器人三 维路径规划方法 (57)摘要 本发明公开了一种基于动态规划的角钢塔 攀爬机器人三维路径规划方法， 涉及电力作业攀 爬机器人技术领域， 该方法： 首先针对角钢塔自 身结构特点， 利用角钢塔上的节 点板构造二维有 向加权图， 基于二维有向加权图利用动态规划算 法进行最小路径规划， 提高路径规划速度。 本发 明针对规划空间较大、 状态总数增多、 规划耗时 变长的问题， 利用双向动态规划算法把搜索空间 重新规划为两个对称区域， 在保证全局最优的同 时， 也提高了最优路径的规划速度， 提高攀爬机 器人的攀爬效率。 本发明依据角钢塔自身结构信 息高效完成攀爬机器人路径规划， 避免负载相关 仪器设备， 增加角钢塔攀爬机器人续航能力减少 能耗。 权利要求书4页 说明书7页 附图3页 CN 115437370 A 2022.12.06 CN 115437370 A 1.一种基于动态规划的角钢塔攀爬机器人三维路径规划方法， 其特征在于， 选取角钢 塔上的特征点构建二维有向加权图， 确定二维有向加权图中特征点之间的代价， 构建二维 有向加权图的代价矩阵； 利用二维有向加权图和对应的代价矩阵进行路径规划。 2.根据权利要求1所述一种基于动态规划的角钢塔攀爬机器人三维路径规划方法， 其 特征在于， 具体包括以下步骤： S1， 根据角钢塔三维模型， 选取角钢塔上的节点板构造二维有向加权图， 角钢塔上存在 连接关系的节点板在二 维有向加权图中也相连接， 根据节点板之 间的连接 关系以及节点板 的位置， 构 建代价矩阵W＝(wi,j)q×q， q表示角钢塔中的节点板总数量； wi,j表示从节点板i到 节点板j的代价； 下 标i、 j表示节点板， i ＝1,2,3…q， j＝1,2,3 …q， i≠j； 代价矩阵W ＝(wi,j)q×q的构建方式， 具体如下 所示： 若在二维有向加权图中， 节点板i与节点板j相连接， 则wi,j＝Li,j， Li,j表示节点板i与节 点板j之间的欧式距离； 若在二维有向加权图中， 节点板i与节点板j不相连接， 则wi,j＝inf， inf表示无穷大； S2， 利用二维有向加权图和对应的代价矩阵， 对起点节点板E与终点节点板F之间的路 径进行路径规划。 3.根据权利要求2所述一种基于动态规划的角钢塔攀爬机器人三维路径规划方法， 其 特征在于， 步骤S2中， 使用动态规划方法对起点节点板E与终点节点板F之间的路径进行最 小路径规划， 具体如下 所示： 假设起点节点板E与终点节点板F之间的路径共需要z个阶段完成， 每个阶段均有对应 的路径点， 形成从起点节点板E到终点节点板F的最小路径； 其中， 第1个阶段的路径点T1为 起点节点板 E， 即T1＝E， 第z阶段的路径点Tz为终点节点板F， 即Tz＝F， S201， 进行正向搜索， 确定每 个阶段的阶段点： 从第1阶段的阶段点S1开始， 到第z阶段的阶段点Sz结束， 先依次寻找下一阶段中与上一 阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点， 并记录该下一阶段 的最小累计成本； 第k阶段的最小累计成本值U1(Sk)为： U1(Sk)＝min(U1(Sk‑1)+d(Sk‑1,Sk)) 其中， U1(Sk‑1)表示第k ‑1阶段的最小累计成本值； Sk‑1、 Sk分别为第k ‑1、 k阶段的阶段点； d(·)函数用于计算两 点之间的欧式距离； 从第1阶段的阶段点S1即为第1阶段的路径点T1即 起点节点板 E， 第z阶段的阶段点Sz即为第z个阶段的路径点Tz即终点节点板F； S202， 进行反向搜索， 确定每 个阶段的路径点： 若进行第k ‑1阶段与第k阶段之间的反向搜索， 则根据二维有向加权图， 得到从第k ‑1阶 段到第k阶段的所有连接路径， 并结合第k ‑1阶段的最小累计成本值U1(Sk‑1)， 得到第k 阶段 的前向最优节点， 第k阶段的前向最优节点即为第k ‑1阶段的路径点Tk‑1： Tk‑1＝argmin(U1(Sk‑1)+d(set_Sk‑1,Sk))； 其中， set_Sk‑1表示与第k阶段的阶段点Sk相连的前 向节点集合； d( ·)函数用于计算两 点之间的欧式距离； U1(Sk‑1)表示阶段Sk‑1的累计成本值； argmin函数用于获取使得目标函 数取最小值时的变量 值； Tk‑1表示第k‑1阶段的路径点；权　利　要　求　书 1/4 页 2 CN 115437370 A 2其中， k‑1≥1， k≤ z； S203， 得到从起 点节点板 E到终点节点板F的最小路径T1,z为： T1,z＝{T1,T2,…Tz‑1,Tz} 得到从起 点节点板 E到终点节点板F的最小路径 代价Umin(T1,z)为： 4.根据权利要求2所述的一种基于动态规划的角钢塔攀爬机器人三维路径规划方法， 其特征在于， 步骤S2中， 使用双向动态规划方法对起点节点板E与 终点节点板F之间的路径 进行最优规划， 得到起 点节点板 E与终点节点板F之间的最优路径， 具体如下 所示： 假设起点节点板E与终点节点板F之间的路径共需要z个阶段完成， 每个阶段均有对应 的路径点， 形成从起点节点板E到终点节点板F的最小路径； 其中， 其中， 第1个阶段的路径 点 T1为起点节点板 E， 即T1＝E， 第z阶段的路径点Tz为终点节点板F， 即Tz＝F， 所述双向动态规划方法包括顺向动态规划和逆向动态规划； 其中， 所述顺向动态规划 是指： 从第1 阶段的路径点T1即起点节点板E开始， 寻找到第z个阶段的路径点Tz即终点节点 板F结束的最小路径； 所述逆向动态规划是指： 从第z 阶段的路径点Tz即终点节点板F开始， 寻找到第1个阶段的路径点T1即起点节点板 E结束的最小路径； 双向动态规划的具体过程如下 所示： S21， 进行顺向动态规划： S211， 进行正向搜索， 确定每 个阶段的阶段点： 从第1阶段的阶段点S1开始， 到第z阶段的阶段点Sz结束， 先依次寻找下一阶段中与上一 阶段的阶段点之间的欧式距离最小的节点板作为该下一阶段的阶段点， 并记录该下一阶段 的最小累计成本； 第k阶段的最小累计成本值U1(Sk)为： U1(Sk)＝min(U1(Sk‑1)+d(Sk‑1,Sk)) 其中， U1(Sk‑1)表示第k ‑1阶段的最小累计成本值； Sk‑1、 Sk分别为第k ‑1、 k阶段的阶段点； d(·)函数用于计算两 点之间的欧式距离； 从第1阶段的阶段点S1即为第1阶段的路径点T1即 起点节点板 E， 第z阶段的阶段点Sz即为第z个阶段的路径点Tz即终点节点板F； S212， 进行反向搜索， 确定每 个阶段的路径点： 若进行第k ‑1阶段到第k阶段的反向搜索， 则根据 二维有向加权图， 得到从第k ‑1阶段到 第k阶段的所有连接路径， 并结合第k ‑1阶段的最小累计成本值U1(Sk‑1)， 得到第k 阶段的前 向最优节点， 第k阶段的前向最优节点即为第k ‑1阶段的路径点Tk‑1： Tk‑1＝argmin(U1(Sk‑1)+d(set_Sk‑1,Sk))； 其中， set_Sk‑1表示与第k阶段的阶段点Sk相连的前 向节点集合； d( ·)函数用于计算两 点之间的欧式距离； U1(Sk‑1)表示阶段Sk‑1的累计成本值； argmin函数用于获取使得目标函 数取最小值时的变量 值； Tk‑1表示第k‑1阶段的路径点； S213， 得到从第z阶段到第k ‑1阶段的最小路径Tz,k‑1为： Tz,k‑1＝{Tz,...,Tk‑1}； 其中， 路径是指由各个阶段的路径点构成的集 合；权　利　要　求　书 2/4 页 3 CN 115437370 A 3