一种改进PSO的室内机器人路径规划方法

李 玽，吴丹丹，赵征凡2，王晓华，张 蕾

(1.西安工程大学 电子信息学院，西安 710048;2.工业与信息化部电子第五研究所 可靠性数据中心, 广州 510610)

0 引言

路径规划是机器人研究中的关键基本问题，旨在具有障碍物的环境中，按照预定义的评价标准，寻找一条从起始位置到目标位置的无碰撞最优(次优)路径[1]。近年，仿生智能算法是机器人路径规划中应用较为广泛的方法，常用的有：微分进化算法[2]、模拟退火算法[3]、遗传算法[4-5]、蚁群算法[6-7]、人工鱼群算法[8]、蜂群算法[9]、粒子群算法( particle swarm optimization，PSO)[10-11]等。众多算法中，PSO具有收敛速度快、设置参数少、实现简单等优点，但同时也存在收敛度低、易早熟、鲁棒性差等缺陷，影响了路径规划的运行效率和可靠性[12]。

目前，针对PSO的缺陷，国内外学者和研究机构提出了较多的改进方法。文献[13]提出极坐标建模和PSO，改善了算法的收敛性，但极坐标系和直角坐标系转换增加了算法的计算量，降低了路径的运行效率；文献[14]提出的利用粒子个体极值的加权平均值，同时加入惯性权重的PSO方法，在一定程度上改善了算法的全局和局部搜索能力，但算法依然存在早熟的问题；文献[15]提出将差分算法引入PSO，避免了PSO早熟的问题，但算法收敛度低，路径规划不精确；文献[16]提出了改进的二阶振荡粒子群算法，在一定程度上改善了算法易陷入局部最优解问题，但规划的路径不平滑；翻阅文献[17]的算法时，发现该文献的算法在一定程度上存在过早收敛的现象，没有很好的平衡全局搜索和局部搜索能力，搜索后期容易停滞陷入局部最优解等现象。此外，室内空间的局限性对于移动机器人速度控制精度、路径运行平滑性等要求较高，已有的研究对于本文的研究对象存在一定的不适用性。

因此，为提高PSO收敛速度，避免陷入局部最优解，提高路径的平滑度、精确度等，使移动机器人路径规划方法更好的适用于室内环境中，文章将分别引入收敛因子、线性递减、非线性凹函数、随机分布方式等方法对惯性权重进行改进，并结合三次样条插值方法建立了带有罚函数的适应度函数，将综合改进后的PSO算法用于求解室内全局路径规划问题，研究能够获得曲线更为平滑、在急停急转时，具有更好的动力学特性等路径规划方法。

1 PSO算法及改进的PSO算法

PSO算法是一种应用于机器人路径规划的经典仿生智能算法。其思想源于对鸟群捕食行为的研究。

1.1 PSO

PSO方法的基本核心是利用群体中的个体对信息的共享从而使整个群体的运动在问题求解空间中产生从无序到有序的演化过程，从而获得问题的最优解。每个寻优的问题解都是“粒子”。 每个粒子通过公式(1)和(2)进行速度和位置更新。

(1)

(2)

然而，当经典PSO算法应用在室内机器人路径规划研究时，算法易陷入局部最优解，造成路径规划效率不高，最优值迭代次数较多甚至不收敛，鲁棒性差，代价大等问题。

1.2 改进的PSO算法

因此，针对上节中PSO算法所存在的问题，本文利用动态权重、三次样条差值、罚函数等方法对经典PSO进行算法综合改进。

1.2.1 动态权重

为平衡较大的惯性权重的高全局搜索能力和低权重w的强局部搜索能力，解决因w过大容易导致“早熟”收敛与后期全局最优解附近的振荡现象。本文引入动态改变w的值，其思想在于：搜索初期w取值较大，随着迭代次数的增加逐渐降低w的值，从而达到全局最优和收敛。为使全局搜索和局部搜索达到最佳的平衡状态，文章利用随机分布方法[18]对经典PSO 进行改进。表达式为：

