移动机器人路径规划安全A*算法

葛文雅， 李平

(华侨大学 信息科学与工程学院， 福建 厦门 361021)

路径规划是移动机器人最重要的应用之一[1-5]，是移动机器人研究领域的热点问题[6-9].移动机器人规划的路径应该是最优、无冲突的，且有助于在复杂环境中完成某项任务，因此，路径的安全性是首要考虑的问题.现有的路径规划方法主要分为传统算法和智能算法两类[10].传统算法主要包括人工势场法和可视图法.人工势场法是局部路径规划常用的方法，其将机器人工作的环境定义为势场，目标点定义为引力源，障碍物定义为斥力源，引力源与斥力源产生的合力共同作用于机器人，使机器人在实时避障的同时向目标点前进.人工势场法实时性好、数学原理易懂、规划路径平滑，但存在局部最优、目标不可达及易抖动等问题，路径规划的全局性不强[11-12].可视图法将机器人视为质点，将障碍物视为近似多边形，通过线段连接质点、目标点和障碍物顶点(禁止连线穿过障碍物多边形)，搜索起始点到目标点的最短线段集合.可视图法直观，容易求得最短路径，适用于全局和连续区域内的路径规划，但可视图法灵活性缺乏、搜索能力不足，不适用于大型动态环境，一般需要结合其他智能算法进行搜索[13-14].智能算法具有很好的全局性，在处理复杂动态环境信息情况下的路径规划问题时，来自于自然界的启示往往能起到很好的作用，如蚁群算法、神经网络算法和遗传算法[15-20].

传统算法和智能算法都是把全局最短路径作为搜索目的，规划的路径缺少路径安全性的考虑.A*算法是一种启发式搜索算法，在多节点的二维或三维地图环境中，能够搜索出最短路径(最小通过代价).A*算法常被用于已知环境下的全局路径规划，其估值函数计算简单，可以规划出有效的最短路径，因此，A*算法被广泛地应用于复杂环境下的路径规划问题[21-23].然而，A*算法规划的路径和障碍物相邻，路径转角角度过大，机器人在跟踪规划的路径时，存在与障碍物碰撞和侧翻的危险，安全性无法得到保障.Park等[24]提出改进的 A*算法，用配置空间计算模型处理潜在风险，并将风险成本引入A*启发式函数中，保证路径和障碍物之间留有安全距离.Bayili等[25]提出有限损伤A*算法，该算法以损伤为可行性准则，考虑碰撞风险，以获得更安全的路径.Sun等[26]提出一种模糊路径规划算法，通过约束机器人的角速度，保证规划路径的安全性.然而，对于移动机器人路径规划时存在路径距离障碍物过近和路径转角角度过大等问题，上述方法仍考虑得不够周全.基于此，本文对A*算法进行改进，提出一种移动机器人路径规划安全A*算法.

1 栅格法环境建模

采用栅格法[27]构建地图模型，栅格法是移动机器人环境建模的常用方法.栅格法将移动机器人的工作空间划分为网格单元.分辨率是度量栅格地图准确度的一项关键参数，它将一张图片分别按横轴、纵轴划分x×y的像素点.栅格地图的准确度与分辨率成正比，文中构建的栅格地图分辨率为100×100.栅格地图模型，如图1所示.图1中：黑色区域为障碍物信息；白色区域为机器人可移动区域.将地图信息存放于矩阵M(i，j)中，i，j分别为矩阵中的行、列，则有

图1 栅格地图模型Fig.1 Grid map model

(1)

在构建栅格地图时，每一个栅格都与实际环境地图中的位置一一对应.假设在一个分辨率为N×N的栅格地图中，移动机器人的坐标为(xg，yg)，有

(2)

式(2)中：O为网格序列编码；mod为取余函数，使用该函数，可得到网格序列编码O所在的列数，即移动机器人的横坐标xg；int为取整函数，使用该函数，可得到网格序列编码O所在的行数，即移动机器人的纵坐标yg；每个网格单位为1，加上0.5表示机器人的坐标位于网格的中心.

2 A*算法及其存在的问题

A*算法是静态图搜索最短路径最有效、直接的方法，启发式搜索综合了迪杰斯特拉(Dijkstra)算法的广度优先及贪婪搜索的做法.A*算法通过待扩展节点open表和已扩展节点close表对节点进行搜索，节点总是朝着启发式函数最小的方向进行扩展.

在A*算法的距离选取中，对于栅格地图上的任意两点(x1，y1)，(x2，y2)的距离，常见的度量方式为欧几里德距离DEuc、曼哈顿距离DMan和切比雪夫距离DChe，这3种度量方式的计算公式分别为

(3)

DMan=|x2-x1|+|y2-y1|，

(4)

DChe=max (|x2-x1|，|y2-y1|).

