CN114046791A - 基于分层监测域和自适应人工势场法的车辆路径规划方法 - Google Patents

Abstract

本发明提供了基于分层监测域和自适应人工势场法的车辆路径规划方法，包括以下步骤：利用分层监测域模型获取无人驾驶车辆的速度；计算无人驾驶车辆所受到的合力；计算下一个路径点的位置；然后无人驾驶车辆移动到下一位置；判断无人驾驶车辆是否到达目标点；如果无人驾驶车辆未到达目标点，判断无人驾驶车辆是否陷入局部极小值点；采用切点‑贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域或使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域。本发明利用分层监测域模型控制无人驾驶车辆的速度，利用切点‑贝塞尔曲线模型和虚拟随机引导策略引导无人驾驶车辆摆脱局部极小值点或震荡点区域，适用于复杂的空间环境。

Description

基于分层监测域和自适应人工势场法的车辆路径规划方法

技术领域

本发明属于无人驾驶车辆路径规划领域，特别涉及一种基于分层监测域和自适应人工势场法的车辆路径规划方法。

背景技术

近年来，无人驾驶车辆广泛应用于民用和军事领域，承担了智能运输、紧急救援以及军事行动等重要任务。路径规划是无人驾驶车辆智能化、自主化的核心技术。其主要是在动力学条件和复杂环境的约束下，无人驾驶车辆实时寻找出一条从当前位置到目标位置的无碰撞最优或次优路径。在本质上，路径规划技术是一种优化复杂***的方法。但当前的路径规划技术仍存在自主避障能力不足、复杂环境适应性差等问题。为了使路径规划技术更好的应用于无人驾驶车辆，众多研究者将关注点聚焦在路径规划的相关算法中，并展开深入研究。目前应用于自动驾驶车辆路径规划的主要算法有人工势场法、模糊逻辑算法、粒子群算法和动态窗口法等。此外，自动驾驶车辆的路径规划算法需要同时满足计算速度、环境适应度以及***鲁棒性等要求。

人工势场法作为无人驾驶车辆路径规划的代表性算法，是由Khatib于1985年提出的一种虚拟力法。该算法具有实时性强、运行效率高、规划路径平滑等优势，因而在路径规划领域中得到广泛应用。但是该算法仍存在局部极小值点定义不清晰、易陷入死锁区域以及障碍物附近目标点不可达等问题。因此，国内外学者对这些问题进行了广泛而深入的研究。Song J等设计了一种新的利用时间信息和预测势的路径规划方法。该方法能够解决传统人工势场法存在的问题，同时提高生成路径的平滑度。但是该方法使得规划路径的实时性降低。为了提高动态环境下规划路径的实时性，Wu Q等提出一种基于人工势场算法和甲虫天线搜索算法的混合算法。该混合算法通过建立势场，加快算法的收敛速度，保证动态环境下规划路径的实时性能。然而混合算法高度依赖算法参数的设计。同时该混合算法也存在避障精度较差、算法求解效率较低等问题。Azzabi A等在势场函数中添加旋转力，提出一种新的排斥势函数，使得机器人逃离死锁位置。Sun J等通过动态窗口法，在速度场中加入危险指标，并采用成本函数来评估模拟轨迹，从而解决局部极小值点问题。但是该方法存在一定的局限性，在复杂场景下无法规划出高质量路径，降低了算法的效率。针对无人驾驶车辆路径规划的安全性问题，Wang P等建立避障安全模型并重新设定障碍物斥力场范围。该改进算法可以有效地生成一条高质量安全路径。由于目前人们对于人工势场法的研究忽略了对环境的考虑，导致无人驾驶车辆在动态环境或未知环境下，无法生成可行路径。因此，如何提高无人驾驶车辆行驶的安全性，生成合理的避障路径是无人驾驶车辆路径规划研究的关键。

发明内容

本发明针对现有技术中存在的技术问题，提供一种基于分层监测域和自适应人工势场法的车辆路径规划方法，利用分层监测域模型控制无人驾驶车辆的速度，利用切点-贝塞尔曲线模型和虚拟随机引导策略引导无人驾驶车辆摆脱局部极小值点或震荡点区域。

本发明采用的技术方案是：一种基于分层监测域和自适应人工势场法的车辆路径规划方法，包括以下步骤：

