一般来说，路径规划问题分为全局避障和实时避障(或称局部路径规划)。全局避障是指在障碍物完全已知的环境中，寻找可行路径。该领域已有许多较为成熟的算法，如人工势场法、A*算法[3]及多种智能避障算法[4-7]等。实时避障算法是指在进行路径规划的过程中，根据传感器感知到的环境信息，对地图进行实时更新并连续规划得到一条合乎要求的路径[8]。与全局避障相比，实时避障的应用范围更广，国内外的许多****已对实时避障做了大量研究。
现有的实时避障算法包括人工势场法、模糊控制法、向量场直方图(VFH)法、人机合作、圆形扩张(CSE+)法等。其中，人工势场法通过引入与障碍物间的斥力来防止碰撞，但易陷入局部最小值而无法运动到终点[9]；利用Q学习算法进行路径规划，通过多次试错，获得最优轨迹。而后又出现了多种改进方法提高算法速度、改进路径冗余问题[10-12]，但仍存在实时性差、运行时间受障碍物数量影响的问题；模糊控制法的实时性较好，但易陷入U型死锁。之后人们对利用模糊控制法进行实时避障的改进在一般环境中的性能不好[13]，存在路径冗余问题[14]。VFH法[15]以机器人为中心建立一维直方图表示不同方向的行进代价，从而确定前进方向。之后的改进版本包括VFH*、VFH+[16-17]，考虑不同尺寸的机器人，并通过在算法中进行前期验证，减少陷入死胡同的风险。但在障碍物分布稀松的环境中，算法对至多有3个候选方向[18]，会导致更优路径丢失。张帅等[19]提出利用人机合作来保证实时避障过程中的安全性，但该方法的自主性差。圆形扩张(CSE+)法[20]通过圆形的不断膨胀在递归过程中连续扩展圆扇形区域获得安全路径，当障碍物为点时效果很好。随后提出的CSE+法[21]，考虑机器人尺寸，可在圆形障碍物之间寻找可行路径。这2种算法在障碍物非常密集时效果较好，但障碍物分布松散时，只有一种选择的方向，会造成更优的路径丢失。
基于上述研究，本文提出了融合鸽群优化算法和CSE+法的实时避障算法，利用鸽群优化算法在障碍物分布稀松，安全范围内寻找下一时刻位置从而确定行进方向，使得所提算法可找到当前位置的最优路径，在保证安全性的同时，大大改进在障碍物分布稀松时规划的路径。
1 在线避障算法的实现 本节介绍基于鸽群优化算法的二维实时避障算法的实现方法，并对CSECSE+法与鸽群优化算法的实现过程进行了介绍。
1.1 CSE+法 CSE+[2]法的基本思想为：利用圆的扩张寻找并避免障碍物，如图 1所示。在由圆形障碍物随机填充的环境中找到到达目标位置的可行路径。通过寻找障碍物并从障碍物构成的通道中穿过，根据与目标的方位角选择向左或向右通道，重复这一过程而不断接近目标位置。
 |
| 图 1 CSE+法避障原理 Fig. 1 Obstacle avoidance principle of CSE+ method |
| 图选项 |
图 1显示了被障碍物包围的机器人，图 1(a)中机器人从起点开始膨胀寻找障碍物，其中蓝色箭头转向为圆的膨胀方向；图 1(b)中沿与障碍物1的圆心方向膨胀确定障碍物2；图 1(c)中穿过障碍物1、2构成的通道继续向前膨胀确定障碍物3；图 1(d)中进行左右转向选择，转向后继续膨胀确定新的障碍物，其中虚线为障碍物间构成的通道。
1.2 鸽群优化算法 研究表明，鸽子可通过地磁场信息、太阳高度信息和地标信息这3个导引工具轻松归巢。受这一现象启发，段海滨和乔沛鑫[21]于2014年提出了鸽群优化算法，经验证，该算法可有效解决数值设计、参数优化等问题[22-23]。
在鸽群优化算法中，第i只鸽子的位置可表示为Xi，速度为Vi，设路径分为n-1段。
 | (1) |
 | (2) |
鸽群优化算法中，利用地图罗盘算子与地标算子分别进行迭代更新以优化鸽群的速度和位置，最终选择获得种群中的最优信息。
地图罗盘算子迭代更新的方法为：以第i只鸽子在第t次迭代中为例，设速度为Vit，位置为Xit，当前最优鸽子位置为Xgt[24]。
 | (3) |
 | (4) |
式中：R为地图罗盘算子；rand为[0, 1]间的随机数。
地标算子的更新方法为：根据适应度值大小将不同鸽子得到的路径进行排序。具体的迭代更新方法如下：以第i只鸽子在第t次迭代中为例。
 | (5) |
 | (6) |
 | (7) |
式中：Nt为第t次迭代种群数量；Xct为第t次迭代后的中心位置；ff(Xit)为Xit位置的适应度值。
1.3 融合鸽群优化算法的二维实时避障算法 在该算法中，用一个半径为Rv的圆来拟合移动机器人。与一般的CSE+[2]法相同，将以圆形障碍物作为输入，忽略环境、数据通信等影响。从移动机器人的当前位置出发到达目标位置，在前进的过程中利用传感器感测环境中的障碍物，并表示为半径不同的圆。从出发穿过2个障碍物后，每次由最近的3个障碍物作为安全范围，安全范围表示方法如图 2所示，由虚线围成的三角形为当前安全范围。
 |
| 图 2 安全范围示意 Fig. 2 The graph of current safe area |
| 图选项 |
当前障碍物分布密集时，即安全范围较小时，按照原始CSE+[2]法，根据Apollonius相切问题确定相切圆，移动机器人运动到相切圆圆心。否则将利用鸽群优化算法在安全范围内寻找下一目标位置。而后进行转向判断，膨胀寻找新的障碍物。不断重复这一过程，直到机器人运动到达终点。
由于机器人有一定的半径，对可通行宽度有一定要求。在转向选择时需对通道宽度进行判断，判断方法如下：
 | (8) |
 | (9) |
 | (10) |
 | (11) |
 | (12) |
 | (13) |
式中：L13和L23分别为障碍物1、3和障碍物2、3构成的通道；(x1, y1)、(x2, y2)和(x3, y3)分别为障碍物1、2、3的圆心坐标；r1、r2、r3分别为障碍物1、2、3的半径；Lw为预设可通行的最小宽度。
当满足式(10)时，选择障碍物1和3所构成的通道前进；满足式(11)时，选择障碍物2和3所构成的通道前进；满足式(12)时，根据文献[12]中所提供的方法，根据与目标位置的方位角进行转向判断；满足式(13)时，机器人将掉头，尝试上一次转向中的另一通道，而算法将返回至搜索树的上一级。
搜索树的建立与使用方法如下：
移动机器人每到达一个位置，将存储障碍物1、2、3的信息、机器人的位置信息以及转向判断信息到搜索树中。当转向判断满足式(12)时，转向选择结果记为“1”，表示存储在搜索树的左分支，可以选择另一方向通行；当转向选择满足式(10)或式(11)，转向选择记为“0”，表示存储在搜索树的右分支，不可选择另一方向通行。若算法需要返回搜索树上一级时，需返回到转向选择为“1”的那一级，选择与转向判断计算相反的通道，并将转向结果更改为“0”。搜索树使用方法如图 3所示，其中数字表示转向选择，箭头表示返回方向, 红标数字表示转向更改。
 |
| 图 3 搜索树返回示意图 Fig. 3 Schematic diagram of return of search tree |
| 图选项 |
下面将给出融合鸽群优化算法的二维实时避障算法工作的具体流程，如图 4所示。图 4中，n1、n2、n3分别为障碍物1、2、3；SA为当前安全范围的面积，A表示安全范围；Sref为衡量安全范围面积大小的指标；(xc, yc)为与3个障碍物相切的相切圆圆心；Xp为当前最优位置；Xg为全局最优位置；M和P分别为存储障碍物信息和路径的搜索树；Nc1max、Nc2max分别为地图罗盘算子和地标算子的最大迭代次数；X和V分别为鸽群的位置和速度；d为下一目标位置；J为成本。融合鸽群优化算法的二维实时避障算法具体实施步骤如下：
 |