(5)

经对比分析后可知，欧几里德距离更符合路径的长度，因此，A*算法选用欧几里德距离进行度量.后续节点的扩展涉及搜索邻域的问题，在栅格地图中，A*算法采用4邻域或8邻域进行搜索.4邻域及8邻域的搜索方向[28]，如图2，3所示.4邻域规划的路径经过每个栅格的中心点；8邻域规划的路径不仅经过每个栅格的中心点，也经过每个栅格的顶点.每个栅格的中心点被假设为节点，节点临近的4个或8个区域都是该节点的扩展个数，运动方向的角度也被限定为π/2或π/4的整数倍.

图2 4邻域的搜索方向 图3 8邻域的搜索方向 Fig.2 Four neighborhood search direction Fig.3 Eight neighborhood search direction

以4邻域为例，其A*算法搜索路径规划，如图4所示.图4中：所选地图为无障碍地图，给定起始点和目标点；蓝色路径(图4(b))为由A*算法得到的自动最佳路径，路径有转折点，较为复杂，并非起始点与目标点之间的最佳期望路径.因此，在栅格地图上，A*算法使用4邻域进行路径规划时，存在以下2个问题:1) 由于邻域的限制，规划路径与实际最短路径的差距较大；2) 规划路径的转角角度相对较大，对路径安全性影响较大，安全性不高.

(a) 搜索区域 (b) 规划路径 图4 A*算法4邻域搜索路径规划Fig.4 Four neighborhood search path planning of A* algorithm

启发式函数是A*算法的重要参数，A*算法的搜索效率与其密切相关，A*算法在分析时采用的估价函数[29]为

f(n)=g(n)+h(n).

(6)

式(6)中：f(n)表示从起始点经由节点n到目标点的估价函数；g(n)为基本项，表示状态空间中从起始点到节点n的实际代价；h(n)为启发项(启发式函数)，表示节点n到目标点的估算代价．

然而，A*算法的启发式函数中的样本信息较为单一，缺少待扩展节点周围障碍物的信息，从而导致A*算法的规划路径与障碍物距离过近，路径安全性较低.

3 安全A*算法

A*算法的搜索区域和规划路径，如图5，6所示.由图5，6可知：A*算法规划的路径转角角度较大，基本为90°的折角，部分路段和障碍物距离过近，移动机器人在实际环境行走时的安全性较低.因此，提出安全A*算法，主要改进以下3个方面的内容:1) 扩展搜索邻域；2) 改进启发式函数，加入安全项，增加样本信息；3) 对路径安全性进行量化，提出安全性指数S.

图5 A*算法的搜索区域 图6 A*算法的规划路径 Fig.5 Search area of A* algorithm Fig.6 Planning path of A* algorithm

3.1 搜索邻域的扩展

针对A*算法路径规划存在转角角度过大的问题，安全A*算法将节点n的搜索邻域扩展至24个.搜索邻域扩展过程如下：对节点n(x，y)遍历临近的24个节点(图7，中间黑色区域为节点n，周围白色区域为其遍历的临近节点，将临近节点作为后续节点m(x′，y′)，m={(x′，y′)|x′=[x-2，x+2]&y′=[y-2，y+2]}，箭头为路径方向).如果后续节点m为障碍点，或后续节点m在close表中，则跳过，选取下一个临近点作为后续节点；如果后续节点m既不是障碍点，又不在close表中，则计算f(m)，判断m是否在open表中，若判断失败，则把后续节点m放入open表，若判断成功，则选取较小的f(m)，更新g(m)，f(m)及后继节点m的前向指针.

图7 24邻域的搜索方向 图8 搜索邻域扩展A*算法的规划路径 Fig.7 Twenty-four neighborhood search direction Fig.8 Planning path of A* algorithm after search neighborhood expansion

经搜索邻域扩展后的A*算法(即搜索邻域扩展A*算法)的规划路径，如图8所示.由图8可知：搜索邻域扩展A*算法的路径转角角度减小较多，路径安全性可得到一定的改善.

3.2 启发式函数的改进

扩展A*算法节点n的搜索邻域可以减小路径的转角角度，在移动机器人的实际运动中提高了路径安全性.然而，路径和障碍物的距离仍然很近，路径的安全性还有很大的提升空间.A*算法最核心的部分在于启发式函数h(n)的设计，改进A*算法时利用了A*算法对当前轨迹节点进行评估的思想，增加了节点及一定范围内障碍物的信息，增大了路径和障碍物的距离，从而使安全A*算法具有更高的路径安全性.

安全A*算法的估价函数可以表示为

f(n)=g(n)+h′(n)，

(7)

h′(n)=h(n)+βL(n)，

(8)

(9)