步骤一：利用分层监测域模型获取无人驾驶车辆的速度：设定无人驾驶车辆的动态监测域，根据障碍物在动态监测域中的位置，计算当前无人驾驶车辆的行驶速度；

步骤二：计算无人驾驶车辆所受到的合力；障碍物对无人驾驶车辆产生斥力作用，目标点对无人驾驶车辆产生引力作用，所述合力为斥力和引力的和；

步骤三：根据当前无人驾驶车辆的行驶速度和无人驾驶车辆所受到的合力，计算下一个路径点的位置；然后无人驾驶车辆移动到下一位置；

步骤四：判断无人驾驶车辆是否到达目标点；如果无人驾驶车辆到达目标点，则步骤结束，输出无人驾驶车辆已经到达目标点；如果无人驾驶车辆未到达目标点，则转至步骤五；

步骤五：判断无人驾驶车辆是否陷入局部极小值点；如果无人驾驶车辆未陷入局部极小值点，则转至步骤一；如果无人驾驶车辆陷入了局部极小值点，则转至步骤六；

步骤六：以距离无人驾驶车辆最近的障碍物为中心，无人驾驶车辆与障碍物之间的距离为半径，构造出斥力等势圈，判断目标点是否在斥力等势圈内；若目标点在斥力等势圈范围外或在斥力等势圈上，则采用切点-贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域；若目标点位于斥力等势圈内部，则使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域；

然后转至步骤四。

进一步的，在步骤一中，利用分层监测域模型获取无人驾驶车辆的速度的具体过程为：

将监测域分为level_σ0、level_σ1、level_σ2三个预警级别：

其中，d_min为无人驾驶车辆到各障碍物的欧几里得距离的最小值，r_σ为动态监测域的半径长度，r₁为内圆距离半径，level表示分层监测域的预警级别；

定义count1统计车辆速度随迭代次数增加而递增的次数，定义count2统计车辆速度随迭代次数增加而递减的次数；

利用分层监测域模型获取无人驾驶车辆速度的执行流程如下：

S2.1令count1＝0，count2＝0；转至步骤S2.2；

S2.2无人驾驶车辆监测周围障碍物的位置，利用公式(1)，判断无人驾驶车辆的分层监测域的预警等级，转至步骤S2.3；

S2.3若当前预警等级为level_σ0，则令count1＝count1+1，count2＝0，然后令v_σ0＝count1*v_β，如果v_σ0大于v_α，则令v_σ0＝v_α；若当前预警等级为level_σ1，则令count2＝count2+1，count1＝0，然后令v_σ0＝v_σ0-v_β，如果v_σ0小于v_β，则令v_σ0＝v_β；若当前预警等级为level_σ2，则令v_σ0＝v_β；

其中，v_σ0为当前无人驾驶车辆的行驶速度；无人驾驶车辆行驶速度的上限值v_α＝2/5*r_σ，无人驾驶车辆行驶速度的下限值v_β＝0.2m/s。

进一步的，在步骤二中，无人驾驶车辆所受到的合力的计算公式如下所示：

