基于双粒子群算法的矿井搜救机器人路径规划

封硕，谢廷船，康靖，李建良

(长安大学 工程机械学院， 陕西 西安 710064)

0 引言

矿井发生事故后，往往受灾现场情况复杂，盲目下井救援可能会造成二次伤亡，而且在狭窄环境下也很难找到被困人员[1]。利用机器人救灾为矿井救援带来了新方向。矿井搜救机器人能够探测现场温度、有毒气体和烟雾浓度等，能把环境数据传送给外界，以供救援人员参考，制定合理的援救方案[2]。为了使矿井搜救机器人可以在复杂地形中及时避障并完成环境探测，效率高、距离短的路径规划至关重要[3]。

目前，国内外学者所研制的矿井搜救机器人都具备一定的避障能力，已经将传统的进化算法,如蚁群算法、遗传算法等应用于矿井搜救机器人路径规划中[4-5]。相比这些算法，粒子群算法收敛更快、资源更节省。但是在障碍物复杂多变的矿井里，传统的粒子群算法应用于路径规划还存在一些不足，如易陷入局部最优、迭代后期个体最优解易波动,导致求解精度低、算法前期收敛速度慢等问题，严重影响了矿井搜救机器人路径规划的效率和可靠性[6]。吴高超[7]采用等分可行空间逐段求解的方法建立了矿井搜救机器人避障模型，以步长作圆，以粒子的位置来表示运动角度，再利用粒子群算法对粒子位置进行优化，进而获得更有利的避障角度。但是在障碍物繁多的环境下，该方法成功率较低，未能发挥出粒子群算法搜索能力强的特点。为了解决上述粒子群算法存在前期收敛速度慢、后期求解精度低的问题，本文引入添加非线性学习因子和动态速度权重的方法对粒子群算法加以改进。针对矿井搜救机器人在复杂路况下路径规划成功率低的问题，提出了一种新的路径规划算法，首先将障碍物膨胀化处理为规则化多边形，建立环境模型，再以双改进粒子群算法作为路径寻优算法，当传感器检测到矿井搜救机器人正前方一定距离内有障碍物时，开始运行双改进粒子群算法，然后评估2种粒子群算法得到的路径是否符合避障条件，若均符合避障条件,则选取最短路径作为最终路径，最后得到矿井搜救机器人在整个路况模型中的最优行驶路径。

1 改进粒子群算法

1.1 标准粒子群算法

粒子群优化算法(Particle Swarm Optimization，PSO)是一种模拟鸟群觅食行为的仿生智能优化算法。PSO算法用随机解初始化一群随机粒子，通过迭代找到最优解，在每一次迭代中，每个粒子通过跟踪个体极值和全局极值来更新自己。种群迭代过程中，个体粒子不断更新其在解空间中的速度，以控制个体的飞行方向及距离，不断向个体极值和全局极值方向迭代[8]。

粒子群算法常规的数学模型有速度和位置更新模型，表达式分别为

vk+1(i,d)=ωvk(i,d)+c1r1[qk(i,d)-

xk(i,d)]+c2r2[gk(i,d)-xk(i,d)]

(1)

xk+1(i,d)=xk(i,d)+vk+1(i,d)

(2)

式中:vk(i,d),qk(i,d),xk(i,d)和gk(i,d)分别表示算法第k次迭代时粒子i的飞行速度矢量、个体极值、个体值及全局极值的第d维分量；ω为惯性因子，反映粒子的运动习惯，代表粒子有维持先前运动的趋势；c1和c2为学习因子，也称加速因子，c1值越大,代表该粒子向个体极值逼近趋势越强，c2值越大,代表粒子向全局极值逼近趋势越强；r1和r2为[0,1]范围内的随机数[9]。

1.2 改进的粒子群算法

1.2.1 非线性学习因子法(CPSO)

式(1)中的c1、c2分别表示粒子向个体极值和全局极值的加速能力，过大或过小的学习因子都不利于粒子群算法迭代。本文提出一种随着迭代次数变化而做非线性变化的学习因子。

(3)

(4)

式中：c1max,c1min为c1的最大值和最小值；c2max,c2min为c2的最大值和最小值；n为当前时刻迭代代数；M为总迭代代数。

1.2.2 速度权重法(PPSO)

由式(2)可看出，标准粒子群算法中粒子位置只与前一时刻的位置和当前时刻的速度有关，若算法后期某次迭代速度过快，个体极值则会过度偏离全局极值，结果波动较大，导致求解精度低。本文提出了一种加入速度权重的方法,粒子位置更新公式为

xk+1(i,d)=xk(i,d)+pvk+1(i,d)

(5)

(6)

1.3 算法性能

