复杂山地环境下的机器人路径规划

胡平志，杨小柳，李泽滔

(贵州大学电气工程学院，贵州 贵阳550025)

1 引言

现实中，机器人所处的环境通常是三维的，因此机器人的三维路径规划更具有普遍性和现实性。然而由于三维空间数据量大，导致其路径规划比较复杂。近年来，关于路径规划算法有：人工势场法[1]、模拟退火算法[2]，蚁群算法等。除使用单一方法进行路径识别外，文献[3]提出了一种动态搜索算子，提高算法收敛的效果；文献[4]提出蚁群算法和遗传算法相结合的方法，增加了算法搜索效率；文献[5]引入一些策略，解决死锁问题，增加解的多样性，提高算法的搜索能力;文献[6]通过引入障碍物排斥权重增加新的启发因子修改转移概率，提高蚂蚁避障能力。

不同于空间机器人和水下机器人，地面机器人移动受到地形曲面约束，即机器人被约束在一个曲面上运动，同时又会出现高度上的变化，既要对路径进行规划，同时又要考虑重力约束，虽然文献[7]提出了结合人工势场算法的改进蚁群算法，做出了一种曲面地形下的最短路径，但是算法并未考虑机器人是否有足够牵引力上坡的问题。

该算法引入了一种新的转移概率函数，能够在完成路径规划的同时，兼顾机器人爬坡的问题，由于引进新的转移概率函数，改进算法不再只局限于规划最短路径，也可以规划坡度符合机器人爬坡能力的路径。同时，在实际应用中，相较于常用的栅格地形图[6，7]，等高线地形图数据更容易获取，该算法采用等高线对丘陵山地地形进行分层，使得路径规划分层进行。

2 传统蚁群算法概述

2.1 基本蚁群算法

模拟蚂蚁觅食寻迹的过程，单只蚂蚁携带信息素总量不变，蚂蚁走过的路径越短，单位路程的信息素量越多，经过多次迭代，较短的路径上累积的信息素浓度逐渐增高，选择该路径的蚂蚁个数越多。最终，整个蚁群会在正反馈的作用下集中到最佳的路径上，此时应的便是待优化问题的最优解。蚁群算法中，第t次迭代下，第k只蚂蚁从i点到j点的转移概率如式(1)所示

(1)

其中：dij为i点到j点的直线距离，τij(t)为t时刻i点到j点的信息素浓度；ρ为信息素耗散系数；Δτij为i点到j点的信息素浓度总和；Q为单只蚂蚁所带信息素总量；Lk为第k只蚂蚁单次迭代所走过的路程。

2.2 三维空间的路径规划

三维空间路径优化也需要先建立一个抽象的三维空间地形图，方法如下：

1)利用栅格将三维空间划分为若干小块。

2)填充部分小块，作为障碍物，这些区域不可通行。

不同于二维平面，三维空间中节点的数量远大于二维平面，若不加以处理集合allowk将包含大量元素，将导致以下情况

(2)

使得Pij趋近于0，导致程序运算时间冗长，所以为了避免这一情况，要缩小allowk的范围，常用的方法有：

1)将三维空间进行分层，使得路径规划过程由起点开始向终点逐层递进。

2)对每一层在划分可达域，即第n层向第n+1层转移时，只选取第n+1层中某一个区域，这样就能进一步缩小allowk的范围。

3 山地环境的路径规划

3.1 数字山地地形建模

贵州山地丘陵地占92.5%，为了在这样的特殊地理环境下大力发展包含光伏等新型能源，遂将光伏阵列建在如图1所示的地形下。

图1 山地地形下的光伏阵列

该光伏阵列所处位置除了山顶一条主干道路外，没有其它可供车辆通行的道路，并且该地形起伏程度较小，不存在断崖式地形，且山地上植被低矮，没有高大障碍物，所以非常适合开发机器人进行巡检以减少人力成本。本文基于这一类地形，探究复杂地形下机器人路径规划问题。

为便于仿真计算，该算法利用式(9)所示函数，模拟生成一个拥有三个山峰，以及三个低洼的山地地形。

-10(x/5-x3-y5)e(-x2-y2)

(3)

图2 数字模拟山地地形

