CN113805597A - 基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法 - Google Patents

Abstract

本发明提出一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，引入粒子群算法，并结合智能车在转弯过程中存在最小转弯半径的转弯约束，即在优化过程中加入最大转向角的约束，对初步规划的路线进行曲线优化，并建立相应适应度函数，进一步采用粒子群算法限制寻优范围并找到符合智能车转向特性的航向角，通过粒子不断迭代得到最优的航向角度，从而建立粒子群障碍物自我保护人工势场法避开障碍物，找到符合智能车转向约束的最优路径。

Description

基于粒子群算法的障碍物自我保护人工势场法局部路径规划 方法

技术领域

本发明属于智能驾驶路径规划和自主导航,无人驾驶汽车和移动机器人避障的局部路径规划技术领域，尤其涉及一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法。

背景技术

近几年随着计算机技术的发展，汽车逐渐朝着与电子技术、网络通信相结合的智能化方向发展，智能汽车有利于改善交通安全、减缓道路拥堵、提高社会效率、倡导低碳生活等。智能车的关键技术包括环境感知、导航定位、路径规划以及决策控制等，而路径规划是智能车辆的关键部分，对智能车的研究具有重大意义。

智能车是智能领域的研究热点与难点，路径规划是智能车的重要组成部分，其目的是在已知起始点和目标点的环境状态下通过算法避开动静态障碍物，寻找一条最优(距离最短或时间最少)路径。路径规划又分为全局规划(固定环境)和局部规划(动态环境)。全局路径规划是收集智能车行驶区域内的所有环境信息为基础，再根据信息绘制地图，在输入起点和终点后，利用算法搜索一条安全、经济的路径。而局部路径规划是智能车利用传感器收集的环境信息，实时进行路径规划，对车载传感器要求较高。在实际过程中，智能车同时涉及全局规划和局部规划，算法的优劣直接影响到规划路径的效果。

发明内容

本发明的目的是提供一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，在传统人工势场法基础上，分别提出了加入虚拟障碍物以解决局部最小问题和加入距离因子解决目标不可达问题，并提出基于粒子群算法的障碍物自我保护人工势场法以解决静态环境下多障碍物的路径规划问题，为了得到智能车最优的旋转角度，本发明引入粒子群算法，并结合智能车在转弯过程中存在最小转弯半径的转弯约束，即在优化过程中加入最大转向角的约束，对初步规划的路线进行曲线优化，并建立相应适应度函数，进一步采用粒子群算法限制寻优范围并找到符合智能车转向特性的航向角，通过粒子不断迭代得到最优的航向角度，从而建立粒子群障碍物自我保护人工势场法避开障碍物，找到符合智能车转向约束的最优路径。最后通过MATLAB平台进行仿真，验证算法的有效性。

人工势场法作为应用广泛的路径规划算法，经常被应用于路径规划问题。本发明在前人改进人工势场法基础上，提出了在静态环境下更加安全的避障策略。

本发明所进行的工作主要包括：第一，总结前人改进人工势场法的可行性与不足，针对距离因子人工势场法在复杂环境下无法避开障碍物问题，提出了障碍物自我保护人工势场法(OSPAPF)。第二：为了找到最优航向角，提出了基于粒子群算法寻优的障碍物自我保护人工势场法(PSO-OSPAPF)，通过旋转航向角进而避开障碍物，建立相应适应度函数，进一步采用粒子群算法(PSO)限制寻优范围找到符合智能车转向特性的航向角。

本发明具体采用以下技术方案：

一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于，包括以下步骤：

步骤S1：初始化人工势场法参数，读取智能车当前坐标、障碍物坐标和目标点坐标，并对不规则障碍物进行标定：将不规则障碍物其标定为矩形障碍物，把矩形分为若干个正方形，并对每个正方形设置障碍物自我保护半径；

步骤S2：计算智能车当前与每个障碍物的角度，计算斥力、引力、合力：包括计算出障碍物与智能车当前位置的角度以及每个障碍物对车辆的斥力，目标点对障碍物的引力，最终求出合力；

