本发明属于机器人路径规划领域，提出一种基于地图栅格与QPSO结合的机器人路径规划方法。

背景技术：

移动机器人路径规划是寻找一条无碰撞的可行路径问题的方法。近些年，群智能优化算法逐渐成为移动机器人路径规划算法研究热点。应用于移动机器人路径规划的群智能优化算法包括蚁群优化算法、PSO(particle swarm optimization,粒子群优化)算法、萤火虫算法等。其中PSO算法是Eberhart博士和Kennedy博士在1995年所提出的群智能化算法。该算法来源于对鸟群和鱼群觅食行为的模拟。它较传统的进化算法具有易实现、较少的调整参数、收敛速度较快等特点，广泛应用在移动机器人路径规划。

而QPSO算法是一种在数学上被证明能以概率为1搜索到全局最优解的群智能算法。QPSO算法建立在δ势阱模型且具有量子运动的粒子群，由于粒子具有量子行为，所以满足聚集态的性质完全不同，因而粒子的轨迹就不是确定的，粒子就可以在整个可行解空间中进行搜索，所以QPSO算法的全局搜索性能优于标准PSO算法，QPSO算法较之PSO算法以其易于实现、参数较少等特点在路径规划中得到很多关注。但是QPSO算法仍会遇到陷入局部最优的问题，其主要原因是该算法在单点处建立的势阱会使粒子快速地聚集在一个较小的搜索区域内，导致种群多样性降低。因而在机器人路径规划中，对QPSO算法的改进是一个研究热点。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种使粒子位置更新公式在不同的搜索阶段有不同的表现，能够有效减少在路径规划中陷入局部最优的情况发生的基于地图栅格与QPSO算法结合的机器人路径规划方法。本发明的技术方案如下：

一种基于地图栅格与QPSO算法结合的机器人路径规划方法，其包括以下步骤：

步骤1：根据移动机器人的工作环境的特点对机器人通过2D激光传感器获取的数据进行地图建模，并且采用自适应蒙特卡罗定位算法对机器人位置进行定位；

步骤2：在创建好的地图中采用轮盘式选择法对可行路径进行初始化；

步骤3：采用参数可变的改进量子行为粒子群优化QPSO算法对初始化的可行路径进行优化，得到最优路径，其中改进QPSO算法的改进体现在：在QPSO算法中对引入聚集度因子，并且对压缩扩张因子进行改进，同时自定义两个控制参数用来划分搜索阶段，使粒子位置更新公式在不同的搜索阶段有不同的表现。

进一步的，所述步骤1进行地图建模前还包括步骤：

假设机器人扫描到的活动场地有限并且为矩形，首先采用栅格法，将机器人矩形场地平均划分成多个小矩形栅格，保证机器人可以在其中进行自由移动，采用直角坐标法确定起点、终点、障碍物与机器人的位置，在创建地图模型后，首先根据里程计模型给出机器人的初始位姿，然后通过激光传感器扫描出的局部地图与全局地图的对应关系，并用局部地图更新全局地图。

进一步的，所述步骤1采用自适应蒙特卡罗定位算法对机器人位置进行定位具体包括步骤：

步骤A：初始化粒子群；

步骤B：采用随机函数模拟粒子在栅格地图中的运动；

步骤C：根据计算传感器定位的障碍物与地图中障碍物的符合个数来计算粒子评分，选择得分最高的粒子作为机器人当前位置；

步骤D：粒子群重采样，将得分较低的粒子舍去，得分较高的粒子保留并且复制，保持粒子群数量基本不变；

步骤E：重复步骤B到步骤D，直到机器人完成构建地图时终止。

进一步的，所述步骤2采用栅格法对可行路径进行初始化具体步骤如下：

步骤A：通过栅格地图，确定目标点、障碍物和机器人自身所在栅格在全局地图中的坐标；

步骤B：将有障碍物的栅格标为1，将自由栅格标为0；

步骤C:采用轮盘式选择方法选择机器人所处栅格相邻的自由栅格，并且移动到选择的自由栅格；

步骤D:判断机器人是否到达目标点，未到达则转至步骤C；

步骤E:机器人到达目标点则初始化路径完成。

进一步的，所述步骤3采用参数可变的QPSO算法对初始化的可行路径进行优化，得到最优路径的具体步骤如下：

步骤A：初始化粒子群数量N和最大迭代次数MaxIter参数并且设立适应度函数，并且在第一次迭代时，每个粒子的初始位置为当前个体最好位置，计算每个粒子的适应度函数值，所有粒子的适应度值相比较后，找到一个具有最小适应度值的粒子，该粒子的轨迹就是全局最好位置；

步骤B：在可行路径上使用粒子位置更新公式进行粒子位置更新；

步骤C：判断更新后的粒子位置是否位于自由栅格，若是则移动到该自由栅格，否则转入步骤B；

步骤D：判断粒子位置是否到达目标点，若否则转到步骤B，若是则计算适应度函数的值，更新个体最好位置与全剧最好位置；

步骤E：重复步骤B～D，直到重复次数达到最大迭代次数时，选取一条适应度值最小的粒子的轨迹作为机器人的最优路径。