算法针对不同难易程度的适应度函数得出的数据结果能够直观反映该算法的性能优劣。为了比较2种改进算法在简单和复杂地形下的性能优劣，选用了2种拥有不同复杂度地形的适应度函数(也称为测试函数)做仿真实验[10]。

首先实验利用粒子群算法求取2个函数的最小值，再通过比较求解精度和速度得出算法性能优劣。实验分别以Sphere、Rosenbrock函数作为适应度函数，分别对应f1、f2。

(7)

(8)

(x1,x2,…,xn)为n维坐标系下的点坐标。f1是一个单峰函数，只有一个峰顶，在坐标为(0,0,…,0)处取得最小值0；f2函数在坐标为(1,1,…,1)处取得最小值0，其最小值位置隐藏在一个平滑、狭长的抛物线形山谷地形内，使得算法很难求取该位置[11-13]。

算法种群规模为50，迭代次数为25。每个适应度函数均用标准PSO,CPSO,PPSO三种算法进行实验，仿真结果如图1、图2所示。

图1 Sphere函数寻优实验

设定当迭代过程中算法适应度值在[0，0.01]范围时称为收敛阶段，适应度值在(0.01，+∞)范围时称为初始阶段，初次进入收敛阶段的迭代代数设为z，其对应的适应度值为f。提取图1、图2中z和f的值(表1)，并用z值大小表示迭代速度的快慢。

图2 Rosenbrock函数寻优实验

表1 算法初次进入收敛阶段数据

比较表1中数据可知，针对测试函数Sphere，PPSO收敛速度最快，其次是CPSO，PSO收敛速度最慢；针对测试函数Rosenbrock，收敛速度最快的是CPSO，其次是PSO，而PPSO收敛速度最慢。

针对每个测试函数，使用PSO,CPSO,PPSO算法做30次仿真实验，每种算法运行结束后得到的最终适应度值作为一次仿真实验结果。每个算法在以Sphere,Rosenbrock为适应度函数下均能得出30个实验结果，对这些结果求最大值、最小值、平均值、标准差，结果见表2、表3。

分析表2、表3可得，针对拥有复杂地形的Rosenbrock函数，PPSO得到的最大值、最小值、平均值以及标准差均最小，表明其求得的最优解精度最高、波动最小，寻优效果比PSO和CPSO更好。针对呈单峰地形的Sphere函数，CPSO算法求得的最优解精度最高、波动最小，效果最优，PPSO次之,并且差距较小，标准PSO寻优效果最差。综合来看，CPSO收敛速度快于PSO，PPSO的寻优效果优于PSO。

表2 Sphere函数对3种算法的测试数据

表3 Rosenbrock函数对3种算法的测试数据

具体分析如下：

CPSO中，随着迭代代数的增加，式(3)中c1逐渐减小且下降速率增大，式(4)中c2逐渐减小且增长速率减小。算法前期c1值长久保持在较大值，而c2值较小，促进了粒子向当前个体极值迭代，使得粒子能够发散出去。而到算法后期，c1和c2值与前面情况相反，促进了粒子向当前全局极值迭代，使得粒子更快地向最优值逼近。以上2种情况都能够加快算法迭代速度[14]。

2 障碍物模型建立及路径规划

2.1 障碍物模型建立

在解决矿井搜救机器人路径规划问题中，正确的环境建模对于路径规划和搜救机器人能否完成避障具有关键性的作用[15]。在实际应用中，采用基于激光雷达的同时定位与地图构建(Simultaneous Localization and Mapping, SLAM)技术构建环境地图。图3(a)是扫描出的二维栅格障碍物，图3(b)是将图3(a)中栅格进行膨胀化后得到的规则障碍物。

(a) 栅格化障碍物

(b) 膨胀化障碍物

2.2 路径规划

建立好路况地图后，矿井搜救机器人的路径规划还面临一个难题，就是在相对空旷路段机器人单步行驶距离(步长)大则效率高，在狭窄路段步长小则路径规划成功率高。针对上述问题，提出了基于2个改进粒子群算法(CPSO+PPSO)相结合的路径规划方法。机器人单次避障模型如图4所示。

路径规划步骤如下：

(1) 如图4(a)所示，在t时刻机器人位置处Pt建立坐标系，PtE与x轴正方向夹角为αt。当传感器未检测到PtE方向上有障碍物时，机器人沿着PtE方向行驶。

(2) 当传感器检测到PtE方向上有障碍物时，在极坐标系下建立以步长h为半径的圆(图4(b))，种群中每个个体的值代表着路径的走向。运行CPSO算法，首先初始化种群中的个体值，根据式(1)不断更新个体值，某次更新后的预估位置为K点，当前位置为O点，然后判断OK是否穿过障碍物边界，若没有穿过边界，根据适应度函数f3取小原则选择出粒子群个体极值和全局极值，否则重新分配粒子位置，最终得出避障最优角u。判断最优角u对应的OK路径和障碍物边界是否相交，若不相交，结束当前算法，得出K点的f3，否则赋值K点f3=10 000。CPSO算法适应度函数为