步骤S3：判断下一步位置是否进入障碍物自我保护区位置：以每个正方形障碍物为圆心，半径为R设置障碍物自我保护区，以保证智能车与障碍物不发生碰撞；如进入障碍物自我保护区，则执行步骤S4-步骤S8；

步骤S4：调用粒子群算法寻优，随机初始化粒子，计算初始粒子的适应度值和最优值；

步骤S5：加入车辆转向特性约束寻优，更新迭代后每个粒子的速度和位置，计算每个粒子的函数适应值；

步骤S6：更新迭代后每个粒子历史最优值，计算群体的全局最优值；

步骤S7：当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；

步骤S8：得到最优航向角后回到步骤S3判断下一步位置是否进入障碍物自我保护区。

进一步地，智能车在运动过程中，判断是否陷入局部最小值点：运动至某一位置时所受的引力和斥力大小相同，方向相反，在此路径点智能车所受的合力为零，智能车该处为整个环境中势场最低位置；

如果陷入局部最小值点，则通过在障碍物和智能车连线的中垂线交点处设置虚拟障碍物,使智能车跳出局部最小值点。

进一步地，通过加入距离因子，以避免目标点不可达的情形。

进一步地，在步骤S1中，将不规则障碍物其标定为矩形障碍物，设障碍物的横坐标、纵坐标最大、最小值分别为x_max,x_min,y_max,y_min，则矩形四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，L_g为标定后矩形的长度，W_d为标定后矩形的宽度；把矩形分为若干个正方形，正方形的个数N₀：

其中ceil为向上取整函数；

正方形的边长为L_o＝W_d，第i_o个正方形的中心坐标为：

障碍物自我保护半径为：

进一步地，步骤S3的具体执行过程为：

设当智能车运动到位置A时，根据当前路径点合力角度值α与步长L，下一步将到达位置B，如果判断位置B在障碍物自我保护区内，则将合力旋转θ，在新合力角α′与步长的作用下可到达位置D，将智能车调整至远离障碍物方向，防止相撞；新合力角度确定方式为：已知障碍物i中心坐标位置为

目标点坐标为(x_G.y_G)，智能车位置坐标为(x_C,y_C)，由合力角度α与步长l，下一路径点位置为:

当判断下一位置进入障碍物自我保护区时，计算智能车位置与障碍物横坐标的差值

根据Δx正负决定合力旋转角θ，当Δx≥0时，表示障碍物位于下一位置右侧，下一位置应该往左旋转，因此在合力角α基础上加θ，此时合力与横轴夹角α′＝α+θ，反之当Δx＜0时表示障碍物位于下一位置左侧，则α′＝α-θ，此时下一路径点为：

进一步地，在步骤S3中，障碍物自我保护区的大小能根据智能车和不同类型的障碍物安全距离设置不同的值。

进一步地，步骤S4具体为：

首先初始化粒子，每个粒子具有位置和速度两个属性；其中位置代表的是新合力角α′，速度表示α′变化大小，每个粒子根据速度和位置的更新计算对应适应度值，根据适应度值判断粒子优劣；在搜索过程中最优值分为个体最优与全局最优，个体最优值记为个体极值α′_pbest，粒子群体之间通过分享个体极值信息，选出粒子群中个体极值中适应度值最优的值作为全局最优解，记为全局极值α′_qbest；在迭代过程中粒子根据α′_pbest和α′_qbest调整速度和位置属性，使粒子逐渐靠近最优值，从而找到最优解；假设存在N_l个粒子，每次迭代过程中，粒子的速度和位置表达式为：

V_j(t+1)＝wV_j(t)+c₁r₁(α′_pbestj(t)-α′_j(t))+c₂r₂(α′_gbestj(t)-α′_j(t))

α′_j(t+1)＝α′_j(t)+V_j(t+1),j＝1,2,…N_l

式中，V_j(t+1)和α′_j(t+1)分别是粒子j在第t+1次迭代时的速度和合力角度值，新合力角度的范围为在原合力角的基础上加减任意值，速度值表示例子的运动快慢；α′_pbestj(t)是粒子j在第t次迭代个体最优值，α′_gbestj(t)是第t次迭代全局最优值；w为惯性权值，反应粒子的个体极值对现在值的影响程度；c₁和c₂分别是个体和群体的加速度因子，表示粒子自身的寻优过程中产生的经验值与其他粒子积累经验值，直接影响各粒子的下一步运行轨迹，r₁和r₂是0到1之间的随机数；

