CN113467472B

CN113467472B - 一种复杂环境下的机器人路径规划方法

Info

Publication number: CN113467472B
Application number: CN202110852703.7A
Authority: CN
Inventors: 赵猛; 程学珍; 张俊明; 李继明; 郑才运
Original assignee: Shandong University of Science and Technology
Current assignee: Shandong University of Science and Technology
Priority date: 2021-07-27
Filing date: 2021-07-27
Publication date: 2023-09-01
Anticipated expiration: 2041-07-27
Also published as: CN113467472A

Abstract

本发明提供一种复杂环境下的机器人路径规划方法，选用栅格法进行对机器人的工作环境进行环境建模；使用粒子群算法进行全局路径规划；利用混沌函数对灰狼粒子群进行初始化，并在算法的迭代过程中使用混沌序列对陷入局部最优的粒子进行处理，同时结合粒子的适应度值构建自适应惯性权重；使用人工势场法对局部路径进行规划，构造斥力势场函数，并引入了相对距离因子；将全局路径规划和局部路径规划结合成混合路径规划，利用本发明灰狼粒子群进行全局路径规划，针对于机器人在运行过程中遇到的未知障碍物使用人工势场法进行局部路径规划，并最终到达预期的目标点。本发明能够有效的避开静态和动态的障碍物，并最终到达预期的目标点。

Description

一种复杂环境下的机器人路径规划方法

技术领域

本发明涉及利用机器人对矿井巷道内进行除尘领域，尤其涉及一种复杂环境下的机器人路径规划方法。

背景技术

铁矿石作为重要的冶金工业原材料，在钢铁生产中占比较大。在铁矿的开采过程中经常会因为开采仪器的震动使得矿井巷道内的粉尘浓度较高，粉尘不仅会对某些精密仪器会产生一定的影响，而且也严重影响着采矿工作人员的人身健康。

使用机器人对矿井内进行除尘操作，可以防止粉尘对开采设备造成破坏，提高矿石的开采效率，同时也保障了开采人员的人身健康。矿井内的环境较为复杂，巷道内的运行车辆以及行走的工作人员会给机器人的移动造成障碍。

目前机器人所使用的环境建模方式为栅格法、拓扑图法、自由空间法等。栅格法可以处理空间环境中任何障碍物的形状并且编码简单，容易编程实现；但如果栅格数较少的话，障碍物的信息就会变少，规划的速度也就会增快，但路径的有效性就会降低。栅格法时公认的最成熟的算法，其直观明了，常与其他算法结合使用，但在使用时会消耗过多的计算资源。

拓扑图法是一种降维的建模方法，将机器人的工作空间降维成子空间，生成的子空间通过各子空间节点之间的连线来建立拓扑图。由于拓扑法的降维操作，其占用内存小、规划简单，并且各个子空间存在着拓扑联系，故机器人在任何条件下都可以知道自己的位置，并且做出路径规划图；拓扑图法缺点是当机器人所获得的信息与当前环境的特征很相似时，容易确定为同一个节点，并且定位精度低。

自由空间法建模过程中不考虑机器人的物理尺寸并且将障碍物进行膨胀然后将工作空间中的障碍物的顶点进行连接，连接后的线段会与障碍物形成自由空间，然后精简去一些多余的线段使多边形尽可能的大。自由空间法是在起点和目的地改变时，不需要重新构建多边形，因此具有较高的灵活性。但工作空间中障碍物的数量会决定着算法的效率高低，障碍物越多，算法就会变复杂，且方法对环境的适应度较差。

发明内容

本发明提供的复杂环境下的机器人路径规划方法可以利用机器人对矿井巷道内进行除尘，可以在提高开采效率的同时保障开采人员的身心健康。方法包括：

步骤一，选用栅格法进行对机器人的工作环境进行环境建模；

步骤二，使用粒子群算法进行全局路径规划；

步骤三，利用混沌函数对灰狼粒子群进行初始化，并在算法的迭代过程中使用混沌序列对陷入局部最优的粒子进行处理，同时结合粒子的适应度值构建自适应惯性权重；

步骤四，使用人工势场法对局部路径进行规划，构造斥力势场函数，并引入了相对距离因子；

步骤五，将全局路径规划和局部路径规划结合成混合路径规划，利用本发明灰狼粒子群进行全局路径规划，针对于机器人在运行过程中遇到的未知障碍物使用人工势场法进行局部路径规划，并最终到达预期的目标点。

从以上技术方案可以看出，本发明具有以下优点：

使用机器人对矿井内进行除尘操作，可以防止粉尘对开采设备造成破坏，提高矿石的开采效率，同时也保障了开采人员的人身健康。矿井内的环境较为复杂，巷道内的运行车辆以及行走的工作人员会给机器人的移动造成障碍，本发明提高机器人在巷道内除尘的效率，还对移动机器人进行了路径规划。

本发明可以基于机器人在其工作环境中，根据自身位置与预期目标位置而规划出一条可行路径，并且机器人在沿着路径移动过程中不与任何障碍物发生碰撞的过程。

本发明涉及的灰狼粒子群能够更快的跳出局部最优，并且能够快速的搜索到全局最优路径。接着使用人工势场法对局部路径进行规划，针对于人工势场法中的不可达的问题，重新构造了斥力势场函数，并引入了相对距离因子；通过增设虚拟目标点的方法使其跳出局部最优。解决不可达和局部最优的问题。将全局路径规划和局部路径规划结合成混合路径规划，利用改进灰狼粒子群进行全局路径规划，针对机器人在运行过程中遇到的未知障碍物使用人工势场法进行局部路径规划。能够有效的避开静态和动态的障碍物，并最终到达预期的目标点。

附图说明

为了更清楚地说明本发明的技术方案，下面将对描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为复杂环境下的机器人路径规划方法流程图；

图2为序号法栅格地图模型图；

图3为不规则障碍物示意图；

图4为基本元素示意图；

图5为处理之后的栅格地图；

图6为实际环境模型示意图；

图7为原始雷达地图模型示意图；

图8为栅格化后的地图模型示意图；

图9为四连接方式示意图；

图10为八连接方式示意图；

图11为粒子运动模型示意图；

图12为粒子速度更新图；

图13为粒子位置重置方法示意图；

图14为灰狼社会等级示意图；

图15为基本粒子群Rosenbrock函数优化粒子轨迹图；

图16为灰狼粒子群Rosenbrock函数优化粒子轨迹图；

图17为本发明灰狼粒子群Rosenbrock函数优化粒子轨迹图；

图18为Rosenbrock函数优化迭代曲线比较图；

图19为基本粒子群Drop Wave函数优化粒子轨迹图；

图20为灰狼粒子群Drop Wave函数优化粒子轨迹图；

图21为本发明的灰狼粒子群Drop Wave函数优化粒子轨迹图；

图22为Drop Wave函数优化迭代曲线比较图；

图23为基本粒子群Peaks函数优化粒子轨迹图；

图24为灰狼粒子群Peaks函数优化粒子轨迹图；

图25为本发明的灰狼粒子群Peaks函数优化粒子轨迹图；

图26为Peaks函数优化迭代曲线比较图；

图27为简单环境下路径规划算法对比仿真图；

图28为简单地图环境下迭代曲线图；

图29为复杂环境下路径规划算法对比仿真图；

图30为复杂地图环境下迭代曲线图；

图31为目标不可达位置关系示意图；

图32为本发明的后斥力势场合力分析图；

图33为障碍物在目标点附近虚拟目标点的位置图；

图34为障碍物在中间时虚拟目标点位置图；

图35为目标点被L型障碍物包围虚拟目标点位置图；

图36为机器人被U型障碍物包围虚拟目标点位置图；

图37为目标不可达路径规划仿真和作用力变化曲线图；

图38为不可达本发明后仿真图和作用力变化曲线图；

图39为传统人工势场法路径仿真图；

图40为增加虚拟目标点后仿真图；

