CN115718496A - 一种路径规划方法、装置和域控制器 - Google Patents

Abstract

本发明公开了一种路径规划方法、装置和域控制器，其中方法包括：基于车道中心线构造Frenet坐标系，并获取车辆特性；获取大地坐标系下车辆的后轴中心当前位姿信息和车头中心当前位姿信息；将道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成道路环境模型；基于道路环境模型和车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，并生成代价函数；基于车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。本发明提供的技术方案，降低了车辆在规划路径上行驶时车头绕障过程的摆动幅度。

Description

一种路径规划方法、装置和域控制器

技术领域

本发明涉及路径规划领域，具体涉及一种路径规划方法、装置和域控制器。

背景技术

在无人驾驶***中，轨迹规划常采用路径-速度解耦规划策略。其中，路径规划结合智能单车自身信息以及周围环境为自身提供空间连续的局部行车路径，以局部行车路径作为时空连续的全局行车轨迹的空间基础。目前，分段加加速度算法作为主流的路径规划算法之一，应用比较广泛。该算法首先由百度Apollo开源项目提出，主要思路是将车辆看做一个质点，并将周围障碍物和道路边界组合转换到Frenet坐标系下，构建最优控制问题，然后数值求解该问题，获得路径。但是该方式存在一个问题：将本车当成一个质点(通常是车辆后轴中心)，则忽略了车辆本身的刚体特性，对于长车型车辆，实际转弯过程中基于后轴中心规划出来行车路径容易导致车辆头部位置与障碍物干涉。

针对上述问题，在李柏、张友民等人编著的《自动驾驶决策规划技术理论与实践》一书中，提出了多圆盘等效替代车辆刚体几何方法。如图1所示，其主要步骤为：S1.将道路场景隧道化改造；S2.使用无穷多个圆盘覆盖车身，这些圆盘的圆心致密地覆盖车体纵轴，其直径均为车宽，得到整个车身的刚体表述；S3.根据车身刚体的几何关系构建碰撞躲避约束；S4.离散化处理碰撞躲避约束加入分段加加速度算法的最优控制问题；S5.求解最优控制问题。

在该技术方案中考虑了车辆的刚体特性，而非将车辆视为一个质点，通过多圆盘等效替代车辆模型，基于等效模型构建碰撞躲避约束，加入最优控制问题，使得规划路径确保车辆整体避免与障碍物干涉情况。虽然该技术方案规划出来的路径保证了车辆整体不与障碍物干涉，但是在实际应用中发现，该技术方案规划出的路径使得汽车车头绕障过程摆动幅度过大，从而给用户带来的路径舒适性较差。

发明内容

有鉴于此，本发明实施方式提供了一种路径规划方法、装置和域控制器，从而降低了车辆在规划路径上行驶时车头绕障过程的摆动幅度。

根据第一方面，本发明实施例提供了一种路径规划方法，所述方法包括：基于车道中心线构造Frenet坐标系，并获取车辆特性；获取大地坐标系下车辆的后轴中心当前位姿信息，并基于所述车辆特性计算所述后轴中心当前位姿信息对应的车头中心当前位姿信息；将道路边界信息、障碍物信息、所述后轴中心当前位姿信息和所述车头中心当前位姿信息映射到所述Frenet坐标系中，生成道路环境模型；基于所述道路环境模型和所述车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束；基于所述道路环境模型生成代价函数；基于所述车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对所述代价函数进行优化求解，得到车辆后轴中心的规划路径。

可选地，基于所述道路环境模型和所述车辆特性创建所述车辆运动学约束，包括：将道路环境模型进行离散化为多个离散段，每段的端点为离散点；基于离散后的道路环境模型创建分段加加速度算法的传统运动学约束；利用所述车辆特性确定Frenet坐标中后轴中心横向偏移参数与车头中心横向偏移参数的转换关系；基于映射到所述Frenet坐标系中的后轴中心当前位姿信息和车头中心当前位姿信息确定起点离散点的优化变量；预设终点离散点的优化变量；将所述转换关系、所述传统运动学约束、所述起点离散点的优化变量、所述终点离散点的优化变量作为所述车辆运动学约束。

