CN114676939A - 一种多车型参数自适应的参考线平滑方法和系统 - Google Patents

一种多车型参数自适应的参考线平滑方法和系统 Download PDF

Info

Publication number
CN114676939A
CN114676939A CN202210580074.1A CN202210580074A CN114676939A CN 114676939 A CN114676939 A CN 114676939A CN 202210580074 A CN202210580074 A CN 202210580074A CN 114676939 A CN114676939 A CN 114676939A
Authority
CN
China
Prior art keywords
vehicle
lane
reference line
information
optimized
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.)
Granted
Application number
CN202210580074.1A
Other languages
English (en)
Other versions
CN114676939B (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 Lab
Original Assignee
Zhejiang Lab
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 Lab filed Critical Zhejiang Lab
Priority to CN202210580074.1A priority Critical patent/CN114676939B/zh
Publication of CN114676939A publication Critical patent/CN114676939A/zh
Application granted granted Critical
Publication of CN114676939B publication Critical patent/CN114676939B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/15—Vehicle, aircraft or watercraft design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G06Q50/40—
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Geometry (AREA)
  • General Physics & Mathematics (AREA)
  • Human Resources & Organizations (AREA)
  • General Engineering & Computer Science (AREA)
  • Economics (AREA)
  • Strategic Management (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Development Economics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Game Theory and Decision Science (AREA)
  • Computational Mathematics (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Marketing (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种多车型参数自适应的参考线平滑方法和系统，该方法包括：步骤一，使用高精地图，定位自车全局位置，根据决策给出目标车道信息；步骤二，计算自车在以原始道路中心线为参考线的Frenet坐标系下的起始坐标；步骤三，从起始坐标开始，等距采样离散原始参考点，并获取对应的车道宽度，再判断自车所在的当前车道的邻道状态，构造出自车可行驶区域；步骤四，构造参考线平滑的代价函数和约束条件，调用优化算法求解库对优化需优化的参考点，得到最优化结果；步骤五，计算出所述最优化结果对应朝向角及曲率，输出平滑后的参考线。本发明可以根据车型参数调节参考线平滑程度，降低轨迹规划难度并提升规划算法的稳定性、舒适性及安全性。

Description

一种多车型参数自适应的参考线平滑方法和系统
技术领域
本发明属于自动驾驶轨迹规划技术领域，涉及一种多车型参数自适应的参考线平滑方法和系统。
背景技术
在自动驾驶的轨迹规划领域，应用在城市公开道路上的轨迹规划方法大多依赖于Frenet坐标系，而基于Frenet坐标系的轨迹规划算法又依赖于参考线的平滑程度（一阶或者二阶连续）。如果参考线直接来源于高精地图的原始车道中心线，由于地图的制作误差、道路连接处不平滑等原因，参考线的平滑程度无法达到后续轨迹规划的需求，因此需要对原始的地图车道中心线即参考线进行平滑。
现有的参考线平滑算法如百度Apollo中的基于样条曲线的参考线平滑方法、基于螺旋线的参考线平滑方法和基于离散点的参考线平滑方法，都只考虑了参考线的连续性，而忽略了不同车型参数（包含轴距、最大方向转角等等）的限制，无法做到不同车型参数的自适应。如果在参考线平滑层不考虑车型参数，则会为后续的轨迹规划带来极大的难度。
发明内容
为了解决现有技术中存在的上述技术问题，本发明提出了一种多车型参数自适应的参考线平滑方法和系统，其具体技术方案如下：
一种多车型参数自适应的参考线平滑方法，包括以下步骤：
步骤一，使用高精地图，定位出自车全局位置，根据用户决策给出目标车道信息；
步骤二，根据定位给出的自车全局位置及用户决策给出的目标车道信息，计算自车在以原始道路中心线为参考线的Frenet坐标系下的起始坐标；
步骤三，从起始坐标开始，等距采样车道中心线上的离散原始参考点，并获取每个离散原始参考点对应的车道宽度，再判断自车所在的当前车道的邻道状态，根据判断结果构造出自车可行驶区域；
步骤四，构造参考线平滑的代价函数和约束条件，并根据所述代价函数和约束条件，调用优化算法求解库对需优化的参考点进行优化，得到最优化结果；
步骤五，利用最优化结果，计算出所述最优化结果对应朝向角及曲率，输出平滑后的参考线。
进一步地，所述步骤三具体包括以下子步骤：
步骤3.1，对高精地图中的目标车道，从起始坐标
Figure 469106DEST_PATH_IMAGE001
开始以
Figure 230389DEST_PATH_IMAGE002
为采样间隔，在固定长度内，等距采样车道中心线信息，包含n个离散原始参考点（
Figure 821907DEST_PATH_IMAGE003
）、(
Figure 985035DEST_PATH_IMAGE004
), …，(
Figure 105438DEST_PATH_IMAGE005
)；其中
Figure 619596DEST_PATH_IMAGE006
表示直角系坐标，
Figure 483647DEST_PATH_IMAGE007
表示向量
Figure 265396DEST_PATH_IMAGE008
与直角坐标系中X轴方向的夹角，
Figure 822279DEST_PATH_IMAGE009
表示为离散原始参考点的曲率，i∈（0，1，2，…，n-1）；
步骤3.2，对目标车道的中心线从起始坐标
Figure 558154DEST_PATH_IMAGE001
开始以
Figure 960316DEST_PATH_IMAGE002
为采样间隔，等距采样得到每个离散原始参考点对应的车道宽度序列
Figure 363616DEST_PATH_IMAGE010
；
步骤3.3，再根据高精地图中的车道线类型信息，判断当前车道的邻道是否可借用，若可借用则对相邻车道以起始坐标
Figure 91400DEST_PATH_IMAGE001
开始以等距采样间隔采样得相邻车道宽度序列
Figure 48992DEST_PATH_IMAGE011
，最终相加得可行驶区域序列
Figure 520425DEST_PATH_IMAGE012
，其中
Figure 778231DEST_PATH_IMAGE013
，若无法借用邻道则使
Figure 909872DEST_PATH_IMAGE011
都为0。
进一步地，所述目标车道的剩余长度无法满足固定长度需求时，则根据全局导航信息及高精地图信息获取目标车道的后续车道，从后续车道的起始点开始，以
Figure 620339DEST_PATH_IMAGE002
为采样间隔，等距采样对应的后续车道中心线信息，并加入至离散原始参考点序列中，直至满足长度需求或抵达全局终点。
进一步地，所述步骤四具体包括以下子步骤：
步骤4.1：计算出参考线平滑的代价函数，包含两部分，第一部分为需优化的参考点即待优化变量，与原始道路中心线参考点的相似代价
Figure 629884DEST_PATH_IMAGE014
；第二部分为平滑代价
Figure 742196DEST_PATH_IMAGE015
，得到代价函数表达式如下所示：
Figure 811783DEST_PATH_IMAGE016
其中相似代价设置为待优化变量与原始道路中心线参考点的欧式距离，即
Figure 743967DEST_PATH_IMAGE017
，平滑代价设置为三点之间的直线相似程度，即
Figure 557203DEST_PATH_IMAGE018
；其中，
Figure 524022DEST_PATH_IMAGE019
为待优化变量（
Figure 498931DEST_PATH_IMAGE020
）、(
Figure 948104DEST_PATH_IMAGE021
), …，(
Figure 299451DEST_PATH_IMAGE022
)，
Figure 120777DEST_PATH_IMAGE023
为原始道路中心线坐标点（
Figure 266587DEST_PATH_IMAGE024
）、(
Figure 438943DEST_PATH_IMAGE025
), …，(
Figure 593981DEST_PATH_IMAGE026
)，
Figure 269813DEST_PATH_IMAGE027
为平滑代价权重，为固定值，
Figure 852104DEST_PATH_IMAGE028
为相似代价权重，与车辆轴距L值成负相关，与车辆最大转向能力
Figure 511755DEST_PATH_IMAGE029
成正相关；
步骤4.2：根据可行驶区域序列
Figure 703440DEST_PATH_IMAGE012
以及采样间隔
Figure 499357DEST_PATH_IMAGE002
，得到每一个待优化变量偏离离散原始参考点的横纵向约束，表达式为：
Figure 518129DEST_PATH_IMAGE030
；
步骤4.3：根据离散原始参考点的朝向角
Figure 133918DEST_PATH_IMAGE007
，将步骤4.2中每一待优化变量的横纵向约束进行旋转至与对应离散原始参考点的朝向一致，即乘以由朝向角
Figure 630759DEST_PATH_IMAGE007
构建的旋转矩阵
Figure 281183DEST_PATH_IMAGE031
，更新步骤4.2的约束条件，表达式为：
Figure 470856DEST_PATH_IMAGE032
；
步骤4.4：再根据车型参数及离散原始参考点的曲率
Figure 839520DEST_PATH_IMAGE009
来设计约束条件的横向偏移量
Figure 874472DEST_PATH_IMAGE033
，并更新约束条件如下所示：
Figure 644982DEST_PATH_IMAGE034
简写为：
Figure 238512DEST_PATH_IMAGE035
其中，
Figure 828893DEST_PATH_IMAGE036
，
Figure 667536DEST_PATH_IMAGE037
，
Figure 558132DEST_PATH_IMAGE038
步骤4.5：根据步骤4.1得到的代价函数和步骤4.4得到的约束条件，调用优化算法求解库求解含线性约束的最优化问题，即对待优化变量
Figure 824028DEST_PATH_IMAGE039
进行优化，得到最终的最优化结果
Figure 901706DEST_PATH_IMAGE040
：
Figure 278461DEST_PATH_IMAGE041
Figure 23563DEST_PATH_IMAGE042
。
进一步地，所述横向偏移量
Figure 460360DEST_PATH_IMAGE033
正负与曲率的正负一致，即右转曲率为负时往Frenet坐标系下的
Figure 523869DEST_PATH_IMAGE043
轴负方向偏移，左转曲率为正时往Frenet坐标系下的
Figure 704314DEST_PATH_IMAGE043
轴正方向偏移，大小与曲率的大小成正相关，
Figure 569502DEST_PATH_IMAGE043
轴表示车辆距离道路中心线的横向距离；横向偏移量
Figure 177201DEST_PATH_IMAGE033
的大小与车辆轴距L值正相关，即车辆轴距越长，横向需要偏移得越多，与车辆最大转向能力
Figure 963892DEST_PATH_IMAGE029
成负相关，即车辆的最大转角能力越大，横向需要偏移得越少。
进一步地，所述步骤五具体为：利用最优化结果
Figure 213607DEST_PATH_IMAGE040
，计算出最优化结果
Figure 402143DEST_PATH_IMAGE040
对应的朝向角及曲率，最终输出为一系列平滑后的离散点信息（
Figure 446323DEST_PATH_IMAGE044
）、(
Figure 720309DEST_PATH_IMAGE045
),…，(
Figure 741092DEST_PATH_IMAGE046
)。
一种多车型参数自适应的参考线平滑系统，包括：局部规划地图生成模块和参考线平滑模块；所述的局部规划地图生成模块，输入为高精地图信息、决策目标车道信息、全局导航信息及全局定位信息，输出为固定长度的局部地图，包括道路中心线及可行驶区域空间信息；所述的参考线平滑模块，输入为局部规划地图生成模块的输出、车辆参数，构造参考线平滑的代价函数和约束条件，并根据所述代价函数和约束条件，调用优化算法求解库对需优化的参考点进行优化，得到最优化结果，利用最优化结果，计算出所述最优化结果对应朝向角及曲率，输出为一条平滑后的参考线。
一种多车型参数自适应的参考线平滑装置，包括一个或多个处理器，用于实现所述的一种多车型参数自适应的参考线平滑方法。
一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的一种多车型参数自适应的参考线平滑方法。
有益效果：
本发明提出的参考线平滑方法和系统可以根据车型参数自适应地调节参考线的平滑程度以及内外切方向，做到针对不同车型生成合理的参考线路径，降低后续的轨迹规划难度并提升规划算法的稳定性、舒适性及安全性。
附图说明
图1是本发明的一种多车型参数自适应的参考线平滑方法流程示意图；
图2是本发明参考线平滑方法的流程框图；
图3是本发明方法的参考点优化示意图；
图4是本发明的一种多车型参数自适应的参考线平滑装置结构示意图。
具体实施方式
为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。
如图1和图2所示，一种多车型参数自适应的参考线平滑方法，包括以下步骤：
步骤一，使用高精地图，定位出自车全局位置，根据用户决策给出目标车道信息。
具体的，在本实施例中高精地图为包含基础道路几何信息、拓扑关系、道路中心线的曲线表达式或离散点等信息的地图，全局位置由定位系统GPS给出与高精地图同一参考系下的坐标，用户决策为规划上游决策模块提供的表示自动驾驶汽车意图的信息，目标车道信息可以为同向的任一一个车道。
步骤二，根据定位给出的自车全局位置及用户决策给出的目标车道信息，计算自车在以原始道路中心线为参考线的Frenet坐标系下的起始坐标；其中，自车全局位置
Figure 315293DEST_PATH_IMAGE047
，起始坐标
Figure 264795DEST_PATH_IMAGE001
。
具体的，在本实施例中首先对原始道路中心线进行采样，再遍历搜索找到离自车全局位置最近的点作为参考点
Figure 291657DEST_PATH_IMAGE049
，Frenet坐标系下的起始坐标
Figure 352016DEST_PATH_IMAGE051
=
Figure 46303DEST_PATH_IMAGE053
计算公式如下：
Figure 166706DEST_PATH_IMAGE055
，
Figure 415284DEST_PATH_IMAGE056
其中，
Figure 544914DEST_PATH_IMAGE058
表示沿着参考线的方向，
Figure 326663DEST_PATH_IMAGE060
表示参考线的法向。
步骤三，从起始坐标开始，等距采样车道中心线上的离散原始参考点，并获取每个离散原始参考点对应的车道宽度，再判断自车所在的当前车道的邻道状态，根据判断结果构造出自车可行驶区域；具体包括以下子步骤：
步骤3.1，对高精地图中的目标车道，从起始坐标
Figure 617967DEST_PATH_IMAGE001
开始以
Figure 353842DEST_PATH_IMAGE002
为采样间隔，在固定长度内，等距采样车道中心线信息，包含n个离散原始参考点（
Figure 21584DEST_PATH_IMAGE003
）、(
Figure 424884DEST_PATH_IMAGE004
), …，(
Figure 887089DEST_PATH_IMAGE005
)；其中
Figure 110260DEST_PATH_IMAGE006
表示直角系坐标，
Figure 581692DEST_PATH_IMAGE007
表示向量
Figure 839498DEST_PATH_IMAGE008
与直角坐标系中X轴方向的夹角，
Figure 971140DEST_PATH_IMAGE009
表示为离散原始参考点的曲率，i∈（0，1，2，…，n-1）；本实施例中
Figure 681607DEST_PATH_IMAGE002
取0.25m,固定长度为50m；
若目标车道的剩余长度无法满足固定长度需求，则根据全局导航信息及高精地图信息获取目标车道的后续车道。从后续车道的起始点开始，以
Figure 691152DEST_PATH_IMAGE002
为采样间隔，等距采样对应的后续车道中心线信息，并加入至离散原始参考点序列中，直至满足长度需求或抵达全局终点。
具体的，在本实施例中，若目标车道剩余行进长度无法满足50m需求，则根据高精地图中的车道先后连接关系获取目标车道的后续车道，若目标车道的后续车道包含多条，则根据全局导航信息来从中挑选出下一条必经车道。从后续车道的起始点开始，以0.25m为采样间隔进行等距采样，并将采样点加入至离散原始参考点序列中，直至达到50m或者抵达任务终点。
步骤3.2，对目标车道的中心线从起始坐标
Figure 537885DEST_PATH_IMAGE001
开始以
Figure 873051DEST_PATH_IMAGE002
为采样间隔，等距采样得到每个离散原始参考点对应的车道宽度序列
Figure 805235DEST_PATH_IMAGE010
；
步骤3.3，再根据高精地图中的车道线类型信息（实线，虚线），判断当前车道的邻道（右转状态邻道找左车道，左转状态邻道找右车道）是否可借用，若可借用则对相邻车道以起始坐标
Figure 618470DEST_PATH_IMAGE001
开始以0.25m为采样间隔采样得相邻车道宽度序列
Figure 319710DEST_PATH_IMAGE011
，最终相加得可行驶区域序列
Figure 825778DEST_PATH_IMAGE012
，其中
Figure 743793DEST_PATH_IMAGE013
，若无法借用邻道则使
Figure 95140DEST_PATH_IMAGE011
都为0。
步骤四，构造参考线平滑的代价函数和约束条件，并根据所述代价函数和约束条件，调用优化算法求解库对需优化的参考点进行优化，得到最优化结果。
步骤4.1：计算出参考线平滑的代价函数，包含两部分，第一部分为需优化的参考点即待优化变量，与原始道路中心线参考点的相似代价
Figure 916465DEST_PATH_IMAGE014
；第二部分为平滑代价
Figure 593434DEST_PATH_IMAGE015
，得到代价函数表达式如下所示：
Figure 500211DEST_PATH_IMAGE061
其中相似代价设置为待优化变量与原始道路中心线参考点的欧式距离，即
Figure 655248DEST_PATH_IMAGE017
，平滑代价可以设置为三点之间的直线相似程度，即
Figure 596660DEST_PATH_IMAGE018
；其中，
Figure 178951DEST_PATH_IMAGE019
为待优化变量（
Figure 573023DEST_PATH_IMAGE020
）、(
Figure 764708DEST_PATH_IMAGE021
), …，(
Figure 826204DEST_PATH_IMAGE022
)，
Figure 313818DEST_PATH_IMAGE023
为原始道路中心线坐标点（
Figure 195186DEST_PATH_IMAGE024
）、(
Figure 692026DEST_PATH_IMAGE025
), …，(
Figure 608030DEST_PATH_IMAGE026
)，
Figure 532123DEST_PATH_IMAGE027
为平滑代价权重，可设为固定值，
Figure 635209DEST_PATH_IMAGE028
为相似代价权重，可设计为与车辆轴距L值成负相关，与车辆最大转向能力
Figure 935740DEST_PATH_IMAGE029
成正相关。
步骤4.2：根据可行驶区域序列
Figure 706250DEST_PATH_IMAGE012
以及采样间隔0.25m，可得到每一个待优化变量偏离离散原始参考点的横纵向约束，如下所示：
Figure 299780DEST_PATH_IMAGE062
步骤4.3：根据离散原始参考点的朝向角
Figure 890161DEST_PATH_IMAGE007
，将步骤4.2中每一待优化变量的横纵向约束进行旋转至与对应离散原始参考点的朝向一致，即乘以由朝向角
Figure 728804DEST_PATH_IMAGE007
构建的旋转矩阵
Figure 619400DEST_PATH_IMAGE031
，更新步骤4.2的约束条件如下所示：
Figure 885296DEST_PATH_IMAGE063
步骤4.4：根据车型参数及离散原始参考点的曲率
Figure 962974DEST_PATH_IMAGE009
来设计约束条件的横向偏移量
Figure 339728DEST_PATH_IMAGE033
；横向偏移量
Figure 84830DEST_PATH_IMAGE033
正负与曲率的正负一致，即右转曲率为负时往Frenet坐标系下的
Figure 521628DEST_PATH_IMAGE043
轴负方向偏移，左转曲率为正时往Frenet坐标系下的
Figure 585137DEST_PATH_IMAGE043
轴正方向偏移，大小与曲率的大小成正相关，
Figure 31162DEST_PATH_IMAGE043
轴表示车辆距离道路中心线的横向距离；横向偏移量
Figure 365191DEST_PATH_IMAGE033
的大小与车辆轴距L值正相关，即车辆轴距越长，横向需要偏移得越多，与车辆最大转向能力
Figure 972890DEST_PATH_IMAGE029
成负相关，即车辆的最大转角能力越大，横向需要偏移得越少，更新约束条件如下所示：
Figure 25159DEST_PATH_IMAGE064
可简写为：
Figure 9296DEST_PATH_IMAGE065
其中，
Figure 463411DEST_PATH_IMAGE036
，
Figure 242011DEST_PATH_IMAGE066
，
Figure 781577DEST_PATH_IMAGE067
步骤4.5：根据步骤4.1得到的代价函数和步骤4.4得到的约束条件，调用优化算法求解库求解含线性约束的最优化（最小值）问题，即对待优化变量
Figure 825798DEST_PATH_IMAGE039
进行优化，得到最终的最优化结果
Figure 399999DEST_PATH_IMAGE040
：
Figure 349500DEST_PATH_IMAGE041
Figure 376362DEST_PATH_IMAGE042
。
步骤五，利用最优化结果，计算出所述最优化结果对应朝向角及曲率，输出平滑后的参考线。
具体的，利用最优化结果
Figure 436722DEST_PATH_IMAGE040
，计算出最优化结果
Figure 865429DEST_PATH_IMAGE040
对应的朝向角及曲率，最终输出为一系列平滑后的离散点信息（
Figure 251411DEST_PATH_IMAGE044
）、(
Figure 499990DEST_PATH_IMAGE045
), …，(
Figure 364041DEST_PATH_IMAGE046
)，如图3所示。
在本实施例中朝向角
Figure DEST_PATH_IMAGE069
的计算方式为：
Figure 880210DEST_PATH_IMAGE070
曲率的计算方式为：
首先计算最优化结果中点与点的距离，并赋值于s中：
Figure 171514DEST_PATH_IMAGE072
然后分别计算笛卡尔坐标系下x，y方向相对于s的导数：
Figure DEST_PATH_IMAGE073
Figure 641810DEST_PATH_IMAGE074
再计算x，y方向相对于s的二阶导数：
Figure DEST_PATH_IMAGE075
Figure 778393DEST_PATH_IMAGE076
最后计算曲率：
Figure DEST_PATH_IMAGE077
最终，平滑后的参考线即由一系列的离散点坐标即优化结果来表示。
一种多车型参数自适应的参考线平滑系统，包括：局部规划地图生成模块和参考线平滑模块；所述的局部规划地图生成模块，输入为高精地图信息、决策目标车道信息、全局导航信息及全局定位信息，输出为固定长度的局部地图，包括道路中心线及可行驶区域空间信息；所述的参考线平滑模块，输入为局部规划地图生成模块的输出、车辆参数，构造参考线平滑的代价函数和约束条件，并根据所述代价函数和约束条件，调用优化算法求解库对需优化的参考点进行优化，得到最优化结果，利用最优化结果，计算出所述最优化结果对应朝向角及曲率，输出为一条平滑后的参考线。
与前述一种多车型参数自适应的参考线平滑方法的实施例相对应，本发明还提供了一种多车型参数自适应的参考线平滑装置的实施例。
参见图4，本发明实施例提供的一种多车型参数自适应的参考线平滑装置，包括一个或多个处理器，用于实现上述实施例中的一种多车型参数自适应的参考线平滑方法。
本发明一种多车型参数自适应的参考线平滑装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图4所示，为本发明一种多车型参数自适应的参考线平滑装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图4所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的一种多车型参数自适应的参考线平滑方法。
所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是风力发电机的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（Smart Media Card，SMC）、SD卡、闪存卡（Flash Card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
以上所述，仅为本发明的优选实施案例，并非对本发明做任何形式上的限制。虽然前文对本发明的实施过程进行了详细说明，对于熟悉本领域的人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明精神和原则之内所做修改、同等替换等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种多车型参数自适应的参考线平滑方法，其特征在于，包括以下步骤：
步骤一，使用高精地图，定位出自车全局位置，根据用户决策给出目标车道信息；
步骤二，根据定位给出的自车全局位置及用户决策给出的目标车道信息，计算自车在以原始道路中心线为参考线的Frenet坐标系下的起始坐标；
步骤三，从起始坐标开始，等距采样车道中心线上的离散原始参考点，并获取每个离散原始参考点对应的车道宽度，再判断自车所在的当前车道的邻道状态，根据判断结果构造出自车可行驶区域；
步骤四，构造参考线平滑的代价函数和约束条件，并根据所述代价函数和约束条件，调用优化算法求解库对需优化的参考点进行优化，得到最优化结果；
步骤五，利用最优化结果，计算出所述最优化结果对应朝向角及曲率，输出平滑后的参考线。
2.如权利要求1所述的一种多车型参数自适应的参考线平滑方法，其特征在于，所述步骤三具体包括以下子步骤：
步骤3.1，对高精地图中的目标车道，从起始坐标
Figure DEST_PATH_IMAGE002
开始以
Figure DEST_PATH_IMAGE004
为采样间隔，在固定长度内，等距采样车道中心线信息，包含n个离散原始参考点（
Figure DEST_PATH_IMAGE006
）、(
Figure DEST_PATH_IMAGE008
), …，(
Figure DEST_PATH_IMAGE010
)；其中
Figure DEST_PATH_IMAGE012
表示直角系坐标，
Figure DEST_PATH_IMAGE014
表示向量
Figure DEST_PATH_IMAGE016
与直角坐标系中X轴方向的夹角，
Figure DEST_PATH_IMAGE018
表示为离散原始参考点的曲率，i∈（0，1，2，…，n-1）；
步骤3.2，对目标车道的中心线从起始坐标
Figure 727222DEST_PATH_IMAGE002
开始以
Figure 55436DEST_PATH_IMAGE004
为采样间隔，等距采样得到每个离散原始参考点对应的车道宽度序列
Figure DEST_PATH_IMAGE020
；
步骤3.3，再根据高精地图中的车道线类型信息，判断当前车道的邻道是否可借用，若可借用则对相邻车道以起始坐标
Figure 726588DEST_PATH_IMAGE002
开始以等距采样间隔采样得相邻车道宽度序列
Figure DEST_PATH_IMAGE022
，最终相加得可行驶区域序列
Figure DEST_PATH_IMAGE024
，其中
Figure DEST_PATH_IMAGE026
，若无法借用邻道则使
Figure 290294DEST_PATH_IMAGE022
都为0。
3.如权利要求2所述的一种多车型参数自适应的参考线平滑方法，其特征在于，所述目标车道的剩余长度无法满足固定长度需求时，则根据全局导航信息及高精地图信息获取目标车道的后续车道，从后续车道的起始点开始，以
Figure 832133DEST_PATH_IMAGE004
为采样间隔，等距采样对应的后续车道中心线信息，并加入至离散原始参考点序列中，直至满足长度需求或抵达全局终点。
4.如权利要求2所述的一种多车型参数自适应的参考线平滑方法，其特征在于，所述步骤四具体包括以下子步骤：
步骤4.1：计算出参考线平滑的代价函数，包含两部分，第一部分为需优化的参考点即待优化变量，与原始道路中心线参考点的相似代价
Figure DEST_PATH_IMAGE028
；第二部分为平滑代价
Figure DEST_PATH_IMAGE030
，得到代价函数表达式如下所示：
Figure DEST_PATH_IMAGE032
其中相似代价设置为待优化变量与原始道路中心线参考点的欧式距离，即
Figure DEST_PATH_IMAGE034
，平滑代价设置为三点之间的直线相似程度，即
Figure DEST_PATH_IMAGE036
；其中，
Figure DEST_PATH_IMAGE038
为待优化变量（
Figure DEST_PATH_IMAGE040
）、(
Figure DEST_PATH_IMAGE042
), …，(
Figure DEST_PATH_IMAGE044
)，
Figure DEST_PATH_IMAGE046
为原始道路中心线坐标点（
Figure DEST_PATH_IMAGE048
）、(
Figure DEST_PATH_IMAGE050
), …，(
Figure DEST_PATH_IMAGE052
)，
Figure DEST_PATH_IMAGE054
为平滑代价权重，为固定值，
Figure DEST_PATH_IMAGE056
为相似代价权重，与车辆轴距L值成负相关，与车辆最大转向能力
Figure DEST_PATH_IMAGE058
成正相关；
步骤4.2：根据可行驶区域序列
Figure 895030DEST_PATH_IMAGE024
以及采样间隔
Figure 787899DEST_PATH_IMAGE004
，得到每一个待优化变量偏离离散原始参考点的横纵向约束，表达式为：
Figure DEST_PATH_IMAGE060
；
步骤4.3：根据离散原始参考点的朝向角
Figure 561820DEST_PATH_IMAGE014
，将步骤4.2中每一待优化变量的横纵向约束进行旋转至与对应离散原始参考点的朝向一致，即乘以由朝向角
Figure 955237DEST_PATH_IMAGE014
构建的旋转矩阵
Figure DEST_PATH_IMAGE062
，更新步骤4.2的约束条件，表达式为：
Figure DEST_PATH_IMAGE064
；
步骤4.4：再根据车型参数及离散原始参考点的曲率
Figure 687569DEST_PATH_IMAGE018
来设计约束条件的横向偏移量
Figure DEST_PATH_IMAGE066
，并更新约束条件如下所示：
Figure DEST_PATH_IMAGE068
简写为：
Figure DEST_PATH_IMAGE070
其中，
Figure DEST_PATH_IMAGE072
，
Figure DEST_PATH_IMAGE074
，
Figure DEST_PATH_IMAGE076
步骤4.5：根据步骤4.1得到的代价函数和步骤4.4得到的约束条件，调用优化算法求解库求解含线性约束的最优化问题，即对待优化变量
Figure DEST_PATH_IMAGE078
进行优化，得到最终的最优化结果
Figure DEST_PATH_IMAGE080
：
Figure DEST_PATH_IMAGE082
Figure DEST_PATH_IMAGE084
。
5.如权利要求4所述的一种多车型参数自适应的参考线平滑方法，其特征在于，所述横向偏移量
Figure 975725DEST_PATH_IMAGE066
正负与曲率的正负一致，即右转曲率为负时往Frenet坐标系下的
Figure DEST_PATH_IMAGE086
轴负方向偏移，左转曲率为正时往Frenet坐标系下的
Figure 553337DEST_PATH_IMAGE086
轴正方向偏移，大小与曲率的大小成正相关，
Figure 69769DEST_PATH_IMAGE086
轴表示车辆距离道路中心线的横向距离；横向偏移量
Figure 910686DEST_PATH_IMAGE066
的大小与车辆轴距L值正相关，即车辆轴距越长，横向需要偏移得越多，与车辆最大转向能力
Figure 512568DEST_PATH_IMAGE058
成负相关，即车辆的最大转角能力越大，横向需要偏移得越少。
6. 如权利要求4所述的一种多车型参数自适应的参考线平滑方法，其特征在于，所述步骤五具体为：利用最优化结果
Figure 97133DEST_PATH_IMAGE080
，计算出最优化结果
Figure 468072DEST_PATH_IMAGE080
对应的朝向角及曲率，最终输出为一系列平滑后的离散点信息（
Figure DEST_PATH_IMAGE088
）、(
Figure DEST_PATH_IMAGE090
), …，(
Figure DEST_PATH_IMAGE092
)。
7.一种多车型参数自适应的参考线平滑系统，其特征在于，包括：局部规划地图生成模块和参考线平滑模块；所述的局部规划地图生成模块，输入为高精地图信息、决策目标车道信息、全局导航信息及全局定位信息，输出为固定长度的局部地图，包括道路中心线及可行驶区域空间信息；所述的参考线平滑模块，输入为局部规划地图生成模块的输出、车辆参数，构造参考线平滑的代价函数和约束条件，并根据所述代价函数和约束条件，调用优化算法求解库对需优化的参考点进行优化，得到最优化结果，利用最优化结果，计算出所述最优化结果对应朝向角及曲率，输出为一条平滑后的参考线。
8.一种多车型参数自适应的参考线平滑装置，其特征在于，包括一个或多个处理器，用于实现权利要求1-6中任一项所述的一种多车型参数自适应的参考线平滑方法。
9.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-6中任一项所述的一种多车型参数自适应的参考线平滑方法。
CN202210580074.1A 2022-05-26 2022-05-26 一种多车型参数自适应的参考线平滑方法和系统 Active CN114676939B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210580074.1A CN114676939B (zh) 2022-05-26 2022-05-26 一种多车型参数自适应的参考线平滑方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210580074.1A CN114676939B (zh) 2022-05-26 2022-05-26 一种多车型参数自适应的参考线平滑方法和系统

Publications (2)

Publication Number Publication Date
CN114676939A true CN114676939A (zh) 2022-06-28
CN114676939B CN114676939B (zh) 2022-09-02

Family

ID=82079956

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210580074.1A Active CN114676939B (zh) 2022-05-26 2022-05-26 一种多车型参数自适应的参考线平滑方法和系统

Country Status (1)

Country Link
CN (1) CN114676939B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115309170A (zh) * 2022-10-12 2022-11-08 之江实验室 一种考虑舒适性约束的轨迹规划方法、装置和系统
CN116698059A (zh) * 2023-07-27 2023-09-05 宁波路特斯机器人有限公司 高精地图参考线的处理方法、存储介质与设备
CN117032201A (zh) * 2023-03-07 2023-11-10 山西省智慧交通研究院有限公司 一种基于车路协同的矿井下自动驾驶车辆协调规划方法

Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109927716A (zh) * 2019-03-11 2019-06-25 武汉环宇智行科技有限公司 基于高精度地图的自主垂直泊车方法
CN110728014A (zh) * 2018-06-27 2020-01-24 百度（美国）有限责任公司 使用具有加权几何成本的分段螺旋曲线的参考线平滑方法
CN110749333A (zh) * 2019-11-07 2020-02-04 中南大学 基于多目标优化的无人驾驶车辆运动规划方法
CN111123927A (zh) * 2019-12-20 2020-05-08 北京三快在线科技有限公司 轨迹规划方法、装置、自动驾驶设备和存储介质
CN111121777A (zh) * 2019-11-26 2020-05-08 北京三快在线科技有限公司 无人驾驶设备轨迹规划方法、装置、电子设备和存储介质
CN111615618A (zh) * 2018-12-26 2020-09-01 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的高速规划的基于多项式拟合的参考线平滑方法
WO2020216315A1 (zh) * 2019-04-26 2020-10-29 纵目科技（上海）股份有限公司 一种参考行驶线快速生成方法、系统、终端和存储介质
CN111898804A (zh) * 2020-07-10 2020-11-06 东风商用车有限公司 局部规划的道路自适应采样方法
CN112429004A (zh) * 2020-12-02 2021-03-02 北京理工大学 一种车辆自动换道控制方法
CN112810630A (zh) * 2021-02-05 2021-05-18 山东大学 一种自动驾驶车辆轨迹规划方法及系统
CN113031583A (zh) * 2020-03-13 2021-06-25 青岛慧拓智能机器有限公司 一种结构化道路避障方法
CN113671941A (zh) * 2020-05-15 2021-11-19 北京京东乾石科技有限公司 一种轨迹规划方法、装置、设备和存储介质
CN113741179A (zh) * 2021-11-08 2021-12-03 北京理工大学 一种面向异构车辆的统一运动规划方法和系统
CN113932823A (zh) * 2021-09-23 2022-01-14 同济大学 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法
CN114115298A (zh) * 2022-01-25 2022-03-01 北京理工大学 一种无人车路径平滑方法及系统
CN114527761A (zh) * 2022-02-27 2022-05-24 重庆长安汽车股份有限公司 一种基于融合算法的智能汽车局部路径规划方法

Patent Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110728014A (zh) * 2018-06-27 2020-01-24 百度（美国）有限责任公司 使用具有加权几何成本的分段螺旋曲线的参考线平滑方法
CN111615618A (zh) * 2018-12-26 2020-09-01 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的高速规划的基于多项式拟合的参考线平滑方法
CN109927716A (zh) * 2019-03-11 2019-06-25 武汉环宇智行科技有限公司 基于高精度地图的自主垂直泊车方法
WO2020216315A1 (zh) * 2019-04-26 2020-10-29 纵目科技（上海）股份有限公司 一种参考行驶线快速生成方法、系统、终端和存储介质
CN110749333A (zh) * 2019-11-07 2020-02-04 中南大学 基于多目标优化的无人驾驶车辆运动规划方法
CN111121777A (zh) * 2019-11-26 2020-05-08 北京三快在线科技有限公司 无人驾驶设备轨迹规划方法、装置、电子设备和存储介质
CN111123927A (zh) * 2019-12-20 2020-05-08 北京三快在线科技有限公司 轨迹规划方法、装置、自动驾驶设备和存储介质
CN113031583A (zh) * 2020-03-13 2021-06-25 青岛慧拓智能机器有限公司 一种结构化道路避障方法
CN113671941A (zh) * 2020-05-15 2021-11-19 北京京东乾石科技有限公司 一种轨迹规划方法、装置、设备和存储介质
CN111898804A (zh) * 2020-07-10 2020-11-06 东风商用车有限公司 局部规划的道路自适应采样方法
CN112429004A (zh) * 2020-12-02 2021-03-02 北京理工大学 一种车辆自动换道控制方法
CN112810630A (zh) * 2021-02-05 2021-05-18 山东大学 一种自动驾驶车辆轨迹规划方法及系统
CN113932823A (zh) * 2021-09-23 2022-01-14 同济大学 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法
CN113741179A (zh) * 2021-11-08 2021-12-03 北京理工大学 一种面向异构车辆的统一运动规划方法和系统
CN114115298A (zh) * 2022-01-25 2022-03-01 北京理工大学 一种无人车路径平滑方法及系统
CN114527761A (zh) * 2022-02-27 2022-05-24 重庆长安汽车股份有限公司 一种基于融合算法的智能汽车局部路径规划方法

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
JASMINA ZUBAČA: "Smooth Reference Line Generation for a Race Track with Gates based on Defined Borders", 《IEEE XPLORE》 *
周扬等: "基于主动优化的无人驾驶客车实时性运动规划算法", 《汽车安全与节能学报》 *
王博洋等: "面向异构履带车辆的统一运动规划方法", 《兵工学报》 *
王沙晶: "基于Frenet坐标系采样的自动驾驶轨迹规划算法研究", 《中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑)》 *
王沙晶等: "基于Frenet坐标系的智能车运动规划研究", 《移动电源与车辆》 *
高阳天: "室外物流机器人轨迹规划系统设计与实现", 《中国优秀硕士学位论文全文数据库 (信息科技辑)》 *
魏民祥等: "基于Frenet坐标系的自动驾驶轨迹规划与优化算法", 《控制与决策》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115309170A (zh) * 2022-10-12 2022-11-08 之江实验室 一种考虑舒适性约束的轨迹规划方法、装置和系统
CN117032201A (zh) * 2023-03-07 2023-11-10 山西省智慧交通研究院有限公司 一种基于车路协同的矿井下自动驾驶车辆协调规划方法
CN117032201B (zh) * 2023-03-07 2024-04-12 山西省智慧交通研究院有限公司 一种基于车路协同的矿井下自动驾驶车辆协调规划方法
CN116698059A (zh) * 2023-07-27 2023-09-05 宁波路特斯机器人有限公司 高精地图参考线的处理方法、存储介质与设备
CN116698059B (zh) * 2023-07-27 2023-11-28 宁波路特斯机器人有限公司 高精地图参考线的处理方法、存储介质与设备

Also Published As

Publication number Publication date
CN114676939B (zh) 2022-09-02

Similar Documents

Publication Publication Date Title
CN114676939B (zh) 一种多车型参数自适应的参考线平滑方法和系统
CN110084840B (zh) 点云配准方法、装置、服务器和计算机可读介质
US11455809B2 (en) Method and apparatus for determining lane centerline
EP3109842B1 (en) Map-centric map matching method and apparatus
CN110595475A (zh) 循迹路径的拟合方法、装置、智能汽车和存储介质
WO2021169993A1 (zh) 自动驾驶地图的构建方法及相关装置
CN111060108B (zh) 路径规划方法和装置、工程车辆
CN112269965A (zh) 一种非完整约束条件下的连续曲率路径优化方法
JP2010086544A (ja) 最新運転者支援システムのための幾何学形状の作成
CN111121777A (zh) 无人驾驶设备轨迹规划方法、装置、电子设备和存储介质
CN111795699B (zh) 无人车的路径规划方法、装置和计算机可读存储介质
CN111177934B (zh) 参考路径规划的方法、设备和存储介质
CN112454367B (zh) 一种轨迹规划方法、装置以及计算机存储介质
CN112747744B (zh) 一种结合航位推算和多车道路网地图的车辆定位方法
Yang et al. An algorithm of curved path tracking with prediction model for autonomous vehicle
CN115309170B (zh) 一种考虑舒适性约束的轨迹规划方法、装置和系统
WO2022252390A1 (zh) 误差补偿方法、装置、计算机设备及存储介质
CN114323051A (zh) 路口行驶轨迹的规划方法、装置和电子设备
CN113515111B (zh) 一种车辆避障路径规划方法及装置
CN114427863A (zh) 车辆定位方法及系统、自动泊车方法及系统、存储介质
CN113568403A (zh) 处理方法、处理装置、车辆及存储介质
CN116817955B (zh) 车辆路径规划方法、装置、电子设备及存储介质
CN116793369B (zh) 路径规划方法、装置、设备及计算机可读存储介质
CN117128976B (zh) 道路中心线的获取方法、装置、车辆和存储介质
CN116952251B (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