基于变网格A*算法的机器人驾驶路径规划

任荣伦,卫 沅,周志刚,李浩然

(河南科技大学,河南 洛阳 471000)

1 引言

在有放射性污染物、废弃核电站等的危险工况下,人类无法进入进行救援工作。因此救灾机器人一直是各国研究领域热点,但是当遇到复杂的环境比如楼梯、不平坦路面的情况且路径距离较远时,移动机器人无法到达,此时人形机器人可以驾驶汽车进行救援工作[1]。在救援开始前,需要对路径进行规划,机器人才能驾驶汽车进行救援[2]。因此研究机器人驾驶路径规划具有重大理论价值与应用前景。根据规划范围不同,可将路径规划分为静态的全局路径规划和动态的局部路径规划[3],常见的全局路径规划算算法有Dijkstra算法、A*算法[4,5]、RRT算法[6]、遗传算法[7]等;常见的局部路径规划算法有动态窗口法(DWA)[8]、人工势场法[9]等。

全局规划中遗传算法在搜索最短路径容易陷入局部最优的问题[10];RRT算法规划出的路径不一定是平滑的,且搜索效率较低[6];Dijkstra算法采用遍历搜索方式,搜索节点数较多,算法效率低下,难以快速满足快速规划路径的需求[11]。A* 算法原理简单、易于实现、效率高、速度快且能搜索到最优解。且A*算法在Dijkstra算法的基础上,根据估计代价决定搜索方向,提高了搜索效率。但是传统A*算法有拐点过多,路径不够平滑、与障碍物距离近等问题。陈娇等[12]在文献中对A*算法删除了多余节点,保证了路径全局最优。Boroujeni.Zahra等[13]在文献中将A*算法用于结构化道路图路径规划。迟旭等[14]在文献中优化了搜索策略减少了三个搜索方向,提高了搜索效率。武义等[15]在文献中将搜索领域扩大为48方向,提高了路径的平滑性。ZhangJing 等[16]在文献中提出结合人工势场法新型启发式函数,并将算法用于陆地车辆,使其更容易控制。

本文提出一种变网格的A*算法。首先,改进评价函数的计算方式和代价计算方式,从而降低搜索时间。其次,将传统的A*算法的3×3搜索邻域和7×7邻域结合起来,根据障碍物自适应调整网络的结构,在无障碍物时,采用3×3邻域快速搜索,增大h(n)比重使函数f(n) 的启发式信息增强,增加搜索效率;遇到障碍物时自适应调整为7×7邻域搜索,将h(n)比重调节略小于g(n),使其保证搜索效率的同时也兼顾路径平滑度。同时为了避免汽车转弯时边缘发生碰撞,对障碍物进行膨胀化处理,保证了与障碍物之间的安全距离。最后对规划出的路径进行提取关键节点和圆弧平滑处理,保证规划的路径足够平滑。

2 A*算法

2.1 传统A*算法

A*算法是一种在静态地图中解决路径规划问题比较优良的算法,它是根据Dijkstra算法改进的一种优化算法。作为一种典型的启发式搜索算法,A*算法的特点是搜索子节点时会根据当前节点信息进行对比,即每次搜索总是选取最小位置作为子节点。选取子节点需要根据特定的代价函数,按照此方法执行,直到搜索到目标节点位置为止。A*算法的启发式函数

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

(1)

其中:f(n)为经过当前节点n的全局评估代价值;g(n)为起始节点到当前节点n的真实代价值;h(n)为当前节点n到目标节点的代价估计。当评价函数中h(n)所占比重较小时,A*倾向于广度优先搜索,可以求解出最短路径,但是耗时较长。反之,h(n)比重较大时,A*算法搜索启发性更强,搜索时间较快,但规划的路径不一定是最优的,因此合理分配二者的比例十分重要。

2.2 变网格A*算法

2.2.1 变换网格