可选地，基于所述道路环境模型和所述车辆特性创建后轴中心碰撞约束，包括：基于离散化的道路环境模型将映射到所述Frenet坐标系中的道路边界信息和障碍物信息形成的映射结果离散化，确定车辆可通过的道路上边界函数和道路下边界函数；将道路上边界函数和道路下边界函数之间的范围扣除车身宽度，并将剩余的范围作为所述后轴中心碰撞约束。

可选地，基于所述道路环境模型和所述车辆特性创建所述车头中心碰撞约束，包括：利用所述车辆特性和当前后轴中心所在的离散点位置推导当前车头中心所在的离散点位置；分别计算所述道路上边界函数和所述道路下边界函数在目标范围内的上下边界结果，所述目标范围是当前后轴中心所在的离散点位置到当前车头中心所在的离散点位置的长度范围；从所述上下边界结果中提取出表征最窄边界的车头中心上边界值和车头中心下边界值；将提取的所述车头中心上边界值和所述车头中心下边界值之间的范围扣除车身宽度，并将剩余的范围作为当前车头中心所在的离散点位置的车头中心碰撞约束。

可选地，创建所述车头摆动速度约束，包括：对所述车头中心横向偏移参数的导数预设导数范围，并将预设的所述导数范围作为所述车头摆动速度约束。

可选地，所述基于所述道路环境模型生成代价函数，包括：利用各个离散点计算的车头中心上边界值和车头中心下边界值生成车头中心上边界函数和车头中心下边界函数；通过所述车头中心上边界函数和所述车头中心下边界函数确定车头中心的车头行驶参考线；基于Frenet坐标中车头中心横向偏移参数与所述车头行驶参考线之间的差值关系，生成改进的代价函数。

可选地，所述方法还包括：将所述车辆后轴中心的规划路径转换到大地坐标系下，并对转换后的路径进行碰撞检测；若检测结果表示转换后的路径未发生碰撞，则将转换后的路径发送至车辆的控制处理单元。

根据第二方面，本发明实施例提供了一种路径规划装置，所述装置包括：初始化模块，用于基于车道中心线构造Frenet坐标系，并获取车辆特性；车辆信息采集模块，用于获取大地坐标系下车辆的后轴中心当前位姿信息，并基于所述车辆特性计算所述后轴中心当前位姿信息对应的车头中心当前位姿信息；道路环境仿真模块，用于将道路边界信息、障碍物信息、所述后轴中心当前位姿信息和所述车头中心当前位姿信息映射到所述Frenet坐标系中，生成道路环境模型；约束生成模块，用于基于所述道路环境模型和所述车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束；代价函数生成模块，用于基于所述道路环境模型生成代价函数；优化计算模块，用于基于所述车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对所述代价函数进行优化求解，得到车辆后轴中心的规划路径。

根据第三方面，本发明实施例提供了一种域控制器，包括：感知处理单元、决策处理单元、控制处理单元和通信单元，所述感知处理单元、所述决策处理单元、所述控制处理单元和所述通信单元之间互相通信连接，所述决策处理单元中存储有计算机指令，所述决策处理单元通过执行所述计算机指令，从而执行第一方面，或者第一方面任意一种可选实施方式中所述的方法。

根据第四方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机从而执行第一方面，或者第一方面任意一种可选实施方式中所述的方法。

本申请提供的技术方案，具有如下优点：

本申请提供的技术方案，首先基于车道中心线构造Frenet坐标系，并获取车辆特性，包括车长、车宽、前后轴距等等几何参数；然后获取大地坐标系下车辆的后轴中心当前位姿信息，并基于车辆特性计算后轴中心当前位姿信息对应的车头中心当前位姿信息；将道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成车辆需要通过的道路环境模型；然后基于道路环境模型和车辆特性创建车辆运动学约束用于控制车辆连续运动，结合车辆特性创建后轴中心碰撞约束和车头中心碰撞约束，从而在车辆的刚体层面避免车辆与道路边界或障碍物碰撞，并且创建车头摆动速度约束，限制车辆绕障时的车头摆动幅度。之后基于道路环境模型生成分段加加速度算法的代价函数，并基于车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。通过上述步骤生成的路径，一方面在车辆行驶的过程中考虑了车辆的刚体结构，避免了车辆碰撞；另一方面，基于车头摆动速度约束对车头摆动幅度进行限制，避免了车辆绕障车头摆动过大的情况，提高了驾乘人员的舒适性。

附图说明

通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：

图1示出了现有技术中对车身的处理效果示意图；

图2示出了本发明一个实施方式中一种路径规划方法的步骤示意图；

