WO2021073079A1 - 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 - Google Patents
一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 Download PDFInfo
- Publication number
- WO2021073079A1 WO2021073079A1 PCT/CN2020/090205 CN2020090205W WO2021073079A1 WO 2021073079 A1 WO2021073079 A1 WO 2021073079A1 CN 2020090205 W CN2020090205 W CN 2020090205W WO 2021073079 A1 WO2021073079 A1 WO 2021073079A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- vehicle
- speed
- candidate
- path
- sequence
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 21
- 230000008878 coupling Effects 0.000 title claims abstract description 16
- 238000010168 coupling process Methods 0.000 title claims abstract description 16
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 16
- 230000001133 acceleration Effects 0.000 claims abstract description 81
- 239000011159 matrix material Substances 0.000 claims abstract description 10
- 230000006870 function Effects 0.000 claims description 46
- 238000005457 optimization Methods 0.000 claims description 9
- 230000008447 perception Effects 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Definitions
- the invention belongs to the technical field of vehicle automatic driving, and in particular relates to a trajectory planning method in which the path and speed of an automatic driving vehicle are highly coupled.
- Vehicle trajectory planning is a complex decision-making behavior. It is mainly based on the surrounding traffic information obtained by the environment and the motion state of the vehicle to decide a safe, efficient and collision-free trajectory. And the planned trajectory should fully consider the kinematic constraints of the vehicle so that the planned trajectory can be tracked smoothly when the underlying control is performed.
- the trajectory planning of the vehicle mainly includes the planning of the vehicle path controlled by the steering wheel and the planning of the vehicle speed controlled by the accelerator pedal.
- the research on trajectory planning mainly stays in two categories.
- One is the independent planning of vehicle path and speed.
- the automatic lane-changing system of the vehicle mainly keeps the speed unchanged, and only plans the path for lane-changing and overtaking; while the automatic car following system is Keep the path unchanged, and follow the car mainly by planning the speed.
- the other type is based on the planning of the path, roughly specifying the speed sequence at each time point, which makes the planned trajectory more rigid and not smooth and flexible. Therefore, a path planning method that can couple speed and path height is particularly important for improving the safety and comfort of autonomous vehicles.
- the purpose of the present invention is to provide a trajectory planning method with a highly coupled path and speed of an autonomous vehicle, so as to solve the problem of the coupling between the velocity and the path of the autonomous vehicle during trajectory planning in the prior art.
- the problem; the method of the present invention enables the vehicle to plan a safe, efficient and stable trajectory in real time under working conditions such as changing lanes and overtaking, decelerating and avoiding evasive.
- the trajectory planning method of the highly coupled path and speed of an automatic driving vehicle of the present invention includes the following steps:
- the RMSProp optimizer is used to obtain the optimal trajectory of the vehicle at the current moment in real time, and the control variable corresponding to the trajectory is used as the input, that is, the coupling planning of the path and the speed is realized.
- the motion state information of the self-vehicle obtained through GPS in the step 1) is: among them, Is the longitudinal position of the vehicle, Is the lateral position of the vehicle, Is the yaw angle of the vehicle, Is the speed of the vehicle, Is the yaw rate of the vehicle, Is the acceleration of the vehicle, Is the angular acceleration of the vehicle;
- the relative motion information of surrounding vehicles is obtained by millimeter wave radar and lidar Among them, ⁇ s t , ⁇ l t , They are the relative longitudinal position, relative lateral position, and relative yaw angle of surrounding vehicles relative to their own vehicle. Is the speed of the surrounding vehicles, Is the yaw angle of the surrounding vehicles, Is the acceleration of the surrounding vehicles, Is the angular acceleration of surrounding vehicles.
- the candidate path model in the step 2) uses a fourth-degree polynomial to establish a function of the lateral position l and the longitudinal position s of the vehicle, and obtains a path sequence that takes the normal acceleration sequence as input and the yaw angle as output, which specifically includes The following steps:
- the corresponding candidate path is fitted by a fourth-degree polynomial, that is, the equation of the lateral position l relative to the longitudinal position s.
- a fourth-degree polynomial that is, the equation of the lateral position l relative to the longitudinal position s.
- the candidate normal acceleration sequence is discretized according to the curvature of the path function, the normal acceleration sequence corresponding to the i-th candidate path for:
- the candidate speed model in step 2) uses a fourth-order polynomial to establish a function of the vehicle longitudinal distance s and time t, and obtains a speed sequence with a tangential acceleration sequence as input and speed as an output, which specifically includes the following steps:
- the candidate speed sequence can be represented by the longitudinal position sequence s t+Np at the candidate end point as follows:
- the corresponding candidate velocity function is fitted using a fourth-degree polynomial, that is, the function of the longitudinal position s with respect to time t, as follows:
- the point motion model that takes vehicle tangential acceleration and normal acceleration as input, and outputs speed, yaw angle, and coordinates, is specifically divided into:
- f is the function of longitudinal distance changing with time
- g is the function of lateral distance changing with time.
- (s t+1 ,l t+1 ) is the position coordinate of the next moment output by the motion equation of the point
- (s t ,l t ) is the vertical and horizontal position coordinate of the current moment t
- T is the planning cycle of the vehicle
- V t is the speed of the vehicle
- Is the tangential acceleration of the vehicle Is the normal acceleration of the vehicle.
- step 2 the input of the two acceleration sequences in step 2) is coupled in a matrix manner, and input to the point motion model to obtain the candidate trajectory sequence coupled with the velocity and the path, which specifically includes the following steps:
- C p is the number of candidate paths
- C s is the number of candidate speeds
- a ij (t) is the input sequence of the candidate trajectory obtained by coupling the input sequence of the i-th candidate path and the j-th candidate speed input sequence
- P ij (t) [p ij (t+1
- the established optimization function needs to consider safety, efficiency, and comfort, and adjust the weights of these three characteristics to meet personalized driving; the details are as follows:
- the established RMS optimizer first defines the learning rate adjustment factor r:
- r 0 is the learning rate adjustment factor at the initial time
- r t is the adjustment factor at time t
- ⁇ is the attenuation coefficient
- g t is the gradient corresponding to the optimization function
- ⁇ t is the current learning rate at time t; ⁇ is a small positive number, ensuring that the denominator is not 0; To solve the optimal longitudinal position of the candidate trajectory; Is the optimal lateral position of the solved candidate trajectory.
- the trajectory planned by the present invention is highly coupled between the path and the speed, so that the planned trajectory is more smooth and continuous, and can meet most driving conditions.
- the present invention comprehensively considers the vehicle's requirements for safety, efficiency, and comfort, and can meet different driving characteristics.
- Figure 1 shows the principle diagram of the method of the present invention.
- the trajectory planning method of the present invention in which the path of an autonomous vehicle is highly coupled with the speed includes the following steps:
- the motion state information of the self-vehicle obtained through GPS is: among them, Is the longitudinal position of the vehicle, Is the lateral position of the vehicle, Is the yaw angle of the vehicle, Is the speed of the vehicle, Is the yaw rate of the vehicle, Is the acceleration of the vehicle, Is the angular acceleration of the vehicle;
- the relative motion information of surrounding vehicles is obtained by millimeter wave radar and lidar Among them, ⁇ s t , ⁇ l t , They are the relative longitudinal position, relative lateral position, and relative yaw angle of surrounding vehicles relative to their own vehicle. Is the speed of the surrounding vehicles, Is the yaw angle of the surrounding vehicles, Is the acceleration of the surrounding vehicles, Is the angular acceleration of surrounding vehicles.
- the candidate path model uses a fourth-degree polynomial to establish the function of the lateral position l and the longitudinal position s of the vehicle and obtains the path sequence with the normal acceleration sequence as the input and the yaw angle as the output, which specifically includes the following steps:
- the corresponding candidate path is fitted by a fourth-degree polynomial, that is, the equation of the lateral position l relative to the longitudinal position s.
- a fourth-degree polynomial that is, the equation of the lateral position l relative to the longitudinal position s.
- the candidate normal acceleration sequence is discretized according to the curvature of the path function, the normal acceleration sequence corresponding to the i-th candidate path for:
- the candidate speed model uses a fourth-degree polynomial to establish the function of the vehicle longitudinal distance s and time t, and obtains the speed sequence with the tangential acceleration sequence as the input and the speed as the output, which specifically includes the following steps:
- the candidate speed sequence can be represented by the longitudinal position sequence s t+Np at the candidate end point as follows:
- the corresponding candidate velocity function is fitted using a fourth-degree polynomial, that is, the function of the longitudinal position s with respect to time t, as follows:
- the point motion model that takes the vehicle tangential acceleration and normal acceleration as input, and speed, yaw angle, and coordinates as output is specifically divided into:
- f is the function of longitudinal distance changing with time
- g is the function of lateral distance changing with time.
- (s t+1 ,l t+1 ) is the position coordinate of the next moment output by the motion equation of the point
- (s t ,l t ) is the vertical and horizontal position coordinate of the current moment t
- T is the planning cycle of the vehicle
- V t is the speed of the vehicle
- Is the tangential acceleration of the vehicle Is the normal acceleration of the vehicle.
- the input of the two acceleration sequences in step 2) is coupled in a matrix manner, and the candidate trajectory sequence coupled with the velocity and the path is obtained by inputting the motion model at this point, which specifically includes the following steps:
- C p is the number of candidate paths
- C s is the number of candidate speeds
- a ij (t) is the input sequence of the candidate trajectory obtained by coupling the input sequence of the i-th candidate path and the j-th candidate speed input sequence
- P ij (t) [p ij (t+1
- the established RMS optimizer first defines the learning rate adjustment factor r:
- r 0 is the learning rate adjustment factor at the initial time
- r t is the adjustment factor at time t
- ⁇ is the attenuation coefficient
- g t is the gradient corresponding to the optimization function
- ⁇ t is the current learning rate at time t; ⁇ is a small positive number, ensuring that the denominator is not 0; To solve the optimal longitudinal position of the candidate trajectory; Is the optimal lateral position of the solved candidate trajectory.
Abstract
一种自动驾驶车辆路径与速度耦合的轨迹规划方法,属于自动驾驶领域,包括:1)获取自车及周围车辆的运动状态信息;2)根据自车当前状态信息,分别建立车辆的候选路径模型和候选速度模型,得到路径和速度单独规划的输入加速度序列;3)建立一个以车辆切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动模型,用矩阵的形式将输入序列耦合在一起作为模型输入,从而得到耦合候选轨迹序列;4)利用RMSProp优化器实时获取当前时刻车辆最优的轨迹,将轨迹对应的控制量作为输入,即实现路径与速度的耦合规划。解决了现有技术中自动驾驶车辆在进行轨迹规划时速度与路径间相耦合的问题。
Description
本发明属于车辆自动驾驶技术领域,尤其涉及一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法。
近些年来,交通事故频发、交通拥堵加剧以及疲劳驾驶依然没有得到有效缓解,使得国内外对于车辆自动驾驶的需求变得越来越迫切。目前对于自动驾驶车辆的研究主要包括环境感知,决策规划及控制执行三个部分。随着传感器精度的提高,芯片计算能力的突破以及车辆线控转向、线控制动等新技术的出现,车辆对于周围环境的感知能力以及对底层执行器的控制精度得到了很大的提升,而轨迹规划方法作为其中的关键部分,对于车辆行驶的安全性,高效性及乘坐舒适性都有很大的影响。
车辆轨迹规划是一种复杂的决策行为,主要根据环境感知得到的周围交通信息以及自车的运动状态来决策出一条安全高效无碰撞的轨迹。并且所规划出的轨迹要充分考虑车辆的运动学约束使得在进行底层控制时能顺利跟踪规划的轨迹。具体对于车辆的行驶轨迹规划,主要包括由方向盘控制的车辆路径的规划,和由油门踏板控制的车辆速度的规划。
目前对于轨迹规划的研究主要停留在两类,一类是对于车辆路径和速度的单独规划,如车辆自动换道系统主要保持速度不变,仅仅规划路径进行换道超车;而自动跟车系统则保持路径不变,主要通过规划速度来进行跟车过程。另一类则是在规划好路径的基础上,粗糙的在各时刻点指定速度序列,这使得所规划出的轨迹比较生硬,不够光滑灵活。因此,一个能将速度与路径高度耦合在一起的路径规划方法对于提高自动驾驶车辆的安全性,舒适性显得尤其重要。
发明内容
针对于上述现有技术的不足,本发明的目的在于提供一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法,以解决现有技术中自动驾驶车辆在进行轨迹规划时速度与路径间相耦合的问题;本发明的方法使车辆在换道超车,减速避让等工况下能实时规划出一条安全、高效、平稳的轨迹。
为达到上述目的,本发明采用的技术方案如下:
本发明的一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法,包括步骤如下:
1)获取自车的运动状态信息及周围车辆的相对运动状态信息;
2)根据自车当前运动状态信息,分别建立自车的候选路径模型和候选速度模型;得到以车辆的法向加速度序列为输入,横摆角为输出的路径序列;以及切向加速度序列为输入,速度为输出的速度序列;
3)建立一个以车辆切向加速度序列和法向加速度序列为输入,速度、横摆角、坐标为输 出的点运动模型,并将上述两个序列的输入利用矩阵的方式耦合起来,输入到该点运动模型即可得到速度与路径相耦合的候选轨迹序列;
4)利用RMSProp优化器实时获取当前时刻车辆最优的轨迹,将该轨迹对应的控制量作为输入,即实现路径与速度的耦合规划。
进一步的,所述步骤1)中通过GPS获取的自车的运动状态信息为:
其中,
是自车的纵向位置,
是自车的侧向位置,
是自车的横摆角,
是自车的速度,
是自车的横摆角速度,
是自车的加速度,
是自车的角加速度;通过毫米波雷达和激光雷达获取周围车辆的相对运动信息为
其中,Δs
t,Δl
t,
分别是周围车辆相对于自车的相对纵向位置,相对横向位置,相对横摆角,
是周围车辆的速度,
是周围车辆的横摆角,
是周围车辆的加速度,
是周围车辆的角加速度。
进一步的,所述步骤2)中的候选路径模型利用4次多项式建立车辆侧向位置l与纵向位置s的函数并得到以法向加速度序列为输入,横摆角为输出的路径序列,具体包括如下步骤:
21)根据道路边界约束,得到候选路径终点时刻的侧向位置l
t+Np序列:
l
t+Np=l
min:Δl/C
p:l
max
其中,l
min和l
max为道路的上、下边界;Δl=l
max-l
min;C
p为候选路径的个数;
22)根据自车当前运动状态,以及给定的候选终点时刻的位置,利用4次多项式拟合出相应的候选路径,即侧向位置l相对于纵向位置s的方程,该过程将车辆的速度看成匀速的,具体如下:
l=a
0+a
1s+a
2s
2+a
3s
3+a
4s
4
进一步的,所述步骤2)中的候选速度模型利用4次多项式建立车辆纵向距离s与时间t的函数,并得到以切向加速度序列为输入,速度为输出的速度序列,具体包括如下步骤:
24)根据车辆加速性能约束,候选速度序列可由候选终点时刻的纵向位置序列s
t+Np表示如下:
s
t+Np=s
min:Δs/C
s:s
max
其中,s
min和s
max为车辆所能达到的距离的上、下边界;Δs=s
max-s
min;C
s为候选速度的个数;
25)根据自车当前运动状态,以及给定的候选纵向距离序列,利用4次多项式拟合出相应的候选速度函数,即纵向位置s关于时间t的函数,具体如下:
s(t)=p
0+p
1t+p
2t
2+p
3t
3+p
4t
4
进一步的,所述步骤3)中的以车辆切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动模型具体分为:
31)用抽象函数将车辆位置随时间变化的关系表示如下:
其中,f是纵向距离随时间变化的函数,g是侧向距离随时间变化的函数,这两个函数即可将 车辆轨迹表示出来;
32)将上述函数用泰勒公式展开,保留到二次项,得到如下方程:
其中,各阶导数表示如下:
33)将以上各阶导数代入到轨迹方程中,得到所建立的以切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动方程:
其中,(s
t+1,l
t+1)为该点运动方程输出的下一时刻的位置坐标,(s
t,l
t)为当前t时刻的纵横向位置坐标,T是车辆的规划周期,v
t是车辆的速度,
是车辆的横摆角,
为车辆的切向加速度,
为车辆的法向加速度。
进一步的,将步骤2)中的两个加速度序列的输入利用矩阵的方式耦合起来,输入到该点运动模型得到速度与路径相耦合的候选轨迹序列,具体包括如下步骤:
34)速度与路径两个方向加速度序列用矩阵方式耦合如下:
其中,C
p是候选路径的个数,C
s是候选速度的个数,A
ij(t)是第i条候选路径输入序列与第j条候选速度输入序列耦合得到的候选轨迹的输入序列,具体如下:
35)将上述耦合后的加速度序列输入到建立的点运动模型中,即可得到如下轨迹序列:
P
ij(t)=[p
ij(t+1|t),p
ij(t+2|t),…,p
ij(t+k|t),…,p
ij(t+N
p|t)]
进一步的,所述步骤4)中利用RMSProp优化器进行优化时,建立的优化函数需考虑安全性、高效性、舒适性,并通过调节这三个特性的权重来满足个性化驾驶;具体如下:
41)建立的优化函数J具体如下:
其中,
为第i个候选速度对应终点时刻的纵向位置;
为第j个候选路径对应终点时刻的侧向位置;
为第i个候选速度与第j个候选路径耦合得到的候选轨迹对应的危险度,R
ref为参考危险度,这一项代表安全性,M为权重;
为第i个候选速度与第j个候选路径耦合得到的轨迹对应的速度,v
ref为参考速度,这一项代表高效性,N为权重;分母中的s
ref为参考纵向位置,l
ref为参考横向位置,代表舒适性;
42)建立的RMS优化器在确定学习率时,首先定义学习率调整因子r:
其中,r
0为初始时刻的学习率调整因子;r
t为t时刻的调整因子;ρ为衰减系数;g
t为优化函数对应的梯度;
43)进一步得到学习率,并确定最优目标位置:
本发明的有益效果:
1、本发明规划出来的轨迹是路径与速度高度耦合的,使得规划出的轨迹更为圆滑连续,能满足大多数驾驶工况。
2、本发明在确定轨迹终点时,综合考虑车辆对于安全性、高效性和舒适性的需求,能满足不同的驾驶特性。
图1绘示本发明方法的原理图。
为了便于本领域技术人员的理解,下面结合实施例与附图对本发明作进一步的说明,实施方式提及的内容并非对本发明的限定。
参照图1所示,本发明的一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法,包括步骤如下:
1)获取自车的运动状态信息及周围车辆的相对运动状态信息;
通过GPS获取的自车的运动状态信息为:
其中,
是自车的纵向位置,
是自车的侧向位置,
是自车的横摆角,
是自车的速度,
是自车的横摆角速度,
是自车的加速度,
是自车的角加速度;通过毫米波雷达和激光雷达获取周围车辆的相对运动信息为
其中,Δs
t,Δl
t,
分别是周围车辆相对于自车的相对纵向位置,相对横向位置,相对横摆角,
是周围车辆的速度,
是周围车辆的横摆角,
是周围车辆的加速度,
是周围车辆的角加速度。
2)根据自车当前运动状态信息,分别建立自车的候选路径模型和候选速度模型;得到以车辆的法向加速度序列为输入,横摆角为输出的路径序列;以及切向加速度序列为输入,速度为输出的速度序列;
候选路径模型利用4次多项式建立车辆侧向位置l与纵向位置s的函数并得到以法向加速度序列为输入,横摆角为输出的路径序列,具体包括如下步骤:
21)根据道路边界约束,得到候选路径终点时刻的侧向位置l
t+Np序列:
l
t+Np=l
min:Δl/C
p:l
max
其中,l
min和l
max为道路的上、下边界;Δl=l
max-l
min;C
p为候选路径的个数;
22)根据自车当前运动状态,以及给定的候选终点时刻的位置,利用4次多项式拟合出相应的候选路径,即侧向位置l相对于纵向位置s的方程,该过程将车辆的速度看成匀速的,具体如下:
l=a
0+a
1s+a
2s
2+a
3s
3+a
4s
4
候选速度模型利用4次多项式建立车辆纵向距离s与时间t的函数,并得到以切向加速度序列为输入,速度为输出的速度序列,具体包括如下步骤:
24)根据车辆加速性能约束,候选速度序列可由候选终点时刻的纵向位置序列s
t+Np表示如下:
s
t+Np=s
min:Δs/C
s:s
max
其中,s
min和s
max为车辆所能达到的距离的上、下边界;Δs=s
max-s
min;C
s为候选速度的个数;
25)根据自车当前运动状态,以及给定的候选纵向距离序列,利用4次多项式拟合出相应的候选速度函数,即纵向位置s关于时间t的函数,具体如下:
s(t)=p
0+p
1t+p
2t
2+p
3t
3+p
4t
4
3)建立一个以车辆切向加速度序列和法向加速度序列为输入,速度、横摆角、坐标为输出的点运动模型,并将上述两个序列的输入利用矩阵的方式耦合起来,输入到该点运动模型即可得到速度与路径相耦合的候选轨迹序列;
其中,以车辆切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动模型具体分为:
31)用抽象函数将车辆位置随时间变化的关系表示如下:
其中,f是纵向距离随时间变化的函数,g是侧向距离随时间变化的函数,这两个函数即可将车辆轨迹表示出来;
32)将上述函数用泰勒公式展开,保留到二次项,得到如下方程:
其中,各阶导数表示如下:
33)将以上各阶导数代入到轨迹方程中,得到所建立的以切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动方程:
其中,(s
t+1,l
t+1)为该点运动方程输出的下一时刻的位置坐标,(s
t,l
t)为当前t时刻的纵横向位置坐标,T是车辆的规划周期,v
t是车辆的速度,
是车辆的横摆角,
为车辆的切向加速度,
为车辆的法向加速度。
将步骤2)中的两个加速度序列的输入利用矩阵的方式耦合起来,输入到该点运动模型得到速度与路径相耦合的候选轨迹序列,具体包括如下步骤:
34)速度与路径两个方向加速度序列用矩阵方式耦合如下:
其中,C
p是候选路径的个数,C
s是候选速度的个数,A
ij(t)是第i条候选路径输入序列与第j条候选速度输入序列耦合得到的候选轨迹的输入序列,具体如下:
35)将上述耦合后的加速度序列输入到建立的点运动模型中,即可得到如下轨迹序列:
P
ij(t)=[p
ij(t+1|t),p
ij(t+2|t),…,p
ij(t+k|t),…,p
ij(t+N
p|t)]
4)利用RMSProp优化器实时获取当前时刻车辆最优的轨迹,将该轨迹对应的控制量作为输入,即实现路径与速度的耦合规划;建立的优化函数需考虑安全性、高效性、舒适性,并通过调节这三个特性的权重来满足个性化驾驶。具体如下:
41)建立的优化函数J具体如下:
其中,
为第i个候选速度对应终点时刻的纵向位置;
为第j个候选路径对应终点时刻的侧向位置;
为第i个候选速度与第j个候选路径耦合得到的候选轨迹对 应的危险度,R
ref为参考危险度,这一项代表安全性,M为权重;
为第i个候选速度与第j个候选路径耦合得到的轨迹对应的速度,v
ref为参考速度,这一项代表高效性,N为权重;分母中的s
ref为参考纵向位置,l
ref为参考横向位置,代表舒适性;
42)建立的RMS优化器在确定学习率时,首先定义学习率调整因子r:
其中,r
0为初始时刻的学习率调整因子;r
t为t时刻的调整因子;ρ为衰减系数;g
t为优化函数对应的梯度;
43)进一步得到学习率,并确定最优目标位置:
本发明具体应用途径很多,以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以作出若干改进,这些改进也应视为本发明的保护范围。
Claims (7)
- 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法,其特征在于,包括步骤如下:1)获取自车的运动状态信息及周围车辆的相对运动状态信息;2)根据自车当前运动状态信息,分别建立自车的候选路径模型和候选速度模型;得到以车辆的法向加速度序列为输入,横摆角为输出的路径序列;以及切向加速度序列为输入,速度为输出的速度序列;3)建立一个以车辆切向加速度序列和法向加速度序列为输入,速度、横摆角、坐标为输出的点运动模型,并将上述两个序列的输入利用矩阵的方式耦合起来,输入到该点运动模型得到速度与路径相耦合的候选轨迹序列;4)利用RMSProp优化器实时获取当前时刻车辆最优的轨迹,将该轨迹对应的控制量作为输入,实现路径与速度的耦合规划。
- 根据权利要求1所述的自动驾驶车辆路径与速度高度耦合的轨迹规划方法,其特征在于,所述步骤2)中的候选路径模型利用4次多项式建立车辆侧向位置l与纵向位置s的函数并得到以法向加速度序列为输入,横摆角为输出的路径序列,具体包括如下步骤:21)根据道路边界约束,得到候选路径终点时刻的侧向位置l t+Np序列:l t+Np=l min:Δl/C p:l max其中,l min和l max为道路的上、下边界;Δl=l max-l min;C p为候选路径的个数;22)根据自车当前运动状态,以及给定的候选终点时刻的位置,利用4次多项式拟合出相应的候选路径,即侧向位置l相对于纵向位置s的方程,该过程将车辆的速度看成匀速的,具体如下:l=a 0+a 1s+a 2s 2+a 3s 3+a 4s 4
- 根据权利要求1或3所述的自动驾驶车辆路径与速度高度耦合的轨迹规划方法,其特征在于,所述步骤2)中的候选速度模型利用4次多项式建立车辆纵向距离s与时间t的函数,并得到以切向加速度序列为输入,速度为输出的速度序列,具体包括如下步骤:24)根据车辆加速性能约束,候选速度序列可由候选终点时刻的纵向位置序列s t+Np表示如下:s t+Np=s min:Δs/C s:s max其中,s min和s max为车辆所能达到的距离的上、下边界;Δs=s max-s min;C s为候选速度的个数;25)根据自车当前运动状态,以及给定的候选纵向距离序列,利用4次多项式拟合出相应的候选速度函数,即纵向位置s关于时间t的函数,具体如下:s(t)=p 0+p 1t+p 2t 2+p 3t 3+p 4t 4
- 根据权利要求1所述的自动驾驶车辆路径与速度高度耦合的轨迹规划方法,其特征在于,所述步骤3)中的以车辆切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动模型具体分为:31)用抽象函数将车辆位置随时间变化的关系表示如下:其中,f是纵向距离随时间变化的函数,g是侧向距离随时间变化的函数,这两个函数即可将车辆轨迹表示出来;32)将上述函数用泰勒公式展开,保留到二次项,得到如下方程:其中,各阶导数表示如下:33)将以上各阶导数代入到轨迹方程中,得到所建立的以切向加速度和法向加速度为输入,速度、横摆角、坐标为输出的点运动方程:
- 根据权利要求5所述的自动驾驶车辆路径与速度高度耦合的轨迹规划方法,其特征在于,将步骤2)中的两个加速度序列的输入利用矩阵的方式耦合起来,输入到该点运动模型得到速度与路径相耦合的候选轨迹序列,具体包括如下步骤:34)速度与路径两个方向加速度序列用矩阵方式耦合如下:其中,C p是候选路径的个数,C s是候选速度的个数,A ij(t)是第i条候选路径输入序列与第j条候选速度输入序列耦合得到的候选轨迹的输入序列,具体如下:35)将上述耦合后的加速度序列输入到建立的点运动模型中,即可得到如下轨迹序列:P ij(t)=[p ij(t+1|t),p ij(t+2|t),…,p ij(t+k|t),…,p ij(t+N p|t)]
- 根据权利要求1所述的自动驾驶车辆路径与速度高度耦合的轨迹规划方法,其特征在于,所述步骤4)中利用RMSProp优化器进行优化时,建立的优化函数需考虑安全性、高效性、舒适性,并通过调节这三个特性的权重来满足个性化驾驶。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910986350.2A CN110703754B (zh) | 2019-10-17 | 2019-10-17 | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 |
CN201910986350.2 | 2019-10-17 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2021073079A1 true WO2021073079A1 (zh) | 2021-04-22 |
Family
ID=69201168
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2020/090205 WO2021073079A1 (zh) | 2019-10-17 | 2020-05-14 | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN110703754B (zh) |
WO (1) | WO2021073079A1 (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113619604A (zh) * | 2021-08-26 | 2021-11-09 | 清华大学 | 自动驾驶汽车的集成式决控方法、装置及存储介质 |
CN113650622A (zh) * | 2021-07-16 | 2021-11-16 | 东风柳州汽车有限公司 | 车速轨迹规划方法、装置、设备及存储介质 |
CN113741450A (zh) * | 2021-08-31 | 2021-12-03 | 的卢技术有限公司 | 一种车辆自动驾驶横向自适应控制方法 |
CN113928338A (zh) * | 2021-10-08 | 2022-01-14 | 南京航空航天大学 | 一种纵横向耦合的智能车辆轨迹规划方法及系统 |
CN114387781A (zh) * | 2021-12-30 | 2022-04-22 | 北京建筑大学 | 车辆引导控制方法 |
CN114995415A (zh) * | 2022-05-25 | 2022-09-02 | 武汉理工大学 | 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法 |
CN117572875A (zh) * | 2024-01-15 | 2024-02-20 | 上海友道智途科技有限公司 | 一种基于热启动的实时速度规划方法、系统、设备及介质 |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110703754B (zh) * | 2019-10-17 | 2021-07-09 | 南京航空航天大学 | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 |
CN111862682A (zh) * | 2020-06-24 | 2020-10-30 | 东软睿驰汽车技术(沈阳)有限公司 | 一种模型确定方法和相关装置 |
CN112567439B (zh) * | 2020-11-09 | 2022-11-29 | 驭势(上海)汽车科技有限公司 | 一种交通流信息的确定方法、装置、电子设备和存储介质 |
CN113032503A (zh) * | 2021-03-02 | 2021-06-25 | 重庆智行者信息科技有限公司 | 备选轨迹评价方法及装置 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104881030A (zh) * | 2015-05-27 | 2015-09-02 | 西安交通大学 | 基于快速终端滑模原理的无人车侧纵向耦合跟踪控制方法 |
US20170039859A1 (en) * | 2015-08-03 | 2017-02-09 | Amber Garage, Inc. | Planning a flight path by identifying key frames |
CN107145936A (zh) * | 2017-04-22 | 2017-09-08 | 大连理工大学 | 一种基于强化学习的车辆跟驰模型建立方法 |
CN107901917A (zh) * | 2017-11-16 | 2018-04-13 | 中国科学院合肥物质科学研究院 | 一种基于滑转滑移耦合估计的无人驾驶车辆路径跟踪控制方法 |
CN109727469A (zh) * | 2019-01-08 | 2019-05-07 | 南京航空航天大学 | 一种多车道下自动驾驶车辆综合危险度评估方法 |
CN109976355A (zh) * | 2019-04-26 | 2019-07-05 | 腾讯科技(深圳)有限公司 | 轨迹规划方法、系统、设备及存储介质 |
CN110703754A (zh) * | 2019-10-17 | 2020-01-17 | 南京航空航天大学 | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 |
Family Cites Families (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5222691A (en) * | 1991-08-28 | 1993-06-29 | United Technologies Corporation | Automatic turn coordination trim control for rotary wing aircraft |
CN103496366B (zh) * | 2013-09-09 | 2016-02-24 | 北京航空航天大学 | 一种基于车车协同的主动换道避撞控制方法与装置 |
EP3123713A4 (en) * | 2014-03-26 | 2017-09-06 | Mark W. Publicover | Computerized method and system for providing customized entertainment content |
US9227632B1 (en) * | 2014-08-29 | 2016-01-05 | GM Global Technology Operations LLC | Method of path planning for evasive steering maneuver |
EP3001272B1 (en) * | 2014-09-26 | 2017-04-12 | Volvo Car Corporation | Method of trajectory planning for yielding manoeuvres |
KR101714118B1 (ko) * | 2014-10-10 | 2017-03-08 | 현대자동차주식회사 | 자율주행차량의 주행모드 전환 제어 장치 |
CN205396080U (zh) * | 2016-01-14 | 2016-07-27 | 南京航空航天大学 | 一种汽车主动避撞系统 |
US9731755B1 (en) * | 2016-02-16 | 2017-08-15 | GM Global Technology Operations LLC | Preview lateral control for automated driving |
EP3443429B1 (en) * | 2016-04-12 | 2020-12-02 | Agjunction LLC | Line acquisition path generation using curvature profiles |
CN107272692A (zh) * | 2017-07-18 | 2017-10-20 | 北京理工大学 | 基于微分平坦和自抗扰的无人车路径规划与跟踪控制方法 |
CN107702716B (zh) * | 2017-08-31 | 2021-04-13 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、系统和装置 |
CN108572643B (zh) * | 2018-01-16 | 2021-07-27 | 上海蔚来汽车有限公司 | 在自动驾驶中使用的避障方法和避障系统、以及记录介质 |
CN108387242B (zh) * | 2018-02-07 | 2021-04-09 | 西南交通大学 | 自动驾驶换道准备和执行一体化轨迹规划方法 |
GB201803292D0 (en) * | 2018-02-28 | 2018-04-11 | Five Ai Ltd | Efficient computation of collision probabilities for safe motion planning |
CN109540159B (zh) * | 2018-10-11 | 2020-11-27 | 同济大学 | 一种快速完备的自动驾驶轨迹规划方法 |
CN109375632B (zh) * | 2018-12-17 | 2020-03-20 | 清华大学 | 自动驾驶车辆实时轨迹规划方法 |
CN109669461B (zh) * | 2019-01-08 | 2020-07-28 | 南京航空航天大学 | 一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法 |
CN109855639B (zh) * | 2019-01-15 | 2022-05-27 | 天津大学 | 基于障碍物预测与mpc算法的无人驾驶轨迹规划方法 |
CN109724614B (zh) * | 2019-02-22 | 2021-06-04 | 百度在线网络技术(北京)有限公司 | 自动驾驶车辆的速度规划方法、装置和存储介质 |
CN109991636A (zh) * | 2019-03-25 | 2019-07-09 | 启明信息技术股份有限公司 | 基于gps、imu以及双目视觉的地图构建方法及系统 |
CN109947112B (zh) * | 2019-04-04 | 2020-07-21 | 大连理工大学 | 两轮自平衡车直线定点运动的最优时间轨迹规划方法 |
CN110077397B (zh) * | 2019-05-14 | 2020-08-04 | 芜湖汽车前瞻技术研究院有限公司 | 一种智能车避障轨迹规划方法及装置 |
CN110244713B (zh) * | 2019-05-22 | 2023-05-12 | 江苏大学 | 一种基于人工势场法的智能车辆换道轨迹规划系统及方法 |
-
2019
- 2019-10-17 CN CN201910986350.2A patent/CN110703754B/zh active Active
-
2020
- 2020-05-14 WO PCT/CN2020/090205 patent/WO2021073079A1/zh active Application Filing
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104881030A (zh) * | 2015-05-27 | 2015-09-02 | 西安交通大学 | 基于快速终端滑模原理的无人车侧纵向耦合跟踪控制方法 |
US20170039859A1 (en) * | 2015-08-03 | 2017-02-09 | Amber Garage, Inc. | Planning a flight path by identifying key frames |
CN107145936A (zh) * | 2017-04-22 | 2017-09-08 | 大连理工大学 | 一种基于强化学习的车辆跟驰模型建立方法 |
CN107901917A (zh) * | 2017-11-16 | 2018-04-13 | 中国科学院合肥物质科学研究院 | 一种基于滑转滑移耦合估计的无人驾驶车辆路径跟踪控制方法 |
CN109727469A (zh) * | 2019-01-08 | 2019-05-07 | 南京航空航天大学 | 一种多车道下自动驾驶车辆综合危险度评估方法 |
CN109976355A (zh) * | 2019-04-26 | 2019-07-05 | 腾讯科技(深圳)有限公司 | 轨迹规划方法、系统、设备及存储介质 |
CN110703754A (zh) * | 2019-10-17 | 2020-01-17 | 南京航空航天大学 | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113650622A (zh) * | 2021-07-16 | 2021-11-16 | 东风柳州汽车有限公司 | 车速轨迹规划方法、装置、设备及存储介质 |
CN113650622B (zh) * | 2021-07-16 | 2023-06-20 | 东风柳州汽车有限公司 | 车速轨迹规划方法、装置、设备及存储介质 |
CN113619604A (zh) * | 2021-08-26 | 2021-11-09 | 清华大学 | 自动驾驶汽车的集成式决控方法、装置及存储介质 |
CN113619604B (zh) * | 2021-08-26 | 2023-08-15 | 清华大学 | 自动驾驶汽车的集成式决控方法、装置及存储介质 |
CN113741450A (zh) * | 2021-08-31 | 2021-12-03 | 的卢技术有限公司 | 一种车辆自动驾驶横向自适应控制方法 |
CN113741450B (zh) * | 2021-08-31 | 2023-11-21 | 的卢技术有限公司 | 一种车辆自动驾驶横向自适应控制方法 |
CN113928338A (zh) * | 2021-10-08 | 2022-01-14 | 南京航空航天大学 | 一种纵横向耦合的智能车辆轨迹规划方法及系统 |
CN114387781A (zh) * | 2021-12-30 | 2022-04-22 | 北京建筑大学 | 车辆引导控制方法 |
CN114387781B (zh) * | 2021-12-30 | 2024-04-09 | 北京建筑大学 | 车辆引导控制方法 |
CN114995415A (zh) * | 2022-05-25 | 2022-09-02 | 武汉理工大学 | 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法 |
CN117572875A (zh) * | 2024-01-15 | 2024-02-20 | 上海友道智途科技有限公司 | 一种基于热启动的实时速度规划方法、系统、设备及介质 |
CN117572875B (zh) * | 2024-01-15 | 2024-04-12 | 上海友道智途科技有限公司 | 一种基于热启动的实时速度规划方法、系统、设备及介质 |
Also Published As
Publication number | Publication date |
---|---|
CN110703754A (zh) | 2020-01-17 |
CN110703754B (zh) | 2021-07-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2021073079A1 (zh) | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 | |
WO2020143288A1 (zh) | 一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法 | |
CN113386795B (zh) | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策系统 | |
CN109375632B (zh) | 自动驾驶车辆实时轨迹规划方法 | |
CN110597245B (zh) | 基于二次型规划和神经网络的自动驾驶换道轨迹规划方法 | |
CN111338340B (zh) | 基于模型预测的无人驾驶汽车局部路径规划方法 | |
CN109177974B (zh) | 一种智能汽车的人机共驾型车道保持辅助方法 | |
WO2018176593A1 (zh) | 一种面向无人自行车的局部避障路径规划方法 | |
CN110286681B (zh) | 一种变曲率弯道的动态自动驾驶换道轨迹规划方法 | |
CN110329263A (zh) | 自动驾驶车辆自适应换道轨迹规划方法 | |
CN110780674A (zh) | 一种提高自动驾驶轨迹跟踪控制的方法 | |
CN108732921B (zh) | 一种自动驾驶汽车横向可拓预瞄切换控制方法 | |
CN111409641A (zh) | 一种基于纵横向协调的轨迹跟踪控制方法及系统 | |
CN106371439B (zh) | 一种统一的自动驾驶横向规划方法与系统 | |
CN111806467A (zh) | 一种基于车辆行驶规律的变速动态换道轨迹规划方法 | |
CN112046484B (zh) | 一种基于q学习的车辆变道超车路径规划方法 | |
CN113276848A (zh) | 一种智能驾驶换道避障轨迹规划、跟踪控制方法及系统 | |
CN108791290A (zh) | 基于在线增量式dhp的双车协同自适应巡航控制方法 | |
CN105644566B (zh) | 一种基于车联网的电动汽车辅助换道轨迹的跟踪方法 | |
Chen et al. | An adaptive path tracking controller based on reinforcement learning with urban driving application | |
Peicheng et al. | Intelligent vehicle path tracking control based on improved MPC and hybrid PID | |
EP3980307A1 (en) | Method and apparatus for trajectory shape generation for autonomous vehicles | |
CN113553726B (zh) | 一种冰雪环境下的主从博弈型人机协同转向控制方法 | |
CN212828326U (zh) | 一种基于纵横向协调的轨迹跟踪控制系统 | |
Liu et al. | Path tracking control for inverse vehicle handling dynamics |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 20875937 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 20875937 Country of ref document: EP Kind code of ref document: A1 |