u=wmin+(wmax-wmin)*rand

(3)

w=u+rande*randn()

(4)

其中，wmin是最小权值；wmax是最大权值；rand()为[0,1]的均匀分布随机数；randn()为正态分布随机数；rande为随机权重方差，文章设置rande=0.5。正态随机变量调整w，不仅可以快速跳出局部最大值，而且能保持种群的多样性提高算法的全局搜索能力。

1.2.2 三次样条插值

在选择以随机分布方法进行动态权值设置的基础上，仿真实验中发现经典的PSO规划的路径存在转折点较多，路径不平滑，在急停，急转弯时动力学特性差等缺陷。三次样条曲线是由若干基于三次多项式的插值区间而拟合出一条光滑的曲线。因此，引入三次样条插值，对前述改进的PSO算法进行进一步的完善，通过结合两种方法的优势规划出一条更为光滑的路径，保障机器人运动过程中较好的动力学特性。

三次样条插值的定义与算法如下：

在区间[a,b]上，有n+1个数据节点(x1,y1),(x2,y2),…,(xn,yn)

1)计算步长hi=xi+1-xi(i=0,1,…,n-1);

2)将数据节点和指点的的首位端点条件带入矩阵方程;

3)解矩阵方程，求得二次微分值。该矩阵为三对角矩阵;

4)计算样条曲线系数:

ai=yi

(5)

(6)

(7)

(8)

其中，i=0,1,…,n-1在每个子区间xi≤x≤xi+1中，创建方程：

gi(x)=ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3

(9)

1.2.3 粒子编码

三次样条曲线的个数等于路径节点的个数。 路径节点为：三次样条段与段的交接处。 由于三次样条曲线为一阶连续，结点处为二阶连续。因此路径转向的最大次数为路径结点的个数。即使在复杂的环境下，路径经过3到5次转向就能避开障碍物。因此文章用路径结点对粒子编码，即一个粒子个体为对应路径上所有的路经结点。

假设有n个路径结点坐标，横坐标与纵坐标分别为：(xn1,xn2,…,xnn)，(yn1,y,…,ynn),路径的起点与终点的坐标分别为：(xs,ys),(xt,yt)。分别通过三次样条插值在区间(xs,xn1,xn2,…,xt)与(ys,yn1,yn2,…,yt)上取插值点，坐标为：(x1,y1),(x2,y2),…,(xm,ym)。

根据三次样条插值的特性，文章用路径节点和插值点以及路径的起点和终点的连线作为机器人的运行轨迹。

1.2.4 带有罚函数的适应度函数

判断一条路经是否最优一般需要满足以下两个条件：①路径是否最短；②没有和障碍物碰撞。

根据以上两个条件构造文章的适应度函数为：

(10)

1.2.5 改进PSO算法流程

通过上述的综合改进，动态惯性权重很好的平衡了全局搜索和局部搜索能力，避免了粒子收敛速度快，容易陷入局部最优点的缺点；引入了三次样条插值方法使得规划的路径是一条平滑的曲线，改善了经典PSO规划的路径存在转折点多，路径不平滑等缺陷，使机器人更好地适应了室内环境；引入罚函数，简化了非法度的计算，避免了路径和障碍物碰撞，规划出一条合法的路径。

改进PSO通过以下几个步骤完成室内移动机器人路径的最优规划。

Step1：根据室内具体布局确定路径节点的个数n，确定插值点个数m。

Step2：设置 PSO 算法中的各个参数，初始化粒子种群、位置和速度。

Step3：计算每个粒子x、y方向的m个插值点坐标。

Step4：根据式(10)计算粒子的适应度值。

Step5：根据式(1)和(2)分别与(3)～(4)更新粒子的速度和位置，更新局部最优值Pbest和全局最优值Gbest。

Step6：根据式(10)判断更新后的粒子是否合法并计算粒子的罚函数和适应度值，得出更新后由n个路径节点坐标组成的路径。迭代次数加1。