进一步的，所述步骤A中利用适应度函数来进行路径的选择，其适应度函数即路径长度为：其中Xi为第i个粒子，j代表粒子第j维，(xj，yj)代表粒子第j维在栅格地图的二维坐标，(xj+1，yj+1)代表粒子第j+1维在栅格地图的二维坐标，D代表粒子的维度数。

所述步骤B中利用粒子位置更新公式进行粒子维度位置的更新，其粒子位置更新公式为：其中Rid代表第i个变异粒子的第d维位置，而pid＝φ*Pid+(1-φ)*Pgd，φ为[0,1]之间均匀分布的随机数，Pid与Pgd是第i个粒子的第d维位置的个体最好位置和第d维位置的全局最好位置；α为QPSO中的压缩扩张因子，表达式为m1与m2是[0,1]之间均匀分布的随机数；c是[0,1]之间服从高斯分布的随机数；β为聚集度因子，表达式为其中t为粒子群适应度函数值的期望；k1、k2与k3是[0,1]之间的随机数，且k1+k2+k3＝1；f(Xi)为第i个粒子的适应度值；it与dev为自定义的两个控制参数，表达式分别为：dev＝c2-β，c1和c2均为常数；MaxIter为最大迭达次数。mbestd为粒子群中第d维的平均最好位置，Xid为第i个粒子的第d维位置。u是[0,1]之间均匀分布的随机数，F为缩放因子，当it与dev均大于1时，缩放因子为0，其他情况，缩放因子均不为0。j和k是[1,N]之间两个互不相同的随机整数，且均不等于i的数。Xjd与Xkd为第j个粒子的第d位置和第k个粒子的第d维位置。

本发明的优点及有益效果如下：

本发明是针对机器人路径规划中使用QPSO算法，提出了一种基于地图栅格和QPSO结合的改进机器人路径规划方法，在QPSO算法中引入聚集度因子，并且对压缩扩张因子进行改进，在压缩扩张因子中引入两个自定义的控制参数，并且通过自定义的两个控制参数用来划分搜索阶段，通过搜索阶段控制缩放因子，使粒子位置更新公式在不同的搜索阶段有不同的表现，能够有效减少在路径规划中陷入局部最优的情况发生。

附图说明

图1是本发明提供优选实施例改进的QPSO算法流程部分。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

步骤1：假设机器人扫描到的活动场地有限并且为矩形。首先采用栅格法，将机器人矩形场地平均划分成多个小矩形栅格，保证机器人可以在其中进行自由移动。栅格法能够有效构建机器人所能理解的地图，并且易保存。采用直角坐标法可以方便确定起点、终点、障碍物与机器人的位置。在创建地图模型后，首先根据里程计模型给出机器人的初始位姿，然后通过激光传感器扫描出的局部地图与全局地图的对应关系，并用局部地图更新全局地图，然后通过自适应蒙特卡罗定位算法完成对机器人的当前定位。

步骤2：(1)通过栅格地图，确定目标点、障碍物和机器人自身所在栅格在全局地图中的坐标。(2)将有障碍物的栅格标为1，将自由栅格标为0。(3)采用轮盘赌选择方法选择机器人所处栅格相邻的自由栅格，保证自由栅格能够被等概率选择，并且移动到选择的自由栅格。(4)判断机器人是否到达目标点，未到达则转至(3)最后机器人到达目标点则初始化路径完成。

步骤3：(1)初始化粒子群数量N和最大迭代次数MaxIter等参数。(2)设立适应度函数即路径长度函数，其中Xi为第i个粒子，j代表粒子第j个维度。(xj，yj)代表粒子第j维在栅格地图的二维坐标。D代表粒子的维度数。(3)在可行路径上使用粒子位置更新公式进行粒子位置更新。其粒子位置更新公式为：其中Rid代表第i个变异粒子的第d维位置。而pid＝φ*Pid+(1-φ)*Pgd，φ为[0,1]之间均匀分布的随机数，Pid与Pgd第i个粒子的第d维位置的个体最好位置和第d维位置的全局最好位置。α为QPSO中的压缩扩张因子，表达式为F为缩放因子，it与dev为自定义的两个控制参数，表达式分别为：dev＝c2-β。c1和c2均为常数，通过这两个控制参数来划分搜索阶段，控制缩放因子。当it与dev均大于1时，缩放因子为0，其他情况，缩放因子均不为0。m1与m2是[0,1]之间均匀分布的随机数，c是[0,1]之间服从高斯分布的随机数。β为聚集度因子，表达式为其中t为粒子群适应度函数值的期望，k1、k2与k3是[0,1]之间的随机数，且k1+k2+k3＝1。mbestd为粒子群中第d维的平均最好位置。u是[0,1]之间均匀分布的随机数。j和k是[1,N]之间两个互不相同的随机整数，且均不等于i的数。(4)判断更新后的粒子位置是否位于自由栅格，若是则移动到该自由栅格，否则转入(3)。(5)判断粒子位置是否到达目标点，若否则转到(3)，若是则计算适应度函数的值，更新全局最好位置与个体最好位置。(6)重复步骤(3)～(5)，直到重复次数达到最大迭代次数时，选取一条适应度值最小的粒子的轨迹作为机器人的最优路径。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。