其中蓝色五角星代表障碍物。本次寻迹仿真，以水平面为起始位置，以最高顶峰为终点，对于水平面以下部分忽略不计。利用等高线，将山地地形分层；水平线及以上部分高度为7.055共23层，随机在每层插入零点(0，0，0)作为障碍物，如图2所示。在路径优化时，会规避这些障碍点。

3.2 初始位置

曲面内蚁群算法可以针对从一个海拔高度到另一个海拔高度上某一点的寻迹，该算法以山脚作为初始位置，以山顶作为终点。第一轮寻路时随机生成初始点，之后根据信息素浓度随机选取初始位置，第一层第s个初始点位搜寻概率如式(4)所示

(4)

由于路程越长信息素浓度越低，所以随着路程变长初始点位在被选取时的概率就会越小。故经过多次迭代，上山起始点，会趋向于靠近目标点。

3.3 等高线分层

三维曲面类似于空间地形，在研究山地地形寻迹的时候，必须考虑山地地形存在高度变化。由于山地地形的特点，该算法利用等高线将地形分层，每层层高为Δz，使得路径优化可以分层进行，为了减少不必要的运算，加快计算速度，就需要减小allowk的范围，该算法不采用栅格，而是根据山地地形特点，利用等高线将山地进行分层，使得每次转移必须从第n层转移到n+1层，不能跨层转移，集合allowk的范围由任意未经过点，缩小至第n+1层任一点。等高线示意图如图3所示

图3 等高线示意图

其中Δx、Δy、Δz分别表示等高线邻近两条等高线沿x、y、z轴方向上的增量。在Δz恒定不变的情况下，dij越长坡度角θ会越小，即对机器人爬坡能力要求也越低。

3.4 曲面上两点间的距离

在曲面上两点的最短距离被称为“测地线”，尽管数学上可以结合曲面方程求取“测地线”，但是山地地形不存在确定的曲面方程，考虑到当|Δx|≪ε，|Δy|≪ε，|Δz|≪ε时有式(5)关系(其中ε为某个非零的极小值)

(5)

所以该算法采用短距离近似曲面上两点间的距离。同时利用dij

3.5 坡度函数

该算法将山地地形抽象为一个三维曲面，不同于二维平面，由于在登山时，机器人上坡能力，如果坡太陡，机器人牵引力不足，则会出现打滑现象。该算法引入一个与坡度有关的函数，如式(6)所示

(6)

该函数描述了，机器人每移动(Δx2+Δy2)0.5的水平距离所上升Δz的高度，函数值越大说明坡度越陡，对机器人爬坡能力要求越高。因为山地地形并不是确定函数关系，所以采用近似量Δx、Δy、Δz来表示，而不适用变分进行计算。同时引入与坡度相关的启发函数[gij(t)]γ=f(Δx，Δy，Δz)-γ。其中γ为坡度函数因子。

3.6 适应度函数及约束条件

最佳个体适应度是体现优化度的重要指标。该算法以路径总长度为优化目标，每只蚂蚁完成搜索，都会记录下该个体所走过的路径长度，并用评价函数记录总长度，得到最短路径，同时也会通过引入的坡度函数，使得该最短路径上所以坡度满足约束条件。评价函数以及约束条件如式(7)所示

(7)

其中文献[16]中机器人最大爬坡角度为θmax=25.6°，tan(θmax)≈0.479，文献[17]，机器人翻越障碍物时俯仰角为68°，tan(θmax)≈2.475，文献[18]中机器人最大爬坡角度为θmax=20°，tan(θmax)≈0.364，由此，即使以最大值作为参考，机器人最大爬坡坡度θmax<68°，tan(θmax)<2.475。

3.7 改进后的节点转移概率函数

改进后，三维曲面上的节点转移概率函数为

(8)

图4 改进蚁群算法流程图

3.8 坡度函数验证

为验证γ变化可以改变坡度大小，通过仿真得到结果如表1所示

表1 γ变化与坡度变化

当γ值由1增加到3.5时tan(θ)方差、均值以及峰值逐渐下降，由此表明引入的坡度函数可以有效地改变路径的坡度，从而使得四足机器人可以适应被规划路径得坡度。当γ=3.5时tan(θ)的值均低于2.475，爬坡角度θ均小于68°，在机器人爬坡能力允许范围内，所以在规划最优路径时取γ=3.5。