图3示出了本发明一个实施方式中一种路径规划方法的流程示意图；

图4示出了本发明一个实施方式中道路边界信息的映射结构示意图；

图5示出了本发明一个实施方式中车头中心与后轴中心的几何关系结构示意图；

图6示出了本发明一个实施方式中一种路径规划装置的结构示意图；

图7示出了本发明一个实施方式中一种域控制器的结构示意图。

具体实施方式

为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。

请参阅图2和图3，在一个实施方式中，一种路径规划方法，具体包括如下步骤：

步骤S101：基于车道中心线构造Frenet坐标系，并获取车辆特性。

步骤S102：获取大地坐标系下车辆的后轴中心当前位姿信息，并基于车辆特性计算后轴中心当前位姿信息对应的车头中心当前位姿信息。

步骤S103：将道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成道路环境模型。

步骤S104：基于道路环境模型和车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束。

步骤S105：基于道路环境模型生成代价函数。

步骤S106：基于车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。

具体地，本发明实施例在分段加加速度的算法基础上，提出了进一步改进。首先从地图模块获取车辆所属车道中心线的离散点集，并基于车道中心线构建Frenet坐标系。从定位模块获取本车后轴中心的当前航向θ_{r_ego}，后轴中心当前位置的大地坐标(x_{r_ego},y_{r_ego})，从而获取本车后轴中心的当前位姿信息。获取表征车辆刚体几何结构的车辆特性(如自卸车、公交车、乘用车等)，并根据车辆特性推导车头中心位姿态信息(x_{f_ego},y_{f_ego},θ_{f_ego},κ_{f_ego})，其中x_{f_ego}、y_{f_ego}是当前车头中心的大地坐标，θ_{f_ego}是车头中心的当前航向，κ_{f_ego}是车头中心的曲率。此外，还获取道路边界信息，并将道路边界信息映射到Frenet坐标系中，得到还没有考虑障碍物的上下边界常函数ruw(s)和rlw(s)，是从道路中线起点位置开始并随道路中线延伸长度s变化而变化的常函数，例如：参考图4，某个在道路中线上的位置，距离起点长度s＝100，该位置相距两边的道路上下边界均为5米，则ruw(100)＝5且rlw(100)＝-5，上下边界函数的数值随着s的变化而变化，例如随着道路的延长可能出现车道数量的变化。另外，获取障碍物信息，包括障碍物的航向和障碍物的大地坐标。

将上述道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成道路环境模型。本车后轴中心及车头中心位姿信息从大地坐标系转换到Frenet坐标系下是为了确定路径的规划起点(s_init,l_init,l′_init,l″_init,l_{f_init})，即当前道路的起点从哪里开始。其中，s_init表示车辆后轴中心当前所在道路中心线的位置与Frenet坐标初始位置s₀的距离，l_init表示车辆后轴中心当前相对道路中心线的横向偏移长度，l′_init是横向偏移的一阶导数，用于表示横向偏移的速度，l″_init是横向偏移的二阶导数，用于表示横向偏移的加速度。道路边界信息和障碍物信息综合映射到Frenet坐标系中是为了确定车辆行驶过程中每个位置能够让车辆通过的道路宽度，主要用于创建车辆刚体的碰撞约束。基于本发明实施例生成的道路环境模型即可确定道路的路况。

然后基于道路环境模型和车辆特性创建用于控制车辆连续运动车辆运动学约束、将车辆的后轴中心和车头中心分开考虑，创建了基于车辆刚体的后轴中心碰撞约束和车头中心碰撞约束，并单独对车头中心创建车头摆动速度约束。同时生成分段加加速度算法的代价函数。

最后，利用车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。一方面在车辆行驶的过程中考虑了车辆的刚体结构，避免了车辆碰撞；另一方面，基于车头摆动速度约束对车头摆动幅度进行限制，避免了车辆绕障车头摆动过大的情况，提高了驾乘人员的舒适性。

具体地，在一实施例中，步骤S104中基于道路环境模型和车辆特性创建车辆运动学约束，具体包括如下步骤：

步骤一：将道路环境模型进行离散化为多个离散段，每段的端点为离散点。

具体地，首先将道路环境模型离散化，用n个均匀分布的离散点{s_k}，将里程区间分成(n-1)个离散段Δs：

式中，s_max是道路环境模型的里程总长度，i表示离散点的角标。

