一种基于贝塞尔曲线的转向路段路径规划方法
阅读说明：本技术 一种基于贝塞尔曲线的转向路段路径规划方法 (Bezier curve-based steering road section path planning method ) 是由 秦兆博 陈鑫 边有钢 秦晓辉 胡满江 徐彪 秦洪懋 谢国涛 王晓伟 丁荣军 于 2021-07-01 设计创作，主要内容包括：本发明实施例提供一种基于贝塞尔曲线的转向路段路径规划方法,以三条贝塞尔曲线规划所述车辆的转向路径,第一条贝塞尔曲线位于所述第一条道路,与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于所述第一条道路和所述第二条道路的重叠区域,与所述第二条贝塞尔曲线连接的第三条贝塞尔曲线位于所述第二条道路。采用本发明实施例提供的方法,设置路径规划的起始状态和终止状态,建立路径规划的模型和限制条件,能够保证贝塞尔曲线的避障要求,满足路径规划的需要。(The embodiment of the invention provides a method for planning a path of a turning road section based on Bezier curves, wherein three Bezier curves are used for planning the turning path of a vehicle, a first Bezier curve is positioned on a first road, a second Bezier curve connected with the first Bezier curve is positioned in an overlapping area of the first road and the second road, and a third Bezier curve connected with the second Bezier curve is positioned on the second road. By adopting the method provided by the embodiment of the invention, the initial state and the termination state of the path planning are set, and the model and the limiting conditions of the path planning are established, so that the obstacle avoidance requirement of the Bezier curve can be ensured, and the requirement of the path planning can be met.)
技术领域
本发明涉及智能网联汽车
技术领域
，具体涉及一种基于贝塞尔曲线的转向路段路径规划方法。背景技术
随着电子技术以及人工智能算法的飞速发展，自动驾驶领域的技术积累越来越丰富。自动驾驶的应用场景主要包括结构化道路和非结构画道路，其中，结构化道路常见的任务包括换道行驶以及转向行驶。
作为自动驾驶系统中的关键技术，路径规划决定着车辆的行驶路线，对车辆的安全性、舒适性具有较高的影响。现有的路径规划技术中，为了满足曲率连续约束，常常采用贝塞尔曲线规划路径。然而，大多数方法都不能保证贝塞尔曲线的避障要求，存在一定的安全隐患。
发明内容
本发明的目的在于提供一种基于贝塞尔曲线的转向路段路径规划方法来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种基于贝塞尔曲线的转向路段路径规划方法，所述转向路段包括车辆转向前的第一条道路和转向后的第二条道路，以三条贝塞尔曲线规划所述车辆的转向路径，第一条贝塞尔曲线位于所述第一条道路，与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于所述第一条道路和所述第二条道路的重叠区域，与所述第二条贝塞尔曲线连接的第三条贝塞尔曲线位于所述第二条道路，所述方法包括：
步骤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'各条边的表达式为：
的表达式：a11x+b11y+c11＝0；
lA'B'的表达式：a12x+b12y+c12＝0；
lB'D'的表达式：a13x+b13y+c13＝0；
的表达式：a14x+b14y+c14＝0
收缩后的四边形的四条边E'F′D'C′2可表示为：
lE′F′的表达式：a21x+b21y+c21＝0
的表达式：a22x+b22y+c22＝0
的表达式：a23x+b23y+c23＝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为车辆的后悬长度。
q在一种实现方式中，由于上述优化模型中，s∈(0,1)是连续变量，因此将s进行离散，令并带入到上述约束中，即可得到离散的约束表达式如下：
其中：
C(xi，j,yi，j)为不等式约束的一般形式，B(xi，j,yi，j)为等式约束的一般形式。
采用序列二次规划算法，以步骤400得到的初始路径为优化模型初始解，求解上述式(3)，即可得到最优的决策变量：即贝塞尔曲线的控制点横纵坐标。将求出的控制点坐标带入下述公式中，即可得到最终的转向路径坐标：
采用本发明实施例提供的方法，设置路径规划的起始状态和终止状态，建立路径规划的模型和限制条件，能够保证贝塞尔曲线的避障要求，满足路径规划的需要。