图41为传统人工势场法路径仿真图；

图42为增加虚拟目标点后仿真图；

图43为传统人工势场法路径仿真图；

图44为增加虚拟目标点后仿真图；

图45为传统人工势场法路径仿真图；

图46为增加虚拟目标点后的仿真图；

图47为混合路径全局路径规划仿真图；

图48为混合路径障碍物相对运动路径规划仿真图；

图49混合路径障碍物侧面运动路径规划仿真图；

图50混合路径障碍物同向运动路径规划仿真图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供的复杂环境下的机器人路径规划方法包括：如图1所示，

S1，选用栅格法进行对机器人的工作环境进行环境建模；

S2，使用粒子群算法进行全局路径规划；

S3，利用混沌函数对灰狼粒子群进行初始化，并在算法的迭代过程中使用混沌序列对陷入局部最优的粒子进行处理，同时结合粒子的适应度值构建自适应惯性权重；

S4，使用人工势场法对局部路径进行规划，构造斥力势场函数，并引入了相对距离因子；

S5，将全局路径规划和局部路径规划结合成混合路径规划，利用本发明灰狼粒子群进行全局路径规划，针对于机器人在运行过程中遇到的未知障碍物使用人工势场法进行局部路径规划，并最终到达预期的目标点。

其中本发明涉及的机器人可以包括无线通信单元、音频/视频(A/V)输入单元、用户输入单元、感测单元、输出单元、存储器、接口单元、控制器和电源单元等等。但是应理解的是，并不要求实施所有示出的组件。可以替代地实施更多或更少的组件。将在下面详细描述移动终端的元件。用户可以通过计算机或服务器与机器人通信，实现参数配置，数据处理以及数据分析等操作，满足现场使用要求。

作为本发明的实施方式中，基于栅格法的环境建模中，在机器人进行路径规划前，需要对其工作环境中的障碍物的位置进行判定。但实际环境中的障碍物信息机器人是无法直接拿来使用的，需要对实际环境中障碍物的信息进行相关的处理，将其转换为机器人可以识别的信息，这就是环境建模。由于矿井巷道内的障碍物形状较为复杂，而栅格法是目前使用最为成熟的建模方式，故采用栅格法对机器人的工作环境进行建模。

栅格法地图具有结构简单、建模准确的特点，使得许多成熟的规划算法都可以在栅格地图中进行搜索。因此通过栅格所建立的地图建模可以确保路快速准确的搜索出一条路径。

对于栅格法来讲，不需要考虑障碍物的形状，能够对任意形状的障碍物建模；形成模型简单且应用范围广；起始点和目标点只要存在通路，就可以搜索出一条路径。

在地图建模中，如何精确的表示障碍物是一个主要的问题，现对环境中的障碍物做两点假设：(1)机器人通过传感器对实际环境中的障碍物的大小和位置进行检测，然后经过相关的转换处理得到所需要的栅格地图模型；(2)对障碍物进行俯视，其向地面所投影的最大面积，就是障碍物的模型。

栅格地图的表示就是将地图表示后的工作空间进行等距分割，分割后的网格的密度又称为栅格粒度。如果栅格粒度较小，路径规划的精度就会越高，但需要的计算机内存越多，系统的抗干扰能力就弱，决策速度也会减慢；反之，栅格粒度选取的较大，需要的计算机内存越少，系统的抗干扰能力就强，但路径规划的精度就会变低。在机器人运动的过程中，如果实际环境不发生变化的话，那么障碍物是不需要进行更新的，处理后的地图会被分割成若干个栅格，其中每个栅格都会存储着相关信息。栅格粒度的大小是由机器人体积的大小和障碍物的多少决定的，一般栅格划分的过程如下：

(1)对地图上的障碍物形状进行分析并选定；

(2)将障碍物所有顶点的坐标构成一个能够包含障碍物的矩形，可得出边长的最大和最小值l_max、l_min；

(3)求解每个矩形的i的面积：

S_i＝a_i×b_i (2.1)

式中的a_i为矩形的长，b_i为矩形的宽，i为第i个矩形；

(4)求解m个矩形的面积S_m：

S_m＝∑S_i (2.2)

式中S_m为所有矩形面积的总和；

(5)第i个栅格的长度为：

栅格粒度长度：

栅格总数：

式中：L为地图的长；W为地图的宽；

通常取M的值为：100～900；若模型需要高度密集的栅格的情况下，可以将M取至上万。栅格地图是路径规划中常用的地图模型，其基本思想是将实际环境进行相关的转换处理，然后将转换后的地图模型进行等距分割，将障碍物的位置通过对其所占的栅格进行赋值来进行表示。

栅格地图主要由两种信息所组成，一种为代表障碍物的栅格信息，用f(x)＝1表示，这种栅格信息表示机器人需要进行绕行；另外一种为代表机器人能够自由行走的栅格信息，用f(x)＝0表示，这种栅格信息表示机器人能够正常移动。为了能够快速区分障碍物和自由移动空间，通常将障碍物与自由空间使用两种颜色的栅格进行区分，然后对所有的栅格进行标号的处理。

对栅格地图的表示方式采用序号法，如图2所示，按照从左到右、从下到上的次序依次编号，用n表示第几个栅格，n的取值范围根据栅格数量的多少来确定。

由序号表示法转换成坐标法：该方法的优点如下：序号法能够合理的表示出相邻栅格的信息，智能算法可以在此基础上进行更好的路径寻优。

示例性的讲，本发明在对不规则障碍物的处理方式中，是对实际环境中的障碍物进行建模后，并不是所有的障碍物都能够直接在地图上表示出来，对于某些不规则且无法占满整个栅格的障碍物，需要对其进行膨胀处理，使其充满整个栅格。

膨胀的模型如式所示：

式中A表示将要进行处理的栅格如图3所示，B表示基本元素如图4所示，C表示膨胀处理之后栅格信息如图5所示，(x,y)表示栅格的坐标。整个膨胀处理的过程用数学形态学解释D是通过结构元素B对A进行边缘膨胀所得，最终以D在栅格地图中进行表示。如图5就是在图3的基础上使用结构元素进行膨胀所得到的图像。

本发明中，环境栅格化的处理过程中，首先使用UG软件创建障碍物的3D模型，然后将障碍物模型导入V-rep仿真软件中建立实际的空间环境模型。环境模型如图6所示，将搭建好的带有激光雷达的机器人模型置于空间内。接着使用Python编写机器人控制程序，控制程序在仿真软件运行之后通过V-rep所提供的联调接口对环境空间内的小车进行移动控制，V-rep通过所创建的Socket接口将雷达传感器所获取的数据传送给Matlab，Matlab通过对雷达的实时数据进行处理，进而转化为地图模型。运行V-rep软件后，机器人所携带的激光雷达开始向机器人前方发射激光。最后运行机器人控制程序和Matlab地图绘制程序，通过键盘控制机器人在空间内进行移动，使雷达传感器能够扫描完整个空间。在机器人运动过程中，经雷达扫描过的空间信息通过Matlab实时处理后，得到的原始环境地图模型如图7所示。

对雷达数据生成的原始环境地图模型使用Matlab软件DIP工具箱中的im2bw函数进行二值处理，进而转化为二值图像。再通过rgb2gray函数将二值图像转换为灰度图，最后转换为栅格图，得到图8所示的栅格地图模型。在对环境进行栅格法建模之后，栅格节点之间必须建立起一种搜索联系，只有在建立了搜索联系之后才可以确保搜索算法能够在起点和目标点之间搜寻出一条有效路径。图9和图10图显示了栅格节点之间的搜索联系，所有箭头的连接点处为机器人本身所在的节点，而箭头的指向为机器人下一步可以前进的方向，依此类推即可完成全部的路径规划。

