CN113701777B - 基于空间向量的高精地图车道关联轨迹线自动生成方法 - Google Patents

基于空间向量的高精地图车道关联轨迹线自动生成方法 Download PDF

Info

Publication number
CN113701777B
CN113701777B CN202110999218.2A CN202110999218A CN113701777B CN 113701777 B CN113701777 B CN 113701777B CN 202110999218 A CN202110999218 A CN 202110999218A CN 113701777 B CN113701777 B CN 113701777B
Authority
CN
China
Prior art keywords
lane
driving
cost function
vector
path
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
Application number
CN202110999218.2A
Other languages
English (en)
Other versions
CN113701777A (zh
Inventor
吕家瑾
杨莹
董万虎
钱赛男
费敏
林莹锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Institute Of Surveying And Mapping Science And Technology
Original Assignee
Zhejiang Institute Of Surveying And Mapping Science And Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Institute Of Surveying And Mapping Science And Technology filed Critical Zhejiang Institute Of Surveying And Mapping Science And Technology
Priority to CN202110999218.2A priority Critical patent/CN113701777B/zh
Publication of CN113701777A publication Critical patent/CN113701777A/zh
Application granted granted Critical
Publication of CN113701777B publication Critical patent/CN113701777B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明提出了一种基于空间向量的高精地图车道关联轨迹线自动生成方法，包括以下步骤：S1，构建所有车道和路口导向线的坐标信息；S2，搜索每个驶出车道对应的路口导向线及周边所有驶入车道信息；S3，对路口导向线进行分类，得到转向信息关联至对应车道；S4，计算各驶入车道在驶出车道的方位及向量角度，确定驶入及驶出车道配对组合；S5，在s‑q坐标系下生成所有的候选路径点，再根据四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系；S6，采用代价函数选择最优路径；S7，将车辆行驶轨迹线与点云进行叠加进行高程修正，形成高精度的三维车道关联轨迹线。本发明能够快速计算出各车道所有的行驶轨迹，更好的满足了自动驾驶企业对高精地图的需求。

Description

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

Claims (4)

1.一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，包括以下步骤：
S1，使用KD_tree构建所有车道和路口导向线的坐标信息；
S2，根据欧式距离搜索出每个驶出车道对应的路口导向线及周边所有驶入车道信息；
S3，使用卷积神经网络对S2得到的驶出车道对应的路口导向线进行分类，得到转向信息，所述卷积神经网络可采用resnet50、VGG-16、Inception v4、DenseNet中的任一种，所述转向信息关联至对应车道；
S4，计算各驶入车道在驶出车道的方位及向量角度，并结合驶出车道的转向信息，确定驶入及驶出车道配对组合；
S5，结合每个驶入驶出车道坐标，在s-q坐标系下生成所有的候选路径点，再根据四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系；
所述微分方程为：
Figure FDA0003858157540000011
其中θbf、kbf分别表示候选路径上每个点的切向角和曲率，dybf表示对ybf求导，dxbf表示对xbf求导，xbf'、xbf”分别表示对应点上xbf对s的一阶导数和二阶导数，ybf'、ybf”分别表示对应点上ybf对s的一阶导数和二阶导数，s表示候选路径车道的弧长；
S6，采用代价函数选择最优路径，作为自动驾驶车辆行驶轨迹线；
S7，将车辆行驶轨迹线与点云进行叠加进行高程修正，形成高精度的三维车道关联轨迹线。
2.根据权利要求1所述的一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，所述S4包括：
根据向量积右手定则求出驶入车道在驶出车道的方位；
根据公式
Figure FDA0003858157540000021
求出驶入驶出车道的夹角；其中x1、y1分别为向量1的横坐标和纵坐标，x2、y2分别为向量2的横坐标和纵坐标，θ表示向量1和向量2的夹角。
3.根据权利要求1所述的一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，所述S5包括：
在确定驶入及驶出车道配对之后，采用以下公式计算出候选的路径：
Figure FDA0003858157540000022
其中s表示候选路径车道的弧长，qstart为驶出道路点横向偏移量，qend为驶入道路点横向偏移量，sstart为驶出车道对应的弧长，send为驶入车道对应的弧长，系数a、b、c用边界条件法进行计算，通过更改qend值，在一个驶出驶入车道组中生成若干条行驶路径进行候选，最后使用四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系。
4.根据权利要求1所述的一种基于空间向量的高精地图车道关联轨迹线自动生成方法，其特征在于，所述S6中的代价函数包括：
f(i)＝wsaffsaf(i)+wsmofsmo(i)
其中fsaf(i)为安全性代价函数，fsmo(i)为平滑性代价函数，wsaf为安全性代价函数的权重，wsmo为平滑性代价函数的权重；
安全性代价函数：
Figure FDA0003858157540000023
其中N表示候选路径的条数；gi[k]是高斯离散函数，R[k+i]表示各候选路径与障碍物的碰撞结果，如果候选路径跨越障碍物或车道边缘即有碰撞，则R[k+i]＝1，无碰撞则R[k+i]＝0，i表示候选车道的序号；
Figure FDA0003858157540000031
其中σ是碰撞风险标准差，k是随机变量，e是自然基数；
平滑性代价函数：
fsmo(i)＝∫Ki 2(s)ds
其中Ki(s)是第i条路径上弧长为s位置的点曲率。
CN202110999218.2A 2021-08-28 2021-08-28 基于空间向量的高精地图车道关联轨迹线自动生成方法 Active CN113701777B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110999218.2A CN113701777B (zh) 2021-08-28 2021-08-28 基于空间向量的高精地图车道关联轨迹线自动生成方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110999218.2A CN113701777B (zh) 2021-08-28 2021-08-28 基于空间向量的高精地图车道关联轨迹线自动生成方法

Publications (2)

Publication Number Publication Date
CN113701777A CN113701777A (zh) 2021-11-26
CN113701777B true CN113701777B (zh) 2022-11-04

Family

ID=78656366

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110999218.2A Active CN113701777B (zh) 2021-08-28 2021-08-28 基于空间向量的高精地图车道关联轨迹线自动生成方法

Country Status (1)

Country Link
CN (1) CN113701777B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114446050B (zh) * 2021-12-29 2023-01-13 武汉中海庭数据技术有限公司 一种分布式的车道级引导线构建方法及装置
CN114964292B (zh) * 2022-05-30 2023-10-20 国汽智控(北京)科技有限公司 全局路径规划方法、装置、电子设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109459047A (zh) * 2018-12-24 2019-03-12 武汉大学 车辆高精度定位数据与导航地图精确匹配及变道行为探测方法
CN110111566A (zh) * 2019-04-19 2019-08-09 腾讯科技（深圳）有限公司 轨迹预测方法、装置和存储介质
CN110852353A (zh) * 2019-10-22 2020-02-28 上海眼控科技股份有限公司 路口分类方法及设备
CN110929655A (zh) * 2019-11-27 2020-03-27 厦门金龙联合汽车工业有限公司 一种行驶过程中车道线识别方法、终端设备及存储介质
CN111750887A (zh) * 2020-06-11 2020-10-09 上海交通大学 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统
CN112257762A (zh) * 2020-10-12 2021-01-22 武汉中海庭数据技术有限公司 一种异源高精度地图间的路网匹配方法及系统

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6988450B2 (ja) * 2017-12-22 2022-01-05 株式会社デンソー 交差点内の走行軌道データ生成装置、交差点内の走行軌道データ生成プログラム及び記憶媒体
US10761535B2 (en) * 2018-08-21 2020-09-01 GM Global Technology Operations LLC Intelligent vehicle navigation systems, methods, and control logic for multi-lane separation and trajectory extraction of roadway segments

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109459047A (zh) * 2018-12-24 2019-03-12 武汉大学 车辆高精度定位数据与导航地图精确匹配及变道行为探测方法
CN110111566A (zh) * 2019-04-19 2019-08-09 腾讯科技（深圳）有限公司 轨迹预测方法、装置和存储介质
CN110852353A (zh) * 2019-10-22 2020-02-28 上海眼控科技股份有限公司 路口分类方法及设备
CN110929655A (zh) * 2019-11-27 2020-03-27 厦门金龙联合汽车工业有限公司 一种行驶过程中车道线识别方法、终端设备及存储介质
CN111750887A (zh) * 2020-06-11 2020-10-09 上海交通大学 降低事故严重程度的无人驾驶车辆轨迹规划方法及系统
CN112257762A (zh) * 2020-10-12 2021-01-22 武汉中海庭数据技术有限公司 一种异源高精度地图间的路网匹配方法及系统

Also Published As

Publication number Publication date
CN113701777A (zh) 2021-11-26

Similar Documents

Publication Publication Date Title
CN110749333B (zh) 基于多目标优化的无人驾驶车辆运动规划方法
Li et al. Path planning based on combinaion of improved A-STAR algorithm and DWA algorithm
CN107702716B (zh) 一种无人驾驶路径规划方法、系统和装置
CN110320933B (zh) 一种巡航任务下无人机避障运动规划方法
CN113701777B (zh) 基于空间向量的高精地图车道关联轨迹线自动生成方法
Zhao et al. Dynamic motion planning for autonomous vehicle in unknown environments
CN110081894A (zh) 一种基于道路结构权值融合的无人车轨迹实时规划方法
CN113805597B (zh) 基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法
Ma et al. A two-level path planning method for on-road autonomous driving
CN112947406A (zh) 一种基于FLOYD和Astar的混合路径规划方法
CN115309161B (zh) 一种移动机器人路径规划方法、电子设备及存储介质
CN114964267B (zh) 一种无人牵引车在多任务点环境下的路径规划方法
CN114323051A (zh) 路口行驶轨迹的规划方法、装置和电子设备
CN116331264A (zh) 一种未知障碍物分布的避障路径鲁棒规划方法及系统
CN113467476A (zh) 考虑转角约束的无碰撞检测快速随机树全局路径规划方法
CN114815845A (zh) 一种基于HybridA*算法的自动驾驶农机平滑路径规划方法
CN113219990B (zh) 基于自适应邻域与转向代价的机器人路径规划方法
Wang et al. Research on autonomous planning method based on improved quantum Particle Swarm Optimization for Autonomous Underwater Vehicle
CN117109620A (zh) 基于采样的车辆行为与环境交互的自动驾驶路径规划方法
CN117029818A (zh) 一种非结构化封闭环境中多车协同路径规划方法
CN116429144A (zh) 基于改进Astar和DWA融合算法的自主车辆路径规划方法
CN114460933B (zh) 一种面向动态环境的移动机器人局部路径规划算法
Shen et al. A three-phase framework for global path planning for nonholonomic autonomous vehicles on 3D terrains
CN115079693A (zh) 基于改进遗传算法和b样条拟合的无人车路径规划方法
Wang et al. Research on Local Path Planning Algorithm Based on Frenet Coordinate System

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
CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: 311121 No. 2, Dixin Road, Wuchang Street, Yuhang District, Hangzhou City, Zhejiang Province

Patentee after: ZHEJIANG INSTITUTE OF SURVEYING AND MAPPING SCIENCE AND TECHNOLOGY

Country or region after: China

Address before: No. 83, Baoju North Road, Xihu District, Hangzhou City, Zhejiang Province, 310012

Patentee before: ZHEJIANG INSTITUTE OF SURVEYING AND MAPPING SCIENCE AND TECHNOLOGY

Country or region before: China