步骤S5具体为：

设L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角；智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式和最大转角即最大内转角表达式为：

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式为：

α-α₂≤α′≤α+α₂

智能车在转弯过程有最大转角限制，基于粒子群寻优的障碍物自我保护人工势场法寻优角度值应被限制在智能车最大转角范围内，使规划获得的路径符合智能车运动学模型；

适应度函数计算中，b(i)表示下一刻位置(x_n,y_n)与第i个障碍物中心

的欧式距离，d(i)表示下一位置与目标点的距离，数学表达式分别为:

在路径规划过程中下一位置应离障碍物中心远，即b(i)越大越好，且距离目标点尽量近，即d(i)越小越好；将两个距离合并建立适应度函数f(i)，适应度值越大越安全，表达式为:

但坐标系中会出现智能车离障碍物很近，离目标点距离很远，适应度函数表现为b(i)很小，d(i)很大，导致b(i)与c(i)相差几百倍，适应度值计算结果偏向某一数值，每个粒子的适应度值不具代表性，因此将两者进行归一化处理为无量纲的值，把两者的距离都映射到[0,1]之间，映射表达式及适应度函数表达式分别为:

f(i)＝b(i)′+c(i)′；

步骤S6具体为：

给定粒子群算法最大迭代次数，粒子速度范围，位置寻优范围，并给定种群粒子数量(N_l),随机产生N_l个角度值α′_j构成初始种群，j＝1,2,…N_l；根据步骤S5给出的适应度函数计算种群中每个个体对第i个障碍物适应度值f(i)，根据个体极值和全局极值调整速度和位置，使粒子逐渐接近最优值，从而找到最优解；记录各个粒子的个体极值α′_pbestj及全局极值α′_gbestj；

步骤S7具体为：

将全局极值与历史最优值对比，更新粒子的速度与位置，当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；

在步骤S8中，将输出最优的航向角α′和步长带入步骤S3路径公式，并返回步骤S3，判断下一步位置是否进入障碍物自我保护区位置。

进一步地，通过设置固定的时间间隔内的路径长度L₀与智能车从位置A运动到位置B走过路径长度L进行比较，如果L₀>L，则判断智能车在运动过程中陷入局部最小值。

进一步地，引入距离因子后斥力势场函数如下式所示：

其中ρ(X,X_goal)ⁿ为引入的距离因子，ρ(X,X_goal)表示智能车与目标点位置的欧式距离，k_rep为斥力场增益系数，N表示障碍物个数，

为智能车位置与第i个障碍物的距离，ρ_o为障碍物斥力的影响范围，表示只有智能车进入斥力影响范围内才产生斥力势场。对势场求负梯度得到斥力的表达式为：

进一步地，采用MATLAB上连线优化后的路径，将无人车运动轨迹可视化。

相较于现有技术，本发明及其优选方案在传统人工势场法基础上，分别提出了加入虚拟障碍物以解决局部最小问题和加入距离因子解决目标不可达问题，并提出基于粒子群算法的障碍物自我保护人工势场法以解决静态环境下多障碍物的路径规划问题，为了得到智能车最优的旋转角度，本发明引入粒子群算法，并结合智能车在转弯过程中存在最小转弯半径的转弯约束，即在优化过程中加入最大转向角的约束，对初步规划的路线进行曲线优化，并建立相应适应度函数，进一步采用粒子群算法限制寻优范围并找到符合智能车转向特性的航向角，通过粒子不断迭代得到最优的航向角度，从而建立粒子群障碍物自我保护人工势场法避开障碍物，找到符合智能车转向约束的最优路径。最后通过MATLAB平台进行仿真，验证算法的有效性。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

图1为本发明实施例不规则障碍物标定原理图。

图2为本发明实施例多障碍物环境下受力分析示意图。

图3为本发明实施例障碍物自我保护区示意图。

图4为本发明实施例旋转角计算示意图。

图5为本发明实施例智能车简易转弯模型示意图。