式(9)中：h′(n)为安全A*算法的启发式函数；L(n)为当前临时节点对应的危险评估值(安全项)；t为评价范围内障碍物的个数；s为机器人与障碍物的最小距离；β为安全项L(n)的权重，文中设定为100.

安全A*算法的核心思想是在A*算法的基础上，引入一项安全项L(n)，作为评价该节点处于最优轨迹路线的可能性度量.在移动机器人的运动过程中，评价范围内障碍物的个数t越小越好，而移动机器人和周围障碍物的最小距离s越大越好，从而更好地保证机器人移动时的路径安全性.因此，采用t和s的比值表示安全项，以反映路径安全程度.当评价范围内障碍物的个数t较大，与障碍物的距离较小时(t较大，s较小)，L(n)的数值较大，对h′(n)的影响增大，使该节点的估价函数值较大，导致选择该节点的可能性变小；反之，L(n)的数值较小，对h′(n)的影响减小，使该节点的估价函数值较小，导致选择该节点的可能性增大.

改进后的估价函数f(n)使A*算法减少了遍历节点的个数，提高了A*算法的搜索效率，搜索方向可以更快地趋近于目标点，远离障碍物.

安全A*算法的规划路径，如图9所示.由图9可知：安全A*算法具有更高的路径安全性.

图9 安全A*算法的规划路径Fig.9 Planning path of safe A* algorithm

3.3 安全性的量化

为了更好地评估算法的路径安全性，将以后续节点m为中心的评价范围内的障碍物与其的最短距离、评价范围内障碍物的个数作为该阶段影响路径安全性的因素，提出安全性指数S，相应的安全性量化规则表，如表1所示.

表1 安全性量化规则表Tab.1 Security quantification rule table

选取评价范围为7×7的评价矩阵P，即

上式中：1表示评价范围内的节点；2表示后续节点m.

3.4 安全A*算法的路径规划

安全A*算法在执行路径规划时主要通过open表和close表实现节点的扩展和最优节点的选取.open表保存搜索过程中遇到的扩展节点，并将这些节点按估价函数值进行排序；close表保存open表中估价函数值最小的后续节点；safe表保存后续节点的安全性指数.

安全A*算法路径规划过程有以下7个步骤.

1) 将起点纳入open表，将障碍点纳入close表.

2) 选取open表中估价函数值最小的节点n，将其纳入close表中，将节点n的安全性指数S纳入safe表.

3) 判断n是否为目标点，若n为目标点，则根据它的前向指针生成最优路径；若n不是目标点，则扩展节点n，生成后续节点m.

4) 在open表中建立从后续节点m返回n的指针，计算

f(m)=g(m)+h′(m)，

(10)

h′(m)=h(m)+βL(m)，

(11)

L(m)=t/s.

(12)

5) 增加判断语句，判断open表中是否已有后续节点m，如果判断失败，则将节点m纳入open表，并将节点m的安全性指数S纳入safe表；若判断成功，则比较不同前向指针的f(m)的大小，并选取较小的f(m).

6) 更新g(m)，f(m)及后续节点m的前向指针.

7) 按照数值大小进行正序排列，将估价函数值在open表中重新排序，并返回步骤2).

4 仿真结果与分析

选用3种障碍物复杂程度的地图环境(简单地图、低复杂地图和高复杂地图)在MATLAB中进行仿真实验.

从规划路径、搜索区域及安全性曲线3个方面，对A*算法、搜索邻域扩展A*算法和安全A*算法(文中算法)进行对比分析，结果如图10～12所示.图11中：灰色区域为搜索区域.

由图10～12可知：与A*算法相比，文中算法在路径规划、搜索区域及安全性曲线3个方面都具有明显优势.

(a) 简单地图(A*算法) (b) 简单地图(搜索邻域扩展A*算法) (c) 简单地图(文中算法)

(d) 低复杂地图(A*算法) (e) 低复杂地图(搜索邻域扩展A*算法) (f) 低复杂地图(文中算法)

(g) 高复杂地图(A*算法) (h) 高复杂地图(搜索邻域扩展A*算法) (i) 高复杂地图(文中算法)图10 3种算法路径规划的对比Fig.10 Comparison of path planning of three algorithms

(a) 简单地图(A*算法) (b) 简单地图(搜索邻域扩展A*算法) (c) 简单地图(文中算法)

(d) 低复杂地图(A*算法) (e) 低复杂地图(搜索邻域扩展A*算法) (f) 低复杂地图(文中算法)

(g) 高复杂地图(A*算法) (h) 高复杂地图(搜索邻域扩展A*算法) (i) 高复杂地图(文中算法)图11 3种算法搜索区域的对比Fig.11 Comparison of search area of three algorithms

(a) 简单地图(A*算法) (b) 简单地图(搜索邻域扩展A*算法) (c) 简单地图(文中算法)