本发明涉及的粒子群优化算法是在用粒子群进行求解时，每个粒子都可以看成一个潜在的解，并且每个粒子自身都具有两种信息，分别是速度信息和位置信息。算法的迭代过程也就是粒子之间进行两种信息交换的过程，并且，每次迭代过程中计算得到的信息都可以继承给下次参与迭代的粒子，粒子在每次迭代后都可以用自身所携带的位置信息使用适应度函数进行评优。将种群中的粒子初始化后，粒子就在解的空间内自由运动，粒子下一次运动的位置是由粒子本身的速度决定的，并且速度受到粒子目前本身能够找到的历史最优解和整体最优解的影响，种群中的所有个体依次进行迭代搜索，并且最终寻找到最优值。图11给出了粒子x从第k次到第k+1次迭代的示意图，v_k代表第k次迭代时粒子的速度，p_b代表在第k次迭代时的最优适应度值，p_g代表在第k次迭代时种群最优适应度值。粒子在迭代的过程中收到v_k、p_b、p_g的影响，三个矢量的合成就是粒子x第k+1次的位置。

粒子群优化算法全局的最优解是通过种群中粒子各自所携带信息的相互共享和相互协作搜索到的。假设某一问题的最优解存在于D维空间中，且存在着一种群，种群的规模为m，则种群可以用公式表示，其中k为算法需要迭代的总次数。在第k次迭代计算中第i个粒子的位置信息可以用d维向量/>i＝1,2,…,m表示，同时d维向量也可表示一个潜在的解。每个粒子的速度可以用/>i＝1,2,…,m来表示。粒子在每一次迭代过程中自身的位置和速度是不断变化的，同时每个粒子利用历史和群体的信息不断来更新自身的状态。粒子i在d维上k+1次迭代的计算公式如下面公式所示：

其中，c₁和c₂表示学习因子，c₁作用是用来控制个体朝着最优值飞行的步长，c₂的作用是用来控制种群朝着最优值飞行的步长。表示在粒子在寻优过程中的历史最优值；表示所有粒子达到的最优值，即种群的最优值；rand()函数用来随机产生(0,1)之间的数，用来保持种群的多样，d(1≤d≤D)表示速度和位置的维数。在上式公式中，粒子群优化算法的速度由三部分组成：/>表示粒子第k次迭代时的速度信息，/>表示粒子自身的信息；/>表示种群中粒子进行协作和信息共享的部分。以上公式中的速度迭代公式可以由图12表示：

在粒子群算法的迭代过程中，粒子还必须满足粒子速度向量的约束公式：

式中V_max表示粒子在飞行过程中允许的最大速度值，上述的粒子速度不等式也可以叫做动态lipschitz约束条件。

上述中的约束条件虽然规定了粒子的速度，但在粒子的实际迭代中，有些粒子会超出搜索空间，故必须对超出搜索空间的粒子进行一定的处理才能确保算法在可行解的空间内运行。通常有如图13所示的四种情况，通常对它们的处理有四种方式：

(1)无穷法：针对存在最小值优化的问题，当时，令/>

(2)最近法：如果则将位置向量重新设成/>其中x′应满足任意的x∈S，/>即x′是搜索空间S里面距离/>最近的位置

(3)随机法：假设存在着一个取值范围是[lb_d,ub_d]的d维空间，如果超出取值范围时，需要重新设置/>rand()表示取(0,1)之间的随机数。

(4)缩放法：若则将位置重新设置

式中

在粒子群算法的迭代计算中，粒子个体位置最优值可以用以下的函数进行更新：

种群最优值可以用以下的函数进行更新，其中f(·)为适应度函数值：

本发明中的粒子群算法的步骤如下所示：

(1)对种群规模、学习因子、迭代次数等相关参数进行初始化；(2)在搜索空间内对粒子的速度和位置进行初始化，粒子的个体最优位置为初始化粒子的位置向量，并利用公式3.8对种群位置最优值进行更新；(3)根据公式3.1对粒子的速度进行更新，且按照公式3.3对粒子进行约束，如果存在着超出搜索空间的粒子还必须对其进行相关的处理；(4)计算适应度值，并且根据公式3.7和3.8更新粒子的个体和整体适应度值；(5)判断是否满足程序结束条件，如果满足，停止算法迭代并且给出最优解；若不满足，跳转到第三步继续运行。

本发明中基于粒子群算法进行求解，在迭代过程中不要求优化函数具有可导等特性，但当迭代次数过多时，种群的多样性会减少，粒子的运动速度也会变慢，许多研究学者针对于该算法进行了深入的研究，并且提出了一些常见的变异算法。其中涉及的带惯性权重的PSO算法模型中，速度迭代公式为：

上述公式中惯性权重w在算法的迭代过程中对寻优结果是有较大影响的：其值大小决定了粒子的两次迭代过程中上一次对下一次的影响。当w较小时种群的多样性减少，故算法的局部搜索能力强，全局搜索能力弱。反之，较大的w可以更好的进行全局搜索，但不容易得到更为优秀的解，但较大的惯性权重却会使PSO的算法收敛速度变快。将w设置为1，在粒子群优化算法的迭代过程中，将惯性权重改为动态的非线性变化，权重公式如下：

w(k)＝(w_ini-w_end)(K-k)/K+w_end (3.10)

上述公式中，w_ini为初始权重，w_end为最终权重，k代表当前迭代次数，K代表最大迭代次数。由于算法在迭代初始时，要求算法能够快速的找到个体最优值，故w_ini初始权重设为0.9，而在算法的迭代后期，要求算法能够尽量的找到最优值，故w_end设为0.4。通过非线性的改变w的能够避免算法在迭代过程中陷入局部最优，提高了算法的搜索速度。对于带收缩因子的PSO算法模型来讲，将收缩因子χ与粒子群算法相结合后，能够使算法的收敛速度变快^[64]。算法公式如下所示：

在上述公式中δ的取值大小能够平衡算法全局和局部的寻优效果；当δ≈0时，算法的局部寻优效果好，但算法容易提前收敛；当δ≈1时，算法的全局搜索能力强，但算法不容易收敛。考虑到算法的平衡能力，将δ设为1，ψ设为4.0，通过计算可得χ为0.795。与3.2.1小节中所述的算法相比，带有收缩因子χ的粒子群优化算法在迭代过程中不需要设置速度的最大值V_max，只需要约束好位置变化的空间范围即可。对于本发明的离散PSO算法模型来讲，粒子的速度公式保持不变，而位置是使用二进制的字符进行编码的，粒子位置的向量公式如下所示：

上式中，rand()函数是用来产生(0,1)之间的随机数，其中

为Sigmoid函数。即在离散PSO中，粒子的位置只有两个值：0与1。在用离散的PSO解决实际工程的问题时，其算法性能是比较优秀的。

本发明的基于本发明灰狼PSO算法的全局路径规划中，涉及的基于灰狼优化的PSO算法是借鉴灰狼捕食提出的本发明PSO算法。如图14所示。图14中的α表示种群中的领导者。领导者α是狼群的绝对核心，主要负责领导和分配的任务。图14中的第二层β代表智囊团队。智囊团队的任务协助领导者进行管理，并且当领导者α出现空缺时，由β快速的接替α的位置。在整个狼群中，β的地位仅仅低于α，在α存在的时候，β的任务是将α的命令下达给其他成员，并将下级的执行情况反映给领导者α。图14中的第三层是δ，δ听从领导者α和智囊团队β的指挥和管理，主要任务是看护和监管等事务。图14中的最底端是ω，主要负责种群内部的成员关系的平衡。

灰狼粒子群算法实质就是在基本粒子群算法的基础上将适应度值最为优秀的粒子设为领导者α，然后由此粒子对其他粒子进行管理。灰狼粒子群算法的第一步将种群中的粒子在其搜索空间内进行初始化；第二步是将评价后的粒子按照历史最优适应度值进行优劣排序；第三步是取出群体中适应度值最为优秀的三个粒子，并将其分别设置为α、β和δ，其余粒子顺次排列。如果在粒子的迭代过程中，如果出现适应度值更好的个体，那么就将其设为领导者α，然后将个体适应度值p_(i-1)取代p_gd。其中x_i＝(x_i1,x_i2,…,x_iN)代表第i个粒子的位置信息，v_i＝(v_i1,v_i2,…,v_iN)是第i个粒子的速度信息。在第k+1次迭代时，种群中前三个适应度值优秀的粒子的位置通过公式3.15进行更新，而其余粒子位置使用公式3.17进行更新：