图6为本发明实施例适应度函数计算原理示意图。

图7为本发明实施例虚拟障碍物的设置示意图。

图8为本发明实施例应用实例示意图((a)限制寻优角度路径，(b)局部放大路径，(c)[0,2π]转角，(d)[α-π/4,α+π/4]转角)。

图9为本发明实施例方法路径规划流程示意图。

具体实施方式

为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图1-图9所示，本实施例提供的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法包括以下实施过程：

步骤一：初始化人工势场法参数，读取无人车当前坐标、障碍物坐标和目标点坐标；

通过读取车载传感器如相机、激光雷达、GPS、惯性测量单元(IMU)等得到车辆在地图中的坐标信息，但是智能车运动中传感器收集的障碍物并不都是规则的，此时把障碍物简化为一个质点进行受力分析得到的路径点在不规则障碍物的凹凸处会出现转角较大的情况，无法满足智能车运动要求，因此本发明提出不规则障碍物的标定方法，如图1所示，先将不规则障碍物其标定为矩形障碍物，设障碍物的横坐标、纵坐标最大、最小值分别为x_max,x_min,y_max,y_min，则矩形四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)。针对细长条形状的障碍物只设置一个障碍物自我保护半径R的取值会很大,并且周围无障碍物区域也划进障碍物自我保护区，导致智能车路径规划时无法得到最优的路径，因此障碍物分为几个最小单元更加合理。图中L_g为标定后矩形的长度，W_d为标定后矩形的宽度。把矩形分为若干个正方形，正方形的个数N₀：

其中ceil为向上取整函数(保证不足一个正方形部分的障碍物视为一个)，正方形的边长为L_o＝W_d，第i_o个正方形的中心坐标为：

障碍物自我保护半径(略大于

保证自我保护区两两完全相交包围障碍物):

在非结构化道路上，道路中存在的障碍物形状是不确定的，通过标定障碍物能够在智能车和障碍物最小单元之间设置一个安全距离，针对凹凸不平的障碍物,算法能够找到最优路径。

步骤二：计算智能车当前与每个障碍物的角度，计算斥力，引力，合力；

在读取传感器的障碍物信息并建立障碍物自我保护区后，如图2所示计算出障碍物与无人车当前位置的角度以及每个障碍物对车辆的斥力，目标点对障碍物的引力，最终求出合力。

步骤三：计算下一步位置是否进入障碍物自我保护区位置；

判断小车下一步是否进入障碍物自我保护区算法原理如图3所示，以每个障碍物为圆心，半径为R设置障碍物自我保护区，半径可根据不同障碍物类型设置不同的值，保证智能车与障碍物不发生碰撞。当智能车运动到位置A时，根据当前路径点合力角度值α与步长L，下一步将到达位置B，算法判断位置B在障碍物自我保护区内，将合力旋转θ，在新合力角α′与步长的作用下可到达位置D，将智能车调整至远离障碍物方向，防止相撞。新合力角度确定方式如图4所示：已知障碍物i中心坐标位置为

目标点坐标为(x_G.y_G)，智能车位置坐标为(x_C,y_C)，由合力角度α与步长l，下一路径点位置为:

当算法判断下一位置进入障碍物自我保护区时，计算智能车位置与障碍物横坐标的差值

根据Δx正负决定合力旋转角θ，当Δx≥0时，表示障碍物位于下一位置右侧，下一位置应该往左旋转，因此在合力角α基础上加θ(逆时针方向为正)，此时合力与横轴夹角α′＝α+θ，反之当Δx＜0时表示障碍物位于下一位置左侧，则α′＝α-θ，此时下一路径点为：

障碍物自我保护可根据智能车与障碍物的位置关系决定旋转角的方向，采用此方法能够避开智能车、障碍物与目标点三者共线的情况，加入障碍物自我保护后，改变合力的方向即改变智能车航向角的方向，另外，障碍物自我保护区的大小能根据智能车和不同类型的障碍物安全距离设置不同的值，保证智能车行驶安全。

步骤四：调用粒子群算法寻优，随机初始化粒子，计算初始粒子的适应度值和最优值；