步骤二：基于离散后的道路环境模型创建分段加加速度算法的传统运动学约束。

具体地，基于上述离散后的道路环境模型，创建分段加加速度算法传统运动学约束，如下式所示：

l′_min≤l′_i≤l′_max

l″_min≤l″_i≤l″_max

l″′_min≤l″′_i≤l″′_max

式中，i表示第i个离散点s_i的下角标，l_i表示后轴中心在第i个离散点位置相对于道路中线的偏移距离，l′_i表示后轴中心偏移距离对s的一阶导数，l″_i表示后轴中心偏移距离对s的二阶导数，l″′_i表示后轴中心偏移距离对s的三阶导数，l′_min和l′_max分别是预设的一阶导数最小最大取值范围，用于限制后轴中心发生横向偏移的速度，l″′_min和l″_max分别是预设的二阶导数最小最大取值范围，用于限制后轴中心发生横向偏移的加速度，l″′_min和l″′_max分别是预设的三阶导数最小最大取值范围，用于限制后轴中心发生横向偏移的加加速度，以避免加速度过大影响乘客舒适性。上述分段加加速度算法的传统运动学约束是实现轨迹规划的必要约束条件，这一部分为现有技术，本发明不再赘述。

步骤三：利用车辆特性确定Frenet坐标中后轴中心横向偏移参数与车头中心横向偏移参数的转换关系。

具体地，本发明实施例为了限制车辆车头的摆动，需要单独考虑车头的规划轨迹，从而需要分别考虑车辆的后轴中心和车头中心各自的碰撞约束，前提则是需要创建后轴中心和车头中心的Frenet坐标转换关系作为运动学约束，才能将后轴中心和车头中心的轨迹关联起来，综合判断车辆是否会发生碰撞，转换关系的具体推导过程如下：

首先获取车辆特性信息，具体包括：L_w前后轮轴距，L_r车辆后悬距离，L_f车辆前悬距离，L_b车宽。如图5所示，后轴中心相对于道路中线的横向偏移l(s)和车头中心相对于道路中线的横向偏移l_f(s)满足如下几何关系：

R·sinθ+l(s)＝l_f(s),R＝L_w+L_f

式中θ是车辆轴线与道路中线的夹角。

因为：

所以：

l′_f(s)＝R·l″(s)+l′(s)

得到后轴中心横向偏移参数与车头中心横向偏移参数的转换关系为：

l_f(s)＝R·l′(s)+l(s)

步骤四：基于映射到Frenet坐标系中的后轴中心当前位姿信息和车头中心当前位姿信息确定起点离散点的优化变量。

步骤五：预设终点离散点的优化变量。

具体地，下一步需要根据离散化区间构建最优控制问题的优化变量，包括l_i,l′_i,l″_i,l_fi，分别表示在

位置的后轴中心横向位移、后轴中心横向位移对s的一阶导、后轴中心横向位移对s的二阶导和车头中心横向位移，优化变量如下：

l_i,l′_i,l″_i,l_fi i＝0,…,n-1

在本发明实施例中，预设终点位置没有横向移动及运动趋势，从而为了让车辆达到终点的时候，能够与道路中线契合，即预设终点离散点的优化变量为l_end＝0,l′_end＝0,l″_end＝0,l_{f_end}＝0。

根据终点离散点的优化变量和起点离散点的优化变量构建如下车辆运动学约束。

步骤六：最后，将转换关系、传统运动学约束、起点离散点的优化变量、终点离散点的优化变量作为车辆运动学约束。

具体地，在本实施例中，上述步骤S104中创建后轴中心碰撞约束的步骤具体包括：

步骤七：基于离散化的道路环境模型将映射到Frenet坐标系中的道路边界信息和障碍物信息形成的映射结果离散化，确定车辆可通过的道路上边界函数和道路下边界函数。

步骤八：将道路上边界函数和道路下边界函数之间的范围扣除车身宽度，并将剩余的范围作为后轴中心碰撞约束。

具体地，将映射障碍物信息与道路自身边界rlw(s)和ruw(s)进行隧道化处理，获得车辆能够通过的道路轮廓(可参考图1)，之后将道路轮廓离散化获得车辆可通过的道路上边界函数ub(i)和道路下边界函数lb(i)。针对每一个离散点的道路上边界和道路下边界值，扣除车辆的1/2宽度值，则可以得到后轴中心碰撞约束，后轴中心在Frenet坐标系中的横向偏移只要不超过上述约束，从车辆刚体的角度考虑，车辆后轴位置则不会发生碰撞，表达式如下：