式中d代表空间解的维数，k代表当前迭代次数，c₁、c₂、c₃代表学习因子，rand()代表(0,1)之间的随机数，w代表惯性权重系数，较大的w能够快速的实现全局搜索；反之，较小的w在局部搜索时更为优秀。灰狼粒子群算法的核心思想是：在粒子的每次迭代过程中，都要对粒子进行适应度值进行排列，然后取出最佳适应度值的三个粒子并将其设置为α、β和δ，其余粒子分别按次进行排列。这样在算法的迭代过程中既保持了粒子的多样性，同时又避免了算法陷入局部最优。

本发明对灰狼PSO算法混沌初始化过程中，是使用随机分布的粒子对问题进行求解的，粒子初始化的分布质量对求解最优解起着至关重要的作用。初始粒子分布越均匀，群体的多样性就越丰富，从而获得最优解的速度就越快。混沌指的是一种在规定的范围内能够遍历所有情况的非线性运动，混沌看似无规律但事实上有着内禀的随机性。混沌有着对初值敏感、内在随机性和能遍历整个空间的特点。混沌序列可以表示规定空间内的所有状态，混沌序列一般采用映射的方式产生。假设有一个空间X，一个连续的映射关系：f:X→X叫做X上的混沌，如果此映射关系能够产生混沌，那么它必须符合三个要求：(1)f对初始条件敏感；(2)f的周期点在X的空间中很稠密；(3)f能够通过拓扑表达。

这里将混沌映射为Logistic映射如式3.18所示：

Z_i+1＝μZ_i(1-Z_i)i＝0,1,2,...；μ∈(0,4] (3.18)

式中，0≤Z₀≤1，Z_i为第i个变量，μ代表控制变量，当μ＝4时，系统处于完全混沌的状态，其混沌空间的范围为[0,1]。

使用混沌初始化灰狼粒子群的步骤为：首先应随机产生一个n维向量z₁＝(z₁,z₂,…,z_n)，然后利用公式3.18对n维向量进行映射产生混沌序列z₁,z₂,…,z_N。然后把混沌序列z_i的从混沌空间[0,1]逆映射到变量取值空间[a,b]，得粒子的位置为x_ij＝a+(b-a)z_ij，j＝1,2,…,n，i＝1,2,…,N，最后通过计算粒子的适应度值，并将适应度值较为优秀的一些粒子作为种群的初始化粒子。

因为混沌序列可以在规定的范围内能够遍历所有的状态，所以混沌搜索是一种有效的搜索方式。根据灰狼粒子群的早熟机制，当灰狼粒子群陷入局部最优后，算法将选出在迭代过程中的粒子历史最优值，将其转换为混沌序列，然后再通过逆映射将混沌序列转换为粒子的最优位置值，并随机取代当前搜索空间中的某个粒子位置，从而使得算法跳出局部最优值。粒子是否陷入早熟时使用群体适应度方差来判断，群体适应度方差如公式3.19所示：

上式中n为种群规模大小，f_i代表第i个粒子的适应度值，f_avg代表当前粒子群的平均适应度值。群体方差σ²反映了粒子的早熟状态，当σ²小于一定阈值时，判定为粒子算法陷入早熟。当陷入局部最优时，利用混沌对种群中的最优粒子进行处理，增加了种群的多样性。详细步骤如下所示：

(1)选出迭代过程中的最优位置值，然后使用函数Logistic将其映射到混沌空间[0,1]。(2)利用logistic产生新的序列，并把序列逆映射到种群中。(3)通过计算粒子的最优适应度值即可判断粒子是否跳出局部最优，并记录最优适应度值，并将相应的粒子设为α、β和δ。(4)利用当前最优混沌粒子管理粒子群中的粒子，使得粒子跳出局部最优。在灰狼粒子群进行混沌初始化操作后，使粒子在搜索空间内分布更加均匀，并且通过混沌序列对其进行早熟的本发明，增加了粒子的多样性，提高了算法的收敛速度。

本发明中还进行自适应惯性权重处理，这里，粒子群优化算法中的收敛度与惯性权重系数w有着较大的影响，当w取值较小时，算法局部搜索优秀；反之，当w取值较大时，算法全局搜索更强。为了使算法在整个运行过程中能够保持较好的搜索能力，针对于惯性权重做出本发明，通过粒子相邻适应度值的比值来控制w的大小。粒子适应度比值公式如下。

上式中的代表第k次迭代时的全局最优适应度值；/>代表第k-1次迭代时全局最优适应度值；w_max值取为0.9；w_min值取为0.4；k代表当前迭代次数；MaxIter代表最大迭代次数。由上式可知，在迭代初期a较大，所以算法的全局搜索能力强，迭代后期a较小，所以算法的局部搜索能力强。综上所述，与相邻粒子适应度比值结合后的惯性权重w能够自适应的平衡整体和局部之间的问题。

使用混沌函数对灰狼粒子群算法本发明后的步骤如下，(1)应用Logistic函数式3.18对种群粒子的位置和速度进行混沌初始化；(2)评估出粒子的适应度值，并将前三个适应度值最优的粒子设置为α、β和δ；(3)根据使用公式3.15对粒子α、β和δ的位置进行更新，剩余粒子使用公式3.17进行位置更新，粒子的速度用公式3.16更新；(4)使用公式3.19判断算法是否陷入早熟，若是，对早熟的粒子进行混沌处理，然后再跳转到步骤三；(5)判断算法是否满足终止条件，若满足停止迭代并输出结果，不满足继续跳转到(3)进行计算。

本发明还对灰狼PSO进行仿真结果与分析。仿真实验环境为Matlab R2018b。分别对基本粒子群算法、灰狼粒子群算法和本发明的灰狼粒子群算法进行验证。对于传统粒子群算法和灰狼粒子群算法的惯性权重采用式3.10进行计算，本发明的灰狼粒子群算法采用式3.21进行计算。仿真实验的变量为：种群规模设为50、优化变量的维数设为4，学习因子c₁＝c₂＝2.05，仿真实验的测试函数分别为Drop Wave、Peaks、Rosenbrock三个基本函数，对于Drop Wave和Peaks函数迭代次数为100次，Rosenbrock函数迭代次数为200次，分别对以上函数进行20次仿真实验并记录实验数据。在仿真实验中，Rosenbrock函数优化结果的粒子的运行轨迹图如图15至18所示。基于Drop Wave函数优化结果如图19至22所示。基于Peaks函数优化结果如图23至26所示。在20次的仿真实验中，记录的实验数据如附录1所示。将三种测试函数的实验数据进行分析并比较如下表所示。

表3.1 Rosenbrock函数测试结果

表3.2 Drop Wave函数测试结果

表3.3 Peaks函数测试结果

可以看出，三种算法在三种测试函数中有着较大的寻优差异，本发明本发明后的灰狼粒子群不仅在搜索最优值方面比基本粒子群和灰狼粒子群优秀，而且在跳出局部最优方面也有着较大的进步。在表3.1到3.3中可以看出，本发明的灰狼粒子群在Rosenbrock、Drop Wave、Peaks测试函数中的寻优精准度都比基本粒子群和灰狼粒子群高。实验仿真数据表明本发明的灰狼粒子群的稳定度和求解精度方面都有着较大的优势，证明本发明的灰狼粒子群可以有效的提高算法的性能。

