CN110077397B - 一种智能车避障轨迹规划方法及装置 - Google Patents
一种智能车避障轨迹规划方法及装置 Download PDFInfo
- Publication number
- CN110077397B CN110077397B CN201910399631.8A CN201910399631A CN110077397B CN 110077397 B CN110077397 B CN 110077397B CN 201910399631 A CN201910399631 A CN 201910399631A CN 110077397 B CN110077397 B CN 110077397B
- Authority
- CN
- China
- Prior art keywords
- obstacle avoidance
- grid
- vehicle
- target
- lattice
- 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
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000001133 acceleration Effects 0.000 claims description 192
- 230000004888 barrier function Effects 0.000 claims description 13
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
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
- B60W30/00—Purposes 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
- B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
- B60W30/09—Taking automatic action to avoid collision, e.g. braking and steering
-
- 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
- B60W30/00—Purposes 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
- B60W30/14—Adaptive cruise control
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/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
-
- 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
-
- 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/80—Spatial relation or speed relative to objects
-
- 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/80—Spatial relation or speed relative to objects
- B60W2554/801—Lateral distance
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明实施例提供了一种智能车避障轨迹规划方法,根据每个障碍物的位置得到所有的避障格的位置;根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置,得到至少一个避障格路径;将避障格代价最小的避障格路径作为目标避障格路径;根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹;根据任一个目标避障格的位置、在任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到任一个目标避障格的避障轨迹;根据目标避障格路径中所有目标避障格对应的避障轨迹,得到目标避障格路径对应的目标避障轨迹。本发明实施例根据障碍物的速度规划路径,提高了避障的准确性。
Description
技术领域
本发明涉及自动驾驶技术领域,特别涉及一种智能车避障轨迹规划方法及装置。
背景技术
面对复杂的交通环境中,在自动驾驶过程中,可能会出现避障不及时而发生事故。因此,需要为智能车进行避障轨迹的规划。
现有技术中,智能车避障轨迹的规划一般为先建立环境模型再检测是否存在障碍物,从而针对障碍物的位置进行路径的规划。
在实现本发明的过程中,发明人发现相关技术中至少存在以下问题:
相关技术中没有考虑障碍物的速度进行路径的规划,降低了智能车避障的准确性。
发明内容
有鉴于此,本发明实施例提供了一种智能车避障轨迹规划方法及装置,能够根据障碍物的运动状态进行路径的规划。所述技术方案如下:
一种智能车避障轨迹规划方法,所述方法包括:
获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度;
根据每个所述障碍物的位置得到所有的避障格的位置,每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,每个所述避障格路径包括:从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格;
计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹;
对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度,得到该任一个目标避障格的避障轨迹;
根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹。
可选的,在所述获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度之前,所述方法还包括:
判断是自车周围是否存在障碍物;
当存在所述障碍物时,判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能;
若是,则执行获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度的步骤。
可选的,所述判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能,包括:
获取相邻的障碍物的速度;
将所述自车的速度与相邻的障碍物的速度进行比较,若相邻的障碍物的速度小于所述自车的速度,则判定有发生碰撞的可能;
若相邻的障碍物的速度大于所述自车的速度,则判定没有发生碰撞的可能。
可选的,所述根据每个所述障碍物的位置得到所有的避障格的位置,包括:
获取所述自车的尺寸;
基于所述自车的尺寸设置避障格的目标尺寸;
在每一障碍物的两侧的预设距离处设置目标尺寸的避障格,得到所有避障格的位置,其中所述避障格对应的位置无障碍物。
可选的,所述根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,包括:
在所有所述避障格之中确定所有最终避障格,其中每个所述最终避障格为从其开始直行而无碰撞障碍物的可能性的避障格;
对于每个所述最终避障格的位置,确定其和所述自车的位置之间在行驶方向上存在的中间避障格的位置,进而得到所有的可能路径,其中每个可能路径不包括中间避障格或者包括至少一个中间避障格,其中每个可能路径中的任两个中间避障格都不属于同一个障碍物两侧的两个避障格;
对于每个可能路径,当所述自车的位置和下一个经过的避障格的位置之间的可能行车区域中以及每一个中间避障格的位置与下一个经过的避障格的位置之间的可能行车区域中均不存在障碍物,且当所述自车的当前位置和下一个经过的避障格的位置之间的纵向距离以及每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离均不小于避障所需的最小纵向距离时,将其作为避障格路径,所述最小纵向距离是根据所述自车的速度和所述障碍物的速度得到。
可选的,所述计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径,包括:
当所述避障格为第一个避障格时,根据所述自车的位置和所述第一避障格的位置,得到所述自车与所述第一个避障格之间的横向距离;
当所述避障格为第一个避障格之外的任一避障格时,根据所述任一避障格的位置以及与所述任一避障格相邻的避障格的位置,得到两相邻避障格之间的横向距离;
将自车与所述第一个避障格之间的横向距离或两相邻避障格之间横向距离、所述障碍物的速度输入到第一公式中,得到所述避障格对应的避障格代价,所述第一公式为:
χ=χw×σw+χv×σv
σw=w/wm
σv=vb/vm
其中,w表示自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离,vb为避障格的速度,wm为预先设定的距离值,vm为预先设定的速度值,σw为相邻避障格与自车之间横向距离对应的代价函数,σv表示避障格的速度对应的代价函数,χw为避障格与自车之间的横向距离对应的权重系数,χv为避障格的速度对应的权重系数,χ为避障格代价;
将每个避障格路径中的避障格所对应的避障格代价相加,得到每个避障格路径的避障格代价;
将所有避障格路径的避障格代价进行比较,得到避障格代价最小的避障格路径,将避障格代价最小的避障格路径作为目标避障格路径。
可选的,所述对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹,包括:
基于预设的标准时刻设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻,T1为第一时长,T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到自车距离所述第一目标避障格预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为避障轨迹中自车距离所述第一目标避障格预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述自车的位置和所述第一目标避障格的位置,得到所述自车与所述第一目标避障格之间实际的横向距离;
判断所述自车距离所述第一目标避障格预测的横向距离是否等于所述自车与所述第一目标避障格之间实际的横向距离,当所述自车距离所述第一目标避障格预测的横向距离不等于所述自车与所述第一目标避障格之间实际的横向距离时,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度,直到所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时,获取所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到自车的第一目标避障格的避障轨迹,所述避障轨迹公式为:
其中,ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
可选的,所述对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹,包括:
基于预设的标准时长设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻、T1为第一时长、T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到两相邻目标避障格之间的预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为两相邻目标避障格之间的预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述两相邻目标避障格的位置,得到所述两相邻目标避障格之间的实际的横向距离;
判断所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,当所述两相邻目标避障格的预测的横向距离不等于所述两相邻目标避障格之间实际的横向距离,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,直到所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,获取所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到所述第一个目标避障格之外的任一个目标避障格的避障轨迹,所述避障轨迹公式为:
其中ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
可选的,根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹,包括:将所述目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线相连,得到目标避障格路径对应的目标避障轨迹。
本发明实施例提供了一种智能车避障轨迹规划装置,所述装置包括:
获取模块,用于获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度;
第一确定模块,用于根据每个所述障碍物的位置得到所有的避障格的位置,每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
第二确定模块,用于根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,每个所述避障格路径包括:从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格;
计算模块,用于计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
第三确定模块,用于对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹;
第四确定模块,用于对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度,得到该任一个目标避障格的避障轨迹;
第五确定模块,用于根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹。
可选的,所述装置还包括:判断模块,用于:
判断是自车周围是否存在障碍物;
当存在所述障碍物时,判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能;
若是,则执行获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度的步骤。
可选的,所述判断模块,还用于:
获取相邻的障碍物的速度;
将所述自车的速度与相邻的障碍物的速度进行比较,若相邻的障碍物的速度小于所述自车的速度,则判定有发生碰撞的可能;
若相邻的障碍物的速度大于所述自车的速度,则判定没有发生碰撞的可能。
可选的,所述第一确定模块,还用于:
获取所述自车的尺寸;
基于所述自车的尺寸设置避障格的目标尺寸;
在每一障碍物的两侧的预设距离处设置目标尺寸的避障格,得到所有避障格的位置,其中所述避障格对应的位置无障碍物。
可选的,所述第二确定模块,还用于:
在所有所述避障格之中确定所有最终避障格,其中每个所述最终避障格为从其开始直行而无碰撞障碍物的可能性的避障格;
对于每个所述最终避障格的位置,确定其和所述自车的位置之间在行驶方向上存在的中间避障格的位置,进而得到所有的可能路径,其中每个可能路径不包括中间避障格或者包括至少一个中间避障格,其中每个可能路径中的任两个中间避障格都不属于同一个障碍物两侧的两个避障格;
对于每个可能路径,当所述自车的位置和下一个经过的避障格的位置之间的可能行车区域中以及每一个中间避障格的位置与下一个经过的避障格的位置之间的可能行车区域中均不存在障碍物,且当所述自车的当前位置和下一个经过的避障格的位置之间的纵向距离以及每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离均不小于避障所需的最小纵向距离时,将其作为避障格路径,所述最小纵向距离是根据所述自车的速度和所述障碍物的速度得到。
可选的,所述计算模块,还用于:
当所述避障格为第一个避障格时,根据所述自车的位置和所述第一避障格的位置,得到所述自车与所述第一个避障格之间的横向距离;
当所述避障格为第一个避障格之外的任一避障格时,根据所述任一避障格的位置以及与所述任一避障格相邻的避障格的位置,得到两相邻避障格之间的横向距离;
将自车与所述第一个避障格之间的横向距离或两相邻避障格之间横向距离、所述障碍物的速度输入到第一公式中,得到所述避障格对应的避障格代价,所述第一公式为:
χ=χw×σw+χv×σv
σw=w/wm
σv=vb/vm
其中,w表示自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离,vb为避障格的速度,wm为预先设定的距离值,vm为预先设定的速度值,σw为相邻避障格与自车之间横向距离对应的代价函数,σv表示避障格的速度对应的代价函数,χw为避障格与自车之间的横向距离对应的权重系数,χv为避障格的速度对应的权重系数,χ为避障格代价;
将每个避障格路径中的避障格所对应的避障格代价相加,得到每个避障格路径的避障格代价;
将所有避障格路径的避障格代价进行比较,得到避障格代价最小的避障格路径,将避障格代价最小的避障格路径作为目标避障格路径。
可选的,所述第三确定模块,还用于:
基于预设的标准时刻设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻,T1为第一时长,T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到自车距离所述第一目标避障格预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为避障轨迹中自车距离所述第一目标避障格预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述自车的位置和所述第一目标避障格的位置,得到所述自车与所述第一目标避障格之间实际的横向距离;
判断所述自车距离所述第一目标避障格预测的横向距离是否等于所述自车与所述第一目标避障格之间实际的横向距离,当所述自车距离所述第一目标避障格预测的横向距离不等于所述自车与所述第一目标避障格之间实际的横向距离时,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度,直到所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时,获取所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到自车的第一目标避障格的避障轨迹,所述避障轨迹公式为:
其中,ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
可选的,所述第四确定模块,还用于:
基于预设的标准时长设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻、T1为第一时长、T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到两相邻目标避障格之间的预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为两相邻目标避障格之间的预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述两相邻目标避障格的位置,得到所述两相邻目标避障格之间的实际的横向距离;
判断所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,当所述两相邻目标避障格的预测的横向距离不等于所述两相邻目标避障格之间实际的横向距离,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,直到所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,获取所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到所述第一个目标避障格之外的任一个目标避障格的避障轨迹,所述避障轨迹公式为:
其中ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
可选的,所述第五确定模块,还用于:
将所述目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线相连,得到目标避障格路径对应的目标避障轨迹
本发明实施例提供的技术方案带来的有益效果是:
本发明实施例提供了一种智能车避障轨迹规划方法,获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度;根据每个所述障碍物的位置得到所有的避障格的位置,每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子;根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,每个所述避障格路径包括:从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格;计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹;对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度,得到该任一个目标避障格的避障轨迹;根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹。本发明实施例中能够根据障碍物的速度进行路径的规划,提高了智能车避障的准确性。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种智能车避障轨迹规划方法的流程图;
图2是本发明实施例提供的一种智能车避障轨迹规划方法的流程图;
图3是本发明实施例提供的障碍物对应的避障格的示意图;
图4是本发明实施例提供的可能行车区域的示意图;
图5是本发明实施例提供的目标避障轨迹的示意图;
图6是本发明实施例提供的一种智能车避障轨迹规划装置的框图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明实施方式作进一步地详细描述。
图1是根据一示例性实施例示出的一种智能车避障轨迹规划方法的流程图,如图1所示,该方法包括:
在步骤S101中,获取自车的位置、自车的速度、自车周围相关的障碍物的位置和每个障碍物的速度;
在步骤S102中,根据每个障碍物的位置得到所有的避障格的位置,每个避障格为与相对应的障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
在步骤S103中,根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置,得到至少一个避障格路径,每个避障格路径包括:从自车的当前位置起避过所有相关的障碍物所依次经过的每个避障格的顺序以及对应的避障格;
在步骤S104中,计算每个避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
在步骤S105中,对于目标避障格路径中自车经过的第一个目标避障格,根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹;
在步骤S106中,对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹;
在步骤S107中,根据目标避障格路径中所有目标避障格对应的避障轨迹,得到目标避障格路径对应的目标避障轨迹。
本发明实施例提供了一种智能车避障轨迹规划方法,获取自车的位置、自车的速度、自车周围相关的障碍物的位置和每个障碍物的速度;根据每个障碍物的位置得到所有的避障格的位置,每个避障格为与相对应的障碍物的一侧相距预设距离且无障碍物的预设大小的格子;根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置,得到至少一个避障格路径,每个避障格路径包括:从自车的当前位置起避过所有相关的障碍物所依次经过的每个避障格的顺序以及对应的避障格;计算每个避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;对于目标避障格路径中自车经过的第一个目标避障格,根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹;对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹;根据目标避障格路径中所有目标避障格对应的避障轨迹,得到目标避障格路径对应的目标避障轨迹。本发明实施例中能够根据障碍物的速度进行路径的规划,提高了智能车避障的准确性。
图2是根据再一示例性实施例示出的一种智能车避障轨迹规划方法,如图2所示,该方法可以包括:
在步骤S201中,判断自车周围是否存在障碍物;
在本发明的实施例中,可以通过雷达、传感器来获取周围的障碍物。
在步骤S202中,当存在障碍物时,判断自车与同一车道上的相邻的障碍物是否有发生碰撞的可能;若是,则执行步骤S203,若否,则执行步骤S201;
其中,判断自车与同一车道上的相邻的障碍物是否有发生碰撞的可能,包括:获取相邻的障碍物的速度;将自车的速度与相邻的障碍物的速度进行比较,若相邻的障碍物的速度小于自车的速度,则判定有发生碰撞的可能;若相邻的障碍物的速度大于自车的速度,则判定没有发生碰撞的可能。
需要说明的是,速度有正负之分,当沿自车的行驶方向的行驶的障碍物的速度为正,与自车的行驶方向相反行驶的障碍物的速度为负,静止的障碍物的速度为零。
本发明的一些可能的实施例中,障碍物可以为道路上的车辆以及道路上的其他物品等,道路上可以检测到的东西都可以作为障碍物。
在步骤S203中,获取自车的位置、自车的速度、自车周围相关的障碍物的位置和每个障碍物的速度;
需要说明的是,自车周围相关的障碍物可以为自车在当前位置上能够检测到的障碍物。
在步骤S204中,根据每个障碍物的位置得到所有的避障格的位置,每个避障格为与相对应的障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
其中,根据每个障碍物的位置得到所有的避障格的位置,包括:
获取自车的尺寸;基于自车的尺寸设置避障格的目标尺寸;在每一障碍物的两侧的预设距离处设置目标尺寸的避障格,得到所有避障格的位置,其中避障格对应的位置无障碍物。
如图3所示,图中C表示自车,Z1、Z2、Z3表示障碍物,S1、S2、S3、S4、S5分别表示避障格,避障格S1、避障格S2是根据障碍物Z1的位置进行设定的,避障格S3、避障格S4是根据障碍物Z2的位置进行设定的,避障格S5是根据障碍物Z3的位置进行设定的,由于障碍物Z3的一侧为道路边缘,没有足够的位置设置障碍物,也就不满足每一障碍物的两侧的预设距离处设置目标尺寸的避障格的要求,因此,只在障碍物Z3的一侧设置有障碍物S5。
需要说明的是,自车的尺寸为预先获取并进行存储的,避障格的目标尺寸可以为自车的尺寸相同。
在步骤S205中,在所有避障格之中确定所有最终避障格,其中每个最终避障格为从其开始直行而无碰撞障碍物的可能性的避障格。
需要说明的是,当避障格为最终避障格时,可以认为在该最终避障格时自车的避障结束,在从该最终避障格开始直行不会碰撞障碍物。
在步骤S206中,对于每个最终避障格的位置,确定其和自车的位置之间在行驶方向上存在的中间避障格的位置,进而得到所有的可能路径,其中每个可能路径不包括中间避障格或者包括至少一个中间避障格,其中每个可能路径中的任两个中间避障格都不属于同一个障碍物两侧的两个避障格。
需要说明的是,得到的所有可能路径为在不考虑障碍物的情况下得到的自车到最终避障格的可能路径。
在步骤S207中,对于每个可能路径,当自车的位置和下一个经过的避障格的位置之间的可能行车区域中以及每一个中间避障格的位置与下一个经过的避障格的位置之间的可能行车区域中均不存在障碍物,且当自车的当前位置和下一个经过的避障格的位置之间的纵向距离以及每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离均不小于避障所需的最小纵向距离时,将其作为避障格路径,该最小纵向距离是根据所述自车的速度和所述障碍物的速度得到。
需要说明的是,在本申请中,自车的当前位置和下一个经过的避障格的位置之间的纵向距离指自车的中心位置到避障格的中心位置之间的纵向距离,每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离是指彼此相邻的每两个避障格中心之间的纵向距离。
如图4所示,可能行车区域为图中的灰色区域,该可能行车区域为自车和障碍物之间所形成的矩形区域。
在本发明的一些实施例中,最小纵向距离根据自车的速度和障碍物的速度得到的方式为:
将所述横摆角速度、所述自车的速度、所述障碍物的速度、轨迹的开始时刻和轨迹的结束时刻输入到纵向距离公式中,得到避障所需的最小纵向距离,所述纵向距离公式为:
其中,ta为轨迹的开始时刻,tl为轨迹的结束时刻,θd(t)为t时刻横摆角速度,νz为自车的速度,νs为障碍物的速度,S为最小纵向距离。
本发明实施例中的横摆角速度、轨迹的开始时刻和轨迹的结束时刻与后续计算横摆角速度、轨迹的开始时刻和轨迹的结束时刻的步骤相同,不再赘述。
需要说明的是,到任一个避障格所需要的最小避障距离需要获取到该任一个避障格避障对应的横摆角速度、轨迹的开始时刻和轨迹的结束时刻,例如:计算到第一个避障格避障所需的最小纵向距离需要获取自车与第一个避障格避障对应的横摆角速度、轨迹的开始时刻和轨迹的结束时刻,计算到除第一个避障格之外的任一避障格所需的最小纵向距离需要获取到该任一避障格避障对应的横摆角速度、轨迹的开始时刻和轨迹的结束时刻。
本发明的一些实施例中,根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置,得到至少一个避障格路径通过上述步骤S205-步骤S207实现,在本发明的另一些实施例中,也可以通过以下方式得到至少一个避障格路径:
确定自车从所有避障格中的任一避障格开始直行无碰撞障碍物的可能所对应的任一避障格,将该任一避障格作为最终避障格,得到所有最终避障格;
选择自车的行驶方向上自车的位置与所有避障格的位置之间形成可能路径所对应的避障格以及自车与所有避障格之间形成可能路径;
在所有的可能路径中选择满足第一预设条件和第二预设条件的可能路径以及对应的预选避障格;
从预选避障格中选择不属于最终避障格的预选避障格,将从预选避障格中选择不属于最终避障格的预选避障格作为可行性避障格;
基于每个可行性避障格,选择自车的行驶方向上每个可行性避障格与位于该可行性避障格前方的所有避障格之间形成可能路径所对应的避障格以及每个可行性避障格与位于该可行性避障格前方的所有避障格之间形成可能路径,可行性避障格前方为自车的行驶方向;
在所有的可能路径所对应的避障格中选择满足第一预设条件和第二预设条件的可能路径以及对应的预选避障格;
从预选避障格中选择不属于最终避障格的预选避障格,将从预选避障格中选择不属于最终避障格的预选避障格作为可行性避障格;
对于可行性避障格,循环执行基于每个可行性避障格,选择自车的行驶方向上每个可行性避障格与位于该可行性避障格前方的所有避障格之间形成可能路径所对应的避障格以及每个可行性避障格与位于该可行性避障格前方的所有避障格之间形成可能路径,可行性避障格前方为自车的行驶方向;在所有的可能路径所对应的避障格中选择满足第一预设条件和第二预设条件的可能路径以及对应的预选避障格;从预选避障格中选择不属于最终避障格的预选避障格,将从预选避障格中选择不属于最终避障格的预选避障格作为可行性避障格的步骤,直至可行性避障格全部为最终避障格为止;
基于所有可能路径以及可行避障格,得到至少一个避障格路径。
其中,第一预设条件为:任一避障格与自车之间的可能行车区域中不存在障碍物或者任一避障格与其他避障格之间的可能行车区域中不存在障碍物的可能路径;
第二预设条件为:任一避障格与自车之间的纵向距离不小于避障所需的最小纵向距离的可能路径或者任一避障格与其他避障格之间的纵向距离不小于避障所需的最小纵向距离的可能路径,该最小纵向距离是根据自车的速度和障碍物的速度得到。
需要说明的是,基于所有可能路径以及预选避障格,得到至少一个避障格路径的方式可以为:从所有的可能路径中,得到可能路径所对应的可行避障格为同一避障格时,将该可能路径以该可行避障格为连接点进行连接得到至少一个避障格路径,对于每个避障格路径,直至连接得到的路径的起点为自车,路径的最后一个避障格为最终避障格为止。
在步骤S208中,计算每个避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
在本发明的一些实施例中,计算每个避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径,包括:
当避障格为第一个避障格时,根据自车的位置和第一避障格的位置,得到自车与第一个避障格之间的横向距离;
当避障格为第一个避障格之外的任一避障格时,根据任一避障格的位置以及与任一避障格相邻的避障格的位置,得到两相邻避障格之间的横向距离;
将自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离、障碍物的速度输入到第一公式中,得到避障格对应的避障格代价,第一公式为:
χ=χw×σw+χv×σv
σw=w/wm
σv=vb/vm
其中,w表示自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离,vb为避障格的速度,wm为预先设定的距离值,vm为预先设定的速度值,σw为相邻避障格与自车之间横向距离对应的代价函数,σv表示避障格的速度对应的代价函数,χw为避障格与自车之间的横向距离对应的权重系数,χv为避障格的速度对应的权重系数,χ为避障格代价;
将每个避障格路径中的避障格所对应的避障格代价相加,得到每个避障格路径的避障格代价;
将所有避障格路径的避障格代价进行比较,得到避障格代价最小的避障格路径,将避障格代价最小的避障格路径作为目标避障格路径。
在步骤S209中,对于目标避障格路径中自车经过的第一个目标避障格,根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹。
在本发明的一些实施例中,对于目标避障格路径中自车经过的第一个目标避障格,根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹,包括:
基于预设的标准时刻设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
在本发明的一些实施例中,预设的标准时刻和标准的横摆角加速度为预先设定的值。
将第一时长和横摆角加速度的预设值输入到横摆角加速度公式中,得到横摆角加速度,横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻,T1为第一时长,T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,横摆角速度的公式为:
将自车的速度和横摆角速度输入到横向距离公式中,得到自车距离第一目标避障格预测的横向距离,横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为避障轨迹中自车距离第一目标避障格预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据自车的位置和第一目标避障格的位置,得到自车与第一目标避障格之间实际的横向距离;
判断自车距离第一目标避障格预测的横向距离是否等于自车与第一目标避障格之间实际的横向距离,当自车距离第一目标避障格预测的横向距离不等于自车与第一目标避障格之间实际的横向距离时,按照初始值设定公式更新第一时长和横摆角加速度的预设值,将更新后的第一时长和更新后的横摆角加速度的预设值作为第一时长和横摆角加速度,再次将第一时长和横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度,直到自车距离第一目标避障格预测的横向距离等于自车与第一目标避障格之间实际的横向距离为止,初始值设定公式为:
需要说明的是,将更新后的第一时长和更新后的横摆角加速度的预设值作为第一时长和横摆角加速度的预设值,再次将第一时长和横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度后,并依次执行后面的步骤,直到自车距离第一目标避障格预测的横向距离等于自车与第一目标避障格之间实际的横向距离为止。
当自车距离第一目标避障格预测的横向距离等于自车与第一目标避障格之间实际的横向距离时,获取自车距离第一目标避障格预测的横向距离等于自车与第一目标避障格之间实际的横向距离时的横摆角速度;
需要说明的是,在实际计算的过程中,可以在自车距离第一目标避障格预测的横向距离与自车距离第一目标避障格之间实际的横向距离的差值小于一定阈值时,获取得到的横摆角速度。
在本发明实施例中,通过横向距离来确定横摆角速度,当确定的横摆角速度可以使自车的横摆角满足横向距离要求时,由于在确定避障格的路径时所选取的避障格能够满足自车到避障格之间的纵向距离大于等于避障所需的最小纵向距离,需要在自车的中心与避障格的中心之间的距离为避障所需的最小纵向距离时开始按照避障轨迹进行避障,在避障完成后,自车能够到达对应的避障格的位置上。
将横摆角速度输入到避障轨迹公式中,得到自车的第一目标避障格的避障轨迹,避障轨迹公式为:
其中,ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
在步骤S210中,对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹;
在本发明的一些实施例中,对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据目标避障格的位置、与目标避障格相邻且位于目标避障格前方的目标避障格的位置和自车的速度,得到第一个目标避障格之外的任一个目标避障格的避障轨迹,包括:
基于预设的标准时长设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值。
在本发明的一些实施例中,预设的标准时刻和标准的横摆角加速度为预先设定的值。
将第一时长和横摆角加速度的预设值输入到横摆角加速度公式中,得到横摆角加速度,横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻、T1为第一时长、T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,横摆角速度的公式为:
将自车的速度和横摆角速度输入到横向距离公式中,得到两相邻目标避障格之间的预测的横向距离,横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为两相邻目标避障格之间的预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据两相邻目标避障格的位置,得到两相邻目标避障格之间的实际的横向距离;
判断两相邻目标避障格的预测的横向距离是否等于两相邻目标避障格之间实际的横向距离时,当两相邻目标避障格的预测的横向距离不等于两相邻目标避障格之间实际的横向距离,按照初始值设定公式更新第一时长和横摆角加速度的预设值,将更新后的第一时长和更新后的横摆角加速度的预设值作为第一时长和横摆角加速度的预设值,再次将第一时长和横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,进而再次执行上面的步骤,得到两相邻目标避障格之间的实际的横向距离后判断其是否等于所述两相邻目标避障格之间实际的横向距离,直到两相邻目标避障格的预测的横向距离等于两相邻目标避障格之间实际的横向距离为止,初始值设定公式为:
需要说明的是,将更新后的第一时长和更新后的横摆角加速度的预设值作为第一时长和横摆角加速度的预设值,再次将第一时长和横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,并依次执行后续的步骤,直到两相邻目标避障格的预测的横向距离等于两相邻目标避障格之间实际的横向距离为止。
当两相邻目标避障格的预测的横向距离是否等于两相邻目标避障格之间实际的横向距离时,获取两相邻目标避障格的预测的横向距离等于两相邻目标避障格之间实际的横向距离时的横摆角速度。
需要说明的是,在实际计算的过程中,可以在自车距离第一目标避障格预测的横向距离与自车距离第一目标避障格之间实际的横向距离的差值小于一定阈值时,获取得到的横摆角速度,将该横摆角速度作为避障轨迹中对应的横摆角速度。
在本发明实施例中,通过横向距离来确定横摆角速度,当确定的横摆角速度可以使自车的横摆角满足横向距离要求,由于在确定避障格的路径时所选取的避障格能够满足自车到避障格之间的纵向距离大于等于避障所需的最小纵向距离,需要在自车的中心与避障格的中心之间的距离为避障所需的最小纵向距离时开始按照避障轨迹进行避障,在避障完成后,自车能够到达对应的避障格的位置上。
将横摆角速度输入到避障轨迹公式中,得到第一个目标避障格之外的任一个目标避障格的避障轨迹,避障轨迹公式为:
其中,ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
在步骤S211中,将目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线相连,得到目标避障格路径对应的目标避障轨迹。
需要说明的是,目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线首尾相连,如图5所示,C表示自车,A1、A2分别表示障碍物,障碍物A2是运动的,其他的障碍物是静止的,B2、B3分别表示避障格,B1、B4分别表示目标避障格,L1、L3分别为目标避障格对应的避障轨迹,L2为连接相邻的避障轨迹的直线,a点表示L3所表示的避障轨迹的起点,最终,L1、L2、L3组成目标避障轨迹。图中虚线表示的矩形框的位置代表避障物A2没有运动时的所对应的避障格的位置,图中虚线表示的障碍物的位置代表避障物A2没有运动时的所对应的位置,在规划轨迹的过程中考虑了障碍物的速度,因此,最终得到的目标避障轨迹会规划到避障物A2运动后的位置所对应的避障格的位置。
根据一示例性实施例示出的一种智能车避障轨迹规划装置的结构示意图,如图6所示,该装置包括:
获取模块301,用于获取自车的位置、自车的速度、自车周围相关的障碍物的位置和每个障碍物的速度;
第一确定模块302,用于根据每个障碍物的位置得到所有的避障格的位置,每个避障格为与相对应的障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
第二确定模块303,用于根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置,得到至少一个避障格路径,每个避障格路径包括:从自车的当前位置起避过所有相关的障碍物所依次经过的每个避障格的顺序以及对应的避障格;
计算模块304,用于计算每个避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
第三确定模块305,用于对于目标避障格路径中自车经过的第一个目标避障格,根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹;
第四确定模块306,用于对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹;
第五确定模块307,用于根据目标避障格路径中所有目标避障格对应的避障轨迹,得到目标避障格路径对应的目标避障轨迹。
可选的,该装置还包括:判断模块308,用于:
判断是自车周围是否存在障碍物;
当存在障碍物时,判断自车与同一车道上的相邻的障碍物是否有发生碰撞的可能;
若是,则执行获取自车的位置、自车的速度、自车周围相关的障碍物的位置和每个障碍物的速度的步骤。
可选的,该判断模块308,还用于:
获取相邻的障碍物的速度;
将自车的速度与相邻的障碍物的速度进行比较,若相邻的障碍物的速度小于自车的速度,则判定有发生碰撞的可能;
若相邻的障碍物的速度大于自车的速度,则判定没有发生碰撞的可能。
可选的,该第一确定模块302,还用于:
获取自车的尺寸;
基于自车的尺寸设置避障格的目标尺寸;
在每一障碍物的两侧的预设距离处设置目标尺寸的避障格,得到所有避障格的位置,其中避障格对应的位置无障碍物。
可选的,该第二确定模块303,还用于:
在所有避障格之中确定所有最终避障格,其中每个最终避障格为从其开始直行而无碰撞障碍物的可能性的避障格;
对于每个最终避障格的位置,确定其和自车的位置之间在行驶方向上存在的中间避障格的位置,进而得到所有的可能路径,其中每个可能路径不包括中间避障格或者包括至少一个中间避障格,其中每个可能路径中的任两个中间避障格都不属于同一个障碍物两侧的两个避障格;
对于每个可能路径,当自车的位置和下一个经过的避障格的位置之间的可能行车区域中以及每一个中间避障格的位置与下一个经过的避障格的位置之间的可能行车区域中均不存在障碍物,且当自车的当前位置和下一个经过的避障格的位置之间的纵向距离以及每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离均不小于避障所需的最小纵向距离时,将其作为避障格路径,最小纵向距离是根据自车的速度和障碍物的速度得到。
可选的,该计算模块304,还用于:
当避障格为第一个避障格时,根据自车的位置和第一避障格的位置,得到自车与第一个避障格之间的横向距离;
当避障格为第一个避障格之外的任一避障格时,根据任一避障格的位置以及与任一避障格相邻的避障格的位置,得到两相邻避障格之间的横向距离;
将自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离、障碍物的速度输入到第一公式中,得到避障格对应的避障格代价,第一公式为:
χ=χw×σw+χv×σv
σw=w/wm
σv=vb/vm
其中,w表示自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离,vb为避障格的速度,wm为预先设定的距离值,vm为预先设定的速度值,σw为相邻避障格与自车之间横向距离对应的代价函数,σv表示避障格的速度对应的代价函数,χw为避障格与自车之间的横向距离对应的权重系数,χv为避障格的速度对应的权重系数,χ为避障格代价;
将每个避障格路径中的避障格所对应的避障格代价相加,得到每个避障格路径的避障格代价;
将所有避障格路径的避障格代价进行比较,得到避障格代价最小的避障格路径,将避障格代价最小的避障格路径作为目标避障格路径。
可选的,该第三确定模块305,还用于:
基于预设的标准时刻设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻,T1为第一时长,T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到自车距离所述第一目标避障格预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为避障轨迹中自车距离所述第一目标避障格预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述自车的位置和所述第一目标避障格的位置,得到所述自车与所述第一目标避障格之间实际的横向距离;
判断所述自车距离所述第一目标避障格预测的横向距离是否等于所述自车与所述第一目标避障格之间实际的横向距离,当所述自车距离所述第一目标避障格预测的横向距离不等于所述自车与所述第一目标避障格之间实际的横向距离时,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度,直到所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时,获取所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到自车的第一目标避障格的避障轨迹,所述避障轨迹公式为:
其中,ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
可选的,第四确定模块306,还用于:
基于预设的标准时长设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻、T1为第一时长、T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到两相邻目标避障格之间的预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为两相邻目标避障格之间的预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述两相邻目标避障格的位置,得到所述两相邻目标避障格之间的实际的横向距离;
判断所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,当所述两相邻目标避障格的预测的横向距离不等于所述两相邻目标避障格之间实际的横向距离,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,直到所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,获取所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到所述第一个目标避障格之外的任一个目标避障格的避障轨迹,所述避障轨迹公式为:
其中ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
可选的,第五确定模块307,还用于:
将目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线相连,得到目标避障格路径对应的目标避障轨迹。
本发明实施例提供了一种智能车避障轨迹规划方法,获取自车的位置、自车的速度、自车周围相关的障碍物的位置和每个障碍物的速度;根据每个障碍物的位置得到所有的避障格的位置,每个避障格为与相对应的障碍物的一侧相距预设距离且无障碍物的预设大小的格子;根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置,得到至少一个避障格路径,每个避障格路径包括:从自车的当前位置起避过所有相关的障碍物所依次经过的每个避障格的顺序以及对应的避障格;计算每个避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;对于目标避障格路径中自车经过的第一个目标避障格,根据自车的位置、自车的速度和第一个目标避障格的位置,得到第一个目标避障格的避障轨迹;对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹;根据目标避障格路径中所有目标避障格对应的避障轨迹,得到目标避障格路径对应的目标避障轨迹。本发明实施例中能够根据障碍物的速度进行路径的规划,提高了智能车避障的准确性。
以上所述仅是为了便于本领域的技术人员理解本发明的技术方案,并不用以限制本发明。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (10)
1.一种智能车避障轨迹规划方法,其特征在于,方法包括:
获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度;
根据每个所述障碍物的位置得到所有的避障格的位置,每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,每个所述避障格路径包括:从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格;
计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹;
对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度,得到该任一个目标避障格的避障轨迹;
根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹。
2.根据权利要求1所述的方法,其特征在于,在所述获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度之前,所述方法还包括:
判断自车周围是否存在障碍物;
当存在所述障碍物时,判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能;
若是,则执行获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度的步骤。
3.根据权利要求2所述的方法,其特征在于,所述判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能,包括:
获取相邻的障碍物的速度;
将所述自车的速度与相邻的障碍物的速度进行比较,若相邻的障碍物的速度小于所述自车的速度,则判定有发生碰撞的可能;
若相邻的障碍物的速度大于所述自车的速度,则判定没有发生碰撞的可能。
4.根据权利要求1所述的方法,其特征在于,所述根据每个所述障碍物的位置得到所有的避障格的位置,包括:
获取所述自车的尺寸;
基于所述自车的尺寸设置避障格的目标尺寸;
在每一障碍物的两侧的预设距离处设置目标尺寸的避障格,得到所有避障格的位置,其中所述避障格的位置上无障碍物。
5.根据权利要求1所述的方法,其特征在于,所述根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,包括:
在所有所述避障格之中确定所有最终避障格,其中每个所述最终避障格为从其开始直行而无碰撞障碍物的可能性的避障格;
对于每个所述最终避障格的位置,确定其和所述自车的位置之间在行驶方向上存在的中间避障格的位置,进而得到所有的可能路径,其中每个可能路径不包括中间避障格或者包括至少一个中间避障格,其中每个可能路径中的任两个中间避障格都不属于同一个障碍物两侧的两个避障格;
对于每个可能路径,当所述自车的位置和下一个经过的避障格的位置之间的可能行车区域中以及每一个中间避障格的位置与下一个经过的避障格的位置之间的可能行车区域中均不存在障碍物,且当所述自车的当前位置和下一个经过的避障格的位置之间的纵向距离以及每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离均不小于避障所需的最小纵向距离时,将其作为避障格路径,所述最小纵向距离是根据所述自车的速度和所述障碍物的速度得到。
6.根据权利要求1所述的方法,其特征在于,所述计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径,包括:
当所述避障格为第一个避障格时,根据所述自车的位置和所述第一个避障格的位置,得到所述自车与所述第一个避障格之间的横向距离;
当所述避障格为第一个避障格之外的任一避障格时,根据所述任一避障格的位置以及与所述任一避障格相邻的避障格的位置,得到两相邻避障格之间的横向距离;
将自车与所述第一个避障格之间的横向距离或两相邻避障格之间横向距离、所述障碍物的速度输入到第一公式中,得到所述避障格对应的避障格代价,所述第一公式为:
χ=χw×σw+χv×σv
σw=w/wm
σv=vb/vm
其中,w表示自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离,vb为避障格的速度,wm为预先设定的距离值,vm为预先设定的速度值,σw为相邻避障格与自车之间横向距离对应的代价函数,σv表示避障格的速度对应的代价函数,χw为避障格与自车之间的横向距离对应的权重系数,χv为避障格的速度对应的权重系数,χ为避障格代价;
将每个避障格路径中的避障格所对应的避障格代价相加,得到每个避障格路径的避障格代价;
将所有避障格路径的避障格代价进行比较,得到避障格代价最小的避障格路径,将避障格代价最小的避障格路径作为目标避障格路径。
7.根据权利要求1所述的方法,其特征在于,所述对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹,包括:
基于预设的标准时刻设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻,T1为第一时长,T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到自车距离所述第一个目标避障格预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为避障轨迹中自车距离所述第一个目标避障格预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述自车的位置和所述第一个目标避障格的位置,得到所述自车与所述第一个目标避障格之间实际的横向距离;
判断所述自车距离所述第一个目标避障格预测的横向距离是否等于所述自车与所述第一个目标避障格之间实际的横向距离,当所述自车距离所述第一个目标避障格预测的横向距离不等于所述自车与所述第一个目标避障格之间实际的横向距离时,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的设定值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度,直到所述自车距离所述第一个目标避障格预测的横向距离等于所述自车与所述第一个目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述自车距离所述第一个目标避障格预测的横向距离等于所述自车与所述第一个目标避障格之间实际的横向距离时,获取所述自车距离所述第一个目标避障格预测的横向距离等于所述自车与所述第一个目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到自车的第一个目标避障格的避障轨迹,所述避障轨迹公式为:
其中,ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
8.根据权利要求1所述的方法,其特征在于,所述对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度,得到该任一个目标避障格的避障轨迹,包括:
基于预设的标准时长设置第一时长,基于标准的横摆角加速度设置横摆角加速度的预设值;
将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,所述横摆角加速度公式为:
其中,tb-ta=td-tc=te-td=tg-tf=ti-th=tj-ti=tl-tk=T1,tc-tb=tf-te=th-tg=tk-tj=T2,T2=5T1,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻、T1为第一时长、T2为第二时长,为横摆角加速度的预设值,为t时刻的横摆角加速度;
将横摆角加速度输入到横摆角速度公式中,得到横摆角速度,所述横摆角速度的公式为:
将所述自车的速度和所述横摆角速度输入到横向距离公式中,得到两相邻目标避障格之间的预测的横向距离,所述横向距离公式为:
其中,vz表示自车的车速,θd(t)为t时刻的横摆角速度,Sby为两相邻目标避障格之间的预测的横向距离,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻;
根据所述两相邻目标避障格的位置,得到所述两相邻目标避障格之间的实际的横向距离;
判断所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,当所述两相邻目标避障格的预测的横向距离不等于所述两相邻目标避障格之间实际的横向距离,按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值,将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值,再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中,得到角加速度,直到所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离为止,所述初始值设定公式为:
当所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时,获取所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离时的横摆角速度;
将所述横摆角速度输入到避障轨迹公式中,得到所述第一个目标避障格之外的任一个目标避障格的避障轨迹,所述避障轨迹公式为:
其中ta≤t≤tl,ta为避障轨迹的开始时刻,tl为避障轨迹的结束时刻,(x(t),y(t))表示t时刻的车辆质心的位置,vz表示自车的车速,θd(t)为t时刻的横摆角速度。
9.根据权利要求1所述的方法,其特征在于,根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹,包括:将所述目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线相连,得到目标避障格路径对应的目标避障轨迹。
10.一种智能车避障轨迹规划装置,其特征在于,所述装置包括:
获取模块,用于获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度;
第一确定模块,用于根据每个所述障碍物的位置得到所有的避障格的位置,每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子;
第二确定模块,用于根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置,得到至少一个避障格路径,每个所述避障格路径包括:从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格;
计算模块,用于计算每个所述避障格路径的避障格代价,将避障格代价最小的避障格路径作为目标避障格路径;
第三确定模块,用于对于所述目标避障格路径中所述自车经过的第一个目标避障格,根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置,得到所述第一个目标避障格的避障轨迹;
第四确定模块,用于对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格,根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度,得到该任一个目标避障格的避障轨迹;
第五确定模块,用于根据所述目标避障格路径中所有目标避障格对应的避障轨迹,得到所述目标避障格路径对应的目标避障轨迹。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910399631.8A CN110077397B (zh) | 2019-05-14 | 2019-05-14 | 一种智能车避障轨迹规划方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910399631.8A CN110077397B (zh) | 2019-05-14 | 2019-05-14 | 一种智能车避障轨迹规划方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110077397A CN110077397A (zh) | 2019-08-02 |
CN110077397B true CN110077397B (zh) | 2020-08-04 |
Family
ID=67420115
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910399631.8A Active CN110077397B (zh) | 2019-05-14 | 2019-05-14 | 一种智能车避障轨迹规划方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110077397B (zh) |
Families Citing this family (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110466514B (zh) * | 2019-08-30 | 2020-10-27 | 北京小马慧行科技有限公司 | 车辆控制方法及装置 |
CN110466513A (zh) * | 2019-08-30 | 2019-11-19 | 北京小马慧行科技有限公司 | 车辆控制方法及装置 |
CN110550045B (zh) * | 2019-09-09 | 2020-11-10 | 奇瑞汽车股份有限公司 | 速度的规划和跟踪方法、装置及存储介质 |
TWI715221B (zh) * | 2019-09-27 | 2021-01-01 | 財團法人車輛研究測試中心 | 自適應軌跡生成方法及系統 |
CN110703754B (zh) * | 2019-10-17 | 2021-07-09 | 南京航空航天大学 | 一种自动驾驶车辆路径与速度高度耦合的轨迹规划方法 |
CN110834630A (zh) * | 2019-10-22 | 2020-02-25 | 中国第一汽车股份有限公司 | 一种车辆的驾驶控制方法、装置、车辆及存储介质 |
CN111506070B (zh) * | 2020-04-26 | 2023-09-08 | 北京踏歌智行科技有限公司 | 一种基于路径点偏移的局部路径规划方法 |
TWI737437B (zh) * | 2020-08-07 | 2021-08-21 | 財團法人車輛研究測試中心 | 軌跡決定方法 |
CN112572416A (zh) * | 2020-11-20 | 2021-03-30 | 芜湖格陆博智能科技有限公司 | 一种矿区无人驾驶车辆的泊车方法及*** |
EP4261092A4 (en) * | 2020-12-21 | 2024-05-22 | Huawei Technologies Co., Ltd. | CONTROL METHOD, ASSOCIATED DEVICE AND COMPUTER-READABLE STORAGE MEDIUM |
JP7325465B2 (ja) * | 2021-03-15 | 2023-08-14 | 三菱重工業株式会社 | 移動体の制御方法、移動体及びプログラム |
CN115092136B (zh) * | 2022-07-27 | 2023-09-12 | 广州小鹏自动驾驶科技有限公司 | 车速规划方法、装置、车辆及存储介质 |
CN115309169B (zh) * | 2022-10-11 | 2022-12-20 | 天地科技股份有限公司 | 一种井下无人车控制方法及装置 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102007013303A1 (de) * | 2007-03-16 | 2008-09-18 | Robert Bosch Gmbh | Verfahren zur Berechnung einer kollisionsvermeidenden Trajektorie für ein Fahrmanöver eines Fahrzeugs |
KR101552074B1 (ko) * | 2011-10-03 | 2015-09-09 | 도요타 지도샤(주) | 차량의 운전 지원 시스템 |
DE102013209764B4 (de) * | 2013-05-27 | 2023-10-05 | Robert Bosch Gmbh | Unterstützung eines Fahrers eines Kraftfahrzeugs |
JP6545507B2 (ja) * | 2015-03-31 | 2019-07-17 | アイシン・エィ・ダブリュ株式会社 | 自動運転支援システム、自動運転支援方法及びコンピュータプログラム |
CN104897168B (zh) * | 2015-06-24 | 2018-01-12 | 清华大学 | 基于道路危险评估的智能车路径搜索方法及*** |
CN109263639B (zh) * | 2018-08-24 | 2020-09-01 | 武汉理工大学 | 基于状态栅格法的驾驶路径规划方法 |
-
2019
- 2019-05-14 CN CN201910399631.8A patent/CN110077397B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110077397A (zh) | 2019-08-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110077397B (zh) | 一种智能车避障轨迹规划方法及装置 | |
CN110834644B (zh) | 一种车辆控制方法、装置、待控制车辆及存储介质 | |
CN109572694B (zh) | 一种考虑不确定性的自动驾驶风险评估方法 | |
CN105270410B (zh) | 用于自主驾驶车辆的路径规划的精确曲率估计算法 | |
CN110809790B (zh) | 车辆用信息存储方法、车辆的行驶控制方法、以及车辆用信息存储装置 | |
EP3407329B1 (en) | Driving assistance method and device | |
KR20220054278A (ko) | 차량의 주행 궤적 예측 방법 및 장치 | |
CN108986540A (zh) | 车辆控制***和方法、以及行驶辅助服务器 | |
US12017648B2 (en) | Vehicle control device | |
DE112016003156T5 (de) | Fahrzeugsteuerungsvorrichtung; Fahrzeugsteuerungsverfahren und Fahrzeugsteuerungsprogramm | |
DE102016206318A1 (de) | Fahrtsteuervorrichtung | |
AU2018278948B2 (en) | A system to optimize scats adaptive signal system using trajectory data | |
CN110553660A (zh) | 一种基于a*算法和人工势场的无人车轨迹规划方法 | |
CN110320531A (zh) | 基于激光雷达的障碍物识别方法、地图创建方法及装置 | |
CN112673230B (zh) | 行驶辅助方法及行驶辅助装置 | |
WO2020116264A1 (ja) | 車両の走行支援方法、車両走行支援装置及び自動運転システム | |
CN114511999B (zh) | 一种行人行为预测方法及装置 | |
CN112406905B (zh) | 基于交通工具的数据处理方法、装置、计算机及存储介质 | |
CN113581181B (zh) | 智能车辆超车轨迹的规划方法 | |
CN113720346A (zh) | 基于势能场和隐马尔可夫模型的车辆路径规划方法及*** | |
EP4397554A1 (en) | Vehicle safety control method and apparatus, electronic device, and storage medium | |
CN114639267A (zh) | 一种车路协同环境中车辆避撞预警方法 | |
CN115230731A (zh) | 行驶路径确定方法、装置、终端及介质 | |
JP6796679B2 (ja) | 車両制御システム及び方法、並びに走行支援サーバ | |
CN113320544B (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 |