传统A*算法主要有8个搜索方向,8个箭头表示机器人在移动过程中的8个搜索方向,机器人的最小转角为90°。武义等将A*算法进行改进,由8领域搜索方式改为48领域搜索方式,机器人的搜索方向变为32个方向,移动机器人的最小转角为11.25°,提高了路径平滑度。但是由于采用7×7邻域搜索,搜索节点增多,大大增加了搜索时间。

本文提出了一种变网格的A*算法,将两者结合起来,在周围没有障碍物时采用3×3搜索邻域快速搜索,遇到障碍物时自适应为7×7搜索邻域,搜索出拐点少,更加平滑的路径,通过障碍物时再采用3×3搜索邻域快速搜索。具体算法流程如下:

1)程序开始,创建OPEN表与CLOSE表,将起点放入CLOSE表中。

2)搜索周围8个子节点作为下一预备子节点放入OPEN表中,根据评价函数计算代价最小的节点放入CLOSE表中作为子节点,继续搜寻下一节点。

3)若周围8个子节点出现障碍物,则打断搜索8个子节点的函数,进入搜索48邻域的搜索函数,将上一父节点作为父节点,搜索周围48个节点作为预备节点,放入OPEN表中,根据评价函数计算代价最小的节点放入CLOSE表中作为子节点继续搜索。否则执行2)。

4)判断CLOSE中新的子节点是否为终点,若是程序结束,否则返回2)。

5)已找到路径,程序结束。

2.2.2 改进评价函数

2.2.2.1 改进评价函数计算方式

传统A*算法计算代价的方式常采用欧式距离,即两个节点间的距离。所以2.1节中函数g(n)和h(n)分别为

(2)

(3)

其中g(n)是指起点到当前节点的代价,本文做出改进,借用上一节点的g(n)值加上一节点到当前节点的代价来表示。具体计算如下

(4)

由于父节点Pn-1的g(n-1)是已知的,所以只需计算父节点到当前节点的代价m(n),计算m(n)运算量是一定小于g(n)的计算量。因此,在规划中,此举可节省大量计算时间。具体计算方式如下

(5)

因此,新的起始节点到当前节点n的真实代价值为

G(n)=g(n-1)+m(n)

(6)

h(n)是指当前节点到目标点的代价,本文不采用常用的欧式距离,而是采用曼哈顿距离,曼哈顿距离是指两点之间横、纵坐标差的绝对值之和。因此欧式距离的值是大于实际距离的,即评价函数中的h(n)的比重是较大的,搜索启发性更强,搜索时间较快。对于汽车来说,显然是更容易接受的。具体计算方式如下

H(n)=|xg-xi|+|yg-yi|

(7)

2.2.2.2 变换函数评价权重比例

由2.1可知在评价函数中g(n)比h(n)比重大时会导致评价函数f(n) 的启发式信息比较弱,可生成更加优化的路径,即与2.2.1小节的扩大搜索邻域相一致。但这些直接导致路径搜索工作量的增加,所以当上文变化网格时,由于扩大搜索邻域使搜索时间变长,此时略微调小h(n)比例,使路径进一步优化的同时保证时搜索效率。由于g(n)比h(n)比重小时,评价函数f(n)的启发式信息强烈,尽管减少了路径搜索的工作量,但规划的路径一般不是最佳,但是在没有障碍物时变换网格时采用3×3搜索邻域,且调整g(n)比例远小于h(n),快速搜索找寻路径。

sin2θ+cos2θ=1

(8)

本文构建的两权重系数如式(8)所示,在式(8)中θ为自变量,取值范围为[0,pi/2],两权重之和为1,当θ从0到pi/2增大时,第一项权重会逐渐增大,第二项权重会逐渐减小,二者成反比关系。将权重系数引入A*算法评价函数中,可调节g(n)和h(n)比例。

F(n)=sin2θ×G(n)+cos2θ×H(n)

(9)