粒子群算法首先初始化粒子，每个粒子具有位置和速度两个属性。本实施例中位置代表的是新合力角α′，速度表示α′变化大小，每个粒子根据速度和位置的更新计算对应适应度值，根据适应度值判断粒子优劣。在搜索过程中最优值分为个体最优与全局最优，个体最优值记为个体极值α′_pbest，粒子群体之间通过分享个体极值信息，选出粒子群中个体极值中适应度值最优的值作为全局最优解，记为全局极值α′_qbest。在迭代过程中粒子根据α′_pbest和α′_qbest调整速度和位置属性，使粒子逐渐靠近最优值，从而找到最优解。假设存在N_l个粒子，每次迭代过程中，粒子的速度和位置表达式为：

V_j(t+1)＝wV_j(t)+c₁r₁(α′_pbestj(t)-α′_j(t))+c₂r₂(α′_gbestj(t)-α′_j(t))

α′_j(t+1)＝α′_j(t)+V_j(t+1),j＝1,2,…N_l

式中，V_j(t+1)和α′_j(t+1)分别是粒子j在第t+1次迭代时的速度和合力角度值，新合力角度的范围为在原合力角的基础上加减任意值，速度值表示例子的运动快慢，太快可能导致粒子直接越过最优值，速度值太小导致算法寻优时间增加；α′_pbestj(t)是粒子j在第t次迭代个体最优值，α′_gbestj(t)是第t次迭代全局最优值；w为惯性权值，一般取0.1～0.4之间，反应粒子的个体极值对现在值的影响程度；c₁和c₂分别是个体和群体的加速度因子，表示粒子自身的寻优过程中产生的经验值与其他粒子积累经验值，直接影响各粒子的下一步运行轨迹，r₁和r₂是0到1之间的随机数。

步骤五：加入车辆转向特性约束寻优，更新迭代后每个粒子的速度和位置，计算每个粒子的函数适应值；

现有的路径规划算法，通常把智能车看成一个可运动的质点，在实际行驶中，智能车在转弯、制动等运动情况下，为了不发生侧滑等不安全动作，规划路径应该满足智能车的运动特性。根据车辆转向的特点，转向过程中的最大转角是有限制的，如图5所示。图中L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角。智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式和最大转角即最大内转角表达式为：

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式为：

α-α₂≤α′≤α+α₂

智能车在转弯过程有最大转角限制，基于粒子群寻优的障碍物自我保护人工势场法寻优角度值应被限制在智能车最大转角范围内，从而规划出来的路径符合智能车运动学模型，为下一步轨迹追踪奠定基础。

适应度函数计算原理如图6所示，b(i)表示下一刻位置(x_n,y_n)与第i个障碍物中心

的欧式距离，d(i)表示下一位置与目标点的距离，数学表达式分别为:

在路径规划过程中下一位置应离障碍物中心远，即b(i)越大越好，且距离目标点尽量近，即d(i)越小越好。将两个距离合并建立适应度函数f(i)，适应度值越大越安全，表达式为:

但坐标系中会出现智能车离障碍物很近，离目标点距离很远，适应度函数表现为b(i)很小，d(i)很大，导致b(i)与c(i)相差几百倍，适应度值计算结果偏向某一数值，每个粒子的适应度值不具代表性，因此将两者进行归一化处理为无量纲的值，把两者的距离都映射到[0,1]之间，映射表达式及适应度函数表达式分别为:

f(i)＝b(i)′+c(i)′。

步骤六：更新迭代后每个粒子历史最优值，计算群体的全局最优值；

给定粒子群算法最大迭代次数，粒子速度范围，位置寻优范围，并给定种群粒子数量(N_l),随机产生N_l个角度值α′_j构成初始种群，j＝1,2,…N_l。根据步骤五给出的适应度函数计算种群中每个个体对第i个障碍物适应度值f(i)，根据个体极值和全局极值调整速度和位置，使粒子逐渐接近最优值，从而找到最优解。记录各个粒子的个体极值α′_pbestj及全局极值α′_gbestj。

步骤七：当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；

将全局极值与历史最优值对比，更新粒子的速度与位置，当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′。

步骤八：得到最优航向角后回到步骤三判断下一步位置是否进入障碍物自我保护区；