Step7：如果达到最大迭代次数，则算法结束，输出最优路径；如果没有达到最大迭代次数，则转向Step3。

具体流程如图1所示。

图1 改进PSO算法流程图

2 实验与结果分析

2.1 实验环境及参数初始化

实验仿真环境以所在实验室空间环境为背景，创建仿真实验路径规划有限二维空间地图如图2(a)所示。为验证本文算法的先进性，实验首先从本文改进的算法选一个最优的算法，然后用最优的算法分别与基于经典的PSO进行对比。

为保证算法对比的客观与公平，所有算法均采用相同的软硬件平台，运行环境为Windows10， core i7，CPU(2.2 GHz)，内存8 GB，编程环境为Matlab R2018a。同时为保证实验数据的真实可信，对于每种算法进行10次实验，对实验数据进行均值化处理进行对比。

仿真参数选择：五种算法的种群规模、最大迭代次数保持一致：Npop=150，MaxIt=500。基本PSO的惯性权重和学习因子w=1,c1=c2=1.5；改进算法的惯性权重和学习因子为动态的，wmin=0.4，wmax=0.9 ；三次样条插值点个数为100，边界为非节点边界。

经典PSO算法记为PSO，基于收敛因子与三次样条插值改进的PSO算法记为YSPSO-SP。基于非线性凹函数惯性权重与三次样条插值改进的PSO记为NCFPSO-SP，基于线性递减惯性权重与三次样条插值改进的PSO记为LinWPSO-SP，基于随机惯性权重与三次样条插值改进的PSO记为RandWPSO-SP。

2.2 室内环境路径规划及算法性能分析

为验证算法路径规划的普适性，将实验路径分为起点与终点在实验室的对角，并进行起点(★)、终点(■)位置互换的二次实验，4次实验起点、终点设置如图2(a)、3(a)、4(a)、5(a)所示。对应不同算法条件下路径规划最优路径值如表1～4所示。

2.2.1 第1次实验

通过图2(a)可以直观的看出，较其它四种算法RandWPSO-SP方法规划出的机器人运动路线拐点更少、拐点弧度较大。这是因为利用三次样条曲线可以对路径的角点进行平滑，所以产生的是一条光滑的路径。

迭代曲线图2(b)中，RandWPSO-SP算法规划的路径最短，这是因为该算法引入随机惯性权重，不仅平衡了全局搜索和局部搜索的能力，还增加了搜索的灵活性和范围，使算法的搜索能力提高，而且扰动效果明显，增强了粒子多样性，更容易跳出局部最优值；YSPSO-SP方法收敛速度最快，这是因为该算法引入收敛因子，虽然使收敛速度加快，但加速度因子是基于线性扰动，扰动小，粒子容易陷入局部最优解；收敛速度第二快的为LinWPSO-SP算法，该方法的扰动属于线性扰动，扰动不明显，粒子容易出现“早熟”；NCFPSO-SP算法引入非线性惯性权重，虽然非线性惯性权重扰动效果明显，但是该方法收敛精度低。

图2 第1次实验结果五种算法实验对比

表1是每个算法在相同的环境下，独立运行500次得出的路径结果。从中可以可以看出RandWPSO-SP算法规划的最短路径、平均路经以及平均仿真运行时间都比其它四种算法少。这是因为随机惯性权重的引入，不但平衡了全局搜索和局部挖掘能力，而且扰动效果明显，增加了粒子多样性。引入三次样条插值使得算法更好得适用于室内环境。开始规划的路径最长，这是因为起点处空间大，提高了搜索范围，改善了粒子陷入局部极值的现象。

表1 第1次实验路径长度对比数据表

2.2.2 第2次实验

本次实验与第一次实验的不同之处在于起点和终点互换。从图3(a)、(b)可以看出。该次实验仿真结果几乎和第一次一样，验证了较其它四种算法，RandWPSO-SP方法规划出的机器人运行轨迹拐点更少、拐点弧度较大等优点。

