CN114046791B - 基于分层监测域和自适应人工势场法的车辆路径规划方法 - Google Patents
基于分层监测域和自适应人工势场法的车辆路径规划方法 Download PDFInfo
- Publication number
- CN114046791B CN114046791B CN202111291864.XA CN202111291864A CN114046791B CN 114046791 B CN114046791 B CN 114046791B CN 202111291864 A CN202111291864 A CN 202111291864A CN 114046791 B CN114046791 B CN 114046791B
- Authority
- CN
- China
- Prior art keywords
- unmanned vehicle
- point
- target point
- vehicle
- repulsive force
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 81
- 238000012544 monitoring process Methods 0.000 title claims abstract description 48
- 230000010355 oscillation Effects 0.000 claims abstract description 16
- 230000008569 process Effects 0.000 claims description 25
- 238000004364 calculation method Methods 0.000 claims description 16
- 230000009471 action Effects 0.000 claims description 15
- 230000003044 adaptive effect Effects 0.000 claims description 8
- 230000000694 effects Effects 0.000 claims description 7
- 101100412093 Schizosaccharomyces pombe (strain 972 / ATCC 24843) rec16 gene Proteins 0.000 claims description 3
- 230000007423 decrease Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 20
- 101100069049 Caenorhabditis elegans goa-1 gene Proteins 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 241000254173 Coleoptera Species 0.000 description 1
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了基于分层监测域和自适应人工势场法的车辆路径规划方法,包括以下步骤:利用分层监测域模型获取无人驾驶车辆的速度;计算无人驾驶车辆所受到的合力;计算下一个路径点的位置;然后无人驾驶车辆移动到下一位置;判断无人驾驶车辆是否到达目标点;如果无人驾驶车辆未到达目标点,判断无人驾驶车辆是否陷入局部极小值点;采用切点‑贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域或使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域。本发明利用分层监测域模型控制无人驾驶车辆的速度,利用切点‑贝塞尔曲线模型和虚拟随机引导策略引导无人驾驶车辆摆脱局部极小值点或震荡点区域,适用于复杂的空间环境。
Description
技术领域
本发明属于无人驾驶车辆路径规划领域,特别涉及一种基于分层监测域和自适应人工势场法的车辆路径规划方法。
背景技术
近年来,无人驾驶车辆广泛应用于民用和军事领域,承担了智能运输、紧急救援以及军事行动等重要任务。路径规划是无人驾驶车辆智能化、自主化的核心技术。其主要是在动力学条件和复杂环境的约束下,无人驾驶车辆实时寻找出一条从当前位置到目标位置的无碰撞最优或次优路径。在本质上,路径规划技术是一种优化复杂系统的方法。但当前的路径规划技术仍存在自主避障能力不足、复杂环境适应性差等问题。为了使路径规划技术更好的应用于无人驾驶车辆,众多研究者将关注点聚焦在路径规划的相关算法中,并展开深入研究。目前应用于自动驾驶车辆路径规划的主要算法有人工势场法、模糊逻辑算法、粒子群算法和动态窗口法等。此外,自动驾驶车辆的路径规划算法需要同时满足计算速度、环境适应度以及系统鲁棒性等要求。
人工势场法作为无人驾驶车辆路径规划的代表性算法,是由Khatib于1985年提出的一种虚拟力法。该算法具有实时性强、运行效率高、规划路径平滑等优势,因而在路径规划领域中得到广泛应用。但是该算法仍存在局部极小值点定义不清晰、易陷入死锁区域以及障碍物附近目标点不可达等问题。因此,国内外学者对这些问题进行了广泛而深入的研究。Song J等设计了一种新的利用时间信息和预测势的路径规划方法。该方法能够解决传统人工势场法存在的问题,同时提高生成路径的平滑度。但是该方法使得规划路径的实时性降低。为了提高动态环境下规划路径的实时性,Wu Q等提出一种基于人工势场算法和甲虫天线搜索算法的混合算法。该混合算法通过建立势场,加快算法的收敛速度,保证动态环境下规划路径的实时性能。然而混合算法高度依赖算法参数的设计。同时该混合算法也存在避障精度较差、算法求解效率较低等问题。Azzabi A等在势场函数中添加旋转力,提出一种新的排斥势函数,使得机器人逃离死锁位置。Sun J等通过动态窗口法,在速度场中加入危险指标,并采用成本函数来评估模拟轨迹,从而解决局部极小值点问题。但是该方法存在一定的局限性,在复杂场景下无法规划出高质量路径,降低了算法的效率。针对无人驾驶车辆路径规划的安全性问题,Wang P等建立避障安全模型并重新设定障碍物斥力场范围。该改进算法可以有效地生成一条高质量安全路径。由于目前人们对于人工势场法的研究忽略了对环境的考虑,导致无人驾驶车辆在动态环境或未知环境下,无法生成可行路径。因此,如何提高无人驾驶车辆行驶的安全性,生成合理的避障路径是无人驾驶车辆路径规划研究的关键。
发明内容
本发明针对现有技术中存在的技术问题,提供一种基于分层监测域和自适应人工势场法的车辆路径规划方法,利用分层监测域模型控制无人驾驶车辆的速度,利用切点-贝塞尔曲线模型和虚拟随机引导策略引导无人驾驶车辆摆脱局部极小值点或震荡点区域。
本发明采用的技术方案是:一种基于分层监测域和自适应人工势场法的车辆路径规划方法,包括以下步骤:
步骤一:利用分层监测域模型获取无人驾驶车辆的速度:设定无人驾驶车辆的动态监测域,根据障碍物在动态监测域中的位置,计算当前无人驾驶车辆的行驶速度;
步骤二:计算无人驾驶车辆所受到的合力;障碍物对无人驾驶车辆产生斥力作用,目标点对无人驾驶车辆产生引力作用,所述合力为斥力和引力的和;
步骤三:根据当前无人驾驶车辆的行驶速度和无人驾驶车辆所受到的合力,计算下一个路径点的位置;然后无人驾驶车辆移动到下一位置;
步骤四:判断无人驾驶车辆是否到达目标点;如果无人驾驶车辆到达目标点,则步骤结束,输出无人驾驶车辆已经到达目标点;如果无人驾驶车辆未到达目标点,则转至步骤五;
步骤五:判断无人驾驶车辆是否陷入局部极小值点;如果无人驾驶车辆未陷入局部极小值点,则转至步骤一;如果无人驾驶车辆陷入了局部极小值点,则转至步骤六;
步骤六:以距离无人驾驶车辆最近的障碍物为中心,无人驾驶车辆与障碍物之间的距离为半径,构造出斥力等势圈,判断目标点是否在斥力等势圈内;若目标点在斥力等势圈范围外或在斥力等势圈上,则采用切点-贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域;若目标点位于斥力等势圈内部,则使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域;
然后转至步骤四。
进一步的,在步骤一中,利用分层监测域模型获取无人驾驶车辆的速度的具体过程为:
将监测域分为levelσ0、levelσ1、levelσ2三个预警级别:
其中,dmin为无人驾驶车辆到各障碍物的欧几里得距离的最小值,rσ为动态监测域的半径长度,r1为内圆距离半径,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=(xveh,yveh)T,目标点的坐标为qgoal=(xgoa1,ygoal)T,障碍物的坐标为qoi=((xoi,yoi)T;
无人驾驶车辆所受到的引力Fatt(q)如式(2)所示:
Fatt(q)=katt·ρ(q,qgoal) (2)
其中,katt为引力增益系数,ρ(q,qgoal)=||qgoal-q||,即为无人驾驶车辆到目标点的欧几里得距离;
无人驾驶车辆所受到的第i个障碍物的斥力Frep,i(q)如式(3)所示:
其中,η为斥力增益系数,ρ0为斥力场半径,ρi(q,qoi)表示无人驾驶车辆到障碍物的欧几里得距离,ρ(q,qgoal)为无人驾驶车辆当前位置到目标点之间的相对距离,m为一个大于0的任意整数,Frep1,i(q)的方向是由障碍物指向无人驾驶车辆,该力对无人驾驶车辆产生斥力作用,Frep2,i(q)的方向是由无人驾驶车辆指向目标点,该力引导无人驾驶车辆向目标点移动;
无人驾驶车辆所受到的合力Fsum在x轴、y轴上的合力分量Fsumx、Fsumy的计算过程如下所示:
其中,n为障碍物的数量,i表示第几个障碍物,Fattx(q)、Fatty(q)分别为Fatt(q)在x轴、y轴的引力分量;Frep,ix(q)、Frep,iy(q)分别为Frep,i(q)在x轴、y轴的斥力分量。
进一步的,在步骤三中,无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角θ的计算公式如下所示:
则下一个路径点位置的计算公式如式(8)所示:
其中,θ为无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角,(xveh,yveh)为无人驾驶车辆当前位置的坐标,(xveh1,yveh1)为无人驾驶车辆的下一个路径点的坐标,l为无人驾驶车辆的移动步长。
进一步的,在步骤五中,判断无人驾驶车辆是否陷入局部极小值点的具体过程为:
以无人驾驶车辆当前位置为圆心,Ra为半径做圆,无人驾驶车辆当前位置为路径中的第k个路径点,j=z表示路径中的第z个路径点,Ncount为圆内无人驾驶车辆已经过的路径点数量,则判断无人驾驶车辆是否陷入局部极小值点的执行过程如下所示:
S3.1令Ncount=0,j=1,转至步骤S3.2;
S3.2若第j个路径点到第k个路径点的距离不大于Ra,则转至步骤S3.3;若第j个路径点到第k个路径点的距离大于Ra,则转至步骤S3.4;
S3.3令Ncount=Ncount+1;若Ncount的值达到设定的阈值则流程结束,输出无人驾驶车辆已陷入局部极小值点;若Ncount的值没有达到设定的阈值/>则转至步骤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三个预警级别:
其中,dmin为无人驾驶车辆到各障碍物的欧几里得距离的最小值,rσ为动态监测域的半径长度,r1为内圆距离半径,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=(xveh,yveh)T,目标点的坐标为qgoal=(xgoal,ygoal)T,障碍物的坐标为qoi=((xoi,yoi)T。则无人驾驶车辆所受到的引力Fatt(q)如式(2)所示:
Fatt(q)=katt·ρ(q,qgoal) (2)
其中,katt为引力增益系数,ρ(q,qgoal)=||qgoal-q||,即为无人驾驶车辆到目标点的欧几里得距离。
则无人驾驶车辆所受到的第i个障碍物的斥力Frep,i(q)如式(3)所示:
其中,η为斥力增益系数,ρ0为斥力场半径,ρi(q,qoi)表示无人驾驶车辆到障碍物的欧几里得距离,ρ(q,qgoa1)为无人驾驶车辆当前位置到目标点之间的相对距离,m为一个大于0的任意整数(m通常取2),Frep1,i(q)的方向是由障碍物指向无人驾驶车辆,该力对无人驾驶车辆产生斥力作用,Frep2,i(q)的方向是由无人驾驶车辆指向目标点,该力引导无人驾驶车辆向目标点移动。
因此,无人驾驶车辆所受到的合力Fsum在x轴、y轴上的合力分量Fsumx、Fsumy的计算过程如下所示,
其中,n为障碍物的数量,i表示第几个障碍物,Fattx(q)、Fatty(q)分别为Fatt(q)在x轴、y轴的引力分量;Frep,ix(q)、Frep,iy(q)分别为Frep,i(q)在x轴、y轴的斥力分量。所以,无人驾驶车辆所受到的合力Fsum为Fsumx与Fsumy的矢量和。
步骤三:根据当前无人驾驶车辆的行驶速度和无人驾驶车辆所受到的合力,计算下一个路径点的位置;然后无人驾驶车辆移动到下一位置。
无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角θ的计算公式如下所示:
则下一个路径点位置的计算公式如式(8)所示:
其中,θ为无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角,(xveh,yveh)为无人驾驶车辆当前位置的坐标,(xveh1,yveh1)为无人驾驶车辆的下一个路径点的坐标,l为无人驾驶车辆的移动步长,与当前无人驾驶车辆的行驶速度相关。
步骤四:判断无人驾驶车辆是否到达目标点;如果无人驾驶车辆到达目标点,则步骤结束,输出无人驾驶车辆已经到达目标点;如果无人驾驶车辆未到达目标点,则转至步骤五。
步骤五:判断无人驾驶车辆是否陷入局部极小值点;如果无人驾驶车辆未陷入局部极小值点,则转至步骤一;如果无人驾驶车辆陷入了局部极小值点,则转至步骤六。
判断无人驾驶车辆是否陷入局部极小值点的具体过程为:
以无人驾驶车辆当前位置为圆心,Ra为半径做圆,无人驾驶车辆当前位置为路径中的第k个路径点,j=z表示路径中的第z个路径点,Ncount为圆内无人驾驶车辆已经过的路径点数量,则判断无人驾驶车辆是否陷入局部极小值点的执行过程如下所示:
S3.1令Ncount=0,j=1,转至步骤S3.2;
S3.2若第j个路径点到第k个路径点的距离不大于Ra,则转至步骤S3.3;若第j个路径点到第k个路径点的距离大于Ra,则转至步骤S3.4;
S3.3令Ncount=Ncount+1;若Ncount的值达到设定的阈值则流程结束,输出无人驾驶车辆已陷入局部极小值点;若Ncount的值没有达到设定的阈值/>则转至步骤S3.4。
S3.4令j=j+1;若j<=k,则转至步骤S3.2;若j>k,则流程结束,输出无人驾驶车辆未陷入局部极小值点。
步骤六:判断目标点是否在斥力等势圈内;若目标点在斥力等势圈范围外或在斥力等势圈上,则采用切点-贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域;若目标点位于斥力等势圈内部,则使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域;
然后转至步骤四。
采用切点-贝塞尔曲线模型引导车辆摆脱局部极小值点或震荡点区域的具体过程为:
当无人驾驶车辆陷入局部极小值点时,以距离无人驾驶车辆最近的障碍物为中心,两者之间的距离为半径Rdist,构造出斥力等势圈,其距离半径公式为:
S4.1判断目标点与斥力等势圈的位置关系;若目标点在斥力等势圈范围外,则以目标点为起点向斥力等势圈做两条切线,两条切线与斥力等势圈的切点分别为Atang、Btang,转至步骤S4.2;若目标点在斥力等势圈上,则目标点位置即为最终切点位置,转至步骤S4.4;
S4.2通过斥力等势圆方程与切线方程计算出两个切点位置,转至步骤S4.3;计算过程如下所示:
Ytang=sbr-far*Xtang (15)
其中,(xoi,Yoi)为障碍物位置坐标,(xveh,yveh)为无人驾驶车辆当前位置的坐标,(xgoal,ygoal)为目标点位置坐标,Rdist为斥力等势圈的半径长度,(Xtang,Ytang)为切点位置坐标;
S4.3在得到两个切点位置的基础上,计算无人驾驶车辆当前位置与两个切点之间的距离S1、S2,并取最小距离Stang所对应的切点位置为最终切点位置,转至步骤S4.4,Stang的计算公式如式(16)所示,
Stang=min{S1,S2} (16)
S4.4在无人驾驶车辆当前位置与最终切点位置之间添加一个新的位置点(Xtang,yveh),新的位置点的横坐标为最终切点位置的横坐标,新的位置点的纵坐标为无人驾驶车辆当前位置的纵坐标;然后结合这三个定点,利用二次贝塞尔函数生成一条二次贝塞尔曲线,无人驾驶车辆将沿着三个定点所做出的贝塞尔曲线到达最终切点位置。从而使得无人驾驶车辆逃离局部极小值点或震荡点区域。
无人驾驶车辆所走过的二次贝塞尔曲线由式:
Ib(τ)=(1-τ)2*Mveh+2*τ*(1-τ)*Mrand+τ2*Mtang,τ∈[0,1] (17)
其中,Mveh为无人驾驶车辆当前位置的坐标,Mrand为新位置点的坐标,Mtang表示最终切点位置的坐标,τ表示动态系数。
使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域的具体过程为:
S5.1以局部极小值点为中心,构造一个环形区域,并在内圆半径和外圆半径范围内,随机选取一个数值作为新的半径长度,构造一个虚拟圆,在虚拟圆上随机选取一点作为待选随机引导点,转至步骤S5.2。其中,Rinner为内圆半径,Router为外圆半径,为第i个待选随机引导点所在虚拟圆的半径。(Xmin,Ymin)为待选随机引导点的位置坐标。(X,Y)为局部极小值点的位置坐标。则待选随机引导点的生成过程如下所示:
S5.2采用步骤S5.1生成δ个待选随机引导点。并转至步骤S5.3。
S5.3求取δ个待选随机引导点与目标点和无人驾驶车辆当前位置之间的距离,然后利用公式(20-22)计算出每一个待选随机引导点的引导常量Tcr_min。
其中,Wr-v和Wr-g为权重因子且为固定常数。Lk为待选随机引导点到目标点之间的距离,Lq为当前无人驾驶车辆到待选随机引导点之间的距离。(Xmin,Ymin)为无人驾驶车辆的位置坐标,(Xgoal,Ygoal)为目标点的位置坐标,(Xrandomi,Yrandomi)为待选随机引导点的位置坐标。
基于δ个待选随机引导点的引导常量,可以得到最小的引导常量,转至步骤S5.4。
S5.4得到最小的引导常量后,将最小的引导常量对应的待选随机引导点作为虚拟目标点,同时,暂时消除最终目标点对无人驾驶车辆的引力作用,只保留虚拟目标点对无人驾驶车辆的引力作用,转至步骤S5.5。
S5.5通过虚拟目标点的引力作用及障碍物的斥力作用,无人驾驶车辆将在合力的作用下移动到虚拟目标点位置,从而逃离局部极小值点。
以上通过实施例对本发明进行了详细说明,但所述内容仅为本发明的示例性实施例,不能被认为用于限定本发明的实施范围。本发明的保护范围由权利要求书限定。凡利用本发明所述的技术方案,或本领域的技术人员在本发明技术方案的启发下,在本发明的实质和保护范围内,设计出类似的技术方案而达到上述技术效果的,或者对申请范围所作的均等变化与改进等,均应仍归属于本发明的专利涵盖保护范围之内。
Claims (5)
1.基于分层监测域和自适应人工势场法的车辆路径规划方法,其特征在于:包括以下步骤:
步骤一:利用分层监测域模型获取无人驾驶车辆的速度:设定无人驾驶车辆的动态监测域,根据障碍物在动态监测域中的位置,计算当前无人驾驶车辆的行驶速度;
在步骤一中,利用分层监测域模型获取无人驾驶车辆的速度的具体过程为:
将监测域分为levelσ0、levelσ1、levelσ2三个预警级别:
其中,dmin为无人驾驶车辆到各障碍物的欧几里得距离的最小值,rσ为动态监测域的半径长度,r1为内圆距离半径,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;
步骤二:计算无人驾驶车辆所受到的合力;障碍物对无人驾驶车辆产生斥力作用,目标点对无人驾驶车辆产生引力作用,所述合力为斥力和引力的和;
步骤三:根据当前无人驾驶车辆的行驶速度和无人驾驶车辆所受到的合力,计算下一个路径点的位置;然后无人驾驶车辆移动到下一位置;
步骤四:判断无人驾驶车辆是否到达目标点;如果无人驾驶车辆到达目标点,则步骤结束,输出无人驾驶车辆已经到达目标点;如果无人驾驶车辆未到达目标点,则转至步骤五;
步骤五:判断无人驾驶车辆是否陷入局部极小值点;如果无人驾驶车辆未陷入局部极小值点,则转至步骤一;如果无人驾驶车辆陷入了局部极小值点,则转至步骤六;
在步骤五中,判断无人驾驶车辆是否陷入局部极小值点的具体过程为:
以无人驾驶车辆当前位置为圆心,Ra为半径做圆,无人驾驶车辆当前位置为路径中的第k个路径点,j=z表示路径中的第z个路径点,Ncount为圆内无人驾驶车辆已经过的路径点数量,则判断无人驾驶车辆是否陷入局部极小值点的执行过程如下所示:
S3.1令Ncount=0,j=1,转至步骤S3.2;
S3.2若第j个路径点到第k个路径点的距离不大于Ra,则转至步骤S3.3;若第j个路径点到第k个路径点的距离大于Ra,则转至步骤S3.4;
S3.3令Ncount=Ncount+1;若Ncount的值达到设定的阈值则流程结束,输出无人驾驶车辆已陷入局部极小值点;若Ncount的值没有达到设定的阈值/>则转至步骤S3.4;
S3.4令j=j+1;若j<=k,则转至步骤S3.2;若j>k,则流程结束,输出无人驾驶车辆未陷入局部极小值点;
步骤六:以距离无人驾驶车辆最近的障碍物为中心,无人驾驶车辆与障碍物之间的距离为半径,构造出斥力等势圈,判断目标点是否在斥力等势圈内;若目标点在斥力等势圈范围外或在斥力等势圈上,则采用切点-贝塞尔曲线模型引导无人驾驶车辆摆脱局部极小值点或震荡点区域;若目标点位于斥力等势圈内部,则使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域;
然后转至步骤四。
2.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法,其特征在于:在步骤二中,无人驾驶车辆所受到的合力的计算公式如下所示:
无人驾驶车辆当前位置的坐标为q=(xveh,yveh)T,目标点的坐标为qgoal=(xgoal,ygoal)T,障碍物的坐标为qoi=((xoi,yoi)T;
无人驾驶车辆所受到的引力Fatt(q)如式(2)所示:
Fatt(q)=katt·ρ(q,qgoal) (2)
其中,katt为引力增益系数,ρ(q,qgoal)=||qgoal-q||,即为无人驾驶车辆到目标点的欧几里得距离;
无人驾驶车辆所受到的第i个障碍物的斥力Frep,i(q)如式(3)所示:
其中,η为斥力增益系数,ρ0为斥力场半径,ρi(q,qoi)表示无人驾驶车辆到障碍物的欧几里得距离,ρ(q,qgoal)为无人驾驶车辆当前位置到目标点之间的相对距离,m为一个大于0的任意整数,Frep1,i(q)的方向是由障碍物指向无人驾驶车辆,该力对无人驾驶车辆产生斥力作用,Frep2,i(q)的方向是由无人驾驶车辆指向目标点,该力引导无人驾驶车辆向目标点移动;
无人驾驶车辆所受到的合力Fsum在x轴、y轴上的合力分量Fsumx、Fsumy的计算过程如下所示:
其中,n为障碍物的数量,i表示第几个障碍物,Fattx(q)、Fatty(q)分别为Fatt(q)在x轴、y轴的引力分量;Frep,ix(q)、Frep,iy(q)分别为Frep,i(q)在x轴、y轴的斥力分量。
3.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法,其特征在于:在步骤三中,无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角θ的计算公式如下所示:
则下一个路径点位置的计算公式如式(8)所示:
其中,θ为无人驾驶车辆所受到的合力的方向与x轴正半轴之间的夹角,(xveh,yveh)为无人驾驶车辆当前位置的坐标,(xveh1,yveh1)为无人驾驶车辆的下一个路径点的坐标,l为无人驾驶车辆的移动步长。
4.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法,其特征在于:所述步骤六中,采用切点-贝塞尔曲线模型引导车辆摆脱局部极小值点或震荡点区域的具体过程为:
S4.1判断目标点与斥力等势圈的位置关系;若目标点在斥力等势圈范围外,则以目标点为起点向斥力等势圈做两条切线,转至步骤S4.2;若目标点在斥力等势圈上,则目标点位置即为最终切点位置,转至步骤S4.4;
S4.2通过斥力等势圆方程与切线方程计算出两条切线对应的两个切点位置,转至步骤S4.3;
S4.3计算无人驾驶车辆当前位置与两个切点之间的距离,并取最小距离所对应的切点位置为最终切点位置,转至步骤S4.4;
S4.4在无人驾驶车辆当前位置与最终切点位置之间添加一个新的位置点,新的位置点的横坐标为最终切点位置的横坐标,新的位置点的纵坐标为无人驾驶车辆当前位置的纵坐标;然后结合这三个定点,利用二次贝塞尔函数生成一条二次贝塞尔曲线,无人驾驶车辆沿着三个定点所做出的贝塞尔曲线到达最终切点位置。
5.如权利要求1所述的基于分层监测域和自适应人工势场法的车辆路径规划方法,其特征在于:所述步骤六中,使用虚拟随机引导策略引导车辆摆脱局部极小值点或震荡点区域的具体过程为:
S5.1以局部极小值点为中心,构造一个环形区域,并在内圆半径和外圆半径范围内,随机选取一个数值作为新的半径长度,构造一个虚拟圆,在虚拟圆上随机选取一点作为待选随机引导点,转至步骤S5.2;
S5.2采用步骤S5.1生成δ个待选随机引导点,并转至步骤S5.3;
S5.3求取δ个待选随机引导点与目标点和无人驾驶车辆当前位置之间的距离,然后计算出每一个待选随机引导点的引导常量,基于δ个待选随机引导点的引导常量,得到最小的引导常量,转至步骤S5.4;
S5.4将最小的引导常量对应的待选随机引导点作为虚拟目标点,同时,暂时消除最终目标点对无人驾驶车辆的引力作用,只保留虚拟目标点对无人驾驶车辆的引力作用,转至步骤S5.5;
S5.5通过虚拟目标点的引力作用及障碍物的斥力作用,无人驾驶车辆在合力的作用下移动到虚拟目标点位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111291864.XA CN114046791B (zh) | 2021-11-02 | 2021-11-02 | 基于分层监测域和自适应人工势场法的车辆路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111291864.XA CN114046791B (zh) | 2021-11-02 | 2021-11-02 | 基于分层监测域和自适应人工势场法的车辆路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114046791A CN114046791A (zh) | 2022-02-15 |
CN114046791B true CN114046791B (zh) | 2023-11-21 |
Family
ID=80206935
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111291864.XA Active CN114046791B (zh) | 2021-11-02 | 2021-11-02 | 基于分层监测域和自适应人工势场法的车辆路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114046791B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117129001B (zh) * | 2023-10-24 | 2024-01-09 | 博创联动科技股份有限公司 | 基于人工智能的自动驾驶路径规划方法及系统 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018176594A1 (zh) * | 2017-03-31 | 2018-10-04 | 深圳市靖洲科技有限公司 | 一种面向无人自行车的人工势场路径规划法 |
CN110244713A (zh) * | 2019-05-22 | 2019-09-17 | 江苏大学 | 一种基于人工势场法的智能车辆换道轨迹规划系统及方法 |
CN112344943A (zh) * | 2020-11-20 | 2021-02-09 | 安徽工程大学 | 一种改进人工势场算法的智能车路径规划方法 |
CN112379672A (zh) * | 2020-11-24 | 2021-02-19 | 浙大宁波理工学院 | 一种基于改进人工势场的智能无人船路径规划方法 |
CN112596525A (zh) * | 2020-12-16 | 2021-04-02 | 中国地质大学(武汉) | 基于遗传算法和改进人工势场法的机器人路径规划方法 |
CN113190004A (zh) * | 2021-05-07 | 2021-07-30 | 河南科技大学 | 一种基于算法融合的无人驾驶车辆避碰路径规划方法 |
CN113342047A (zh) * | 2021-06-23 | 2021-09-03 | 大连大学 | 未知环境中基于障碍物位置预测改进人工势场法的无人机路径规划方法 |
CN113359768A (zh) * | 2021-07-05 | 2021-09-07 | 哈尔滨理工大学 | 一种基于改进的a*算法的路径规划方法 |
-
2021
- 2021-11-02 CN CN202111291864.XA patent/CN114046791B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018176594A1 (zh) * | 2017-03-31 | 2018-10-04 | 深圳市靖洲科技有限公司 | 一种面向无人自行车的人工势场路径规划法 |
CN110244713A (zh) * | 2019-05-22 | 2019-09-17 | 江苏大学 | 一种基于人工势场法的智能车辆换道轨迹规划系统及方法 |
CN112344943A (zh) * | 2020-11-20 | 2021-02-09 | 安徽工程大学 | 一种改进人工势场算法的智能车路径规划方法 |
CN112379672A (zh) * | 2020-11-24 | 2021-02-19 | 浙大宁波理工学院 | 一种基于改进人工势场的智能无人船路径规划方法 |
CN112596525A (zh) * | 2020-12-16 | 2021-04-02 | 中国地质大学(武汉) | 基于遗传算法和改进人工势场法的机器人路径规划方法 |
CN113190004A (zh) * | 2021-05-07 | 2021-07-30 | 河南科技大学 | 一种基于算法融合的无人驾驶车辆避碰路径规划方法 |
CN113342047A (zh) * | 2021-06-23 | 2021-09-03 | 大连大学 | 未知环境中基于障碍物位置预测改进人工势场法的无人机路径规划方法 |
CN113359768A (zh) * | 2021-07-05 | 2021-09-07 | 哈尔滨理工大学 | 一种基于改进的a*算法的路径规划方法 |
Non-Patent Citations (3)
Title |
---|
基于改进人工势场法的无人驾驶车辆局部路径规划的研究;修彩靖;陈慧;;汽车工程(第09期);全文 * |
基于改进人工势场法的机器人实时路径规划;王翼虎;王思明;;兰州交通大学学报(第03期);全文 * |
基于梯度下降法和改进人工势场法的无人车避障方法;方朋朋;杨家富;施杨洋;于凌宇;;制造业自动化(第11期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114046791A (zh) | 2022-02-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110609552B (zh) | 一种水下无人航行器编队平面航迹规划方法 | |
CN112327885B (zh) | 一种无人船自适应全局-局部混合路径规划的方法 | |
CN115079705A (zh) | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 | |
CN112506199B (zh) | 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法 | |
CN114046791B (zh) | 基于分层监测域和自适应人工势场法的车辆路径规划方法 | |
CN113538973B (zh) | 一种基于改进粒子群算法的船舶自动避碰方法 | |
CN116360434A (zh) | 基于改进的csac-apf算法的船舶路径规划方法 | |
Zhang et al. | Multi-UUV path planning based on improved artificial potential field method | |
Wu et al. | Trajectory prediction based on planning method considering collision risk | |
Kim et al. | Curve tracking control for autonomous vehicles with rigidly mounted range sensors | |
CN111121804A (zh) | 一种具有安全性约束的智能车辆路径规划方法及系统 | |
Guo et al. | Research on path-tracking control method of intelligent vehicle based on adaptive two-point preview | |
Zhang et al. | Local path planning of unmanned underwater vehicle based on improved APF and rolling window method | |
Zhang et al. | Path planning for autonomous driving with curvature-considered quadratic optimization | |
Fan et al. | An Improved Path Planning Algorithm With Adaptive Parameters and Predictions | |
Li et al. | Path tracking and local obstacle avoidance for automated vehicle based on improved artificial potential field | |
Er et al. | Hybrid Adaptive Path Planning for a USV under Complex Marine Environment | |
Li et al. | Research on real-time obstacle avoidance planning for an unmanned surface vessel based on the grid cell mechanism | |
Zhuang et al. | Adaptive Stanley Control Method Based on Dynamic Window Approach | |
Lin et al. | An adaptive dynamic window approach for UUV obstacle avoidance planning in 3D environments | |
Luo et al. | DWA-based Dynamic Obstacle Avoidance Planning Method for USVs | |
Shen et al. | Optimal hybrid path planning considering dynamic and static characteristics of intelligent vehicles | |
Wang et al. | Research on Local Path Planning for Autonomous Collision Avoidance of USV | |
Xu et al. | Parking Dispatch System for Infrastructure-based Automated Valet Parking | |
Zhu et al. | Path Planning and Tracking Control of Car-like Robot Based on Improved NSGA-III and Fuzzy Sliding Mode Control |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |