一种考虑舒适性约束的轨迹规划方法、装置和系统与流程

1.本发明属于自动驾驶的轨迹规划领域，涉及一种考虑舒适性约束的轨迹规划方法、装置和系统。


背景技术：

2.在自动驾驶的轨迹规划领域，一般将路径规划与速度规划解耦开求解，轨迹规划作为自动驾驶中重要的一环，不仅决定了最终车辆行驶轨迹的安全性，也体现了自动驾驶乘坐的舒适性。现有的轨迹规划算法大多通过构建优化问题来求解，并且将舒适性项放入优化问题的代价函数中（软约束），这导致在某些场景中规划的结果不能完全满足乘坐的舒适性要求。


技术实现要素：

3.为了解决现有技术中存在的上述技术问题，本发明提出了一种考虑舒适性约束的轨迹规划方法、装置和系统，能够对自动驾驶车辆的规划路径和速度进行约束优化，实现自动驾驶的乘坐舒适性体验，具体技术方案如下：一种考虑舒适性约束的轨迹规划方法，包括以下步骤：步骤一，根据定位的自车全局位置及参考线的信息，计算出自车投影到参考线的投影点坐标，从投影点坐标开始等距采样至一固定长度，得到若干个离散的原始参考点；步骤二，通过构造并计算车辆行驶路径规划的代价函数及约束，对路径规划进行优化，得到路径优化结果，再根据路径优化结果，计算出路径规划对应的朝向角及曲率，得到平滑后的路径；步骤三，通过构造并计算车辆行驶速度规划的代价函数及约束，对规划速度进行优化，得到速度优化结果，计算出速度规划对应的朝向角及曲率，得到平滑后的速度；步骤四，将平滑后的速度与平滑后的路径融合得到带有速度信息的平滑轨迹。
4.进一步的，所述步骤一，具体包括以下子步骤：步骤1.1：根据车辆的定位模块给出的自车全局位置及决策给出的目标车道信息，计算自车在以原始道路中心线为参考线的投影的参考点坐标)，即自车距离参考线最近点的坐标；步骤1.2：对输入的参考线信息，从匹配的参考线的投影点坐标)开始，以为采样间隔等距采样至一固定长度，包含n个离散参考点（）、(),
ꢀ…
，()，其中表示直角坐标系坐
标，表示向量与直角坐标系中x轴的夹角，表示为离散点处参考线的曲率。
5.进一步的，所述步骤二，具体包括以下子步骤：步骤2.1：构造路径规划的代价函数，该路径代价函数包含两部分，第一部分为待优化变量与原始参考线参考点的相似代价，第二部分为平滑代价，如下所示：其中，为待优化变量（）、（），
…
，（）；为原始参考线参考点（）、（），
…
，（）；为相似代价权重，与车辆轴距l值成负相关，与车辆最大转向能力成正相关；平滑代价权重，为固定值权重；所述相似代价设置为待优化变量与原始参考点的欧式距离，表达式为：所述平滑代价设置为三点之间的直线相似程度，表达式为：；步骤2.2：构造路径规划的安全性约束，得到每一待优化变量（）偏离原始参考点的横纵向约束，表达式为：点的横纵向约束，表达式为：其中，、、、为设置在、方向上的上下界约束；步骤2.3：根据车型参数构造路径规划的曲率约束，离散点之间的曲率关系满足车辆的最小转弯半径约束，表达式为:，可转换为：其中，为两点间的距离，为最小转弯半径约束，由车辆自身参数设定；步骤2.4：根据步骤2.1得到的路径代价函数及步骤2.2和步骤2.3得到的约束条
件，通过求解该含约束条件的非线性优化路径，得到最终的路径优化结果即为平滑后的路径点坐标信息；步骤2.5：利用步骤2.4的路径优化结果，计算出路径点对应的朝向角及曲率，最终输出为一系列平滑后的路径点信息()，（），
…
，()，即输出平滑路径。
6.进一步的，所述步骤三，具体包括以下子步骤：步骤3.1：构造速度规划的代价函数及约束，该速度代价函数包含三部分：第一部分为待优化速度变量与本路段期望速度的相似代价，第二部分为待优化变量纵向加速度代价，第三部分为待优化变量纵向加速度代价，具体表达式如下：，具体表达式如下：，具体表达式如下：，具体表达式如下：，其中，为待优化变量，为该规划路段中期望速度值，为与期望速度相似代价权重，为加速度代价权重，为加速度代价权重,为预设的固定采样时间长度；步骤3.2：构造速度规划的第一舒适性约束，表达式为：步骤3.2：构造速度规划的第一舒适性约束，表达式为：其中，为保证舒适性的最大限速，为满足人体舒适性的向心加速度最大值，为步骤2.5得到的每个路径点对应的曲率；步骤3.3：构造速度规划的第二舒适性约束，表达式为：
其中，，为满足人体舒适度的最大纵向加速度值；步骤3.4：构造速度规划的连续性约束，表达式为：步骤3.5：根据步骤3.1得到的速度代价函数及步骤3.2至步骤3.4得到的约束条件，通过求解该含约束条件的最优化速度，得到最终的速度优化结果即为平滑的速度信息。
7.进一步的，所述步骤四具体为：根据步骤3.5得到的速度优化结果，将其和步骤2.5输出的平滑后的路径点进行联合，得到最终的满足舒适性约束的规划轨迹点()，（），
…
，()。
8.一种考虑舒适性约束的轨迹规划系统，包括路径平滑规划模块和速度规划模块，所述路径平滑规划模块输入设定的参考线信息、车辆全局定位信息、车辆参数，然后对其生成的路径施加车辆最大转向能力的曲率约束，输出一条满足车辆最小转弯半径约束的平滑路径；所述速度规划模块，针对平滑后路径点的曲率值计算出满足乘坐舒适性的最大限速，并将该限速作为速度优化的约束条件，从而求解出平滑速度的值，最后将平滑后的速度与路径融合，输出一系列带有速度信息的轨迹点。
9.一种考虑舒适性约束的轨迹规划装置，包括一个或多个处理器，用于实现所述的一种考虑舒适性约束的轨迹规划方法。
10.一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的一种考虑舒适性约束的轨迹规划方法。
11.有益效果：本发明对规划路径施加车辆最大转向能力的曲率约束，使平滑后的路径能满足不同车辆的控制执行能力；再进行速度规划，针对上一步平滑后路径点的曲率值计算出满足乘坐舒适性的最大限速，并将该限速放入速度优化问题的约束中，从而保证求解出的速度值能够满足乘坐舒适性体验。
附图说明
12.图1为本发明的一种考虑舒适性约束的轨迹规划方法的整体流程示意图；图2为本发明的考虑舒适性约束的轨迹规划方法的详细流程图；图3为本发明实施例的一种考虑舒适性约束的轨迹规划装置的结构示意图。
具体实施方式
13.为了使本发明的目的、技术方案和技术效果更加清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。
14.如图1和图2所示，本发明的一种考虑舒适性约束的轨迹规划方法，包括以下步骤：步骤一，根据定位的自车全局位置及参考线的信息，计算出自车投影到参考线的
投影点坐标，从投影点坐标开始等距采样至一固定长度，得到若干个离散原始参考点，具体包括以下子步骤：步骤1.1：根据车辆的定位模块给出的自车全局位置及决策给出的目标车道信息，计算自车在给定参考线（如以原始道路中心线为参考线）的投影的参考点坐标)，即自车距离参考线最近点的坐标；步骤1.2：对输入的参考线信息，从匹配的参考线的投影点坐标)开始，以为采样间隔等距采样至一固定长度，本实施例以0.25m为采样间隔等距采样对应的参考线至50m，包含n个离散参考点（）、(),
ꢀ…
，()，其中表示直角坐标系坐标，表示向量与直角坐标系中x轴的夹角，表示为离散点处参考线的曲率。
15.步骤二，通过构造并计算车辆行驶路径规划的代价函数及约束，以求解路径规划优化问题，得到路径优化结果，即对规划路径进行优化，再根据路径优化结果，计算出路径规划对应的朝向角及曲率，得到平滑后的路径，具体包括以下子步骤：步骤2.1：构造路径规划的代价函数，该路径代价函数包含两部分，第一部分为待优化变量与原始参考线参考点的相似代价，第二部分为平滑代价，如下所示：其中，为待优化变量（）、（），
…
，（）；为原始参考线参考点（）、（），
…
，（）；为相似代价权重，可设计为与车辆轴距l值成负相关，与车辆最大转向能力成正相关；平滑代价权重，可设为固定值权重；所述相似代价可以设置为待优化变量与原始参考点的欧式距离，表达式为：所述平滑代价可以设置为三点之间的直线相似程度，表达式为：；步骤2.2：构造路径规划的安全性约束，得到每一待优化变量（）偏离原始参考
点的横纵向约束，表达式为：点的横纵向约束，表达式为：其中，、、、为设置在、方向上的上下界约束，保证约束优化变量在安全范围内；步骤2.3：根据车型参数构造路径规划的曲率约束，离散点之间的曲率关系满足车辆的最小转弯半径约束，表达式为:，可转换为：其中，为两点间的距离，为最小转弯半径约束，由车辆自身参数设定；步骤2.4：根据步骤2.1得到的代价函数及步骤2.2和步骤2.3得到的约束条件，通过求解该含约束的非线性优化路径，得到最终的路径优化结果即为平滑后的路径点坐标信息；步骤2.5：利用步骤2.4的路径优化结果，计算出路径点对应的朝向角及曲率，最终输出为一系列平滑后的路径点信息()，（），
…
，()，即输出平滑路径。
16.步骤三，通过构造并计算车辆行驶速度规划的代价函数及约束，以求解速度规划优化问题，得到速度优化结果，即对规划速度进行优化，再根据速度优化结果，计算出速度规划对应的朝向角及曲率，得到平滑后的速度，具体包括以下子步骤：步骤3.1：构造速度规划的代价函数及约束，该速度代价函数包含三部分：第一部分为待优化速度变量与本路段期望速度的相似代价，第二部分为待优化变量纵向加速度代价，第三部分为待优化变量纵向加速度代价，具体表达式如下：，具体表达式如下：，具体表达式如下：
，其中，为待优化变量，为该规划路段中期望速度值，为与期望速度相似代价权重，为加速度代价权重，为加速度代价权重，为预设的固定采样时间长度；步骤3.2：构造速度规划的第一舒适性约束，表达式为：步骤3.2：构造速度规划的第一舒适性约束，表达式为：其中，为保证舒适性的最大限速，为满足人体舒适性的向心加速度最大值，为步骤2.5得到的每个路径点对应的曲率；步骤3.3：构造速度规划的第二舒适性约束，表达式为：其中，，为满足人体舒适度的最大纵向加速度值；步骤3.4：构造速度规划的连续性约束，表达式为：步骤3.5：根据步骤3.1得到的速度代价函数及步骤3.2至步骤3.4得到的约束条件，通过求解该含约束的最优化速度，得到最终的速度优化结果即为平滑的速度信息。
17.步骤四，最后将平滑后的速度与平滑后的路径融合得到带有速度信息的平滑轨迹。
18.具体的，根据步骤3.5得到的速度优化结果，将其和步骤2.5输出的平滑后的路径点进行联合，得到最终的满足舒适性约束的规划轨迹点()，（），
…
，()。
19.采用上述方法的一种考虑舒适性约束的轨迹规划系统，包括：路径平滑规划模块、速度规划模块。所述路径平滑规划模块首先输入设定的参考线信息、车辆全局定位信息、车辆参数，再对其生成的路径施加车辆最大转向能力的曲率约束，从而输出一条满足车辆最小转弯半径约束的平滑路径，使平滑路径满足不同车辆的控制执行能力；所述速度规划模块，针对平滑后路径点的曲率值计算出满足乘坐舒适性的最大限速，并将该限速放入速度优化问题的约束中，从而保证求解出的平滑速度的值能够满足乘坐舒适性体验，最后将平滑后的速度与路径融合，输出一系列带有速度信息的轨迹点。
20.与前述一种考虑舒适性约束的轨迹规划方法的实施例相对应，本发明还提供了一种考虑舒适性约束的轨迹规划装置的实施例。
21.参见图3，本发明实施例提供的一种考虑舒适性约束的轨迹规划装置，包括一个或多个处理器，用于实现上述实施例中的一种考虑舒适性约束的轨迹规划方法。
22.本发明一种考虑舒适性约束的轨迹规划装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图3所示，为本发明一种考虑舒适性约束的轨迹规划装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图3所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
23.上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。
24.对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
25.本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的一种考虑舒适性约束的轨迹规划方法。
26.所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（smart media card，smc）、sd卡、闪存卡（flash card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
27.以上所述，仅为本发明的优选实施案例，并非对本发明做任何形式上的限制。虽然前文对本发明的实施过程进行了详细说明，对于熟悉本领域的人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行同等替换。凡在本发明精神和原则之内所做修改、同等替换等，均应包含在本发明的保护范围之内。