本发明为了验证本发明的灰狼粒子群算法在机器人全局路径规划中的有效性，下面将对三种算法使用Matlab R2018b进行仿真实验。首先采用栅格法分别对简单和复杂两种地图环境进行建模。在使用栅格法进行构建栅格地图时，地图的大小为20×20。在构建好的地图上分别对基本粒子群、灰狼粒子群、本发明的灰狼粒子群三种算法进行全局路径规划的仿真试验。算法中的参数分别设置为：种群数为50，迭代次数为200次，c₁＝c₂＝1.6，起点为(0,0)，终点为(20,20)。简单地图环境下仿真结果如下图27和28所示：

表3.4简单地图三种算法结果对比

根据上述的路径规划线路和迭代收敛曲线可知：基本粒子群算法在第78次迭代时就已经搜索到最优路径，得到的最优适应度值为31.80；灰狼粒子群算法在第130次迭代时也搜索到最优路径，最后得到的最优适应度值为30.63；本发明灰狼粒子群算法在第52次迭代时已经搜索到和基本粒子群算法搜索到相同的最优值，算法进一步迭代搜索，最终在83次迭代时搜索出一条最优的路径，最终的最优适应度值为30.40。复杂环境时三种算法仿真结果如图29和30所示。

表3.5复杂地图三种算法结果对比

根据上述路径仿真结果和迭代曲线可知：基本粒子群算法在第138次迭代时搜索到最优路径，得到的最优适应度值为30.36；灰狼粒子群算法在第127次迭代时搜索到最优路径，得到的最优适应度值为29.10；本发明灰狼粒子群在第47次迭代时已经超过基本粒子群的优化结果，并在第75次迭代时搜索到最优路径，得到的最优适应度值为28.63。

综上所述，使用混沌初始化本发明后的灰狼粒子群无论是在简单的地图环境还是复杂的地图环境都较本发明前的灰狼粒子群和基本粒子群算法全局搜索能力效果更好，能够跳出局部最优，且最终得到的路径都比较优秀。

本发明涉及的混合规划中的全局路径规划，首先对粒子群算法以及其变种进行了简单的介绍，然后对灰狼粒子群算法进行混沌处理，避免了算法过早收敛导致无法寻找全局最优值；接着给出了一种自适应的惯性权重，并通过三个测试函数对本发明后的灰狼粒子群的进行寻优测试，其结果表明本发明后的灰狼粒子群算法在寻优精确度方面比基本粒子群和灰狼粒子群高；最后，使用Matlab对本发明后的灰狼粒子群算法进行全局路径规划仿真实验，仿真结果表明本发明的灰狼粒子群算法在路径规划方面明显优于基本粒子群和灰狼粒子群算法，不管是从迭代速度还是适应度值方面，本发明后的算法都要优越之前的算法。

本发明基于人工势场法进行局部路径规划。由于机器人在工作环境中的任意一点都会受到目标点的引力作用，目标点对机器人产生的引力与两者之间的距离成反比。而当机器人与目标点重合时，引力相应的变为零，即不受引力的作用。根据以上描述，引力势场函数如下：

上式中：k_att为增益系数，其取值对引力的大小有着直接的影响；q,q_g分别代表机器人当前所在的位置和目标点的位置，d(q,q_g)代表机器人与目标点之间的距离，其大小可用公式进行计算：

d(q,q_g)＝||q_g-q|| (4.2)

机器人受到的引力大小由式4.3进行计算，式中的-grad代表负梯度计算。引力方向为机器人指向目标点。

机器人所受引力与坐标轴x的夹角可用公式表示为：

其中每个坐标轴方向的分力可表示为：

本发明中，机器人只有进入到障碍物的斥力影响范围内才会受到斥力作用，越靠近障碍物，其受到的斥力就越大；反之越远离障碍物，其所受到的斥力就越小。根据以上表现形式可以定义斥力势场的函数为：

同引力函数一样，上式中：k_rep代表斥力场函数的增益系数，其值的大小也对斥力有着非常大的影响，故其设为正常值；q代表机器人本身的位置，而q_o代表障碍物的位置；d(q,q_o)＝||q_o-q||为两者之间距离的大小；d₀为某个障碍物斥力的影响范围，d₀的大小可表示为：

上式中：a_max代表机器人的减速最大值；v_max代表机器人在前进过程中的最大行进速度。虽然在d(q,q_o)>d₀的情况下，机器人不会受到障碍物的斥力，但是随着d(q,q₀)→d₀，机器人所受到的斥力会瞬间消失，为了避免这种情况的发生，将上式调整为：

同引力一样，机器人受到的斥力由式4.9进行计算，式中的-grad代表负梯度计算，斥力方向为障碍物指向机器人。

同样斥力与坐标轴x的夹角为：

其中每个坐标轴方向的分力可表示为：

本发明基于合力势场函数进行计算。通过对引力势场U_att和斥力势场U_rep进行矢量合成，可以得到合成势场函数：

机器人受到的合力计算公式如式4.13所示：

坐标轴方向上的合力为引力和斥力在x、y轴方向上分力的矢量和，其大小可表示为：

机器人所受合力与坐标轴x的夹角为：

因此机器人在引力和斥力合力的作用下的位置更新可以表示为：

上式中，(x_k,y_k)代表当前机器人所处的位置；(x_k+1,y_k+1)代表机器人下一步的位置；c代表机器人的步长。

当机器人在某种环境下进行路径规划时，其人工势场法的步骤为：(1)首先对各项参数进行初始化，并给出预设目标点的位置、机器人本身的位置。(2)根据势场的引力函数和斥力函数对机器人所受到的合力进行计算，并且推出与x轴的夹角θ；(3)根据机器人位置的更新公式计算机器人下一步的位置；(4)根据目标点对机器人的引力是否为零判断机器人是否到达目标点；(5)如果机器人未到达目标点，转回步骤二；如果已经到达，则算法结束。

由于人工势场法是借鉴静电模型的理论进而通过数学模型的转变而来的。其符合局部路径规划的要求，目前被广泛的应用。在机器人工作中的目标不可达问题主要是指，当在机器人附近存在障碍物时会因为障碍物产生的斥力较大，使其无法到达目标点。一般情况下，障碍物不会出现在目标点附近，并且只有机器人与障碍物的距离较近时才会受到斥力，因此，在目标点附近无障碍物时，机器人可以很好的完成路径规划任务；但当目标点附近存在障碍物时，随着机器人与目标点间距离的减小，斥力逐渐增大而引力逐渐减小，此时目标点不再是全局最小势能点。因此，机器人无法正常的到达预先设定的目标位置。在不能达到预先设定的目标点时，机器人、目标点、障碍物之间的位置关系如下图35所示：

当使用人工势场法进行局部路径规划时，机器人受到的作用力是根据传感器对障碍物进行距离检测进而转换而得到的，所以，合力的方向直接影响着机器人移动的方向。当障碍物在目标点的右侧时，机器人会在引力的作用下朝着目标点运动，随着与障碍物间距离的减小，机器人受到的斥力大于引力，根据算法的思想，机器人将会背离目标点运动；当距离目标点一定距离后，引力的作用就会大于斥力的作用，机器人受到合力的影响，距离目标点就会越近。

根据上述分析，目标不可达的问题主要是因为障碍物的分布距离目标点的位置太近，而使得目标点势能增加，又因为机器人是由势能高的点向势能低的点运动的，从而出现目标不可达的问题。因此，如果确保引力在整个过程中一直大于斥力，那么就会解决目标不可达的问题。

本发明通过引入机器人与目标点之间的距离因数ρ(p,p_g)对基础斥力势场函数进行本发明，为了使距离因数ρ(p,p_g)在调节中更加灵活，通过在距离因数中增加了调节因子m，如式4.17所示，通过调节m值的大小，即可改变机器人与目标点之间作用力的大小，从而确保目标点始终是势能的最低点，避免了不可达问题。本发明后的斥力势场函数的表达式为：

