自动驾驶路径规划方法、装置、设备及存储介质

技术领域

本申请涉及自动驾驶技术领域，具体而言，本申请涉及一种自动驾驶路径规划方法、装置、设备及存储介质。

背景技术

随着人工智能技术的的全面推广，自动驾驶技术呈现高速发展态势，主要体现在环境感知、决策与规划、控制与执行、高精度地图和实时定位等技术的发展。近年来，自动驾驶技术已经成为全世界汽车产业的最新发展方向。与传统汽车相比，自动驾驶汽车能够有效地提升车辆的安全性、通行效率和舒适性。无人驾驶车辆不仅可以减少交通事故的发生率，还可以提高汽车出行的效率，因此受到科研机构、汽车厂家和互联网公司等广泛关注。作为无人驾驶车辆的核心技术之一的路径规划，一直都是无人驾驶技术领域的难点和热点。

路径规划算法为无人驾驶车辆规划出一条已知环境地图信息下最优路径。无人驾驶汽车在前进过程中，处在不可预测和高度动态的道路环境中，障碍物很有可能出现在已经规划好的路径上，也有可能在前进的过程中一些障碍物动态地出现在路径上。自动驾驶车辆必须对这些不可预测的事件以某种方式做出反应。

自动驾驶汽车的路径规划需要考虑的问题。无人驾驶车辆需要实时规划出满足车辆自身大小和形状约束的可行驶路径，以应对复杂变化的交通环境。然而，现有的路径规划方案在解决上述问题方面存在许多缺点。

发明内容

本申请的目的旨在至少能解决上述的技术缺陷之一，特别是现有技术存在有累计误差，造成在点云定位过程中存在定位不收敛，最终造成定位不准确的技术缺陷。

第一方面，提供了一种自动驾驶路径规划方法，其特征在于，包括：

获取激光雷达采集的道路信息；

基于所述道路信息构建高精度地图；

确定无人车在所述高精度地图中的当前位置和所述无人车需要达到的目标位置；

确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径；

采用预设的方法计算各所述行驶路径的行驶代价，选取行驶代价最小的所述行驶路径作为目标行驶路径。

作为本申请一种可能的实施方式，在该实施方式中，所述获取激光雷达采集的道路信息，包括：

获取无人车的位姿信息，所述位姿信息包括所述无人车的全局定位坐标、偏航角、俯仰角以及翻滚角；

所述基于所述道路信息构建高精度地图，包括：

基于所述位姿信息确定点云到刚体变换的旋转矩阵，并将激光雷达采集的的全局定位坐标通过所述旋转矩阵变换为同一坐标系中，构建所述道路的三维环境地图。

作为本申请一种可能的实施方式，在该实施方式中，所述基于所述位姿信息确定点云到刚体变换的旋转矩阵，并将激光雷达采集的的全局定位坐标通过所述旋转矩阵变换为同一坐标系中，构建所述道路的三维环境地图，包括：

所述激光雷达在t时刻采集的点云数据为F

其中，R

作为本申请一种可能的实施方式，在该实施中，所述方法还包括：

将所述位姿信息作为预设的IPC配准算法的输入，提取所述道路的主干点云；

针对所述主干点云进行配准，确定所述高精度地图。

作为本申请一种可能的实施方式，在该实施中，所述确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径，包括：

初始化道路空间，确定所述当前位置点、目标位置点、采样点数、以及采样点之间的步长；

在所述道路空间中随机生成中间点；

在所述采样点中选择距离所述中间点最近的采样点作为目标中间点；

从所述采样点到所述目标中间点之间截取所述步长长度的中间路径；

当所述中间路径中不存在障碍物时，将所述中间路径确定为所述行驶路径中的一段。

作为本申请一种可能的实施方式，在该实施中，所述方法还包括：

计算所述无人车在所述行驶路径上的曲率；

选取所述曲率满足所述无人车自身运动学约束的路径构建所述目标行驶路径。

