本发明提出了一种基于空间向量的高精地图车道关联轨迹线自动生成方法，包括以下步骤：S1，构建所有车道和路口导向线的坐标信息；S2，搜索每个驶出车道对应的路口导向线及周边所有驶入车道信息；S3，对路口导向线进行分类，得到转向信息关联至对应车道；S4，计算各驶入车道在驶出车道的方位及向量角度，确定驶入及驶出车道配对组合；S5，在s‑q坐标系下生成所有的候选路径点，再根据四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系；S6，采用代价函数选择最优路径；S7，将车辆行驶轨迹线与点云进行叠加进行高程修正，形成高精度的三维车道关联轨迹线。本发明能够快速计算出各车道所有的行驶轨迹，更好的满足了自动驾驶企业对高精地图的需求。
1.一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，包括以下步骤：S1，使用KD_tree构建所有车道和路口导向线的坐标信息；
S2，根据欧式距离搜索出每个驶出车道对应的路口导向线及周边所有驶入车道信息；
S3，使用卷积神经网络对S2得到的驶出车道对应的路口导向线进行分类，得到转向信息，所述卷积神经网络可采用resnet50、VGG‑16、Inception v4、DenseNet中的任一种，所述转向信息关联至对应车道；
S4，计算各驶入车道在驶出车道的方位及向量角度，并结合驶出车道的转向信息，确定驶入及驶出车道配对组合；
S5，结合每个驶入驶出车道坐标，在s‑q坐标系下生成所有的候选路径点，再根据四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系；
其中θbf、kbf分别表示候选路径上每个点的切向角和曲率，dybf表示对ybf求导，dxbf表示对xbf求导，xbf'、xbf”分别表示对应点上xbf对s的一阶导数和二阶导数，ybf'、ybf”分别表示对应点上ybf对s的一阶导数和二阶导数，s表示候选路径车道的弧长；
S6，采用代价函数选择最优路径，作为自动驾驶车辆行驶轨迹线；
S7，将车辆行驶轨迹线与点云进行叠加进行高程修正，形成高精度的三维车道关联轨迹线。
2.根据权利要求1所述的一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，所述S4包括：根据向量积右手定则求出驶入车道在驶出车道的方位；
根据公式 求出驶入驶出车道的夹角；其中x1、y1分别为向量1的横坐标和纵坐标，x2、y2分别为向量2的横坐标和纵坐标，θ表示向量1和向量2的夹角。
3.根据权利要求1所述的一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，所述S5包括：在确定驶入及驶出车道配对之后，采用以下公式计算出候选的路径：其中s表示候选路径车道的弧长，qstart为驶出道路点横向偏移量，qend为驶入道路点横向偏移量，sstart为驶出车道对应的弧长，send为驶入车道对应的弧长，系数a、b、c用边界条件法进行计算，通过更改qend值，在一个驶出驶入车道组中生成若干条行驶路径进行候选，最后使用四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系。
4.根据权利要求1所述的一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，所述S6中的代价函数包括：f(i)＝wsaffsaf(i)+wsmofsmo(i)其中fsaf(i)为安全性代价函数，fsmo(i)为平滑性代价函数，wsaf为安全性代价函数的权重，wsmo为平滑性代价函数的权重；
其中N表示候选路径的条数；gi[k]是高斯离散函数，R[k+i]表示各候选路径与障碍物的碰撞结果，如果候选路径跨越障碍物或车道边缘即有碰撞，则R[k+i]＝1，无碰撞则R[k+i]＝0，i表示候选车道的序号；
其中σ是碰撞风险标准差，k是随机变量，e是自然基数；
其中Ki(s)是第i条路径上弧长为s位置的点曲率。
基于空间向量的高精地图车道关联轨迹线自动生成方法
技术领域
[0001] 本发明涉及高精度地图构建领域，尤其涉及一种基于空间向量的高精地图车道关联轨迹线自动生成方法。
背景技术
[0002] 为了辅助智能车辆进行自动驾驶定位定轨、全局和局部路径规划、车辆安全预警等路测和应用，需要在自动驾驶高精地图中构建道路、车道及道路附属要素之间的智能化关联规则之后，形成一个自动驾驶的行驶轨迹网。
[0003] 目前，国内外专家通过自由空间法建立目标物体的自由空间和连通图的方式确定物体的最优关联轨迹。通过栅格法将智能车辆自动驾驶的行驶环境进行二值化，根据障碍物位置划分自由栅格、障碍栅格的方式优化最优路径算法。通过模拟退火算法进行大规模样本的全局路径规划。
[0004] 以上算法的局限在于当环境复杂、障碍物较多时算法寻优效率较低，影响运行速度，得到结果时间较长。而高精地图是需要满足各自动驾驶车商、图商应用的最大化需求，以便进行完整、全面的实际路测，这就要求道路要素间的行驶轨迹线需要包含所有可行的、可避让障碍物的行驶轨迹，方便自动驾驶车辆能够准确快速的进行行驶路径规划，对此，本专利设计了一种基于空间向量的高精地图车道关联轨迹线自动生成方法。
发明内容
[0005] 本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种基于空间向量的高精地图车道关联轨迹线自动生成方法。
[0006] 为了实现本发明的上述目的，本发明提供了一种基于空间向量的高精地图车道关联轨迹线自动生成方法，包括以下步骤：
[0007] S1，使用KD_tree构建所有车道和路口导向线的坐标信息；
[0008] S2，根据欧式距离搜索出每个驶出车道对应的路口导向线及周边所有驶入车道信息；
[0009] 所述驶出车道指的是当前需要做空间搜索的车道，所述驶入车道指的是当前车道附近的所有可能会开进去的车道的集合。
[0010] S3，使用卷积神经网络对S2得到的驶出车道对应的路口导向线进行分类，分类的结果是该路口导向线是左转、右转、掉头等；得到转向信息，所述转向信息关联至对应车道；
[0011] S4，计算各驶入车道在驶出车道的方位及向量角度，并结合驶出车道的转向信息，确定驶入及驶出车道配对组合；
[0012] S5，结合每个驶入驶出车道坐标，在s‑q坐标系下生成所有的候选路径点，再根据四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系；
[0013] 所述s‑q坐标系为车道对应的弧长与道路点横向偏移量构成的坐标系；
[0014] S6，采用代价函数选择最优路径，作为自动驾驶车辆行驶轨迹线；
[0015] S7，将车辆行驶轨迹线与点云进行叠加进行高程修正，形成高精度的三维车道关联轨迹线；
[0016] 所述卷积神经网络可采用resnet50、VGG‑16、Inception v4、DenseNet中的任一种，优先选用resnet50；
[0017] 所述向量角度指的是驶入驶出车道间的向量夹角。
[0018] 进一步地，所述S4包括：
[0019] 根据向量积右手定则求出驶入车道在驶出车道的方位；
[0020] 根据公式 求出驶入驶出车道的夹角；其中x1、y1分别为向量1的横坐标和纵坐标，x2、y2分别为向量2的横坐标和纵坐标，θ表示向量1和向量2的夹角。
[0021] 进一步地，所述S5包括：
[0022] 在确定驶入及驶出车道配对之后，采用以下公式计算出候选的路径：
[0023]
[0024] 其中s表示车道对应的弧长即候选路径车道的弧长，qstart为驶出道路点横向偏移量，qend为驶入道路点横向偏移量，sstart为驶出车道对应的弧长，send为驶入车道对应的弧长，系数a、b、c用边界条件法进行计算(详见CHU K，LEE M，SUNWOO M.Local path planning for off‑road autonomous driving with avoidance of static obstacles【J】.IEEE Transactions on Intefligent Transportation Systems，2012，13(4)：1599一1616.)，通过更改qend值，在一个驶出驶入车道组中生成若干条行驶路径进行候选，最后使用四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系。
[0025] 进一步地，所述S6中的代价函数包括：
[0026] f(i)＝wsaffsaf(i)+wsmofsmo(i)
[0027] 其中fsaf(i)为安全性代价函数，fsmo(i)为平滑性代价函数，wsaf为安全性代价函数的权重，wsmo为平滑性代价函数的权重；
[0028] 为保证运算效率，所以采用离散的高斯卷积来结合碰撞结果，通过安全性代价函数计算出每条候选路径的碰撞风险。
[0029] 安全性代价函数：
[0030]
[0031] 其中N表示候选路径的条数；gi[k]是高斯离散函数，R[k+i]表示各候选路径与障碍物的碰撞结果，如果候选路径跨越障碍物或车道边缘即有碰撞，则R[k+i]＝1，无碰撞则R[k+i]＝0，i表示候选车道的序号，k是随机变量；
[0032]
[0033] 其中σ是碰撞风险标准差，本算法中设定σ＝2；k是随机变量，e是自然基数；平滑性代价函数：
[0034] fsmo(i)＝∫Ki2(s)ds
[0035] 其中Ki(s)是第i条路径上弧长为s位置的点曲率。
[0036] 综上所述，由于采用了上述技术方案，本发明的有益效果是：
[0037] 能够根据高精地图中道路及其附属要素信息、车辆长度、轨迹弧度等参数，快读计算出车道行驶轨迹，且生成的轨迹线包含了车辆能够行驶的所有路径组合，进而构建高精地图自动驾驶静态轨迹网。同其他轨迹生成方法相比，本发明在效率、生成轨迹完整性等方面有较大完善，更好的满足了自动驾驶企业对高精地图的需求。
[0038] 能够快速计算出各车道所有的行驶轨迹，其轨迹能够自动避让道路中的障碍物并满足驾驶舒适感。进而满足自动驾驶定位定轨、全局和局部路径规划、车辆安全预警等路测任务和其他应用。
[0039] 本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0040] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0041] 图1是本发明的流程示意图。
具体实施方式
[0042] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0043] 本发明提出一种基于空间向量的高精地图车道关联轨迹线自动生成方法，如图1所示。
[0044] 步骤一：将车道及路口导向线的各节点的三维坐标信息构建成一个三维空间kd_tree，方便进行空间查找；
[0045] 步骤二：遍历车道：在kd_tree中根据计算欧式距离搜索出每条车道能够驶入的车道组合，及车道附近的路口导向线。
[0046] 步骤三：将路口导向线转为二维图像，使用resnet50神经网络进行分类，确定导向线的类型，如左拐、直行、右转、汇入等。
[0047] 步骤四：根据右手定则求出驶入车道在驶出车道的方位，根据公式求出驶入驶出车道的夹角，结合根据路口导向线识别出来
的道路转向信息(如果附近无导向线则默认该车道可直行左拐右拐)，可判断驶出及驶入车道的组合。比如左拐车道可驶入在其左侧且向量角度在30度至150度之间的车道；
[0048] 步骤五：在确定驶入及驶出车道配对之后，采用以下公式计算出候选的路径：
[0049]
[0050] 其中s表示候选路径车道的弧长，qstart为驶出道路点横向偏移量，qend为驶入道路点横向偏移量、sstart为驶出车道对应的弧长、send为驶入车道对应的弧长，系数a、b、c用边界条件法进行计算，通过更改qend值，在一个驶出驶入车道组中生成若干条行驶路径进行候选；
[0051] 最后使用四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系，具体如下：
[0052] 微分方程为：
[0053]
[0054] 其中θbf、kbf分别表示候选路径上每个点的切向角和曲率，dybf表示对ybf求导，dxbf表示对xbf求导，xbf'、xbf”、ybf'、ybf”分别表示对应点上xbf和ybf对s的一阶导数和二阶导数，s表示候选路径车道的弧长。
[0055] 使用四阶龙格库塔法求解此微分方程，将路径点转换至笛卡尔坐标系。
[0056] 步骤六：通过代价函数的最小化获得代价函数的最小值，从而在候选路径中进行选择一条最优路径作为最后的结果，代价函数为：
[0057] f(i)＝wsaffsaf(i)+wsmofsmo(i)
[0058] 其中fsaf(i)为安全性代价函数，fsmo(i)为平滑性代价函数，wsaf为安全性代价函数的权重，wsmo为平滑性代价函数的权重，不同的权重能够代表驾驶风格，根据实验结果优选wsaf为0.7，wsmo为0.3。
[0059] 安全性代价函数：
[0060]
[0061] 其中gi[k]是高斯离散函数，
[0062] 平滑性代价函数：
[0063] fsmo(i)＝∫Ki2(s)ds
[0064] 其中Ki(s)是第i条路径上弧长为s位置的点曲率。可得出车道行驶的最优路径。
[0065] 步骤七：将实地采集的点云数据构建三维kd_tree，根据欧氏距离搜索路径各节点附近的点云，取点云高程均值对各节点高程进行修正，最终得到高精度的三维车道关联轨迹线。
[0066] 尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。
