CN113386795A - 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 - Google Patents
一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 Download PDFInfo
- Publication number
- CN113386795A CN113386795A CN202110756066.3A CN202110756066A CN113386795A CN 113386795 A CN113386795 A CN 113386795A CN 202110756066 A CN202110756066 A CN 202110756066A CN 113386795 A CN113386795 A CN 113386795A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- track
- speed
- target
- longitudinal
- 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
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Human Computer Interaction (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Traffic Control Systems (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明提供了一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统,方法包括下列步骤:步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点;步骤二、通过Frenet坐标系对轨迹计算进行简化,利用多项式插值出车辆未来的遍历轨迹对应的路径,通过汽车的初始状态与目标状态求解轨迹集合;步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序,根据排序结果依次进行是否满足约束条件的检查,直至确定同时满足各约束条件的最优轨迹;步骤四、获得最优轨迹后下发行车命令用于车辆运动控制。本发明将复杂的非线性轨迹规划问题简化,降低了轨迹规划的难度,规划出光滑、平顺的轨迹,令车辆行驶更加平稳。
Description
技术领域
本发明涉及一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统。
背景技术
随机科技的不断发展,目前国内外很多知名公司都投入了大量的高精尖人员和技术来发展高水平的智能驾驶技术。决策系统与轨迹规划算法作为车辆自动驾驶最核心的模块之一,越来越受到研究者的关注。通常车辆的决策系统主要是根据传感器感知车辆周围的环境信息,如动态障碍物、静态障碍物、车辆位置、车辆速度、车道边界信息等。系统根据这些信息先规划出一条安全、合理、可行的行车轨迹,并将轨迹信息发送给执行模块,从而控制车辆的加减速、航向角等,实现自动驾驶功能。目前市面上出现的自动驾驶车辆、大多能够感知静态障碍物,而事实上道路上的信息千变万化,对更为复杂场景下的动态障碍物难以应对。
目前轨迹规划方法分为全局轨迹规划与局部轨迹规划。通常全局路径规划已经事先给定于汽车,但汽车在行驶过程中会受到各种各样的外界干扰。因此,除了全局路径规划外,还需要局部轨迹规划(二次规划),以应对复杂突变的环境。目前,局部轨迹规划方法有:A*算法、RRT(Rapidly-exploring Random Tree,快速搜索随机树)算法等,这类图搜索方法通过随机采样的方式实现对环境的逐步随机探索,能够快速得到局部轨迹的同时还能避免陷入局部最优。然而由于其探索过程具备随机性,因此导致这种方法的规划结果难以预测,在实际工业应用中存在一定的不可控性。概率地图方法、人工势场等,需要不断搜索自由空间(无障碍空间),需要对每次规划都执行一次,因此会造成极大的时间和资源的浪费。采样差值法的主要思想是,通过密集采样得到一系列的汽车状态,针对每一种状态通过多项式差值、样条曲线拟合差值等手段规划得到轨迹集合,并从中选取最优轨迹。但是不合适的差值方法容易导致汽车的运动学及动力学参数不连续,乘坐体验感欠佳。随着计算机技术不但的发展,人工智能方法也逐渐应用于自动驾驶,不过人工智能的计算对汽车硬件要求较高,同时人工智能技术对样本的需求较大,当样本数量不够时,最终输出的结果将对行驶轨迹有较大的影响因此,智能驾驶系统如何规划出一条能能够应对复杂环境的可靠轨迹尤为重要。
自动驾驶的另一个难点在于汽车位置、姿态与道路的相对关系的建立。通常描述二者之间关系的方法有:全局笛卡尔坐标系、里程坐标系及Frenet坐标系。全局坐标系规划全局路径相对容易,但对于局部道路感知变化难以应对;里程坐标系由于受到里程长度的影响,道路拟合的难度会逐渐变得复杂。
发明内容
本发明的目的在于提供一种自动驾驶车辆智能决策及局部轨迹规划方法,以解决现有技术中对复杂环境下进行轨迹规划存在局部道路复杂难以实现,不可控性高并且计算量大,耗时长的技术问题。
所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,包括下列步骤,
步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点;
步骤二、通过Frenet坐标系对轨迹计算进行简化,利用多项式插值出车辆未来的遍历轨迹对应的路径,通过汽车的初始状态与目标状态求解轨迹集合;
步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序,根据排序结果依次进行是否满足约束条件的检查,不能同时满足所有约束条件时则按排序取下一轨迹重复检查,直至确定同时满足各约束条件的最优轨迹;
步骤四、获得最优轨迹后对其进行Frenet坐标系到笛卡尔坐标的转换,然后将转换结果下发行车命令用于车辆运动控制。
优选的,所述步骤二具体包括:给定车辆当前位置及给定的目标位置点利用6个约束,使用5次多项式进行拟合,具体如下:
若已知t0时刻的状态[f(t0),f'(t0),f”(t0)]T及t1时刻的状态[f(t1),f'(t1),f”(t1)]T,将已知条件代入上式并用矩阵的形式表达线性方程组如下:
MA=F
其中:
A=[a0 a1 a2 a3 a4 a5]T
F=[f(t0),f′(t0),f″(t0),f(t1),f′(t1),f″(t1)]T
确定车辆的初始状态d(t0)=[d0,d′0,d″0]T及车辆的目标状态d(t1)=[d1,0,0]T,令t0=0;确定车辆纵向的初始状态s(t0)=[s0,s′0,s″0]T及车辆的目标状态s(t1)=[s1,s′1,s″1]T,令t0=0,通过上式便可求解线性方程;
由上述多项式插值计算分别得到对应的横向轨迹集合{di(t)|i=0,1,2...}和纵向轨迹集合{si(t)|i=0,1,2...},二者结合考虑时间离散{ΔTt0,t0+ΔT,t0+2ΔT,...,t1}得到横、纵合成轨迹集合{di,j,si,j|i,j=0,1,2...}。
优选的,所述步骤二中,目标状态根据场景变化包括下列方法确定目标状态s(t1):
巡航:对于横向目标状态,使用本车的横向速度和更新步长N,计算目标状态的横向偏移,其中横向速度和加速度设置为零,对于纵向,终端速度设置为道路限制速度,终端加速度设置为零,纵向位置不受限制,指定为Nan,最终生成在给定时间范围内与道路速度平滑匹配的轨迹:其中,表示目标纵向速度;dexplane表示横向位移,其基本更新方法为:dexplane=v0+0.5×d2t(i),i=1,2,...,N;其中,v0表示初始速度;
变道:首先确定本车辆的当前车道,然后检查相邻车道是否存在,对于每个现有车道,目标状态的定义方式与巡航控制行为相同,将终端速度设置为当前速度:其中,表示车辆目标速度;ddesLane表示车辆横向目标位置;
跟车:首先确定本车当前车道,根据设定的前车时间范围,预测前车的未来状态,并查找与本车占用同一车道的所有车辆,同时确定哪个是最接近本车的车辆,如果前车不存在,则该功能不会返回任何内容,通过前车的位置和速度,将终端纵向位置减少一定的安全距离,计算出自车辆的目标状态:其中,slead-lsafety表示前车与本车的纵向间隔距离;表示前车纵向速度,作为本车的纵向目标速度;dlead表示前车的横向位移,作为本车的横向目标位移;表示前车的横向速度,作为本车的横向目标速度。
优选的,所述步骤三中的约束条件包括,
位置条件:需要保证车辆始终在车道之内,如果目标位置超出道路边界,车辆将会冲出道路发生危险,位置条件可以表示为:d(s)<drode,其中:drode表示道路边界位置;
速度条件:车辆在道路上行驶时,速度vcar不能超过车辆本身的速度阈值vmax,同时在路上也会有道路限速,因此速度条件可以表示为:vcar<vmax;
加速度条件:车辆的行驶的加速度acar不能超过加速度的上限amax,即:acar<amax;
曲率条件:轨迹曲率kx反应了车辆的转弯效果,若曲率kx过大,车辆速度过快,容易导致车辆侧翻,曲率可以表示为:kx<kmax,kmax为曲率阈值;
碰撞条件:假设以障碍物为中心的安全距离是rblock,车身外接圆的半径是rcar,则车辆与障碍物的碰撞限制可以表示为:其中(xx,yx)为车辆自身的坐标,(xblock,yblock)为障碍物的坐标。
优选的,所述步骤三中的约束条件包括,
乘坐舒适度:转化为对车辆加加速度的评估,即在初始状态时刻t0到目标状态时刻t1的加加速度积分,横向加加速度的评估Jd和纵向加加速度的评估Js以及总的乘坐舒适度可以表示为:
Jcomfort=KdJd+KsJs
其中,Kd、Ks分别表示横向加加速度的评估Jd和纵向加加速度的评估Js的加权系数;
车辆安全性:车辆安全性评估用车辆碰撞时间tTTC来描述,当碰撞时间大于碰撞阈值时间,则车辆是安全的,并且碰撞时间越长车辆越安全,车辆安全评估值表示为:
其中:ε是一个正数值,为了避免tTTC≈0时产生奇异值,vx为车辆自身的速度,vblock障碍物的速度,由此得到车辆碰撞时间tTTC的算式如下:
本发明还提供了一种自动驾驶车辆智能决策系统,采用如前所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,所述自动驾驶车辆智能决策系统包括,
环境感知模块,用于感知车辆及车辆周围环境信息;
智能决策与规划模块,用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动下发行车命令,还承担处理并传递信息的任务;
执行系统模块,用于根据下发的行车命令完成车辆的运行控制。
优选的,所述智能决策与规划模块包括智能决策模块和轨迹规划模块;
所述智能决策模块与环境感知模块、所述轨迹规划模块通过CAN总线进行通讯,所述执行系统模块包括控制各个执行系统的车辆控制模块,所述车辆控制模块与智能决策模块通过CAN总线进行通讯;
轨迹规划模块,进行局部轨迹规划,通过汽车的初始状态与目标状态,考虑汽车的动力学与运动学约束及环境影响,求解满足约束条件的轨迹集合,并在集合中根据成本代价条件选择最合适的轨迹,确定最优轨迹后据此下达相应的行车命令。
本发明的技术效果:本方案通过建立Frenet坐标系与笛卡尔坐标系的相对关系,可以将复杂的非线性轨迹规划问题简化,并通过基于该坐标系进行的多项式插值规划方法进一步降低了轨迹规划的难度,更深层次地降低了对硬件计算能力的要求。
通过多项式插值规划的方法,可以规划出光滑、平顺的轨迹,宏观上看,汽车横纵向的行驶更加平稳。通过对速度、加速度、加加速度建立代价函数,可以筛选出当前状态下的最优轨迹,并能根据需要将道路边界、速度条件、加速度条件,以及乘坐舒适度,车辆安全性等多个约束条件考虑到最优轨迹的选择过程中,满足使用中的不同需要,确保车辆安全的同时改善乘坐舒适度。
附图说明
图1为本发明一种自动驾驶车辆智能决策及局部轨迹规划方法的流程图。
图2为本发明一种自动驾驶车辆智能决策系统的系统示意图。
具体实施方式
下面对照附图,通过对实施例的描述,对本发明具体实施方式作进一步详细的说明,以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
如图1-2所示,本发明提供了一种自动驾驶车辆智能决策及局部轨迹规划方法,包括下列步骤:
步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点;
步骤二、通过Frenet坐标系对轨迹计算进行简化,利用多项式插值出车辆未来的遍历轨迹对应的路径,通过汽车的初始状态与目标状态求解轨迹集合;
具体为:给定车辆当前位置及给定的目标位置点利用6个约束,使用5次多项式进行拟合,具体如下:
若已知t0时刻的状态[f(t0),f′(t0),f″(t0)]T及t1时刻的状态[f(t1),f′(t1),f″(t1)]T,将已知条件代入上式并用矩阵的形式表达线性方程组如下:
MA=F
其中:
A=[a0 a1 a2 a3 a4 a5]T
F=[f(t0),f′(t0),f″(t0),f(t1),f′(t1),f″(t1)]T
该方法预先建立建立Frenet坐标系与笛卡尔坐标的转化关系,本步骤中采用Frenet坐标系分别实现横向轨迹规划和纵向轨迹规划,并综合二者建立横、纵合成轨迹集合。其中横向轨迹规划主要的作用是控制车辆及时变道或避障,但无论如何控制车辆,对于车辆的目标位置我们始终希望:车辆与车道保持平行的状态。纵向轨迹规划主要面向的场景有定速巡航、变道塞车、跟车等。利用Frenet坐标系简化后横向初始配置为横向终端配置为纵向初始配置为纵向终端配置为因此,可以确定车辆的初始状态d(t0)=[d0,d′0,d″0]T及车辆的目标状态d(t1)=[d1,0,0]T,同时为了计算方便,令t0=0。
与横向轨迹规划类似,当确定了车辆纵向的初始状态s(t0)=[s0,s′0,s″0]T及车辆的目标状态s(t1)=[s1,s′1,s″1]T,同时为了计算方便,令t0=0,通过上式便可求解线性方程。根据不同的场景,其目标状态s(t1)也不同,具体分析如下。
巡航:对于横向目标状态,使用本车的横向速度和更新步长N,计算目标状态的横向偏移,其中横向速度和加速度设置为零,对于纵向,终端速度设置为道路限制速度,终端加速度设置为零,纵向位置不受限制,指定为Nan,最终生成在给定时间范围内与道路速度平滑匹配的轨迹:其中,表示目标纵向速度;dexplane表示横向位移,其基本更新方法为:dexplane=v0+0.5×d2t(i),i=1,2,...,N;其中,v0表示初始速度。
变道:首先确定本车辆的当前车道,然后检查相邻车道是否存在,对于每个现有车道,目标状态的定义方式与巡航控制行为相同,将终端速度设置为当前速度:其中,表示车辆目标速度;ddesLane表示车辆横向目标位置。
跟车:首先确定本车当前车道,根据设定的前车时间范围,预测前车的未来状态,并查找与本车占用同一车道的所有车辆,同时确定哪个是最接近本车的车辆,如果前车不存在,则该功能不会返回任何内容,通过前车的位置和速度,将终端纵向位置减少一定的安全距离,计算出自车辆的目标状态:其中,slead-lsafety表示前车与本车的纵向间隔距离;表示前车纵向速度,作为本车的纵向目标速度;dlead表示前车的横向位移,作为本车的横向目标位移;表示前车的横向速度,作为本车的横向目标速度。
由上述方式计算分别得到对应的横向轨迹集合{di(t)|i=0,1,2...}和纵向轨迹集合{si(t)|i=0,1,2...}。二者结合考虑时间离散{ΔT|t0,t0+ΔT,t0+2ΔT,...,t1}得到横、纵合成轨迹集合{di,j,si,j|i,j=0.1.2...}。
步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序,根据排序结果依次进行是否满足约束条件的检查,不能同时满足所有约束条件时则按排序取下一轨迹重复检查,直至确定同时满足各约束条件的最优轨迹。
成本代价C的评估公式为:{cost(i)=sort(Ci)|i=0,1,2,...,N}。
一般情况下,约束条件的检查包括进行边界约束、车辆自身运动参数校核和碰撞检测,具体过程为:选择排序结果的第一条轨迹进行车辆运动参数校核,根据校核结果确定是否为最优轨迹,不满足校核条件则是该轨迹为次优轨迹,否则对该轨迹进行碰撞检测,满足碰撞检测则将该轨迹作为最优轨迹,否则将该轨迹作为次优轨迹,当车辆运动参数校核或碰撞检测得到轨迹为次优轨迹时依次从排序后的轨迹集合中选择下一轨迹重复进行车辆运动参数校核。车辆运动参数校核包括运动学校核、加速度校核、曲率校核和速度校核。碰撞检测主要为检测与障碍物之间的安全距离是否满足要求,边界约束为确定车辆位置是否在道路边界内。
上述校核和检测均为了保证最优轨迹满足相应约束条件,这些约束条件包括:位置条件、速度条件、加速度条件、碰撞条件,具体如下。
位置条件:需要保证车辆始终在车道之内,如果目标位置超出道路边界,车辆将会冲出道路发生危险。位置条件可以表示为:d(s)<drode,其中:drode表示道路边界位置。
速度条件:车辆在道路上行驶时,速度vcar不能超过车辆本身的速度阈值vmax,同时在路上也会有道路限速。因此速度条件可以表示为:vcar<vmax。
加速度条件:车辆的行驶的加速度acar不能超过加速度的上限amax,即:acar<amax。
曲率条件:轨迹曲率kx反应了车辆的转弯效果,若曲率kx过大,车辆速度过快,容易导致车辆侧翻。曲率可以表示为:kx<kmax,kmax为曲率阈值。
碰撞条件。车辆在行驶过程中难免会遇到各种各样的路况,如车辆、行人、路障等。对车辆来说,这些道路上的物体可以全部认为障碍物。所谓碰撞限制,就是汽车规划的路径不能与障碍物发生安全距离的干涉。假设以障碍物为中心的安全距离是rblock,车身外接圆的半径是rcar,则车辆与障碍物的碰撞限制可以表示为:
其中(xx,yx)为车辆自身的坐标,(xblock,yblock)为障碍物的坐标。
进一步地,所述轨迹规划单元在规划时还可以考虑车辆的乘坐舒适度、安全性及车辆行驶效率,从而将上述影响因素增加为约束条件。
乘坐舒适度:车辆的加加速度反应了汽车加速度的变化率,宏观上乘客最直观的感受就是乘坐舒适度。若加加速度过大,车辆会急加速或急减速甚至有顿挫感。乘坐舒适度评估可以转化为对车辆加加速度的评估,即在初始状态时刻t0到目标状态时刻t1的加加速度积分。横向加加速度的评估Jd和纵向加加速度的评估Js以及总的乘坐舒适度可以表示为:
Jcomfort=KdJd+KsJs
其中,Kd、Ks分别表示横向加加速度的评估Jd和纵向加加速度的评估Js的加权系数;
车辆安全性:车辆安全性评估可以用车辆碰撞时间tTTC来描述。当碰撞时间大于碰撞阈值时间,则车辆是安全的,并且碰撞时间越长车辆越安全。车辆安全评估值可以表示为:
其中:ε是一个正数值,为了避免tTTC≈0时产生奇异值,vx为车辆自身的速度,vblock障碍物的速度,由此得到车辆碰撞时间tTTC的算式如下:
步骤四、获得最优轨迹后对其进行Frenet坐标系到笛卡尔坐标的转换,然后将转换结果用于车辆运动控制。
当决策模块根据上述方法做出决策并下发行车命令时,控制模块将根据与决策模块下发的车辆目标位置、车辆转角等数据控制电机电流大小、方向盘转角等。当遇到突发情况时决策模块下发急停命令,车辆控制模块将停止执行行车命令,并且对车辆紧急制动。
本发明还提供了一种自动驾驶车辆智能决策系统,具有局部轨迹规划功能,包括:环境感知模块、智能决策与规划模块和执行系统模块。其中:
环境感知模块,该模块用于感知车辆及车辆周围环境信息,包括:车辆速度、车辆位置、道路上静态障碍物位置及动态障碍物位置速度等。
所述环境感知模块包括车辆上的GPS定位传感器、激光雷达定位、超声波雷达测速雷达等装置。
智能决策与规划模块,该模块用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动,包括车辆行驶、停止、急停等;同时该模块还承担处理并传递其他模块信息的任务,在完成行为决策后进一步进行轨迹规划,具体通过其中的轨迹规划模块实现,因此可将该模块进一步包括智能决策模块和轨迹规划模块。
所述智能决策模块相当于系统中枢,环境感知模块与智能决策模块通过CAN总线进行通讯,轨迹规划模块与智能决策模块通过CAN总线进行通讯,车辆控制模块与智能决策模块通过CAN总线进行通讯。
轨迹规划模块,通常全局路径规划已经事先给定于汽车,但汽车在行驶过程中会受到各种各样的外界干扰。因此,除了全局路径规划外,还需要局部轨迹规划(二次规划),以应对复杂突变的环境。该模块将通过汽车的初始状态与目标状态,考虑汽车的动力学与运动学约束及环境影响(如障碍物碰撞等),求解满足约束条件的轨迹集合,并在集合中根据成本代价条件选择最合适的轨迹。确定最优轨迹后据此下达相应的横纵向控制指令即行车命令。
执行系统模块,用于根据下发的行车命令完成车辆的运行控制,由车辆控制模块与受其控制的转向系统、制动系统、动力系统等执行系统一同构成。
车辆控制模块,该模块主要执行由决策模块下发的行车命令。当决策模块下发行车命令时,控制模块将根据与决策模块下发的车辆目标位置、车辆转角等数据控制电机电流大小、方向盘转角等。当遇到突发情况时决策模块下发急停命令,车辆控制模块将停止执行行车命令,并且对车辆紧急制动。进一步地,所述轨迹规划单元在规划时考虑车辆的边界条件及约束。
转向系统、制动系统、动力系统则为现有车辆的常规技术,在此不做赘述。
本决策系统在工作时,环境感知单元先利用各种传感器来获取自动驾驶车辆周围的运动信息,并将这些信息传输到车载计算机内进行实时决,具体决策时,利用“遍历轨迹+最优搜索法”来进行轨迹规划,该轨迹既包含了自动驾驶车辆的路径信息,又包含了速度信息,从而很好的适应实际复杂工况下的决策规划。得到轨迹信息后,车辆的控制单元会利用相应的控制算法将轨迹信息转化为控制器直接输入的控制量信息,并将这些控制量输入到主动转向电机、制动及油门踏板位移电机上,从而实现车辆的自动驾驶。
上面结合附图对本发明进行了示例性描述,显然本发明具体实现并不受上述方式的限制,只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进,或未经改进将本发明构思和技术方案直接应用于其它场合的,均在本发明保护范围之内。
Claims (7)
1.一种自动驾驶车辆智能决策及局部轨迹规划方法,其特征在于:
步骤一、由道路信息采样给定车辆当前位置及给定的目标位置点;
步骤二、通过Frenet坐标系对轨迹计算进行简化,利用多项式插值出车辆未来的遍历轨迹对应的路径,通过汽车的初始状态与目标状态求解轨迹集合;
步骤三、对上述轨迹集合进行成本代价评估并按最小原则排序,根据排序结果依次进行是否满足约束条件的检查,不能同时满足所有约束条件时则按排序取下一轨迹重复检查,直至确定同时满足各约束条件的最优轨迹;
步骤四、获得最优轨迹后对其进行Frenet坐标系到笛卡尔坐标的转换,然后将转换结果下发行车命令用于车辆运动控制。
2.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,其特征在于:所述步骤二具体包括:给定车辆当前位置及给定的目标位置点利用6个约束,使用5次多项式进行拟合,具体如下:
若已知t0时刻的状态[f(t0),f'(t0),f”(t0)]T及t1时刻的状态[f(t1),f'(t1),f”(t1)]T,将已知条件代入上式并用矩阵的形式表达线性方程组如下:
MA=F
其中:
A=[a0 a1 a2 a3 a4 a5]T
F=[f(t0),f'(t0),f”(t0),f(t1),f'(t1),f”(t1)]T
确定车辆的初始状态d(t0)=[d0,d'0,d″0]T及车辆的目标状态d(t1)=[d1,0,0]T,令t0=0;确定车辆纵向的初始状态s(t0)=[s0,s'0,s″0]T及车辆的目标状态s(t1)=[s1,s1',s″1]T,令t0=0,通过上式便可求解线性方程;
由上述多项式插值计算分别得到对应的横向轨迹集合{di(t)|i=0,1,2...}和纵向轨迹集合{si(t)|i=0,1,2...},二者结合考虑时间离散{ΔT|t0,t0+ΔT,t0+2ΔT,...,t1}得到横、纵合成轨迹集合{di,j,si,j|i,j=0,1,2...}。
3.根据权利要求2所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,其特征在于:所述步骤二中,目标状态根据场景变化包括下列方法确定目标状态s(t1):
巡航:对于横向目标状态,使用本车的横向速度和更新步长N,计算目标状态的横向偏移,其中横向速度和加速度设置为零,对于纵向,终端速度设置为道路限制速度,终端加速度设置为零,纵向位置不受限制,指定为Nan,最终生成在给定时间范围内与道路速度平滑匹配的轨迹:其中,表示目标纵向速度;dexplane表示横向位移,其基本更新方法为:dexplane=v0+0.5×d2t(i),i=1,2,...,N;其中,v0表示初始速度;
变道:首先确定本车辆的当前车道,然后检查相邻车道是否存在,对于每个现有车道,目标状态的定义方式与巡航控制行为相同,将终端速度设置为当前速度:其中,表示车辆目标速度;ddesLane表示车辆横向目标位置;
4.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,其特征在于:所述步骤三中的约束条件包括,
位置条件:需要保证车辆始终在车道之内,如果目标位置超出道路边界,车辆将会冲出道路发生危险,位置条件可以表示为:d(s)<drode,其中:drode表示道路边界位置;
速度条件:车辆在道路上行驶时,速度vcar不能超过车辆本身的速度阈值vmax,同时在路上也会有道路限速,因此速度条件可以表示为:vcar<vmax;
加速度条件:车辆的行驶的加速度acar不能超过加速度的上限amax,即:acar<amax;
曲率条件:轨迹曲率kx反应了车辆的转弯效果,若曲率kx过大,车辆速度过快,容易导致车辆侧翻,曲率可以表示为:kx<kmax,kmax为曲率阈值;
5.根据权利要求1所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,其特征在于:所述步骤三中的约束条件包括,
乘坐舒适度:转化为对车辆加加速度的评估,即在初始状态时刻t0到目标状态时刻t1的加加速度积分,横向加加速度的评估Jd和纵向加加速度的评估Js以及总的乘坐舒适度可以表示为:
Jcomfort=KdJd+KsJs
其中,Kd、Ks分别表示横向加加速度的评估Jd和纵向加加速度的评估Js的加权系数;
车辆安全性:车辆安全性评估用车辆碰撞时间tTTC来描述,当碰撞时间大于碰撞阈值时间,则车辆是安全的,并且碰撞时间越长车辆越安全,车辆安全评估值表示为:
其中:ε是一个正数值,为了避免tTTC≈0时产生奇异值,vx为车辆自身的速度,vblock障碍物的速度,由此得到车辆碰撞时间tTTC的算式如下:
6.一种自动驾驶车辆智能决策系统,其特征在于:采用如权利要求1-5中任一所述的一种自动驾驶车辆智能决策及局部轨迹规划方法,所述自动驾驶车辆智能决策系统包括,
环境感知模块,用于感知车辆及车辆周围环境信息;
智能决策与规划模块,用于结合车辆及周围环境、驾驶员命令等决策车辆如何运动下发行车命令,还承担处理并传递信息的任务;
执行系统模块,用于根据下发的行车命令完成车辆的运行控制。
7.根据权利要求6所述的一种自动驾驶车辆智能决策系统,其特征在于:所述智能决策与规划模块包括智能决策模块和轨迹规划模块;
所述智能决策模块与环境感知模块、所述轨迹规划模块通过CAN总线进行通讯,所述执行系统模块包括控制各个执行系统的车辆控制模块,所述车辆控制模块与智能决策模块通过CAN总线进行通讯;
轨迹规划模块,进行局部轨迹规划,通过汽车的初始状态与目标状态,考虑汽车的动力学与运动学约束及环境影响,求解满足约束条件的轨迹集合,并在集合中根据成本代价条件选择最合适的轨迹,确定最优轨迹后据此下达相应的行车命令。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110756066.3A CN113386795B (zh) | 2021-07-05 | 2021-07-05 | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110756066.3A CN113386795B (zh) | 2021-07-05 | 2021-07-05 | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113386795A true CN113386795A (zh) | 2021-09-14 |
CN113386795B CN113386795B (zh) | 2022-07-01 |
Family
ID=77625206
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110756066.3A Active CN113386795B (zh) | 2021-07-05 | 2021-07-05 | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113386795B (zh) |
Cited By (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113870316A (zh) * | 2021-10-19 | 2021-12-31 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
CN113886764A (zh) * | 2021-10-28 | 2022-01-04 | 哈尔滨工业大学 | 一种基于Frenet坐标系的智能车辆多场景轨迹规划方法 |
CN113916247A (zh) * | 2021-10-13 | 2022-01-11 | 苏州科技大学 | 智能网联游览车辆道路选择及路径规划装置和方法 |
CN113928338A (zh) * | 2021-10-08 | 2022-01-14 | 南京航空航天大学 | 一种纵横向耦合的智能车辆轨迹规划方法及系统 |
CN113928341A (zh) * | 2021-11-30 | 2022-01-14 | 广州文远知行科技有限公司 | 一种道路决策方法、系统、设备和介质 |
CN113978465A (zh) * | 2021-09-28 | 2022-01-28 | 阿波罗智能技术(北京)有限公司 | 一种变道轨迹规划方法、装置、设备以及存储介质 |
CN114084155A (zh) * | 2021-11-15 | 2022-02-25 | 清华大学 | 预测型智能汽车决策控制方法、装置、车辆及存储介质 |
CN114115299A (zh) * | 2022-01-25 | 2022-03-01 | 上海仙工智能科技有限公司 | 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 |
CN114234993A (zh) * | 2021-12-15 | 2022-03-25 | 北京福田戴姆勒汽车有限公司 | 车辆局部路径规划方法、自动驾驶系统和自动驾驶车辆 |
CN114407925A (zh) * | 2022-01-20 | 2022-04-29 | 江苏大学 | 一种基于时空鸟瞰图和策略梯度算法的自动驾驶轨迹规划系统及方法 |
CN114407902A (zh) * | 2022-01-19 | 2022-04-29 | 浙江大学 | 一种基于道路水层深度估计的驾驶决策的系统 |
CN114442491A (zh) * | 2022-04-08 | 2022-05-06 | 杭州国辰机器人科技有限公司 | 室内机器人的局部路径规划和跟踪控制方法、装置及介质 |
CN114524020A (zh) * | 2022-02-28 | 2022-05-24 | 重庆长安汽车股份有限公司 | 一种车辆人机共驾的方向盘角度控制方法 |
CN114578808A (zh) * | 2022-01-10 | 2022-06-03 | 美的集团(上海)有限公司 | 路径规划方法、电子设备、计算机程序产品及存储介质 |
CN114590274A (zh) * | 2022-03-25 | 2022-06-07 | 奥特酷智能科技(南京)有限公司 | 用于驾驶辅助系统的路径规划方法及驾驶辅助系统 |
CN114664094A (zh) * | 2022-05-26 | 2022-06-24 | 青岛慧拓智能机器有限公司 | 一种车辆轨迹预测方法、装置、芯片、终端和计算机设备 |
CN115140048A (zh) * | 2022-06-28 | 2022-10-04 | 哈尔滨工业大学 | 一种自动驾驶行为决策与轨迹规划模型与方法 |
CN115520218A (zh) * | 2022-09-27 | 2022-12-27 | 李晓赫 | 自动驾驶车辆四点转弯轨迹规划方法 |
CN115525047A (zh) * | 2022-03-21 | 2022-12-27 | 江苏集萃清联智控科技有限公司 | 一种具备多型避障方式的车辆局部轨迹规划方法及系统 |
CN115817515A (zh) * | 2023-01-18 | 2023-03-21 | 禾多科技(北京)有限公司 | 车辆控制方法、装置、电子设备和计算机可读介质 |
WO2023097874A1 (zh) * | 2021-12-01 | 2023-06-08 | 上海仙途智能科技有限公司 | 用于行驶轨迹规划的方法和装置 |
CN116495013A (zh) * | 2023-06-21 | 2023-07-28 | 北京理工大学 | 面向任务需求的速度规划方法、系统、设备及存储介质 |
CN116653963A (zh) * | 2023-07-31 | 2023-08-29 | 福思(杭州)智能科技有限公司 | 车辆变道控制方法、系统和智能驾驶域控制器 |
CN117433539A (zh) * | 2023-12-20 | 2024-01-23 | 中国汽车技术研究中心有限公司 | 一种汽车场地测试多目标物协同轨迹规划的方法及装置 |
CN117496734A (zh) * | 2023-10-19 | 2024-02-02 | 南昌大学 | 一种基于信息物理社会智能的新能源汽车自主行为的决策方法及其系统 |
CN117622227A (zh) * | 2024-01-24 | 2024-03-01 | 广汽埃安新能源汽车股份有限公司 | 一种车辆换道避障控制方法及装置 |
CN117734646A (zh) * | 2023-05-06 | 2024-03-22 | 中车株洲电力机车研究所有限公司 | 轨道列车及其循环空气制动方法、装置、设备和存储介质 |
CN117433539B (zh) * | 2023-12-20 | 2024-06-11 | 中国汽车技术研究中心有限公司 | 一种汽车场地测试多目标物协同轨迹规划的方法及装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109669461A (zh) * | 2019-01-08 | 2019-04-23 | 南京航空航天大学 | 一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法 |
CN111273668A (zh) * | 2020-02-18 | 2020-06-12 | 福州大学 | 针对结构化道路的无人驾驶汽车运动轨迹规划系统及方法 |
CN111338335A (zh) * | 2019-12-31 | 2020-06-26 | 清华大学 | 一种结构化道路场景下的车辆局部轨迹规划方法 |
CN111750887A (zh) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统 |
CN112965489A (zh) * | 2021-02-05 | 2021-06-15 | 北京理工大学 | 一种基于碰撞检测的智能车高速换道规划方法 |
-
2021
- 2021-07-05 CN CN202110756066.3A patent/CN113386795B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109669461A (zh) * | 2019-01-08 | 2019-04-23 | 南京航空航天大学 | 一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法 |
CN111338335A (zh) * | 2019-12-31 | 2020-06-26 | 清华大学 | 一种结构化道路场景下的车辆局部轨迹规划方法 |
CN111273668A (zh) * | 2020-02-18 | 2020-06-12 | 福州大学 | 针对结构化道路的无人驾驶汽车运动轨迹规划系统及方法 |
CN111750887A (zh) * | 2020-06-11 | 2020-10-09 | 上海交通大学 | 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统 |
CN112965489A (zh) * | 2021-02-05 | 2021-06-15 | 北京理工大学 | 一种基于碰撞检测的智能车高速换道规划方法 |
Cited By (43)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113978465A (zh) * | 2021-09-28 | 2022-01-28 | 阿波罗智能技术(北京)有限公司 | 一种变道轨迹规划方法、装置、设备以及存储介质 |
CN113928338A (zh) * | 2021-10-08 | 2022-01-14 | 南京航空航天大学 | 一种纵横向耦合的智能车辆轨迹规划方法及系统 |
CN113916247A (zh) * | 2021-10-13 | 2022-01-11 | 苏州科技大学 | 智能网联游览车辆道路选择及路径规划装置和方法 |
CN113916247B (zh) * | 2021-10-13 | 2023-10-13 | 苏州科技大学 | 智能网联游览车辆道路选择及路径规划装置和方法 |
CN113870316A (zh) * | 2021-10-19 | 2021-12-31 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
CN113870316B (zh) * | 2021-10-19 | 2023-08-15 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
CN113886764A (zh) * | 2021-10-28 | 2022-01-04 | 哈尔滨工业大学 | 一种基于Frenet坐标系的智能车辆多场景轨迹规划方法 |
CN113886764B (zh) * | 2021-10-28 | 2024-05-03 | 哈尔滨工业大学 | 一种基于Frenet坐标系的智能车辆多场景轨迹规划方法 |
CN114084155A (zh) * | 2021-11-15 | 2022-02-25 | 清华大学 | 预测型智能汽车决策控制方法、装置、车辆及存储介质 |
CN114084155B (zh) * | 2021-11-15 | 2023-10-20 | 清华大学 | 预测型智能汽车决策控制方法、装置、车辆及存储介质 |
CN113928341A (zh) * | 2021-11-30 | 2022-01-14 | 广州文远知行科技有限公司 | 一种道路决策方法、系统、设备和介质 |
WO2023097874A1 (zh) * | 2021-12-01 | 2023-06-08 | 上海仙途智能科技有限公司 | 用于行驶轨迹规划的方法和装置 |
CN114234993A (zh) * | 2021-12-15 | 2022-03-25 | 北京福田戴姆勒汽车有限公司 | 车辆局部路径规划方法、自动驾驶系统和自动驾驶车辆 |
CN114578808A (zh) * | 2022-01-10 | 2022-06-03 | 美的集团(上海)有限公司 | 路径规划方法、电子设备、计算机程序产品及存储介质 |
CN114407902A (zh) * | 2022-01-19 | 2022-04-29 | 浙江大学 | 一种基于道路水层深度估计的驾驶决策的系统 |
CN114407902B (zh) * | 2022-01-19 | 2023-11-28 | 浙江大学 | 一种基于道路水层深度估计的驾驶决策的系统 |
CN114407925A (zh) * | 2022-01-20 | 2022-04-29 | 江苏大学 | 一种基于时空鸟瞰图和策略梯度算法的自动驾驶轨迹规划系统及方法 |
CN114407925B (zh) * | 2022-01-20 | 2024-05-14 | 江苏大学 | 一种基于时空鸟瞰图和策略梯度算法的自动驾驶轨迹规划系统及方法 |
CN114115299A (zh) * | 2022-01-25 | 2022-03-01 | 上海仙工智能科技有限公司 | 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 |
CN114115299B (zh) * | 2022-01-25 | 2022-04-22 | 上海仙工智能科技有限公司 | 一种移动机器人平滑回归给定轨迹的路径规划方法及装置 |
CN114524020A (zh) * | 2022-02-28 | 2022-05-24 | 重庆长安汽车股份有限公司 | 一种车辆人机共驾的方向盘角度控制方法 |
CN114524020B (zh) * | 2022-02-28 | 2023-09-01 | 重庆长安汽车股份有限公司 | 一种车辆人机共驾的方向盘角度控制方法 |
CN115525047A (zh) * | 2022-03-21 | 2022-12-27 | 江苏集萃清联智控科技有限公司 | 一种具备多型避障方式的车辆局部轨迹规划方法及系统 |
CN114590274A (zh) * | 2022-03-25 | 2022-06-07 | 奥特酷智能科技(南京)有限公司 | 用于驾驶辅助系统的路径规划方法及驾驶辅助系统 |
CN114442491A (zh) * | 2022-04-08 | 2022-05-06 | 杭州国辰机器人科技有限公司 | 室内机器人的局部路径规划和跟踪控制方法、装置及介质 |
CN114664094B (zh) * | 2022-05-26 | 2022-08-26 | 青岛慧拓智能机器有限公司 | 一种车辆轨迹预测方法、装置、芯片、终端和计算机设备 |
CN114664094A (zh) * | 2022-05-26 | 2022-06-24 | 青岛慧拓智能机器有限公司 | 一种车辆轨迹预测方法、装置、芯片、终端和计算机设备 |
CN115140048B (zh) * | 2022-06-28 | 2023-07-25 | 哈尔滨工业大学 | 一种自动驾驶行为决策与轨迹规划模型与方法 |
CN115140048A (zh) * | 2022-06-28 | 2022-10-04 | 哈尔滨工业大学 | 一种自动驾驶行为决策与轨迹规划模型与方法 |
CN115520218B (zh) * | 2022-09-27 | 2023-05-23 | 李晓赫 | 自动驾驶车辆四点转弯轨迹规划方法 |
CN115520218A (zh) * | 2022-09-27 | 2022-12-27 | 李晓赫 | 自动驾驶车辆四点转弯轨迹规划方法 |
CN115817515B (zh) * | 2023-01-18 | 2023-05-26 | 禾多科技(北京)有限公司 | 车辆控制方法、装置、电子设备和计算机可读介质 |
CN115817515A (zh) * | 2023-01-18 | 2023-03-21 | 禾多科技(北京)有限公司 | 车辆控制方法、装置、电子设备和计算机可读介质 |
CN117734646A (zh) * | 2023-05-06 | 2024-03-22 | 中车株洲电力机车研究所有限公司 | 轨道列车及其循环空气制动方法、装置、设备和存储介质 |
CN116495013B (zh) * | 2023-06-21 | 2023-12-01 | 北京理工大学 | 面向任务需求的速度规划方法、系统、设备及存储介质 |
CN116495013A (zh) * | 2023-06-21 | 2023-07-28 | 北京理工大学 | 面向任务需求的速度规划方法、系统、设备及存储介质 |
CN116653963B (zh) * | 2023-07-31 | 2023-10-20 | 福思(杭州)智能科技有限公司 | 车辆变道控制方法、系统和智能驾驶域控制器 |
CN116653963A (zh) * | 2023-07-31 | 2023-08-29 | 福思(杭州)智能科技有限公司 | 车辆变道控制方法、系统和智能驾驶域控制器 |
CN117496734A (zh) * | 2023-10-19 | 2024-02-02 | 南昌大学 | 一种基于信息物理社会智能的新能源汽车自主行为的决策方法及其系统 |
CN117433539A (zh) * | 2023-12-20 | 2024-01-23 | 中国汽车技术研究中心有限公司 | 一种汽车场地测试多目标物协同轨迹规划的方法及装置 |
CN117433539B (zh) * | 2023-12-20 | 2024-06-11 | 中国汽车技术研究中心有限公司 | 一种汽车场地测试多目标物协同轨迹规划的方法及装置 |
CN117622227A (zh) * | 2024-01-24 | 2024-03-01 | 广汽埃安新能源汽车股份有限公司 | 一种车辆换道避障控制方法及装置 |
CN117622227B (zh) * | 2024-01-24 | 2024-04-16 | 广汽埃安新能源汽车股份有限公司 | 一种车辆换道避障控制方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN113386795B (zh) | 2022-07-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113386795B (zh) | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 | |
CN109669461B (zh) | 一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法 | |
CN111361564B (zh) | 一种考虑效益最大化的车道变更系统及综合决策方法 | |
Khodayari et al. | A historical review on lateral and longitudinal control of autonomous vehicle motions | |
CN109727469B (zh) | 一种多车道下自动驾驶车辆综合危险度评估方法 | |
CN114222691A (zh) | 基于成本的路径确定 | |
Shladover | Review of the state of development of advanced vehicle control systems (AVCS) | |
CN112230642B (zh) | 道路可行驶区域推理方法及装置 | |
CN110155046A (zh) | 自动紧急制动分级控制方法与系统 | |
CN114270360A (zh) | 让步行为建模和预测 | |
US20200238980A1 (en) | Vehicle control device | |
EP3715204A1 (en) | Vehicle control device | |
US20200353918A1 (en) | Vehicle control device | |
EP3911548B1 (en) | Vehicle control | |
CN111002993B (zh) | 一种基于场景识别的自动驾驶低油耗运动规划方法及系统 | |
US20230021615A1 (en) | Vehicle control device, and vehicle control system | |
WO2019084009A1 (en) | LATERAL CLEARANCE REQUIRED FOR SPEED FOR AUTHORIZED VEHICLE TRACK PLANNING | |
CN113848914B (zh) | 动态环境下碰撞系数人工势场法局部路径规划方法 | |
US11719549B2 (en) | Vehicle control apparatus | |
US20200180614A1 (en) | Vehicle control device | |
Morales et al. | Proactive driving modeling in blind intersections based on expert driver data | |
CN115520218B (zh) | 自动驾驶车辆四点转弯轨迹规划方法 | |
EP4222033A1 (en) | Systems and methods for imminent collision avoidance | |
CN110426215B (zh) | 一种用于车辆平顺性测试的模型建立方法及智能驾驶系统 | |
JP2021160659A (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 |