作为本申请一种可能的实施方式，在该实施中，所述计算所述无人车在所述行驶路径上的曲率之前还包括：

采用状态栅格算法导向扩展域；

基于所述当前位置点、目标位置点、采样点数、以及采样点之间的步长采用RRT算法搜索可行路径。

第二方面，提供了一种自动驾驶路径规划装置，该装置包括：

信息获取模块，用于获取激光雷达采集的道路信息；

地图构建模块，用于基于所述道路信息构建高精度地图；

位置确定模块，用于确定无人车在所述高精度地图中的当前位置和所述无人车需要达到的目标位置；

路径规划模块，用于确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径；

路径确定模块，用于采用预设的方法计算各所述行驶路径的行驶代价，选取行驶代价最小的所述行驶路径作为目标行驶路径。

第三方面，提供了一种电子设备，该电子设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述的自动驾驶路径规划方法。

第四方面，提供了一种计算机可读存储介质，所述存储介质存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述的自动驾驶路径规划方法。

本申请实施例通过激光雷达获取道路信息，并基于该道路信息构建高精度地图，确定无人车在高精度地图中的当前位置和无人车的目标位置，能够减少路径规划的随机性，在给定起点和终点坐标后，能够找到可行驶路径并推导出相应的输入信息，在对动态障碍物避障的时候，无人车进行局部路径规划，能够合理选择一条可行驶路径。根据运动学约束条件，如速度、转弯半径、路径曲率等，对规划路线进行修正。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对本申请实施例描述中所需要使用的附图作简单地介绍。

图1为本申请实施例提供的一种自动驾驶路径规划方法的流程示意图；

图2为本申请实施例提供的一种构建高精度地图的方法的流程示意图；

图3为本申请实施例提供的一种车辆建模示意图；

图4为本申请实施例提供的一种RRT算法流程图；

图5为本申请实施例提供的一种状态示意图；

图6为本申请实施例提供的一种自动驾驶路径规划装置的结构示意图；

图7为本申请实施例提供的一种电子设备的结构示意图。

结合附图并参考以下具体实施方式，本申请各实施例的上述和其他特征、优点及方面将变得更加明显。贯穿附图中，相同或相似的附图标记表示相同或相似的元素。应当理解附图是示意性的，原件和元素不一定按照比例绘制。

具体实施方式

下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能解释为对本申请的限制。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数行驶“一”、“一个”、“所述”和“该”也可包括复数行驶。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。

为使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请实施方式作进一步地详细描述。

随着人工智能技术的的全面推广，自动驾驶技术呈现高速发展态势，主要体现在环境感知、决策与规划、控制与执行、高精度地图和实时定位等技术的发展。近年来，自动驾驶技术已经成为全世界汽车产业的最新发展方向。与传统汽车相比，自动驾驶汽车能够有效地提升车辆的安全性、通行效率和舒适性。无人驾驶车辆不仅可以减少交通事故的发生率，还可以提高汽车出行的效率，因此受到科研机构、汽车厂家和互联网公司等广泛关注。作为无人驾驶车辆的核心技术之一的路径规划，一直都是无人驾驶技术领域的难点和热点。

路径规划算法为无人驾驶车辆规划出一条已知环境地图信息下最优路径。无人驾驶汽车在前进过程中，处在不可预测和高度动态的道路环境中，障碍物很有可能出现在已经规划好的路径上，也有可能在前进的过程中一些障碍物动态地出现在路径上。自动驾驶车辆必须对这些不可预测的事件以某种方式做出反应。

自动驾驶汽车的路径规划需要考虑的问题。无人驾驶车辆需要实时规划出满足车辆自身大小和形状约束的可行驶路径，以应对复杂变化的交通环境。然而，现有的路径规划方案在解决上述问题方面存在许多缺点。

本申请提供的基于自动驾驶路径规划方法、装置、电子设备和计算机可读存储介质，旨在解决现有技术的如上技术问题。

下面以具体地实施例对本申请的技术方案以及本申请的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本申请的实施例进行描述。