当2.2.1小节中A*算法变换网格时,同时给出信号对评价函数的权重比例也加以变换。当以3×3搜索邻域,快速搜索时,此时将自变量设为较小值。当以7×7搜索邻域,搜索更加优良路径时,将自变量设为略小值,保证路径平滑的同时也保证搜索效率。最后经过加权求平均处理取得合适值,使在有障碍物时,搜索较优路径,无障碍物时快速搜索路径,到达终点。

3 路径优化

3.1 提取关键节点

针对A*算法有多余的节点问题,采取提取关键节点算法,删除冗余点处理,使路径更短,转折点更少,具体步骤如下:

1)将变网格的A*算法规划路径的所有节点放入一个集合P{P1,P2,P3,…,Pn(n为所有节点个数)},P1是路径规划的起点,Pn是路径规划的终点。

2)创建一个集合U,将P1,Pn放入集合U。

3)从P1开始作直线连接P2,判断直线P1P2是否经过障碍物,若没有经过障碍物,连接P1P3,判断是否经过障碍物,若经过障碍物,则P2为关键节点,放入集合U,连接P2P4,以此类推,直到遍布所有节点。

4)依次连接集合U中所有节点,完成对关键节点的提取。

3.2 可调节圆弧优化

由于受汽车本身结构问题限制,当汽车转弯半径过小时,汽车是无法转弯的。所以对路径进行圆弧优化处理时,当转弯半径较小时,汽车是无法通过的,因此圆弧半径的选取十分重要。

本文选取汽车最小转弯半径作为圆弧优化算法的半径,就可以避免无法转弯问题。由于不同的汽车最小转弯半径不同,且汽车最小转弯半径与汽车宽度无关,与车长和汽车最大转角有关。所以根据不同车型选取不同的半径,具体计算如下

(10)

其中R为汽车最小转弯半径,L为车长,Ψ为汽车方向最大转角。

由于A*算法所规划的路径转折处不够平滑,机器人驾驶汽车进行行驶时,路径不够平滑。因此,需要对所规划的路径进行平滑处理,用圆弧代替转折点,其原理如图3所示:

4 仿真研究

4.1 环境建模与膨胀化处理

4.1.1 环境建模

栅格法是一种经典的环境建模方法,是A*算法常用的建模方法。其主要原理是将机器人的工作环境模拟为相同大小且相互连接的栅格,每个栅格对应相应位置信息。

如图4所示,建立一个30×30的地图仿真栅格图。每个栅格长度为汽车车长的正方形,障碍物不满一格按照一格计算。红色为起点,绿色为终点,黑色为障碍物,白色栅格表示可以通行的区域。

4.1.2 膨胀化处理

由于在仿真环境中将机器人驾驶的汽车理想化成一格,转弯时可能会在边缘和障碍物发生碰撞,所以在构建栅格地图时将障碍物进行膨胀化处理,膨胀距离的选取如图5所示。

本文定义了一种膨胀化距离RX,根据汽车转弯时的三角函数关系推导,具体公式如下:

(11)

其中R为上文的最小转弯半径,即图中的OM,d为汽车的宽度,YM为轮距,Ψ为汽车方向最大转角,k为安全系数,根据汽车的实际工况进行选择,取值范围为1～2。膨胀距离的确定可以保证汽车与障碍物间的安全距离,提高路径的可行性。

利用膨胀化距离将障碍物一定距离外设为不可行区域,由于U形和L形障碍物凹进去区域依旧不可行或降低平滑度,亦将其设为不可行区域,具体如图6所示。

4.2 仿真对比

4.2.1 A*算法仿真

