CN113701777B - 基于空间向量的高精地图车道关联轨迹线自动生成方法 - Google Patents
基于空间向量的高精地图车道关联轨迹线自动生成方法 Download PDFInfo
- 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
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包括:
根据向量积右手定则求出驶入车道在驶出车道的方位;
进一步地,所述S5包括:
在确定驶入及驶出车道配对之后,采用以下公式计算出候选的路径:
其中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为平滑性代价函数的权重;
为保证运算效率,所以采用离散的高斯卷积来结合碰撞结果,通过安全性代价函数计算出每条候选路径的碰撞风险。
安全性代价函数:
其中N表示候选路径的条数;gi[k]是高斯离散函数,R[k+i]表示各候选路径与障碍物的碰撞结果,如果候选路径跨越障碍物或车道边缘即有碰撞,则R[k+i]=1,无碰撞则R[k+i]=0,i表示候选车道的序号,k是随机变量;
其中σ是碰撞风险标准差,本算法中设定σ=2;k是随机变量,e是自然基数;平滑性代价函数:
fsmo(i)=∫Ki 2(s)ds
其中Ki(s)是第i条路径上弧长为s位置的点曲率。
综上所述,由于采用了上述技术方案,本发明的有益效果是:
能够根据高精地图中道路及其附属要素信息、车辆长度、轨迹弧度等参数,快读计算出车道行驶轨迹,且生成的轨迹线包含了车辆能够行驶的所有路径组合,进而构建高精地图自动驾驶静态轨迹网。同其他轨迹生成方法相比,本发明在效率、生成轨迹完整性等方面有较大完善,更好的满足了自动驾驶企业对高精地图的需求。
能够快速计算出各车道所有的行驶轨迹,其轨迹能够自动避让道路中的障碍物并满足驾驶舒适感。进而满足自动驾驶定位定轨、全局和局部路径规划、车辆安全预警等路测任务和其他应用。
本发明的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解,其中:
图1是本发明的流程示意图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。
本发明提出一种基于空间向量的高精地图车道关联轨迹线自动生成方法,如图1所示。
步骤一:将车道及路口导向线的各节点的三维坐标信息构建成一个三维空间kd_tree,方便进行空间查找;
步骤二:遍历车道:在kd_tree中根据计算欧式距离搜索出每条车道能够驶入的车道组合,及车道附近的路口导向线。
步骤三:将路口导向线转为二维图像,使用resnet50神经网络进行分类,确定导向线的类型,如左拐、直行、右转、汇入等。
步骤四:根据右手定则求出驶入车道在驶出车道的方位,根据公式求出驶入驶出车道的夹角,结合根据路口导向线识别出来的道路转向信息(如果附近无导向线则默认该车道可直行左拐右拐),可判断驶出及驶入车道的组合。比如左拐车道可驶入在其左侧且向量角度在30度至150度之间的车道;
步骤五:在确定驶入及驶出车道配对之后,采用以下公式计算出候选的路径:
其中s表示候选路径车道的弧长,qstart为驶出道路点横向偏移量,qend为驶入道路点横向偏移量、sstart为驶出车道对应的弧长、send为驶入车道对应的弧长,系数a、b、c用边界条件法进行计算,通过更改qend值,在一个驶出驶入车道组中生成若干条行驶路径进行候选;
最后使用四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系,具体如下:
微分方程为:
其中θ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。
安全性代价函数:
平滑性代价函数:
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坐标系下生成所有的候选路径点,再根据四阶龙格库塔法求解微分方程将路径点转换至笛卡尔坐标系;
所述微分方程为:
其中θbf、kbf分别表示候选路径上每个点的切向角和曲率,dybf表示对ybf求导,dxbf表示对xbf求导,xbf'、xbf”分别表示对应点上xbf对s的一阶导数和二阶导数,ybf'、ybf”分别表示对应点上ybf对s的一阶导数和二阶导数,s表示候选路径车道的弧长;
S6,采用代价函数选择最优路径,作为自动驾驶车辆行驶轨迹线;
S7,将车辆行驶轨迹线与点云进行叠加进行高程修正,形成高精度的三维车道关联轨迹线。
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是自然基数;
平滑性代价函数:
fsmo(i)=∫Ki 2(s)ds
其中Ki(s)是第i条路径上弧长为s位置的点曲率。
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)
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)
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)
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 |
-
2021
- 2021-08-28 CN CN202110999218.2A patent/CN113701777B/zh active Active
Patent Citations (6)
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) | 基于多目标优化的无人驾驶车辆运动规划方法 | |
CN107702716B (zh) | 一种无人驾驶路径规划方法、***和装置 | |
Li et al. | Path planning based on combinaion of improved A-STAR algorithm and DWA algorithm | |
CN110320933B (zh) | 一种巡航任务下无人机避障运动规划方法 | |
CN113701777B (zh) | 基于空间向量的高精地图车道关联轨迹线自动生成方法 | |
CN110031011A (zh) | 集成时空邻近与改进权重圆的车载实时地图匹配计算方法 | |
Ma et al. | A two-level path planning method for on-road autonomous driving | |
CN113805597B (zh) | 基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法 | |
CN112577506B (zh) | 一种自动驾驶局部路径规划方法和*** | |
CN112947406A (zh) | 一种基于FLOYD和Astar的混合路径规划方法 | |
CN115309161B (zh) | 一种移动机器人路径规划方法、电子设备及存储介质 | |
CN114964267B (zh) | 一种无人牵引车在多任务点环境下的路径规划方法 | |
CN116331264A (zh) | 一种未知障碍物分布的避障路径鲁棒规划方法及*** | |
CN113467476A (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
CN113219990B (zh) | 基于自适应邻域与转向代价的机器人路径规划方法 | |
CN114815845A (zh) | 一种基于HybridA*算法的自动驾驶农机平滑路径规划方法 | |
CN114323051A (zh) | 路口行驶轨迹的规划方法、装置和电子设备 | |
Wang et al. | Research on autonomous planning method based on improved quantum Particle Swarm Optimization for Autonomous Underwater Vehicle | |
CN117109620A (zh) | 基于采样的车辆行为与环境交互的自动驾驶路径规划方法 | |
CN117029818A (zh) | 一种非结构化封闭环境中多车协同路径规划方法 | |
CN115079693A (zh) | 基于改进遗传算法和b样条拟合的无人车路径规划方法 | |
CN114460933A (zh) | 一种面向动态环境的移动机器人局部路径规划算法 | |
CN113739805A (zh) | 路径规划信息采集方法、路径规划方法及电子设备 | |
Shen et al. | A three-phase framework for global path planning for nonholonomic autonomous vehicles on 3D terrains | |
CN113670632A (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 |