式中，k_rep代表增益系数，p代表机器人本身坐标，p_obs代表障碍物位置坐标，p_g代表目标点坐标，ρ₀代表障碍物斥力范围，ρ(p,p_obs)代表机器人与障碍物之间的距离，ρ(p,p_g)代表机器人与目标点的距离，m代表调节因子，其取值范围0≤m≤1。根据上式可知，随机机器人与目标点之间距离的减小，机器人受到的斥力会受到调节函数ρ^m(p,p_g)的影响，当m＝0时，调节函数为1，此时机器人不受到与目标点距离的影响，当m≥1时，斥力与目标点距离成正比。将式4.17加入负梯度公式后可得到本发明后的斥力势场函数表达式：

其中ρ₀代表障碍物的影响范围。故根据公式4.18可知，当ρ(p,p_obs)>ρ₀时，斥力为0；当ρ(p,p_obs)≤ρ₀时，机器人受到合成斥力，合成斥力分别由以下两个力组成：

其中，代表障碍物对机器人的斥力，/>代表目标点对机器人的引力。由图36可知，两者的合力更加趋向于目标点。

根据本发明后的斥力势场函数，进一步分析调节因子m对斥力势场函数的影响，具体可分为以下四种情况：

(1)当m＝0时，斥力分量/>故本发明后的斥力势场函数和未本发明前相同，所以障碍物对机器人的斥力不受机器人与目标点之间距离的影响。

(2)当0<m<1时，式中不是连续的，根据定义可知，当机器人与障碍物的相对距离ρ(p,p_obs)<ρ₀且ρ(p,p_obs)≠0时，本发明后的斥力势场函数分量如下所示：

/>

根据上两式可知，当相对距离ρ(p,p_obs)趋向于0时，分量趋向于0，而分量/>趋向于无穷大，由图31和图32可知，机器人的合力为/>与目标点的方向更为接近，故机器人可以到达目标点。

(3)当m＝1时，机器人与障碍物之间的相对距离ρ(p,p_obs)<ρ₀且ρ(p,p_obs)≠0，本发明后的斥力势场函数分量如下式所示：

由公式4.23、4.24可知，当机器人向障碍物移动时ρ(p,p_obs)趋向于0，分量趋向于0，而分量/>趋向于常数a，机器人的合力方向为指向目标点，从而机器人可以达到目标点。

(4)当m>1时，式中的是连续的，根据定义可知，机器人向障碍物移动时ρ(p,p_g)减小，分量/>减小，当机器人到达目标点时ρ(p,p_g)＝0，/>故机器人可以到达目标点。

根据上述可知，通过调节m的值就可以方便的改变机器人受到的斥力大小，从而确保目标点始终为全局最小势能点，保证机器人可以到达目标点。

本发明对人工势场法局部最小值进行了本发明，具体的来讲，对于如何跳出局部最小值，最重要的是对机器人当前的状态进行检测。在路径规划中，通常将机器人理想化成二维空间的一个质点，当机器人处于局部最小值情况下，机器人会在路径规划中的某一位置受到的合力为，从而停止运动，此时机器人周围的势能够会比当前所处位置的势能高^[80]，从而导致机器人无法逃离陷阱。根据人工势场法的定义，可以通过实时计算机器人所处位置的势能进而与单位时间内可到达区域势能进行比较，根据两个势能的大小进行检测机器人是否陷局部最小值，实现步骤如下：

(1)根据机器人传感器获取到的位置距离信息，对距离信息进行实时计算得到机器人所处位置受到的势能合力值U(p)，其中p代表机器人的位置。

(2)根据机器人的步长计算出单位时间内可以到达的位置，设该位置为R(p)，并实时计算机器人到达R(p)时任意一点的势能。设p^*为R(p)区域内的任意位置，则U(p^*)代表此位置的势能值。

(3)机器人所处位置需满足以下函数：

/>

根据上式可知，当机器人的引力和斥力都不为零且单位时间内能够到达位置的势能都大于当前位置的势能时，机器人进入了局部最小值。

本发明增设虚拟目标点，基于传统人工势场法容易陷入局部最小值的缺点，本发明将虚拟目标点与局部最小检验相结合，通过增加虚拟目标点的方法使机器人避免局部最小值的问题，虚拟目标点的加入，打破了机器人二力平衡的状态，由于虚拟目标点对机器人产生引力作用，所以此时机器人受到的引力大于机器人受到的斥力，此时机器人会在虚拟目标点的吸引下跳出困境。当机器人到达虚拟目标点的位置时，虚拟目标点消失，从而机器人按照原本设定的目标点进行路径规划。

虚拟目标点的增设的原理是通过增加目标点的位置，使机器人受到的作用力发生改变，从而确保机器人所在的位置的势能不是最低点。根据人工势场的定义，当检测到机器人陷入局部最小值时，应该增加子目标，子目标的位置应满足以下约束：

U(p_tg)<U(p^*)_min,p⁺∈R(p⁺) (4.26)

上式中U(p_tg)代表虚拟目标点的势能，由不等式可知，虚拟目标点的势能应小于机器人当前所处环境的势能，为了提高机器人跳出局部最优的效率，下面将对几种障碍物位置的情况进行分析：

(1)如下图33所示，当目标点在机器人与障碍物之间时，机器人陷入局部最优。此时在目标点附近增设虚拟目标点，机器人合力为零的状态被打破，从而使机器向目标点继续移动。

(2)如下图34所示，当障碍物位置在机器人与目标点之间时，机器人陷入局部最优。此时在障碍物周围增设虚拟目标点，机器人的合力平衡被打破从而向虚拟目标点移动，当到达子目标后，虚拟目标点消失，继续以原来的目标点进行路径规划。

(3)如图35所示，当目标点被L型障碍物包围时，在距离机器人比较近的障碍物周围增设虚拟目标点，此时，机器人二力平衡被打破而向虚拟目标点移动。当机器人到达虚拟目标点时，虚拟目标点消失，机器人继续以原来的目标带你进行路径规划。

(4)如图36所示，当障碍物呈U型时，当机器人运行到障碍物的缺口处时，机器人无法从另一面跳出，此时应选择距离机器人较近的一侧障碍物的后方处增设虚拟目标点，此时，虚拟目标点会取代原目标点，机器人在其引力作用下走出缺口，从而绕过障碍物，如果障碍物较大的话，可多次增设虚拟目标点，使机器人跳出局部最优。

为了验证上述本发明方法的有效性，通过使用Matlab R2018b编写程序进行仿真实验，对于上述的不同问题进行验证分析，参数设置如下表4.1所示：仿真结果如下。

表4.1仿真参数设置

根据本发明后的斥力势场函数，建立适当的仿真模型后，设置调节因子m＝0.5，机器人的起始坐标设置为(2.5,1.5)，目标点坐标设置为(16.5,16.5)，传统人工势场路径规划和合力变化曲线如图41所示：

由图37可知，采用传统的人工势场法进行规划时，机器人未能够成功的到达目标点。此时机器人所受到的合力不为零且不停的在某个值附近震荡，目标点不是全局势能最小点。通过重构斥力势场函数并增加距离因子后的路径规划图和合力变化曲线如图42所示：

由图38可知，在122次迭代时机器人已经到达全局势能最小点，说明机器人能够不受障碍物的影响到达预先设定的目标点，通过仿真对比可以得出，增加距离因数后的人工势场法能够保持目标点为全局势能最小点，从而确保机器人能够顺利到达目标点。

根据虚拟目标点增设方法，针对上发明所述的几种局部最小值情况进行仿真分析。情况一：当目标点位置在机器人与障碍物之间时，设置机器人的起始点坐标为(0.5,11.5)，目标点坐标为(17.5,11.5)，传统人工势场法路径规划图如图39所示，机器人在还没有达到目标点前因受力平衡而陷入局部最小值问题，所以无法继续移动。在障碍物前增设虚拟目标点，增设虚拟目标点后的路径规划如图40所示：