本申请实施例中提供了一种自动驾驶路径规划方法，如图1所示，该方法包括：

步骤S101，获取激光雷达采集的道路信息；

步骤S102，基于所述道路信息构建高精度地图；

步骤S103，确定无人车在所述高精度地图中的当前位置和所述无人车需要达到的目标位置；

步骤S104，确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径；

步骤S105，采用预设的方法计算各所述行驶路径的行驶代价，选取行驶代价最小的所述行驶路径作为目标行驶路径。

在本申请实施例中，计算平台选取是英伟达Jetson agx xavier，并装有Ubuntu18.04和机器操作系统ROS，硬件有velodyne 16线的机械激光雷达，线控电动底盘，组合惯导系统(IMU)。遥控驾驶车辆对道路信息进行采集，将原始点云地图数据录好，通过高精地图软件3D unity对原始点云地图数据处理，构建有行驶区域、车道线的高精度地图，将构建好的高精地图导入计算平台。对于高精度地图的构建，先通过激光雷达采集道路信息，再基于该道路信息构建高精度地图，其中，所述获取激光雷达采集的道路信息，包括：

获取无人车的位姿信息，所述位姿信息包括所述无人车的全局定位坐标、偏航角、俯仰角以及翻滚角；

所述基于所述道路信息构建高精度地图，包括：

基于所述位姿信息确定点云到刚体变换的旋转矩阵，并将激光雷达采集的的全局定位坐标通过所述旋转矩阵变换为同一坐标系中，构建所述道路的三维环境地图。

在本申请实施例中，对于高精度点云地图的构建，通常是通过由组合惯性导航系统提供的位置数据来构建，其位姿数据包括全局定位坐标以及以惯性导航设备为中心的偏航角、俯仰角以及翻滚角三个姿态数据。将t时刻采集的一组位姿数据表示为Z

其中，R

然而，由于惯性导航系统所提供的位姿在定位以及姿态上都具有一定偏差，甚至在定位信号缺失的情况下，只能依靠惯性测量单元进行位姿推算，造成位姿误差的累积。因此，构建的粗精度点云地图还需要通过点云的帧间配准，以减小误差，进一步提高地图的精度。

作为本申请一种可能的实施方式，在该实施方式中，如图2所示，所述方法还包括：

步骤S201，将所述位姿信息作为预设的IPC配准算法的输入，提取所述道路的主干点云；

步骤S202，针对所述主干点云进行配准，确定所述高精度地图。

在本申请实施例中，获取采集得到的惯性导航系统提供的位姿数据，将其融合进ICP算法中提取道路主干点云，作为改进ICP算法的输入，提取道路主干点云后即可利用ICP算法进行配准，输入定位点集P与当前待配准的点云S，运用改进的ICP算法，通过公式(1)对符合道路主干的点云进行提取并实现主干道点云配准，得到配准矩阵R

通过上述改进的ICP配准方法，最终实现高精度点云地图的构建。

在本申请实施例中，需要对车辆进行建模，如图3所示，车辆运动学方程形式如下：

对其进行线性化，得到线性时变模型为：

式中，各矩阵和状态变量为：

T为采样时间。

作为本申请一种可能的实施方式，在该实施方式中，所述确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径，包括：

初始化道路空间，确定所述当前位置点、目标位置点、采样点数、以及采样点之间的步长；

在所述道路空间中随机生成中间点；

在所述采样点中选择距离所述中间点最近的采样点作为目标中间点；

从所述采样点到所述目标中间点之间截取所述步长长度的中间路径；

当所述中间路径中不存在障碍物时，将所述中间路径确定为所述行驶路径中的一段。

作为本申请一种可能的实施方式，在该实施方式中，所述方法还包括：

计算所述无人车在所述行驶路径上的曲率；

选取所述曲率满足所述无人车自身运动学约束的路径构建所述目标行驶路径。