将输出最优的航向角α′和步长带入步骤三路径公式，并判断下一位置是否位于障碍物自我保护区，如果落入障碍物自我保护区，则回到步骤三重复操作；如果位于障碍物自我保护区以外，则进行步骤九操作。

步骤九：判断是否陷入局部最小值点；

智能车在运动过程中，因周围障碍物的影响，运动至某一位置时所受的引力和斥力大小相同，方向相反，在此路径点智能车所受的合力为零，算法判断智能车该处为整个环境中势场最低位置，如果没有外力的作用，智能车将不再往终点运动或者在路径点周围徘徊，此路径点称为局部最小值点。

针对局部最小值问题，本发明原理如图7所示，算法先判断智能车是否陷入局部最小值，通过设置固定的时间间隔内的路径长度L₀与智能车从位置A运动到位置B走过路径长度L进行比较，如果L₀>L，则智能车在运动过程中陷入局部最小值，本发明方法在障碍物和智能车连线的中垂线交点处设置虚拟障碍物,加入虚拟障碍物后智能车受到的斥力发现发生改变，这样就打破了原先的平衡，从而跳出局部最小值点，在目标点指引下继续前进。

步骤十：判断是否可达目标点，当无人车到到达目标点时，在MATLAB上连线优化后的路径，将无人车运动轨迹可视化；

目标不可达问题产生原因是目标点周围存在障碍物，智能车在靠近目标点同时两者距离在减小，根据传统人工势场引力函数可知引力势场趋于0，但目标点周围有障碍物即存在斥力势场，目标点位置势场不是整个环境中最低的，导致智能车在目标点附近震荡。如果智能车逐渐靠近目标点的同时让斥力势场也逐渐减少，让目标点成为整个环境中势场最低点，就能避免目标点不可达的情况。针对目标不可达问题，本发明在传统人工势场法模型的基础上加入距离因子，引入距离因子后斥力势场函数如公式所示：

其中ρ(X,X_goal)ⁿ为引入的距离因子，ρ(X,X_goal)表示智能车与目标点位置的欧式距离，N表示障碍物个数，

为智能车位置与第i个障碍物的距离，ρ_o为障碍物斥力的影响范围，表示只有智能车进入斥力影响范围内才产生斥力势场。对势场求负梯度得到斥力的表达式为：

为了验证本发明方法的有效性，在图8所示在地图中随机选取14处标定好的障碍物，结合本发明算法路径规划，图8(a)为在相同障碍物环境下采用寻优角度限制前后的路径对比图，根据乘用车对应的前轮最大转向角范围35°～45°之间，故本发明设置最大转角值为π/4，未设置寻优范围算法寻优范围为整个空间即[0,2π]。为了便于观察，将路径一部分放大，如图8(b)所示为选取x轴与y轴(600,850)区间路径对比，由图中结果可知，设置寻优角度范围[α-π/4,α+π/4]的路径更加平滑，转弯弧度平滑，震荡减少。如图8(c)所示为未设置寻优角度范围在477个路径点中，计算的相邻两两路径点的角度差值，即为智能车在每个路径点的转角(以逆时针为正方向)，未设置寻优范围的路径转角较多且前后路径点转角值较大，图8(d)为寻优角度值的转角大小变化，与设置前相比转角明显变小，路径更加平滑。

随机运行10次取寻优范围分别为[α-π/4,α+π/4]与[0,2π]，取路径长度，转角最大与最小值，平均值对比结果如下表所示。由下表可知设定寻优范围后，路径长度减少7.8％，最大转角(以逆时针为正方向)由51.1182°限制为3.6634°，负方向最大转角也由49.8706°限制为3.6169°，限制寻优范围后得到的路径更加平滑，符合智能车转向特性。

优化前后仿真结果比较

本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。

Claims (10)

1.一种基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于，包括以下步骤：

步骤S1：初始化人工势场法参数，读取智能车当前坐标、障碍物坐标和目标点坐标，并对不规则障碍物进行标定：将不规则障碍物其标定为矩形障碍物，把矩形分为若干个正方形，并对每个正方形设置障碍物自我保护半径；