为了验证变网格A*算法的有效性,在Matlab 2019b环境下对提出的算法进行仿真验证。构建50×50的栅格地图场景,栅格地图每格边长为1米(汽车车长为1米)。由于车长宽为1m×0.51 m,汽车方向最大转角为30°,轮距为0.44m。安全系数k选取为1.6,由式(11)可得膨胀化的距离为0.5m。其中黑色为障碍物、白色为可行区域,红色为起点,坐标为(1,1)、绿色为终点,坐标为(49,49)。将传统的A*、7×7A*算法与变网格A*算法进行对比仿真,其仿真结果如图7所示。

如图7所示,图中蓝绿色路径为3×3邻域搜索的路径,紫色路径为7×7邻域搜索的路径,黑色路径为变网格算法搜索的路径。取每个算法10次运行时间,进行均值处理,记录结果如表1所示。

表1 改进A*算法时间对比

从表1可以得知。变网格A*算法的路径长度比传统算法多了1.61%,比7×7A*算法多了0.61%。总转折角度比传统A*算法优化了13.95%,比7×7A*算法多了10.63%。但是寻路时间相比传统A*算法优化了61.23%,比7×7A*算法优化了66.96%。如果路径转折角度少,路径更加平滑,机器人操作汽车时,动作就更加简易,所以对于机器人驾驶来说,牺牲部分距离换来更好的驾驶体验是可行的。

4.2.2 路径优化仿真

本文在第3节中对路径进行了优化,首先剔除A*不重要的节点,只留下必要的节点。然后对路径转折点进行可调节圆弧优化,具体结果如图8和表2所示。图中紫色为优化之前的路径,黑色为优化后路径,从图8和表2可以得知优化之后的路径在拐点处变为了弧线且是根据驾驶汽车最小转弯半径设置的半径,提高了机器人驾驶汽车的可操作性。同时3个场景路径长度也有所优化。

表2 路径优化仿真

4.3 实验验证

为了验证本文改进的A*算法具有一定的实效性,本文采用25自由度的NAO机器人驾驶微型宝马Z4电动车在走廊进行实验。NAO机器人身高0.58m,重量为5.4kg,宝马Z4电动车长宽为1m×0.51 m,汽车方向最大转角为30°,由式(10)可得最小转弯半径R为1m,膨胀距离亦采取仿真的0.5m。

实验环境为9m×5m的矩形区域,栅格地图每格边长为1m,设起点坐标为(1,1),终点坐标为(9,5),障碍物坐标为(5,1)和(6,5)。实验中,4个椅子组成边长为1m的方形障碍物,胶带区域为安全区域,紫色路线为规划路径。实验结果表明,NAO机器人驾驶微型宝马电动车能够安全到达目标点。因此可以证明算法的可行性。具体结果如图9所示。

机器人驾驶部分是由matlab输出仿真结果,choregraphy软件通过仿真路线,将机器人控制方向盘和踩油门的动作集成为一个模块化包。由于篇幅原因,驾驶部分不做详细介绍,流程如图10所示。

图1 变网格A*算法

图2 最小转弯半径

图3 圆弧优化原理示意图

图4 环境建模

图5 膨胀距离选取

图6 膨胀处理

图7 算法仿真结果

图8 优化仿真结果

图9 仿真环境与实验环境

图10 机器人驾驶过程

5 结论与展望

本文提出一种基于变网格的A*算法,该算法根据不同情况自适应不同的搜索方式。为了解决驾驶过程中可能出现的边缘碰撞问题,对障碍物进行膨胀化处理,留出安全距离,保证汽车行驶的安全性。同时对路径进行关键节点和圆弧处理,增加搜索效率的同时也使路径更加平滑。为了验证算法的有效性,在Matlab2019b的仿真环境下,对比传统A*算法和7×7的A*算法,变网格A*算法结合了两种算法的优势,使其可以快速搜索的同时也使路径更加平滑。最后采用NAO机器人驾驶微型宝马Z4电动车进行实验验证,证明了算法的可行性。

本文算法尚不能处理突然出现的动态障碍物,未来将对这一问题进行探索,且结合机器视觉,对更为复杂条件下的路径规划进行研究。