式中，道路下边界函数lb(i)的函数值为负数，道路上边界函数ub(i)的函数值为正数。

具体地，在本实施例中，上述步骤S104中创建车头中心碰撞约束的步骤具体包括：

步骤九：利用车辆特性和当前后轴中心所在的离散点位置推导当前车头中心所在的离散点位置。

步骤十：分别计算道路上边界函数和道路下边界函数在目标范围内的上下边界结果，目标范围是当前后轴中心所在的离散点位置到当前车头中心所在的离散点位置的长度范围。

步骤十一：从上下边界结果中提取出表征最窄边界的车头中心上边界值和车头中心下边界值。

步骤十二：将提取的车头中心上边界值和车头中心下边界值之间的范围扣除车身宽度，并将剩余的范围作为当前车头中心所在的离散点位置的车头中心碰撞约束。

具体地，为了进一步从车辆刚体角度考虑车头的碰撞，本实施例创建车头中心碰撞约束的表达式如下：

lb_f(i)＝max[lb(i),…,lb(min(i+index,n-1))]

ub_f(i)＝min[ub(i),…,ub(min(i+index,n-1))]

式中，ceil表示向上取整算法，index表示利用车辆特性推导的下角标增量，L_w表示前后轮轴距，L_f表示车辆前悬距离，若当前后轴中心所在的离散点位置为第i个离散点，则推导得到当前车头中心所在的离散点位置为第i+index个离散点。

上式表征的物理含义为，利用道路上下边界函数分别计算第i个离散点到第i+index个离散点的上下边界数值，然后取其中的最窄上边界和最窄下边界，其中因为下边界相对于道路中线是负数，所以上式取最窄下边界的运算是最大值max运算。需要注意的是，若利用index计算的当前车头中心所在离散点位置超过了里程终点位置(第n-1个离散点)，则取第i个离散点到第n-1个离散点的最窄边界。每个离散点经过上述运算则得到了各个离散点的车头中心上边界值和车头中心下边界值，lb_f(i)和ub_f(i)则分别是车头下边界函数和车头上边界函数。

之后，从各个离散点的车头中心上边界值和车头中心下边界值中扣除车身宽度，并以扣除车身宽度L_b的范围作为车头中心碰撞约束，使车头中心的横向偏移参数l_f(s)在车头中心碰撞约束内，即可从车辆刚体角度，保证规划出的轨迹使车头遇到最窄边界也不会发生碰撞，公式如下所示：

具体地，在本实施例中，还需创建车头摆动速度约束，用来限制车头的摆动幅度，具体步骤为：

步骤十三：对车头中心横向偏移参数的导数预设导数范围，并将预设的导数范围作为车头摆动速度约束。

具体地，车头摆动速度约束如下式所示

l′_f-min≤l′_fi≤l′_f-max

根据步骤三推导的转换关系得到

l′_f-min≤(L_w+_f)·l″_i+l′_i≤l′_f-max

式中，l′_f-min为预设的车头横向偏移导数的最小值，l′_f-max为预设的车头横向偏移导数的最大值，从而通过上述约束条件即可对车头的摆动速度进行约束，使车辆按照规划的路径行驶时，降低绕障的车头摆动。

具体地，在一实施例中，上述步骤S105，具体包括如下步骤：

步骤十四：利用各个离散点计算的车头中心上边界值和车头中心下边界值生成车头中心上边界函数和车头中心下边界函数。

步骤十五：通过车头中心上边界函数和车头中心下边界函数确定车头中心的车头行驶参考线。

步骤十六：基于Frenet坐标中车头中心横向偏移参数与车头行驶参考线之间的差值关系，生成改进的代价函数。

具体地，在本实施例中，为了进一步降低车辆行驶时的车头摆动幅度，还提出了改进的分段加加速度算法代价函数，额外添加了车头优化项。由于道路中障碍物的存在，车辆不能完全按照道路中心行驶，从而首先根据车头中心上边界函数ub_f(i)和车头中心下边界函数lb_f(i)确定车头中心的车头行驶参考线，即车辆行驶时，基于车头中心上下边界设定的车头最佳行驶路线，在本实施例中，令车头行驶参考线

