CN115309170A - 一种考虑舒适性约束的轨迹规划方法、装置和*** - Google Patents

一种考虑舒适性约束的轨迹规划方法、装置和*** Download PDF

Info

Publication number
CN115309170A
CN115309170A CN202211243619.6A CN202211243619A CN115309170A CN 115309170 A CN115309170 A CN 115309170A CN 202211243619 A CN202211243619 A CN 202211243619A CN 115309170 A CN115309170 A CN 115309170A
Authority
CN
China
Prior art keywords
speed
path
constraint
vehicle
comfort
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
CN202211243619.6A
Other languages
English (en)
Other versions
CN115309170B (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 CN202211243619.6A priority Critical patent/CN115309170B/zh
Publication of CN115309170A publication Critical patent/CN115309170A/zh
Application granted granted Critical
Publication of CN115309170B publication Critical patent/CN115309170B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • 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)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开一种考虑舒适性约束的轨迹规划方法、装置和***，该方法包括：步骤一，根据自车全局位置及参考线的信息，计算出自车投影到参考线的投影点坐标，采样得到离散原始参考点；步骤二，通过构造并计算车辆行驶路径规划的代价函数及约束，得到路径优化结果，根据路径优化结果，计算出路径规划对应的朝向角及曲率，得到平滑后的路径；步骤三，通过构造并计算车辆行驶速度规划的代价函数及约束，得到速度优化结果，根据速度优化结果，计算出速度规划对应的朝向角及曲率，得到平滑后的速度；步骤四，将平滑后的速度与路径融合得到带有速度信息的平滑轨迹。本发明能够对自动驾驶车辆的规划路径和速度进行约束优化，实现自动驾驶的乘坐舒适性体验。

Description

