CN113460091B - 一种无保护十字路口无人车滚动优化决策方法 - Google Patents
一种无保护十字路口无人车滚动优化决策方法 Download PDFInfo
- Publication number
- CN113460091B CN113460091B CN202110965872.1A CN202110965872A CN113460091B CN 113460091 B CN113460091 B CN 113460091B CN 202110965872 A CN202110965872 A CN 202110965872A CN 113460091 B CN113460091 B CN 113460091B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- coordinate system
- track
- intersection
- distance
- 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
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
- B60W60/0015—Planning or execution of driving tasks specially adapted for safety
- B60W60/0016—Planning or execution of driving tasks specially adapted for safety of the vehicle or its occupants
-
- 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
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
-
- 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
- B60W60/0027—Planning or execution of driving tasks using trajectory prediction for other traffic participants
- B60W60/00272—Planning or execution of driving tasks using trajectory prediction for other traffic participants relying on extrapolation of current movement
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- 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
- B60W2520/00—Input parameters relating to overall vehicle dynamics
- B60W2520/10—Longitudinal speed
-
- 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
- B60W2552/00—Input parameters relating to infrastructure
- B60W2552/53—Road markings, e.g. lane marker or crosswalk
-
- 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
- B60W2554/00—Input parameters relating to objects
- B60W2554/40—Dynamic objects, e.g. animals, windblown objects
- B60W2554/404—Characteristics
- B60W2554/4041—Position
-
- 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
- B60W2554/00—Input parameters relating to objects
- B60W2554/40—Dynamic objects, e.g. animals, windblown objects
- B60W2554/404—Characteristics
- B60W2554/4042—Longitudinal speed
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Mathematical Physics (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Human Computer Interaction (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Software Systems (AREA)
- Databases & Information Systems (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- Computing Systems (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种无保护十字路口无人车滚动优化决策方法,获取本车和障碍物的位置及速度信息以及路口的车道线信息,生成本车的参考轨迹及预测周车轨迹;然后根据本车的参考轨迹对车辆信息进行Frenet坐标转换,得到Frenet坐标系下的本车和周车轨迹;接着建立Frenet坐标系下的无人车十字路口决策模型,来描述本车在无保护十字路口的运动;最后设计模型预测控制的无保护十字路口决策控制器,通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策;本方法在无保护十字路口无人车行为决策过程中将本车与周围物体的相对位置关系在纵向和横向上分离开,更能描述本车与周车的碰撞危险。
Description
技术领域
本发明属于汽车驾驶控制领域,涉及一种无保护十字路口无人车滚动优化决策方法。
背景技术
无人驾驶是当前的一个研究热点,其主要由环境感知、行为决策、运动控制三大部分组成,其中,行为决策是无人驾驶的核心。无人驾驶车辆的行为决策***是决定其安全性、稳定性的关键,是其智能程度的体现。当前无人驾驶车辆采用的决策方法主要分为两大类:基于学习的方法和基于规则的方法。基于学习的决策方法需要大量的数据和长时间的训练,并且其可解释性较差;而基于规则的方法很难覆盖无保护十字路口等复杂的交通场景。且现有的决策模型大部分将决策过程建模为马尔可夫过程,由于其复杂度相对较高,且需要状态之间的转移概率,不利于决策算法的设计。
发明内容
本发明针对无人车在无保护十字路口的决策问题,提出一种无保护十字路口无人车滚动优化决策方法;
本发明所述的一种无保护十字路口无人车滚动优化决策方法,是采用如下技术方案实现的:
一种无保护十字路口无人车滚动优化决策方法,其特征在于,步骤过程如下:
步骤一、建立路口大地坐标系,获取路口大地坐标系下的本车的位置及速度信息、周车的位置及速度信息以及路口的车道线信息,生成本车的参考轨迹及预测周车轨迹;
步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立Frenet坐标系,对本车位置和预测周车轨迹进行Frenet坐标转换,得到Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹;
步骤三、根据本车对本车的参考轨迹的跟踪特性,建立Frenet坐标系下的无人车十字路口决策模型,来描述本车在无保护十字路口的运动;
步骤四、根据步骤二中得到的Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹以及步骤三中获得的无人车十字路口决策模型,设计模型预测控制的无保护十字路口决策控制器,通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策。
进一步的技术方案包括:
步骤一的具体过程如下:
以十字路口中心为坐标原点,以本车在路口直行驶入的方向为横轴,以本车在路口左转驶入的方向为纵轴建立路口大地坐标系;本方法中,定义本车为具有无保护十字路口无人车滚动优化决策功能的一辆无人车,周车为在本车周围所有的能够使用传感器探测到的除本车外的其他车辆;
根据路口大地坐标系,十字路口的四个朝向分别为横轴正方向、横轴负方向、纵轴正方向、纵轴负方向,相邻朝向的路互为垂直关系;定义本车进入路口的朝向为驶入方向、本车从驶入方向直行的朝向为直行方向、本车从驶入方向左转的朝向为左转方向、本车从驶入方向右转的朝向为右转方向;
针对直行状态,只考虑本车直行驶入前方车道的情况,即本车只沿x轴方向行驶,针对左转状态和右转状态,本方法中定义期望转向方向为本车期望在路口执行的转向动作的方向,当左转状态时期望转向方向即为左转方向,当右转状态时期望转向方向即为右转方向,将左转状态和右转状态分为从起始时刻到质心越过驶入方向停止线进入路口的直行段、从质心越过驶入方向停止线进入路口的到质心进入期望转向方向车道线离开路口的转弯段和从质心进入期望转向方向车道线离开路口到结束时刻的直行三段,选取一段椭圆弧作为转弯段的期望轨迹,椭圆弧表达式如式(1)所示:
其中椭圆弧的范围为从质心越过驶入方向停止线进入路口,到质心进入期望转向方向的车道线离开路口的部分,(α,β)为椭圆中心,α=-dr×nn,β=dr×wn,dr为车道宽,单位m,nn为从左转方向可驶入路口的车道数,wn为从驶入方向可驶出路口的车道数,p为椭圆长轴,单位m,p=dr×(nn+z+0.5),q为椭圆短轴,单位m,q=dr×(wn+z+0.5),z=1,2,…,Z,Z为期望转向方向的可驶出路口的车道数;根据无人车在期望转向方向的可驶出路口的车道数Z并根据式(1)生成Z条参考轨迹;
通过摄像头、激光雷达、惯性导航***、高精度地图之一或者组合感知周车在路口大地坐标系下的位置和车速,将周车车速进行沿路口大地坐标系的横轴和纵轴分解,假设预测时域内周车在路口大地坐标系下的速度保持不变,可以预测周车轨迹,如式(2)所示:
其中,(X,Y)为周车当前时刻的横纵坐标,(Xj,Yj)为预测的周车在第j个预测周期后的横纵坐标,j=1,2…P,P为预测步长,Tj为采样时间,单位s,vobs,x是周车车速沿路口大地坐标系的横轴分量,单位m/s,vobs,y是周车车速沿路口大地坐标系的纵轴分量,单位m/s;
根据式(2)得到P×Tj时间内预测周车轨迹。
步骤二的具体过程如下:
采用Frenet坐标转换,以本车的参考轨迹为参考系,描述本车和周车在本车的参考轨迹下的位置关系;
在进行Frenet坐标转换时,将本车位置向本车的参考轨迹投影得到的本车投影点作为Frenet坐标系的原点,以本车的参考轨迹为Frenet坐标系s轴,沿着本车行驶方向为s轴的正方向,依据右手法则旋转九十度定义为Frenet坐标系d轴正方向;
坐标转换分为纵向距离S计算和横向距离D计算,计算公式如式(3)所示:
其中S为纵向距离,D为横向距离,x为路口大地坐标系下轨迹点序列的横坐标向量,y为路口大地坐标系下轨迹点序列的纵坐标向量,x和y由本车的参考轨迹进行等距采样获得,即每隔采样距离Δ取一个参考轨迹点,依沿轨迹行驶的先后顺序组成轨迹点序列,将轨迹点序列的横坐标组成x、纵坐标组成y,Δ为根据实际情况人为设定的采样距离,单位m,Δ越小时轨迹点数量越多、Frenet坐标转换结果越准确、但需要耗费的计算资源越多;xi为x的第i个点,yi为y的第i个点,i0为Frenet坐标系的原点在路口大地坐标系下轨迹点序列中的编号,i1为物体向参考轨迹s轴投影点在路口大地坐标系下轨迹点序列中的编号,X0为待转换的在路口大地坐标系下的横坐标,Y0为待转换的在路口大地坐标系下的纵坐标;
将本车当前时刻的横纵坐标(Xh,Yh)代入式(3)中的(X0,Y0),得到Frenet坐标系下相对于本车的参考轨迹的本车位移(s,d),s为本车纵向位移,单位m,d为本车侧向位移,单位m;将预测的周车在第j个预测周期后的横纵坐标(Xj,Yj)逐时刻代入式(3)中的(X0,Y0),得到相对于本车的参考轨迹的预测周车轨迹(S(i,j),D(i,j)),S(i,j)为第i个周车在j时刻的纵向距离,D(i,j)为第i个周车在j时刻的横向距离。
步骤三的具体过程如下:
因为在车身坐标系中,车辆的侧向速度vy对车速的影响很小,可忽略不计,如式(4)所示:
v=vx (4)
因此,纵向的车辆运动学方程如式(5)所示:
其中,vx是本车的纵向速度,单位m/s,v是本车的速度,单位m/s,s为本车的纵向位移,单位m;
本车在大地坐标系下的直道换道表达式如式(6)所示:
其中,T为侧向换道时间常数,单位s,定义为本车跟踪本车的参考轨迹的速度快慢,当T越大时跟踪过程越长、从一条本车的参考轨迹换道至另一条本车的参考轨迹的调节过程越长,yd为本车在大地坐标系下的侧向位移,单位m,yt为本车在大地坐标系下期望的侧向位移,单位m;
做本车的参考轨迹的决策时,以本车的参考轨迹进行Frenet坐标转换,因为本车期望位移总是与Frenet坐标系s轴重合,因此本车期望的侧向位移yt始终为0,推导出Frenet坐标系下换道模型如式(7)所示:
其中,fd(dc)为本车在参考轨迹dc下的横向距离,单位m,dc为期望轨迹编号,由式(3)、式(6)可知fd(dc)=d;
建立Frenet坐标系下的车辆运动学模型,如式(8)所示:
步骤四的具体过程如下:
将式(9)离散化,得到式(10):
设定预测时域为P,控制时域为N,且N≤P,则k时刻该***的输出预测方程和状态预测方程分别为:
根据实际上横向距离和纵向距离对车辆行驶安全的影响,将周车与本车之间距离表示为如式(12)所示:
其中C1={(S,D)|S∈[S0(j)-ws,S0(j)+ws],D∈[D0(j)-wd,D0(j)+wd]}为危险区域,D∈[D0(j)-wd,D0(j)+wd]}为潜在碰撞区域,C3={(S,D)|S∈R,为安全区域,ws为横向安全距离,含义为能够保证本车不与周车发生碰撞的最小的横向距离常值,wd为纵向安全距离,含义为能够保证本车不与周车发生碰撞的最小的纵向距离常值,Dlong表示本车与周车距离较远,短时间内不会发生碰撞,Dshort表示本车与周车距离较近,容易发生碰撞,S0(j)为本车在j时刻的纵向距离,D0(j)为本车在j时刻的横向距离,S(i,j)为第i个周车在j时刻的纵向距离,D(i,j)为第i个周车在j时刻的横向距离,Ddist(i,j)表示本车与第i辆周车在预测时域第j时刻的距离指标,每一时刻找出一辆与本车距离指标最小的周车用于目标函数的计算中,即Dmin(j)=min(Ddist(i,j));
为了保证车距尽量远,建立目标函数J1如式(13)所示:
为了使车辆保持设定车速行驶,建立目标函数J2如式(14)所示:
其中,Δvx=vref(k)-vx(k),vref(k)为设定车速,单位m/s,vx(k)为期望车速,单位m/s;
为了减少不必要的换道动作,建立目标函数J3如式(15)所示:
其中,sgn(Δdc)为对Δdc取符号的符号函数,Δdc=dc(k)-dc(k-1),dc为期望车道编号;
结合上述三个目标函数得到优化问题:
s.t.vxmin≤vx≤vxmax
1≤dc≤Z,dc∈N
其中,J为目标函数,Γ1,Γ2,Γ3为加权矩阵,Γ1=γ1EP×P,Γ2=γ2EN×N,Γ3=γ3EN×N,γ1,γ2,γ3为权重系数,vxmax为车辆的纵向速度的上界,vxmin为车辆的纵向速度的下界,单位均为m/s;
滚动求解优化问题,就能够得到期望车速和期望轨迹序号,即可实现基于模型预测控制的无保护十字路口无人车行为决策。
与现有技术相比本发明的有益效果为:
本发明提供了一种无保护十字路口无人车滚动优化决策方法,设计了模型预测控制的无保护十字路口决策控制器,所以可以实现无保护十字路口无人车行为决策;建立了Frenet坐标系下的无人车十字路口决策模型,所以可以描述Frenet坐标系下本车相对于期望轨迹的运动关系;设计了对车辆信息进行Frenet坐标转换方法,所以可以在无保护十字路口无人车行为决策过程中将本车与周围物体的相对位置关系在纵向和横向上分离开,更易于描述本车与周车的碰撞危险。
附图说明
图1为本发明提供的一种无保护十字路口无人车滚动优化决策方法的流程图。
图2为本方法中车辆在十字路口左转行驶时本车的参考轨迹示意图。
图3为本方法中车辆在十字路口右转行驶时本车的参考轨迹示意图。
图4为本方法中车辆在十字路口直行行驶时本车的参考轨迹示意图。
图5为本发明所述对车辆信息进行Frenet坐标转换示意图。
具体实施方式
一种无保护十字路口无人车滚动优化决策方法,其特征在于,步骤过程如下:
步骤一、建立路口大地坐标系,获取路口大地坐标系下的本车的位置及速度信息、周车的位置及速度信息以及路口的车道线信息,生成本车的参考轨迹及预测周车轨迹;具体过程如下:
以十字路口中心为坐标原点,以本车在路口直行驶入的方向为横轴,以本车在路口左转驶入的方向为纵轴建立路口大地坐标系;本方法中,定义本车为具有无保护十字路口无人车滚动优化决策功能的一辆无人车,周车为在本车周围所有的能够使用传感器探测到的除本车外的其他车辆;
根据路口大地坐标系,十字路口的四个朝向分别为横轴正方向、横轴负方向、纵轴正方向、纵轴负方向,相邻朝向的路互为垂直关系;定义本车进入路口的朝向为驶入方向、本车从驶入方向直行的朝向为直行方向、本车从驶入方向左转的朝向为左转方向、本车从驶入方向右转的朝向为右转方向;
针对直行状态,只考虑本车直行驶入前方车道的情况,即本车只沿x轴方向行驶,针对左转状态和右转状态,本方法中定义期望转向方向为本车期望在路口执行的转向动作的方向,当左转状态时期望转向方向即为左转方向,当右转状态时期望转向方向即为右转方向,将左转状态和右转状态分为从起始时刻到质心越过驶入方向停止线进入路口的直行段、从质心越过驶入方向停止线进入路口的到质心进入期望转向方向车道线离开路口的转弯段和从质心进入期望转向方向车道线离开路口到结束时刻的直行三段,选取一段椭圆弧作为转弯段的期望轨迹,椭圆弧表达式如式(1)所示:
其中椭圆弧的范围为从质心越过驶入方向停止线进入路口,到质心进入期望转向方向的车道线离开路口的部分,(α,β)为椭圆中心,α=-dr×nn,β=dr×wn,dr为车道宽,单位m,nn为从左转方向可驶入路口的车道数,wn为从驶入方向可驶出路口的车道数,p为椭圆长轴,单位m,p=dr×(nn+z+0.5),q为椭圆短轴,单位m,q=dr×(wn+z+0.5),z=1,2,…,Z,Z为期望转向方向的可驶出路口的车道数;根据无人车在期望转向方向的可驶出路口的车道数Z并根据式(1)生成Z条参考轨迹;
通过摄像头、激光雷达、惯性导航***、高精度地图之一或者组合感知周车在路口大地坐标系下的位置和车速,将周车车速进行沿路口大地坐标系的横轴和纵轴分解,假设预测时域内周车在路口大地坐标系下的速度保持不变,可以预测周车轨迹,如式(2)所示:
其中,(X,Y)为周车当前时刻的横纵坐标,(Xj,Yj)为预测的周车在第j个预测周期后的横纵坐标,j=1,2…P,P为预测步长,Tj为采样时间,单位s,vobs,x是周车车速沿路口大地坐标系的横轴分量,单位m/s,vobs,y是周车车速沿路口大地坐标系的纵轴分量,单位m/s;
根据式(2)得到P×Tj时间内预测周车轨迹。
步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立Frenet坐标系,对本车位置和预测周车轨迹进行Frenet坐标转换,得到Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹;具体过程如下:
由于路口大地坐标系下的相对位置不能简便的描述本车和本车的参考轨迹的位置关系,以及本车和周车的位置关系,所以采用Frenet坐标转换,以本车的参考轨迹为参考系,描述本车和周车在本车的参考轨迹下的位置关系;
在进行Frenet坐标转换时,将本车位置向本车的参考轨迹投影得到的本车投影点作为Frenet坐标系的原点,以本车的参考轨迹为Frenet坐标系s轴,沿着本车行驶方向为s轴的正方向,依据右手法则旋转九十度定义为Frenet坐标系d轴正方向;
坐标转换分为纵向距离S计算和横向距离D计算,计算公式如式(3)所示:
其中S为纵向距离,D为横向距离,x为路口大地坐标系下轨迹点序列的横坐标向量,y为路口大地坐标系下轨迹点序列的纵坐标向量,x和y由本车的参考轨迹进行等距采样获得,即每隔采样距离Δ取一个参考轨迹点,依沿轨迹行驶的先后顺序组成轨迹点序列,将轨迹点序列的横坐标组成x、纵坐标组成y,Δ为根据实际情况人为设定的采样距离,单位m,Δ越小时轨迹点数量越多、Frenet坐标转换结果越准确、但需要耗费的计算资源越多;xi为x的第i个点,yi为y的第i个点,i0为Frenet坐标系的原点在路口大地坐标系下轨迹点序列中的编号,i1为物体向参考轨迹s轴投影点在路口大地坐标系下轨迹点序列中的编号,X0为待转换的在路口大地坐标系下的横坐标,Y0为待转换的在路口大地坐标系下的纵坐标;
将本车当前时刻的横纵坐标(Xh,Yh)代入式(3)中的(X0,Y0),得到Frenet坐标系下相对于本车的参考轨迹的本车位移(s,d),s为本车纵向位移,单位m,d为本车侧向位移,单位m;将预测的周车在第j个预测周期后的横纵坐标(Xj,Yj)逐时刻代入式(3)中的(X0,Y0),得到相对于本车的参考轨迹的预测周车轨迹(S(i,j),D(i,j)),S(i,j)为第i个周车在j时刻的纵向距离,D(i,j)为第i个周车在j时刻的横向距离。
步骤三、根据本车对本车的参考轨迹的跟踪特性,建立Frenet坐标系下的无人车十字路口决策模型,来描述本车在无保护十字路口的运动;具体过程如下:
因为在车身坐标系中,车辆的侧向速度vy对车速的影响很小,可忽略不计,如式(4)所示:
v=vx (4)
因此,纵向的车辆运动学方程如式(5)所示:
其中,vx是本车的纵向速度,单位m/s,v是本车的速度,单位m/s,s为本车的纵向位移,单位m;
由于换道过程中的位移曲线y(t)相对复杂,可以采用一阶惯性环节拟合位移曲线,故本车在大地坐标系下的直道换道表达式如式(6)所示:
其中,T为侧向换道时间常数,单位s,定义为本车跟踪本车的参考轨迹的速度快慢,当T越大时跟踪过程越长、从一条本车的参考轨迹换道至另一条本车的参考轨迹的调节过程越长,yd为本车在大地坐标系下的侧向位移,单位m,yt为本车在大地坐标系下期望的侧向位移,单位m;
做本车的参考轨迹的决策时,以本车的参考轨迹进行Frenet坐标转换,因为本车期望位移总是与Frenet坐标系s轴重合,因此本车期望的侧向位移yt始终为0,推导出Frenet坐标系下换道模型如式(7)所示:
其中,fd(dc)为本车在参考轨迹dc下的横向距离,单位m,dc为期望轨迹编号,由式(3)、式(6)可知fd(dc)=d;
建立Frenet坐标系下的车辆运动学模型,如式(8)所示:
步骤四、根据步骤二中得到的Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹以及步骤三中获得的无人车十字路口决策模型,设计模型预测控制的无保护十字路口决策控制器,通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策;具体过程如下:
将式(9)离散化,得到式(10):
设定预测时域为P,控制时域为N,且N≤P,则k时刻该***的输出预测方程和状态预测方程分别为:
由于是在Frenet坐标系下建立的车辆运动学模型,通过坐标转换之后能够得到本车和周车之间的横、纵向距离,根据实际上横向距离和纵向距离对车辆行驶安全的影响,将周车与本车之间距离表示为如式(12)所示:
其中C1={(S,D)|S∈[S0(j)-ws,S0(j)+ws],D∈[D0(j)-wd,D0(j)+wd]}为危险区域,D∈[D0(j)-wd,D0(j)+wd]}为潜在碰撞区域,C3={(S,D)|S∈R,为安全区域,ws为横向安全距离,含义为能够保证本车不与周车发生碰撞的最小的横向距离常值,wd为纵向安全距离,含义为能够保证本车不与周车发生碰撞的最小的纵向距离常值,Dlong表示本车与周车距离较远,短时间内不会发生碰撞,Dshort表示本车与周车距离较近,容易发生碰撞,S0(j)为本车在j时刻的纵向距离,D0(j)为本车在j时刻的横向距离,S(i,j)为第i个周车在j时刻的纵向距离,D(i,j)为第i个周车在j时刻的横向距离,Ddist(i,j)表示本车与第i辆周车在预测时域第j时刻的距离指标,每一时刻找出一辆与本车距离指标最小的周车用于目标函数的计算中,即Dmin(j)=min(Ddist(i,j));
为了保证车距尽量远,建立目标函数J1如式(13)所示:
因为车辆在道路上行驶总是希望其保持设定车速行驶,建立目标函数J2如式(14)所示:
其中,Δvx=vref(k)-vx(k),vref(k)为设定车速,单位m/s,vx(k)为期望车速,单位m/s;
在保证本车能够顺利换道的同时,还要保证换道动作尽量小,建立目标函数J3如式(15)所示:
其中,sgn(Δdc)为对Δdc取符号的符号函数,Δdc=dc(k)-dc(k-1),dc为期望车道编号;
结合上述三个目标函数得到优化问题:
s.t.vxmin≤vx≤vxmax
1≤dc≤Z,dc∈N
其中,J为目标函数,Γ1,Γ2,Γ3为加权矩阵,Γ1=γ1EP×P,Γ2=γ2EN×N,Γ3=γ3EN×N,γ1,γ2,γ3为权重系数,vxmax为车辆的纵向速度的上界,vxmin为车辆的纵向速度的下界,单位均为m/s;
滚动求解优化问题,就能够得到期望车速和期望轨迹序号,即可实现基于模型预测控制的无保护十字路口无人车行为决策。
Claims (2)
1.一种无保护十字路口无人车滚动优化决策方法,其特征在于,步骤过程如下:
步骤一、建立路口大地坐标系,获取路口大地坐标系下的本车的位置及速度信息、周车的位置及速度信息以及路口的车道线信息,生成本车的参考轨迹及预测周车轨迹;
步骤二、根据步骤一中获得的本车的参考轨迹和本车位置建立Frenet坐标系,对本车位置和预测周车轨迹进行Frenet坐标转换,得到Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹;
步骤三、根据本车对本车的参考轨迹的跟踪特性,建立Frenet坐标系下的无人车十字路口决策模型,来描述本车在无保护十字路口的运动;
步骤四、根据步骤二中得到的Frenet坐标系下相对于本车的参考轨迹的本车位移和相对于本车的参考轨迹的预测周车轨迹以及步骤三中获得的无人车十字路口决策模型,设计模型预测控制的无保护十字路口决策控制器,通过求解优化问题并将纵向速度和期望轨迹作用到底层控制器即可实现基于模型预测控制的无保护十字路口无人车行为决策;
步骤一的具体过程如下:
以十字路口中心为坐标原点,以本车在路口直行驶入的方向为横轴,以本车在路口左转驶入的方向为纵轴建立路口大地坐标系;本方法中,定义本车为具有无保护十字路口无人车滚动优化决策功能的一辆无人车,周车为在本车周围所有的能够使用传感器探测到的除本车外的其他车辆;
根据路口大地坐标系,十字路口的四个朝向分别为横轴正方向、横轴负方向、纵轴正方向、纵轴负方向,相邻朝向的路互为垂直关系;定义本车进入路口的朝向为驶入方向、本车从驶入方向直行的朝向为直行方向、本车从驶入方向左转的朝向为左转方向、本车从驶入方向右转的朝向为右转方向;
针对直行状态,只考虑本车直行驶入前方车道的情况,即本车只沿x轴方向行驶,针对左转状态和右转状态,本方法中定义期望转向方向为本车期望在路口执行的转向动作的方向,当左转状态时期望转向方向即为左转方向,当右转状态时期望转向方向即为右转方向,将左转状态和右转状态分为从起始时刻到质心越过驶入方向停止线进入路口的直行段、从质心越过驶入方向停止线进入路口的到质心进入期望转向方向车道线离开路口的转弯段和从质心进入期望转向方向车道线离开路口到结束时刻的直行三段,选取一段椭圆弧作为转弯段的期望轨迹,椭圆弧表达式如式(1)所示:
其中椭圆弧的范围为从质心越过驶入方向停止线进入路口,到质心进入期望转向方向的车道线离开路口的部分,(α,β)为椭圆中心,α=-dr×nn,β=dr×wn,dr为车道宽,单位m,nn为从左转方向可驶入路口的车道数,wn为从驶入方向可驶出路口的车道数,p为椭圆长轴,单位m,p=dr×(nn+z+0.5),q为椭圆短轴,单位m,q=dr×(wn+z+0.5),z=1,2,…,Z,Z为期望转向方向的可驶出路口的车道数;根据无人车在期望转向方向的可驶出路口的车道数Z并根据式(1)生成Z条参考轨迹;
通过摄像头、激光雷达、惯性导航***、高精度地图之一或者组合感知周车在路口大地坐标系下的位置和车速,将周车车速进行沿路口大地坐标系的横轴和纵轴分解,假设预测时域内周车在路口大地坐标系下的速度保持不变,可以预测周车轨迹,如式(2)所示:
其中,(X,Y)为周车当前时刻的横纵坐标,(Xj,Yj)为预测的周车在第j个预测周期后的横纵坐标,j=1,2…P,P为预测步长,Tj为采样时间,单位s,vobs,x是周车车速沿路口大地坐标系的横轴分量,单位m/s,vobs,y是周车车速沿路口大地坐标系的纵轴分量,单位m/s;
根据式(2)得到P×Tj时间内预测周车轨迹;
步骤二的具体过程如下:
采用Frenet坐标转换,以本车的参考轨迹为参考系,描述本车和周车在本车的参考轨迹下的位置关系;
在进行Frenet坐标转换时,将本车位置向本车的参考轨迹投影得到的本车投影点作为Frenet坐标系的原点,以本车的参考轨迹为Frenet坐标系s轴,沿着本车行驶方向为s轴的正方向,依据右手法则旋转九十度定义为Frenet坐标系d轴正方向;
坐标转换分为纵向距离S计算和横向距离D计算,计算公式如式(3)所示:
其中S为纵向距离,D为横向距离,x为路口大地坐标系下轨迹点序列的横坐标向量,y为路口大地坐标系下轨迹点序列的纵坐标向量,x和y由本车的参考轨迹进行等距采样获得,即每隔采样距离Δ取一个参考轨迹点,依沿轨迹行驶的先后顺序组成轨迹点序列,将轨迹点序列的横坐标组成x、纵坐标组成y,Δ为根据实际情况人为设定的采样距离,单位m,Δ越小时轨迹点数量越多、Frenet坐标转换结果越准确、但需要耗费的计算资源越多;xi为x的第i个点,yi为y的第i个点,i0为Frenet坐标系的原点在路口大地坐标系下轨迹点序列中的编号,i1为物体向参考轨迹s轴投影点在路口大地坐标系下轨迹点序列中的编号,X0为待转换的在路口大地坐标系下的横坐标,Y0为待转换的在路口大地坐标系下的纵坐标;
将本车当前时刻的横纵坐标(Xh,Yh)代入式(3)中的(X0,Y0),得到Frenet坐标系下相对于本车的参考轨迹的本车位移(s,d),s为本车纵向位移,单位m,d为本车侧向位移,单位m;将预测的周车在第j个预测周期后的横纵坐标(Xj,Yj)逐时刻代入式(3)中的(X0,Y0),得到相对于本车的参考轨迹的预测周车轨迹(S(i,j),D(i,j)),S(i,j)为第i个周车在j时刻的纵向距离,D(i,j)为第i个周车在j时刻的横向距离;
步骤三的具体过程如下:
因为在车身坐标系中,车辆的侧向速度vy对车速的影响很小,可忽略不计,如式(4)所示:
v=vx (4)
因此,纵向的车辆运动学方程如式(5)所示:
其中,vx是本车的纵向速度,单位m/s,v是本车的速度,单位m/s,s为本车的纵向位移,单位m;
本车在大地坐标系下的直道换道表达式如式(6)所示:
其中,T为侧向换道时间常数,单位s,定义为本车跟踪本车的参考轨迹的速度快慢,当T越大时跟踪过程越长、从一条本车的参考轨迹换道至另一条本车的参考轨迹的调节过程越长,yd为本车在大地坐标系下的侧向位移,单位m,yt为本车在大地坐标系下期望的侧向位移,单位m;
做本车的参考轨迹的决策时,以本车的参考轨迹进行Frenet坐标转换,因为本车期望位移总是与Frenet坐标系s轴重合,因此本车期望的侧向位移yt始终为0,推导出Frenet坐标系下换道模型如式(7)所示:
其中,fd(dc)为本车在参考轨迹dc下的横向距离,单位m,dc为期望轨迹编号,由式(3)、式(6)可知fd(dc)=d;
建立Frenet坐标系下的车辆运动学模型,如式(8)所示:
2.根据权利要求1所述的一种无保护十字路口无人车滚动优化决策方法,其特征在于,步骤四的具体过程如下:
将式(9)离散化,得到式(10):
设定预测时域为P,控制时域为N,且N≤P,则k时刻该***的输出预测方程和状态预测方程分别为:
根据实际上横向距离和纵向距离对车辆行驶安全的影响,将周车与本车之间距离表示为如式(12)所示:
其中C1={(S,D)|S∈[S0(j)-ws,S0(j)+ws],D∈[D0(j)-wd,D0(j)+wd]}为危险区域,D∈[D0(j)-wd,D0(j)+wd]}为潜在碰撞区域,为安全区域,ws为横向安全距离,含义为能够保证本车不与周车发生碰撞的最小的横向距离常值,wd为纵向安全距离,含义为能够保证本车不与周车发生碰撞的最小的纵向距离常值,Dlong表示本车与周车距离较远,短时间内不会发生碰撞,Dshort表示本车与周车距离较近,容易发生碰撞,S0(j)为本车在j时刻的纵向距离,D0(j)为本车在j时刻的横向距离,S(i,j)为第i个周车在j时刻的纵向距离,D(i,j)为第i个周车在j时刻的横向距离,Ddist(i,j)表示本车与第i辆周车在预测时域第j时刻的距离指标,每一时刻找出一辆与本车距离指标最小的周车用于目标函数的计算中,即Dmin(j)=min(Ddist(i,j));
为了保证车距尽量远,建立目标函数J1如式(13)所示:
为了使车辆保持设定车速行驶,建立目标函数J2如式(14)所示:
其中,Δvx=vref(k)-vx(k),vref(k)为设定车速,单位m/s,vx(k)为期望车速,单位m/s;
为了减少不必要的换道动作,建立目标函数J3如式(15)所示:
其中,sgn(Δdc)为对Δdc取符号的符号函数,Δdc=dc(k)-dc(k-1),dc为期望车道编号;
结合上述三个目标函数得到优化问题:
s.t.vxmin≤vx≤vxmax
1≤dc≤Z,dc∈N
其中,J为目标函数,Γ1,Γ2,Γ3为加权矩阵,Γ1=γ1EP×P,Γ2=γ2EN×N,Γ3=γ3EN×N,γ1,γ2,γ3为权重系数,vxmax为车辆的纵向速度的上界,vxmin为车辆的纵向速度的下界,单位均为m/s;
滚动求解优化问题,就能够得到期望车速和期望轨迹序号,即可实现基于模型预测控制的无保护十字路口无人车行为决策。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110965872.1A CN113460091B (zh) | 2021-08-23 | 2021-08-23 | 一种无保护十字路口无人车滚动优化决策方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110965872.1A CN113460091B (zh) | 2021-08-23 | 2021-08-23 | 一种无保护十字路口无人车滚动优化决策方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113460091A CN113460091A (zh) | 2021-10-01 |
CN113460091B true CN113460091B (zh) | 2022-10-21 |
Family
ID=77867941
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110965872.1A Active CN113460091B (zh) | 2021-08-23 | 2021-08-23 | 一种无保护十字路口无人车滚动优化决策方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113460091B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117576928B (zh) * | 2024-01-15 | 2024-05-03 | 中铁一局集团电务工程有限公司 | 一种无轨电车的交通信号控制方法及*** |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111201170A (zh) * | 2017-10-10 | 2020-05-26 | 本田技研工业株式会社 | 车辆控制装置、车辆控制方法及程序 |
WO2020173489A1 (en) * | 2019-02-28 | 2020-09-03 | Huawei Technologies Co., Ltd. | Method and system for controlling safety of ego and social objects |
CN112068545A (zh) * | 2020-07-23 | 2020-12-11 | 哈尔滨工业大学(深圳) | 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、***及存储介质 |
CN112572472A (zh) * | 2020-12-08 | 2021-03-30 | 重庆大学 | 一种基于Frenet坐标系的自动驾驶碰撞预测方法 |
WO2021142793A1 (zh) * | 2020-01-17 | 2021-07-22 | 华为技术有限公司 | 路径规划方法和路径规划装置 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102018222783A1 (de) * | 2018-12-21 | 2020-06-25 | Robert Bosch Gmbh | Verfahren zum Steuern eines automatisierten oder autonomen Fortbewegungsmittels und Auswerteeinheit |
CN109878515B (zh) * | 2019-03-12 | 2021-03-16 | 百度在线网络技术(北京)有限公司 | 预测车辆轨迹的方法、装置、存储介质和终端设备 |
CN110187639B (zh) * | 2019-06-27 | 2021-05-11 | 吉林大学 | 一种基于参数决策框架的轨迹规划控制方法 |
US20210197819A1 (en) * | 2019-12-31 | 2021-07-01 | Zoox, Inc. | Vehicle control to join route |
-
2021
- 2021-08-23 CN CN202110965872.1A patent/CN113460091B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111201170A (zh) * | 2017-10-10 | 2020-05-26 | 本田技研工业株式会社 | 车辆控制装置、车辆控制方法及程序 |
WO2020173489A1 (en) * | 2019-02-28 | 2020-09-03 | Huawei Technologies Co., Ltd. | Method and system for controlling safety of ego and social objects |
WO2021142793A1 (zh) * | 2020-01-17 | 2021-07-22 | 华为技术有限公司 | 路径规划方法和路径规划装置 |
CN112068545A (zh) * | 2020-07-23 | 2020-12-11 | 哈尔滨工业大学(深圳) | 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、***及存储介质 |
CN112572472A (zh) * | 2020-12-08 | 2021-03-30 | 重庆大学 | 一种基于Frenet坐标系的自动驾驶碰撞预测方法 |
Non-Patent Citations (4)
Title |
---|
A Dynamic Motion Planning Framework for Autonomous Driving in Urban Environments;Yuncheng Jiang等;《2020 39th Chinese Control Conference》;20200909;全文 * |
基于Frenet坐标系的智能车运动规划研究;王沙晶等;《移动电源与车辆》;20190320(第01期);全文 * |
陈虹 ; 申忱 ; 郭洪艳 ; 刘俊.面向动态避障的智能汽车滚动时域路径规划.《中国公路学报》.2019, * |
魏民祥 ; 滕德成 ; 吴树凡.基于Frenet坐标系的自动驾驶轨迹规划与优化算法.《控制与决策》.2019, * |
Also Published As
Publication number | Publication date |
---|---|
CN113460091A (zh) | 2021-10-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110333714B (zh) | 一种无人驾驶汽车路径规划方法和装置 | |
US20210394757A1 (en) | Vehicle trajectory modification for following | |
KR20210074366A (ko) | 자율주행 차량 계획 및 예측 | |
JP3714258B2 (ja) | 車両用推奨操作量生成装置 | |
Andersen et al. | Trajectory optimization for autonomous overtaking with visibility maximization | |
CN111338346A (zh) | 一种自动驾驶控制方法、装置、车辆及存储介质 | |
Yu et al. | A path planning and navigation control system design for driverless electric bus | |
JP2018192954A (ja) | 運転支援装置 | |
Bautista-Montesano et al. | Autonomous navigation at unsignalized intersections: A coupled reinforcement learning and model predictive control approach | |
CN112068574A (zh) | 一种无人车在动态复杂环境中的控制方法及*** | |
Xu et al. | System and experiments of model-driven motion planning and control for autonomous vehicles | |
CN113255998B (zh) | 基于多智能体强化学习的高速道路无人驾驶车辆编队方法 | |
JP2021011173A (ja) | 予測装置、予測方法、プログラムおよび車両制御システム | |
Tian et al. | Multi-vehicle tracking using an environment interaction potential force model | |
CN114442630B (zh) | 一种基于强化学习和模型预测的智能车规划控制方法 | |
Guo et al. | Toward human-like behavior generation in urban environment based on Markov decision process with hybrid potential maps | |
CN113460091B (zh) | 一种无保护十字路口无人车滚动优化决策方法 | |
López et al. | Efficient local navigation approach for autonomous driving vehicles | |
Sheng et al. | A cooperation-aware lane change method for autonomous vehicles | |
Reda et al. | Path planning algorithms in the autonomous driving system: A comprehensive review | |
Gong et al. | Game theory-based decision-making and iterative predictive lateral control for cooperative obstacle avoidance of guided vehicle platoon | |
Sheng et al. | A cooperation-aware lane change method for automated vehicles | |
Wang et al. | Research on path planning for autonomous vehicle based on Frenet system | |
Yuan et al. | Vehicle path planning based on pedestrian collision avoidance action | |
CN115432008A (zh) | 一种基于优化多任务的自动驾驶决策规划方法、电子设备及计算机存储介质 |
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 |