| 图 4 基于鸽群优化算法的实时避障算法流程图 Fig. 4 Flowchart of real-time obstacle avoidance algorithm based on pigeon-inspired optimization |
| 图选项 |
步骤1??从起点出发向前膨胀直到找到障碍物1，如图 1(a)所示；沿障碍物1与起点方向延长线进行膨胀确定障碍物2，如图 1(b)所示，若构成的通道不可通行，即L12 < Lw，调整膨胀方向重新寻找障碍物2。确定障碍物1、2后执行步骤2。
步骤2??从障碍物1、2所构成的通道中穿过进行膨胀寻找障碍物3，如图 1(c)所示。并确定由障碍物构成的安全范围A的大小。当SA < Sref时进行步骤3，否则将进行步骤4。
步骤3??经过障碍物1、2所围成的通道到达与3个障碍物相切的相切圆的圆心，后执行步骤7。
步骤4??初始化鸽群优化算法的参数，如空间维度D、地图罗盘算子R、种群数量N、最大迭代次数Nc1max、Nc2max。并确定鸽群优化算法的适应度值计算函数ftc，随机初始化鸽群的速度与位置，执行步骤5。
步骤5??以安全范围重心作为现有最优结果，运算地图罗盘算子，多次迭代更新(直到迭代次数Nc1=Nc1max)鸽群的速度V与路径X。根据适应度值Zf的大小确定每只鸽子的最优位置Xp，以及全局最优位置Xg。
步骤6??根据适应度值淘汰20%的鸽子，再利用地标算子进行迭代更新直到迭代次数Nc2=Nc2max)，调整鸽群的速度V与路径X，以当前全局最优Xg作为下一位置，并移动到这一位置，而后执行步骤7。
步骤7??判断是否可直接达到目标位置，不可以时进行步骤8。
步骤8??记录当前位置与障碍物信息至搜索树中。
步骤9??进行转向判断。若可进行转向，选择构成当前通道的2个障碍物作为新的障碍1、2，并进行步骤3；若不可转向，将进行步骤10。
步骤10??标记当前通道不可行，机器人掉头返回上一层搜索树，选择另一条通道，若仍不可行，则机器人再一次掉头并返回搜索树更上一层再次重新执行步骤10；若可行将进行步骤2。
在步骤4中，鸽群优化算法中参数的确定方法为：在二维空间内，空间维度D的取值为2；据多次实验测试确定取R的值为0.5时效果较好；为保证该算法的实时性，种群数量与最大迭代次数取值不应过大，一般N取值范围为[50, 200]，Nc1max取值范围为[40, 100]、Nc2max的取值范围为[10, 25]，当障碍物分布疏松时可取较大的数值。
将利用适应度函数ftc计算得到的适应度值作为目标位置的成本J，在使鸽群优化算法中以成本最小为指标选择下一目标位置。将考虑包括障碍物对机器人产生的威胁成本，以及为得到更平滑的轨迹将考虑机器人的转弯成本。
在进行下一目标位置选择时，应先保证移动机器人的安全性，在进行路径规划时不能进入受当前障碍物影响的区域，否则将威胁值取为无穷大。而当移动机器人不在受障碍物影响的禁区时，机器人距离障碍物越远越安全。当前检测到的每一个障碍物对该点的威胁值计算方法如下：
 | (14) |
式中：ft(j)为第j个障碍物对当前点的威胁值；Rv为移动机器人半径；Ldj为当前点与当前位置的线段与第j个障碍物的距离；Roj为第j个障碍物的半径；Ztkj为障碍物的威胁因子；No为当前检测到的障碍物数量。
当前已检测到的障碍物对该点产生的威胁成本Jo的计算方法为
 | (15) |
其中转向角成本Jθ的计算方法为
 | (16) |