一种考虑舒适性约束的轨迹规划方法、装置和***
技术领域
本发明属于自动驾驶的轨迹规划领域，涉及一种考虑舒适性约束的轨迹规划方法、装置和***。
背景技术
在自动驾驶的轨迹规划领域，一般将路径规划与速度规划解耦开求解，轨迹规划作为自动驾驶中重要的一环，不仅决定了最终车辆行驶轨迹的安全性，也体现了自动驾驶乘坐的舒适性。现有的轨迹规划算法大多通过构建优化问题来求解，并且将舒适性项放入优化问题的代价函数中（软约束），这导致在某些场景中规划的结果不能完全满足乘坐的舒适性要求。
发明内容
为了解决现有技术中存在的上述技术问题，本发明提出了一种考虑舒适性约束的轨迹规划方法、装置和***，能够对自动驾驶车辆的规划路径和速度进行约束优化，实现自动驾驶的乘坐舒适性体验，具体技术方案如下：
一种考虑舒适性约束的轨迹规划方法，包括以下步骤：
步骤一，根据定位的自车全局位置及参考线的信息，计算出自车投影到参考线的投影点坐标，从投影点坐标开始等距采样至一固定长度，得到若干个离散的原始参考点；
步骤二，通过构造并计算车辆行驶路径规划的代价函数及约束，对路径规划进行优化，得到路径优化结果，再根据路径优化结果，计算出路径规划对应的朝向角及曲率，得到平滑后的路径；
步骤三，通过构造并计算车辆行驶速度规划的代价函数及约束，对规划速度进行优化，得到速度优化结果，计算出速度规划对应的朝向角及曲率，得到平滑后的速度；
步骤四，将平滑后的速度与平滑后的路径融合得到带有速度信息的平滑轨迹。
进一步的，所述步骤一，具体包括以下子步骤：
步骤1.1：根据车辆的定位模块给出的自车全局位置
Figure 100002_DEST_PATH_IMAGE001
及决策给出的目标车道信息，计算自车在以原始道路中心线为参考线的投影的参考点坐标
Figure 528164DEST_PATH_IMAGE002
)，即自车距离参考线最近点的坐标；
步骤1.2：对输入的参考线信息，从匹配的参考线的投影点坐标
Figure 914146DEST_PATH_IMAGE002
)开始，以
Figure 100002_DEST_PATH_IMAGE003
为采样间隔等距采样至一固定长度，包含n个离散参考点（
Figure 490621DEST_PATH_IMAGE004
）、(
Figure 100002_DEST_PATH_IMAGE005
), …，(
Figure 948147DEST_PATH_IMAGE006
)，其中
Figure 100002_DEST_PATH_IMAGE007
表示直角坐标系坐标，
Figure 621574DEST_PATH_IMAGE008
表示向量
Figure 100002_DEST_PATH_IMAGE009
与直角坐标系中X轴的夹角，
Figure 506353DEST_PATH_IMAGE010
表示为离散点处参考线的曲率。
进一步的，所述步骤二，具体包括以下子步骤：
步骤2.1：构造路径规划的代价函数
Figure 100002_DEST_PATH_IMAGE011
，该路径代价函数包含两部分，第一部分为待优化变量与原始参考线参考点的相似代价
Figure 570124DEST_PATH_IMAGE012
，第二部分为平滑代价
Figure 100002_DEST_PATH_IMAGE013
，如下所示：
Figure 100002_DEST_PATH_IMAGE015
其中，
Figure 96920DEST_PATH_IMAGE016
为待优化变量（
Figure 100002_DEST_PATH_IMAGE017
）、（
Figure 828116DEST_PATH_IMAGE018
），…，（
Figure 100002_DEST_PATH_IMAGE019
）；
Figure 618218DEST_PATH_IMAGE020
为原始参考线参考点（
Figure 100002_DEST_PATH_IMAGE021
）、（
Figure 434864DEST_PATH_IMAGE022
），…，（
Figure 100002_DEST_PATH_IMAGE023
）；
Figure 906296DEST_PATH_IMAGE024
为相似代价权重，与车辆轴距L值成负相关，与车辆最大转向能力成正相关；
Figure 100002_DEST_PATH_IMAGE025
平滑代价权重，为固定值权重；
所述相似代价
Figure 226419DEST_PATH_IMAGE026
设置为待优化变量与原始参考点的欧式距离，表达式为：
Figure 249739DEST_PATH_IMAGE028
所述平滑代价
Figure 960206DEST_PATH_IMAGE013
设置为三点之间的直线相似程度，表达式为：
Figure 100002_DEST_PATH_IMAGE029
；
步骤2.2：构造路径规划的安全性约束，得到每一待优化变量（
Figure 94384DEST_PATH_IMAGE030
）偏离原始参考点的横纵向约束，表达式为：
Figure 206697DEST_PATH_IMAGE032
Figure 135338DEST_PATH_IMAGE034
其中，
Figure 100002_DEST_PATH_IMAGE035
、
Figure 926577DEST_PATH_IMAGE036
、
Figure 100002_DEST_PATH_IMAGE037
、
Figure 802129DEST_PATH_IMAGE038
为设置在
Figure 768948DEST_PATH_IMAGE016
、
Figure 100002_DEST_PATH_IMAGE039
方向上的上下界约束；
步骤2.3：根据车型参数构造路径规划的曲率约束，离散点之间的曲率关系满足车辆的最小转弯半径约束，表达式为:
Figure 399650DEST_PATH_IMAGE040
，
可转换为：
Figure 100002_DEST_PATH_IMAGE041
其中，
Figure 412605DEST_PATH_IMAGE042
为两点间的距离，
Figure 100002_DEST_PATH_IMAGE043
为最小转弯半径约束，由车辆自身参数设定；
步骤2.4：根据步骤2.1得到的路径代价函数及步骤2.2和步骤2.3得到的约束条件，通过求解该含约束条件的非线性优化路径，得到最终的路径优化结果
Figure 91848DEST_PATH_IMAGE044
即为平滑后的路径点坐标信息；
步骤2.5：利用步骤2.4的路径优化结果
Figure 913173DEST_PATH_IMAGE044
，计算出路径点对应的朝向角及曲率，最终输出为一系列平滑后的路径点信息(
Figure 100002_DEST_PATH_IMAGE045
)，（
Figure 714776DEST_PATH_IMAGE046
），…，(
Figure 100002_DEST_PATH_IMAGE047
)，即输出平滑路径。
进一步的，所述步骤三，具体包括以下子步骤：
步骤3.1：构造速度规划的代价函数及约束，该速度代价函数包含三部分：第一部分为待优化速度变量与本路段期望速度的相似代价
Figure 215028DEST_PATH_IMAGE048
，第二部分为待优化变量纵向加速度代价
Figure 100002_DEST_PATH_IMAGE049
，第三部分为待优化变量纵向加速度代价
Figure 432382DEST_PATH_IMAGE050
，具体表达式如下：
Figure 373794DEST_PATH_IMAGE052
Figure 752822DEST_PATH_IMAGE054
Figure 271528DEST_PATH_IMAGE056
Figure 100002_DEST_PATH_IMAGE057
，
其中，
Figure 292574DEST_PATH_IMAGE058
为待优化变量
Figure 100002_DEST_PATH_IMAGE059
，
Figure 416388DEST_PATH_IMAGE060
为该规划路段中期望速度值
Figure 100002_DEST_PATH_IMAGE061
，
Figure 435159DEST_PATH_IMAGE062
为与期望速度相似代价权重，
Figure 100002_DEST_PATH_IMAGE063
为加速度代价权重，
Figure 910003DEST_PATH_IMAGE064
为加速度代价权重,
Figure 100002_DEST_PATH_IMAGE065
为预设的固定采样时间长度；
步骤3.2：构造速度规划的第一舒适性约束，表达式为：
Figure 100002_DEST_PATH_IMAGE067
Figure 100002_DEST_PATH_IMAGE069
其中，
Figure 797056DEST_PATH_IMAGE070
为保证舒适性的最大限速，
Figure 100002_DEST_PATH_IMAGE071
为满足人体舒适性的向心加速度最大值，
Figure 837694DEST_PATH_IMAGE072
为步骤2.5得到的每个路径点对应的曲率；
步骤3.3：构造速度规划的第二舒适性约束，表达式为：
Figure 496208DEST_PATH_IMAGE074
其中，
Figure 100002_DEST_PATH_IMAGE075
，
Figure 192769DEST_PATH_IMAGE076
为满足人体舒适度的最大纵向加速度值；
步骤3.4：构造速度规划的连续性约束，表达式为：
Figure 352355DEST_PATH_IMAGE078
步骤3.5：根据步骤3.1得到的速度代价函数及步骤3.2至步骤3.4得到的约束条件，通过求解该含约束条件的最优化速度，得到最终的速度优化结果
Figure 100002_DEST_PATH_IMAGE079
即为平滑的速度信息。
进一步的，所述步骤四具体为：根据步骤3.5得到的速度优化结果
Figure 388444DEST_PATH_IMAGE079
，将其和步骤2.5输出的平滑后的路径点进行联合，得到最终的满足舒适性约束的规划轨迹点(
Figure 342493DEST_PATH_IMAGE080
)，（
Figure 100002_DEST_PATH_IMAGE081
），…，(
Figure 791929DEST_PATH_IMAGE082
)。
一种考虑舒适性约束的轨迹规划***，包括路径平滑规划模块和速度规划模块，所述路径平滑规划模块输入设定的参考线信息、车辆全局定位信息、车辆参数，然后对其生成的路径施加车辆最大转向能力的曲率约束，输出一条满足车辆最小转弯半径约束的平滑路径；所述速度规划模块，针对平滑后路径点的曲率值计算出满足乘坐舒适性的最大限速，并将该限速作为速度优化的约束条件，从而求解出平滑速度的值，最后将平滑后的速度与路径融合，输出一系列带有速度信息的轨迹点。
一种考虑舒适性约束的轨迹规划装置，包括一个或多个处理器，用于实现所述的一种考虑舒适性约束的轨迹规划方法。
一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的一种考虑舒适性约束的轨迹规划方法。
有益效果：
本发明对规划路径施加车辆最大转向能力的曲率约束，使平滑后的路径能满足不同车辆的控制执行能力；再进行速度规划，针对上一步平滑后路径点的曲率值计算出满足乘坐舒适性的最大限速，并将该限速放入速度优化问题的约束中，从而保证求解出的速度值能够满足乘坐舒适性体验。
附图说明
图1为本发明的一种考虑舒适性约束的轨迹规划方法的整体流程示意图；
图2为本发明的考虑舒适性约束的轨迹规划方法的详细流程图；
图3为本发明实施例的一种考虑舒适性约束的轨迹规划装置的结构示意图。
具体实施方式
为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。
如图1和图2所示，本发明的一种考虑舒适性约束的轨迹规划方法，包括以下步骤：
步骤一，根据定位的自车全局位置及参考线的信息，计算出自车投影到参考线的投影点坐标，从投影点坐标开始等距采样至一固定长度，得到若干个离散原始参考点，具体包括以下子步骤：
步骤1.1：根据车辆的定位模块给出的自车全局位置
Figure 364993DEST_PATH_IMAGE001
及决策给出的目标车道信息，计算自车在给定参考线（如以原始道路中心线为参考线）的投影的参考点坐标
Figure 849064DEST_PATH_IMAGE002
)，即自车距离参考线最近点的坐标；
步骤1.2：对输入的参考线信息，从匹配的参考线的投影点坐标
Figure 114960DEST_PATH_IMAGE002
)开始，以
Figure 317271DEST_PATH_IMAGE003
为采样间隔等距采样至一固定长度，本实施例以0.25m为采样间隔等距采样对应的参考线至50m，包含n个离散参考点（
Figure 756343DEST_PATH_IMAGE004
）、(
Figure 235866DEST_PATH_IMAGE005
), …，(
Figure 797297DEST_PATH_IMAGE006
)，其中
Figure 96691DEST_PATH_IMAGE007
表示直角坐标系坐标，
Figure 401771DEST_PATH_IMAGE008
表示向量
Figure 735800DEST_PATH_IMAGE009
与直角坐标系中X轴的夹角，
Figure 468133DEST_PATH_IMAGE010
表示为离散点处参考线的曲率。
步骤二，通过构造并计算车辆行驶路径规划的代价函数及约束，以求解路径规划优化问题，得到路径优化结果，即对规划路径进行优化，再根据路径优化结果，计算出路径规划对应的朝向角及曲率，得到平滑后的路径，具体包括以下子步骤：
步骤2.1：构造路径规划的代价函数，该路径代价函数包含两部分，第一部分为待优化变量与原始参考线参考点的相似代价
Figure 520402DEST_PATH_IMAGE012
，第二部分为平滑代价
Figure 35697DEST_PATH_IMAGE013
，如下所示：
Figure 614446DEST_PATH_IMAGE015
其中，
Figure 127467DEST_PATH_IMAGE016
为待优化变量（
Figure 791667DEST_PATH_IMAGE017
）、（
Figure 313915DEST_PATH_IMAGE018
），…，（
Figure 747170DEST_PATH_IMAGE019
）；
Figure 696672DEST_PATH_IMAGE020
为原始参考线参考点（
Figure 582588DEST_PATH_IMAGE021
）、（
Figure 705265DEST_PATH_IMAGE022
），…，（
Figure 868393DEST_PATH_IMAGE023
）；
Figure 113430DEST_PATH_IMAGE024
为相似代价权重，可设计为与车辆轴距L值成负相关，与车辆最大转向能力成正相关；
Figure 362008DEST_PATH_IMAGE025
平滑代价权重，可设为固定值权重；
所述相似代价
Figure 350693DEST_PATH_IMAGE026
可以设置为待优化变量与原始参考点的欧式距离，表达式为：
Figure 368328DEST_PATH_IMAGE028
所述平滑代价
Figure 49845DEST_PATH_IMAGE013
可以设置为三点之间的直线相似程度，表达式为：
Figure 785719DEST_PATH_IMAGE029
；
步骤2.2：构造路径规划的安全性约束，得到每一待优化变量（
Figure 250199DEST_PATH_IMAGE030
）偏离原始参考点的横纵向约束，表达式为：
Figure 246974DEST_PATH_IMAGE032
Figure 974758DEST_PATH_IMAGE034
其中，
Figure 322563DEST_PATH_IMAGE035
、
Figure 262837DEST_PATH_IMAGE036
、
Figure 379698DEST_PATH_IMAGE037
、
Figure 278384DEST_PATH_IMAGE038
为设置在
Figure 520009DEST_PATH_IMAGE016
、
Figure 654187DEST_PATH_IMAGE039
方向上的上下界约束，保证约束优化变量在安全范围内；
步骤2.3：根据车型参数构造路径规划的曲率约束，离散点之间的曲率关系满足车辆的最小转弯半径约束，表达式为:
Figure 766500DEST_PATH_IMAGE040
，
可转换为：
Figure 695141DEST_PATH_IMAGE041
其中，
Figure 627325DEST_PATH_IMAGE042
为两点间的距离，
Figure 299615DEST_PATH_IMAGE043
为最小转弯半径约束，由车辆自身参数设定；
步骤2.4：根据步骤2.1得到的代价函数及步骤2.2和步骤2.3得到的约束条件，通过求解该含约束的非线性优化路径，得到最终的路径优化结果
Figure 266434DEST_PATH_IMAGE044
即为平滑后的路径点坐标信息；
步骤2.5：利用步骤2.4的路径优化结果
Figure 365977DEST_PATH_IMAGE044
，计算出路径点对应的朝向角及曲率，最终输出为一系列平滑后的路径点信息(
Figure 847774DEST_PATH_IMAGE045
)，（
Figure 199121DEST_PATH_IMAGE046
），…，(
Figure 145080DEST_PATH_IMAGE047
)，即输出平滑路径。
步骤三，通过构造并计算车辆行驶速度规划的代价函数及约束，以求解速度规划优化问题，得到速度优化结果，即对规划速度进行优化，再根据速度优化结果，计算出速度规划对应的朝向角及曲率，得到平滑后的速度，具体包括以下子步骤：
步骤3.1：构造速度规划的代价函数及约束，该速度代价函数包含三部分：第一部分为待优化速度变量与本路段期望速度的相似代价
Figure 290891DEST_PATH_IMAGE048
，第二部分为待优化变量纵向加速度代价
Figure 322301DEST_PATH_IMAGE049
，第三部分为待优化变量纵向加速度代价
Figure 211759DEST_PATH_IMAGE050
，具体表达式如下：
Figure 277804DEST_PATH_IMAGE052
Figure DEST_PATH_IMAGE083
Figure 860095DEST_PATH_IMAGE084
Figure 378801DEST_PATH_IMAGE057
，
其中，
Figure 806372DEST_PATH_IMAGE058
为待优化变量
Figure 562861DEST_PATH_IMAGE059
，
Figure 316053DEST_PATH_IMAGE060
为该规划路段中期望速度值
Figure 322055DEST_PATH_IMAGE061
，
Figure 350054DEST_PATH_IMAGE062
为与期望速度相似代价权重，
Figure 478DEST_PATH_IMAGE063
为加速度代价权重，
Figure 49206DEST_PATH_IMAGE064
为加速度代价权重，
Figure 152291DEST_PATH_IMAGE065
为预设的固定采样时间长度；
步骤3.2：构造速度规划的第一舒适性约束，表达式为：
Figure DEST_PATH_IMAGE085
Figure 780718DEST_PATH_IMAGE086
其中，
Figure 410283DEST_PATH_IMAGE070
为保证舒适性的最大限速，
Figure 302015DEST_PATH_IMAGE071
为满足人体舒适性的向心加速度最大值，
Figure 157976DEST_PATH_IMAGE072
为步骤2.5得到的每个路径点对应的曲率；
步骤3.3：构造速度规划的第二舒适性约束，表达式为：
Figure 855674DEST_PATH_IMAGE074
其中，
Figure 480690DEST_PATH_IMAGE075
，
Figure 605641DEST_PATH_IMAGE076
为满足人体舒适度的最大纵向加速度值；
步骤3.4：构造速度规划的连续性约束，表达式为：
Figure 683318DEST_PATH_IMAGE078
步骤3.5：根据步骤3.1得到的速度代价函数及步骤3.2至步骤3.4得到的约束条件，通过求解该含约束的最优化速度，得到最终的速度优化结果
Figure 450286DEST_PATH_IMAGE079
即为平滑的速度信息。
步骤四，最后将平滑后的速度与平滑后的路径融合得到带有速度信息的平滑轨迹。
具体的，根据步骤3.5得到的速度优化结果
Figure 929809DEST_PATH_IMAGE079
，将其和步骤2.5输出的平滑后的路径点进行联合，得到最终的满足舒适性约束的规划轨迹点(
Figure 163344DEST_PATH_IMAGE080
)，（
Figure 852951DEST_PATH_IMAGE081
），…，(
Figure 767818DEST_PATH_IMAGE082
)。
采用上述方法的一种考虑舒适性约束的轨迹规划***，包括：路径平滑规划模块、速度规划模块。所述路径平滑规划模块首先输入设定的参考线信息、车辆全局定位信息、车辆参数，再对其生成的路径施加车辆最大转向能力的曲率约束，从而输出一条满足车辆最小转弯半径约束的平滑路径，使平滑路径满足不同车辆的控制执行能力；所述速度规划模块，针对平滑后路径点的曲率值计算出满足乘坐舒适性的最大限速，并将该限速放入速度优化问题的约束中，从而保证求解出的平滑速度的值能够满足乘坐舒适性体验，最后将平滑后的速度与路径融合，输出一系列带有速度信息的轨迹点。
与前述一种考虑舒适性约束的轨迹规划方法的实施例相对应，本发明还提供了一种考虑舒适性约束的轨迹规划装置的实施例。
参见图3，本发明实施例提供的一种考虑舒适性约束的轨迹规划装置，包括一个或多个处理器，用于实现上述实施例中的一种考虑舒适性约束的轨迹规划方法。
本发明一种考虑舒适性约束的轨迹规划装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图3所示，为本发明一种考虑舒适性约束的轨迹规划装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图3所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的一种考虑舒适性约束的轨迹规划方法。
所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（Smart Media Card，SMC）、SD卡、闪存卡（Flash Card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
以上所述，仅为本发明的优选实施案例，并非对本发明做任何形式上的限制。虽然前文对本发明的实施过程进行了详细说明，对于熟悉本领域的人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明精神和原则之内所做修改、同等替换等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种考虑舒适性约束的轨迹规划方法，其特征在于，包括以下步骤：
步骤一，根据定位的自车全局位置及参考线的信息，计算出自车投影到参考线的投影点坐标，从投影点坐标开始等距采样至一固定长度，得到若干个离散的原始参考点；
步骤二，通过构造并计算车辆行驶路径规划的代价函数及约束，对路径规划进行优化，得到路径优化结果，再根据路径优化结果，计算出路径规划对应的朝向角及曲率，得到平滑后的路径；
步骤三，通过构造并计算车辆行驶速度规划的代价函数及约束，对规划速度进行优化，得到速度优化结果，计算出速度规划对应的朝向角及曲率，得到平滑后的速度；
步骤四，将平滑后的速度与平滑后的路径融合得到带有速度信息的平滑轨迹。
2.如权利要求1所述的一种考虑舒适性约束的轨迹规划方法，其特征在于，所述步骤一，具体包括以下子步骤：
步骤1.1：根据车辆的定位模块给出的自车全局位置
Figure DEST_PATH_IMAGE001
及决策给出的目标车道信息，计算自车在以原始道路中心线为参考线的投影的参考点坐标
Figure 775980DEST_PATH_IMAGE002
)，即自车距离参考线最近点的坐标；
步骤1.2：对输入的参考线信息，从匹配的参考线的投影点坐标
Figure 383679DEST_PATH_IMAGE002
)开始，以
Figure DEST_PATH_IMAGE003
为采样间隔等距采样至一固定长度，包含n个离散参考点（
Figure 29424DEST_PATH_IMAGE004
）、(
Figure DEST_PATH_IMAGE005
), …，(
Figure 934932DEST_PATH_IMAGE006
)，其中
Figure DEST_PATH_IMAGE007
表示直角坐标系坐标，
Figure 982523DEST_PATH_IMAGE008
表示向量
Figure DEST_PATH_IMAGE009
与直角坐标系中X轴的夹角，
Figure 354598DEST_PATH_IMAGE010
表示为离散点处参考线的曲率。
3.如权利要求2所述的一种考虑舒适性约束的轨迹规划方法，其特征在于，所述步骤二，具体包括以下子步骤：
步骤2.1：构造路径规划的代价函数
Figure DEST_PATH_IMAGE011
，该路径代价函数包含两部分，第一部分为待优化变量与原始参考线参考点的相似代价
Figure 159743DEST_PATH_IMAGE012
，第二部分为平滑代价
Figure DEST_PATH_IMAGE013
，如下所示：
Figure DEST_PATH_IMAGE015
其中，
Figure 603363DEST_PATH_IMAGE016
为待优化变量（
Figure DEST_PATH_IMAGE017
）、（
Figure 505460DEST_PATH_IMAGE018
），…，（
Figure DEST_PATH_IMAGE019
）；
Figure 48437DEST_PATH_IMAGE020
为原始参考线参考点（
Figure DEST_PATH_IMAGE021
）、（
Figure 75298DEST_PATH_IMAGE022
），…，（
Figure DEST_PATH_IMAGE023
）；
Figure 729134DEST_PATH_IMAGE024
为相似代价权重，与车辆轴距L值成负相关，与车辆最大转向能力成正相关；
Figure DEST_PATH_IMAGE025
平滑代价权重，为固定值权重；
所述相似代价
Figure 485737DEST_PATH_IMAGE026
设置为待优化变量与原始参考点的欧式距离，表达式为：
Figure 730774DEST_PATH_IMAGE028
所述平滑代价
Figure 41669DEST_PATH_IMAGE013
设置为三点之间的直线相似程度，表达式为：
Figure DEST_PATH_IMAGE029
；
步骤2.2：构造路径规划的安全性约束，得到每一待优化变量（
Figure 499195DEST_PATH_IMAGE030
）偏离原始参考点的横纵向约束，表达式为：
Figure 516830DEST_PATH_IMAGE032
Figure 198347DEST_PATH_IMAGE034
其中，
Figure DEST_PATH_IMAGE035
、
Figure 262118DEST_PATH_IMAGE036
、
Figure DEST_PATH_IMAGE037
、
Figure 257756DEST_PATH_IMAGE038
为设置在
Figure 395476DEST_PATH_IMAGE016
、
Figure DEST_PATH_IMAGE039
方向上的上下界约束；
步骤2.3：根据车型参数构造路径规划的曲率约束，离散点之间的曲率关系满足车辆的最小转弯半径约束，表达式为:
Figure 513474DEST_PATH_IMAGE040
，
可转换为：
Figure DEST_PATH_IMAGE041
其中，
Figure 798961DEST_PATH_IMAGE042
为两点间的距离，
Figure DEST_PATH_IMAGE043
为最小转弯半径约束，由车辆自身参数设定；
步骤2.4：根据步骤2.1得到的路径代价函数及步骤2.2和步骤2.3得到的约束条件，通过求解该含约束条件的非线性优化路径，得到最终的路径优化结果
Figure 863869DEST_PATH_IMAGE044
即为平滑后的路径点坐标信息；
步骤2.5：利用步骤2.4的路径优化结果
Figure 652834DEST_PATH_IMAGE044
，计算出路径点对应的朝向角及曲率，最终输出为一系列平滑后的路径点信息(
Figure DEST_PATH_IMAGE045
)，（
Figure 879416DEST_PATH_IMAGE046
），…，(
Figure DEST_PATH_IMAGE047
)，即输出平滑路径。
4.如权利要求3所述的一种考虑舒适性约束的轨迹规划方法，其特征在于，所述步骤三，具体包括以下子步骤：
步骤3.1：构造速度规划的代价函数及约束，该速度代价函数包含三部分：第一部分为待优化速度变量与本路段期望速度的相似代价
Figure 917779DEST_PATH_IMAGE048
，第二部分为待优化变量纵向加速度代价
Figure DEST_PATH_IMAGE049
，第三部分为待优化变量纵向加速度代价
Figure 520799DEST_PATH_IMAGE050
，具体表达式如下：
Figure 164270DEST_PATH_IMAGE052
Figure 233857DEST_PATH_IMAGE054
Figure 290675DEST_PATH_IMAGE056
Figure DEST_PATH_IMAGE057
，
其中，
Figure 431806DEST_PATH_IMAGE058
为待优化变量
Figure DEST_PATH_IMAGE059
，
Figure 992100DEST_PATH_IMAGE060
为该规划路段中期望速度值
Figure DEST_PATH_IMAGE061
，
Figure 232589DEST_PATH_IMAGE062
为与期望速度相似代价权重，
Figure DEST_PATH_IMAGE063
为加速度代价权重，
Figure 245544DEST_PATH_IMAGE064
为加速度代价权重,
Figure DEST_PATH_IMAGE065
为预设的固定采样时间长度；
步骤3.2：构造速度规划的第一舒适性约束，表达式为：
Figure DEST_PATH_IMAGE067
Figure DEST_PATH_IMAGE069
其中，
Figure 49421DEST_PATH_IMAGE070
为保证舒适性的最大限速，
Figure DEST_PATH_IMAGE071
为满足人体舒适性的向心加速度最大值，
Figure 464222DEST_PATH_IMAGE072
为步骤2.5得到的每个路径点对应的曲率；
步骤3.3：构造速度规划的第二舒适性约束，表达式为：
Figure 610032DEST_PATH_IMAGE074
其中，
Figure DEST_PATH_IMAGE075
，
Figure 844704DEST_PATH_IMAGE076
为满足人体舒适度的最大纵向加速度值；
步骤3.4：构造速度规划的连续性约束，表达式为：
Figure 124376DEST_PATH_IMAGE078
步骤3.5：根据步骤3.1得到的速度代价函数及步骤3.2至步骤3.4得到的约束条件，通过求解该含约束条件的最优化速度，得到最终的速度优化结果
Figure DEST_PATH_IMAGE079
即为平滑的速度信息。
5.如权利要求4所述的一种考虑舒适性约束的轨迹规划方法，其特征在于，所述步骤四具体为：根据步骤3.5得到的速度优化结果
Figure 65787DEST_PATH_IMAGE079
，将其和步骤2.5输出的平滑后的路径点进行联合，得到最终的规划轨迹点(
Figure 772712DEST_PATH_IMAGE080
)，（
Figure DEST_PATH_IMAGE081
），…，(
Figure 760260DEST_PATH_IMAGE082
)。
6.一种采用如权利要求1至5任一一项所述的考虑舒适性约束的轨迹规划方法的轨迹规划***，包括路径平滑规划模块和速度规划模块，其特征在于，所述路径平滑规划模块输入设定的参考线信息、车辆全局定位信息、车辆参数，然后对其生成的路径施加车辆最大转向能力的曲率约束，输出一条满足车辆最小转弯半径约束的平滑路径；所述速度规划模块，针对平滑后路径点的曲率值计算出最大限速，并将该限速作为速度优化的约束条件，从而求解出平滑速度的值，最后将平滑后的速度与路径融合，输出一系列带有速度信息的轨迹点。
7.一种考虑舒适性约束的轨迹规划装置，其特征在于，包括一个或多个处理器，用于实现权利要求1-5中任一项所述的一种考虑舒适性约束的轨迹规划方法。
8.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-5中任一项所述的一种考虑舒适性约束的轨迹规划方法。
CN202211243619.6A 2022-10-12 2022-10-12 一种考虑舒适性约束的轨迹规划方法、装置和*** Active CN115309170B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211243619.6A CN115309170B (zh) 2022-10-12 2022-10-12 一种考虑舒适性约束的轨迹规划方法、装置和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211243619.6A CN115309170B (zh) 2022-10-12 2022-10-12 一种考虑舒适性约束的轨迹规划方法、装置和***