(d) 低复杂地图(A*算法) (e) 低复杂地图(搜索邻域扩展A*算法) (f) 低复杂地图(文中算法)

(g) 高复杂地图(A*算法) (h) 高复杂地图(搜索邻域扩展A*算法) (i) 高复杂地图(文中算法)图12 3种算法安全性曲线的对比Fig.12 Comparison of security curves of three algorithms

在规划路径方面，对比图10(a)和10(b)，图10(d)和10(e)，图10(g)和图10(h)可知：在3种复杂程度不同的地图上，相较于A*算法，搜索邻域扩展A*算法的规划路径具有更小的转角角度，路径的平滑性更高，安全性得到了提高，但搜索邻域扩展A*算法没有改善规划路径和障碍物之间距离过近的问题，甚至在简单地图中，搜索邻域扩展A*算法的规划路径和障碍物之间的距离更近，这是因为搜索邻域扩展A*算法的扩展领域通过减少路径折角使路径变得更加平滑，以此提高安全性，未考虑规划路径和障碍物之间的距离.对比图10(b)和图10(c)，图10(e)和图10(f)，图10(h)和图10(i)可知：文中算法在启发式函数中引入安全项，增大了路径和障碍物之间的距离，因此，文中算法不仅具有更小的转角角度，路径的平滑性更高，而且能够适应不同的地图，和障碍物保持一定的距离，具有更高的安全性.

在搜索区域方面，由图11可知：相较于A*算法和搜索邻域扩展A*算法，文中算法的搜索区域较小，尤其是在高复杂地图中，文中算法的搜索区域比A*算法缩小了很多.

3种地图环境下搜索节点数的对比，如表2所示.表2中：c为搜索节点数.由表2可知：在简单地图中，文中算法搜索的节点数和搜索邻域扩展A*算法相仿，但比A*算法减少了78.5%；在低复杂地图中，文中算法搜索的节点数分别比搜索邻域扩展A*算法、A*算法减少了33.7%和35.5%；在高复杂地图中，文中算法搜索的节点数分别比搜索邻域扩展A*算法、A*算法减少了68.0%和74.3%.因此，文中算法具有更低的时间成本和空间成本.

表2 3种地图环境下搜索节点数的对比Tab.2 Comparison of search node number in three map environments

在安全性曲线方面，由图12可知：A*算法经过搜索邻域扩展后的安全性曲线变得更加杂乱，安全指数S下降；低复杂地图和高复杂地图的搜索邻域扩展A*算法安全性指数(图12(e)，12(h))降至2，路径安全性变得更差.这是因为A*算法经搜索邻域扩展后只改善了路径转角角度过大的问题，并未考虑对路径与障碍物之间的距离，而文中算法在搜索邻域扩展A*算法的基础上，对启发式函数进行改进，引入安全项，使规划路径和障碍物之间的距离增加，安全性曲线得到优化，安全性指数明显提高，即使在高复杂地图中，文中算法的安全性指数也能够保持在9以上，表明文中算法具有更高的安全性.

对路径规划而言，安全性是最重要的考虑指标，在安全性得到保证的前提下，应尽量缩短路径的长度(l，栅格地图中无量纲).A*算法以路径长度为主要指标规划路径，选择长度较短，但距离障碍物较近的路径节点，由于未考虑周围障碍物的信息，导致路径安全性过低.文中算法将节点周围障碍物的信息引入了启发式函数，把安全性作为主要的衡量指标，在路径规划时，选择与障碍物有一定距离且路径较短的节点，其路径长度虽比A*算法略有增加，但可以保证路径安全性.由于安全性指标更为重要，所以通过一定的路径长度换取安全性是值得的.

3种地图环境下规划路径长度的对比，如表3所示.由表3可知：在简单地图、低复杂地图和高复杂地图中，搜索邻域扩展A*算法的路径长度比A*算法分别减少了24.4%，17.3%和8.6%；文中算法虽然牺牲了一定的路径长度，但路径安全性得到了较大改善，比搜索邻域扩展A*算法的路径长度略有增加，比A*算法的路径长度略有减少.

表3 3种地图环境下规划路径长度的对比Tab.3 Comparison of planned path length in three map environments

5 结束语

针对路径规划安全性不高的问题，对A*算法进行相应改进，以栅格环境为基础，对搜索邻域进行扩展，减小转角角度，避免不必要的折角；针对启发函数的选取，引入周围障碍物的信息作为评估要素，使样本信息更充分，增加了路径和障碍物之间的距离；对路径进行安全性量化，更好地对路径安全性进行评估.在MATLAB软件上进行仿真实验，结果表明，相较于A*算法和搜索邻域扩展A*算法，文中算法的路径质量和安全性更佳.