在本申请实施例中，为方便说明，以一个具体实施例为例，在基于RRT算法搜索的时候，其算法流程图如图4所示，初始化整个空间，定义初始点，终点，采样点数、点与点之间的步长等信息；在空间中随机产生一点x

式中Length表示从起点到终点总的路线长度，

在本申请实施例中，RRT算法在进行全局路径规划，是没有动力学约束的条件下搜索的轨迹，考虑到自动驾驶车辆实际运动的速度、加速度等动力学约束，需要运用状态栅格算法，对当前路径进行优化，以满足自动驾驶车辆行驶的要求。状态栅格算法具体实现方法是基于控制空间的采样：假设无人车的动力模型为

状态变量

输入向量

状态转移矩阵

系统方程式：

度量函数目的是在随机扩展树上找到一点x

N维的欧式距离：

由上述轨迹方程，可计算无人车的驾驶轨迹。

本申请实施例通过激光雷达获取道路信息，并基于该道路信息构建高精度地图，确定无人车在高精度地图中的当前位置和无人车的目标位置，能够减少路径规划的随机性，在给定起点和终点坐标后，能够找到可行驶路径并推导出相应的输入信息，在对动态障碍物避障的时候，无人车进行局部路径规划，能够合理选择一条可行驶路径。根据运动学约束条件，如速度、转弯半径、路径曲率等，对规划的路线进行修正。

本申请实施例提供了一种自动驾驶路径规划装置，如图6所示，该基于三维点云的定位装置60可以包括：信息获取模块601、地图构建模块602、位置确定模块603、路径规划模块604、以及路径确定模块605，其中：

信息获取模块601，用于获取激光雷达采集的道路信息；

地图构建模块602，用于基于所述道路信息构建高精度地图；

位置确定模块603，用于确定无人车在所述高精度地图中的当前位置和所述无人车需要达到的目标位置；

路径规划模块604，用于确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径；

路径确定模块605，用于采用预设的方法计算各所述行驶路径的行驶代价，选取行驶代价最小的所述行驶路径作为目标行驶路径。

作为本申请一种可能的实施方式，在该实施方式中，所述获取激光雷达采集的道路信息，包括：

获取无人车的位姿信息，所述位姿信息包括所述无人车的全局定位坐标、偏航角、俯仰角以及翻滚角；

所述基于所述道路信息构建高精度地图，包括：

基于所述位姿信息确定点云到刚体变换的旋转矩阵，并将激光雷达采集的的全局定位坐标通过所述旋转矩阵变换为同一坐标系中，构建所述道路的三维环境地图。

作为本申请一种可能的实施方式，在该实施方式中，所述基于所述位姿信息确定点云到刚体变换的旋转矩阵，并将激光雷达采集的的全局定位坐标通过所述旋转矩阵变换为同一坐标系中，构建所述道路的三维环境地图，包括：

所述激光雷达在t时刻采集的点云数据为F

其中，R

作为本申请一种可能的实施方式，在该实施方式中，所述方法还包括：

将所述位姿信息作为预设的IPC配准算法的输入，提取所述道路的主干点云；

针对所述主干点云进行配准，确定所述高精度地图。

作为本申请一种可能的实施方式，在该实施方式中，所述确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径，包括：

初始化道路空间，确定所述当前位置点、目标位置点、采样点数、以及采样点之间的步长；

在所述道路空间中随机生成中间点；

在所述采样点中选择距离所述中间点最近的采样点作为目标中间点；

从所述采样点到所述目标中间点之间截取所述步长长度的中间路径；

当所述中间路径中不存在障碍物时，将所述中间路径确定为所述行驶路径中的一段。

作为本申请一种可能的实施方式，在该实施方式中，所述方法还包括：

计算所述无人车在所述行驶路径上的曲率；

选取所述曲率满足所述无人车自身运动学约束的路径构建所述目标行驶路径。