仅以此举例，不以此为限，车头行驶参考线还可以在车头中心上边界函数ub_f(i)和车头中心下边界函数lb_f(i)之间由用户自定义。然后，基于Frenet坐标中车头中心横向偏移参数l_fi与车头行驶参考线

之间的差值关系，添加车头优化项

到原始的代价函数中，令车头行驶时尽可能贴近参考线，从而实现对代价函数的改进。在后续步骤中，基于约束条件对本实施例提供的代价函数进行寻优计算，能够进一步使规划出的路径降低车辆绕障时的车头摆动幅度。具体地，改进后的代价函数如下所示：

式中，w表示代价函数中各项的预设权重，根据用户的实际需求调整；

表示后轴中心横向偏移与后轴中心参考线的差值关系，该值越小表征车辆行驶时后轴中心越靠近后轴中心参考线，在本实施例中，后轴中心参考线可设为

即道路上下边界函数的中值，还可根据用户需求自定义，仅以此举例，不以此为限；

表示后轴中心横向偏移的速度；

表示后轴中心横向偏移的加速度；

表示后轴中心横向偏移的加加速度；其中横向偏移的速度、加速度和加加速度三项越小，给驾乘人员带来的乘坐体验越舒适；最后一项

即车头中心横向偏移与车头中心参考线的差值关系，用来优化车头摆动幅度。

具体地，在一实施例中，当上述改进的代价函数和约束条件构造完成后，将代价函数和上述约束条件转换成计算机能够进行计算的QP问题形式，通过数值解决器，如OSQP进行求解，即可获得路径曲线序列。

具体地，在一实施例中，当车辆后轴中心的规划路径计算完成后，本发明实施例还将车辆后轴中心的规划路径转换到大地坐标系下，并对转换后的路径进行碰撞检测；若检测结果表示转换后的路径未发生碰撞，则将转换后的路径发送至车辆的控制处理单元，使车辆响应于规划的路径控制车辆的速度和转向，否则不将将转换后的路径发送至车辆的控制处理单元，以进一步保证车辆自动驾驶的安全性。

通过上述步骤，本申请提供的技术方案，首先基于车道中心线构造Frenet坐标系，并获取车辆特性，包括车长、车宽、前后轴距等等几何参数；然后获取大地坐标系下车辆的后轴中心当前位姿信息，并基于车辆特性计算后轴中心当前位姿信息对应的车头中心当前位姿信息；将道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成车辆需要通过的道路环境模型；然后基于道路环境模型和车辆特性创建车辆运动学约束用于控制车辆连续运动，结合车辆特性创建后轴中心碰撞约束和车头中心碰撞约束，从而在车辆的刚体层面避免车辆与道路边界或障碍物碰撞，并且创建车头摆动速度约束，限制车辆绕障时的车头摆动幅度。之后基于道路环境模型生成分段加加速度算法的代价函数，并基于车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。通过上述步骤生成的路径，一方面在车辆行驶的过程中考虑了车辆的刚体结构，避免了车辆碰撞；另一方面，基于车头摆动速度约束对车头摆动幅度进行限制，避免了车辆绕障车头摆动过大的情况，提高了驾乘人员的舒适性。

如图6所示，本实施例还提供了一种路径规划装置，该装置包括：

初始化模块101，用于基于车道中心线构造Frenet坐标系，并获取车辆特性。详细内容参见上述方法实施例中步骤S101的相关描述，在此不再进行赘述。

车辆信息采集模块102，用于获取大地坐标系下车辆的后轴中心当前位姿信息，并基于车辆特性计算后轴中心当前位姿信息对应的车头中心当前位姿信息。详细内容参见上述方法实施例中步骤S102的相关描述，在此不再进行赘述。

道路环境仿真模块103，用于将道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成道路环境模型。详细内容参见上述方法实施例中步骤S103的相关描述，在此不再进行赘述。

约束生成模块104，用于基于道路环境模型和车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束。详细内容参见上述方法实施例中步骤S104的相关描述，在此不再进行赘述。

代价函数生成模块105，用于基于道路环境模型生成代价函数。详细内容参见上述方法实施例中步骤S105的相关描述，在此不再进行赘述。

优化计算模块106，用于基于车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。详细内容参见上述方法实施例中步骤S106的相关描述，在此不再进行赘述。