(a) 未检测到障碍物

(b) CPSO算法路径

(c) PPSO算法路径

f3=h+D1

(9)

式中D1为粒子更新后的位置对应的K点到终点E的距离。

(3) CPSO算法结束后，运行PPSO算法。PPSO算法路径如图4(c)所示，在极坐标系中，将机器人运动分解为10步，步长为h/10。具体迭代过程和步骤(2)中CPSO算法相似，不同的是每次更新的是10个个体值,且算法运行一次求出的是10个全局极值。10个全局极值对应10段路径角度，分别为e(1),e(2),…,e(10)，其中,e(1)对应着矿井搜救机器人O-A行驶路线，e(10)对应着I-J路线。则该算法得到的最优路径为O—A—B—C—D—E—F—G—H—I—J。若该路径与障碍物边界不相交，得出J点的f4，否则赋值J点f4=10 000。PPSO适应度函数为

f4=L+D2

(10)

(11)

式中：L为粒子i更新后的位置对应的10段路径长度之和；D2为J点到终点E的距离；θ为粒子i前后2个维度值之差，是一个角度差,θ=x(i,j)-x(i,j-1)。

(4) 比较CPSO和PPSO算法结果对应的适应度值f3和f4，选择适应度值小的路径作为矿井搜救机器人的最终路径。

3 仿真结果与分析

建立井下障碍物地图，对改进的双粒子群算法进行仿真实验。算法程序中参数设置如下：

矿井搜救机器人运动起点为S(60,60)，终点为E(250,250)，步长h=4 dm。

PPSO算法：迭代代数为30，其他参数均与CPSO算法相同。

为了便于分析和观察CPSO和PPSO算法结合的过程，给出了矿井搜救机器人的路径规划仿真以及2个位置路径选取细节，如图5—图7所示。

图5 路径规划仿真结果

图6中2种算法均完整运行3次，最终矿井搜救机器人路径为A—C—D—E，路径选取流程如下：

(1) CPSO和PPSO分别得出2条路径，即虚线AC和折线AB，由式(9)计算出C点处的f3，由式(10)得出B点处的f4，且f3

图6 图5中第1处路径

图7 图5中第2处路径

(2) CPSO和PPSO分别得出虚线CF和折线CD，由于虚线CF与障碍物边界相交，所以F处f3=10 000，D处f4小于f3，取折线CD作为最终路径。

(3) CPSO和PPSO分别得出虚线DG和折线DE，E处f3小于G处f4，取折线DE作为最终路径。

图7中，地形较为开阔，CPSO和PPSO各运行了6次，实取路径均是CPSO计算生成的虚线线段，机器人行走的路线为A—B—C—D—E—F—G。

根据此次仿真数据得出，矿井搜救机器人按照2.2节步骤(1)走了56步，CPSO和PPSO算法均运行了24次。算法得出的终点为(248.19，248.18)，由于到E点的距离小于机器人步长，因此最后一步是直达E点。所以，理论路径长度为

L1|dm=(56+24)×4+

(12)

按照Matlab仿真的数据计算出路径长度为325.41 dm，耗时223.35 s。算法中粒子每走一步，需要将粒子极坐标系下坐标转换成直角坐标系下坐标，再加上Matlab数据有效位有限和距离公式误差等原因，导致理论路径长度和仿真计算长度不等，本文均以仿真数据计算的路径长度为准。

分别采用PSO,PSO+PSO和CPSO+PPSO算法进行路径规划仿真,仿真次数为50，测试数据见表4。从表4可知，CPSO+PPSO的最优路径最短、成功率最高、平均路径长度最短，运行时间虽然最长,但是与其他2种算法差距较小。实验结果表明，改进的双粒子群算法和路径规划模型的有效结合，提高了路径规划成功率，缩短了路径长度。

表4 3种方案的测试数据

4 结论

(1) 针对矿井搜救机器人路径规划问题，提出了基于双粒子群算法的路径规划方法。利用基于激光雷达的SLAM技术建立栅格化障碍物模型，再经过膨胀化处理形成规则图形，并投影到二维坐标系中，构建井下环境模型。

(2) 通过改进学习因子和添加动态速度权重提高了粒子群算法的收敛速度，降低了最优解波动幅度。CPSO算法粒子步长大，适用于相对开阔地带寻找路径。PPSO算法粒子步长小，擅长在障碍物形状复杂多变地带寻找路径。2种算法共同寻找路径的方案提高了路径规划效率,在复杂路段中能够寻找到最优路径。

(3) 在构建的环境模型中进行3种算法的仿真实验，结果证明,CPSO和PPSO结合的双粒子群算法提高了路径规划成功率，缩短了路径长度。