步骤S2：计算智能车当前与每个障碍物的角度，计算斥力、引力、合力：包括计算出障碍物与智能车当前位置的角度以及每个障碍物对车辆的斥力，目标点对障碍物的引力，最终求出合力；

步骤S3：判断下一步位置是否进入障碍物自我保护区位置：以每个正方形障碍物为圆心，半径为R设置障碍物自我保护区，以保证智能车与障碍物不发生碰撞；如进入障碍物自我保护区，则执行步骤S4-步骤S8；

步骤S4：调用粒子群算法寻优，随机初始化粒子，计算初始粒子的适应度值和最优值；

步骤S5：加入车辆转向特性约束寻优，更新迭代后每个粒子的速度和位置，计算每个粒子的函数适应值；

步骤S6：更新迭代后每个粒子历史最优值，计算群体的全局最优值；

步骤S7：当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；

步骤S8：得到最优航向角后回到步骤S3判断下一步位置是否进入障碍物自我保护区。

2.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：

智能车在运动过程中，判断是否陷入局部最小值点：运动至某一位置时所受的引力和斥力大小相同，方向相反，在此路径点智能车所受的合力为零，智能车该处为整个环境中势场最低位置；

如果陷入局部最小值点，则通过在障碍物和智能车连线的中垂线交点处设置虚拟障碍物,使智能车跳出局部最小值点。

3.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：通过加入距离因子，以避免目标点不可达的情形。

4.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：

在步骤S1中，将不规则障碍物其标定为矩形障碍物，设障碍物的横坐标、纵坐标最大、最小值分别为x_max,x_min,y_max,y_min，则矩形四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，L_g为标定后矩形的长度，W_d为标定后矩形的宽度；把矩形分为若干个正方形，正方形的个数N₀：

其中ceil为向上取整函数；

正方形的边长为L_o＝W_d，第i_o个正方形的中心坐标为：

障碍物自我保护半径为：

5.根据权利要求4所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：步骤S3的具体执行过程为：

设当智能车运动到位置A时，根据当前路径点合力角度值α与步长L，下一步将到达位置B，如果判断位置B在障碍物自我保护区内，则将合力旋转θ，在新合力角α′与步长的作用下可到达位置D，将智能车调整至远离障碍物方向，防止相撞；新合力角度确定方式为：已知障碍物i中心坐标位置为

目标点坐标为(x_G.y_G)，智能车位置坐标为(x_C,y_C)，由合力角度α与步长l，下一路径点位置为:

当判断下一位置进入障碍物自我保护区时，计算智能车位置与障碍物横坐标的差值

根据Δx正负决定合力旋转角θ，当Δx≥0时，表示障碍物位于下一位置右侧，下一位置应该往左旋转，因此在合力角α基础上加θ，此时合力与横轴夹角α′＝α+θ，反之当Δx＜0时表示障碍物位于下一位置左侧，则α′＝α-θ，此时下一路径点为：

6.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：在步骤S3中，障碍物自我保护区的大小能根据智能车和不同类型的障碍物安全距离设置不同的值。

7.根据权利要求5所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：

步骤S4具体为：

首先初始化粒子，每个粒子具有位置和速度两个属性；其中位置代表的是新合力角α′，速度表示α′变化大小，每个粒子根据速度和位置的更新计算对应适应度值，根据适应度值判断粒子优劣；在搜索过程中最优值分为个体最优与全局最优，个体最优值记为个体极值α′_pbest，粒子群体之间通过分享个体极值信息，选出粒子群中个体极值中适应度值最优的值作为全局最优解，记为全局极值α′_qbest；在迭代过程中粒子根据α′_pbest和α′_qbest调整速度和位置属性，使粒子逐渐靠近最优值，从而找到最优解；假设存在N_l个粒子，每次迭代过程中，粒子的速度和位置表达式为：

V_j(t+1)＝wV_j(t)+c₁r₁(α′_pbestj(t)-α′_j(t))+c₂r₂(α′_gbestj(t)-α′_j(t))

α′_j(t+1)＝α′_j(t)+V_j(t+1),j＝1,2,…N_l