本发明实施例提供的一种路径规划装置，用于执行上述实施例提供的一种路径规划方法，其实现方式与原理相同，详细内容参见上述方法实施例的相关描述，不再赘述。

通过上述各个组成部分的协同合作，本申请提供的技术方案，首先基于车道中心线构造Frenet坐标系，并获取车辆特性，包括车长、车宽、前后轴距等等几何参数；然后获取大地坐标系下车辆的后轴中心当前位姿信息，并基于车辆特性计算后轴中心当前位姿信息对应的车头中心当前位姿信息；将道路边界信息、障碍物信息、后轴中心当前位姿信息和车头中心当前位姿信息映射到Frenet坐标系中，生成车辆需要通过的道路环境模型；然后基于道路环境模型和车辆特性创建车辆运动学约束用于控制车辆连续运动，结合车辆特性创建后轴中心碰撞约束和车头中心碰撞约束，从而在车辆的刚体层面避免车辆与道路边界或障碍物碰撞，并且创建车头摆动速度约束，限制车辆绕障时的车头摆动幅度。之后基于道路环境模型生成分段加加速度算法的代价函数，并基于车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对代价函数进行优化求解，得到车辆后轴中心的规划路径。通过上述步骤生成的路径，一方面在车辆行驶的过程中考虑了车辆的刚体结构，避免了车辆碰撞；另一方面，基于车头摆动速度约束对车头摆动幅度进行限制，避免了车辆绕障车头摆动过大的情况，提高了驾乘人员的舒适性。

图7示出了本发明实施例的一种域控制器，该域控制器至少包括感知处理单元901、决策处理单元902、控制处理单元903和通信单元904，其中感知处理单元901、决策处理单元902、控制处理单元903和通信单元904之间可以通过总线或者其他方式互相通信连接，图7中以总线方式为例。

在本实施例中，感知处理单元901和决策处理单元902分别包括独立的处理器，感知处理单元901和决策处理单元902可以分别包括独立的存储器，也可以使用共享的存储器。

在本发明实施例中，感知处理单元901主要应用于工程机械场景，主要作用为对传感器数据进行感知融合处理，得到当前工程机械所处环境的环境信息，然后根据环境信息信号的数据类型将环境信息发送至控制处理单元903或者决策处理单元902。

存储器作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如上述方法实施例中的方法所对应的程序指令/模块。感知处理单元901通过运行存储在存储器中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理，即实现上述方法实施例中的方法。

决策处理单元902的作用为：结合周边环境、作业场景、车辆状态等信息融合之后，制定驾驶或作业策略，最终发出控制命令。

控制处理单元903的主要作用为：通信协议转换(CAN、以太网、LIN等)、AD转换(传感器输入)、DA转换(控制驱动)等的不同类型信号之间的转换。例如将激光雷达扫描到的信号转换为点云数据，控制处理单元903可以是美国德州仪器TI芯片TDA4VM的MCU、以色列Mobileye公司EyeQ系列芯片的MCU、日本瑞萨R-CAR芯片R-CAR H3的MCU、中国地平线公司征程系列芯片的MCU等。

通信单元904的主要作用为：进行无线通信，通信方式包括但不限于5G/4G网络通信、Wi-Fi通信、卫星通信，与云端服务器进行通信，其主要作用为：将设备相关状态以及信息上传到云端服务，请求云端服务器协助计算处理，也可以通过云端服务器下载数据，对控制器进行OTA软件升级；与附近的设备通信，可以接收其他设备的状态，协同完成作业任务。控制模块的通信单元110可以为5G模组、Wi-Fi模组、蓝牙模组等。

本领域技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，实现的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)、随机存储记忆体(Random Access Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid-State Drive，SSD)等；存储介质还可以包括上述种类的存储器的组合。

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下作出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (10)

1.一种路径规划方法，其特征在于，所述方法包括：

基于车道中心线构造Frenet坐标系，并获取车辆特性；

获取大地坐标系下车辆的后轴中心当前位姿信息，并基于所述车辆特性计算所述后轴中心当前位姿信息对应的车头中心当前位姿信息；

将道路边界信息、障碍物信息、所述后轴中心当前位姿信息和所述车头中心当前位姿信息映射到所述Frenet坐标系中，生成道路环境模型；

基于所述道路环境模型和所述车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束；

基于所述道路环境模型生成代价函数；