图3 第2次实验结果五种算法实验对比

表2是五种算法分别在相同环境下，独立运行500次得出的路径结果。从中可以看出RandWPSO-SP算法较其它四种算法的的最短路径、平均路径以及平均仿真运行时都最短。

表2 第2次实验路径长度对比数据表

表2数据和表1数据比较发现，表2的路径和平均仿真运行时间比表1长，这是因为虽然两次实验只是调换了起点和终点，但第一次实验起点处，由于室内空间布局大，非法路径少，而第二次实验开始阶段空间布局小，非法路径多，要多次排除非法路径，所以算法路径、运行时间长。

2.2.3 第3次实验

本次实验五种方法规划的路径比较集中，这是由于实验起点处的空间小，可供选择路径少。该算法的仿真结果再次验证了RandWPSO-SP方法规划出的机器人运行轨迹拐点更少、拐点弧度较大。该算法规划的路径能使机器人再室内更好的运行。

图4 第3次实验结果五种算法实验对比

本次实验起点处空间小，可供选择的路径少，各算法选择相似的空间规划的路径，这样更容易比较规划的最长路径、最短路径、平均路径。从表3可以看出：独立运行500次实验，RandWPSO-SP规划的最长路径、最短路径、平均路径都最短，且仿真运行时间也最短。

表3 第3次实验路径长度对比数据表

2.2.4 第4次实验

从图5(a)、(b)可以看出本次实验再次验证了RandWPSO-SP方法规划出的机器人运行轨迹拐点更少、拐点弧度较大，鲁棒性更好等优点。

图5 第4次实验结果五种算法实验对比

表4 第4次实验路径长度对比数据表

从表4可以看出：本次实验验证了RandWPSO-SP算法规划的最短路径，平均路径，最长路径较其它四个算法规划的路径较优，且仿真时间也最短的。较第三次实验，本次规划的路径及平均仿真运行时间都比第三次时间长，这是因为空间布局上本次实验起点空间大，算法进行了大范围的搜索。

上述四次实验结果表明虽然本文提出RandWPSO-SP的算法收敛速度不是最快的，但扰动小，搜索能力强，改善了粒子早熟现象，收敛路径最短，收敛效率更高，规划的路径拐点更少，拐点弧度更大。这是由于该算法引入随机惯性权重，不仅平衡了全局搜索和局部搜索的能力，还增加了搜索的灵活性，使算法的搜索能力提高，而且扰动效果明显，增强了粒子多样性，更容易跳出局部最优值。此外，该算法引入三次样条插值很好改善了室内环境造成的路径不光滑，转折点较多等问题。

3 结束语

本文根据室内环境局限性特征，引入三次样条插值，将三次样条插值和PSO结合规划机器人路径。因为造成基本的PSO存在收敛精度低，搜索停滞两个原因[19]：①算法搜索后期，粒子多样性少，易“早熟”。 ②粒子容易陷入较差的搜索区并很难跳出，造成搜索停滞，收敛效率低。本文给传统的PSO分别引入收敛因子、线性递减权重、非线性凹函数权重、随机权重并与三次样条插值结合。增加粒子的扰动效果，提高粒子的多样性，平衡粒子全局和局部搜索能力。把所有的改进的算法进行仿真实验比较，从中选出一个最优的算法即RandWPSO-SP，再与经典的PSO进行路径规划仿真实验比较。实验结果表明：RandWPSO-SP即基于随机惯性权重和三次样条插值的PSO算法规划的路径最短，用时最短，算法最稳定。对于室内环境有更好的有效性和优越性。

下一步实验将会把RandWPSO-SP算法应用于实际环境中，软件背景为ROS(robot operation system)操作系统，实验平台为类turtlebot机器人。使智能算法更好应用于室内环境。此外，还会进行实验：同时改进惯性权重和学习因子并将其用在真实的实验环境中，使学习因子和惯性权重达到更好的平衡，使路径规划效率更高，可行性更好。