本申请实施例通过激光雷达获取道路信息，并基于该道路信息构建高精度地图，确定无人车在高精度地图中的当前位置和无人车的目标位置，能够减少路径规划的随机性，在给定起点和终点坐标后，能够找到可行驶路径并推导出相应的输入信息，在对动态障碍物避障的时候，无人车进行局部路径规划，能够合理选择一条可行驶路径。根据逆推出来运动学约束条件，如速度、转弯半径、路径曲率等，使无人车完成进行运动路径规划。

本申请实施例中提供了一种电子设备，该电子设备包括：存储器和处理器；至少一个程序，存储于存储器中，用于被处理器执行时，获取激光雷达采集的道路信息；基于所述道路信息构建高精度地图；确定无人车在所述高精度地图中的当前位置和所述无人车需要达到的目标位置；确定所述行驶路径上的障碍物，并采用状态栅格算法计算多条从所述当前位置到所述目标位置的行驶路径；采用预设的方法计算各所述行驶路径的行驶代价，选取行驶代价最小的所述行驶路径作为目标行驶路径。

与现有技术相比可实现：本申请实施例通过激光雷达获取道路信息，并基于该道路信息构建高精度地图，确定无人车在高精度地图中的当前位置和无人车的目标位置，能够减少路径规划的随机性，在给定起点和终点坐标后，能够找到可行驶路径并推导出相应的输入信息，在对动态障碍物避障的时候，无人车进行局部路径规划，能够合理选择一条可行驶路径。根据运动学约束条件，如速度、转弯半径、路径曲率等，对规划路线进行修正。

在一个可选实施例中提供了一种电子设备，如图7所示，图7所示的电子设备4000包括：处理器4001和存储器4003。其中，处理器4001和存储器4003相连，如通过总线4002相连。可选地，电子设备4000还可以包括收发器4004。需要说明的是，实际应用中收发器4004不限于一个，该电子设备4000的结构并不构成对本申请实施例的限定。

处理器4001可以是CPU(Central Processing Unit，中央处理器)，通用处理器，DSP(Digital Signal Processor，数据信号处理器)，ASIC(Application SpecificIntegrated Circuit，专用集成电路)，FPGA(Field Programmable Gate Array，现场可编程门阵列)或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本申请公开内容所描述的各种示例性的逻辑方框，模块和电路。处理器4001也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，DSP和微处理器的组合等。

总线4002可包括一通路，在上述组件之间传送信息。总线4002可以是PCI(Peripheral Component Interconnect，外设部件互连标准)总线或EISA(ExtendedIndustry Standard Architecture，扩展工业标准结构)总线等。总线4002可以分为地址总线、数据总线、控制总线等。为便于表示，图7中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。

存储器4003可以是ROM(Read Only Memory，只读存储器)或可存储静态信息和指令的其他类型的静态存储设备，RAM(Random Access Memory，随机存取存储器)或者可存储信息和指令的其他类型的动态存储设备，也可以是EEPROM(Electrically ErasableProgrammable Read Only Memory，电可擦可编程只读存储器)、CD-ROM(Compact DiscRead Only Memory，只读光盘)或其他光盘存储、光碟存储(包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等)、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构行驶的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。

存储器4003用于存储执行本申请方案的应用程序代码，并由处理器4001来控制执行。处理器4001用于执行存储器4003中存储的应用程序代码，以实现前述方法实施例所示的内容。

本申请实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，当其在计算机上运行时，使得计算机可以执行前述方法实施例中相应内容。与现有技术相比，本申请实施例通过分别获取组合导航数据和激光雷达测量的点云数据，并分别对点云数据和组合导航数据进行处理，先基于组合导航数据确定车辆的当前位姿信息，并确定点云定位中的变换矩阵和平移矩阵，再基于该变换矩阵和平移矩阵确定车辆的激光雷达位姿信息，然后基于该激光雷达位姿信息和车辆的姿态信息确定车辆的姿态计算法方程，然后采用扩展卡尔曼滤波方法基于所述位姿状态计算方程对所述车辆进行定位，更加符合车辆的行驶规律，保证车辆定位的准确性。

应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

以上所述仅是本申请的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。