CN110018689A - 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 - Google Patents
一种基于动态窗口的多虚拟目标点全局动态路径规划算法 Download PDFInfo
- Publication number
- CN110018689A CN110018689A CN201910403311.5A CN201910403311A CN110018689A CN 110018689 A CN110018689 A CN 110018689A CN 201910403311 A CN201910403311 A CN 201910403311A CN 110018689 A CN110018689 A CN 110018689A
- Authority
- CN
- China
- Prior art keywords
- unmanned vehicle
- target point
- virtual target
- speed
- path planning
- 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.)
- Granted
Links
- 238000013439 planning Methods 0.000 title claims abstract description 46
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 20
- 230000004888 barrier function Effects 0.000 claims abstract description 37
- 238000000034 method Methods 0.000 claims abstract description 20
- 238000004088 simulation Methods 0.000 claims abstract description 16
- 238000011156 evaluation Methods 0.000 claims description 18
- 238000005070 sampling Methods 0.000 claims description 4
- 238000012800 visualization Methods 0.000 claims 1
- 230000007547 defect Effects 0.000 abstract description 3
- 230000006870 function Effects 0.000 description 18
- 238000010586 diagram Methods 0.000 description 11
- 238000004590 computer program Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 230000006872 improvement Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000008034 disappearance Effects 0.000 description 1
- 238000004387 environmental modeling Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000007634 remodeling Methods 0.000 description 1
- 238000005204 segregation Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于动态窗口的多虚拟目标点全局动态路径规划算法,将动态窗口法应用在全局路径规划方向,改进传统全局规划路径不连续不平滑的缺陷。具体操作是通过增加多个虚拟目标点、在特殊位置更新无人车的航向角并加入了随机障碍物,实现了无人车的全局动态路径规划,并将其应用在“中国大学生无人驾驶方程式大赛”中的绕“8”赛道等复杂环境下。MATLAB仿真结果表明无人车能够有效的避开随机出现的障碍物,并安全稳定地完成全局路径的规划与追踪。
Description
技术领域
本发明涉及无人驾驶导航技术领域,特别是一种基于动态窗口的多虚拟目标点全局动态路径规划算法。
背景技术
无人车路径规划就是在综合考虑行驶时间、速度、油耗和安全等因素的前提下,为无人车规划出最优的行驶轨迹。无人车为了完成在复杂道路上的安全行驶,其自主移动能力是其基础,其中路径规划和导航是保证无人车自主移动能力的核心算法,是体现车辆智慧水平的关键所在。
从路径规划的目标范围看,通常分为全局路径规划和局部路径规划;从规划环境是否随时间变化方面看,还可以分为静态路径规划和动态路径规划。全局路径规划通常假设环境信息完全已知,在无人车运动之前,离线计算出一条从起点到终点的路径;但是环境并不是一成不变的,当环境信息改变时全局路径规划方法并不能快速规划出一条安全可行的路径。由于局部路径规划方法有计算量小,实时性好,规划轨迹圆滑等优点,目前通常采用把全局与局部路径规划结合的二层避障结构,在第一层,通过已知的环境信息(一般为地图),离线规划出一条从起点到目标点代价值(油耗或者时间)最小的路径,该路径不需平滑但应能避开地图上已知的静态障碍物;在第二层,无人车沿着第一层规划的路径行驶,当在行驶过程中通过传感器检测到新的障碍物时,再通过获得的障碍物详细的位置信息进行局部在线避障。
发明内容
有鉴于此,本发明的目的是提出一种基于动态窗口的多虚拟目标点全局动态路径规划算法,能够有效地避开随机出现的障碍物,并安全稳定地完成全局路径的规划与追踪。
本发明采用以下方案实现:一种基于动态窗口的多虚拟目标点全局动态路径规划算法,具体包括以下步骤:
步骤S1:获取障碍物坐标信息、无人车运动模型和初始状态信息;
步骤S2:设置多个虚拟目标点goal;
步骤S3:判断虚拟目标点与无人车当前时刻的空间位置,选择合适的航向角;
步骤S4:随机产生障碍物;
步骤S5:根据采样的线速度和角速度,计算动态速度窗口;
步骤S6:判断是否需要开始避障;
步骤S7:在动态窗口中计算可达的速度矢量;
步骤S8:在可达的速度矢量窗口中,根据评价函数选择最佳避障速度矢量;
步骤S9:输出最佳避障速度的大小和方向;
步骤S10:重复步骤S3至步骤S9,直至到达终点完成全局的路径追踪,并描点连线对无人车运动轨迹可视化。
进一步地,步骤S2具体为:在原来一个目标点的基础上增加多个目标点,即最后一个目标点为实际的终点,而在终点之前的所有目标点为虚拟目标点,小车在到达虚拟目标点时不会停止运动,而是接着向下一个虚拟目标点运动,并且当前虚拟目标点消失不再对无人车起导向作用,直到下一个目标点为终点,至终点时将小车的速度更新为0,从而完成全局路径的追踪行驶。
进一步地,步骤S3具体包括以下步骤:
步骤S31:无人车当前时刻的航向角是由初始航向角加上无人车角速度wt对时间t的积分求出的,即:
yaw(t)∈[0,+∞);
式中,yaw(t)表示当前时刻航向角,yaw(0)表示初始时刻航向角;wt>0表示逆时针行驶,wt<0表示顺时针行驶;
步骤S32:令评价函数为:
heading=180°-κ;
式中,κ表示当前时刻无人车速度方向和与目标点方向之间的夹角;
当时:
当时:
式中,ygi表示第i个目标点的纵坐标,yt表示当前时刻无人车的纵坐标,xgi表示第i个目标点的横坐标,xt表示当前时刻无人车的横坐标。
进一步地,在无人车行驶过程中,判断虚拟目标点纵坐标ygi与当前时刻无人车的纵坐标yt的大小,当ygi>yt时,更新yaw(t)∈[0,π]。
进一步地,当ygi>yt时,取当ygi<yt时,取
进一步地,步骤S5具体包括以下步骤:
步骤S51:明确无人车运动学模型的线速度和角速度的最大值vmax、ωmax,通过设置一定的时间间隔,计算在当前时刻速度(v,ω)和无人车加减速度的限制下,所能达到的最大和最小线速度vtmax、vtmin以及最大和最小角速度ωtmax、ωtmin,通过将其与vmax、ωmax比较得出在当前时刻速度下(v,ω)的动态速度窗口;
步骤S52:计算速度空间中每组线速度和角速度在前向模拟时间δ内的运动轨迹。
进一步地,步骤S52中,所述前向模拟时间δ的取值为4s。
进一步地,步骤S6具体为:求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离Stopdist;若dist>Stopdist,则此时刻速度为安全速度,需刹车减速防止碰撞障碍物;若dist<Stopdist,则需要舍弃动态窗口中的这些轨迹,选取可行的行驶轨迹,即需要开始避障。
进一步地,步骤S7中,所述可达的速度矢量为:无人车与最近障碍物之间的距离大于无人车到最近障碍物的刹车距离时的速度矢量。
进一步地,步骤S8具体为:经过步骤S7得到的可达速度窗口,其中包含有多组可行的运动轨迹,这些轨迹中只能选择其中的一条作为最优轨迹用来追踪,选择哪一条取决于评价函数:
G=δ(α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω));
取评价函数的最大值对应的速度(v,ω)作为最佳的避障速度矢量;评价函数中包含三个决定轨迹最优的因素,分别为heading(v,ω)、dist(v,ω)和velocity(v,ω),其中velocity(v,ω)为在当前(v,ω)下无人车运动速度的大小,heading(v,ω)为无人车速度方向和与目标点方向之间夹角的补角,dist(v,ω)为无人车与最近障碍物之间的距离,β为安全权重,γ为速度权重,δ为前向模拟时间。
与现有技术相比,本发明有以下有益效果:本发明将动态窗口法应用在全局路径规划方向,改进传统全局规划路径不连续不平滑的缺陷。MATLAB仿真结果表明无人车能够有效地避开随机出现的障碍物,并安全稳定地完成全局路径的规划与追踪。
附图说明
图1为本发明实施例的绕“8”赛道示意图。
图2为本发明实施例的绕“8”赛道轨迹示意图。
图3为本发明实施例的无人车按比赛规则避障示意图。
图4为本发明实施例的无人车绕行赛道避障示意图。
图5为本发明实施例的方法流程示意图。
具体实施方式
下面结合附图及实施例对本发明做进一步说明。
应该指出,以下详细说明都是示例性的,旨在对本申请提供进一步的说明。除非另有指明,本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本申请的示例性实施方式。如在这里所使用的,除非上下文另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
本实施例提供了一种基于动态窗口的多虚拟目标点全局动态路径规划算法,本实施例以“中国大学生无人驾驶方程式大赛”中的绕“8”赛道等复杂环境下,赛道如图1所示,规则如图2所示。中国大学生无人驾驶方程式大赛中的绕“8”赛道需要无人车按照规则在右圆转两圈,然后进入左圆行驶两圈,最后行驶出赛道。因为行驶第二圈其实是对第一圈的重复,仿真得到的轨迹会重合,从而为了仿真结果显示的可观性,本实施例在两圆各行驶一圈。
本实施所进行的工作具体为:第一,本实施例依据比赛行驶规则设计了多组虚拟目标点,虚拟目标点根据无人车和锥桶的尺寸,设置在靠近内圆的位置保证行驶路径最短,每个虚拟目标点依次显示并作用于无人车,最终实现了全局路径的行驶(绕“8”运动)。过程中:通过判定虚拟目标点和小车当前位置在全局坐标系下y值的大小,更新此时小车运动状态的航向角,避免了航向角在运动过程中的不断增大(逆时针运动)或减小(顺时针运动),保证了动态窗口算法中函数始终为正,其中,heading=180°-κ,κ为当前时刻无人车运动方向和与目标点方向之间的夹角。第二,本实施例在“8”型复杂赛道中加入了随机障碍物(默认为和赛道同型号的锥桶),用于模拟无人车在实际行驶中遇到的行人或者车辆等其它障碍物,并在MATLAB环境下进行仿真,结果表明:当随机障碍物没有堵塞赛道时,无人车绕过障碍物安全实现了绕“8”行走;当随机出现的障碍物堵塞了赛道1时,无人车进入赛道2到达了虚拟目标点,此过程中没有触及赛道1的障碍物,从而更加验证了本发明的局部路径规划和跟踪的稳定性和安全性。
如图5所示,本实施例具体包括以下步骤:
步骤S1:获取障碍物坐标信息、无人车运动模型和初始状态信息;
步骤S2:设置多个虚拟目标点goal;
步骤S3:判断虚拟目标点与无人车当前时刻的空间位置,选择合适的航向角;
步骤S4:随机产生障碍物;
步骤S5:根据采样的线速度和角速度,计算动态速度窗口;
步骤S6:判断是否需要开始避障;
步骤S7:在动态窗口中计算可达的速度矢量;
步骤S8:在可达的速度矢量窗口中,根据评价函数选择最佳避障速度矢量;
步骤S9:输出最佳避障速度的大小和方向;
步骤S10:重复步骤S3至步骤S9,直至到达终点完成全局的路径追踪,并描点连线对无人车运动轨迹可视化。
在本实施例中,步骤S1具体为:获取障碍物坐标信息、无人车运动模型和初始状态信息通过无人车自身携带的摄像头、激光雷达、GPS、惯导、里程计以及线加速度和角加速度等传感器的数据融合进行建模得到。其中,建模时考虑了无人车自身的尺寸和赛道所使用的锥桶尺寸,即将圆形锥桶的半径加上无人车的长半轴作为安全距离dist,当无人车质心至锥桶圆心的距离小于安全距离时就代表无人车碰撞了障碍物,反之则为安全通过。因此无人车和实际障碍物(锥桶)可以作为一个质点来进行环境建模,具体表现为坐标系中的坐标形式。
在本实施例中,步骤S2具体为:在原来一个目标点的基础上增加多个目标点,即最后一个目标点为实际的终点,而在终点之前的所有目标点为虚拟目标点,小车在到达虚拟目标点时不会停止运动,而是接着向下一个虚拟目标点运动,并且当前虚拟目标点消失不再对无人车起导向作用,直到下一个目标点为终点,至终点时将小车的速度更新为0,从而完成全局路径的追踪行驶。
在本实施例中,步骤S3具体包括以下步骤:
步骤S31:无人车当前时刻的航向角是由初始航向角加上无人车角速度wt对时间t的积分求出的,即:
yaw(t)∈[0,+∞);
式中,yaw(t)表示当前时刻航向角,yaw(0)表示初始时刻航向角;wt>0表示逆时针行驶,wt<0表示顺时针行驶;
步骤S32:令评价函数为:
heading=180°-κ;
式中,κ表示当前时刻无人车速度方向和与目标点方向之间的夹角;
当时:
当时:
式中,ygi表示第i个目标点的纵坐标,yt表示当前时刻无人车的纵坐标,xgi表示第i个目标点的横坐标,xt表示当前时刻无人车的横坐标。
根据以上推导,会出现heading=180°-κ<0,比如当yaw(t)>π,时。评价函数影响因素heading<0的发生,会造成无人车背离目标点的现象。heading<0情况的出现,根本原因在于无人车在绕“8”赛道中需要持续逆时针或者顺时针行驶,使得|yaw(t)|不断地增大,导致κ也在增大。为保证heading>0,本发明改进使其值域为(0,π)。
具体的,在本实施例中,在无人车行驶过程中,判断虚拟目标点纵坐标ygi与当前时刻无人车的纵坐标yt的大小,当ygi>yt时,更新yaw(t)∈[0,π]。
在本实施例中,根据赛道形状和虚拟目标点选取位置,当ygi>yt时,取当ygi<yt时,取经过仿真分析,验证了此方法的灵活性和可行性。
较佳的,在本实施例中,步骤S4具体为:在经过步骤S2和步骤S3的改进之后,无人车初步具有了全局路径规划的能力,但在实际行驶中意外障碍物可能随时出现,这就要求无人车在满足全局路径规划的要求下,实现实时的避障行驶。本实施例在“8”型复杂赛道中加入了随机障碍物(默认为和赛道同型号的锥桶),当随机障碍物没有堵塞赛道时,无人车绕过障碍物按照规则安全实现了绕“8”行走,如附图3,其中圆圈标记的随机障碍物为无人车通过之后在运行轨迹上产生的,不是无人车碰撞了障碍物;当随机出现的障碍物堵塞了赛道1时,无人车进入赛道2到达了虚拟目标点,此过程中没有触及赛道1的障碍物,从而更加验证了本算法的稳定性和安全性,仿真结果如附图4。
在本实施例中,步骤S5具体包括以下步骤:
步骤S51:明确无人车运动学模型的线速度和角速度的最大值vmax、ωmax,通过设置一定的时间间隔(本实施例取Δt=0.1),计算在当前时刻速度(v,ω)和无人车加减速度的限制下,所能达到的最大和最小线速度vtmax、vtmin以及最大和最小角速度ωtmax、ωtmin,通过将其与vmax、ωmax比较得出在当前时刻速度下(v,ω)的动态速度窗口;之所以称为动态窗口是因为无人车的加减速性能限定速度采样空间在一个可行的动态范围内;
步骤S52:计算速度空间中每组线速度和角速度在前向模拟时间δ内的运动轨迹。
在本实施例中,,前向模拟时间取得过大和过小会影响无人车的实时避障能力,经过调试参数,步骤S52中,所述前向模拟时间δ的取值为4s,很好的满足了本文所设计的复杂避障环境。
在本实施例中,通过判断动态窗口中每条轨迹上是否存在障碍物以及与最近障碍物之间的距离大小,进行无人车的刹车避障功能。具体说来,步骤S6具体为:求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离Stopdist;若dist>Stopdist,则此时刻速度为安全速度,需刹车减速防止碰撞障碍物;若dist<Stopdist,则需要舍弃动态窗口中的这些轨迹,选取可行的行驶轨迹,即需要开始避障。
在本实施例中,步骤S7中,所述可达的速度矢量为:无人车与最近障碍物之间的距离大于无人车到最近障碍物的刹车距离时的速度矢量。即:除去步骤S6中提到的使得dist<stopdist的速度矢量,剩余的速度矢量即为可达的速度矢量。进入步骤S8选取最佳速度矢量。
在本实施例中,步骤S8具体为:经过步骤S7得到的可达速度窗口,其中包含有多组可行的运动轨迹,这些轨迹中只能选择其中的一条作为最优轨迹用来追踪,选择哪一条取决于评价函数:
G=δ(α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω));
取评价函数的最大值对应的速度(v,ω)作为最佳的避障速度矢量;评价函数中包含三个决定轨迹最优的因素,分别为heading(v,ω)、dist(v,ω)和velocity(v,ω),其中velocity(v,ω)为在当前(v,ω)下无人车运动速度的大小,heading(v,ω)为无人车速度方向和与目标点方向之间夹角的补角,dist(v,ω)为无人车与最近障碍物之间的距离,β为安全权重,γ为速度权重,δ为前向模拟时间。总的来说,由三者构建的评价函数保证了,在局部导航过程中,使得无人车朝着目标以较快速度行驶并且避开障碍物。
较佳的,在本实施例中,步骤S9具体为:步骤S8中求出评价函数最大时对应轨迹的(v,ω),并将其发送至无人车的执行机构,通过电信号控制电机的转速和方向盘转角,控制无人车沿着最优避障路径行驶。
步骤S10具体为:重复以上循环,直至到达终点完成全局的路径追踪,并描点连线对无人车运动轨迹可视化。
特别的,本实施例属于无人驾驶路径规划和导航技术领域,具体为一种基于动态窗口(dynamic window approach,DWA)的多虚拟目标点全局动态路径规划方法。动态窗口法的典型应用是在局部路径规划方面,因动态窗口法具有路径平滑、计算效率高等优点,因此本实施例旨在将动态窗口法应用在全局路径规划方向,改进传统全局规划路径不连续不平滑的缺陷。具体操作是通过增加多个虚拟目标点、在特殊位置更新无人车的航向角并加入了随机障碍物,实现了无人车的全局动态路径规划,并将其应用在“中国大学生无人驾驶方程式大赛”中的绕“8”赛道等复杂环境下。MATLAB仿真结果表明无人车能够有效的避开随机出现的障碍物,并安全稳定地完成全局路径的规划与追踪。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述,仅是本发明的较佳实施例而已,并非是对本发明作其它形式的限制,任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型,仍属于本发明技术方案的保护范围。
Claims (10)
1.一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于,包括以下步骤:
步骤S1:获取障碍物坐标信息、无人车运动模型和初始状态信息;
步骤S2:设置多个虚拟目标点goal;
步骤S3:判断虚拟目标点与无人车当前时刻的空间位置,选择合适的航向角;
步骤S4:随机产生障碍物;
步骤S5:根据采样的线速度和角速度,计算动态速度窗口;
步骤S6:判断是否需要开始避障;
步骤S7:在动态窗口中计算可达的速度矢量;
步骤S8:在可达的速度矢量窗口中,根据评价函数选择最佳避障速度矢量;
步骤S9:输出最佳避障速度的大小和方向;
步骤S10:重复步骤S3至步骤S9,直至到达终点完成全局的路径追踪,并描点连线对无人车运动轨迹可视化。
2.根据权利要求1所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S2具体为:在原来一个目标点的基础上增加多个目标点,即最后一个目标点为实际的终点,而在终点之前的所有目标点为虚拟目标点,小车在到达虚拟目标点时不会停止运动,而是接着向下一个虚拟目标点运动,并且当前虚拟目标点消失不再对无人车起导向作用,直到下一个目标点为终点,至终点时将小车的速度更新为0,从而完成全局路径的追踪行驶。
3.根据权利要求1所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S3具体包括以下步骤:
步骤S31:无人车当前时刻的航向角是由初始航向角加上无人车角速度wt对时间t的积分求出的,即:
yaw(t)∈[0,+∞);
式中,yaw(t)表示当前时刻航向角,yaw(0)表示初始时刻航向角;wt>0表示逆时针行驶,wt<0表示顺时针行驶;
步骤S32:令评价函数为:
heading=180°-κ;
式中,κ表示当前时刻无人车速度方向和与目标点方向之间的夹角;
当时:
当时:
式中,ygi表示第i个目标点的纵坐标,yt表示当前时刻无人车的纵坐标,xgi表示第i个目标点的横坐标,xt表示当前时刻无人车的横坐标。
4.根据权利要求3所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:在无人车行驶过程中,判断虚拟目标点纵坐标ygi与当前时刻无人车的纵坐标yt的大小,当ygi>yt时,更新yaw(t)∈[0,π]。
5.根据权利要求4所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:当ygi>yt时,取当ygi<yt时,取
6.根据权利要求1所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S5具体包括以下步骤:
步骤S51:明确无人车运动学模型的线速度和角速度的最大值vmax、ωmax,通过设置一定的时间间隔,计算在当前时刻速度(v,ω)和无人车加减速度的限制下,所能达到的最大和最小线速度vtmax、vtmin以及最大和最小角速度ωtmax、ωtmin,通过将其与vmax、ωmax比较得出在当前时刻速度下(v,ω)的动态速度窗口;
步骤S52:计算速度空间中每组线速度和角速度在前向模拟时间δ内的运动轨迹。
7.根据权利要求6所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S52中,所述前向模拟时间δ的取值为4s。
8.根据权利要求1所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S6具体为:求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离Stopdist;若dist>Stopdist,则此时刻速度为安全速度,需刹车减速防止碰撞障碍物;若dist<Stopdist,则需要舍弃动态窗口中的这些轨迹,选取可行的行驶轨迹,即需要开始避障。
9.根据权利要求1所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S7中,所述可达的速度矢量为:无人车与最近障碍物之间的距离大于无人车到最近障碍物的刹车距离时的速度矢量。
10.根据权利要求1所述的一种基于动态窗口的多虚拟目标点全局动态路径规划算法,其特征在于:步骤S8具体为:经过步骤S7得到的可达速度窗口,其中包含有多组可行的运动轨迹,这些轨迹中只能选择其中的一条作为最优轨迹用来追踪,选择哪一条取决于评价函数:
G=δ(α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω));
取评价函数的最大值对应的速度(v,ω)作为最佳的避障速度矢量;评价函数中包含三个决定轨迹最优的因素,分别为heading(v,ω)、dist(v,ω)和velocity(v,ω),其中velocity(v,ω)为在当前(v,ω)下无人车运动速度的大小,heading(v,ω)为无人车速度方向和与目标点方向之间夹角的补角,dist(v,ω)为无人车与最近障碍物之间的距离,β为安全权重,γ为速度权重,δ为前向模拟时间。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910403311.5A CN110018689B (zh) | 2019-05-15 | 2019-05-15 | 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910403311.5A CN110018689B (zh) | 2019-05-15 | 2019-05-15 | 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110018689A true CN110018689A (zh) | 2019-07-16 |
CN110018689B CN110018689B (zh) | 2020-07-07 |
Family
ID=67193743
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910403311.5A Expired - Fee Related CN110018689B (zh) | 2019-05-15 | 2019-05-15 | 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110018689B (zh) |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110514224A (zh) * | 2019-08-26 | 2019-11-29 | 中国人民解放军军事科学院国防科技创新研究院 | 一种无人驾驶汽车局部路径规划性能评价方法 |
CN110609559A (zh) * | 2019-10-25 | 2019-12-24 | 江苏恒澄交科信息科技股份有限公司 | 用于无人船路径跟随和避障的改进dwa动态窗口法 |
CN110703767A (zh) * | 2019-11-08 | 2020-01-17 | 江苏理工学院 | 一种基于改进智能水滴算法的无人车避障路径规划方法 |
CN111506081A (zh) * | 2020-05-15 | 2020-08-07 | 中南大学 | 一种机器人轨迹跟踪方法、系统及存储介质 |
CN111679667A (zh) * | 2020-05-20 | 2020-09-18 | 东南大学 | 一种面向无人驾驶赛车的路径与车速协同规划方法 |
CN111986224A (zh) * | 2020-08-05 | 2020-11-24 | 七海行(深圳)科技有限公司 | 一种目标行为预测追踪方法及装置 |
CN112445222A (zh) * | 2019-09-05 | 2021-03-05 | 阿里巴巴集团控股有限公司 | 导航方法、装置、存储介质以及终端 |
CN112504272A (zh) * | 2020-07-14 | 2021-03-16 | 北京理工大学 | 一种快速无人机路径重构方法 |
CN112731925A (zh) * | 2020-12-21 | 2021-04-30 | 浙江科技学院 | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 |
CN112859842A (zh) * | 2020-12-31 | 2021-05-28 | 中山大学 | 一种路径跟随导航方法及其系统 |
CN112904855A (zh) * | 2021-01-19 | 2021-06-04 | 四川阿泰因机器人智能装备有限公司 | 基于改进动态窗口的跟随机器人局部路径规划方法 |
CN113050648A (zh) * | 2021-03-24 | 2021-06-29 | 珠海市一微半导体有限公司 | 一种机器人避障方法和系统 |
CN113467480A (zh) * | 2021-08-09 | 2021-10-01 | 广东工业大学 | 一种用于无人驾驶方程式的全局路径规划算法 |
CN113568435A (zh) * | 2021-09-24 | 2021-10-29 | 深圳火眼智能有限公司 | 一种基于无人机自主飞行态势感知趋势的分析方法与系统 |
CN113608531A (zh) * | 2021-07-26 | 2021-11-05 | 福州大学 | 基于安全a*引导点的动态窗口的无人车实时全局路径规划方法 |
CN113625711A (zh) * | 2021-08-02 | 2021-11-09 | 福州大学 | 一种基于危险系数的自适应动态窗口无人车实时避障方法 |
CN113885771A (zh) * | 2021-12-09 | 2022-01-04 | 北京车网科技发展有限公司 | 一种轨迹信息的可视化处理方法 |
CN114842660A (zh) * | 2022-06-29 | 2022-08-02 | 石家庄铁道大学 | 一种无人车道路轨迹预测方法、装置和电子设备 |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20100299013A1 (en) * | 2009-05-22 | 2010-11-25 | Toyota Motor Engin. & Manufact. | Using topological structure for path planning in semi-structured environments |
CN104102219A (zh) * | 2014-07-09 | 2014-10-15 | 大连理工大学 | 智能导购车行驶路径的规划方法、装置及智能导购车 |
CN105717929A (zh) * | 2016-04-29 | 2016-06-29 | 中国人民解放军国防科学技术大学 | 一种多分辨率障碍物环境下移动机器人混合路径规划方法 |
CN106843235A (zh) * | 2017-03-31 | 2017-06-13 | 深圳市靖洲科技有限公司 | 一种面向无人自行车的人工势场路径规划法 |
WO2017199751A1 (ja) * | 2016-05-16 | 2017-11-23 | 本田技研工業株式会社 | 車両制御システム、車両制御方法、および車両制御プログラム |
CN107491070A (zh) * | 2017-08-31 | 2017-12-19 | 成都通甲优博科技有限责任公司 | 一种移动机器人路径规划方法及装置 |
CN107885209A (zh) * | 2017-11-13 | 2018-04-06 | 浙江工业大学 | 一种基于动态窗口与虚拟目标点的避障方法 |
CN108594834A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种面向未知环境下多auv自适应目标搜索和避障方法 |
CN108594807A (zh) * | 2018-03-30 | 2018-09-28 | 武汉理工大学 | 一种无人水面艇折线路径跟踪控制系统及方法 |
CN108664020A (zh) * | 2018-04-11 | 2018-10-16 | 上海大学 | 一种基于速度障碍法和动态窗口法的无人艇动态避障算法 |
WO2018237112A1 (en) * | 2017-06-21 | 2018-12-27 | Locus Robotics Corp. | ROBOT WAITING SYSTEM AND METHOD FOR ONE OR MORE PROCESSING STATIONS |
CN109434836A (zh) * | 2018-12-14 | 2019-03-08 | 浙江大学 | 一种结合球树模型的机械手人工势场空间路径规划方法 |
-
2019
- 2019-05-15 CN CN201910403311.5A patent/CN110018689B/zh not_active Expired - Fee Related
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20100299013A1 (en) * | 2009-05-22 | 2010-11-25 | Toyota Motor Engin. & Manufact. | Using topological structure for path planning in semi-structured environments |
CN104102219A (zh) * | 2014-07-09 | 2014-10-15 | 大连理工大学 | 智能导购车行驶路径的规划方法、装置及智能导购车 |
CN105717929A (zh) * | 2016-04-29 | 2016-06-29 | 中国人民解放军国防科学技术大学 | 一种多分辨率障碍物环境下移动机器人混合路径规划方法 |
WO2017199751A1 (ja) * | 2016-05-16 | 2017-11-23 | 本田技研工業株式会社 | 車両制御システム、車両制御方法、および車両制御プログラム |
CN106843235A (zh) * | 2017-03-31 | 2017-06-13 | 深圳市靖洲科技有限公司 | 一种面向无人自行车的人工势场路径规划法 |
WO2018237112A1 (en) * | 2017-06-21 | 2018-12-27 | Locus Robotics Corp. | ROBOT WAITING SYSTEM AND METHOD FOR ONE OR MORE PROCESSING STATIONS |
CN107491070A (zh) * | 2017-08-31 | 2017-12-19 | 成都通甲优博科技有限责任公司 | 一种移动机器人路径规划方法及装置 |
CN107885209A (zh) * | 2017-11-13 | 2018-04-06 | 浙江工业大学 | 一种基于动态窗口与虚拟目标点的避障方法 |
CN108594834A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种面向未知环境下多auv自适应目标搜索和避障方法 |
CN108594807A (zh) * | 2018-03-30 | 2018-09-28 | 武汉理工大学 | 一种无人水面艇折线路径跟踪控制系统及方法 |
CN108664020A (zh) * | 2018-04-11 | 2018-10-16 | 上海大学 | 一种基于速度障碍法和动态窗口法的无人艇动态避障算法 |
CN109434836A (zh) * | 2018-12-14 | 2019-03-08 | 浙江大学 | 一种结合球树模型的机械手人工势场空间路径规划方法 |
Non-Patent Citations (2)
Title |
---|
YUANHUI WANG,等: "Real-time Obstacle Avoidance of Hovercraft Based on Follow the Gap with Dynamic Window Approach", 《OCEANS 2018 MTS/IEEE CHARLESTON》 * |
张洋扬,等: "基于速度障碍法和动态窗口法的无人水面艇动态避障", 《上海大学学报(自然科学版)》 * |
Cited By (30)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110514224B (zh) * | 2019-08-26 | 2021-04-02 | 中国人民解放军军事科学院国防科技创新研究院 | 一种无人驾驶汽车局部路径规划性能评价方法 |
CN110514224A (zh) * | 2019-08-26 | 2019-11-29 | 中国人民解放军军事科学院国防科技创新研究院 | 一种无人驾驶汽车局部路径规划性能评价方法 |
CN112445222A (zh) * | 2019-09-05 | 2021-03-05 | 阿里巴巴集团控股有限公司 | 导航方法、装置、存储介质以及终端 |
CN110609559A (zh) * | 2019-10-25 | 2019-12-24 | 江苏恒澄交科信息科技股份有限公司 | 用于无人船路径跟随和避障的改进dwa动态窗口法 |
CN110703767A (zh) * | 2019-11-08 | 2020-01-17 | 江苏理工学院 | 一种基于改进智能水滴算法的无人车避障路径规划方法 |
CN111506081B (zh) * | 2020-05-15 | 2021-06-25 | 中南大学 | 一种机器人轨迹跟踪方法、系统及存储介质 |
CN111506081A (zh) * | 2020-05-15 | 2020-08-07 | 中南大学 | 一种机器人轨迹跟踪方法、系统及存储介质 |
CN111679667A (zh) * | 2020-05-20 | 2020-09-18 | 东南大学 | 一种面向无人驾驶赛车的路径与车速协同规划方法 |
CN111679667B (zh) * | 2020-05-20 | 2022-09-02 | 东南大学 | 一种面向无人驾驶赛车的路径与车速协同规划方法 |
CN112504272A (zh) * | 2020-07-14 | 2021-03-16 | 北京理工大学 | 一种快速无人机路径重构方法 |
CN111986224A (zh) * | 2020-08-05 | 2020-11-24 | 七海行(深圳)科技有限公司 | 一种目标行为预测追踪方法及装置 |
CN111986224B (zh) * | 2020-08-05 | 2024-01-05 | 七海行(深圳)科技有限公司 | 一种目标行为预测追踪方法及装置 |
CN112731925B (zh) * | 2020-12-21 | 2024-03-15 | 浙江科技学院 | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 |
CN112731925A (zh) * | 2020-12-21 | 2021-04-30 | 浙江科技学院 | 用于无人驾驶方程式赛车锥桶识别和路径规划及控制方法 |
CN112859842A (zh) * | 2020-12-31 | 2021-05-28 | 中山大学 | 一种路径跟随导航方法及其系统 |
CN112859842B (zh) * | 2020-12-31 | 2022-06-14 | 中山大学 | 一种路径跟随导航方法及其系统 |
CN112904855A (zh) * | 2021-01-19 | 2021-06-04 | 四川阿泰因机器人智能装备有限公司 | 基于改进动态窗口的跟随机器人局部路径规划方法 |
CN112904855B (zh) * | 2021-01-19 | 2022-08-16 | 四川阿泰因机器人智能装备有限公司 | 基于改进动态窗口的跟随机器人局部路径规划方法 |
CN113050648A (zh) * | 2021-03-24 | 2021-06-29 | 珠海市一微半导体有限公司 | 一种机器人避障方法和系统 |
CN113608531A (zh) * | 2021-07-26 | 2021-11-05 | 福州大学 | 基于安全a*引导点的动态窗口的无人车实时全局路径规划方法 |
CN113608531B (zh) * | 2021-07-26 | 2023-09-12 | 福州大学 | 基于安全a*引导点的无人车实时全局路径规划方法 |
CN113625711A (zh) * | 2021-08-02 | 2021-11-09 | 福州大学 | 一种基于危险系数的自适应动态窗口无人车实时避障方法 |
CN113625711B (zh) * | 2021-08-02 | 2023-05-30 | 福州大学 | 一种基于危险系数的自适应动态窗口无人车实时避障方法 |
CN113467480A (zh) * | 2021-08-09 | 2021-10-01 | 广东工业大学 | 一种用于无人驾驶方程式的全局路径规划算法 |
CN113467480B (zh) * | 2021-08-09 | 2024-02-13 | 广东工业大学 | 一种用于无人驾驶方程式的全局路径规划算法 |
CN113568435B (zh) * | 2021-09-24 | 2021-12-24 | 深圳火眼智能有限公司 | 一种基于无人机自主飞行态势感知趋势的分析方法与系统 |
CN113568435A (zh) * | 2021-09-24 | 2021-10-29 | 深圳火眼智能有限公司 | 一种基于无人机自主飞行态势感知趋势的分析方法与系统 |
CN113885771A (zh) * | 2021-12-09 | 2022-01-04 | 北京车网科技发展有限公司 | 一种轨迹信息的可视化处理方法 |
CN114842660B (zh) * | 2022-06-29 | 2022-10-11 | 石家庄铁道大学 | 一种无人车道路轨迹预测方法、装置和电子设备 |
CN114842660A (zh) * | 2022-06-29 | 2022-08-02 | 石家庄铁道大学 | 一种无人车道路轨迹预测方法、装置和电子设备 |
Also Published As
Publication number | Publication date |
---|---|
CN110018689B (zh) | 2020-07-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110018689A (zh) | 一种基于动态窗口的多虚拟目标点全局动态路径规划算法 | |
Huang et al. | A motion planning and tracking framework for autonomous vehicles based on artificial potential field elaborated resistance network approach | |
CN110703762B (zh) | 一种复杂环境下水面无人艇混合路径规划方法 | |
CN103335658B (zh) | 一种基于弧线路径生成的自主车辆避障方法 | |
Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
CN113608531B (zh) | 基于安全a*引导点的无人车实时全局路径规划方法 | |
Yoo et al. | Stackelberg game based model of highway driving | |
CN105045649B (zh) | 一种智能汽车仿真方法及装置 | |
CN107063280A (zh) | 一种基于控制采样的智能车辆路径规划系统及方法 | |
Ma et al. | A two-level path planning method for on-road autonomous driving | |
Hang et al. | Driving conflict resolution of autonomous vehicles at unsignalized intersections: A differential game approach | |
CN105151044B (zh) | 车辆辅助驾驶方法及装置 | |
Chen et al. | Tracking with UAV using tangent-plus-Lyapunov vector field guidance | |
CN112327888B (zh) | 路径规划方法、装置、电子设备和存储介质 | |
Zhang et al. | Structured road-oriented motion planning and tracking framework for active collision avoidance of autonomous vehicles | |
CN109115220A (zh) | 一种用于停车场系统路径规划的方法 | |
Fu et al. | Path planning and decision making for autonomous vehicle in urban environment | |
CN113985875A (zh) | 基于碰撞预测模型的人工势场无人车动态路径规划方法 | |
Wang et al. | Steering behaviors for autonomous vehicles in virtual environments | |
CN113391633A (zh) | 一种面向城市环境的移动机器人融合路径规划方法 | |
Ziegler et al. | Anytime tree-based trajectory planning for urban driving | |
CN114187781B (zh) | 一种分布式多车协同行为决策方法和系统 | |
Velenis et al. | Optimal velocity profile generation for given acceleration limits: Receding horizon implementation | |
Ma et al. | Overtaking Path Planning for CAV based on Improved Artificial Potential Field | |
WO2021229825A1 (ja) | 操船計算装置 |
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 | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20200707 |
|
CF01 | Termination of patent right due to non-payment of annual fee |