CN113419534A - 一种基于贝塞尔曲线的转向路段路径规划方法 - Google Patents
一种基于贝塞尔曲线的转向路段路径规划方法 Download PDFInfo
- Publication number
- CN113419534A CN113419534A CN202110744409.4A CN202110744409A CN113419534A CN 113419534 A CN113419534 A CN 113419534A CN 202110744409 A CN202110744409 A CN 202110744409A CN 113419534 A CN113419534 A CN 113419534A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- constraint
- curve
- corridor
- road
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000005457 optimization Methods 0.000 claims description 21
- 230000014509 gene expression Effects 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 2
- 230000008602 contraction Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 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/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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明实施例提供一种基于贝塞尔曲线的转向路段路径规划方法,以三条贝塞尔曲线规划所述车辆的转向路径,第一条贝塞尔曲线位于所述第一条道路,与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于所述第一条道路和所述第二条道路的重叠区域,与所述第二条贝塞尔曲线连接的第三条贝塞尔曲线位于所述第二条道路。采用本发明实施例提供的方法,设置路径规划的起始状态和终止状态,建立路径规划的模型和限制条件,能够保证贝塞尔曲线的避障要求,满足路径规划的需要。
Description
技术领域
本发明涉及智能网联汽车技术领域,具体涉及一种基于贝塞尔曲线的转向路段路径规划方法。
背景技术
随着电子技术以及人工智能算法的飞速发展,自动驾驶领域的技术积累越来越丰富。自动驾驶的应用场景主要包括结构化道路和非结构画道路,其中,结构化道路常见的任务包括换道行驶以及转向行驶。
作为自动驾驶系统中的关键技术,路径规划决定着车辆的行驶路线,对车辆的安全性、舒适性具有较高的影响。现有的路径规划技术中,为了满足曲率连续约束,常常采用贝塞尔曲线规划路径。然而,大多数方法都不能保证贝塞尔曲线的避障要求,存在一定的安全隐患。
发明内容
本发明的目的在于提供一种基于贝塞尔曲线的转向路段路径规划方法来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的,本发明提供一种基于贝塞尔曲线的转向路段路径规划方法,所述转向路段包括车辆转向前的第一条道路和转向后的第二条道路,以三条贝塞尔曲线规划所述车辆的转向路径,第一条贝塞尔曲线位于所述第一条道路,与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于所述第一条道路和所述第二条道路的重叠区域,与所述第二条贝塞尔曲线连接的第三条贝塞尔曲线位于所述第二条道路,所述方法包括:
步骤1,确定路径规划的起始状态和终止状态;其中,所述起始状态包括所述车辆的起始位置的横纵坐标和所述车辆在所述起始位置的横摆角度,所述终止状态包括所述车辆的终止位置的横纵坐标和所述车辆在所述终止位置的横摆角度;
步骤2,基于所述车辆的起始位置和所述终止位置以及所述第一条道路和所述第二条道路的边界建立第一四边形模型,收缩所述第一四边形模型的边,以收缩后的第二四边形模型为走廊模型,包括在所述第一道路内的第一走廊,在所述第二道路内的第二走廊,所述第一走廊和所述第二走廊在转弯处重叠,收缩宽度为Rs,其中
其中,Lw为所述车辆的宽度,Lx为所述车辆的轴距,LR为所述车辆的后悬长度;
所述第一走廊的四条边的表达式为:
a11x+b11y+c11=0,a12x+b12y+c12=0,a13x+b13y+c13=0,a14x+b14y+c14=0;
所述第二走廊的四条边的表达式为:
a21x+b21y+c21=0,a22x+b22y+c22=0,a23x+b23y+c23=0,a24x+b24y+c24=0;
基于所述车辆的起始位置和所述终止位置、所述第一条道路和所述第二条道路的边界以及所述收缩宽度Rs的值,可以确定系数a11、b11、c11、a12、b12、c12、a13、b13、c13、a14、b14、c14以及系数a21、b21、c21、a22、b22、c22、a23、b23、c23、a24、b24、c24的值;
将所述走廊模型记为T1和T2:
步骤3,建立优化模型,所述优化模型包括:决策变量、目标函数和约束条件;其中,所述决策变量为所述贝塞尔曲线的控制点坐标,所述贝塞尔曲线为五次贝塞尔曲线,所述控制点坐标包括Pij(xi,j,yi,j),i=1,2,3,j=0,1,…5,其中,Pij代表第i条曲线的第j+1个控制点,(xi,j,yi,j)表示第i条曲线的第j+1个控制点的坐标;所述目标函数为所述三条贝塞尔曲线的总长度;所述约束条件包括起始位置约束和终止位置约束、曲线连接处的位置连续约束和横摆角连续约束、避障约束;基于所述决策变量、目标函数和约束条件计算所述三条贝塞尔曲线的总长度最短时的控制点坐标值,并根据控制点坐标与所述贝塞尔曲线之间的关系,得到所述三条贝塞尔曲线,其中,所述控制点坐标与所述贝塞尔曲线之间的关系为:
其中,s∈(0,1)为连续变量;
步骤4,以步骤3中得到的所述三条贝塞尔曲线的轨迹作为所述车辆的转向路径。
其中,所述目标函数的表达式为:
其中,
矩阵M满足:
矩阵Q满足:
所述起始位置约束和终止位置约束为:
其中,xini和yini分别为所述车辆的起始位置的横纵坐标,xter和yter分别为所述车辆的终止位置的横纵坐标;
所述曲线连接处的位置连续约束为:
Pi,5=Pi+1,0, i=1,2
所述曲线连接处的横摆角连续约束为:
Pi,5-Pi,4=Pi+1,1-Pi+1,0, i=1,2
所述避障约束包括:
第一避障约束,限制所述车辆基于所述第一贝塞尔曲线行驶至所述第一走廊与所述第二走廊的重叠区域,并且不和所述第一走廊的边界发生碰撞:
第二避障约束,限制所述车辆基于所述第二贝塞尔曲线穿过所述第一走廊与所述第二走廊的重叠区域,并且不和两条走廊的边界发生碰撞:
第三避障约束,限制所述车辆基于所述第三贝塞尔曲线穿过所述第二走廊到达所述终止位置,并且不和所述第二走廊的边界发生碰撞:
可选的,该方法还可以包括:建立非线性规划模型,将步骤4中所述车辆的转向路径作为初始解,根据所述非线性规划模型得到最终的转向路径,其中
所述非线性规划模型包括第二决策变量、第二目标函数和第二约束条件,其中,所述第二决策变量与所述步骤3中的所述决策变量相同,所述第二目标函数与所述步骤3中的所述目标函数相同,所述第二约束条件包括所述步骤3中的所述起始位置约束和终止位置约束、所述曲线连接处的位置连续约束和横摆角连续约束,还包括:起始横摆角约束和终止横摆角约束、所述曲线连接处的曲率连续约束、曲率最大约束和优化避障约束。
其中,所述起始横摆角约束和终止横摆角约束为:
其中,θini为所述车辆在所述起始位置的横摆角,θter为所述车辆在所述终止位置的横摆角。
其中,所述曲线连接处的曲率连续约束为:
Pi,5-2Pi,4+Pi,3=Pi+1,2-2Pi+1,1+Pi+1,0,i=1,2
所述曲率最大约束为:
κi,j(s)≤κmax,s∈(0,1)
其中,κmax为预设值,
其中,所述优化避障约束包括:
第一优化避障约束:
第二优化避障约束:
第三优化避障约束:
其中,(xf,yf)和(xr,yr)分别为用两个包络圆覆盖车辆轮廓时所述两个包络圆的圆心,通过下式计算得到:
采用本发明实施例提供的方法,设置路径规划的起始状态和终止状态,建立路径规划的模型和限制条件,能够保证贝塞尔曲线的避障要求,满足路径规划的需要。
本发明的其他特征和优点将在随后的说明书中阐述,并且部分的从说明书中变得显而易见,或者通过实施本发明而了解。
附图说明
图1是本发明实施例提供的基于贝塞尔曲线的转向路段路径规划方法的流程示意图。
图2是转向路段的道路示意图。
图3和图4是走廊模型生成图。
图5是用两个包络圆覆盖车辆轮廓的示意图。
图6是本发明实施例中用三条五次贝塞尔曲线规划路径的示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述,其中,附图构成本申请一部分,并与本发明的实施例一起用于阐释本发明。但本领域的技术人员应该知道,以下实施例并不是对本发明技术方案作的唯一限定,凡是在本发明技术方案精神实质下所做的任何等同变换或改动,均应视为属于本发明的保护范围。
本发明实施例提出一种基于贝塞尔曲线的转向路段路径规划方法。该方法将贝塞尔曲线的控制点坐标当作决策变量,通过优化建模,将路径规划问题转化为优化求解问题,并利用二次规划求初始解,加快求解效率,完成路径规划。
图1示出本发明实施例提供的基于贝塞尔曲线的转向路段路径规划方法。该转向路段包括车辆转向前的第一条道路和转向后的第二条道路,以三条贝塞尔曲线作为车辆的转向路径,第一条贝塞尔曲线位于第一条道路,与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于第一条道路和第二条道路的重叠区域,与第二条贝塞尔曲线连接的第三条贝塞尔曲线位于第二条道路。该方法包括步骤100-步骤500。
步骤100,确定路径规划的起始状态和和终止状态。其中,所述起始状态包括所述车辆的起始位置的横纵坐标和所述车辆在所述起始位置的横摆角度,所述终止状态包括所述车辆的终止位置的横纵坐标和所述车辆在所述终止位置的横摆角度。
步骤200,将道路转化为走廊模型,并求出走廊模型。
步骤300,建立优化模型,计算得到最优的贝塞尔曲线控制点坐标,进而得到三条贝塞尔曲线;
步骤400,以步骤300中得到的三条贝塞尔曲线作为车辆的转向路径。
在步骤400之后,还可以包括:
步骤500,建立非线性规划模型,将步骤400中车辆转向路径作为初始解,求解得到最终的转向路径。
下面一一予以细述。
在步骤100中:确定路径规划的起始状态和终止状态。其中,起始状态包括车辆的起始位置的横纵坐标和车辆在起始位置的横摆角度,终止状态包括车辆的终止位置的横纵坐标和车辆在终止位置的横摆角度。
设车辆的起始状态为(xini,yini,θini),终止状态为(xter,yter,θter)。其中xini,yini为车辆起始位置的横纵坐标,θini为车辆起始位置处的横摆角度。xter,yter为车辆终止位置的横纵坐标,θter为车辆终止位置处的横摆角度。
在步骤200中,将道路转化为走廊模型,并求出走廊模型。
图2示出转向路段的道路示意图。令线段AC和BD为第一条道路两条边界,其中AC为道路里侧边界,BD为道路外侧边界。EC和FD为第二条道路的两条边界,其中EC为道路里侧边界,FD为道路外侧边界。并且,C和D分别为两条道路边界的里侧边界和外侧边界的交点。
令D点为坐标原点,BD为X轴,延长AC,直至与FD相交,令交点为C1。延长EC,直至与BD相交,令交点为C2。为保证安全距离,令四边形ABDC1和四边形EFDC2进行收缩,收缩宽度为Rs,收缩后的四边形分别为A'B'C1'D'和E′F′D′C′2,如图3和图4所示。
通过以下方式确定Rs:以用两个包络圆覆盖车辆轮廓时的半径为Rs。图5示出用两个包络圆覆盖车辆轮廓的示意图。其中(xb,yb)为后包络圆的圆心坐标,(xf,yf)为前包络圆的圆心坐标。
其中,Lw为车的宽度,Lx为车的轴距,LR为车的后悬长度。
在图3和图4中,A点,B点,C点,D点,E点,F点坐标皆当作已知信息,由自动驾驶系统中的定位系统和/或地图模块提供。记A点坐标为(xA,yA),B点坐标为(xB,yB),C点坐标为(xC,yC),D点坐标为(xD,yD),E点坐标为(xE,yE),F点坐标为(xF,yF)。
收缩后的四边形A'B'C'1D'各个顶点坐标可表示为:A’点坐标为(x'A,y'A),B’点坐标为(x'B,y'B),C'1点坐标为D'点坐标为(x'D,y'D)。四边形E'F'D'C'2各个顶点坐标可表示为:E’点坐标为(x'E,y'E),F’点坐标为(x'F,y'F),C'2点坐标为坐标计算方式为:
x'A=xA+Rs
y'A=yA-Rs
x'B=xB+Rs
y'B=yB+Rs
x'D=xD-Rs
y'D=yD+Rs
x'E=xE+Rs
y'E=yE-Rs
x'F=xF-Rs
y'F=yF-Rs
记四边形A'B'C'1D'各条边的表达式为:
lA'B'的表达式:a12x+b12y+c12=0;
lB'D'的表达式:a13x+b13y+c13=0;
收缩后的四边形的四条边E'F′D'C′2可表示为:
lE′F′的表达式:a21x+b21y+c21=0
lD'F'的表达式:a24x+b24y+c24=0
其中,以直线lA'C'为例,上述直线模型的系数a,b,c计算方式为:
收缩后的两个四边形,即为安全走廊模型。记两个安全走廊分别为T1和T2:
在步骤300中,确定决策变量,构建目标函数和对应约束,得到优化模型,求解得到转向路径。
本发明实施例中,用三条五次贝塞尔曲线规划路径。
图6是本发明实施例中用三条五次贝塞尔曲线规划路径的示意图。其中,第一条曲线为U1U2,位于四边形A'B'C'D'内,作用为引导车辆从起始位置U1前进到转向处。需要说明,U1的横坐标还可以与A'和B'点相同或不同,与选择的用于计算路径的车辆上的位置有关系,本文对此不做限制。第二条曲线为U2U3,位于四边形A′B′C′D′和E'F'D'C'2的重叠区域内,引导车辆进行转向。第三条曲线为U3U4,位于四边形E'F'D'C'2内,引导车辆驶向目标点U4,即路径终止位置。上述点U1,U2,U3和U4仅仅为示意说明,其具体坐标将在下述优化模型中求出。
优化模型包括:决策变量、目标函数和约束条件。决策变量是待优化的变量,本实施例中,决策变量为贝塞尔曲线的控制点坐标,贝塞尔曲线为五次贝塞尔曲线,控制点坐标包括Pij(xi,j,yi,j,i=1,2,3,j=0,1,…5,其中,Pij代表第i条曲线的第j+1个控制点,(xi,j,yi,j)表示第i条曲线的第j+1个控制点的坐标;其中,控制点坐标与贝塞尔曲线之间的关系为:
其中,s∈(0,1)为连续变量。
五次贝塞尔曲线为一种样条曲线,具体可以理解为:确定六个控制点的位置后,通过六个控制点生成的曲线,该曲线可由五次多项式表示。根据上述内容可知,对于贝塞尔曲线,控制点的坐标决定着曲线的形状。要想获得最优的曲线,就要求得最优的控制点坐标。
本发明实施例中,将贝塞尔曲线的控制点坐标xij和yij作为决策变量,求出最优的控制点,进而获得最优的贝塞尔曲线,当作车辆转向的路径。
目标函数为决策变量的一个函数,当目标函数取极小值时,决策变量即达到最优。本发明实施例中,目标函数为三条贝塞尔曲线的总长度,转向路径最短时决策变量最优,三条贝塞尔曲线的总长度可以表示为:
其中,矩阵M满足:
矩阵Q满足:
约束条件包括起始位置约束和终止位置约束、曲线连接处的位置连续约束和横摆角连续约束、避障约束。
为保证满足起始位置约束和终止位置约束,形成边界状态约束:
为保证曲线连接处的位置连续性,形成位置连续约束:
Pi,5=Pi+1,0,i=1,2
为保证曲线连接处的横摆角连续,形成横摆角连续约束:
Pi,5-Pi,4=Pi+1,1-Pi+1,0,i=1,2
为保证不发生碰撞,对三条曲线的控制点分别施加避障约束:
曲线1应引导车辆行驶至第一个走廊与第二个走廊的重叠区域,并且不和第一个走廊的边界发生碰撞,因此避障约束为:
曲线2应引导车辆穿过第一个走廊与第二个走廊的重叠区域,并且不和两条走廊的边界发生碰撞,因此避障约束为:
曲线3应引导车辆穿过第二个走廊到达目标点,并且不和走廊的边界发生碰撞,因此避障约束为:
综上,基于上述目标函数、决策变量以及约束条件,可将优化模型转化为下列形式:
采用拉格朗日法对上述式(1)和(2)进行求解,即可得到控制点的坐标。再根据上文中控制点与曲线的公式,即可计算得到贝塞尔曲线。如上文所述,在步骤400中,以计算得到的三条贝塞尔曲线规划作为车辆的转向路径。
在步骤500中,建立非线性规划模型,将步骤400中初始路径作为初始解,求解得到最终的转向路径。
非线性规划模型包括第二决策变量、第二目标函数和第二约束条件。第二决策变量与步骤300中的决策变量相同,第二目标函数与步骤300中的目标函数相同,第二约束条件包括步骤300中的起始位置约束和终止位置约束、曲线连接处的位置连续约束和横摆角连续约束,还包括:起始横摆角约束和终止横摆角约束、曲线连接处的曲率连续约束、曲率最大约束和优化避障约束。
贝塞尔曲线起始点处的横摆角θi,ini,终止点处的横摆角θi,ter表达式为:
因此,横摆角约束为:
连续性约束方面,包括位置连续约束、横摆角连续约束和曲率连续约束。其中,位置连续约束和横摆角连续约束与步骤300中相同,曲率连续约束如下:
Pi,5-2Pi,4+Pi,3=Pi+1,2-2Pi+1,1+Pi+1,0,i=1,2
曲率最大约束为:
κi,j(s)≤κmax,s∈(0,1)
其中,κmax为预设值,为车辆的最大曲率值。
优化避障约束同样是对各个曲线分别进行约束:
对于第一条曲线,避障约束如下:
对于第二条曲线,避障约束如下:
对于第三条曲线,避障约束如下:
其中,(xf,yf)和(xr,yr)分别为图5中两个包络圆的圆心,可由决策变量计算得到:
其中,Lx为车辆的轴距,Lf为车辆的前悬长度,Lr为车辆的后悬长度。
其中:
C(xi,j,yi,j)为不等式约束的一般形式,B(xi,j,yi,j)为等式约束的一般形式。
采用序列二次规划算法,以步骤400得到的初始路径为优化模型初始解,求解上述式(3),即可得到最优的决策变量:即贝塞尔曲线的控制点横纵坐标。将求出的控制点坐标带入下述公式中,即可得到最终的转向路径坐标:
采用本发明实施例提供的方法,设置路径规划的起始状态和终止状态,建立路径规划的模型和限制条件,能够保证贝塞尔曲线的避障要求,满足路径规划的需要。
Claims (6)
1.一种基于贝塞尔曲线的转向路段路径规划方法,其特征在于,所述转向路段包括车辆转向前的第一条道路和转向后的第二条道路,以三条贝塞尔曲线规划所述车辆的转向路径,第一条贝塞尔曲线位于所述第一条道路,与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于所述第一条道路和所述第二条道路的重叠区域,与所述第二条贝塞尔曲线连接的第三条贝塞尔曲线位于所述第二条道路,所述方法包括:
步骤1,确定路径规划的起始状态和终止状态;其中,所述起始状态包括所述车辆的起始位置的横纵坐标和所述车辆在所述起始位置的横摆角度,所述终止状态包括所述车辆的终止位置的横纵坐标和所述车辆在所述终止位置的横摆角度;
步骤2,基于所述车辆的起始位置和所述终止位置以及所述第一条道路和所述第二条道路的边界建立第一四边形模型,收缩所述第一四边形模型的边,以收缩后的第二四边形模型为走廊模型,包括在所述第一道路内的第一走廊,在所述第二道路内的第二走廊,所述第一走廊和所述第二走廊在转弯处重叠,收缩宽度为Rs,其中
其中,Lw为所述车辆的宽度,Lx为所述车辆的轴距,LR为所述车辆的后悬长度;
所述第一走廊的四条边的表达式为:
a11x+b11y+c11=0,a12x+b12y+c12=0,a13x+b13y+c13=0,a14x+b14y+c14=0;
所述第二走廊的四条边的表达式为:
a21x+b21y+c21=0,a22x+b22y+c22=0,a23x+b23y+c23=0,a24x+b24y+c24=0;
基于所述车辆的起始位置和所述终止位置、所述第一条道路和所述第二条道路的边界以及所述收缩宽度Rs的值,可以确定系数a11、b11、c11、a12、b12、c12、a13、b13、c13、a14、bi4、c14以及系数a21、b21、c21、a22、b22、c22、a23、b23、c23、a24、b24、c24的值;
将所述走廊模型记为T1和T2:
步骤3,建立优化模型,所述优化模型包括:决策变量、目标函数和约束条件;其中,所述决策变量为所述贝塞尔曲线的控制点坐标,所述贝塞尔曲线为五次贝塞尔曲线,所述控制点坐标包括Pij(xi,j,yi,j),i=1,2,3,j=0,1,...5,其中,Pij代表第i条曲线的第j+1个控制点,(xi,j,yi,j)表示第i条曲线的第j+1个控制点的坐标;所述目标函数为所述三条贝塞尔曲线的总长度;所述约束条件包括起始位置约束和终止位置约束、曲线连接处的位置连续约束和横摆角连续约束、避障约束;基于所述决策变量、目标函数和约束条件计算所述三条贝塞尔曲线的总长度最短时的控制点坐标值,并根据控制点坐标与所述贝塞尔曲线之间的关系,得到所述三条贝塞尔曲线,其中,所述控制点坐标与所述贝塞尔曲线之间的关系为:
其中,s∈(0,1)为连续变量;
步骤4,以步骤3中得到的所述三条贝塞尔曲线的轨迹作为所述车辆的转向路径。
2.根据权利要求1所述的方法,其特征在于,所述目标函数的表达式为:
其中,
矩阵M满足:
矩阵Q满足:
所述起始位置约束和终止位置约束为:
其中,xini和yini分别为所述车辆的起始位置的横纵坐标,xter和yter分别为所述车辆的终止位置的横纵坐标;
所述曲线连接处的位置连续约束为:
Pi,5=Pi+1,0,i=1,2
所述曲线连接处的横摆角连续约束为:
Pi,5-Pi,4=Pi+1,1-Pi+1,0,i=1,2
所述避障约束包括:
第一避障约束,限制所述车辆基于所述第一贝塞尔曲线行驶至所述第一走廊与所述第二走廊的重叠区域,并且不和所述第一走廊的边界发生碰撞:
第二避障约束,限制所述车辆基于所述第二贝塞尔曲线穿过所述第一走廊与所述第二走廊的重叠区域,并且不和两条走廊的边界发生碰撞:
第三避障约束,限制所述车辆基于所述第三贝塞尔曲线穿过所述第二走廊到达所述终止位置,并且不和所述第二走廊的边界发生碰撞:
3.根据权利要求1或2所述的方法,其特征在于,还包括:建立非线性规划模型,将步骤4中所述车辆的转向路径作为初始解,根据所述非线性规划模型得到最终的转向路径,其中
所述非线性规划模型包括第二决策变量、第二目标函数和第二约束条件,其中,所述第二决策变量与所述步骤3中的所述决策变量相同,所述第二目标函数与所述步骤3中的所述目标函数相同,所述第二约束条件包括所述步骤3中的所述起始位置约束和终止位置约束、所述曲线连接处的位置连续约束和横摆角连续约束,还包括:起始横摆角约束和终止横摆角约束、所述曲线连接处的曲率连续约束、曲率最大约束和优化避障约束。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110744409.4A CN113419534B (zh) | 2021-07-01 | 2021-07-01 | 一种基于贝塞尔曲线的转向路段路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110744409.4A CN113419534B (zh) | 2021-07-01 | 2021-07-01 | 一种基于贝塞尔曲线的转向路段路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113419534A true CN113419534A (zh) | 2021-09-21 |
CN113419534B CN113419534B (zh) | 2022-03-08 |
Family
ID=77717722
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110744409.4A Active CN113419534B (zh) | 2021-07-01 | 2021-07-01 | 一种基于贝塞尔曲线的转向路段路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113419534B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114371712A (zh) * | 2022-01-11 | 2022-04-19 | 湖南大学 | 一种具有不停车绕障功能的泊车轨迹重规划方法 |
CN114509086A (zh) * | 2022-02-15 | 2022-05-17 | 湖南大学无锡智能控制研究院 | 智能车辆在连续弯道场景下的最优轨迹规划方法及系统 |
CN115933701A (zh) * | 2023-01-06 | 2023-04-07 | 北京理工大学 | 一种基于二次规划的安全走廊优化生成方法及系统 |
Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080255728A1 (en) * | 2005-12-09 | 2008-10-16 | Hella Kgaa Hueck & Co. | Path Planning |
CN102207736A (zh) * | 2010-03-31 | 2011-10-05 | 中国科学院自动化研究所 | 基于贝塞尔曲线的机器人路径规划方法及装置 |
US20170364076A1 (en) * | 2016-06-20 | 2017-12-21 | Hypertherm, Inc. | Systems and Methods for Planning Paths to Guide Robots |
US20180074507A1 (en) * | 2017-11-22 | 2018-03-15 | GM Global Technology Operations LLC | Road corridor |
CN108153328A (zh) * | 2017-12-18 | 2018-06-12 | 北京理工大学 | 一种基于分段贝塞尔曲线的多导弹协同航迹规划方法 |
CN109253735A (zh) * | 2018-11-30 | 2019-01-22 | 奇瑞汽车股份有限公司 | 路径规划方法、装置及存储介质 |
US20190086932A1 (en) * | 2017-09-18 | 2019-03-21 | Baidu Usa Llc | Smooth road reference line for autonomous driving vehicles based on 2d constrained smoothing spline |
US20190155290A1 (en) * | 2017-07-13 | 2019-05-23 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for trajectory determination |
US20200003564A1 (en) * | 2018-06-27 | 2020-01-02 | Baidu Usa Llc | Reference line smoothing method using piecewise spiral curves with weighted geometry costs |
CN110749333A (zh) * | 2019-11-07 | 2020-02-04 | 中南大学 | 基于多目标优化的无人驾驶车辆运动规划方法 |
CN110949374A (zh) * | 2019-11-14 | 2020-04-03 | 江苏大学 | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 |
CN111551178A (zh) * | 2020-04-27 | 2020-08-18 | 广东工业大学 | 一种基于最短路径的分段轨迹时间规划方法 |
CN112026772A (zh) * | 2020-08-14 | 2020-12-04 | 清华大学 | 一种智能网联汽车的路径实时规划与分布式控制方法 |
CN112068588A (zh) * | 2020-08-12 | 2020-12-11 | 浙江大学 | 一种基于飞行走廊和贝塞尔曲线的无人飞行器轨迹生成方法 |
WO2021120200A1 (en) * | 2019-12-20 | 2021-06-24 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Spline curve and spiral curve based reference line smoothing method |
CN113031592A (zh) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统 |
-
2021
- 2021-07-01 CN CN202110744409.4A patent/CN113419534B/zh active Active
Patent Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080255728A1 (en) * | 2005-12-09 | 2008-10-16 | Hella Kgaa Hueck & Co. | Path Planning |
CN102207736A (zh) * | 2010-03-31 | 2011-10-05 | 中国科学院自动化研究所 | 基于贝塞尔曲线的机器人路径规划方法及装置 |
US20170364076A1 (en) * | 2016-06-20 | 2017-12-21 | Hypertherm, Inc. | Systems and Methods for Planning Paths to Guide Robots |
US20190155290A1 (en) * | 2017-07-13 | 2019-05-23 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for trajectory determination |
US20190086932A1 (en) * | 2017-09-18 | 2019-03-21 | Baidu Usa Llc | Smooth road reference line for autonomous driving vehicles based on 2d constrained smoothing spline |
US20180074507A1 (en) * | 2017-11-22 | 2018-03-15 | GM Global Technology Operations LLC | Road corridor |
CN108153328A (zh) * | 2017-12-18 | 2018-06-12 | 北京理工大学 | 一种基于分段贝塞尔曲线的多导弹协同航迹规划方法 |
US20200003564A1 (en) * | 2018-06-27 | 2020-01-02 | Baidu Usa Llc | Reference line smoothing method using piecewise spiral curves with weighted geometry costs |
CN109253735A (zh) * | 2018-11-30 | 2019-01-22 | 奇瑞汽车股份有限公司 | 路径规划方法、装置及存储介质 |
CN110749333A (zh) * | 2019-11-07 | 2020-02-04 | 中南大学 | 基于多目标优化的无人驾驶车辆运动规划方法 |
CN110949374A (zh) * | 2019-11-14 | 2020-04-03 | 江苏大学 | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 |
WO2021120200A1 (en) * | 2019-12-20 | 2021-06-24 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Spline curve and spiral curve based reference line smoothing method |
CN111551178A (zh) * | 2020-04-27 | 2020-08-18 | 广东工业大学 | 一种基于最短路径的分段轨迹时间规划方法 |
CN112068588A (zh) * | 2020-08-12 | 2020-12-11 | 浙江大学 | 一种基于飞行走廊和贝塞尔曲线的无人飞行器轨迹生成方法 |
CN112026772A (zh) * | 2020-08-14 | 2020-12-04 | 清华大学 | 一种智能网联汽车的路径实时规划与分布式控制方法 |
CN113031592A (zh) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统 |
Non-Patent Citations (6)
Title |
---|
QINGYANG LI: "3D Cubic Bezier Curves for Multi-target Path Planning for Autonomous Underwater Vehicles", 《OCEANS 2019 - MARSEILLE》 * |
VAHID HASSANI;SIMEN V.LANDE: "Path Planning for Marine Vehicles using Bézier Curves", 《IFAC-PAPERSONLINE》 * |
史恩秀; 黄玉美; 朱从民; 张亚旭: "差速驱动轮式移动机器人路径规划新策略", 《中国机械工程》 * |
周兵; 万希; 吴晓建; 陈晓龙; 曾凡沂: "紧急避撞工况下的路径规划与跟踪", 《湖南大学学报(自然科学版)》 * |
王少博: "动态场景下基于交互性预测的自动驾驶汽车轨迹规划方法研究", 《中国博士学位论文全文数据库·工程科技Ⅱ辑》 * |
蒲文东: "LCS四足机器人动力学建模及运动控制", 《中国优秀硕士学位论文全文数据库·信息科技辑》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114371712A (zh) * | 2022-01-11 | 2022-04-19 | 湖南大学 | 一种具有不停车绕障功能的泊车轨迹重规划方法 |
CN114509086A (zh) * | 2022-02-15 | 2022-05-17 | 湖南大学无锡智能控制研究院 | 智能车辆在连续弯道场景下的最优轨迹规划方法及系统 |
CN114509086B (zh) * | 2022-02-15 | 2022-11-25 | 湖南大学无锡智能控制研究院 | 智能车辆在连续弯道场景下的最优轨迹规划方法及系统 |
CN115933701A (zh) * | 2023-01-06 | 2023-04-07 | 北京理工大学 | 一种基于二次规划的安全走廊优化生成方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN113419534B (zh) | 2022-03-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113419534B (zh) | 一种基于贝塞尔曲线的转向路段路径规划方法 | |
CN110949374B (zh) | 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法 | |
CN109976329B (zh) | 一种车辆避障换道路径的规划方法 | |
WO2021227304A1 (zh) | 一种自动泊车路径规划的避障方法及泊车路径规划系统 | |
CN109927716B (zh) | 基于高精度地图的自主垂直泊车方法 | |
CN113916246B (zh) | 一种无人驾驶避障路径规划方法和系统 | |
WO2021135617A1 (zh) | 一种适用于多场景的自主泊车轨迹规划方法 | |
CN112193244B (zh) | 基于线性约束的自动驾驶车辆运动规划方法 | |
CN108088456A (zh) | 一种具有时间一致性的无人驾驶车辆局部路径规划方法 | |
WO2019042295A1 (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
CN110766220A (zh) | 一种结构化道路局部路径规划方法 | |
JP7083306B2 (ja) | 走行軌道決定処理及び自動運転装置 | |
JP7082940B2 (ja) | 走行軌道決定処理及び自動運転装置 | |
CN103158703A (zh) | 一种平行泊车的控制方法 | |
CN111653113A (zh) | 车辆的局部路径确定方法、装置、终端和存储介质 | |
CN114610016B (zh) | 基于障碍物动态虚拟膨胀的智能车辆避碰路径规划方法 | |
CN110553660A (zh) | 一种基于a*算法和人工势场的无人车轨迹规划方法 | |
CN111896004A (zh) | 一种狭窄通道车辆轨迹规划方法及系统 | |
CN113978452B (zh) | 一种自动平行泊车路径规划方法 | |
CN112141091A (zh) | 解决车位偏移和定位偏移的二次泊车方法、系统及车辆 | |
CN113335270B (zh) | 一种泊车路径规划方法和装置 | |
CN114852085A (zh) | 基于路权侵入度的车辆自动驾驶轨迹规划方法 | |
CN113525509A (zh) | 一种铰接车辆转向控制方法及装置 | |
CN116465427B (zh) | 一种基于时空风险量化的智能车辆换道避障路径规划方法 | |
CN112519783B (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 |