1、科学技术创新 2023.04核岛机器人巡检导航 A 算法优化及其应用陈姝1,2，马奔宇1,2，魏文斌1,2（1.中核武汉核电运行技术股份有限公司，湖北 武汉;2.核动力运行研究所，湖北 武汉）引言核岛厂房属特殊的封闭空间，无法使用常规在线导航技术手段，若要实现巡检机器人点对点快速移动，必须构建核岛巡检机器人专用导航系统。该系统载入的专用大尺寸核岛地图，其路线、障碍物等元素均为不规则形态，无法简单抽象为节点或线段。通常，距离最短路径被认为最优，但大尺寸地图节点多，在所有通路中求解最短路径耗时过长，难以满足性能要求。为解决上述问题，人们提出了 Dijkstra 算法，但该算法易受当前近节点影响，从
2、而丢失近线路最优或次优解，为解决这一问题，人们提出了基于“方向优先”的 A*算法，该算法考虑当前节点到终点的距离，因此在方向和距离之间进行了折衷，路径既不过偏也不过远。参考文献1侧重于对 A*算法的数学研究；文献2侧重 A*算法应用于无人机三维路径规划；文献3研究 A*算法避障路径规划。A*算法应用型研究成果较多，公开网络上4可获取关于 A*算法详细介绍，故不进行赘述。本文基于核岛位置信息点状分布特性构建地图矩阵，用 0 和 1 分别表示未连通和连通状态。根据核岛巡检机器人在通过弯点时执行“减速-停止-转向-加速”的运动特征，以及弯点越多耗时越长的客观情况，从理论上提出了基于 A*算法的寻找“
3、距离最短”、“弯点个数最少”的路径求解方法，使用该方法可让核岛巡检机器人高效省时地完成路径规划和导航运动。1基于 A*的自主导航算法作为一种启发式搜索型快速路径规划算法，A*算法相比遍历式路径规划算法如广度优先算法（BFS）等都更加迅速和节省空间4。1.1基本思想4引入当前节点 j 的估计函数 f*，当前节点 j 的估计函数 f*定义为：（1）其中，g（j）是从起点到当前节点 j 的实际费用量度，h*（j）是从节点 j 到终点的最小费用的估计。从起始节点向目的节点搜索时，每次搜索 f*（j）最小节点直至发现目的节点，即在和起点距离相等的中间节点集合里，选取与起点和终点构成的直线距离最小节点。显
4、然，起点和选取的该节点构成的连线与起点和终点构成的连线形成的方向夹角最小，即 A*算法的“方向优先”。1.2数学函数模型4A*算法核心为估价函数 h*（j）设计。估价函数 1：欧几里德距离假设起点 S 的坐标（Sx，Sy），中间点坐标为（Nx，Ny），终点 G 坐标（Gx，Gy），估价函数 h*取欧几里德距离，表示为：摘要：核岛巡检导航线路规划既要利用核岛的地图信息，也要考虑核岛巡检机器人的机械运动特性。本文构造了核岛数字地图矩阵，并运用基于 A*算法封装的方法求解出近乎最短路径，同时发现了所获路径长短不一、弯点冗余等问题，提出了路径弯点数计算方法，设计了同时满足“距离最短”和“弯点数最少”的
5、导航算法。实验结果表明，该算法实用性强，能在较短时间内找到一条距离最短、弯点数最少的路径，能够适用核岛机器人巡检。关键词：核岛；机器人；导航；地图矩阵；A*算法中图分类号：TP242；TP18文献标识码：A文章编号：2096-4390（2023）04-0078-04作者简介：陈姝（1983-），女，硕士研究生，研究员级高级工程师，研究方向：核工业智能机器人。()()()*fjg jhj=+78-2023.04 科学技术创新（2）该估价函数的计算量较大，不适用海量数据计算。估价函数 2：曼哈顿距离考虑将两点之间的曼哈顿距离作为估价函数。其中 A 点的经纬度为（Ax，Ay），B 点的经纬度是（Bx
6、，By）,则 A、B 之间的曼哈顿距离可以表示为:（3）其中，P=2R/360。因 P 为常数，可简化为（4）此估价函数计算量小，非严格方向优先，但可保证较短路径搜索方向接近目标点方向，故采用。1.3基于 A*算法的设计流程步骤 1）根据核岛地图构建 nn 尺寸 0-1 方阵，明确各点可通行性（0 代表有障碍物，1 代表无障碍物），记为 F；步骤 2）根据待检测目标名称及其所在位置，建立另一 nn 方阵，将焊缝名称以字符串形式存储所在位置对应方阵方格中，记为 W；步骤 3）确定机器人在 F 中当前位置Ri，Rj；步骤 4）根据要检测目标名称，找到其在矩阵 W中行列位置Oi，Oj；步骤 5）根据
7、“方向优先”、“距离最短”的原则，运用函数 WRouteAStar 实现机器人行进路径的搜寻。该函数形式为：P=WRouteAStar(F，Ri，Rj，Oi，Oj)，输入为前面得到的信息矩阵 F、机器人在 F 中的当前位置Ri，Rj和要检测目标的 F 中的位置Oi，Oj。1.4测试结果分析核岛地图方阵如下：机器人在矩阵中的位置为：RobotPosition=2，1;检测目标在矩阵中的位置为：WeldPosition=16，20;其中两次运用 WRouteAStar 函数得到的路径结果见图 1。算法运行后反馈不同的可用路径，单路径长度（前者 48，后者 46）相差 2 个单位，即单次求解路径长度
8、不为最短。所有求解路径均存在冗余拐点，导致路径距离、时耗和能耗均会增加。图 1WRouteAStar 算法求解导航路径2基于最短距离和最少拐弯点的导航提升算法基于之前测试结果，核岛机器人巡检移动导航使用 A*算法求解路径，因弯点处机器人必须经历减速-停止-转向-加速至通常速度，耗时耗能，故还必须考虑拐弯点个数。2.1利用路径矩阵 P 求解拐弯点个数根据前述程序执行后的路径可知，原算法输出路径 P 为 N2 矩阵，N 代表距离点数，每一行为一个点，第一列代表矩阵高度(Y 轴)，第二列代表矩阵宽度(X 轴)，起点为矩阵最下行，终点为矩阵最上行。为与平面图形坐标表示完全一致，前后行与前后列需互换，语
9、句 B=fliplr(flipud(P)可实现互换功能。通过相邻两个离散点可根据坐标求出方向斜率（5）两个相邻点方向角 i与其斜率 ki满足如下反正切关系（6）无论横向或纵向，路径段在无拐弯点情况下，任意相邻两点方向角相同，而在拐弯点，前方向角和后方向角不同。要找到拐弯点，必须对方向角向量进行()()()?()?f xg xNGNG=+-+-()(,)*?M A BPABAB=-+-()()()xxyyf jg jABAB=+-+-(1,2)(,2)(1,1)(,1)iB iB ikB iB i+-=+-atan()iik=79-科学技术创新 2023.04差分处理（7）然后运用 find 函
10、数找到差分处理结果中非零元素位置向量 PWanDian，再运用 length 确定向量长度，即拐弯点的个数 NumWanDian。上述过程的代码语句表达如下：2.2路径距离和弯点数的特征基于上述地图数据，以给定的起始点(2,1)和终点(16,20)为例，运行 50 次获取距离和拐弯点个数，参见图 2。图 2路径距离和拐弯点个数通过采用哈曼顿距离，80%求解距离可达最短（46个距离单位），其余求解距离与最短距离相差不超过 4个距离单位；但在 50 次运行的结果中，机器人需要拐弯的次数变化较大，最少为 14 次，最多为 24 次，相差10 次，故拐弯点个数在路径质量评价中为重要指标。路径距离和拐弯
11、点个数的特征可以归纳如下：基于“方向优先”的 A*算法获取路径多数情况可满足最短距离要求；A*算法获取的路径拐弯点个数变化无规律，且变化范围较大；A*算法获取的路径拐弯点个数与距离长短无关联。2.3基于最短距离和最少拐弯点的导航算法核岛巡检过程中，机器人导航路径若能同时实现距离最短、拐弯点数最少，可极大提升机器人工作效率。下面给出这种基于 A*的可实现最短距离和最少拐弯点的导航提升算法步骤。步骤 1）输入核岛地图矩阵 F 以及机器人起点 S和终点 E 在矩阵中的坐标；步骤 2）给定循环次数 T，运行 A*算法，产生 T 条路径，求出路径距离向量 D 和弯点向量 W；步骤 3）根据距离向量 D
12、和弯点向量 W 求出最短距离 Dmin和最少弯点数 Wmin；步骤 4）给新的距离变量 DX和弯点变量 WX赋初值，如 DX=Dmin+1，WX=Wmin+1；步骤 5）执行循环语句：当 DXDmin或者 WXWmin时，求出其路径 P，并依此求出路径距离和拐弯点数，分别赋值给 DX和 WX；当 DXDmin且 WXWmin时，结束该循环；步骤 6）输出最后获得的路径 P。在该算法中，步骤 1）给出算法实现的基本数据；步骤 2）至步骤 3）确定认可的最短距离和最少拐点数；步骤 4）至步骤 5）确定循环变量初值、循环变量的每次循环值；步骤 6）完成最短距离、最少拐弯点路径的输出。用流程图表示，见
13、图 3。图 3基于最短距离和最少拐弯点的导航算法流程图基于上述地图数据，以给定的起始点(2,1)和终点(16,20)为例，在 Intel(R)Core(TM)i5-7400 CPU 3.00GHz 3.00GHz 处理器，内存为 8.00GB 的计算机平台运行 50 次，得到最小距离 46 个距离单位，最少拐弯点个数 14 个，循环 4 次耗时 0.14 秒即找到了最优路径，参见图 4。具体路径在地图上的显示见图 5。图 4最优路径信息1iiiD+=-80-2023.04 科学技术创新图 5最优路径图示上述结果表明，基于最短距离和最少拐弯点的导航提升算法可在较短时间内求解距离最短、拐点最少路径
14、。该算法可以应用于核岛机器人巡航导航以及其它实时导航系统5。3实用情况总结核岛机器人巡检移动导航，采用 A*算法作为核心算法，兼顾距离最短、方向优先法则，可极大减少寻求最优路径的时间，快速获取最优路径。同时，考虑到机器人行走减速拐弯耗时的特性，本文提出了基于最短路径和最少弯点的提升算法，其中介绍了弯点数计算方法，多次运用 A*算法获取多条路径，通过这些路径计算出从起点到终点的距离向量和弯点数向量，找到从起点到终点可能的最短距离和从起点到终点的最少弯点数的路径，以最短距离和最少弯点数作为条件，循环搜寻满足以上两条件的路径。通过本文中的算法代码运行实验结果表明，该算法可以在较短时间内找到同时满足“
15、距离最短”和“弯点数最少”的路径，具有很强的实用性。参考文献1延卫军,张利军,毕冬瑶.A*算法的代数表示J.系统科学与数学,2022,42(6)：1478-1489.2卞强,孙齐,童余德.一种新的改进 A*算法无人机三维路径规划J.武汉理工大学学报,2022,44(7)：80-88.3绳红强,黄海英,崔毅刚.基于 A*蚁群融合算法的避障路径规划研究J.机电工程技术,2022,51(7)：45-49.4A*算法.百度搜索.5李明龙,李清忠.基于改进蚁群算法的无人天车路径规划J.计算机仿真，2021(8)：172-176，226.Analysis and Improvement for AAlgo
16、rithm Used in Robotic On-siteInspection and Navigation in A NuclearIslandChen shu1,2，Ma Benyu1,2，Wei Wenbin1,2（1.China Nuclear Power Operation Technology Corporation,LTD,Wuhan,China;2.Research Institute Of Nuclear Power Operation,Wuhan,China）Abstrat：The nuclear-island inspection plan for the navigating route has to use the map of a nuclearisland,and consider the character of the mechanical movement of the nuclear-island robot.In this paper,amap matrix about a nuclear island is made,and some near
