改进A*算法在无人船路径规划中的应用

赵春宇,姜 皓,徐茂竹,满伟俊,杨伟明,陈范模

(1.上海交通大学 电子信息与电气工程学院,上海 201100;2.上海十方生态园林股份有限公司,上海 201100)

无人船作为一种新型机器人,在城市水域的水质监测以及水体管养等领域发挥着重要的作用[1-3]。路径规划作为机器人的核心能力,近年已有诸多学者在该方面提出了多种传统算法,如人工势场法[4]、蚁群算法[5]、RRT算法[6]和A*算法等。其中,A*算法因在静态环境规划的应用中简单有效、对不同地图的适应性强以及在机器人路径规划中应用广泛等特点,很多学者依据A*算法作出改进,提出更加有效的算法[7-10]。目前,针对无人船的路径规划聚焦在海洋领域。余必秀等[11]针对传统A*算法规划的路径在绕过障碍物后继续前进的路径与原规划路径相差过大的问题,提出在原代价函数基础上增加一项和当前点到原绘画航线垂直距离相关的代价值,以达到在避障后快速回到原路径的效果。吕霞付等[12]使用改进的A*算法解决无人船在复杂环境下遍历所有区域时陷入死区的问题,并降低了全覆盖规划的重复率和路径步数。Lee等[13]在改进路径规划算法时考虑了无人船本身的转弯能力,在满足航向角约束的同时为实际行驶生成更可靠的航路点。Singh等[14]考虑了无人船在行进过程中周围环境的作用,包括移动障碍物以及水流干扰作用,并对生成轨迹的安全性和平滑性进行了讨论。

综上,目前的研究工作主要集中在优化路径总长度以及在外界干扰下路径的鲁棒性等方面,而针对用于河道监测的无人船研究起步较晚,针对其在城市水域中的路径规划研究则更匮乏。基于此,首先分析了传统A*算法的不足;然后针对无人船在城市水域中航行时的特点对A*算法进行优化;最后选取典型的城市河道环境模型,通过Python进行可视化仿真对比,使无人船航行时不仅能够尽量远离障碍物,而且路径更加平滑。

1 传统A*算法

A*算法是一种经典的路径搜索和图形遍历算法,常用于二维栅格地图的路径规划。通过将启发式搜索和常规的图搜索法结合在一起,在减少了搜索范围的同时又能保证规划出来的路径为最短路径。A*算法通过一个估值函数来引导路径的搜索,估值函数为

F(N)=G(N)+H(N)

(1)

式中:F(N)为节点N的综合优先级,选择下一个路径子节点的方法是从当前节点的相邻节点中选出优先级最高(F(N)最低)的点;G(N)为从起点移动到节点N的实际距离代价,即节点N的G(N)等于其父节点的G(N)加上其父节点到节点N的距离代价;H(N)为从节点N移动到终点的预估距离代价,通常取节点N到终点的欧几里得距离作为预计代价,设终点为P,即

(2)

2 改进的A*算法

水质监测无人船及工作环境如图1所示。无人船需要装载水质检测传感器进入这些狭窄河道作业。传统A*算法规划出来的路径有两个明显的缺点:1) 由于传统A*算法的估值函数仅考虑了路径在距离上的最优解,其规划出来的路径通常和障碍物边缘贴得很近。由于在建立地图过程中坐标转换的误差以及障碍物形状测量上的误差,路径在移动端地图上绘制时,甚至会向障碍物内部一定程度地偏移。无人船如按照这样的路径行驶,则很容易和障碍物相撞。2) 传统A*算法由于只能遍历栅格中的某一个固定点,其转折方向只能是45°的整数倍,致使其规划的路径会产生一些不必要的转折。这些转折不利于无人船的运动控制,会让无人船消耗更多资源。笔者针对这两方面缺陷,提出了以下两种改进方法。

图1 水质监测无人船Fig.1 Water quality monitoring unmanned surface vehicle

2.1 估值函数改进

针对规划的路径贴近障碍物的问题,对传统A*算法的估值函数作出改进。在原有估值函数的基础上,引入新的估值函数D(N)。将D(N)定义为附近的障碍物对节点N的影响程度,D(N)越大,代表节点N间隔障碍物越远。新的估值函数的计算式为

F(N)=G(N)+H(N)-D(N)k

(3)

式中k为遍历节点的安全性对路径搜索的影响权重。k值越大,规划的路径距障碍物边界越远,路径安全性越高。k值大小根据多次实验的结果确定,不断调整k值使得规划的路径既与障碍物之间留有一定的安全空间,又在距离上接近最短路径。对于不同的地图,数据k的选取不同,将k选取为2。

栅格搜索方式如图2所示。D(N)的计算方法是以节点N为中心,逐层向外遍历周围的节点,设定搜索的节点最远与节点N相距m个栅格。

图2 栅格搜索方式Fig.2 Raster search mode

设当前访问的节点与节点N相距d个栅格,d从1开始取值,即从第1层栅格开始搜索,若出现含有障碍物的禁航栅格,则返回1,并跳出当前循环;若未出现禁航栅格,则d加1,继续搜索第2层栅格,重复前面步骤;若m层栅格仍未出现禁航栅格,则返回m。对下一个节点重复上述步骤。

若在路径搜索的过程中计算D(N),则会延长路径规划完成的时间。故在程序运行开始前,对存储栅格地图的二维矩阵进行预处理,即遍历栅格地图中所有可航行的栅格点,计算它们的D(N)并存储到二维矩阵中。

2.2 遍历方式改进

传统A*算法通常把栅格的一个端点作为节点来访问,遍历节点N的周围节点,只访问8个离散的栅格端点,具体方式如图3所示。

图3 传统A*算法探索邻域方式Fig.3 Traditional A* algorithm exploring neighborhood mode