无人驾驶车辆当前位置的坐标为q＝(x_veh，y_veh)^T，目标点的坐标为q_goal＝(x_goa1，y_goal)^T，障碍物的坐标为q_oi＝((x_oi，y_oi)^T；

无人驾驶车辆所受到的引力F_att(q)如式(2)所示：

F_att(q)＝k_att·ρ(q，q_goal) (2)

其中，k_att为引力增益系数，ρ(q，q_goal)＝||q_goal-q||，即为无人驾驶车辆到目标点的欧几里得距离；

无人驾驶车辆所受到的第i个障碍物的斥力F_rep，i(q)如式(3)所示：

其中，η为斥力增益系数，ρ₀为斥力场半径，ρ_i(q，q_oi)表示无人驾驶车辆到障碍物的欧几里得距离，ρ(q，q_goal)为无人驾驶车辆当前位置到目标点之间的相对距离，m为一个大于0的任意整数，F_rep1，i(q)的方向是由障碍物指向无人驾驶车辆，该力对无人驾驶车辆产生斥力作用，F_rep2，i(q)的方向是由无人驾驶车辆指向目标点，该力引导无人驾驶车辆向目标点移动；

无人驾驶车辆所受到的合力F_sum在x轴、y轴上的合力分量F_sumx、F_sumy的计算过程如下所示：

其中，n为障碍物的数量，i表示第几个障碍物，F_attx(q)、F_atty(q)分别为F_att(q)在x轴、y轴的引力分量；F_rep，ix(q)、F_rep，iy(q)分别为F_rep，i(q)在x轴、y轴的斥力分量。

进一步的，在步骤三中，无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角θ的计算公式如下所示：

则下一个路径点位置的计算公式如式(8)所示：

其中，θ为无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角，(x_veh，y_veh)为无人驾驶车辆当前位置的坐标，(x_veh1，y_veh1)为无人驾驶车辆的下一个路径点的坐标，l为无人驾驶车辆的移动步长。

进一步的，在步骤五中，判断无人驾驶车辆是否陷入局部极小值点的具体过程为：

以无人驾驶车辆当前位置为圆心，R_a为半径做圆，无人驾驶车辆当前位置为路径中的第k个路径点，j＝z表示路径中的第z个路径点，N_count为圆内无人驾驶车辆已经过的路径点数量，则判断无人驾驶车辆是否陷入局部极小值点的执行过程如下所示：

S3.1令N_count＝0，j＝1，转至步骤S3.2；

S3.2若第j个路径点到第k个路径点的距离不大于R_a，则转至步骤S3.3；若第j个路径点到第k个路径点的距离大于R_a，则转至步骤S3.4；

S3.3令N_count＝N_count+1；若N_count的值达到设定的阈值

则流程结束，输出无人驾驶车辆已陷入局部极小值点；若N_count的值没有达到设定的阈值

则转至步骤S3.4；

S3.4令j＝j+1；若j＜＝k，则转至步骤S3.2；若j＞k，则流程结束，输出无人驾驶车辆未陷入局部极小值点。

进一步的，所述步骤六中，采用切点-贝塞尔曲线模型引导车辆摆脱局部极小值点或震荡点区域的具体过程为：

S4.1判断目标点与斥力等势圈的位置关系；若目标点在斥力等势圈范围外，则以目标点为起点向斥力等势圈做两条切线，转至步骤S4.2；若目标点在斥力等势圈上，则目标点位置即为最终切点位置，转至步骤S4.4；

S4.2通过斥力等势圆方程与切线方程计算出两条切线对应的两个切点位置，转至步骤S4.3；

S4.3计算无人驾驶车辆当前位置与两个切点之间的距离，并取最小距离所对应的切点位置为最终切点位置，转至步骤S4.4；

S4.4在无人驾驶车辆当前位置与最终切点位置之间添加一个新的位置点，新的位置点的横坐标为最终切点位置的横坐标，新的位置点的纵坐标为无人驾驶车辆当前位置的纵坐标；然后结合这三个定点，利用二次贝塞尔函数生成一条二次贝塞尔曲线，无人驾驶车辆沿着三个定点所做出的贝塞尔曲线到达最终切点位置。

进一步的，所述步骤六中，使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域的具体过程为：

S5.1以局部极小值点为中心，构造一个环形区域，并在内圆半径和外圆半径范围内，随机选取一个数值作为新的半径长度，构造一个虚拟圆，在虚拟圆上随机选取一点作为待选随机引导点，转至步骤S5.2；

S5.2采用步骤S5.1生成δ个待选随机引导点，并转至步骤S5.3；

S5.3求取δ个待选随机引导点与目标点和无人驾驶车辆当前位置之间的距离，然后计算出每一个待选随机引导点的引导常量，基于δ个待选随机引导点的引导常量，得到最小的引导常量，转至步骤S5.4；

S5.4将最小的引导常量对应的待选随机引导点作为虚拟目标点，同时，暂时消除最终目标点对无人驾驶车辆的引力作用，只保留虚拟目标点对无人驾驶车辆的引力作用，转至步骤S5.5；

S5.5通过虚拟目标点的引力作用及障碍物的斥力作用，无人驾驶车辆在合力的作用下移动到虚拟目标点位置。

本发明利用分层监测域模型实现无人驾驶车辆速度的自适应变化，以提高无人驾驶车辆行驶时的安全性，优化算法的计算效率。当距离无人驾驶车辆最近的障碍物位于监测域外时，其无人驾驶车辆速度呈现一个自适应上升的趋势。当障碍物位于监测域内的外侧时，无人驾驶车辆的速度逐级递减。当障碍物位于监测域内的内侧时，无人驾驶车辆将保持最低行驶速度。本发明提出一种新的局部极小值点判定方法。通过路径点的聚集情况，判断局部极小值点位置。针对传统人工势场法中缺少相应的无人驾驶车辆逃离局部极小值点的逃逸机制，本发明设计了切点-贝塞尔曲线模型和虚拟随机引导策略。当目标点位于斥力等势圈的不同位置时，无人驾驶车辆采用不同的模型和策略以适应不同的地图环境。本发明提出的基于分层监测域的自适应人工势场法不但适用于复杂的空间环境，而且还能大幅度提高算法的各项性能指标并解决传统人工势场法所存在的问题。

与现有技术相比，本发明所具有的有益效果是：第一，在静态环境和动态环境下，无人驾驶车辆能有效的避开静态和动态障碍物，并成功到达目标点。第二，相较于传统人工势场法，基于分层监测域的自适应人工势场法能够发现更多潜在的局部极小值点，从而规划出可行有效的高质量路径。第三，基于分层监测域的自适应人工势场法在程序运行时间、迭代次数以及局部极小值点判定方法方面均优于传统人工势场法。第四，在路径安全性方面，基于分层监测域的自适应人工势场法的安全敏感指数更高，能更好的避免无人驾驶车辆与障碍物发生碰撞。第五，基于分层监测域的自适应人工势场法能够解决传统人工势场法中存在的局部极小值问题和障碍物附近目标点不可达问题。

附图说明

图1为本发明实施例的工作原理示意图。

具体实施方式

为使本领域技术人员更好的理解本发明的技术方案，下面结合附图和具体实施例对本发明作详细说明。

本发明的实施例提供了一种基于分层监测域和自适应人工势场法的车辆路径规划方法，如图1所示，包括以下步骤：

步骤一：利用分层监测域模型获取无人驾驶车辆的速度：设定无人驾驶车辆的动态监测域，根据障碍物在动态监测域中的位置，计算当前无人驾驶车辆的行驶速度；分层监测域模型针对环境变化，设定不同的预警等级，实现无人驾驶车辆速度自适应变化。

利用分层监测域模型获取无人驾驶车辆的速度的具体过程为：

将监测域分为level_σ0、level_σ1、level_σ2三个预警级别：

其中，d_min为无人驾驶车辆到各障碍物的欧几里得距离的最小值，r_σ为动态监测域的半径长度，r₁为内圆距离半径，level表示分层监测域的预警级别。level_σ0表示无人驾驶车辆处于绝对安全的状态。level_σ1表示障碍物出现在动态监测域内的外侧。level_σ2表示障碍物进入警戒区(动态监测域内的内侧)。

定义count1统计车辆速度随迭代次数增加而递增的次数，定义count2统计车辆速度随迭代次数增加而递减的次数。则利用分层监测域模型获取无人驾驶车辆速度的执行流程如下：

S2.1令count1＝0，count2＝0；转至步骤S2.2；

S2.2无人驾驶车辆监测周围障碍物的位置，利用公式(1)，判断无人驾驶车辆的分层监测域的预警等级，转至步骤S2.3；

S2.3若当前预警等级为level_σ0，则令count1＝count1+1，count2＝0，然后令v_σ0＝count1*v_β，如果v_σ0大于v_α，则令v_σ0＝v_α；若当前预警等级为level_σ1，则令count2＝count2+1，count1＝0，然后令v_σo＝v_σ0-v_β，如果v_σ0小于v_β，则令v_σo＝v_β；若当前预警等级为level_σ2，则令v_σ0＝v_β；

其中，v_σ0为当前无人驾驶车辆的行驶速度；无人驾驶车辆行驶速度的上限值v_α＝2/5*r_σ，无人驾驶车辆行驶速度的下限值v_β＝0.2m/s。

步骤二：计算无人驾驶车辆所受到的合力；障碍物对无人驾驶车辆产生斥力作用，目标点对无人驾驶车辆产生引力作用，所述合力为斥力和引力的和；本实施例在斥力函数中引入无人驾驶车辆到目标点的相对距离。使得当目标点在障碍物附近时，无人驾驶车辆通过减小相对距离来削弱所受到的斥力。无人驾驶车辆所受到的合力的计算公式如下所示：

无人驾驶车辆当前位置的坐标为q＝(x_veh，y_veh)^T，目标点的坐标为q_goal＝(x_goal，y_goal)^T，障碍物的坐标为q_oi＝((x_oi，y_oi)^T。则无人驾驶车辆所受到的引力F_att(q)如式(2)所示：

F_att(q)＝k_att·ρ(q，q_goal) (2)

其中，k_att为引力增益系数，ρ(q，q_goal)＝||q_goal-q||，即为无人驾驶车辆到目标点的欧几里得距离。

则无人驾驶车辆所受到的第i个障碍物的斥力F_rep，i(q)如式(3)所示：

其中，η为斥力增益系数，ρ₀为斥力场半径，ρ_i(q，q_oi)表示无人驾驶车辆到障碍物的欧几里得距离，ρ(q，q_goa1)为无人驾驶车辆当前位置到目标点之间的相对距离，m为一个大于0的任意整数(m通常取2)，F_rep1，i(q)的方向是由障碍物指向无人驾驶车辆，该力对无人驾驶车辆产生斥力作用，F_rep2，i(q)的方向是由无人驾驶车辆指向目标点，该力引导无人驾驶车辆向目标点移动。

因此，无人驾驶车辆所受到的合力F_sum在x轴、y轴上的合力分量F_sumx、F_sumy的计算过程如下所示，

其中，n为障碍物的数量，i表示第几个障碍物，F_attx(q)、F_atty(q)分别为F_att(q)在x轴、y轴的引力分量；F_rep，ix(q)、F_rep，iy(q)分别为F_rep，i(q)在x轴、y轴的斥力分量。所以，无人驾驶车辆所受到的合力F_sum为F_sumx与F_sumy的矢量和。

步骤三：根据当前无人驾驶车辆的行驶速度和无人驾驶车辆所受到的合力，计算下一个路径点的位置；然后无人驾驶车辆移动到下一位置。

无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角θ的计算公式如下所示：

则下一个路径点位置的计算公式如式(8)所示：

其中，θ为无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角，(x_veh，y_veh)为无人驾驶车辆当前位置的坐标，(x_veh1，y_veh1)为无人驾驶车辆的下一个路径点的坐标，l为无人驾驶车辆的移动步长，与当前无人驾驶车辆的行驶速度相关。

步骤四：判断无人驾驶车辆是否到达目标点；如果无人驾驶车辆到达目标点，则步骤结束，输出无人驾驶车辆已经到达目标点；如果无人驾驶车辆未到达目标点，则转至步骤五。

步骤五：判断无人驾驶车辆是否陷入局部极小值点；如果无人驾驶车辆未陷入局部极小值点，则转至步骤一；如果无人驾驶车辆陷入了局部极小值点，则转至步骤六。

判断无人驾驶车辆是否陷入局部极小值点的具体过程为：

以无人驾驶车辆当前位置为圆心，R_a为半径做圆，无人驾驶车辆当前位置为路径中的第k个路径点，j＝z表示路径中的第z个路径点，N_count为圆内无人驾驶车辆已经过的路径点数量，则判断无人驾驶车辆是否陷入局部极小值点的执行过程如下所示：

S3.1令N_count＝0，j＝1，转至步骤S3.2；

S3.2若第j个路径点到第k个路径点的距离不大于R_a，则转至步骤S3.3；若第j个路径点到第k个路径点的距离大于R_a，则转至步骤S3.4；

S3.3令N_count＝N_count+1；若N_count的值达到设定的阈值

则流程结束，输出无人驾驶车辆已陷入局部极小值点；若N_count的值没有达到设定的阈值

则转至步骤S3.4。

S3.4令j＝j+1；若j＜＝k，则转至步骤S3.2；若j＞k，则流程结束，输出无人驾驶车辆未陷入局部极小值点。

步骤六：判断目标点是否在斥力等势圈内；若目标点在斥力等势圈范围外或在斥力等势圈上，则采用切点-贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域；若目标点位于斥力等势圈内部，则使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域；

然后转至步骤四。

采用切点-贝塞尔曲线模型引导车辆摆脱局部极小值点或震荡点区域的具体过程为：

当无人驾驶车辆陷入局部极小值点时，以距离无人驾驶车辆最近的障碍物为中心，两者之间的距离为半径R_dist，构造出斥力等势圈，其距离半径公式为：

S4.1判断目标点与斥力等势圈的位置关系；若目标点在斥力等势圈范围外，则以目标点为起点向斥力等势圈做两条切线，两条切线与斥力等势圈的切点分别为A_tang、B_tang，转至步骤S4.2；若目标点在斥力等势圈上，则目标点位置即为最终切点位置，转至步骤S4.4；

S4.2通过斥力等势圆方程与切线方程计算出两个切点位置，转至步骤S4.3；计算过程如下所示：

Y_tang＝s_br-f_ar*X_tang (15)

其中，(x_oi，Y_oi)为障碍物位置坐标，(x_veh，y_veh)为无人驾驶车辆当前位置的坐标，(x_goal，y_goal)为目标点位置坐标，R_dist为斥力等势圈的半径长度，(X_tang，Y_tang)为切点位置坐标；

S4.3在得到两个切点位置的基础上，计算无人驾驶车辆当前位置与两个切点之间的距离S₁、S₂，并取最小距离S_tang所对应的切点位置为最终切点位置，转至步骤S4.4，S_tang的计算公式如式(16)所示，

S_tang＝min{S₁，S₂} (16)

S4.4在无人驾驶车辆当前位置与最终切点位置之间添加一个新的位置点(X_tang，y_veh)，新的位置点的横坐标为最终切点位置的横坐标，新的位置点的纵坐标为无人驾驶车辆当前位置的纵坐标；然后结合这三个定点，利用二次贝塞尔函数生成一条二次贝塞尔曲线，无人驾驶车辆将沿着三个定点所做出的贝塞尔曲线到达最终切点位置。从而使得无人驾驶车辆逃离局部极小值点或震荡点区域。

无人驾驶车辆所走过的二次贝塞尔曲线由式：

I_b(τ)＝(1-τ)²*M_veh+2*τ*(1-τ)*M_rand+τ²*M_tang，τ∈[0，1] (17)

其中，M_veh为无人驾驶车辆当前位置的坐标，M_rand为新位置点的坐标，M_tang表示最终切点位置的坐标，τ表示动态系数。

使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域的具体过程为：

S5.1以局部极小值点为中心，构造一个环形区域，并在内圆半径和外圆半径范围内，随机选取一个数值作为新的半径长度，构造一个虚拟圆，在虚拟圆上随机选取一点作为待选随机引导点，转至步骤S5.2。其中，R_inner为内圆半径，R_outer为外圆半径，

为第i个待选随机引导点所在虚拟圆的半径。(X_min，Y_min)为待选随机引导点的位置坐标。(X，Y)为局部极小值点的位置坐标。则待选随机引导点的生成过程如下所示：

S5.2采用步骤S5.1生成δ个待选随机引导点。并转至步骤S5.3。

S5.3求取δ个待选随机引导点与目标点和无人驾驶车辆当前位置之间的距离，然后利用公式(20-22)计算出每一个待选随机引导点的引导常量T_{cr_min}。

其中，W_r-v和W_r-g为权重因子且为固定常数。L_k为待选随机引导点到目标点之间的距离，L_q为当前无人驾驶车辆到待选随机引导点之间的距离。(X_min，Y_min)为无人驾驶车辆的位置坐标，(X_goal，Y_goal)为目标点的位置坐标，(X_randomi，Y_randomi)为待选随机引导点的位置坐标。

基于δ个待选随机引导点的引导常量，可以得到最小的引导常量，转至步骤S5.4。

S5.4得到最小的引导常量后，将最小的引导常量对应的待选随机引导点作为虚拟目标点，同时，暂时消除最终目标点对无人驾驶车辆的引力作用，只保留虚拟目标点对无人驾驶车辆的引力作用，转至步骤S5.5。

S5.5通过虚拟目标点的引力作用及障碍物的斥力作用，无人驾驶车辆将在合力的作用下移动到虚拟目标点位置，从而逃离局部极小值点。

以上通过实施例对本发明进行了详细说明，但所述内容仅为本发明的示例性实施例，不能被认为用于限定本发明的实施范围。本发明的保护范围由权利要求书限定。凡利用本发明所述的技术方案，或本领域的技术人员在本发明技术方案的启发下，在本发明的实质和保护范围内，设计出类似的技术方案而达到上述技术效果的，或者对申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖保护范围之内。

Claims (7)

1.基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：包括以下步骤：

步骤一：利用分层监测域模型获取无人驾驶车辆的速度：设定无人驾驶车辆的动态监测域，根据障碍物在动态监测域中的位置，计算当前无人驾驶车辆的行驶速度；

步骤二：计算无人驾驶车辆所受到的合力；障碍物对无人驾驶车辆产生斥力作用，目标点对无人驾驶车辆产生引力作用，所述合力为斥力和引力的和；

步骤三：根据当前无人驾驶车辆的行驶速度和无人驾驶车辆所受到的合力，计算下一个路径点的位置；然后无人驾驶车辆移动到下一位置；

步骤四：判断无人驾驶车辆是否到达目标点；如果无人驾驶车辆到达目标点，则步骤结束，输出无人驾驶车辆已经到达目标点；如果无人驾驶车辆未到达目标点，则转至步骤五；

步骤五：判断无人驾驶车辆是否陷入局部极小值点；如果无人驾驶车辆未陷入局部极小值点，则转至步骤一；如果无人驾驶车辆陷入了局部极小值点，则转至步骤六；

步骤六：以距离无人驾驶车辆最近的障碍物为中心，无人驾驶车辆与障碍物之间的距离为半径，构造出斥力等势圈，判断目标点是否在斥力等势圈内；若目标点在斥力等势圈范围外或在斥力等势圈上，则采用切点-贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域；若目标点位于斥力等势圈内部，则使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域；

然后转至步骤四。

2.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：在步骤一中，利用分层监测域模型获取无人驾驶车辆的速度的具体过程为：

将监测域分为level_σ0、level_σ1、level_σ2三个预警级别：

其中，d_min为无人驾驶车辆到各障碍物的欧几里得距离的最小值，r_σ为动态监测域的半径长度，r₁为内圆距离半径，level表示分层监测域的预警级别；

定义count1统计车辆速度随迭代次数增加而递增的次数，定义count2统计车辆速度随迭代次数增加而递减的次数；

利用分层监测域模型获取无人驾驶车辆速度的执行流程如下：

S2.1令count1＝0，count2＝0；转至步骤S2.2；

S2.2无人驾驶车辆监测周围障碍物的位置，利用公式(1)，判断无人驾驶车辆的分层监测域的预警等级，转至步骤S2.3；

S2.3若当前预警等级为level_σ0，则令count1＝count1+1，count2＝0，然后令v_σ0＝countl*v_β，如果v_σ0大于v_α，则令v_σ0＝v_α；若当前预警等级为level_σ1，则令count2＝count2+1，count1＝0，然后令v_σ0＝v_σ0-v_β，如果v_σ0小于v_β，则令v_σ0＝v_β；若当前预警等级为level_σ2，则令v_σ0＝v_β；

其中，v_σ0为当前无人驾驶车辆的行驶速度；无人驾驶车辆行驶速度的上限值v_α＝2/5*r_σ，下限值v_β＝0.2m/s。

3.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：在步骤二中，无人驾驶车辆所受到的合力的计算公式如下所示：

无人驾驶车辆当前位置的坐标为q＝(x_veh，y_veh)^T，目标点的坐标为q_goal＝(x_goal，y_goal)^T，障碍物的坐标为q_oi＝((x_oi，y_oi)^T；

无人驾驶车辆所受到的引力F_att(q)如式(2)所示：

F_att(q)＝k_att·ρ(q，q_goal) (2)

其中，k_att为引力增益系数，ρ(q，q_goal)＝||q_goal-q||，即为无人驾驶车辆到目标点的欧几里得距离；

无人驾驶车辆所受到的第i个障碍物的斥力F_rep，i(q)如式(3)所示：

其中，η为斥力增益系数，ρ₀为斥力场半径，ρ_i(q，q_oi)表示无人驾驶车辆到障碍物的欧几里得距离，ρ(q，q_goal)为无人驾驶车辆当前位置到目标点之间的相对距离，m为一个大于0的任意整数，F_rep1，i(q)的方向是由障碍物指向无人驾驶车辆，该力对无人驾驶车辆产生斥力作用，F_rep2，i(q)的方向是由无人驾驶车辆指向目标点，该力引导无人驾驶车辆向目标点移动；

无人驾驶车辆所受到的合力F_sum在x轴、y轴上的合力分量F_sumx、F_sumy的计算过程如下所示：

其中，n为障碍物的数量，i表示第几个障碍物，F_attx(q)、F_atty(q)分别为F_att(q)在x轴、y轴的引力分量；F_rep，ix(q)、F_rep，iy(q)分别为F_rep，i(q)在x轴、y轴的斥力分量。

4.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：在步骤三中，无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角θ的计算公式如下所示：

则下一个路径点位置的计算公式如式(8)所示：

其中，θ为无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角，(x_veh，y_veh)为无人驾驶车辆当前位置的坐标，(x_veh1，y_veh1)为无人驾驶车辆的下一个路径点的坐标，l为无人驾驶车辆的移动步长。

5.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：在步骤五中，判断无人驾驶车辆是否陷入局部极小值点的具体过程为：

以无人驾驶车辆当前位置为圆心，R_a为半径做圆，无人驾驶车辆当前位置为路径中的第k个路径点，j＝z表示路径中的第z个路径点，N_count为圆内无人驾驶车辆已经过的路径点数量，则判断无人驾驶车辆是否陷入局部极小值点的执行过程如下所示：

S3.1令N_count＝0，j＝1，转至步骤S3.2；

S3.2若第j个路径点到第k个路径点的距离不大于R_a，则转至步骤S3.3；若第j个路径点到第k个路径点的距离大于R_a，则转至步骤S3.4；

S3.3令N_count＝N_count+1；若N_count的值达到设定的阈值

则流程结束，输出无人驾驶车辆已陷入局部极小值点；若N_count的值没有达到设定的阈值

则转至步骤S3.4；

S3.4令j＝j+1；若j＜＝k，则转至步骤S3.2；若j＞k，则流程结束，输出无人驾驶车辆未陷入局部极小值点。

6.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：所述步骤六中，采用切点-贝塞尔曲线模型引导车辆摆脱局部极小值点或震荡点区域的具体过程为：

S4.1判断目标点与斥力等势圈的位置关系；若目标点在斥力等势圈范围外，则以目标点为起点向斥力等势圈做两条切线，转至步骤S4.2；若目标点在斥力等势圈上，则目标点位置即为最终切点位置，转至步骤S4.4；

S4.2通过斥力等势圆方程与切线方程计算出两条切线对应的两个切点位置，转至步骤S4.3；

S4.3计算无人驾驶车辆当前位置与两个切点之间的距离，并取最小距离所对应的切点位置为最终切点位置，转至步骤S4.4；

S4.4在无人驾驶车辆当前位置与最终切点位置之间添加一个新的位置点，新的位置点的横坐标为最终切点位置的横坐标，新的位置点的纵坐标为无人驾驶车辆当前位置的纵坐标；然后结合这三个定点，利用二次贝塞尔函数生成一条二次贝塞尔曲线，无人驾驶车辆沿着三个定点所做出的贝塞尔曲线到达最终切点位置。

7.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法，其特征在于：所述步骤六中，使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域的具体过程为：

S5.1以局部极小值点为中心，构造一个环形区域，并在内圆半径和外圆半径范围内，随机选取一个数值作为新的半径长度，构造一个虚拟圆，在虚拟圆上随机选取一点作为待选随机引导点，转至步骤S5.2；

S5.2采用步骤S5.1生成δ个待选随机引导点，并转至步骤S5.3；

S5.3求取δ个待选随机引导点与目标点和无人驾驶车辆当前位置之间的距离，然后计算出每一个待选随机引导点的引导常量，基于δ个待选随机引导点的引导常量，得到最小的引导常量，转至步骤S5.4；

S5.4将最小的引导常量对应的待选随机引导点作为虚拟目标点，同时，暂时消除最终目标点对无人驾驶车辆的引力作用，只保留虚拟目标点对无人驾驶车辆的引力作用，转至步骤S5.5；

S5.5通过虚拟目标点的引力作用及障碍物的斥力作用，无人驾驶车辆在合力的作用下移动到虚拟目标点位置。

Images