基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法
【专利说明】基于Bez i er曲线构建交叉路口驶入驶出车道L i nk的连接曲线
筲?异/古
技术领域
[0001 ]本发明涉及一种基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法，属于导航和电子地图的交叉领域。
【背景技术】
[0002]随着高精度导航平台的应用以及无人驾驶的逐步走向现实，无人驾驶车辆对车道线的识别成为一个重要的环节，但现实路况中交叉路口往往没有车道印刷线，如何确保无人驾驶车辆能在此情况下按照合理安全的行车轨迹通过路口是一个很重要的技术问题。
[0003]另一方面，传统导航系统基于道路级的路径规划已经不能满足高精度导航以及无人驾驶的要求。路径规划必须提升到车道级别。因此必须在高精度导航地图数据中建立车道级别的拓扑连接网络。在高精度地图制作过程中，由于驶入驶出交叉路口的车道Link是断开的，因此需要将驶入该路口的车道和驶出该路口的车道Link之间建立一条表示其连接关系的行车轨迹曲线。现有办法主要上有一下三种:
[0004](I)直接直线连接，该类方法最为简单，但只能表达拓扑连接性，直线本身无法贴合行车轨迹线，对无人驾驶的行车轨迹判读没有参考意义。
[0005](2)圆弧方式连接驶入车道Link的起点和驶出车道的结束点。这种方式虽然保证了曲线的光滑，但圆弧的定义比较困难，很难保证圆弧与驶入车道和驶出车道相切。同时也无法避免圆弧曲线穿越安全岛位置。
[0006](3)缓和曲线，该类曲线主要应用在高速道路的上下匝道上，在市区的交叉路口内，往往转弯角度小，不适合缓和曲线方式。
[0007]因此，提供一条与行车轨迹相贴合且安全性高的参考轨迹曲线，就成为一项非常重要的问题。
【发明内容】
[0008]有鉴于此，本发明提供一种与行车轨迹相贴合且安全性高的参考轨迹曲线的基于Bez i er曲线构建交叉路口驶入驶出车道Link的连接曲线算法。
[0009 ] 一种基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法，所述基于Bez i er曲线构建交叉路口驶入驶出车道Link的连接曲线算法包括以下步骤:
[0010]S1、获取车辆自驶入车道Link的驶入矢量信息，以及车辆自驶出车道Link的驶出矢量信息，并根据驶入矢量信息和驶出矢量信息得到车辆行驶轨迹的变化角度；
[0011]S2、设定多种阈值条件，根据阈值条件与变化角度的比较结果，确定车辆行驶轨迹的变化模式；
[0012]S3、根据车辆行驶轨迹的变化模式确定控制点，并基于Bezier曲线方程建交叉路口驶入驶出车道Link的连接曲线。
[0013]本发明所述基于Bezi er曲线构建交叉路口驶入驶出车道Link的连接曲线算法，通过Bezier曲线的性质能保证曲线与驶入车道Link和驶出车道Link的相切，同时利用中间控制点的位置，可以非常安全控制曲线穿越区域，避免发生穿越路口安全岛以及穿越道路边缘的情况，所生成的曲线与安全的行车轨迹线贴合。因此可以给无人驾驶车辆在通过路口时提供安全的行车轨迹参考线。本发明利用Bezier曲线的特性，建立了一种连接交叉路口驶入驶出车道Link的行车轨迹曲线的算法，该算法满足了与实际行车轨迹的贴合性和安全性，且算法统一，便于实现，不同走向的连接关系均可以使用该算法。
【附图说明】
[OOM]图1是本发明所述基于Bez ier曲线构建交叉路口驶入驶出车道Link的连接曲线算法的流程框图；
[0015]图2是图1中步骤SI的流程框图；
[0016]图3是本发明所述基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法的变化角度计算示意图；
[0017]图4是图1中步骤S2的流程框图；
[0018]图5是图1中步骤S3的流程框图；
[0019]图6是本发明所述基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法的左右转弯控制点计算示意图；
[0020]图7是本发明所述基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法的掉头专用道控制点计算示意图；
[0021 ]图8是本发明所述基于Bez ier曲线构建交叉路口驶入驶出车道Link的连接曲线算法的直行偏移控制点计算示意图。
【具体实施方式】
[0022]为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明，应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0023]如图1所示，本发明实施例提供一种基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法，所述基于Bezier曲线构建交叉路口驶入驶出车道Link的连接曲线算法包括以下步骤:
[0024]S1、获取车辆自驶入车道Link的驶入矢量信息，以及车辆自驶出车道Link的驶出矢量信息，并根据驶入矢量信息和驶出矢量信息得到车辆行驶轨迹的变化角度；
[0025]S2、设定多种阈值条件，根据阈值条件与变化角度的比较结果，确定车辆行驶轨迹的变化模式；
[0026]S3、根据车辆行驶轨迹的变化模式确定控制点，并基于Bezier曲线方程建交叉路口驶入驶出车道Link的连接曲线。
[0027]其中，如图2所示，所述步骤SI包括以下分步骤；
[0028]S11、根据车辆在驶入车道Link中的走向确定驶入矢量信息；
[0029]S12、根据车辆在驶出车道Link中的走向确定驶出矢量信息；
[0030]S13、驶入矢量信息和驶出矢量信息的延长线相交，形成车辆行驶轨迹的第一变化角度Al;
[0031]S14、驶入车道Link的最后一个坐标点为车辆驶入点，驶出车道Link的第一个坐标点为车辆驶出点，车辆驶入点与驶出点的连线相对驶出矢量信息相交，得到第二变化角度A2o
[0032]具体的，如图3所示，取驶入车道Link的最后一个坐标点Ps,然后在车道Link形状线上，距离Ps—定距离取一个坐标点Ps ’，一般该距离取一个车身的长度，为4.3到4.5米(但不限于此长度)。这样构成一个Ps->Ps，的一个向量Vl。
[0033]取驶出车道Link的最有一个坐标点Pe，然后在车道Link形状线上，距离Pe—定距离取第一个坐标点Pe’，一般该距离取一个车身的长度，为4.3到4.5米(但不限于此长度)。这样构成一个Pe_>Pe’的一个向量V2。
[0034]向量Vl和V2所发出的射线相交于01点，则计算线段PsOl和线段OlPe之间的夹角Al，Ps->Pe所构成向量V3，向量Vl和V3之间的夹角A2，如图3所示。
[0035 ] 如图4所示，所述步骤S2包括以下分步骤；
[0036]S21、设置车辆的转弯判定阈值nl、掉头判定阈值n2、同向偏移阈值n3 ；
[0037]S22、将第一变化角度Al和第二变化角度A2与各项阈值进行比较，设定多种判定条件；
[0038]S23、根据判定条件确定车辆行驶轨迹的变化模式。
[0039]具体的，所述车辆的行驶轨迹的变化模式包括直行模式、直行偏移模式、左(右)转弯模式、掉头模式。
[0040]所述多种判定条件包括:
[0041]判定l:Al〈nl 或者 Al>360-nl;
[0042]判定2:Al>270-nl 并且 Al〈270+nl;
[0043]判定3:Al>90-nl 并且 Al〈90+nl;
[0044]判定4:Α1>180+η2 并且 Al〈180-n2;
[0045]判定5:A2〈n3 或者 A2>360-n3。
[0046]所述根据判定条件确定车辆行驶轨迹的变化模式包括如下判定方法:
[0047]如果判定I成立并且判定5也成立，则认为车辆的行驶模式是直行方向；
[0048]如果判定I成立，但判定5不成立，则认为车辆的行驶模式是直行偏移方向；
[0049]如果判定I不成立，同时判定2或判定3成立，则认为车辆的行驶模式是左(右)转弯；
[0050]如果判定I，判定2，判定3均不成立，但判定4成立，则认为车辆的行驶模式是掉头方向。
[0051]如图5所示，所述步骤S3包括以下分步骤；
[0052]S31、根据车辆的行驶轨迹的变化模式确定Bezier曲线方程的阶层；
[0053]S32、通过Bez ier曲线方程确定控制点的数量和控制点的位置坐标；
[0054]S33、通过控制点构建交叉路口驶入驶出车道Link的连接曲线。
[0055]具体的，当车辆的行驶模式为直行方向，选择I次Bezier曲线方程，只有两个控制点Cl和C2。将驶入车道Link的Ps点作为控制点Cl;驶出车道Link的Pe作为控制点C2。
[0056]当车辆的行驶模式为左(右)转弯，选择2次Bezier曲线方程，有三个控制点Cl和C2，C3。将驶入车道Link的Ps点作为控制点Cl;驶出车道Link的Pe作为控制点C2，
[0057]其中，控制点C3可通过以下两种方式获得:
[0058](I)在路口有明显安全岛的情况，如图6所示，可以取安全岛的外围矩形的某个角点，设脚点符号记为，￥^1，￥以2，￥^3，￥故4。分别计算以上4个角点到直线段?8-汗6的垂直距离。取距离最短的角点作为第三个控制点C3。
[0059](2)如果没有明确的安全岛，可以将VI，和V2所发出的射线相交于01点作为第三个控制点C3。
[0060]所述2次Bezier曲线方程为P(x，y)=