情况二：当障碍物位置在目标点与机器人之间时，设定机器人的起始点坐标为(1.5,5.5)，目标点坐标为(16.5,12.5)，传统人工势场法如图41所示，机器人在障碍物之前就因受力平衡而陷入局部最小值停止不动。机器人检测到陷入局部最小值后，通过在障碍物附近增加虚拟目标点，打破合力为零的状态，从而使得机器人能够顺利到达目标点。本发明后的路径规划仿真如图42所示：

情况三：当目标点被L型障碍物包围时，设定机器人的起始点坐标为(0.5,19.5)，目标点坐标为(14.5,6.5)，传统人工势场法如图43所示，机器人在障碍物附近因受力平衡而陷入局部最小值停止不动。机器人检测到陷入局部最小值后，在障碍物附近增设虚拟目标点，打破合力为零的状态，从而使机器人顺利到达目标点。增设虚拟目标点后的路径规划如图44所示：

情况四：实际环境中，最容易出现的情况是U型的障碍物，当存在U型障碍物时，设定机器人起点坐标(0.5,0.5)，目标点坐标(19.5,16.5)，传统人工势场法路径仿真图如图45所示，当机器人进入U型区域后，受到的合力为零陷入局部最小值，机器人停止不动。机器人检测到陷入局部最小值后，在U型障碍物距离目标点近的一侧增设虚拟目标点，产生一个对机器人的引力，打破机器人合力为零的状态，使机器人跳出局部最小值，从而顺利到达目标点。增设虚拟目标点后的路径仿真图如图46所示，机器人跳出局部最优后顺利到达目标点。本发明针对于目标不可达的问题，重新构造了斥力势场的函数，并引入了距离因子；针对于局部最小值问题，通过增设虚拟目标点的方法使机器人跳出局部最小值，并根据障碍物位置的不同分别在不同的位置设立虚拟目标点，完善了模型。最后针对于这两个问题分别进行了Matlab仿真，验证了该算法的有效性。

本发明还实现混合路径规划。利用本发明的粒子群算法和人工势场法进行路径规划的流程如下所示：

(1)将机器人工作环境的障碍物信息发送给机器人，然后根据第二章所介绍的栅格法进行建模得到栅格地图。

(2)确定机器人的起点坐标和机器人的终点坐标，在地图模型中使用本发明后的灰狼粒子群算法搜索全局最优路径。

(3)机器人在按照预先设定的路径规划运动的同时，利用自身携带的传感器检测与周围障碍物的距离信息，若传感器检测到事先未知的障碍物，则使用人工势场法进行局部路径规划。

(4)判断是否避过当前的障碍物，如果是进入下一步；如果不是则返回步骤3继续进行局部路径规划。

(5)判断是否到达目标点，如果是，则算法结束；如果不是，则返回步骤3。

为了验证混合路径规划算法的有效性，通过Matlab R2018b编写程序进行仿真，利用本发明的栅格法进行地图建模，地图大小设置为20×20，机器人的起始点坐标为(0,0)，终点坐标为(20,20)，本发明灰狼粒子群进行全局路径规划，得到的全局最优路径如图47所示。

如图48所示，机器人沿着已经规划好的全局路径向目标点移动，当机器人到达A点后，由于与障碍物的距离变小，机器人携带的传感器检测到障碍物，并且通过计算动态障碍物的移动速度判断出如果继续前行的话会与障碍物相撞，此时，机器人通过第四章所述的局部路径规划方法规划出一条合适的路径，然后检测到绕过障碍物后，再返回到原来的路径进行移动。

如图49所示，机器人绕过障碍物Obs1，回到原来的路径继续向前移动，当机器人到达B点后，距离传感器感应到与机器人运行方向相垂直的动态障碍物Obs2，而且机器人通过运算如果继续前行的话会与动态障碍物相碰撞。此时，机器人将停止一段时间，并在动态障碍物的后方绕过，然后再回到原来的全局路径进行前进。

如图50所示，机器人沿着全局最优路径移动到C点时，机器人携带的传感器检测到运动的前方有与和机器人运动方向一致的障碍物，这时，机器人就会检测障碍物的运动速度，并于自身速度进行比较，若自身的运动速度小于障碍物的运动速度时，则保持原有的路径前行；若自身的运动速度大于障碍物的运动速度，则机器人会选在在局部位置进行路径规划，选择在背离静态障碍物的方向超越动态障碍物，超越后继续回到原有的路径进行前行，并最终到达目标点。

通过上述仿真实验可知，本发明中的混合路径规划算法能够解决原有算法不能避开静态和动态障碍物的问题，并且能够顺利的到达所规定的目标点，验证了算法的有效性。发明通过将本发明灰狼粒子群算法和人工势场结合形成一种混合路径规划算法，同时介绍了优化算法的工作原理和步骤，最后通过Matlab仿真实验证明了混合算法的合理性，并且解决了单一算法存在的缺陷，最终使机器人能够顺利的到达目标点。

Claims

1.一种复杂环境下的机器人路径规划方法，其特征在于，方法包括：

步骤二，使用粒子群算法进行全局路径规划；

粒子群算法步骤包括：

(1) 对种群规模、学习因子、迭代次数进行初始化；

(2) 在搜索空间内对粒子的速度和位置进行初始化，粒子的个体最优位置为初始化粒子的位置向量，并利用公式

对种群位置最优值进行更新；

(3) 根据下述方式对粒子的速度进行更新；

粒子在/>维上/>次迭代的计算公式如下面公式所示：

(3.1)

(3.2)

其中，和/>表示学习因子，/>作用是用来控制个体朝着最优值飞行的步长，/>的作用是用来控制种群朝着最优值飞行的步长；/>为第/>个粒子在/>维上k次迭代计算中的位置信息，/>代表当前迭代次数，/>代表最大迭代次数；/>表示在粒子在寻优过程中的历史最优值；/>表示所有粒子达到的最优值，即种群的最优值；/>函数用来随机产生(0,1)之间的数，用来保持种群的多样，/>表示速度和位置的维数；粒子群优化算法的速度由三部分组成：/>表示粒子第/>次迭代时的速度信息，/>表示粒子自身的信息；/>表示种群中粒子进行协作和信息共享的部分；

再按照公式(3.3)对粒子进行约束，如果存在着超出搜索空间的粒子还必须对其进行相关的处理；/>表示粒子在飞行过程中允许的最大速度值；

(4) 计算适应度值，并且根据下述方式更新粒子的个体和整体适应度值；

(3.7)

种群最优值可以用以下的函数进行更新，其中为适应度函数值：

(3.8)

(5) 判断是否满足程序结束条件，如果满足，停止算法迭代并且给出最优解；若不满足，跳转到(3)继续运行；

方法中：使用混沌函数对灰狼粒子群算法的步骤如下：

1) 应用Logistic函数的下述方式对种群粒子的位置和速度进行混沌初始化；

(3.18)

式中，，/>为第/>个变量，/>代表控制变量，当/>时，系统处于完全混沌的状态，其混沌空间的范围为/>；

使用混沌初始化灰狼粒子群的步骤为：首先随机产生一个维向量/>，然后利用公式(3.18)对/>维向量进行映射产生混沌序列/>；然后把混沌序列/>的从混沌空间/>逆映射到变量取值空间/>，得粒子的位置为/>，，/>，最后通过计算粒子的适应度值，并将适应度值较为优秀的一些粒子作为种群的初始化粒子；

2) 评估出粒子的适应度值，并将前三个适应度值最优的粒子设置为、/>和/>；

3)使用(3.15)对粒子/>、/>和/>的位置进行更新，剩余粒子使用(3.17)

进行位置更新；

粒子的速度用(3.16)更新；

式中，代表空间解的维数，/>代表当前迭代次数，/>、/>、/>代表学习因子，/>代表(0,1)之间的随机数，/>代表惯性权重系数；

4) 使用下述群体适应度方差，判断算法是否陷入早熟，若是，对早熟的粒子进行混沌处理，然后再跳转到3)；

(3.19)