基于所述车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对所述代价函数进行优化求解，得到车辆后轴中心的规划路径。

2.根据权利要求1所述的方法，其特征在于，基于所述道路环境模型和所述车辆特性创建所述车辆运动学约束，包括：

将道路环境模型进行离散化为多个离散段，每段的端点为离散点；

基于离散后的道路环境模型创建分段加加速度算法的传统运动学约束；

利用所述车辆特性确定Frenet坐标中后轴中心横向偏移参数与车头中心横向偏移参数的转换关系；

基于映射到所述Frenet坐标系中的后轴中心当前位姿信息和车头中心当前位姿信息确定起点离散点的优化变量；

预设终点离散点的优化变量；

将所述转换关系、所述传统运动学约束、所述起点离散点的优化变量、所述终点离散点的优化变量作为所述车辆运动学约束。

3.根据权利要求2所述的方法，其特征在于，基于所述道路环境模型和所述车辆特性创建后轴中心碰撞约束，包括：

基于离散化的道路环境模型将映射到所述Frenet坐标系中的道路边界信息和障碍物信息形成的映射结果离散化，确定车辆可通过的道路上边界函数和道路下边界函数；

将道路上边界函数和道路下边界函数之间的范围扣除车身宽度，并将剩余的范围作为所述后轴中心碰撞约束。

4.根据权利要求2所述的方法，其特征在于，基于所述道路环境模型和所述车辆特性创建所述车头中心碰撞约束，包括：

利用所述车辆特性和当前后轴中心所在的离散点位置推导当前车头中心所在的离散点位置；

分别计算所述道路上边界函数和所述道路下边界函数在目标范围内的上下边界结果，所述目标范围是当前后轴中心所在的离散点位置到当前车头中心所在的离散点位置的长度范围；

从所述上下边界结果中提取出表征最窄边界的车头中心上边界值和车头中心下边界值；

将提取的所述车头中心上边界值和所述车头中心下边界值之间的范围扣除车身宽度，并将剩余的范围作为当前车头中心所在的离散点位置的车头中心碰撞约束。

5.根据权利要求2所述的方法，其特征在于，创建所述车头摆动速度约束，包括：

对所述车头中心横向偏移参数的导数预设导数范围，并将预设的所述导数范围作为所述车头摆动速度约束。

6.根据权利要求4所述的方法，其特征在于，所述基于所述道路环境模型生成代价函数，包括：

利用各个离散点计算的车头中心上边界值和车头中心下边界值生成车头中心上边界函数和车头中心下边界函数；

通过所述车头中心上边界函数和所述车头中心下边界函数确定车头中心的车头行驶参考线；

基于Frenet坐标中车头中心横向偏移参数与所述车头行驶参考线之间的差值关系，生成改进的代价函数。

7.根据权利要求1所述的方法，其特征在于，所述方法还包括：

将所述车辆后轴中心的规划路径转换到大地坐标系下，并对转换后的路径进行碰撞检测；

若检测结果表示转换后的路径未发生碰撞，则将转换后的路径发送至车辆的控制处理单元。

8.一种路径规划装置，其特征在于，所述装置包括：

初始化模块，用于基于车道中心线构造Frenet坐标系，并获取车辆特性；

车辆信息采集模块，用于获取大地坐标系下车辆的后轴中心当前位姿信息，并基于所述车辆特性计算所述后轴中心当前位姿信息对应的车头中心当前位姿信息；

道路环境仿真模块，用于将道路边界信息、障碍物信息、所述后轴中心当前位姿信息和所述车头中心当前位姿信息映射到所述Frenet坐标系中，生成道路环境模型；

约束生成模块，用于基于所述道路环境模型和所述车辆特性创建车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束；

代价函数生成模块，用于基于所述道路环境模型生成代价函数；

优化计算模块，用于基于所述车辆运动学约束、后轴中心碰撞约束、车头中心碰撞约束和车头摆动速度约束，对所述代价函数进行优化求解，得到车辆后轴中心的规划路径。

9.一种域控制器，其特征在于，包括：感知处理单元、决策处理单元、控制处理单元和通信单元，所述感知处理单元、所述决策处理单元、所述控制处理单元和所述通信单元之间互相通信连接，所述决策处理单元中存储有计算机指令，所述决策处理单元通过执行所述计算机指令，从而执行如权利要求1-7任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机从而执行如权利要求1-7任一项所述的方法。

Images