CN112937606B - 用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统 - Google Patents

用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统 Download PDF

Info

Publication number
CN112937606B
CN112937606B CN202110335084.4A CN202110335084A CN112937606B CN 112937606 B CN112937606 B CN 112937606B CN 202110335084 A CN202110335084 A CN 202110335084A CN 112937606 B CN112937606 B CN 112937606B
Authority
CN
China
Prior art keywords
vehicle
point
collision
path
grid
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
Application number
CN202110335084.4A
Other languages
English (en)
Other versions
CN112937606A (zh
Inventor
王雷
王更泽
王宜飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ziqing Zhixing Technology Beijing Co ltd
Original Assignee
Ziqing Zhixing Technology Beijing Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Ziqing Zhixing Technology Beijing Co ltd filed Critical Ziqing Zhixing Technology Beijing Co ltd
Priority to CN202110335084.4A priority Critical patent/CN112937606B/zh
Publication of CN112937606A publication Critical patent/CN112937606A/zh
Application granted granted Critical
Publication of CN112937606B publication Critical patent/CN112937606B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B60VEHICLES IN GENERAL
    • B60WCONJOINT 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/00Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001Planning or execution of driving tasks
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B60VEHICLES IN GENERAL
    • B60WCONJOINT 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
    • B60W30/00Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/08Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B60VEHICLES IN GENERAL
    • B60WCONJOINT 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/00Drive control systems specially adapted for autonomous road vehicles
    • B60W60/001Planning or execution of driving tasks
    • B60W60/0015Planning or execution of driving tasks specially adapted for safety
    • B60W60/0016Planning or execution of driving tasks specially adapted for safety of the vehicle or its occupants
    • B60W2420/408
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B60VEHICLES IN GENERAL
    • B60WCONJOINT 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/00Input parameters relating to overall vehicle dynamics
    • B60W2520/10Longitudinal speed
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B60VEHICLES IN GENERAL
    • B60WCONJOINT 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/00Input parameters relating to infrastructure
    • B60W2552/30Road curve radius
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B60VEHICLES IN GENERAL
    • B60WCONJOINT 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/00Input parameters relating to infrastructure
    • B60W2552/50Barriers

Abstract

本发明公开了一种用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统,该方法包括:步骤1,获取车辆及障碍物信息;步骤2,规划防撞路径;步骤3,选择最佳防撞方式;步骤2包括:步骤21,计算预瞄距离;步骤22,选取车辆前方循迹路径上与车辆之间的距离最接近预瞄距离的采样点作为预瞄点;步骤23,解析车辆循迹路径上是否存在障碍物,若有障碍物,则进行防撞路径规划;步骤24,将车辆前方区域划分成等大小的网格,并给每个网格进行编号,根据激光雷达获取的前方障碍物点云信息确定网格的占用状态,取防撞路径上的等间隔离散点,根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。本发明能够解决循迹自动驾驶车辆局部路径规划和最优避障行为决策问题。

Description

用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统
技术领域
本发明涉及自动驾驶技术领域,特别是关于一种用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统。
背景技术
主动避障技术是保障自动驾驶车辆安全行驶的关键技术之一,随着智能车辆技术的发展,主动避障技术得到了深入研究和应用。该技术主要通过先进的现代信息技术和传感器技术实时获取道路交通环境等多维信息,根据自车运动状况和道路交通环境信息识别潜在的行车风险因素,并根据风险等级采取不同的避障行为(换道避障、制动避障、巡航跟车等),特别在紧急工况下,避障系统将采取主动制动以确保行车安全,通过紧急制动防止碰撞事故发生或降低事故损失。
循迹自动驾驶车辆通常应用于限定区域的固定场景,如景区观光车、巡检车等低速自动驾驶场景。车辆在自主循迹过程中需要进行局部路径规划和避撞控制,但实际车辆周围环境复杂,存在多类型且运动状态不确定的障碍物,如果采用基于规则的避撞控制方法无法系统地考虑全局避撞约束,而基于全局约束优化算法的路径规划计算量大,难以满足车辆实时性需求。因此,如何设计既能满足车辆动力学和全局碰撞约束,又满足车辆控制实时性要求的局部路径规划和最优避障行为决策方法,是实现车辆全自动驾驶的一个关键问题。
发明内容
本发明的目的在于提供一种用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的,本发明提供一种用于循迹自动驾驶车辆的防撞路径规划与控制方法,该方法包括:
步骤1,获取车辆相关信息及障碍物信息;
步骤2,规划防撞路径;
步骤3,根据所规划的防撞路径对应的目标车速,选择最佳防撞方式;
其中,步骤2具体包括:
步骤21,根据当前车速v和道路曲率ρ,计算预瞄距离;
步骤22,根据预瞄距离dp和当前车辆位置(xi,yi),选取车辆前方循迹路径上与车辆之间的距离最接近预瞄距离的采样点作为预瞄点;
步骤23,根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物,若不存在障碍物,则按照循迹路径继续行驶;若有障碍物,则进行防撞路径规划;
步骤24,在车辆坐标系xOy中,将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格,并给每个网格进行编号,根据激光雷达获取的前方障碍物点云信息确定网格的占用状态,取步骤23所规划的防撞路径上的等间隔离散点,根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。
进一步地,步骤21具体包括:
步骤211,通过如下式(1)示出的插值方式计算预瞄距离初始值dp_ρ
Figure GDA0003172722240000021
式中,K1,K2为曲率预瞄系数,且K1<0,K2<0;C1,C2为常数项;ρcon1为预设值;
步骤212,通过如下式(2)计算预瞄距离修正值dp_v
dp_v=K*v (2)
式中,K为车速预瞄系数;
步骤213,通过式(3)计算预瞄距离dp
dp=dp_ρ+dp_v (3)。
进一步地,步骤22具体包括:
步骤221,计算循迹路径上距离车辆当前位置Ai(xi,yi)最近的点坐标Ar_i(xr_i,yr_i);
步骤222,计算点Ar_i和车辆前方循迹路径上的点Aj的距离Dij,如式(4)示出地选择dp与Dij差值绝对值最小的对应车辆前方循迹路径上的点为预瞄点Pi_p(xi_p,yi_p);
Figure GDA0003172722240000031
进一步地,步骤23具体包括:
步骤231,以车辆实际位置点为起点,采用如下坐标变换公式(5)和(6),将预瞄点Pi_p(xi_p,yi_p)通过坐标变换转换到车辆坐标系xOy中的Pi_p_V(xi_p_V,yi_p_V):
xi_p_V=Δx cosΨi-Δy sinΨi (5)
yi_p_V=Δx sinΨi+Δy cosΨi (6)
式中,Δx=xi_p-xi,Δy=yi_p-yi,Ψi表示车辆实际航向角;
步骤232,以Pi_p_V(xi_p_V,yi_p_V)为参考点,根据障碍物位置信息,按照式(7)和(8)提供的方式设置不同横向偏移规划防撞路径的终点位置Pn(xn,yn):
xn=xi_p_V+n*ΔH*sin(θi) (7)
yn=yi_p_V+n*ΔH*cos(θi) (8)
其中,ΔH为防撞路径终点距离参考点的横向基本偏移量,n为非零整数,θi为车辆循迹轨迹上预瞄点Pi_p对应的车辆航向角;
步骤233,通过式(9)所示的四个控制点确定三阶贝塞尔曲线,作为防撞路径C(t):
Figure GDA0003172722240000032
式中,Δs为防撞曲线调整参数,
Figure GDA0003172722240000041
进一步地,步骤24具体包括:
步骤241,根据步骤233所规划的防撞路径C(t上的所述等间隔离散点P在车辆坐标系xOy中的坐标,利用式(10)计算点P沿x轴方向所对应的网格编号:
Index(P)=yIndex*n+xIndex (10)
式中,
Figure GDA0003172722240000042
(xp,yp)为点P在车辆坐标系xOy中的坐标,Δ为每个网格的长和宽,横向网格数n=L/Δ,纵向网格数m=H/Δ,
步骤242,根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划,该邻近区域车载激光雷达探测的横向安全距离Lsafe为边界,横向最大安全距离为Lmax
若点P沿x轴方向的邻近区域
Figure GDA0003172722240000043
内的有网格被激活,则Vlat=0;
若点P沿x轴方向的邻近区域
Figure GDA0003172722240000044
内的无网格被激活,则Vlat=Vlat_max
若点P有且仅有沿x轴方向的邻近区域
Figure GDA0003172722240000045
Figure GDA0003172722240000046
内的网格被激活,横向车速计算式如下式(11)所示:
Figure GDA0003172722240000047
式中,xac为该邻近区域
Figure GDA0003172722240000048
内距离点P最近的激活网格中心点横坐标,Vlat_max表示车辆的横向车速最大值,Vlat表示车辆的横向车速;
步骤243,根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划,车载激光雷达探测的纵向安全距离为Hsafe,其最大值为Hmax,最小值为Hmin
若0≤yp≤Hmin,Vlon=0;
若Hmax≤yp,Vlon=0;
若Hmin<yp<Hmax,纵向车速计算式如下式(12)所示:
Vlon=Vlon_max*(yp-Hmin)/(Hmax-Hmin) (12)
式中,Vlon_max表示车辆的纵向车速最大值,Vlon表示车辆的纵向车速;
步骤244,根据横向车速规划和纵向车速规划,按照式(13)计算车辆防撞路径C(t)对应的目标车速Vtar(t);
Vtar(t)=min{Vlat,Vlon} (13)
重复步骤241至步骤244得到防撞路径C(t)对应的目标车速集合{Vtar(1),Vtar(2),…Vtar(t)。
进一步地,如果网格被占用,则认为网格处于激活转态,f(i)=1;若果网格未被占用,则认为网格未被激活,f(i)=0。
进一步地,步骤3具体包括:
若所规划的防撞路径对应的目标车速Vtar(t)均等于0,则车辆采用预设减速度沿当前循迹路径进行避障;
若所规划的防撞路径有一条路径对应的目标车速Vtar(t)不等于0,则车辆选择该防撞路径防止碰撞的发生;
若所规划的防撞路径有两条或以上路径对应的目标车速Vtar(t)不等于0,则优先选择左换道进行防撞,若存在两条或以上做换道路径,则优先选择靠近当前循迹路径的防撞路径。
本发明还提供一种用于循迹自动驾驶车辆的防撞路径规划与控制系统,该系统包括:
信息采集装置,其用于获取车辆相关信息及障碍物信息;
防撞路径规划装置,其用于规划防撞路径;
控制装置,其用于根据所规划的防撞路径对应的目标车速,选择最佳防撞方式;
其中,防撞路径规划装置具体包括:
预瞄距离计算单元,其用于根据当前车速v和道路曲率ρ,计算预瞄距离;
预瞄点计算单元,其用于根据预瞄距离dp和当前车辆位置(xi,yi),选取车辆前方循迹路径上与车辆之间的距离最接近预瞄距离的采样点作为预瞄点;
障碍物识别与防撞路径规划单元,其用于根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物,若不存在障碍物,则按照循迹路径继续行驶;若有障碍物,则进行防撞路径规划;
车辆控制参数计算单元,其用于在车辆坐标系xOy中,将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格,并给每个网格进行编号,根据激光雷达获取的前方障碍物点云信息确定网格的占用状态,取障碍物识别与防撞路径规划单元所规划的防撞路径上的等间隔离散点,根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。
进一步地,障碍物识别与防撞路径规划单元包括:
坐标转换子单元,其用于以车辆实际位置点为起点,采用如下坐标变换公式(5)和(6),将预瞄点Pi_p(xi_p,yi_p)通过坐标变换转换到车辆坐标系xOy中的Pi_p_V(xi_p_V,yi_p_V):
xi_p_V=Δx cosΨi-Δy sinΨi (5)
yi_p_V=Δx sinΨi+Δy cosΨi (6)
式中,Δx=xi_p-xi,Δy=yi_p-yi,Ψi表示车辆实际航向角;
终点设置子单元,其用于以Pi_p_V(xi_p_V,yi_p_V)为参考点,根据障碍物位置信息,按照式(7)和(8)提供的方式设置不同横向偏移规划防撞路径的终点位置Pn(xn,yn):
xn=xi_p_V+n*ΔH*sin(θi) (7)
yn=yi_p_V+n*ΔH*cos(θi) (8)
其中,ΔH为防撞路径终点距离参考点的横向基本偏移量,n为非零整数,θi为车辆循迹轨迹上预瞄点Pi_p对应的车辆航向角;
防撞曲线计算子单元,其用于通过式(9)所示的四个控制点确定三阶贝塞尔曲线,作为防撞路径C(t):
Figure GDA0003172722240000071
式中,Δs为防撞曲线调整参数,
Figure GDA0003172722240000072
进一步地,车辆控制参数计算单元包括:
网格编号计算子单元,其用于根据障碍物识别与防撞路径规划单元所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标,利用式(10)计算点P沿x轴方向所对应的网格编号:
Index(P)=yIndex*n+xIndex (10)
式中,
Figure GDA0003172722240000073
(xp,yp)为点P在车辆坐标系xOy中的坐标,Δ为每个网格的长和宽,横向网格数n=L/Δ,纵向网格数m=H/Δ;
横向车速规划子单元,其用于根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划,该邻近区域车载激光雷达探测的横向安全距离Lsafe为边界,横向最大安全距离为Lmax
若点P沿x轴方向的邻近区域
Figure GDA0003172722240000074
内的有网格被激活,则Vlat=0;
若点P沿x轴方向的邻近区域
Figure GDA0003172722240000075
内的无网格被激活,则Vlat=Vlat_max
若点P有且仅有沿x轴方向的邻近区域
Figure GDA0003172722240000076
Figure GDA0003172722240000077
内的网格被激活,横向车速计算式如下式(11)所示:
Figure GDA0003172722240000081
式中,xac为该邻近区域
Figure GDA0003172722240000082
内距离点P最近的激活网格中心点横坐标,Vlat_max表示车辆的横向车速最大值,Vlat表示车辆的横向车速;
纵向车速规划子单元,其用于根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划,车载激光雷达探测的纵向安全距离为Hsafe,其最大值为Hmax,最小值为Hmin
若0≤yp≤Hmin,Vlon=0;
若Hmax≤yp,Vlon=0;
若Hmin<yp<Hmax,纵向车速计算式如下式(12)所示:
Vlon=Vlon_max*(yp-Hmin)/(Hmax-Hmin) (12)
式中,Vlon_max表示和辆的纵向车速最大值,Vlon表示车辆的纵向车速;
目标车速计算子单元,其用于根据横向车速规划和纵向车速规划,按照式(13)计算车辆防撞路径C(t)对应的目标车速Vtar(t)及目标车速集合{Vtar(1),Vtar(2),…Vtar(t)};
Vtar(t)=min{Vlat,Vlon} (13)。
本发明在满足车辆全局碰撞约束和控制实时性要求的前提下,解决循迹自动驾驶车辆局部路径规划和最优避障行为决策问题。
附图说明
图1为本发明实施例提供的用于循迹自动驾驶车辆的防撞路径规划与控制系统的原理示意图。
图2为本发明实施例提供的预瞄点计算原理示意图。
图3为本发明实施例提供的车辆前方障碍物网格划分示意图。
图4为本发明实施例提供的激光雷达感知安全区域定义示意图。
图5为本发明实施例提供的防撞横向车速规划示意图。
图6为本发明实施例提供的防撞纵向车速规划示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
如图1所示,本发明实施例提供的用于循迹自动驾驶车辆的防撞路径规划与控制方法包括:
步骤1,获取车辆定位及障碍物信息。
车辆利用车载导航与定位系统获取车辆实时位置信息,包括车辆位置和位姿,记为(xi,yii),(xi,yi)表示车辆在大地坐标系中的位置,Ψi表示车辆实际航向角。车辆利用前置激光雷达探测前方障碍物点云信息,通过点云解析获得障碍物的位置和形状信息。
步骤2,规划防撞路径。
步骤21,根据当前车速v和道路曲率ρ计算预瞄距离。其中,当前车速v可通过CAN通信获取,也通过IMU或者GPS测量得到。道路曲率ρ可以通过车辆循迹轨迹方法获得。
步骤211,根据当前道路曲率ρ,通过如下式(1)示出的插值方式计算预瞄距离初始值dp_ρ
Figure GDA0003172722240000091
式中,K1,K2为曲率预瞄系数,且K1<0,K2<0;C1,C2为常数项;ρcon1为车辆以最高稳定车速的50%行驶时能安全通过的弯道曲率,定义为安全转向曲率。实际应用中,K1,K2,C1,C2的具体取值可通过实车标定三个关键转向曲率及其对应的预瞄距离进行计算,三个关键转向曲率分别为最小转弯半径曲率、最大转弯半径曲率和安全转向曲率。
步骤212,根据当前车速v,通过如下式(2)计算预瞄距离修正值dp_v
dp_v=K*v (2)
式中,K为车速预瞄系数,其常用数值在1.0-4.0范围内。
步骤213,通过式(3)计算初始值和修正值之和,得到预瞄距离dp
dp=dp_ρ+dp_v (3)
步骤22,如图2所示,根据预瞄距离dp和当前车辆位置信息(xi,yi),选取车辆前方循迹路径上与车辆之间的距离最接近预瞄距离的采样点作为预瞄点。
步骤221,计算循迹路径上距离车辆当前位置Ai(xi,yi)最近的点坐标,称之为循迹真实位置坐标Ar_i(xr_i,yr_i)。
步骤222,计算循迹真实位置点Ar_i和车辆前方循迹轨迹点Aj的距离Dij,如式(4)示出地选择dp与Dij差值绝对值最小的对应车辆前方循迹轨迹点为预瞄点Pi_p(xi_p,yi_p)。
Figure GDA0003172722240000101
当然,也可以在模型预测控制、驾驶员模型等预测轨迹获取方法获得的预测轨迹(预期参考轨迹)上选择预瞄点,根据预瞄距离来计算预瞄点。
步骤23,根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物,若不存在障碍物,则按照循迹路径继续行驶;若有障碍物,则需要进行防撞路径规划,具体规划流程如下:
步骤231,首先以车辆实际位置点为起点,采用如下坐标变换公式(5)和(6),将预瞄点Pi_p(xi_p,yi_p)通过坐标变换转换到车辆坐标系xOy中的Pi_p_V(xi_p_V,yi_p_V):
xi_p_V=Δx cosΨi-Δy sinΨi (5)
yi_p_V=Δx sinΨi+Δy cosΨi (6)
式中,Δx=xi_p-xi,Δy=yi_p-yi,Ψi表示车辆实际航向角。
步骤232,以车辆坐标系xOy中的预瞄点Pi_p_V(xi_p_V,yi_p_V)为参考点,根据障碍物位置信息,如式(7)和(8)所示,通过设置不同横向偏移规划防撞路径的终点位置Pn(xn,yn):
xn=xi_p_V+n*ΔH*sin(θi) (7)
yn=yi_p_V+n*ΔH*cos(θi) (8)
其中,ΔH为防撞路径终点距离参考点的横向基本偏移量,为常数,具体数值可以根据道路宽度、车辆尺寸和障碍物信息进行确定,实车测试中,我们通常选择2-4m作为初始参数,然后通过测试进一步标定;n为非零整数,其取值范围根据障碍物位置和道路约束条件确定;θi为车辆循迹轨迹上预瞄点Pi_p对应的车辆航向角。
除了上述实施例中设置的终点位置Pn(xn,yn)的方法之外,还可以根据障碍物的形状信息,通过设定车辆与障碍物之间的最小安全距离得到防撞路径终点位置坐标。
步骤233,本实施例采用贝塞尔曲线规划防撞路径,根据三阶贝塞尔曲线特点,四个控制点即可确定三阶贝塞尔曲线,作为防撞路径C(t),四个控制点如下式(9)所示:
Figure GDA0003172722240000111
式中,Δs为防撞曲线调整参数,Δs的选择决定了防撞路径的形状,实际工程中,可以选择一个初始参数(例如3),然后通过实车测试进行标定;
Figure GDA0003172722240000112
Figure GDA0003172722240000113
上述实施例中的贝塞尔曲线规划防撞路径也可以由曲线拟合、数值优化、人工势能场和图搜索等自动驾驶车辆轨迹规划方法替代。
步骤24,计算控制参数。
如图3和图4所示,在车辆坐标系xOy中,将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格,每个网格的长和宽均为Δ,横向网格数n=L/Δ,纵向网格数m=H/Δ。沿x轴方向给每个网格进行编号,每个网点对应唯一的编号i,则1≤i≤m*n。其中,“车辆前方纵向长度H、横向宽度L的区域”要求能够覆盖车传感器的感知范围,本实施例中可以理解为激光雷达的探测范围。
根据激光雷达获取的前方障碍物点云信息,从障碍物顶部向下做投影,可以得到网格是否被障碍物占用,将网格激活状态函数定义为f(i)。如果网格被占用,则认为网格处于激活转态,f(i)=1;若果网格未被占用,则认为网格未被激活,f(i)=0。
取步骤233所规划的防撞路径C(t)上的等间隔离散点{A,B,C,D,E,F…},根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算,具体计算流程如下:
步骤241,根据步骤233所规划的防撞路径C(t)上的点P在车辆坐标系xOy中的坐标计算对应网格编号,计算公式如下式(10)所示:
Index(P)=yIndex*n+xIndex (10)
式中,
Figure GDA0003172722240000121
(xp,yp)为轨迹点P在车辆坐标系xOy中的坐标,且P∈{A,B,C,D,E,F…}。
步骤242,如图5所示,根据轨迹点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划。该邻近区域车载激光雷达探测的横向安全距离Lsafe为边界,横向最大安全距离为Lmax,横向最小安全距离为Lmin。Lmax和Lmin的数值与车载激光雷达有关,同时考虑车辆与障碍物之间的最小安全距离,比如:Lmin=2m,Lmax=3.5m。
若轨迹点P沿x轴方向的邻近区域
Figure GDA0003172722240000122
内的有网格被激活,则Vlat=0;
若轨迹点P沿x轴方向的邻近区域
Figure GDA0003172722240000123
内的无网格被激活,则Vlat=Vlat_max
若轨迹点P有且仅有沿x轴方向的邻近区域
Figure GDA0003172722240000124
Figure GDA0003172722240000125
内的网格被激活,横向车速计算式如下式(11)所示:
Figure GDA0003172722240000126
式中,xac为该邻近区域
Figure GDA0003172722240000131
内距离轨迹点P最近的激活网格中心点横坐标,Vlat_max表示车辆的横向车速最大值,Vlat表示车辆的横向车速,Vlat_max和Vlat与车辆的实际应用场景有关,也与具体车型有关。
步骤243,如图6所示,根据轨迹点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划。车载激光雷达探测的纵向安全距离为Hsafe,其最大值为Hmax,最小值为Hmin,Hmax和Hmin的数值与车载激光雷达参数有关,以激光雷达能够探测的最远距离为Hmax,以车辆预瞄距离为Hmin
若0≤yp≤Hmin,Vlon=0;
若Hmax≤yp,Vlon=0;
若Hmin<yp<Hmax,纵向车速计算式如下式(12)所示:
Vlon=Vlon_max*(yp-Hmin)/(Hmax-Hmin) (12)
式中,Vlon_max表示车辆的纵向车速最大值,Vlon表示车辆的纵向车速,Vlon_max和Vlon的与车辆的实际应用场景有关,也与具体车型有关。
步骤244,综合考虑横向车速规划和纵向车速规划,按照式(13)计算车辆防撞路径C(t)对应的目标车速Vtar(t)。
Vtar(t)=min{Vlat,Vlon} (13)
重复前述步骤就可以得到所规划的防撞路径对应的目标车速集合{Vtar(1),Vtar(2),…Vtar(t)}。
步骤3,根据所规划的防撞路径对应的目标车速选择最佳防撞方式。
若所规划的防撞路径对应的目标车速Vtar(t)均等于0,即num=0,则车辆采用预设减速度沿当前循迹路径进行避障。其中,“预设减速度”需要根据车辆与障碍物之间的距离进行计算,减速度的选取原则是保证车辆不与障碍物发生碰撞,如果碰撞无法避免,则车辆以最大减速度制动以减低碰撞损失。
若所规划的防撞路径有一条路径对应的目标车速Vtar(t)不等于0,即num=1,则车辆选择该防撞路径防止碰撞的发生。
若所规划的防撞路径有两条或以上路径对应的目标车速Vtar(t)不等于0,即num≥2,则优先选择左换道进行防撞,若存在两条或以上做换道路径,则优先选择靠近当前循迹轨迹的防撞路径。
本发明实施例还提供一种用于循迹自动驾驶车辆的防撞路径规划与控制系统,该系统包括信息采集装置、防撞路径规划装置和控制装置,其中:
信息采集装置用于获取车辆相关信息及障碍物信息。车辆利用车载导航与定位系统获取车辆实时位置信息,包括车辆位置和位姿,记为(xi,yii),(xi,yi)表示车辆在大地坐标系中的位置,Ψi表示车辆实际航向角。车辆利用前置激光雷达探测前方障碍物点云信息,通过点云解析获得障碍物的位置和形状信息。
防撞路径规划装置用于规划防撞路径。作为防撞路径规划装置的一种优选实施方式,防撞路径规划装置具体包括预瞄距离计算单元、预瞄点计算单元、障碍物识别与防撞路径规划单元和车辆控制参数计算单元。
预瞄距离计算单元用于根据当前车速v和道路曲率ρ,计算预瞄距离dp。其中,当前车速v可通过CAN通信获取,也通过IMU或者GPS测量得到。道路曲率ρ可以通过车辆循迹轨迹方法获得。
在一个实施例中,预瞄距离计算单元包括预瞄距离初始值计算子单元、预瞄距离修正值计算子单元和预瞄距离计算子单元。
预瞄距离初始值计算子单元用于根据当前道路曲率ρ,通过如下式(1)示出的插值方式计算预瞄距离初始值dp_ρ
Figure GDA0003172722240000141
式中,K1,K2为曲率预瞄系数,且K1<0,K2<0;C1,C2为常数项;ρcon1为车辆以最高稳定车速的50%行驶时能安全通过的弯道曲率,定义为安全转向曲率。实际应用中,K1,K2,C1,C2的具体取值可通过实车标定三个关键转向曲率及其对应的预瞄距离进行计算,三个关键转向曲率分别为最小转弯半径曲率、最大转弯半径曲率和安全转向曲率。
预瞄距离修正值计算子单元根据当前车速v,通过如下式(2)计算预瞄距离修正值dp_v
dp_v=K*v (2)
式中,K为车速预瞄系数,其常用数值在1.0-4.0范围内。
预瞄距离计算子单元通过式(3)计算初始值和修正值之和,得到预瞄距离dp
dp=dp_ρ+dp_v (3)
如图2所示,预瞄点计算单元用于根据预瞄距离dp和当前车辆位置(xi,yi),选取车辆前方循迹路径上与车辆之间的距离最接近预瞄距离的采样点作为预瞄点。
在一个实施例中,预瞄点计算单元包括循迹真实位置坐标计算子单元和预瞄点确定子单元。
循迹真实位置坐标计算子单元用于计算循迹路径上距离车辆当前位置Ai(xi,yi)最近的点坐标,称之为循迹真实位置坐标Ar_i(xr_i,yr_i)。
预瞄点确定子单元用于计算循迹真实位置点Ar_i和车辆前方循迹轨迹点Aj的距离Dij,如式(4)示出地选择dp与Dij差值绝对值最小的对应车辆前方循迹轨迹点为预瞄点Pi_p(xi_p,yi_p)。
Figure GDA0003172722240000151
当然,也可以在模型预测控制、驾驶员模型等预测轨迹获取方法获得的预测轨迹(预期参考轨迹)上选择预瞄点,根据预瞄距离来计算预瞄点。
障碍物识别与防撞路径规划单元用于根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物,若不存在障碍物,则按照循迹路径继续行驶;若有障碍物,则进行防撞路径规划。
在一个实施例中,障碍物识别与防撞路径规划单元包括坐标转换子单元、终点设置子单元和防撞曲线计算子单元:
坐标转换子单元用于以车辆实际位置点为起点,采用如下坐标变换公式(5)和(6),将预瞄点Pi_p(xi_p,yi_p)通过坐标变换转换到车辆坐标系xOy中的Pi_p_V(xi_p_V,yi_p_V):
xi_p_V=Δx cosΨi-Δy sinΨi (5)
yi_p_V=Δx sinΨi+Δy cosΨi (6)
式中,Δx=xi_p-xi,Δy=yi_p-yi,Ψi表示车辆实际航向角;
终点设置子单元用于以Pi_p_V(xi_p_V,yi_p_V)为参考点,根据障碍物位置信息,按照式(7)和(8)提供的方式设置不同横向偏移规划防撞路径的终点位置Pn(xn,yn):
xn=xi_p_V+n*ΔH*sin(θi) (7)
yn=yi_p_V+n*ΔH*cos(θi) (8)
其中,ΔH为防撞路径终点距离参考点的横向基本偏移量,n为非零整数,θi为车辆循迹轨迹上预瞄点Pi_p对应的车辆航向角。
防撞曲线计算子单元用于根据三阶贝塞尔曲线特点,通过式(9)所示的四个控制点确定三阶贝塞尔曲线,作为防撞路径C(t):
Figure GDA0003172722240000161
式中,Δs为防撞曲线调整参数,Δs的选择决定了防撞路径的形状,实际工程中,可以选择一个初始参数(例如3),然后通过实车测试进行标定;
Figure GDA0003172722240000162
Figure GDA0003172722240000163
车辆控制参数计算单元用于在车辆坐标系xOy中,将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格,并给每个网格进行编号,根据激光雷达获取的前方障碍物点云信息确定网格的占用状态,取障碍物识别与防撞路径规划单元所规划的防撞路径上的等间隔离散点,根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算。
在一个实施例中,如图3至图6所示,车辆控制参数计算单元包括网格编号计算子单元、横向车速规划子单元、纵向车速规划子单元和目标车速计算子单元。
网格编号计算子单元用于根据障碍物识别与防撞路径规划单元所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标,利用式(10)计算点P沿x轴方向所对应的网格编号:
Index(P)=yIndex*n+xIndex (10)
式中,
Figure GDA0003172722240000171
(xp,yp)为点P在车辆坐标系xOy中的坐标,Δ为每个网格的长和宽,横向网格数n=L/Δ,纵向网格数m=H/Δ;
横向车速规划子单元用于根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划,该邻近区域车载激光雷达探测的横向安全距离Lsafe为边界,横向最大安全距离为Lmax
若点P沿x轴方向的邻近区域
Figure GDA0003172722240000172
内的有网格被激活,则Vlat=0;
若点P沿x轴方向的邻近区域
Figure GDA0003172722240000173
内的无网格被激活,则Vlat=Vlat_max
若点P有且仅有沿x轴方向的邻近区域
Figure GDA0003172722240000174
Figure GDA0003172722240000175
内的网格被激活,横向车速计算式如下式(11)所示:
Figure GDA0003172722240000176
式中,xac为该邻近区域
Figure GDA0003172722240000177
内距离点P最近的激活网格中心点横坐标,Vlat_max表示车辆的横向车速最大值,Vlat表示车辆的横向车速;
纵向车速规划子单元用于根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划,车载激光雷达探测的纵向安全距离为Hsafe,其最大值为Hmax,最小值为Hmin
若0≤yp≤Hmin,Vlon=0;
若Hmax≤yp,Vlon=0;
若Hmin<yp<Hmax,纵向车速计算式如下式(12)所示:
Vlon=Vlon_max*(yp-Hmin)/(Hmax-Hmin) (12)
式中,Vlon_max表示车辆的纵向车速最大值,Vlon表示车辆的纵向车速;
目标车速计算子单元用于根据横向车速规划和纵向车速规划,按照式(13)计算车辆防撞路径C(t)对应的目标车速Vtar(t)及目标车速集合{Vtar(1),Vtar(2),…Vtar(t)};
Vtar(t)=min{Vlat,Vlon} (13)。
控制装置用于根据所规划的防撞路径对应的目标车速,选择最佳防撞方式。
若所规划的防撞路径对应的目标车速Vtar(t)均等于0,即num=0,则车辆采用预设减速度沿当前循迹路径进行避障。其中,“预设减速度”需要根据车辆与障碍物之间的距离进行计算,减速度的选取原则是保证车辆不与障碍物发生碰撞,如果碰撞无法避免,则车辆以最大减速度制动以减低碰撞损失。
若所规划的防撞路径有一条路径对应的目标车速Vtar(t)不等于0,即num=1,则车辆选择该防撞路径防止碰撞的发生。
若所规划的防撞路径有两条或以上路径对应的目标车速Vtar(t)不等于0,即num≥2,则优先选择左换道进行防撞,若存在两天或以上做换道路径,则优先选择靠近当前循迹轨迹的防撞路径。
本发明所提出的用于循迹自动驾驶车辆的防撞路径规划与控制系统,基于循迹自动驾驶车辆配备的定位和障碍物探测传感器,能够在满足车辆全局碰撞约束和控制实时性要求的前提下,采用三阶贝塞尔曲线完成循迹自动驾驶车辆局部路径规划,通过对所规划防撞路径的目标车速计算实现最优避障行为决策。
最后需要指出的是:以上实施例仅用以说明本发明的技术方案,而非对其限制。本领域的普通技术人员应当理解:可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种用于循迹自动驾驶车辆的防撞路径规划与控制方法,其特征在于,包括:
步骤1,获取车辆相关信息及障碍物信息;
步骤2,规划防撞路径;
步骤3,根据所规划的防撞路径对应的目标车速,选择最佳防撞方式;
其中,步骤2具体包括:
步骤21,根据当前车速v和道路曲率ρ,计算预瞄距离dp
步骤22,根据预瞄距离dp和当前车辆位置(xi,yi),选取车辆前方循迹路径上与当前车辆位置(xi,yi)之间的距离最接近预瞄距离的采样点作为预瞄点;
步骤23,根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物,若不存在障碍物,则按照循迹路径继续行驶;若有障碍物,则进行防撞路径规划;
步骤24,在车辆坐标系xOy中,将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格,并给每个网格进行编号,根据激光雷达获取的前方障碍物点云信息确定网格的占用状态,取步骤23所规划的防撞路径上的等间隔离散点,根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算;
步骤24具体包括:
步骤241,根据步骤233所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标,利用式(10)计算点P沿x轴方向所对应的网格编号:
Index(P)=yIndex*n+xIndex (10)
式中,
Figure FDA0003250504010000011
(xp,yp)为点P在车辆坐标系xOy中的坐标,Δ为每个网格的长和宽,横向网格数n=L/Δ,纵向网格数m=H/Δ,
步骤242,根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划,该邻近区域车载激光雷达探测的横向安全距离Lsafe为边界,横向最大安全距离为Lmax,横向最小安全距离为Lmin
若点P沿x轴方向的邻近区域
Figure FDA0003250504010000021
内的有网格被激活,则Vlat=0;
若点P沿x轴方向的邻近区域
Figure FDA0003250504010000022
内的无网格被激活,则Vlat=Vlat_max
若点P有且仅有沿x轴方向的邻近区域
Figure FDA0003250504010000023
Figure FDA0003250504010000024
内的网格被激活,横向车速计算式如下式(11)所示:
Figure FDA0003250504010000025
式中,xac为该邻近区域
Figure FDA0003250504010000026
内距离点P最近的激活网格中心点横坐标,Vlat_max表示车辆的横向车速最大值,Vlat表示车辆的横向车速;
步骤243,根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划,车载激光雷达探测的纵向安全距离为Hsafe,其最大值为Hmax,最小值为Hmin
若0≤yp≤Hmin,Vlon=0;
若Hmax≤yp,Vlon=0;
若Hmin<yp<Hmax,纵向车速计算式如下式(12)所示:
Vlon=Vlon_max*(yp-Hmin)/(Hmax-Hmin) (12)
式中,Vlon_max表示车辆的纵向车速最大值,Vlon表示车辆的纵向车速;
步骤244,根据横向车速规划和纵向车速规划,按照式(13)计算车辆防撞路径C(t)对应的目标车速Vtar(t);
Vtar(t)=min{Vlat,Vlon} (13)
重复步骤241至步骤244得到防撞路径C(t)对应的目标车速集合{Vtar(1),Vtar(2),…Vtar(t)}。
2.如权利要求1所述的用于循迹自动驾驶车辆的防撞路径规划与控制方法,其特征在于,步骤21具体包括:
步骤211,通过如下式(1)示出的插值方式计算预瞄距离初始值dp_ρ
Figure FDA0003250504010000031
式中,K1,K2为曲率预瞄系数,且K1<0,K2<0;C1,C2为常数项;ρcon1为预设值;
步骤212,通过如下式(2)计算预瞄距离修正值dp_v
dp_v=K*v (2)
式中,K为车速预瞄系数;
步骤213,通过式(3)计算预瞄距离:
dp=dp_ρ+dp_v (3)。
3.如权利要求1所述的用于循迹自动驾驶车辆的防撞路径规划与控制方法,其特征在于,步骤22具体包括:
步骤221,计算循迹路径上距离车辆当前位置Ai(xi,yi)最近的点坐标Ar_i(xr_i,yr_i);
步骤222,计算点Ar_i和车辆前方循迹路径上的点Aj的距离Dij,如式(4)示出地选择dp与Dij差值绝对值最小的对应车辆前方循迹路径上的点为预瞄点Pi_p(xi_p,yi_p);
Figure FDA0003250504010000041
4.如权利要求1所述的用于循迹自动驾驶车辆的防撞路径规划与控制方法,其特征在于,步骤23具体包括:
步骤231,以车辆实际位置点为起点,采用如下坐标变换公式(5)和(6),将预瞄点Pi_p(xi_p,yi_p)通过坐标变换转换到车辆坐标系xOy中的Pi_p_V(xi_p_V,yi_p_V):
xi_p_V=Δx cosΨi-Δy sinΨi (5)
yi_p_V=Δx sinΨi+Δy cosΨi (6)
式中,Δx=xi_p-xi,Δy=yi_p-yi,Ψi表示车辆实际航向角;
步骤232,以Pi_p_V(xi_p_V,yi_p_V)为参考点,根据障碍物位置信息,按照式(7)和(8)提供的方式设置不同横向偏移规划防撞路径的终点位置Pn(xn,yn):
xn=xi_p_V+n*ΔH*sin(θi) (7)
yn=yi_p_V+n*ΔH*cos(θi) (8)
其中,ΔH为防撞路径终点距离参考点的横向基本偏移量,n为非零整数,θi为车辆循迹轨迹上预瞄点Pi_p对应的车辆航向角;
步骤233,通过式(9)所示的四个控制点确定三阶贝塞尔曲线,作为防撞路径C(t):
Figure FDA0003250504010000042
式中,Δs为防撞曲线调整参数,
Figure FDA0003250504010000043
5.如权利要求1所述的用于循迹自动驾驶车辆的防撞路径规划与控制方法,其特征在于,如果网格被占用,则认为网格处于激活转态,f(i)=1;若果网格未被占用,则认为网格未被激活,f(i)=0。
6.如权利要求1所述的用于循迹自动驾驶车辆的防撞路径规划与控制方法,其特征在于,步骤3具体包括:
若所规划的防撞路径对应的目标车速Vtar(t)均等于0,则车辆采用预设减速度沿当前循迹路径进行避障;
若所规划的防撞路径有一条路径对应的目标车速Vtar(t)不等于0,则车辆选择该防撞路径防止碰撞的发生;
若所规划的防撞路径有两条或以上路径对应的目标车速Vtar(t)不等于0,则优先选择左换道进行防撞,若存在两条或以上做换道路径,则优先选择靠近当前循迹路径的防撞路径。
7.一种用于循迹自动驾驶车辆的防撞路径规划与控制系统,其特征在于,包括:
信息采集装置,其用于获取车辆相关信息及障碍物信息;
防撞路径规划装置,其用于规划防撞路径;
控制装置,其用于根据所规划的防撞路径对应的目标车速,选择最佳防撞方式;
其中,防撞路径规划装置具体包括:
预瞄距离计算单元,其用于根据当前车速v和道路曲率ρ,计算预瞄距离;
预瞄点计算单元,其用于根据预瞄距离dp和当前车辆位置(xi,yi),选取车辆前方循迹路径上与车辆之间的距离最接近预瞄距离的采样点作为预瞄点;
障碍物识别与防撞路径规划单元,其用于根据车载激光雷达点云信息解析车辆循迹路径上是否存在障碍物,若不存在障碍物,则按照循迹路径继续行驶;若有障碍物,则进行防撞路径规划;
车辆控制参数计算单元,其用于在车辆坐标系xOy中,将车辆前方纵向长度H、横向宽度L的区域划分成等大小的网格,并给每个网格进行编号,根据激光雷达获取的前方障碍物点云信息确定网格的占用状态,取障碍物识别与防撞路径规划单元所规划的防撞路径上的等间隔离散点,根据各离散点及其邻近区域内网格状态对车辆控制参数进行实时计算;
车辆控制参数计算单元包括:
网格编号计算子单元,其用于根据障碍物识别与防撞路径规划单元所规划的防撞路径C(t)上的所述等间隔离散点P在车辆坐标系xOy中的坐标,利用式(10)计算点P沿x轴方向所对应的网格编号:
Index(P)=yIndex*n+xIndex (10)
式中,
Figure FDA0003250504010000061
(xp,yp)为点P在车辆坐标系xOy中的坐标,Δ为每个网格的长和宽,横向网格数n=L/Δ,纵向网格数m=H/Δ;
横向车速规划子单元,其用于根据点P在沿x轴方向的邻近区域内网格激活状态进行横向车速规划,该邻近区域车载激光雷达探测的横向安全距离Lsafe为边界,横向最大安全距离为Lmax,横向最小安全距离为Lmin
若点P沿x轴方向的邻近区域
Figure FDA0003250504010000062
内的有网格被激活,则Vlat=0;
若点P沿x轴方向的邻近区域
Figure FDA0003250504010000063
内的无网格被激活,则Vlat=Vlat_max
若点P有且仅有沿x轴方向的邻近区域
Figure FDA0003250504010000064
Figure FDA0003250504010000065
内的网格被激活,横向车速计算式如下式(11)所示:
Figure FDA0003250504010000066
式中,xac为该邻近区域
Figure FDA0003250504010000071
内距离点P最近的激活网格中心点横坐标,Vlat_max表示车辆的横向车速最大值,Vlat表示车辆的横向车速;
纵向车速规划子单元,其用于根据点P与车载激光雷达探测的纵向安全距离相对位置关系进行纵向车速规划,车载激光雷达探测的纵向安全距离为Hsafe,其最大值为Hmax,最小值为Hmin
若0≤yp≤Hmin,Vlon=0;
若Hmax≤yp,Vlon=0;
若Hmin<yp<Hmax,纵向车速计算式如下式(12)所示:
Vlon=Vlon_max*(yp-Hmin)/(Hmax-Hmin) (12)
式中,Vlon_max表示车辆的纵向车速最大值,Vlon表示车辆的纵向车速;
目标车速计算子单元,其用于根据横向车速规划和纵向车速规划,按照式(13)计算车辆防撞路径C(t)对应的目标车速Vtar(t)及目标车速集合{Vtar(1),Vtar(2),…Vtar(t)};
Vtar(t)=min{ylat,Vlon} (13)。
8.如权利要求7所述的用于循迹自动驾驶车辆的防撞路径规划与控制系统,其特征在于,障碍物识别与防撞路径规划单元包括:
坐标转换子单元,其用于以车辆实际位置点为起点,采用如下坐标变换公式(5)和(6),将预瞄点Pi_p(xi_p,yi_p)通过坐标变换转换到车辆坐标系xOy中的Pi_p_V(xi_p_V,yi_p_V):
xi_p_V=Δx cosWi-Δy sinΨi (5)
yi_p_V=Δx sinWi+Δy cosWi (6)
式中,Δx=xi_p-xi,Δy=yi_p-yi,Ψi表示车辆实际航向角;
终点设置子单元,其用于以Pi_p_V(xi_p_V,yi_p_V)为参考点,根据障碍物位置信息,按照式(7)和(8)提供的方式设置不同横向偏移规划防撞路径的终点位置Pn(xn,yn):
xn=xi_p_V+n*ΔH*sin(θi) (7)
yn=yi_p_V+n*ΔH*cos(θi) (8)
其中,ΔH为防撞路径终点距离参考点的横向基本偏移量,n为非零整数,θi为车辆循迹轨迹上预瞄点Pi_p对应的车辆航向角;
防撞曲线计算子单元,其用于通过式(9)所示的四个控制点确定三阶贝塞尔曲线,作为防撞路径C(t):
Figure FDA0003250504010000081
式中,Δs为防撞曲线调整参数,
Figure FDA0003250504010000082
CN202110335084.4A 2021-03-29 2021-03-29 用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统 Active CN112937606B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110335084.4A CN112937606B (zh) 2021-03-29 2021-03-29 用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110335084.4A CN112937606B (zh) 2021-03-29 2021-03-29 用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统

Publications (2)

Publication Number Publication Date
CN112937606A CN112937606A (zh) 2021-06-11
CN112937606B true CN112937606B (zh) 2021-10-26

Family

ID=76227270

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110335084.4A Active CN112937606B (zh) 2021-03-29 2021-03-29 用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统

Country Status (1)

Country Link
CN (1) CN112937606B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113377112B (zh) * 2021-06-30 2023-05-05 东风商用车有限公司 一种自动驾驶速度规划及状态协调方法和装置
CN113655801B (zh) * 2021-08-23 2022-04-05 紫清智行科技(北京)有限公司 一种园区智能车辆自动驾驶系统架构及循迹控制方法
CN113504782B (zh) * 2021-09-09 2022-02-18 北京智行者科技有限公司 障碍物防撞方法、装置、系统和移动工具
CN114261389B (zh) * 2021-11-29 2023-05-12 东风商用车有限公司 自动驾驶商用车的行驶轨迹碰撞风险分析方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109976321A (zh) * 2017-12-27 2019-07-05 湖南中车时代电动汽车股份有限公司 一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统
DE102018205242A1 (de) * 2018-04-09 2019-10-10 Bayerische Motoren Werke Aktiengesellschaft Bewegungsplanung mittels invariant sicherer Zustände eines Kraftfahrzeugs
CN110794820B (zh) * 2018-07-17 2023-07-07 宇通客车股份有限公司 一种智能车速度跟踪方法及系统
CN109263639B (zh) * 2018-08-24 2020-09-01 武汉理工大学 基于状态栅格法的驾驶路径规划方法
CN110471289B (zh) * 2019-08-28 2021-06-04 湖南大学 一种基于视觉导航移动设备的自适应路径跟踪方法及系统
CN111256719B (zh) * 2020-02-18 2022-02-22 北京九曜智能科技有限公司 一种绕障方法及装置

Also Published As

Publication number Publication date
CN112937606A (zh) 2021-06-11

Similar Documents

Publication Publication Date Title
CN112937606B (zh) 用于循迹自动驾驶车辆的防撞路径规划与控制方法及系统
CN105292116B (zh) 自动驾驶车辆的车道变换路径规划算法
CN107646114B (zh) 用于估计车道的方法
CN111750886B (zh) 局部路径规划方法及装置
US20210188356A1 (en) Vehicle control device
US20210188258A1 (en) Vehicle control device
US8825265B1 (en) Approach for consolidating observed vehicle trajectories into a single representative trajectory
US20210188262A1 (en) Vehicle control device
JP6696593B2 (ja) 走行履歴の記憶方法、走行軌跡モデルの生成方法、自己位置推定方法、及び走行履歴の記憶装置
US20200238980A1 (en) Vehicle control device
CN109426261B (zh) 自动驾驶装置
Kim et al. Design of integrated risk management-based dynamic driving control of automated vehicles
US20200317192A1 (en) Vehicle control device
JP7422661B2 (ja) 走行軌跡補正方法、走行制御方法、及び走行軌跡補正装置
US20200353918A1 (en) Vehicle control device
CN113165648A (zh) 用于基于采样对机动车辆的可能轨迹进行规划的控制系统和控制方法
US20200317266A1 (en) Vehicle controller
CN111006667B (zh) 高速场景下的自动驾驶轨迹生成系统
CN112400193B (zh) 行驶环境信息的生成方法、驾驶控制方法、行驶环境信息生成装置
US20210325197A1 (en) Vehicle control apparatus
US11042759B2 (en) Roadside object recognition apparatus
US20200391747A1 (en) Vehicle control device
CN113498519A (zh) 用于识别拐入车辆或拐出车辆的方法及控制单元
Ben-Messaoud et al. Smooth obstacle avoidance path planning for autonomous vehicles
CN112498347A (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