上式中为种群规模大小，/>代表第/>个粒子的适应度值，/>代表当前粒子群的平均适应度值；群体方差/>反映了粒子的早熟状态；

5) 判断算法是否满足终止条件，若满足停止迭代并输出结果，不满足继续跳转到3)进行计算；

步骤五，将全局路径规划和局部路径规划结合成混合路径规划，利用灰狼粒子群进行全局路径规划，针对于机器人在运行过程中遇到的未知障碍物使用人工势场法进行局部路径规划，并最终到达预期的目标点。

2.根据权利要求1所述的机器人路径规划方法，其特征在于，

步骤一中，

(1) 对地图上的障碍物形状进行分析并选定；

(2) 将障碍物所有顶点的坐标构成一个包含障碍物的矩形，得出边长的最大和最小值、/>；

(3) 求解每个矩形的的面积：

(2.1)

式中的为矩形的长，/>为矩形的宽，/>为第/>个矩形；

(4) 求解个矩形的面积/>：

(2.2)

式中为所有矩形面积的总和；

(5) 第个栅格的长度为：

(2.3)

栅格粒度长度：

(2.4)

栅格总数：

(2.5)

式中：为地图的长；

为地图的宽；

的值为：100~900。

3.根据权利要求1所述的机器人路径规划方法，其特征在于，

对于环境中的不规则障碍物膨胀设置为矩形状规则障碍物，同时利用V-rep仿真软件建立了真实环境下的地图模型，并在Matlab中对所得地图进行处理，最终得到栅格地图模型；

膨胀的模型如式所示：

(2.7)

式中表示将要进行处理的栅格，/>表示基本元素，/>表示膨胀处理之后栅格信息，表示栅格的坐标；整个膨胀处理的过程用数学形态学解释/>是通过结构元素/>对/>进行边缘膨胀所得，最终以/>在栅格地图中进行表示。

4.根据权利要求3所述的机器人路径规划方法，其特征在于，

创建障碍物的3D模型，然后将障碍物模型导入V-rep仿真软件中建立实际的空间环境模型；

将搭建好的带有激光雷达的机器人模型置于空间内；

使用Python编写机器人控制程序，控制程序在仿真软件运行之后通过V-rep所提供的联调接口对环境空间内的小车进行移动控制，V-rep通过所创建的Socket接口将雷达传感器所获取的数据传送给Matlab模块，Matlab模块通过对雷达的实时数据进行处理，进而转化为地图模型；

运行V-rep软件后，机器人所携带的激光雷达开始向机器人前方发射激光；

最后运行机器人控制程序和Matlab地图绘制程序，通过键盘控制机器人在空间内进行移动，使雷达传感器能够扫描完整个空间；在机器人运动过程中，经雷达扫描过的空间信息通过Matlab实时处理后，得到的原始环境地图模型；

对雷达数据生成的原始环境地图模型使用Matlab模块进行二值处理，转化为二值图像；再通过rgb2gray函数将二值图像转换为灰度图，最后转换为栅格图，得到栅格地图模型。

5.根据权利要求1所述的机器人路径规划方法，其特征在于，

方法中，当机器人在某种环境下进行路径规划时，其人工势场法的步骤为：

S101，对各项参数进行初始化，并给出预设目标点的位置、机器人本身的位置；

S102，根据势场的引力函数和斥力函数对机器人所受到的合力进行计算，并且推出与x轴的夹角；

S103，根据机器人位置的更新公式计算机器人下一步的位置；

S104，根据目标点对机器人的引力是否为零判断机器人是否到达目标点；

S105，如果机器人未到达目标点，转回S102；如果已经到达，则算法结束。

6.根据权利要求5所述的机器人路径规划方法，其特征在于，

斥力势场函数的表达式为：

(4.17)

式中，代表增益系数，/>代表机器人本身坐标，/>代表障碍物位置坐标，/>代表目标点坐标，/>代表障碍物斥力范围，/>代表机器人与障碍物之间的距离，/>代表机器人与目标点的距离，/>代表调节因子，其取值范围/>；

随机机器人与目标点之间距离的减小，机器人受到的斥力会受到调节函数的影响，当/>时，调节函数为1，此时机器人不受到与目标点距离的影响；

当时，斥力与目标点距离成正比；将式(4.17)加入负梯度公式后，得到更新后的斥力势场函数表达式：

(4.18)

其中代表障碍物的影响范围；故根据公式(4.18)可知，当/>时，斥力为0；

当时，机器人受到合成斥力，合成斥力分别由以下两个力组成：

(4.19)

(4.20)

其中，代表障碍物对机器人的斥力，/>代表目标点对机器人的引力；

根据斥力势场函数，分析调节因子对斥力势场函数/>的影响，具体分为以下四种情况：

(1) 当时，/>，斥力分量/>，故斥力势场函数和未更新前相同，所以障碍物对机器人的斥力不受机器人与目标点之间距离的影响；

(2) 当时，式中/>不是连续的，当机器人与障碍物的相对距离且/>时，更新后的斥力势场函数分量如下所示：

(4.21)

(4.22)

根据上两式可知，当相对距离趋向于0时，分量/>趋向于0，而分量/>趋向于无穷大，机器人的合力为/>与目标点的方向更为接近，故机器人到达目标点；

(3) 当时，机器人与障碍物之间的相对距离/>且/>，斥力势场函数分量如下式所示：

(4.23)

(4.24)

由公式(4.23)、(4.24)可知，当机器人向障碍物移动时趋向于0，分量/>趋向于0，而分量/>趋向于常数/>，机器人的合力方向为指向目标点，从而机器人达到目标点；

(4) 当时，式中的/>是连续的，根据定义可知，机器人向障碍物移动时/>减小，分量/>减小，当机器人到达目标点时/>，/>；故机器人到达目标点。

7.根据权利要求5所述的机器人路径规划方法，其特征在于，

通过实时计算机器人所处位置的势能，并与单位时间内可到达区域势能进行比较，根据两个势能的大小进行检测机器人是否陷局部最小值，实现步骤如下：

S201，根据机器人传感器获取到的位置距离信息，对距离信息进行实时计算得到机器人所处位置受到的势能合力值，其中/>代表机器人的位置；

S202，根据机器人的步长计算出单位时间内可以到达的位置，设该位置为，并实时计算机器人到达/>时任意一点的势能；设/>为/>区域内的任意位置，则/>代表此位置的势能值；

S203，机器人所处位置需满足以下函数：

(4.25)

为引力势场，/>为斥力势场；

8.根据权利要求7所述的机器人路径规划方法，其特征在于，

当检测到机器人陷入局部最小值时，增加子目标，子目标的位置应满足以下约束：

(4.26)

上式中代表虚拟目标点的势能，由不等式可知，虚拟目标点的势能应小于机器人当前所处环境的势能，为了提高机器人跳出局部最优的效率，下面将对几种障碍物位置的情况进行分析：

当目标点在机器人与障碍物之间时，机器人陷入局部最优；此时在目标点附近增设虚拟目标点，机器人合力为零的状态被打破，使机器向目标点继续移动；

当障碍物位置在机器人与目标点之间时，机器人陷入局部最优；此时在障碍物周围增设虚拟目标点，机器人的合力平衡被打破从而向虚拟目标点移动，当到达子目标后，虚拟目标点消失，继续以原来的目标点进行路径规划；

当目标点被L型障碍物包围时，在距离机器人比较近的障碍物周围增设虚拟目标点，此时，机器人二力平衡被打破而向虚拟目标点移动；当机器人到达虚拟目标点时，虚拟目标点消失，机器人继续以原来的目标带你进行路径规划；

当障碍物呈U型时，当机器人运行到障碍物的缺口处时，机器人无法从另一面跳出，此时选择距离机器人较近的一侧障碍物的后方处增设虚拟目标点；虚拟目标点会取代原目标点，机器人在其引力作用下走出缺口，从而绕过障碍物，如果障碍物较大的话，多次增设虚拟目标点，使机器人跳出局部最优。