技术领域
[0001] 本发明涉及智能车辆技术领域，尤其涉及一种基于改进的双向A星搜索算法的路径规划方法。
相关背景技术
[0002] 路径规划指的是基于已知的环境信息，通过搜索等方法获得一条连接起点位置到目标位置的路径点。A*搜索算法是一种启发式搜索算法，用于在图或栅格中寻找两点之间的最短路径。A*搜索算法利用启发函数f(n)＝g(n)+h(n)确定每个子节点的优先级和下一步扩展方向，其中g(n)表示起点到当前位置的累计移动代价，h(n)表示当前位置到目标位置的启发式函数值(简称“启发值”)。A*搜索算法结合了Dijkstra算法的最短路径搜索和最佳优先搜索的思想，在许多情况下比单纯的贪婪搜索或Dijkstra算法更加高效，其广泛用于路径规划等领域。A*搜索算法虽然能够一定程度上改善暴力搜索的不足，但是在搜索过程中常常会因为遇到障碍物等原因产生大量无效的搜索节点，从而影响算法的空间与时间效率。
[0003] 双向A*搜索算法是一种用于解决搜索问题的启发式搜索算法，它是A*搜索算法的一种改进，旨在减少搜索的节点数量，从而提高搜索的效率。双向A*搜索算法同时从起点和终点开始搜索，分别利用启发式函数来评估节点的优先级。在每一步中，从两个方向选择具有最低优先级的节点进行扩展。当两个搜索方向的搜索路径相交时，即找到了一条从起点到终点的路径。然而，由于双向A*搜索算法在搜索过程中存在部分位置搜索节点臃肿导致搜索效率降低，以及由于地形导致搜索不到中间节点，最终变成由单向搜索到对应位置生成路径。由于双向A*搜索算法在搜索中是正向与反向交替搜索的，因此当出现由地形导致搜索不到中间节点的情况，双向A*搜索算法的求解时间比单向A*搜索算法更长。
[0004] 滑移与滑转是车辆在行驶过程中，接地部分与地面发生相对移动的现象。滑移与滑转(统称为“滑动”)是广泛存在于车辆行驶过程中不可避免的一项参数，对车辆的行驶产生重要影响。经过长时间的研究和发展，路径规划领域已经较为成熟，形成了多种框架与方法。但是在这些方法中，适应于越野场景履带平台的规划方法较少，且研究内容涉及滑动参数对路径规划的影响的内容更少。然而在越野场景下，履带车辆受到滑动参数的影响较为明显，现有路径规划方案中没有充分利用滑动参数对于路径选择与生成的引导作用，进而导致规划生成的路径结果在滑动参数方面表现并不理想，轨迹质量降低，安全性不足。如何根据滑动参数合理规划行驶路径进而提高无人驾驶安全性是当前亟待解决的问题。
具体实施方式
[0057] 下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
[0058] 图1示出了根据本发明实施例的基于改进的双向A星搜索算法的路径规划方法的流程示意图。如图1所示，该基于改进的双向A星搜索算法的路径规划方法包括以下步骤：
[0059] 步骤100：获取目标场景的障碍物栅格地图。
[0060] 在本实施例中，可以利用车辆自身的传感器来采集车辆周围的环境信息。具体而言，可以利用激光雷达采集车辆周围环境的点云地图。
[0061] 在本实施例中，可以对目标场景的点云数据进行障碍物检测得到障碍物地图。之后，根据栅格分辨率对障碍物地图进行分割，得到障碍物栅格地图。在本实施例中，目标场景的点云数据指的是车辆自身的激光雷达所采集的点云地图中位于目标区域的点云数据。示例性的，目标区域可以是位于车辆前方一定距离处的块状区域。
[0062] 在一些实施例中，步骤100中的障碍物栅格地图是通过以下方法确定的：
[0063] 步骤S1001：基于障碍物检测算法，确定目标场景的基础障碍物地图，其中，所述基础障碍物地图的格式为栅格。
[0064] 步骤S1002：根据车辆的几何尺寸对所述基础障碍物地图中的障碍物进行膨胀并对所述基础障碍物地图中的离散区域进行填充，得到障碍物栅格地图。
[0065] 图2示出了根据本发明实施例的生成障碍物栅格地图的流程示意图。如图2所示，首先可以生成场景的点云数据；之后，通过对点云数据进行障碍物检测算法得到基础障碍物地图；随后，在基础障碍物地图上进行障碍物膨胀处理；最后，对离散障碍物之间的空白区域进行填充处理以扩大障碍物的面积、降低碰撞风险。在本实施例中，障碍物膨胀处理可以是将障碍物按照一定比例进行放大。在本实施例中，离散区域的选择可以由车辆自身的几何尺寸确定。
[0066] 步骤200：基于优化的双向A*搜索算法在所述障碍物栅格地图中确定从起点位置至目标位置的运动路径。
[0067] 传统的单向A*搜索算法在平面栅格地图上进行路径搜索时，首先将起点位置放入到开放集合中，然后遍历并计算相邻节点的移动代价与启发值之和；在每一次迭代时选择开放集合中具有最小代价总和的节点，对其进行子节点生成并将其父节点加入到闭集合中，如果生成的子节点存在与障碍物碰撞或者已经处于闭集合中，则跳过该子节点；如果生成的子节点已经处于开集合中，则根据当前生成方式是否路径更优来选择是否更新其移动代价和启发值之和；如果生成的子节点是目标位置，则将其加入到闭集合中，算法结束，沿着父节点指针一直追溯到起点位置即可生成一条目标路径。
[0068] 传统的双向A*搜索算法指的是在搜索阶段从两端(即起点位置和目标位置)同时开始向中间交替式搜索，两端分别维护各自的开放集，直到在中间某个节点相遇。具体的算法流程如下：在起点位置与目标位置开始分别进行正向A*搜索和反向A*搜索，正向A*搜索的终点为目标位置，反向A*搜索的终点为起点位置，每一端初始化各自的开集合与闭集合；接下来交替地对于正向搜索与反向搜索进行子节点生成与选择，更新各自的开集合与闭集合，记录已经搜索过的节点并更新节点的代价总和以及设置相应节点之间的父子关系；之后通过不断迭代直到当两个开集合中有共同元素时，将其作为相遇节点并加入正向搜索的闭集合中，翻转反向搜索中节点的父子关系并拼接两个闭集合即可得到规划路径，如果一方开集合为空则说明没有可行路径。
[0069] 在本实施例中，采用了优化的双向A*搜索算法，具体体现在对正向A*搜索中终点的更改以及反向A*搜索中终点的更改。在优化的双向A*搜索算法中，子节点的正向总代价包括子节点的正向移动代价及其权重和子节点的正向启发式函数值及其权重，子节点的反向总代价包括子节点的反向移动代价及其权重和子节点的反向启发式函数值及其权重；子节点的正向启发式函数值为从子节点到当前具有最小反向总代价的子节点的启发式函数值，子节点的反向启发式函数值为从子节点到当前具有最小正向总代价的子节点的启发式函数值。
[0070] 下面以中间某一时刻对本实施例中的优化的双向A*搜索算法进行说明：
[0071] 图3示出了根据本发明实施例的优化的双向A*搜索算法的一个中间时刻的概念性示意图。如图3所示，本实施例中的优化的双向A*搜索算法需要在地图上的起点位置至目标位置之间寻找一条可通行的路径，正向A*搜索的起点为图中的“起点位置”，反向A*搜索的起点为图中的“目标位置”。在图3中，灰色小圆和黄色小圆代表了从开始时刻到当前时刻正向A*搜索过程中所搜索过的所有节点，蓝色小圆和棕色小圆代表了从开始时刻到当前时刻反向A*搜索过程中所搜索过的所有节点。黄色线段代表了当前时刻正向A*搜索过程中所形成的可通行路径，棕色线段代表了当前时刻反向A*搜索过程中所形成的可通行路径。黄色小圆还代表了当前时刻正向A*搜索过程中所形成的路径上的最新节点，棕色小圆代表了当前时刻反向A*搜索过程中所形成的路径上的最新节点。在当前时刻，正向A*搜索的终点为棕色小圆所代表的节点，反向A*搜索的终点为黄色小圆所代表的节点。由图3可知，在本实施例中，正向A*搜索的终点由传统的目标位置更改为当前反向A*搜索中总代价最低的节点(即棕色小圆所代表的节点)，反向A*搜索的终点由传统的起点位置更改为当前正向A*搜索中总代价最低的节点(即黄色小圆所代表的节点)。
[0072] 图4示出了传统的双向A*搜索算法与本实施例中的优化的双向A*搜索算法的一个搜索结果对比示意图。如图4所示，传统的双向A*搜索算法需要进行171次搜索更新节点才能够相遇(每个小圆代表一次更新节点)，本实施例中的优化的双向A*搜索算法只需要40次节点更新即可找到相遇位置，本实施例中的优化的双向A*搜索算法将搜索时间从原本的4.98ms减少到1.02ms，因此显著提高了搜索效率。
[0073] 在一些实施例中，优化的双向A*搜索算法还包括对A*搜索算法的优化，具体体现在对A*搜索算法中代价函数的修改。也即，本实施例中的优化的双向A*搜索算法还包括对正向A*搜索算法中代价函数的修改和对反向A*搜索算法中代价函数的修改。由于本实施例中对正向A*搜索算法中代价函数的修改方式和对反向A*搜索算法中代价函数的修改方式相同，这里统一以优化的A*搜索算法表示。
[0074] 在本实施例中，采用了优化的A*搜索算法，具体体现在对代价函数g(n)的修改。在优化的A*搜索算法中，子节点的代价总和包括子节点的移动代价及其权重和子节点的启发式函数值及其权重，其中，子节点的移动代价包括当前父节点的移动代价、当前父节点与子节点的距离代价及其权重和子节点位置处的车辆滑动参数估计值及其权重。在本实施例中，子节点位置处的车辆滑动参数估计值是通过索引滑动参数预测栅格地图得到。
[0075] 在本实施例中，滑动参数预测栅格地图中的每个栅格记录了该栅格位置处的车辆滑动参数估计值。车辆滑动参数估计值可以取值0‑1之间，当车辆滑动参数估计值为0时，车辆不存在滑移或滑转；当车辆滑动参数估计值为1时,车辆处于纯滑移状态或纯滑转状态。
[0076] 下面对本实施例中的目标场景的滑动参数预测栅格地图的生成过程进行说明。
[0077] 图5示出了根据本发明实施例的车辆滑动参数的估计方法的流程示意图。如图5所示，该车辆滑动参数的估计方法包括以下步骤：
[0078] 步骤S201：获取目标场景的点云数据、图像数据以及车辆的轨迹数据。
[0079] 在本实施例中，可以利用车辆自身的传感器来采集车辆周围的环境信息以及车辆的轨迹数据。具体而言，可以分别利用激光雷达、单目RGB相机、惯性导航系统、GPS接收器、车辆底层控制器信息等传感器采集车辆周围环境的点云地图、图像数据、惯性导航系统的三轴加速度信息、GPS系统的定位信息以及车辆底层的转速转向信息。
[0080] 在本实施例中，目标场景的点云数据指的是车辆自身的激光雷达所采集的点云地图中位于目标区域的点云数据。示例性的，目标区域可以是位于车辆前方一定距离处的块状区域。
[0081] 在一些实施例中，目标场景的点云数据可以通过以下方法获取：
[0082] 根据车辆的几何尺寸和车辆的行驶轨迹从点云地图中截取目标区域的点云块数据。
[0083] 图6示出了根据本发明实施例的获取目标场景的点云数据的示意图。如图6所示，车辆的几何中心为原点o，车辆的前进方向为x轴，车辆的速度为v(在x轴上)，以车辆几何中心为原点，建立车辆坐标系x‑o‑y。在本实施例中，可以根据车辆几何尺寸提取相对应点云块，点云块的大小是根据包围车辆左右履带外轮廓的矩形框得到。为了保证车辆能够完全落在点云块内部且预留一定的误差范围，可以对车辆长度与宽度向上取整。在图6中，矩形框的长度为[length]，宽度为[width]。根据车辆的行驶轨迹可以截取车辆前方距离为d位置处的点云块。已知车辆几何中心点o为当前离散轨迹点，该处的航向角为θ，可以得到目标区域顶点坐标为A(xa,ya)、B(xb,yb)、C(xc,yc)和D(xd,yd)，如下表所示：
[0084]
[0085] 已知x‑o‑y坐标系下点原点o的坐标为(xo,yo)，航向角为θ，即可计算出大地坐标系X‑O‑Y下的点A(xA,yA),B(xB,yB),C(xC,yC),D(xD,yD)的真实坐标，如下表所示：
[0086]
[0087] 最后，根据大地坐标系下点A(xA,yA),B(xB,yB),C(xC,yC),D(xD,yD)的真实坐标进行点云截取，得到目标场景的点云数据。
[0088] 在本实施例中，通过根据车辆几何尺寸提取相对应点云块，可以提高点云数据表达的特征的质量与关联度。
[0089] 在本实施例中，可以通过对点云数据、图像数据以及车辆的轨迹数据进行时间对齐和空间对齐的方式得到目标场景的图像数据以及时空对齐后的车辆的轨迹数据。下面对点云数据、图像数据以及车辆的轨迹数据的时空对齐方式进行说明。
[0090] 在采集数据时，相机是通过瞬间曝光来获取图像帧的，因此图像数据频率较大，而激光雷达是通过激光束旋转来获取点云数据的，因此点云数据频率较低。另外，GPS和惯性导航系统采集数据的频率也较高，因此车辆的轨迹数据的频率也较高。由于这三类数据的采样频率存在一定差异，这里首先就需要对点云数据、图像数据和车辆轨迹数据进行时间上的同步。
[0091] 图7示出了对本发明实施例中的点云数据、图像数据和车辆轨迹数据进行时间上的同步的示意图。如图7所示，可以采用多线程和队列缓存方法，同时开辟三个线程接受点云、图像和轨迹数据，可以以较低频的点云数据的时间戳作为消息同步基准，将图像和轨迹数据与基准时间戳比较，直到时间差值在设定阈值内，则完成了一帧数据的时间同步。
[0092] 在完成在对点云数据、图像数据和车辆轨迹数据的时间同步后，需要获取激光雷达和相机的坐标系的位姿转换关系，进而实现图像数据和点云数据空间位置的对齐。需要说明的是，激光雷达和相机的坐标系的位姿转换关系是一个超参数，可以在下文即将描述的滑动参数估计模型的训练阶段学习到。在滑动参数估计模型的应用阶段，只要激光雷达和相机在车辆上的位置没有变，即可以使用在训练阶段学习到的激光雷达和相机的坐标系的位姿转换关系。
[0093] 下面对学习激光雷达和相机的坐标系的位姿转换关系的过程进行说明。这里可以采用现有的标定方法对相机和激光雷达进行快速联合标定。例如，可以首先在地面关键角点位置摆放特征物，并在线获取图像的实时数据；然后利用OpenCV库中基于Harris角点和边界跟踪的角点检测算法，得到相机的标定内参；之后对于点云数据选择标定物的四个角点作为种子点，通过区域生长算法获得扫描到关键点区域的点云数据；随后经过基于随机一致性算法进行平面拟合可以得到图像数据和点云数据对于同一块位置区域的多组姿态数据表示，进而求解图像数据和点云数据之间的空间转换关系。
[0094] 在学习到图像数据和点云数据的空间转换关系之后，可以根据目标场景的点云数据中关键点的位置信息(例如图6中的A点、B点、C点、D点)，通过两者的空间转换关系即可以得到关键点在图像数据中的匹配位置，进而得到表征同一块位置区域的图像数据。
[0095] 对于滑动参数而言，其影响因素除了通过图像数据与点云数据能够进行一定程度反应，车辆的行驶状态也会对滑动参数产生一定的影响。因此为了实现对于滑动参数的准确估计，本实施例还考虑了车辆的轨迹数据，例如包括车辆的速度与加速度信息等。由于根据本实施例得到的目标场景的图像数据与点云数据属于低频信号，而惯性导航信号与GPS信号属于高频信号，因此在对于轨迹数据进行匹配时，可以采用一帧目标场景的点云数据对应的最近时间的附近固定时间窗口内的速度信息、加速度信息、车辆转向程度信息。该固定时间窗口内的速度信息、加速度信息、车辆转向程度信息即为时空对齐后的车辆的轨迹数据。对于时间窗口中包含N个采样信号的速度、加速度与转向程度信息x(i)，可以采用以下5个常用的统计量来表征其特征：
[0096] (1)均值：
[0097]
[0098] (2)标准差：
[0099]
[0100] (3)方根幅值：
[0101]
[0102] (4)均方根值：
[0103]
[0104] (5)最大绝对值：
[0105] f5＝max|x(i)|
[0106] 在一些实施例中，加速度信息可以选取其中的纵向加速度以及垂向加速度进行分析。纵向加速度反应车辆的加速或减速操作，在此过程中滑动参数变化较大。垂向加速度是2
描述车辆在垂线方向的加速度量，需要提前消除重力加速度ag＝9.8m/s的影响，代表着车辆行驶中的震动程度，能够在一定程度上反映当前的地面属性。速度信息可以选取其中的三轴速度进行分析，其中纵向速度代表着车辆的行驶速度，横向速度代表着车辆的转向等操作，垂向速度代表着车辆在垂直高度上的爬升。转向程度信息是通过底层电机左右转速结合惯性导航系统计算出的车辆实时转向曲率。对于以上六个变量，每个变量可以生成5个统计量特征，一共可以生成30个统计量特征。
[0107] 步骤S202：将所述目标场景的点云数据、图像数据以及车辆的轨迹数据输入至预先训练完成的滑动参数估计模型，得到滑动参数的估计结果。
[0108] 图8示出了根据本发明实施例的滑动参数估计模型的网络框架示意图。如图8所示，该滑动参数估计模型包括依次连接的传感器数据特征提取网络、特征融合网络、参数(即滑动参数)识别网络、分层线性化层。传感器数据特征提取网络包括点云数据残差网络、相机数据残差网络、轨迹特征提取网络。点云数据残差网络用于从目标场景的点云数据中提取点云特征。相机数据残差网络用于从目标场景的图像数据中提取图像特征。轨迹特征提取网络用于从车辆的轨迹数据中提取轨迹特征。特征融合网络包括第一融合网络和第二融合网络(图8中未示出)。第一融合网络用于将点云特征和轨迹特征进行融合。第二融合网络用于将图像特征和轨迹特征进行融合。参数(即滑动参数)识别网络包括点云数据卷积长短期记忆网络(即图8中的“点云数据ConvLSTM”)和相机数据卷积长短期记忆网络(即图8中的“相机数据ConvLSTM”)。第一融合网络的输出输入至点云数据卷积长短期记忆网络，第二融合网络的输出输入至相机数据卷积长短期记忆网络。点云数据卷积长短期记忆网络用于对融合后的点云特征和轨迹特征进行滑动参数学习，相机数据卷积长短期记忆网络用于对融合后的图像特征和轨迹特征进行滑动参数学习。点云数据ConvLSTM的输出结果和相机数据ConvLSTM的输出结果输入至分层线性化层，通过多个线性化层的融合与提取，最终输出滑动参数的估计结果。
[0109] 图9示出了根据本发明另一实施例的滑动参数估计模型的网络结构示意图。如图9所示，该滑动参数估计模型的输入数据有三部分，分别为：表示目标场景的图像数据的三通道图像信息(RGB)，表示目标场景的点云数据的二通道图像信息(高度与反射强度)，表示车辆的轨迹数据的速度、加速度、转向信息。
[0110] 在本实施例中，在将目标场景的点云数据输入至滑动参数估计模型之前，可以对目标场景的点云数据进行预处理，得到点云数据的二维图像信息。具体而言，可以将每个点云块投影成一个包含高度和强度信息的二通道图像。图10示出了根据本发明实施例的点云投影图像的示意图。如图10所示，三维点云块中每个点都被指定给图像中与其最近的像素，每个像素单元内对应着许多个点云数据点pi，称为点云簇。计算每个像素中点云簇的高度均值hmean和反射强度均值imean，并将其赋给图像对应像素的红蓝两通道，对于点云缺失的区域则通过周围像素块进行线性插值填充。
[0111] 在本实施例中，在将车辆的轨迹数据输入至滑动参数估计模型之前，可以对车辆的轨迹数据进行特征提取。例如对于加速度信息中的纵向加速度和垂向加速度，分别计算其如下的五个特征：
[0112] (1)均值、(2)标准差、(3)方根幅值、(4)均方根值、(5)最大绝对值。
[0113] 对于速度信息中的纵向速度、横向速度、垂向速度，分别计算其如下的五个特征：
[0114] (1)均值、(2)标准差、(3)方根幅值、(4)均方根值、(5)最大绝对值。
[0115] 对于转向程度信息，分别计算其如下的五个特征：
[0116] (1)均值、(2)标准差、(3)方根幅值、(4)均方根值、(5)最大绝对值。
[0117] 对于以上六个变量，每个变量可以生成5个统计量特征，一共可以生成30个统计量特征。
[0118] 回到图9，三通道的图像信息的输入参数为[b,3,a,b']，第一维的参数b代表了一个批量的大小，第二维的参数代表图像为三个通道，第三维和第四维的参数代表了图像的尺寸信息。车辆的轨迹数据的输入参数为[b,6*5]，第一维的参数b代表了一个批量的大小，6*5代表了“六个变量，每个变量可以生成5个统计量特征”。点云数据的输入参数为[b,3,a,b']，第一维的参数b代表了一个批量的大小，第二维的参数代表图像为二个通道，第三维和第四维的参数代表了图像的尺寸信息。
[0119] 三通道的图像信息输入至一个残差网络模块(即图9中上方的“Resnet”)，二通道的点云信息输入至另一个残差网络模块(即图9中下方的“Resnet”)。轨迹特征通过连续的线性化层扩展到[b,w*h]，并且通过重整(reshape)操作重整到[b,w,h]。上方的残差网络模块可以从三通道的图像信息中提取图像特征，下方的残差网络模块可以从二通道的点云信息中提取点云特征。之后，将图像特征与点云特征分别与轨迹特征通过元素乘法相融合，并对融合结果进行卷积、批标准化、激活、池化处理。图9中的“CBRP”模块分别代表卷积层(C)、批标准化层(B)、激活层(R)、池化层(P)。“CBRP”模块输出的融合特征分别输入至ConvLSTM模块进行滑动参数的学习。最后对两个ConvLSTM模块的输出进行扁平化拼接，然后对数据采用先分层线性化再通道线性化的方式，最终回归到滑动参数估计值。
[0120] 在一些实施例中，图像特征提取采用的为ResNet‑34网络，点云特征提取采用的为ResNet‑18网络。图像数据蕴含丰富的环境特征，通过采用更复杂的ResNet‑34网络可以充分利用图像中蕴含特征。
[0121] 在一些实施例中，在将目标场景的两通道图像、图像数据输入至预先训练完成的滑动参数估计模型之前，可以对目标场景的两通道图像、图像数据执行如下操作：
[0122] 步骤S111：根据车辆的几何尺寸和栅格分辨率分别对目标场景的两通道图像、图像数据进行分割，得到栅格格式的目标场景的两通道图像、图像数据。
[0123] 步骤S222：对栅格格式的目标场景的两通道图像、图像数据中表征相同位置的栅格进行编码。
[0124] 并且，图5中的估计方法还包括：
[0125] 步骤S203：基于栅格的编码将所述滑动参数的估计结果还原至对应位置处的栅格，生成目标场景的滑动参数预测栅格地图。
[0126] 图11示出了根据本发明实施例的滑动参数地图的生成流程示意图。
[0127] 如图11所示，左侧的彩色图像代表的是表示目标场景的点云数据的二通道图像信息，左侧的灰色图像代表的是表示目标场景的图像数据的三通道图像信息，左侧上方的函数“f1.2.3.4.5(vx,v,αx,v.z,δsteer)”代表的是上文描述轨迹特征中的“六个变量，每个变量可以生成5个统计量特征”。中间的模块“module”代表的是本发明实施例中的滑动参数估计模型。首先可以结合车辆尺寸与栅格分辨率对于左侧的两个图像进行位置分割，得到一系列表征相同位置的点云数据与图像数据。在生成栅格图的过程中可以对于每个位置进行编码，从而保证滑动参数估计模型输出的滑动参数预测结果能够匹配到对应位置。最后将点云数据与图像数据结合车辆的轨迹特征作为输入，使用训练好的滑动参数估计模型进行估计，并将估计结果基于位置编码还原至对应栅格位置，生成当前场景(即本实施例中的目标场景)的滑动参数预测栅格地图。
[0128] 下面对本发明实施例中的滑动参数估计模型的训练过程进行说明。
[0129] 本发明实施例中的滑动参数估计模型的训练方法包括：
[0130] 步骤S100：获取目标场景的点云数据、图像数据、车辆的轨迹数据及其对应的滑动参数标签。
[0131] 目标场景的点云数据、图像数据、车辆的轨迹数据的获取方法已经在步骤S201中予以说明，这里不再赘述。本实施例中滑动参数标签的处理过程如下：
[0132] 滑动参数的生成采用滑移的绝对值表示，如下所示：
[0133]
[0134] 其中，s为滑动参数标签；vq为车辆牵连速度，即车辆真实行驶速度；vs为卷绕速度，为车辆电机的驱动速度。当车辆不存在滑移滑转时，vq＝vs，此时滑动参数为0；当主动轮抱死完全制动时，vs＝0，此时为纯滑移状态，滑动参数为1。
[0135] 步骤S200：将所述目标场景的点云数据、图像数据以及车辆的轨迹数据输入至滑动参数估计模型的初始模型，得到滑动参数的初步估计结果。
[0136] 步骤S300：基于所述滑动参数的初步估计结果和所述滑动参数标签之间的比较，更新所述滑动参数估计模型的初始模型。
[0137] 在本实施例中，滑动参数估计模型的初始模型的训练过程可以使用均方误差(Mean Squared Error，MSE)。均方误差是一种回归任务中的损失函数，它是预测值f(x)与目标值y之间差值平方和的均值，用于衡量滑动参数估计模型的初始模型的预测值与真实值之间的差异，计算公式如下：
[0138]
[0139] 其中，n是样本数量。
[0140] 下面以中间某一时刻对本实施例中的优化的A*搜索算法进行说明：
[0141] 首先在障碍物栅格地图中对当前父节点进行扩展。图12示出了根据本发明实施例的在障碍物栅格地图中进行节点扩展的一个示例。如图12所示，在以当前父节点nparent为圆心的圆上，可以以△θ为角度差均匀的生成N个子节点n0,n0,…,nN，每个子节点继续扩展的时候需要判断子节点是否与障碍物发生碰撞。在图12中，子节点nk继续扩展时，其与障碍物发生了碰撞(图12中以芒星表示)；子节点nk+1继续扩展时，其未与障碍物发生碰撞。在一些实施例中，在当前父节点进行扩展时，可以对扩展半径设置上下界以防止无限扩展，即扩展半径Ro需要满足Ro∈[Rmin,Rmax]，其中，Rmin为扩展半径下界，Rmax为扩展半径上界。在一些实施例中，在当前父节点进行扩展时，若当前父节点与障碍物的距离较远时，选择扩展半径上界Rmax作为子节点与当前父节点的扩展半径；若当前父节点与障碍物的距离较近时，选择当前父节点与障碍物的距离作为子节点与当前父节点的扩展半径；若将子节点与当前父节点的扩展半径选为扩展半径下界Rmin，子节点仍与障碍物发生碰撞，则忽略当前父节点。
[0142] 对于启发函数f(n)＝g(n)+h(n)，子节点的移动代价g(ni)包含当前父节点的移动代价g(nparent)、子节点与当前父节点之间的欧式距离d(nparent,ni)、以及子节点位置处的车辆滑动参数估计值slip(ni)。具体而言，g(ni)的计算公式如下：
[0143] g(ni)＝g(npaent))+wdnd(nparent，ni)+wslipslip(ni)
[0144] 其中，wdn为父子节点距离代价的权重，wslip为滑动参数代价的权重，车辆滑动参数估计值slip(ni)通过索引滑动参数预测栅格地图中的数据得到。需要说明的是，父子节点距离代价的权重wdn和滑动参数代价的权重wslip均可以根据最终规划的路径优劣进行调整。
[0145] 在另一些实施例中，子节点的移动代价g(ni)包含当前父节点的移动代价g(nparent)、子节点与当前父节点之间的欧式距离d(nparent，ni)、子节点位置处的稳定性代价s(ni)、子节点的转向代价t(nparent，ni)、子节点的障碍物距离代价disobs(ni)。具体而言，g(ni)的计算公式如下：
[0146] g(ni)＝g(nparent)+wdnd(nparent，ni)+wss(ni)+wtt(nparent，ni)+wobsdobs(ni)+wslipslip(ni)
[0147] 其中，wdn为父子节点距离代价的权重，ws为子节点位置处的稳定性代价的权重，wt为子节点的转向代价的权重，wobs为子节点的障碍物距离代价的权重，wslip为滑动参数代价的权重。需要说明的是，上述计算公式中的权重均可以根据最终规划的路径优劣进行调整。
[0148] 下面分别对子节点位置处的稳定性代价s(ni)、子节点的转向代价t(nparent，ni)以及子节点的障碍物距离代价disobs(ni)的计算方法进行说明。
[0149] 对于子节点位置处的稳定性代价s(ni)，其可以由子节点处车辆的俯仰角与侧倾角和车辆允许的最大俯仰角与最大侧倾角确定。稳定性代价s(ni)的计算可以是沿着当前父节点指向子节点的方向，得到在子节点处车辆几何尺寸下的点云数据，之后通过平面拟合得到子节点处车辆的俯仰角与侧倾角，最后通过将得到的俯仰角和侧倾角分别与俯仰角阈值和侧倾阈值做比例得到。具体计算公式如下：
[0150]
[0151] 其中，pitchi为第i个子节点处车辆的俯仰角，pitchmax为车辆允许的最大俯仰角，rolli为第i个子节点处车辆的侧倾角，rollmax为车辆允许的最大侧倾角。
[0152] 对于子节点的转向代价t(nparent,ni)，其可以由当前父节点的方向和子节点的扩展方向确定。转向代价t(nparent,ni)由当前父节点的方向与子节点扩展方向得到。当前父节点的方向为当前父节点和当前父节点的父节点之间连线的方向。子节点扩展方向为子节点和当前父节点之间的连线的方向。转向代价t(nparent,ni)为当前父节点的方向和子节点扩展方向之间的夹角。
[0153] 对于子节点的障碍物距离代价disobs(ni)，其可以由子节点与障碍物的最近距离确定。障碍物距离代价disobs(ni)可以是子节点的位置与障碍物的最近距离d(xi,yi)的倒数。具体计算公式如下：
[0154]
[0155] 在另一些实施例中，子节点的启发式函数值h(ni)可以由子节点ni到目标位置ngoal的距离确定。在A*搜索算法中，启发值h(n)主要作用是引导搜索方向朝着目标节点前进，以便更快寻找到合适路径。在启发值的选择过程中，当启发值被过高的预估时，可能会导致搜索向着错误的方向进行；而当启发值过小时，虽然搜索速度较快，但是搜索结果往往是是不完整的。由于车辆在任意方向都可以移动，因此子节点的启发式函数值h(ni)可以由子节点ni到目标位置ngoal的距离确定。这里的距离可以是欧式距离，具体计算公式如下：
[0156]
[0157] 在本实施例的优化的A*搜索算法中，子节点ni的代价总和f(ni)可以表示为：
[0158] f(ni)＝wgg(ni)+whζh(ni)
[0159] 其中，wg表示移动代价权重，wh表示启发值的权重，ζ表示比例因子，比例因子ζ的计算方式如下：
[0160]
[0161] 其中，hmin表示相邻节点间移动的最小代价，Texcept表示路径期望的最大节点数量。
[0162] 综上，本实施例中的优化的双向A*搜索算法两端的启发函数分别为：
[0163] ff(n)＝wgfgf(n)+whfζhf(n)
[0164] fb(n)＝wgbgb(n)+whbζhb(n)
[0165] 其中，ff(n)代表从起点位置到反向节点nk的正向搜索的总代价，gf(n)代表从起点位置到当前正向节点的移动代价，hf(n)代表从当前正向节点到反向节点nk的启发代价；fb(n)代表从目标位置到正向节点nf的反向搜索的总代价，gb(n)代表从目标位置到当前反向节点的移动代价，hb(n)代表从当前反向节点到正向节点nf的启发代价；wgf、whf、wgb、whb表示正向搜索与反向搜索的移动代价与启发代价的权重；反向节点nk代表当前具有最小反向搜索的总代价的子节点，正向节点nf代表当前具有最小正向搜索的总代价的子节点。
[0166] 在一些实施例中，图1中的基于改进的双向A星搜索算法的路径规划方法包括以下步骤：
[0167] 步骤300：通过梯度下降法对所述运动路径进行第一轮次平滑处理，其中，在所述梯度下降法中，用以确定优化方向的能量函数包括路径曲率。
[0168] 经过本实施例中的优化的双A*搜索算法搜索到的运动路径是离散的折线段。为了使车辆能够在环境中流畅地运行，需要降低折线段之间的转角程度与曲率，生成连续的平滑曲线。梯度下降法是通过迭代调整路径上各点位置，以减小路径曲率为目标，实现优化路径的目标。在使用梯度下降法进行迭代更新时需要定义一个能量函数JE作为优化目标，实现确定优化方向与进度的作用。在本实施例中，能量函数JE可以包括迭代更新后所形成的新路径的曲率。在本实施例中，通过最小化路径曲率方式可以为迭代更新过程指明方向，进而降低折线段之间的转角程度与曲率，生成连续的平滑曲线。
[0169] 在一些实施例中，能量函数JE可以包括路径长度代价 及其权重、障碍物距离代价dobs(ni，n′i)及其权重、滑动参数代价sslip(ni，n′i)及其权重。具体而言，能量函数JE表示为：
[0170]
[0171] 下面分别对路径长度代价 障碍物距离代价dobs(ni，n′i)、滑动参数代价sslip(ni，n′i)进行说明。
[0172] 图13示出了根据本发明实施例的采用梯度下降法平滑运动路径的一个示例。首先为了保证起点位置和目标位置的准确性，在本实施例中不对路径上的起点位置和目标位置进行调整。对于其余待调整点ni，如图13所示，可以结合障碍物栅格地图的参数与前后两个节点ni‑1、ni+1构成三角形。之后，可以将待调整点ni向ni‑1、ni+1的连线做投影点n′i。随后，根据ni、n′i连线与障碍物栅格地图的栅格分辨率生成待调整点ni的候选节点集Si＝{ni,ni0,ni1,…,nim,ni′}。然后，从ni0开始逐个评价候选节点：若候选节点与障碍物发生碰撞则跳过该候选节点，否则计算该候选节点的能量函数值JEi,JEi0,JEi1,…,JEim,JEi′。对于路径长度代价 其为候选节点到待调整点ni的累计距离长度。例如，对于候选节点ni0，其路径长度代价为候选节点ni0到待调整点ni的距离d(ni0,ni)；对于候选节点ni1，其路径长度代价为候选节点ni0到待调整点ni的距离d(ni0,ni)加上候选节点ni1到候选节点ni0的距离d(ni1,ni0)，依次类推。对于障碍物距离代价dobs(ni，n′i)，其为候选节点到障碍物的最近距离。对于滑动参数代价sslip(ni，n′i)，其为候选节点位置处的车辆滑动参数估计值。候选节点位置处的车辆滑动参数估计值可以通过索引滑动参数预测栅格地图得到。最后，选择具有最小能量函数值的候选节点替换节点ni，至此完成了待调整点ni的平滑处理。
[0173] 通过以上方法可以对运动路径上除了起点位置和目标位置外的所有节点进行调整，进而完成了对运动路径的单次迭代更新。之后，可以计算本次路径迭代更新后的所有节点的能量函数值之和。最后，判断是否满足终止条件，若满足则停止迭代更新，否则继续下一次迭代更新。在本实施例中，终止条件可以是迭代更新的次数总和超过第一阈值(例如1万次)或者单次迭代更新后计算得到的能量函数值之和的改进差值小于第二阈值。在图13中，黑色折线段代表的是原始的运动路径，橙色折线段代表的是第一次迭代更新后所形成的新运动路径。蓝色小圆代表的是投影点，黄色小圆代表的是第一次迭代更新过程中的候选节点。
[0174] 需要说明的是，在能量函数JE中路径长度代价的权重wd、障碍物距离代价的权重wo、滑动参数代价的权重ws均可以根据平滑处理的最终结果的优劣进行调整。
[0175] 在一些实施例中，图1中的基于改进的双向A星搜索算法的路径规划方法包括以下步骤：
[0176] 步骤400：采用样条插值的方法对经第一轮次平滑处理后的所述运动路径进行第二轮次平滑处理。
[0177] 样条插值是一种数学方法，用于通过一组离散的数据点构建一个连续且光滑的曲线。样条插值的基本思想是在每个数据点之间构造一个高阶多项式，这些多项式在数据点上连续，并且其一阶导数和二阶导数也在数据点上连续，从而保证了插值曲线的光滑性。
[0178] 为了得到均匀连续路径用于走廊构建与局部规划，本实施例可以采用分段三次样条插值的方法做进一步平滑处理，进而生成曲率连续的路径。
[0179] 三次样条插值是将包含N+1个离散点集的区间分成N个拟合小区间，其中每个小区间包含两个点{[n0,n1],[n1,n2],…,[nN‑1,nN]}，然后在每个区间的两个点上使用不同的三2 3
次函数y＝ai+bix+cix +dix 进行拟合，实现曲线经过节点且一阶连续、二阶相等，每个小区间中的插值函数可以表示为Si(xi)，有四个参数需要确定，因此整体一共有4N个待定参数，需要构造等量等式以满足求解要求。
[0180] 由于区间两端需要满足经过节点，则可以列出关于节点位置的2N个等式。为满足路径曲线的高阶连续需求，在相邻区间节点处需要满足一阶连续和二阶相等，可以列出2N‑2个等式。再结合自然边界条件，即起始点与最终点处二阶导数为0，可构建2个等式。根据上述条件共构建4N等式，与待定参数数量相同，可使用矩阵求解得到所有区间的拟合函数，将其连接起来生成一条经过所有节点且连续平滑的参考路径。
[0181] 与现有技术相比，本发明实施例至少可实现如下有益效果之一：
[0182] 1、本发明方案将双向A*搜索算法中的单次正向搜索的启发项计算从终点更新为当前反向搜索的总代价最低节点，反向搜索的启发项计算从起点更新为正向搜索的总代价最低节点，防止搜索过程中距离很近但需要消耗长时间更新的现象，提高了搜索效率。
[0183] 2、针对越野场景运动规划过程忽略滑动参数，导致车辆行驶安全性不足的问题，本发明方案提出使用多层地图的优化双向A*搜索算法。在包含障碍物层、高度层、滑动参数层的多层地图上进行优化双向A*搜索与参考线平滑处理，实现在越野场景中对高滑动参数风险区域的避让，提高了现有规划方法的时效性与安全性，降低了轨迹跟踪难度。
[0184] 以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换都应涵盖在本发明的保护范围之内。