式中：Zθ为运行到该点时所需转向角。
因此，该点的成本为
 | (17) |
2 仿真和实验 在本节中利用对所提算法的仿真，在圆形障碍物分布的环境中对融入鸽群优化算法前后实时避障算法性能进行测试，并对所提算法在狭长通道中的路径规划能力与死角检测进行验证。
2.1 鸽群优化算法融入前后对比 以半径为0.05 m的移动机器人在圆形障碍物随机填充的环境中从出发点(-50, 0)m找到能到达目标位置(60, 0)m的路径。其中黑色填充圆形为障碍物，红色与蓝色轨迹分别为融入鸽群优化算法前后的移动机器人的路径。
图 5为障碍物分布密集时，搜索得到的路径。图 6为障碍物分布疏松时，搜索得到的路径。表 1为在不同障碍物分布的环境下，融入鸽群优化算法前后搜索得到的路径对比。在该算法中以平均转向角来衡量路径的曲率。
 |
| 图 5 障碍物分布密集时路径 Fig. 5 Circumatances when obstacles are densely distributed |
| 图选项 |
 |
| 图 6 障碍物分布疏松时路径 Fig. 6 Circumstances when obstacles are loosely distributed |
| 图选项 |
表 1 不同环境下2种算法参数对比 Table 1 Comparison of two methods' parameters in different environments
| 障碍物分布 | 算法 | 路径长度/m | 平均转向角/(°) |
| 密集 | PIO & CSE+ | 117.34 | 40.97 |
| CSE+ | 125.11 | 52.59 | |
| 疏松 | PIO & CSE+ | 95.19 | 35.22 |
| CSE+ | 113.50 | 61.76 |
表选项
从图 5(a)中可以得到，在障碍物分布密集的环境下，直接利用CSE+法可以规划得到较好的路径，但路径中仍存在一些不必要的凸起，而造成路径的冗余。而在图 5(b)中，融合鸽群优化算法后可在原路径上进行优化，避免部分凸起，减少冗余与转弯次数。从图 6(a)中可以得到，当障碍分布较为疏松时，直接利用CSE+法得到的路径，不必要的凸起与路径冗余较多，而在图 6(b)中几乎可以修正所有不必要的冗余。与图 6(a)得到的轨迹相比，图 6(b)中的转弯次数显著减少。
从表 1中可见，融入鸽群优化算法后进行路径规划的结果，路径长度更短，平均转向角更小。其中在障碍分布密集时，融入鸽群优化算法后路径长度减少到原来的93.78%、平均转向角减少到原来的77.90%；而当障碍物分布疏松时，路径长度减少到原来的83.86%，而平均转向角减少到原来的57.03%。
仿真结果显示，融入鸽群优化算法后得到路径冗余度减少且更平滑，在障碍物分布疏松时效果尤其明显。
2.2 长廊运行与死角检测 为验证算法在长廊中的通行能力，设计如图 7所示的障碍物分布。利用算法半径为0.01 m的移动机器人，搜索得到出发点(0, 0)m找到能到达目标位置(50, -60) m的路径。
 |