当起点和终点的连线与栅格的边线形成的夹角不是45°的整数倍时,会产生多余的转折,此时规划的路径和最短路径的对比如图4所示。

图4 传统A*算法规划路径与最短路径比较Fig.4 Comparison between traditional A* algorithm and shortest path

为了在遍历当前节点的子节点时不仅只访问其周围8个栅格的端点,Ferguson等[15]提出了Field D*算法,对栅格进行近似线性插值,提高路径规划规划结果的平滑性。在这种线性插值的思想上,根据无人船栅格地图实际情况来进行改进。重新定义节点位置的方法如图5所示。将栅格边线上的任意点,而不是栅格的端点,作为节点来访问。

图5 重新定义节点位置Fig.5 Redefines node positions

设当前节点为N(x0,y0),当节点N与某栅格边线之间的栅格为可通行栅格时,将栅格边线上估值函数值最小的点作为节点N的子节点。设边线端点为X1和X2,该边线上有一任意点X距X1距离为d,设点X到起点的实际距离代价为G(X),点X到终点的预计距离代价为H(X),点X的G(X)和H(X)的计算式为

(4)

在实际的路径规划算法中,分成两类情况来讨论栅格边线上估值函数最小节点的位置:节点N是栅格的端点和节点N位于栅格边线上。

第一类情况如图6所示,节点N的横纵坐标均为整数。设X1为边线距离节点N较近的端点(横坐标或者纵坐标与节点N相同),X2为另一个端点;X为边线X1X2上的一个任意点;d为点X到X1的距离;栅格为边长是1的正方形;G(X)为从起点移动到点X的实际距离代价;H(X)为从点X移动到终点的预计代价。

图6 节点N位于栅格边线交点Fig.6 Node N located at the intersection of grid edges

根据不同情况讨论X的位置,具体情况如下：

(5)

3) 当H(X1)和H(X2)的值上述两种情况均不满足时,点X取点X1的位置。

第二类情况如图7所示,节点N的横坐标含有小数,纵坐标为整数(或者横坐标为整数,纵坐标含有小数),与之相邻的6条栅格边线有两种类型：第一种栅格边线有一个端点的横坐标或纵坐标与节点N的横坐标或纵坐标相同;第二种栅格边线两个端点的横纵坐标均与节点N的横纵坐标不相同。

图7 节点N位于栅格边线上Fig.7 Node N on the grid edge

当栅格边线如图7(a)所示时,设X1为栅格边线X1X2上距离节点N较近的端点,X2为另一个端点;d为边线X1X2上有一任意点X距离点X1的距离;t为节点N与点X1的距离;G(X)为从起点移动到点X的实际距离代价;H(X)为从点X移动到终点的预计代价。根据不同情况,具体讨论为

(6)

3) 当H(X1)和H(X2)的值不满足上述两种情况时,点X取点X1。

当栅格边线如图7(b)所示时,设m为边线X1X2上有一任意点X与点X2的距离;t为节点N的纵坐标与点X2的纵坐标差值的绝对值;G(X)为从起点移动到点X的实际距离代价;H(X)为从点X移动到终点的预计代价。通过推导得出的结果为

3) 当H(X1)和H(X2)的值不满足上述两种情况时,点X取在栅格边线X1X2上,距点X2的距离m计算式为

(7)

2.3 时间复杂度分析

改进后的A*算法流程图如图8所示。其中计算障碍估值函数为离线任务,在构建地图后进行初始化。假设栅格大小为M×M,该过程的时间复杂度为O(M2),属于地图的一部分信息,不影响A*算法在线规划时的速度。传统遍历节点的方式改进为遍历节点之间边线的方式,遍历最大数量由M2变为4M2,平均时间复杂度为O(2M·log(2M)),相较于传统A*算法的平均时间复杂度O(M·logM)属于同一数量级,不会引入大量的时间开销。

图8 改进A*算法流程Fig.8 Flow chart of improved A* algorithm

3 仿真及实验结果

通过Python进行可视化仿真,选取典型的城市河道环境模型,路径规划结果如图9所示。传统A*算法规划的路径上多个节点紧贴障碍物边缘。由于无人船对电机的控制存在误差,且栅格地图和实际地图存在一定的偏差,无人船按照这样的路径行驶极易撞上障碍物。

图9 不同算法的路径规划结果Fig.9 Path planning results of different algorithms

两种路径规划算法的对比如表1所示。改进A*算法规划的路径不仅始终与障碍物保持安全距离,在河道的中央处行驶,保证了无人船航行的安全性,而且无效转折次数明显减少,提高了路径搜索的质量。

表1 仿真结果对比

为了验证路径规划算法的可行性,将路径规划算法实际部署到船载主机树莓派上,并在移动端的电子地图上实时绘制无人船的位置,生成路径,以此来评估规划路径的长度、安全性和平滑程度。

无人船实际航行路线如图10所示。其中,图10(a)为传统A*算法的路径规划结果;图10(b)为改进A*算法的路径规划结果。在移动端地图中,设置相同的起始点,分别用传统A*算法和改进A*算法进行路径规划。由图10(a,b)可知:改进以后的规划路线距离河岸更远,安全性更高,路径更为平滑。

图10 无人船实际航行路线Fig.10 Actual route of unmanned surface vehicle

4 结 论

根据无人船工作环境的特点,分析了传统A*算法在规划路径时存在的缺陷,通过引入新的障碍距离评价函数D(N)和遍历栅格边线的方式对A*算法进行了改进。仿真和实验结果表明:笔者方法相较于传统A*算法规划的路径能够和障碍物保持一定安全距离,同时路线更加平滑,转折次数明显减少。改进的A*算法提高了路径的安全性,为无人船在城市河道作业时的路径规划问题提供了新思路。