在上升高度相同的前提下，γ越大，所规划路径坡度越小路程越长，反之γ越小，坡度越大，路程也会越短。为确定γ变化时对路径长度的影响，进行仿真，得到结果如图4所示。

图5 γ值对收敛效果的影响

同样150只蚂蚁，20次迭代，随着γ值逐渐增大，路径长度逐渐增大，同时由图4结合表2可以看出，随着坡度因子γ减小坡度不断减小，但是算法收敛速度下降，当γ>0时会严重影响算法收敛性。

3.9 改进信息素函数

(9)

为验证改进后的算法可以使得算法加速收敛，以路程平均值作为y轴，以迭代次数作为x轴，令γ=3.5，分别测试偏差因子ν=2、4、6时对于算法收敛速度的影响，经过20次迭代，收敛情况对比如图6所示。

图6 ν值对最优路径的影响

由图6可知，该改进算法可以有效提升算法收敛效果，当ν=6时能得到更好的收敛效果，所以确定参数取值参数ν=6。

4 算法仿真及结果分析

4.1 算法仿真

以表2作为算法初始条件进行仿真。

表2 算法初始条件

在文献[5，16]中，蚁群算法种群大小通常不超过50，但是在做仿真时发现，种群数量太少难以达到理想效果，因此设置如表初始参数，以N只蚂蚁平均路程为y轴，以迭代次数为x轴做了多次实验得到如下折线图

图7 种群大小对收敛结果的影响

如图7所示，当种群数量N=150时收敛效果较好，故该算法中所算法方法均设置种群数量为150。

4.3 路径优化方案

机器人由山底到达山顶，路径越短，坡度越陡，运输相同负载，攀爬所需的牵引力越大。反之路径越长，坡度越缓，运输相同负载攀爬所需牵引力越小。坡度平缓和路径最短是相互矛盾的。以下将路程最短路径简称为最短路径，将最适坡度路径简称为最优路径。下文将着重对比二者的路径长度以及坡度陡峭程度。

4.3.1 最短路径

本节基于原始蚁群算法规划出最短路径，迭代条件如表2最短路径参数为初始条件，其中取ν=6，γ=-3如图8所示。

图8 最短路径仿真效果

其中图8(a)为最短路径三维图形，中图8(b)为最短路径的俯视图。如图所示，本次规划路径直接由水平面上某一点，直接到达山顶，多次仿真结果如表3所示。

表3 最短路径仿真结果

最小路程收敛于7.7附近，但是这一路径坡度极大，坡度最大值达到8.216，坡度角达到83°，远超过机器人爬坡能力极限。所以尽管路径最短但是不符合实际情况。

4.3.2 机器人爬坡能力允许范围内的最优路径

图9 最优路径仿真效果

其中图9(a)为最优路径三维图形，图9(b)为最优路径的俯视图。如图所示，路径不再是直接行进，而是在保证坡度不超过机器人爬坡能力的前提下，盘山前行。

表4 最优路径路程仿真结果

对比可明显看出，尽管路径曲折，路程较最短路径更长，但是始终保证tan(θ)<2.475，即上坡角度θ<68°，符合机器人爬坡能力。

图10 最短路径与最优路径坡度对比

如图10所示，改进后算法所规划最优路径的坡度相较于最小路径的坡度更小了，该算法确保了所规划路径符合四足机器人爬坡能力。

5 结论

本文提出了一种加入了带有坡度函数的蚁群算法。由于引入了坡度函数，使得该算法在搜寻最优路径时，能充分考虑到机器人有限的爬坡能力问题。

针对于引入坡度函数后的蚁群算法存在收敛速度慢以及难以收敛到最优路径的问题，本文进一步改进了信息素浓度计算方法，使得该算法快速收敛，更好地适应复杂地形。通过仿真及结果分析表明，相较于原始的蚁群算法，该算法能更快的搜索到更优的解。该算法为三维曲面上的路径规划提供了一种快速有效地解决问题的方法。

这一改进算法所得到的路径，充分地考虑了机器人在复杂地形下的爬坡能力，相较于以往的路径规划算法更具有实用价值。