| 图 7 长廊检测 Fig. 7 Test in corridor |
| 图选项 |
测试结果表明，所提算法能在长廊中通行，且可检测并避免死角。利用所提算法可有效避免陷入U型死锁，且可实现在连续障碍环境进行路径规划，这为算法在室内环境中的应用提供了可能性。
3 结论 本文提出的实时避障算法在原CSE+法的基础上巧妙地融入智能优化算法，可适用于利用圆形物体进行建模的移动机器人在障碍物周围找到可通行路径，如以树木石头为主的障碍物。此外，可通过将墙壁等其他不规则障碍物视为由多个小的圆形障碍物组成，而实现室内的实时避障。
根据测试结果显示，与原算法相比，本文算法能找到更好的路径。算法的优点如下：
1) 可避免大量路径冗余。
2) 寻找得到的路径更平滑。
3) 避障机器人的转向次数大大减少。
4) 可有效避免陷入U型死锁，可应用于连续的障碍物环境。
参考文献
| [1] | VILSON G, ANGALID K.Spatial and reactive navigation for an autonomous vehicle in an unknown environment[D].Sweden: Ume? Universitet, 2011. |
| [2] | R?NNB?CK S, WESTERBERG S, PROROK K.CSE+: Path planning amid circles, 2009[C]//The 4th International Conference on Autonomous Robots and Agents.Piscataway: IEEE Press, 2009: 447-452. |
| [3] | 王海群, 王水满, 张怡. 基于激光雷达信息的无人机避障控制研究[J]. 激光杂志, 2019, 40(12): 76-79. WANG H Q, WANG S M, ZHANG Y. Control of UAV barrier avoidance based on lidar information[J]. Laser Journal, 2019, 40(12): 76-79. (in Chinese) |
| [4] | AZIMIRAD V, SHORAKAEI H. Dual hierarchical genetic-optimal control: A new global optimal path planning method for robots[J]. Journal of Manufacturing Systems, 2014, 33(1): 139-148. DOI:10.1016/j.jmsy.2013.09.006 |
| [5] | MAC T T, COPOT C, TRAN D T, et al. A hierarchical global path planning approach for mobile robots based on multi-objective particle swarm optimization[J]. Applied Soft Computing, 2017, 59: 68-76. DOI:10.1016/j.asoc.2017.05.012 |
| [6] | 王雷, 李明. 改进自适应遗传算法在移动机器人路径规划中的应用[J]. 南京理工大学学报(自然科学版), 2017, 41(5): 627-633. WANG L, LI M. Application of improved adaptive genetic algorithm in mobile robot path planning[J]. Journal of Nanjing University of Science and Technology(Natural Sciences), 2017, 41(5): 627-633. (in Chinese) |
| [7] | 魏彤, 龙琛. 基于改进遗传算法的移动机器人路径规划[J]. 北京航空航天大学学报, 2020, 46(4): 703-711. WEI T, LONG C. Path planning for mobile robot based on improved genetic algorithm[J]. Journal of Beijing University of Aeronautics and Astronautics, 2020, 46(4): 703-711. (in Chinese) |
| [8] | 王雄奇, 柯艳国, 吴贤斌, 等. 基于虚拟墙的移动机器人路径跟踪与实时避障方法[J]. 机械与电子, 2019, 37(11): 76-80. WANG X Q, KE Y G, WU X B, et al. A virtual wall-based path tracking and obstacle avoidance method for mobile robot[J]. Machinery & Electronics, 2019, 37(11): 76-80. (in Chinese) |
| [9] | ROSTAMI S M H, SANGAIAH A K, WANG J, et al. Obstacle avoidance of mobile robots using modified artificial potential field algorithm[J]. EURASIP Journal on Wireless Communications and Networking, 2019, 2019(1): 70. DOI:10.1186/s13638-019-1396-2 |
| [10] | 张宁, 李彩虹, 郭娜, 等. 基于CM-Q学习的自主移动机器人局部路径规划[J]. 山东理工大学学报(自然科学版), 2020, 34(4): 37-43. ZHANG N, LI C H, GUO N, et al. Local path planning of autonomous mobile robot based on CM-Q learning[J]. Journal of Shandong University of Technology (Natural Science Edition), 2020, 34(4): 37-43. (in Chinese) |
| [11] | 高乐, 马天录, 刘凯, 等. 改进Q-Learning算法在路径规划中的应用[J]. 吉林大学学报(信息科学版), 2018, 36(4): 439-443. GAO L, MA T L, LIU K, et al. Application of improved Q-Learning algorithm in path planning[J]. Journal of Jilin University (Information Science Edition), 2018, 36(4): 439-443. (in Chinese) |
| [12] | LOW E S, ONG P, CHEAH K C. Solving the optimal path planning of a mobile robot using improved Q-learning[J]. Robotics and Autonomous Systems, 2019, 115: 143-161. DOI:10.1016/j.robot.2019.02.013 |
| [13] | 魏立新, 吴绍坤, 孙浩, 等. 基于多行为的移动机器人路径规划[J]. 控制与决策, 2019, 34(12): 2721-2726. WEI L X, WU S K, SUN H, et al. Mobile robot path planning based on multi-behaviors[J]. Control and Decision, 2019, 34(12): 2721-2726. (in Chinese) |
| [14] | 郭娜, 李彩虹, 王迪, 等. 基于模糊控制的移动机器人局部路径规划[J]. 山东理工大学学报(自然科学版), 2020, 34(4): 24-29. GUO N, LI C H, WANG D, et al. Local path planning of mobile robot based on fuzzy control[J]. Journal of Shandong University of Technology (Natural Science Edition), 2020, 34(4): 24-29. (in Chinese) |
| [15] | BORENSTEIN J, KOREN Y. The vector field histogram-fast obstacle avoidance for mobile robots[J]. IEEE Transactions on Robotics and Automation, 1991, 7(3): 278-288. DOI:10.1109/70.88137 |
| [16] | ULRICH I, BORENSTEIN J.VFH*: Local obstacle avoidance with look-ahead verification[C]//IEEE International Conference on Robotics and Automation.Piscataway: IEEE Press, 2000: 2505-2511. |
| [17] | ULRICH I, BORENSTEIN J.VFH+: Reliable obstacle avoidance for fast mobile robots[C]//IEEE International Conference on Robotics and Automation.Piscataway: IEEE Press, 1998: 1572-1577. |
| [18] | 江超, 邢科新, 林叶贵, 等. 未知环境下移动机器人静态与动态实时避障方法研究[J]. 高技术通讯, 2019, 29(10): 1012-1020. JIANG C, XING K X, LIN Y G, et al. Research on static and dynamic real-time obstacle avoidance methods for mobile robots in unknown environment[J]. Chinese High Technology Letters, 2019, 29(10): 1012-1020. (in Chinese) |
| [19] | 张帅, 李学仁, 张鹏, 等. 基于人机合作的无人机实时航迹规划[J]. 北京航空航天大学学报, 2017, 43(4): 814-822. ZHANG S, LI X R, ZHANG P, et al. UAV real-time path planning based on human-machine cooperation[J]. Journal of Beijing University of Aeronautics and Astronautics, 2017, 43(4): 814-822. (in Chinese) |
| [20] | R?NNB?CK S, BERGLUND T, FREDRIKSSON H, et al.Circle sector expansions for on-line exploration[C]//IEEE International Conference on Robotics and Biomimetics.Piscataway: IEEE Press, 2006: 1227-1232. |
| [21] | DUAN H B, QIAO P X. Pigeon-inspired optimization: A new swarm intelligence optimizer for air robot path planning[J]. International Journal of Intelligent Computing and Cybernetics, 2014, 7: 24-37. DOI:10.1108/IJICC-02-2014-0005 |
| [22] | CHEN B, LEI H, SHEN H, et al. A hybrid quantum-based PIO algorithm for global numerical optimization[J]. Science China Information Sciences, 2019, 62(7): 33-44. DOI:10.1007/s11432-018-9546-4 |
| [23] | 胡耀龙, 冯强, 海星朔, 等. 基于自适应学习策略的改进鸽群优化算法[J]. 北京航空航天大学学报, 2020, 46(12): 2348-2356. HU Y L, FENG Q, HAI X S, et al. Improved pigeon-inspired optimization algorithm based on adaptive learningstrategy[J]. Journal of Beijing University of Aeronautics and Astronautics, 2020, 46(12): 2348-2356. (in Chinese) |
| [24] | 张亚平, 孙佩华, 李昱辉, 等. 基于改进鸽群优化算法的高超声速飞行器轨迹优化[J]. 飞行力学, 2017, 35(4): 60-64. ZHANG Y P, SUN P H, LI Y H, et al. Hypersonic vehicle trajectory optimization based on improved pigeon-inspired optimization algorithm[J]. Flight Dynamics, 2017, 35(4): 60-64. (in Chinese) |