Publications (2)

Publication Number Publication Date
CN115309170A true CN115309170A (zh) 2022-11-08
CN115309170B CN115309170B (zh) 2023-03-24

Family

ID=83868178

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211243619.6A Active CN115309170B (zh) 2022-10-12 2022-10-12 一种考虑舒适性约束的轨迹规划方法、装置和***

Country Status (1)

Country Link
CN (1) CN115309170B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116166061A (zh) * 2023-04-26 2023-05-26 中国农业大学 无人驾驶的速度控制方法、装置、无人驾驶机和电子设备
CN117406756A (zh) * 2023-12-06 2024-01-16 苏州元脑智能科技有限公司 一种运动轨迹参数的确定方法、装置、设备和存储介质

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111413966A (zh) * 2020-03-12 2020-07-14 天津大学 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN111806467A (zh) * 2020-07-27 2020-10-23 福州大学 一种基于车辆行驶规律的变速动态换道轨迹规划方法
CN112389427A (zh) * 2021-01-19 2021-02-23 腾讯科技（深圳）有限公司 车辆轨迹优化方法、装置、电子设备和存储介质
CN112677995A (zh) * 2021-01-07 2021-04-20 腾讯科技（深圳）有限公司 一种车辆轨迹规划方法、装置、存储介质及设备
US20210200231A1 (en) * 2019-12-30 2021-07-01 Baidu Usa Llc Speed planning guidance line for mild slow down
CN114312848A (zh) * 2022-01-17 2022-04-12 吉林大学 基于双层mpc的智能驾驶汽车轨迹规划与跟踪控制方法
CN114676939A (zh) * 2022-05-26 2022-06-28 之江实验室 一种多车型参数自适应的参考线平滑方法和***
CN115079699A (zh) * 2022-06-30 2022-09-20 北京理工大学 一种基于模型预测控制的人机共驾汽车的运动规划方法
WO2022193584A1 (zh) * 2021-03-15 2022-09-22 西安交通大学 一种面向多场景的自动驾驶规划方法及***

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20210200231A1 (en) * 2019-12-30 2021-07-01 Baidu Usa Llc Speed planning guidance line for mild slow down
CN111413966A (zh) * 2020-03-12 2020-07-14 天津大学 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN111806467A (zh) * 2020-07-27 2020-10-23 福州大学 一种基于车辆行驶规律的变速动态换道轨迹规划方法
CN112677995A (zh) * 2021-01-07 2021-04-20 腾讯科技（深圳）有限公司 一种车辆轨迹规划方法、装置、存储介质及设备
WO2022148282A1 (zh) * 2021-01-07 2022-07-14 腾讯科技（深圳）有限公司 一种车辆轨迹规划方法、装置、存储介质及设备
CN112389427A (zh) * 2021-01-19 2021-02-23 腾讯科技（深圳）有限公司 车辆轨迹优化方法、装置、电子设备和存储介质
WO2022193584A1 (zh) * 2021-03-15 2022-09-22 西安交通大学 一种面向多场景的自动驾驶规划方法及***
CN114312848A (zh) * 2022-01-17 2022-04-12 吉林大学 基于双层mpc的智能驾驶汽车轨迹规划与跟踪控制方法
CN114676939A (zh) * 2022-05-26 2022-06-28 之江实验室 一种多车型参数自适应的参考线平滑方法和***
CN115079699A (zh) * 2022-06-30 2022-09-20 北京理工大学 一种基于模型预测控制的人机共驾汽车的运动规划方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张培志等: "非结构化道路环境下的无人车运动规划算法研究", 《上海汽车》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116166061A (zh) * 2023-04-26 2023-05-26 中国农业大学 无人驾驶的速度控制方法、装置、无人驾驶机和电子设备
CN117406756A (zh) * 2023-12-06 2024-01-16 苏州元脑智能科技有限公司 一种运动轨迹参数的确定方法、装置、设备和存储介质
CN117406756B (zh) * 2023-12-06 2024-03-08 苏州元脑智能科技有限公司 一种运动轨迹参数的确定方法、装置、设备和存储介质