式中，V_j(t+1)和α′_j(t+1)分别是粒子j在第t+1次迭代时的速度和合力角度值，新合力角度的范围为在原合力角的基础上加减任意值，速度值表示例子的运动快慢；α′_pbestj(t)是粒子j在第t次迭代个体最优值，α′_gbestj(t)是第t次迭代全局最优值；w为惯性权值，反应粒子的个体极值对现在值的影响程度；c₁和c₂分别是个体和群体的加速度因子，表示粒子自身的寻优过程中产生的经验值与其他粒子积累经验值，直接影响各粒子的下一步运行轨迹，r₁和r₂是0到1之间的随机数；

步骤S5具体为：

设L为前后轮轴距，(x_c,y_c,α)表示智能车位姿，(x_c,y_c)为智能车在二维空间中的坐标，α为当前时刻智能车的航向角，同时也是人工势场法中的合力角；智能车在转弯时存在最小转弯半径R_min，前轮最大内转角为α₂，前轮最大外转角α₁，车辆在转弯的过程中，内外转角与车辆的类型、自身速度和加速度有关，设v_x为运动过程中的纵向速度，a_y为横向加速度，其中最小转弯半径表达式和最大转角即最大内转角表达式为：

智能车在转向过程中内轮转角比外轮转角大，在转弯的过程中转角不超过内外轮转角的最大值，即重新寻找的合力角度值α′与前轮内转角之间的关系为式为：

α-α₂≤α′≤α+α₂

智能车在转弯过程有最大转角限制，基于粒子群寻优的障碍物自我保护人工势场法寻优角度值应被限制在智能车最大转角范围内，使规划获得的路径符合智能车运动学模型；

适应度函数计算中，b(i)表示下一刻位置(x_n,y_n)与第i个障碍物中心

的欧式距离，d(i)表示下一位置与目标点的距离，数学表达式分别为:

在路径规划过程中下一位置应离障碍物中心远，即b(i)越大越好，且距离目标点尽量近，即d(i)越小越好；将两个距离合并建立适应度函数f(i)，适应度值越大越安全，表达式为:

但坐标系中会出现智能车离障碍物很近，离目标点距离很远，适应度函数表现为b(i)很小，d(i)很大，导致b(i)与c(i)相差几百倍，适应度值计算结果偏向某一数值，每个粒子的适应度值不具代表性，因此将两者进行归一化处理为无量纲的值，把两者的距离都映射到[0,1]之间，映射表达式及适应度函数表达式分别为:

f(i)＝b(i)′+c(i)′；

步骤S6具体为：

给定粒子群算法最大迭代次数，粒子速度范围，位置寻优范围，并给定种群粒子数量(N_l),随机产生N_l个角度值α′_j构成初始种群，j＝1,2,…N_l；根据步骤S5给出的适应度函数计算种群中每个个体对第i个障碍物适应度值f(i)，根据个体极值和全局极值调整速度和位置，使粒子逐渐接近最优值，从而找到最优解；记录各个粒子的个体极值α′_pbestj及全局极值α′_gbestj；

步骤S7具体为：

将全局极值与历史最优值对比，更新粒子的速度与位置，当达到最大迭代次数或全局最优值不变时停止迭代并输出最优的航向角α′；

在步骤S8中，将输出最优的航向角α′和步长带入步骤S3路径公式，并返回步骤S3，判断下一步位置是否进入障碍物自我保护区位置。

8.根据权利要求2所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：通过设置固定的时间间隔内的路径长度L₀与智能车从位置A运动到位置B走过路径长度L进行比较，如果L₀>L，则判断智能车在运动过程中陷入局部最小值。

9.根据权利要求3所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：

引入距离因子后斥力势场函数如下式所示：

其中ρ(X,X_goal)ⁿ为引入的距离因子，ρ(X,X_goal)表示智能车与目标点位置的欧式距离，k_rep为斥力场增益系数，N表示障碍物个数，

为智能车位置与第i个障碍物的距离，ρ_o为障碍物斥力的影响范围，表示只有智能车进入斥力影响范围内才产生斥力势场；对势场求负梯度得到斥力的表达式为：

10.根据权利要求1所述的基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法，其特征在于：采用MATLAB上连线优化后的路径，将无人车运动轨迹可视化。

Images