Also Published As

Publication number Publication date
CN115309170B (zh) 2023-03-24

Similar Documents

Publication Publication Date Title
CN115309170B (zh) 一种考虑舒适性约束的轨迹规划方法、装置和***
US11531110B2 (en) LiDAR localization using 3D CNN network for solution inference in autonomous driving vehicles
US20210365712A1 (en) Deep learning-based feature extraction for lidar localization of autonomous driving vehicles
CN110533943A (zh) 确定自动驾驶车辆的车道改变轨迹的方法
CN110398963A (zh) 无轨导航纠偏控制方法、装置、存储介质及控制器
JP2020001678A (ja) 重み付け幾何学的コストを有する区分的螺旋曲線を使用した基準線平滑化方法
CN110531750A (zh) 用于自动驾驶车辆的pid嵌入式lqr
CN108995657A (zh) 操作自动驾驶车辆的方法和数据处理***
JP2019135145A (ja) 自動運転車両のための走行軌跡生成方法、システム及び機械可読媒体
KR20200096411A (ko) 자율 주행 차량에서 rnn 및 lstm을 사용하여 시간적 평활화를 수행하는 lidar 위치 추정
CN108136867A (zh) 自动驾驶车辆的车辆位置点转发方法
CN111522245B (zh) 用于控制无人设备的方法及装置
CN114676939B (zh) 一种多车型参数自适应的参考线平滑方法和***
CN111795699A (zh) 无人车的路径规划方法、装置和计算机可读存储介质
CN115339453B (zh) 车辆换道决策信息生成方法、装置、设备和计算机介质
CN114194217A (zh) 车辆自动驾驶方法、装置、电子设备以及存储介质
CN113671941A (zh) 一种轨迹规划方法、装置、设备和存储介质
Zeng et al. Driving-behavior-oriented trajectory planning for autonomous vehicle driving on urban structural road
CN110962858A (zh) 目标识别方法及设备
CN112009460B (zh) 一种车辆控制方法、装置、设备及存储介质
CN117724501A (zh) 路径跟踪控制方法、装置、车辆及介质
CN113553881A (zh) 泊车位检测方法及相关装置
CN115542899A (zh) 车辆路径跟踪的方法、装置、车辆、电子设备及介质
CN115489544A (zh) 路口转弯路径参考线生成方法、设备、车辆及存储介质
CN113148227B (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