CN116067392A - 路径处理方法及电子设备 - Google Patents

Abstract

本申请提供一种路径处理方法及电子设备，该方法包括：电子设备通过获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据采样位置点的位姿以及RRT树中各节点的节点信息，从RRT树中确定出采样位置点对应的父节点，根据父节点的节点信息以及障碍物信息，在RRT树中添加父节点对应的目标子节点，在目标子节点的节点信息满足预设条件时，根据目标子节点和根节点，确定目标路径，有效提高了目标路径的质量。该技术方案能够满足牵引半挂车的特殊避障和拉直需求，提高了目标路径的平顺度和质量。

Description

路径处理方法及电子设备

技术领域

本申请涉及导航技术领域，尤其涉及一种路径处理方法及电子设备。

背景技术

随着车辆技术的进步，自动驾驶技术应运而生，极大地推动了交通运输业的发展。其中，由于牵引车半挂拖车可以应用于较多的场景，如机场、港口以及货运站等，因此其自动化程度受到了研究者们的广泛关注。一般来说，牵引车半挂车比单刚体车辆具有更小的转弯半径，且牵引车半挂车系统具有高度非线性和非完整的复杂动力学特性，这给牵引车半挂车的运动规划和控制算法的开发带来了巨大的挑战。

目前，对牵引车半挂车进行运动规划主要通过随机采样方法中的快速搜索随机树算法(Rapidly exploring Random Tree，RRT)实现。RRT适合解决具有微分方程约束限制的运动规划问题，其核心思想是从起始构型增量式地采样构建随机路径，直到该路径体系某一分支成功连通终止构型为止。

然而，现有的随机采样方法无法精准处理复杂约束条件，得到的路径质量较差，无法满足牵引车半挂车的特殊避障和拉直需求。

发明内容

本申请提供一种路径处理方法及电子设备，以解决现有的随机采样方法无法精准处理复杂约束条件，得到的路径质量较差，无法满足牵引车半挂车的特殊避障和拉直需求的问题。

第一方面，本申请实施例提供一种路径处理方法，包括：

获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，所述障碍物信息用于表示从所述起始位置点到所述终止位置点的多个第一路径两侧的障碍物；

从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据所述采样位置点的位姿以及RRT树中各节点的节点信息，从所述RRT树中确定出所述采样位置点对应的父节点，所述RRT树包括根节点以及所述根节点下的用于表示位置的多级节点，每个节点的节点信息用于表示所述车辆在对应位置的运行状态，所述根节点的节点信息与所述起始位置点的位姿有关；

根据所述父节点的节点信息以及所述障碍物信息，在所述RRT树中添加所述父节点对应的目标子节点；

若所述目标子节点的节点信息满足预设条件，则根据所述目标子节点和所述根节点，确定目标路径，所述目标路径为从所述起始位置点到所述终止位置点的路径，所述预设条件与所述终止位置点的位姿有关。

在第一方面的一种可能设计中，所述从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据所述采样位置点的位姿以及RRT树中各节点的节点信息，从所述RRT树中确定出所述采样位置点对应的父节点，包括：

从任一第一路径所处的道路中进行采样，获取所述采样位置点的位姿；

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及处于所述节点至所述根节点所在第二路径中的所有节点的节点信息，计算所述车辆从所述节点对应的位置点行驶至所述采样位置点的第一可能性程度；

将所述第一可能性程度最大的节点确定为所述采样位置点对应的父节点。

可选的，所述车辆为牵引车半挂拖车，所述节点信息包括坐标，牵引车姿态角，挂车姿态角，牵引车前轮的转向角和转向角变化率；

相应的，所述在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及处于所述节点至所述根节点所在第二路径中的所有节点的节点信息，计算所述车辆从所述节点对应的位置点行驶至所述采样位置点的第一可能性程度，包括：

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及所述节点的坐标，牵引车姿态角以及挂车姿态角，确定所述采样位置点与所述节点对应的位置点之间的第三路径长度以及第三路径不平滑程度；

针对RRT树中每个节点，根据所述第二路径中的所有节点的牵引车前轮的转向角和转向角变化率，计算所述第二路径中的所有节点的牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和；

针对RRT树中每个节点，确定所述节点的离散转向角变化率集合中已使用的转向角变化率的数量，所述已使用的离散转向角变化率已用于确定所述节点是否可以添加所述已使用的离散转向角变化率对应的下一级子节点；

针对RRT树中每个节点，根据所述第三路径长度，所述第三路径不平滑程度，所述已使用的转向角变化率的数量，牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和，确定所述第一可能性程度。

可选的，所述根据所述父节点的节点信息以及所述障碍物信息，在所述RRT树中添加所述父节点对应的目标子节点，包括：

根据所述父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，确定所述RRT树中所述父节点的可添加子节点；

针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及所述障碍物信息，计算所述车辆从所述父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度；

将所述第二可能性程度最大的可添加子节点确定为所述父节点对应的目标子节点；

在所述RRT树中添加所述父节点对应的所述目标子节点。

可选的，所述针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及所述障碍物信息，计算所述车辆从所述父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度，包括：

针对每一可添加子节点，根据所述可添加子节点的坐标，牵引车的前轮转向角以及转向角变化率，以及所述父节点的坐标，牵引车的前轮转向角以及转向角变化率，确定所述可添加子节点对应位置点与所述父节点对应位置点之间的第四路径长度以及第四路径不平滑程度；

针对每一可添加子节点，根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，确定所述可添加子节点对应的位置点与道路边界的碰撞情况；

针对每一可添加子节点，确定所述可添加子节点与所述父节点之间的转向角变化方向，与所述父节点与所述父节点的上一级节点的转向角变化方向的一致性；

针对每一可添加子节点，根据所述第四路径长度，所述第四路径不平滑程度，所述碰撞情况，所述一致性，计算所述第二可能性程度。

可选的，所述若所述目标子节点的节点信息满足预设条件，则根据所述目标子节点和所述根节点，确定目标路径，所述目标路径为从所述起始位置点到所述终止位置点的路径，包括：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点相对于所述终止位置点的误差；

若所述误差小于预设误差，则将所述目标子节点至根节点的路径确定为第五路径；

根据每个第五路径的误差，以及所述第五路径中的所有节点的牵引车前轮的转向角变化率绝对值累加和，从所有第五路径中确定出所述目标路径；

其中，所述预设条件为所述目标子节点与所述终止位置点的误差小于预设误差。

可选的，所述方法还包括：

通过三次样条插值生成所述目标路径的分段多项式；

对所述分段多项式进行二次规划，获取所述目标路径的优化后节点；

根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，判断每个优化后节点对应的位置点与道路边界是否发生碰撞；

若否，则根据优化后节点对所述目标路径进行更新，生成更新后的目标路径；

若是，则根据优化后节点构成的路径与所述道路边界发生碰撞的碰撞位置点，通过所述碰撞位置点的坐标从所有优化后节点中确定出需要再次优化的节点；

调小所述需要再次优化的节点的曲率权重和曲率变化率权重。

可选的，所述从任一第一路径所处的道路中进行采样，获取所述采样位置点的位姿，包括：

从预设概率范围内进行随机正态采样，得到将所述终止位置点确定为所述采样位置点的概率值；

若所述概率值大于预设概率值，则将所述终止位置点确定为所述采样位置点，所述预设概率值处于所述预设概率范围内；

否则，则将从任一第一路径所在道路的道路中心线上进行采样，获取初始采样位置点；

根据所述初始采样位置点以及预设距离，确定采样范围；

从所述采样范围内进行采样，获取所述采样位置点，所述采样位置点的牵引车姿态角和挂车姿态角与所述初始采样位置点相同。

可选的，所述根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点相对于所述终止位置点的误差，包括：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点与所述终止位置点的横坐标差值小于第一预设距离，所述目标子节点与所述终止位置点的纵坐标差值小于第二预设距离，所述目标子节点与所述终止位置点的牵引车姿态角差值小于第一姿态角差值，所述目标子节点与所述终止位置点的挂车姿态角差值小于第二姿态角差值；

其中，所述误差包括所述横坐标差值，所述纵坐标差值，所述牵引车姿态角差值以及所述挂车姿态角差值。

第二方面，本申请实施例提供一种路径处理装置，包括：

获取模块，用于获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，所述障碍物信息用于表示从所述起始位置点到所述终止位置点的多个第一路径两侧的障碍物；

确定模块，用于从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据所述采样位置点的位姿以及RRT树中各节点的节点信息，从所述RRT树中确定出所述采样位置点对应的父节点，所述RRT树包括根节点以及所述根节点下的用于表示位置的多级节点，每个节点的节点信息用于表示所述车辆在对应位置的运行状态，所述根节点的节点信息与所述起始位置点的位姿有关；

添加模块，用于根据所述父节点的节点信息以及所述障碍物信息，在所述RRT树中添加所述父节点对应的目标子节点；

所述确定模块，还用于若所述目标子节点的节点信息满足预设条件，则根据所述目标子节点和所述根节点，确定目标路径，所述目标路径为从所述起始位置点到所述终止位置点的路径，所述预设条件与所述终止位置点的位姿有关。

在第二方面的一种可能设计中，所述确定模块，具体用于：

从任一第一路径所处的道路中进行采样，获取所述采样位置点的位姿；

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及处于所述节点至所述根节点所在第二路径中的所有节点的节点信息，计算所述车辆从所述节点对应的位置点行驶至所述采样位置点的第一可能性程度；

将所述第一可能性程度最大的节点确定为所述采样位置点对应的父节点。

可选的，所述车辆为牵引车半挂拖车，所述节点信息包括坐标，牵引车姿态角，挂车姿态角，牵引车前轮的转向角和转向角变化率；

相应的，所述确定模块，具体用于：

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及所述节点的坐标，牵引车姿态角以及挂车姿态角，确定所述采样位置点与所述节点对应的位置点之间的第三路径长度以及第三路径不平滑程度；

针对RRT树中每个节点，根据所述第二路径中的所有节点的牵引车前轮的转向角和转向角变化率，计算所述第二路径中的所有节点的牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和；

针对RRT树中每个节点，确定所述节点的离散转向角变化率集合中已使用的转向角变化率的数量，所述已使用的离散转向角变化率已用于确定所述节点是否可以添加所述已使用的离散转向角变化率对应的下一级子节点；

针对RRT树中每个节点，根据所述第三路径长度，所述第三路径不平滑程度，所述已使用的转向角变化率的数量，牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和，确定所述第一可能性程度。

可选的，所述添加模块，具体用于：

根据所述父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，确定所述RRT树中所述父节点的可添加子节点；

针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及所述障碍物信息，计算所述车辆从所述父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度；

将所述第二可能性程度最大的可添加子节点确定为所述父节点对应的目标子节点；

在所述RRT树中添加所述父节点对应的所述目标子节点。

可选的，所述添加模块，具体用于：

针对每一可添加子节点，根据所述可添加子节点的坐标，牵引车的前轮转向角以及转向角变化率，以及所述父节点的坐标，牵引车的前轮转向角以及转向角变化率，确定所述可添加子节点对应位置点与所述父节点对应位置点之间的第四路径长度以及第四路径不平滑程度；

针对每一可添加子节点，根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，确定所述可添加子节点对应的位置点与道路边界的碰撞情况；

针对每一可添加子节点，确定所述可添加子节点与所述父节点之间的转向角变化方向，与所述父节点与所述父节点的上一级节点的转向角变化方向的一致性；

针对每一可添加子节点，根据所述第四路径长度，所述第四路径不平滑程度，所述碰撞情况，所述一致性，计算所述第二可能性程度。

可选的，所述确定模块，具体用于：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点相对于所述终止位置点的误差；

若所述误差小于预设误差，则将所述目标子节点至根节点的路径确定为第五路径；

根据每个第五路径的误差，以及所述第五路径中的所有节点的牵引车前轮的转向角变化率绝对值累加和，从所有第五路径中确定出所述目标路径；

其中，所述预设条件为所述目标子节点与所述终止位置点的误差小于预设误差。

可选的，所述装置还包括：

生成模块，用于通过三次样条插值生成所述目标路径的分段多项式；

所述获取模块，还用于对所述分段多项式进行二次规划，获取所述目标路径的优化后节点；

判断模块，用于根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，判断每个优化后节点对应的位置点与道路边界是否发生碰撞；

所述生成模块，还用于若否，则根据优化后节点对所述目标路径进行更新，生成更新后的目标路径；

所述确定模块，还用于若是，则根据优化后节点构成的路径与所述道路边界发生碰撞的碰撞位置点，通过所述碰撞位置点的坐标从所有优化后节点中确定出需要再次优化的节点；

调小模块，用于调小所述需要再次优化的节点的曲率权重和曲率变化率权重。

可选的，所述确定模块，具体用于：

从预设概率范围内进行随机正态采样，得到将所述终止位置点确定为所述采样位置点的概率值；

若所述概率值大于预设概率值，则将所述终止位置点确定为所述采样位置点，所述预设概率值处于所述预设概率范围内；

否则，则将从任一第一路径所在道路的道路中心线上进行采样，获取初始采样位置点；

根据所述初始采样位置点以及预设距离，确定采样范围；

从所述采样范围内进行采样，获取所述采样位置点，所述采样位置点的牵引车姿态角和挂车姿态角与所述初始采样位置点相同。

可选的，所述确定模块，具体用于：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点与所述终止位置点的横坐标差值小于第一预设距离，所述目标子节点与所述终止位置点的纵坐标差值小于第二预设距离，所述目标子节点与所述终止位置点的牵引车姿态角差值小于第一姿态角差值，所述目标子节点与所述终止位置点的挂车姿态角差值小于第二姿态角差值；

其中，所述误差包括所述横坐标差值，所述纵坐标差值，所述牵引车姿态角差值以及所述挂车姿态角差值。

第三方面，本申请实施例提供一种电子设备，包括：处理器、存储器及存储在所述存储器上并可在处理器上运行的计算机程序指令，所述处理器执行所述计算机程序指令时用于实现第一方面以及各可能设计提供的方法。

第四方面，本申请实施例可提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，所述计算机执行指令被处理器执行时用于实现第一方面以及各可能设计提供的方法。

第五方面，本申请实施例提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时用于实现第一方面以及各可能设计提供的方法。

本申请实施例提供的路径处理方法及电子设备，在该方法中，电子设备通过获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据采样位置点的位姿以及RRT树中各节点的节点信息，从RRT树中确定出采样位置点对应的父节点，根据父节点的节点信息以及障碍物信息，在RRT树中添加父节点对应的目标子节点，在目标子节点的节点信息满足预设条件时，根据目标子节点和根节点，确定目标路径，有效提高了目标路径的质量。在本技术方案中，在搜索过程中迭代计算牵引车和半挂车的姿态，结合各节点的位置以及车辆在该位置的运行状态两方面因素对RRT树进行拓展，能够满足牵引半挂车的特殊避障和拉直需求，根据拓展后的RRT树确定的目标路径满足牵引车半挂车运动学约束，提高了目标路径的平顺度，保证了车辆有良好的跟踪效果，避免牵引车和半挂车与已知的静态障碍发生碰撞。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。

图1为本申请实施例提供的路径处理方法实施例一的流程示意图；

图2为本申请实施例提供的路径处理方法实施例二的流程示意图；

图3为本申请实施例提供的四个圆弧示意图；

图4为本申请实施例提供的路径处理方法实施例三的流程示意图；

图5为本申请实施例提供的路径处理方法实施例四的流程示意图；

图6为本申请实施例提供的路径处理方法实施例五的流程示意图；

图7为本申请实施例提供的路径对比示意图；

图8为本申请实施例提供的曲率对比示意图；

图9为本申请实施例提供的曲率变化率对比示意图；

图10为本申请实施例提供的路径处理装置的结构示意图；

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

通过上述附图，已示出本申请明确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本申请构思的范围，而是通过参考特定实施例为本领域技术人员说明本申请的概念。

具体实施方式

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

在介绍本申请的实施例之前，首先对本申请实施例的应用背景进行解释：

自动驾驶技术能够提高工作效率和安全性，极大的改变了交通运输业的工作模式，其中广泛应用于机场、港口、货运站的牵引车半挂拖车的自动化受到了研究者们的关注。牵引车半挂拖车具有两个大尺寸的车体，分别为牵引车和半挂拖车，牵引车是一种类似汽车的车辆，带有牵引发动机，而半挂拖车是一种用于运送货物的铰接车辆。一般来说，牵引车半挂车比单刚体车辆具有更小的转弯半径，且牵引车半挂车系统具有高度非线性和非完整的复杂动力学特性，这给牵引车半挂车的运动规划和控制算法的开发带来了巨大的挑战。

目前，常用的运动规划方法有图搜索方法、随机采样方法以及基于优化的方法。其中，随机采样方法适合求解高维度构型空间中的运动规划问题，并具有概率完备性：只要运动规划问题的解存在，则随机采样方法在充分长的时间内找到它的概率为100％，因此在各行业的运动规划工作中得到了广泛的应用。热门的随机采样方法包括概率路标算法(Probailistic Roadmap Method，PRM)以及RRT，RRT适合解决具有微分方程约束限制的运动规划问题，其核心思想是从起始构型增量式地采样构建随机路径，直到该路径体系某一分支成功连通终止构型为止。

然而，尽管随机采样方法具有概率完备性，但它们无法精准处理复杂约束条件。特别地随机采样方法生成运动状态的过程具有盲目性，无法有针对性地发现精确满足约束条件的解，内在的随机机制导致随机采样方法无法直接处理含有复杂约束的运动规划任务，并且通过采样获得的粗糙路径是曲率不连续的，不满足控制环节的要求。通过随机采样方法得到的路径质量较差，路径存在有棱角以及不够光滑的问题，无法满足牵引车半挂车的特殊避障和拉直需求。

基于上述技术问题，本申请的技术构思为：在现有技术中，更新RRT树的节点主要是通过在RRT树中确定与采样位置点距离最近的节点，将该节点作为父节点，并将采样位置点添加为该父节点的子节点。也就是说，现有技术在更新节点时只考虑了节点间的距离，没有考虑多个节点之间的流畅性，导致确定出的目标路径存在包含棱角以及不够光滑等问题，无法满足牵引车半挂车的特殊避障和拉直需求。本申请提出的路径处理方法，重新定义了RRT树以及更新RRT树的节点的过程，为RRT树中的节点增加了用于表示牵引车半挂车在对应位置的运行状态的节点信息，在更新RRT树的节点的过程中，结合牵引车半挂车在采样位置点的位姿以及RRT树中各节点的节点信息，从RRT树中确定出采样位置点对应的父节点，并根据父节点的节点信息以及障碍物信息，从该父节点对应的多个可添加子节点中确定出目标子节点，并添加至RRT树中的相应位置。这样，在该目标子节点的节点信息满足与终止位置点的位姿相关的预设条件时，将目标子节点至根节点的路径确定为目标路径，就能够有效提高确定的目标路径的质量，从而满足牵引车半挂车的特殊避障和拉直需求。

下面，通过具体实施例对本申请的技术方案进行详细说明。

需要说明的是，下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。

图1为本申请实施例提供的路径处理方法实施例一的流程示意图。如图1所示，该路径处理方法应用于电子设备，该电子设备可以为服务器，还可以为车辆本身。该路径处理方法可以包括如下步骤：

S101、获取障碍物信息以及车辆在起始位置点和终止位置点的位姿。

在本步骤中，由于路径规划需要根据起始位置点(也就是出发地)、终止位置点(也就是目的地)以及道路两侧的障碍物情况，规划出一条能够躲避障碍物的路径，从而控制车辆在该路径上行驶。因此，需要先获取障碍物信息以及起始位置点的坐标和终止位置点的坐标。

进一步的，由于车辆的行驶方向需要与道路方向一致。因此，在获取起始位置点和终止位置点后，还需要确定车辆方向，从而保证后续规划的路径满足交通规则。其中，位置点的位姿包括该位置点的坐标以及车辆处于该位置点时的车辆方向。

其中，障碍物信息用于表示从起始位置点到终止位置点的多个第一路径两侧的障碍物。

一方面，可以获取用户通过车辆的触控显示屏输入的起始位置点和终止位置点的坐标，根据高精地图确定最靠近位置点的道路方向，从而确定车辆在该位置点的行驶方向，获取起始位置点和终止位置点的位姿。以电子设备为车辆为例进行举例说明，车辆可以通过触控显示屏向用户展示图形用户界面，该用户图形界面用于显示高精地图，用户可以根据自己需求，输入起始位置点和终止位置点，车辆则响应于用户的操作，获取用户输入的起始位置点和终止位置点的坐标。进一步的，车辆根据最靠近位置点的道路方向，确定车辆在该位置点的行驶方向。也就是说，位置点的位姿包括该位置点的坐标以及车辆在该位置点的行驶方向。

另一方面，针对于障碍物信息来说，可以通过高精地图获取从起始位置点到终止位置点的所有第一路径，针对于每一条第一路径，根据高精地图提取该第一路径沿途预设范围内的障碍物外轮廓，从而获取障碍物信息。示例性的，可以确定每个第一路径所要经过的道路和路口的组合，提取每个道路和路口组合的障碍物外轮廓，生成障碍物信息。

可选的，可以通过迪杰斯特拉算法(英文：Dijkstra)计算高精地图中的从起始位置点到终止位置点的所有第一路径。应理解，还可以通过其他现有的算法计算得到高精地图中的所有第一路径，具体使用哪个算法进行计算可以根据实际情况进行确定，本申请实施例对此不进行具体限制。

可选的，在车辆为牵引车半挂拖车时，位置点的位姿包括坐标，以及车辆在该位置点时的牵引车姿态角和挂车姿态角。

S102、从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据采样位置点的位姿以及RRT树中各节点的节点信息，从RRT树中确定出采样位置点对应的父节点。

在本步骤中，在获取起始位置点和终止位置点的位姿后，需要根据起始位置点和终止位置点的位姿构建并拓展RRT树，以使通过拓展后的RRT树获取平滑的适合车辆行驶的目标路径。在拓展RRT树的过程中，需要从任一第一路径所处的道路中进行采样，采样得到的采样位置点用于在RRT树中添加新节点，若无法通过添加了新节点的RRT树获取目标路径，且采样次数未达到预设次数，则继续从任一第一路径所处的道路中进行采样，并重复上述拓展过程，否则，则停止采样，通过添加了新节点的RRT树确定目标路径或提示用户无法对起始位置点和终止位置点进行路径规划。其中，每次采样的第一路径可以是同一个，也可以不同。

拓展RRT树主要包括两个过程：过程1、根据采样位置点的位姿，从RRT树中确定出采样位置点对应的父节点。过程2、根据该父节点，确定该父节点对应的可以添加到RRT树中的目标子节点，S102对应上述过程1。

其中，RRT树包括根节点以及该根节点下的用于表示位置的多级节点，每个节点的节点信息用于表示车辆在对应位置的运行状态，根节点的节点信息与起始位置点的位姿有关。

在一种可能的实现方式中，针对RRT树中每个节点，可以根据采样位置点的位姿以及该节点至根节点的第二路径中的所有节点的节点信息，计算车辆从节点对应的位置点行驶至采样位置点的第一可能性程度，从而将第一可能性程度最大的节点确定为采样位置点对应的父节点。

应理解，本步骤的具体实现方式和原理可以参照图2所示实施例，此处不再赘述。

S103、根据父节点的节点信息以及障碍物信息，在RRT树中添加父节点对应的目标子节点。

在本步骤中，在确定出采样位置点对应的父节点后，可以根据该父节点，确定该父节点对应的可以添加到RRT树中的目标子节点，从而完成一次拓展RRT树的过程。

在一种可能的实现方式中，在车辆为牵引车半挂车时，可以根据父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，递推得到RRT树中父节点的可添加子节点。

S104、若目标子节点的节点信息满足预设条件，则根据目标子节点和根节点，确定目标路径，目标路径为从起始位置点到终止位置点的路径。

在本步骤中，在每一次对RRT树进行拓展后，需要根据预设条件判断是否能够从拓展后的RRT树中确定出目标路径。若是，则停止拓展，从拓展后的RRT树中获取目标路径，若否，则继续拓展或停止拓展。

其中，预设条件与终止位置点的位姿有关。

在一种可能的实现方式中，S104可以通过步骤(1)至步骤(3)实现：

步骤(1)、根据目标子节点的节点信息与终止位置点的位姿，计算目标子节点相对于终止位置点的误差。

可选的，在车辆为牵引车半挂拖车时，可以根据目标子节点的节点信息与终止位置点的位姿，计算目标子节点与终止位置点的横坐标差值小于第一预设距离，目标子节点与终止位置点的纵坐标差值小于第二预设距离，目标子节点与终止位置点的牵引车姿态角差值小于第一姿态角差值，目标子节点与终止位置点的挂车姿态角差值小于第二姿态角差值。

也就是说，上述误差包括横坐标差值，纵坐标差值，牵引车姿态角差值以及挂车姿态角差值。

在步骤(1)中，在车辆为牵引车半挂拖车时，对终止位置点的位姿与目标子节点的节点信息中的位姿进行对比，通过计算两者之间的误差确定目标子节点对应的位置与终止位置点的接近程度，以及车辆处于目标子节点对应的位置时与处于终止位置点时车辆方向的接近程度，能够更准确的判断RRT树是否需要再次拓展。

步骤(2)、若误差小于预设误差，则将目标子节点至根节点的路径确定为第五路径。

步骤(3)、根据每个第五路径的误差，以及第五路径中的所有节点的牵引车前轮的转向角变化率绝对值累加和，从所有第五路径中确定出目标路径。

应理解，步骤(3)可以在第五路径数量达到预设路径数量，或在拓展RRT树的过程中的采样次数是否超过预设次数时执行。

示例性的，可以预先对误差以及转向角变化率绝对值累加和赋予对应的权重，针对每个第五路径，对该第五路径的误差以及转向角变化率绝对值累加和进行加权求和，得到处理后的值，将最小的处理后的值对应的第五路径确定为目标路径。

由于处理后的值可以体现出目标子节点相对于终止位置点的误差以及该第五路径的光滑程度，处理后的值越小，目标子节点相对于终止位置点的误差越小，该第五路径越光滑，因此将最小的处理后的值对应的第五路径确定为目标路径能够保证目标路径的准确性和光滑性。

在另一种可能的实现方式中，可以根据目标子节点的节点信息与终止位置点的位姿，计算目标子节点相对于终止位置点的误差。若误差小于预设误差，则将目标子节点至根节点的路径确定为目标路径。

可选的，在上述两种可能的实现方式中，在确定出目标路径之后，还可以添加终止位置点对应的终止节点，并将该节点与目标路径中目标子节点进行连接，生成处理后的目标路径。

可选的，在目标子节点的节点信息不满足预设条件时，则判断在拓展RRT树的过程中的采样次数是否超过预设次数，若超过，则停止拓展，反之，则继续拓展，也就是重复执行S102至S104，直到确定出目标路径或采样次数超过预设次数。

本申请实施例提供的路径处理方法，电子设备通过获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据采样位置点的位姿以及RRT树中各节点的节点信息，从RRT树中确定出采样位置点对应的父节点，根据父节点的节点信息以及障碍物信息，在RRT树中添加父节点对应的目标子节点，在目标子节点的节点信息满足预设条件时，根据目标子节点和根节点，确定目标路径，有效提高了目标路径的质量。在本技术方案中，在搜索过程中迭代计算牵引车和半挂车的姿态，结合各节点的位置以及车辆在该位置的运行状态两方面因素对RRT树进行拓展，能够满足牵引半挂车的特殊避障和拉直需求，根据拓展后的RRT树确定的目标路径满足牵引车半挂车运动学约束，提高了目标路径的平顺度，保证了车辆有良好的跟踪效果，避免牵引车和半挂车与已知的静态障碍发生碰撞。

基于图1所示实施例，接下来对S102进行具体的解释说明。图2为本申请实施例提供的路径处理方法实施例二的流程示意图。如图2所示，S102可以通过以下步骤实现：

S201、从任一第一路径所处的道路中进行采样，获取采样位置点的位姿。

在本步骤中，在拓展RRT树时，需要获取采样位置点，以使根据采样位置点在RRT树中添加新节点。由于在确定了起始位置点和终止位置点后，两个位置点之间的第一路径也就被确定出来了，后续确定出的目标路径与其中一个第一路径所处的道路相同。也就是说明，新添加到RRT树中的节点对应的位置需要处于任一第一路径所处的道路中，那么就可以直接从任一第一路径所处的道路中进行采样，来获取用于在RRT树中添加新节点的采样位置点。

在一种可能的实现方式中，S201可以通过下述步骤(4)至步骤(8)实现：

步骤(4)、从预设概率范围内进行随机正态采样，得到将终止位置点确定为采样位置点的概率值。

示例性的，预设概率范围可以为(0,1)。应理解，在实际应用中，预设概率范围还可以为其他预先设定的概率范围，例如(0,0.5)、(0.5,1)、(0.25,0.75)等，本申请实施例对此不进行具体限制。

示例性的，以预设概率范围为(0,1)为例进行具体说明，可以通过随机正态采样k＝random(0,1)获取上述概率值。其中，k为将终止位置点确定为采样位置点的概率值。

步骤(5)、若概率值大于预设概率值，则将终止位置点确定为采样位置点。

可以的，该预设概率值处于预设概率范围内，具体数值可以根据实际情况进行预先设定，本申请实施例对此不进行具体限制。

执行完步骤(5)后停止执行后续步骤(6)至步骤(8)。

步骤(6)、否则，则将从任一第一路径所在道路的道路中心线上进行采样，获取初始采样位置点。

可选的，上述采样方式可以为随机正态采样。

步骤(7)、根据初始采样位置点以及预设距离，确定采样范围。

可选的，可以以初始采样位置点为中心，预设距离为边长，确定一个正方形的采样范围。

步骤(8)、从采样范围内进行采样，获取采样位置点。

其中，采样位置点的牵引车姿态角和挂车姿态角与初始采样位置点相同。

可选的，可以在上述采样范围内随机正态采样一个位置点，将该位置点确定为采样位置点，将初始采样位置点的方向赋予该采样位置点，从而获取该采样位置点的位姿。

在上述实现方式中，通过从预设概率范围内进行随机正态采样的方式，使得有一定概率将终止位置点直接确定为采样位置点，保证RRT树在向周围拓展的同时向终止位置点方向生长。同时，在采样时，由于不同车辆的作业要求和车辆结构不同，有些车辆存在不能一直沿着道路中心线行驶的情况，因此还可以在距离初始采样位置点一定范围内进行采样，并将初始采样位置点的方向赋给采样得到的采样位置点，将两者的状态进行组合作为采样位置点的位姿，更贴近实际车辆行驶情况。

S202、在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据采样位置点的位姿以及处于节点至根节点所在第二路径中的所有节点的节点信息，计算车辆从节点对应的位置点行驶至采样位置点的第一可能性程度。

在本步骤中，在确定出采样位置点的位姿后，需要从RRT树中确定出该采样位置点对应的父节点，以使根据该父节点对RRT树进行拓展，添加该父节点对应的目标子节点。

在一种可能的实现方式中，车辆为牵引车半挂拖车，节点信息包括坐标，牵引车姿态角，挂车姿态角，牵引车前轮的转向角和转向角变化率。

相应的，在该可能的实现方式中，S202可以通过以下步骤(9)至步骤(12)实现：

步骤(9)、在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据采样位置点的位姿以及节点的坐标，牵引车姿态角以及挂车姿态角，确定采样位置点与节点对应的位置点之间的第三路径长度以及第三路径不平滑程度。

可选的，可以根据采样位置点的位姿以及节点的坐标，牵引车姿态角以及挂车姿态角，确定节点的坐标与采样位置点的左边之间的四个圆弧，由确定的四个圆弧确定采样位置点与节点对应的位置点之间的第三路径长度以及第三路径不平滑程度。

图3为本申请实施例提供的四个圆弧示意图。如图3所示，四个圆弧的半径分别为R1、R2、R2、R1。节点与采样位置点之间的灰色的弧形线段为两者之间的第三路径长度，第三路径不平滑程度与dy呈正相关。

第三路径长度以及第三路径不平滑程度也可以表示车辆从节点的对应位置点到采样位置点的可到达程度。

在步骤(9)中，在计算采样位置点与节点对应的位置点之间的路径长度时，不仅仅根据坐标计算两者之间的直线距离，而是结合车辆方向计算车辆在两者之间行驶时的实际路径长度，使得计算得到的值更准确。同时，由于路径越不平滑驾驶时的流畅度就越差，因此加入不平滑程度因素，使其参与后续确定该节点是否为采样位置点对应的父节点的相关处理，进一步提高后续确定出的目标路径质量。

步骤(10)、针对RRT树中每个节点，根据第二路径中的所有节点的牵引车前轮的转向角和转向角变化率，计算第二路径中的所有节点的牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和。

步骤(11)、针对RRT树中每个节点，确定节点的离散转向角变化率集合中已使用的转向角变化率的数量。

其中，已使用的离散转向角变化率已用于确定节点是否可以添加已使用的离散转向角变化率对应的下一级子节点。

在实际应用中，离散转向角变化率集合可以通过enum_dsteer进行表示，enum_dsteer包含多个离散的转向角变化率，例如dsteer₁、dsteer₂,……,dsteer_i。可以记录enum_dsteer中已使用的离散转向角变化率的下标，从而确定节点的离散转向角变化率集合中已使用的转向角变化率的数量。

步骤(12)、针对RRT树中每个节点，根据第三路径长度，第三路径不平滑程度，已使用的转向角变化率的数量，牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和，确定第一可能性程度。

可选的，可以对第三路径长度，第三路径不平滑程度，已使用的转向角变化率的数量，牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和进行加权求和处理，从而确定第一可能性程度。

示例性的，可以通过下述公式确定第一可能性程度：

Cost1＝Dist+Valid.size+StrreCost+dSteerCost

其中，Cost1为第一可能性程度，Dist为第三路径长度和第三路径不平滑程度，Valid.size为已使用的转向角变化率的数量，SteerCost为牵引车前轮的转向角绝对值累加和，dSteerCost为牵引车前轮的转向角变化率绝对值累加和。

S203、将第一可能性程度最大的节点确定为采样位置点对应的父节点。

在本步骤中，在计算出车辆从各节点对应的位置点行驶至采样位置点的第一可能性程度后，可以将其中第一可能性程度最大的节点确定为采样位置点对应的父节点，以便后续确定该父节点对应的可以添加到RRT树中的目标子节点，达到在随机位置拓展RRT树的目的。

上述实施例提供的方法，在确定RRT树中采样位置点对应父节点时，考虑了车辆方向以及路径各节点的转向角和转向角变化率对车辆实际运行路径的影响，从而确定出第一可能性程度最大的节点作为采样位置点对应的父节点，提高了确定的准确性，为后续准确获取目标路径奠定了基础。

基于图2所示实施例，接下来对S103进行具体的解释说明。图4为本申请实施例提供的路径处理方法实施例三的流程示意图。如图4所示，S103可以通过以下步骤实现：

S401、根据父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，确定RRT树中父节点的可添加子节点。

在本步骤中，在确定父节点后，还需要确定该父节点的可添加子节点，以使后续从这些可添加子节点中确定出目标子节点，并将该目标子节点添加至RRT树中。

可选的，在车辆为牵引车半挂车时，可以根据父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，递推得到RRT树中父节点的可添加子节点。

示例性的，该节点信息包括坐标，牵引车姿态角，挂车姿态角，牵引车前轮的转向角和转向角变化率时，递推过程可以根据下述公式实现：

其中，左侧为父节点，右侧为该父节点的可添加子节点，x,y为参考线坐标系下以车辆后轴为中心点的坐标，yaw1为参考线坐标系下牵引车姿态角，yaw2为参考线坐标系下挂车姿态角，steer牵引车前轮转向角，dsteer为牵引车前轮转向角变化率，c_yaw1为牵引车动力学修正系数，c_yaw2为挂车动力学系数。

S402、针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及障碍物信息，计算车辆从父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度。

在本步骤中，在401确定出多个可添加子节点后，还需要从这些可添加子节点中确定出需要添加至RRT树中的目标子节点。其中，可以计算车辆从父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度，根据该第二可能性程度确定目标子节点。

在一种可能的实现方式中，S402可以通过步骤(13)至步骤(16)实现：

步骤(13)、针对每一可添加子节点，根据可添加子节点的坐标，牵引车的前轮转向角以及转向角变化率，以及父节点的坐标，牵引车的前轮转向角以及转向角变化率，确定可添加子节点对应位置点与父节点对应位置点之间的第四路径长度以及第四路径不平滑程度。

应理解，步骤(13)的具体实现方式和原理可以参照步骤(9)中的相关内容，此处不再赘述。

步骤(14)、针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及障碍物信息，确定可添加子节点对应的位置点与道路边界的碰撞情况。

可选的，针对每一可添加子节点，可以根据该可添加子节点以及父节点，判断牵引车前轮的转向角是否超过允许的牵引车前轮最大转向角，以及判断是否存在障碍物。在判断得到超过允许的牵引车前轮最大转向角和/或存在障碍物时，确定碰撞情况为有碰撞，并为其赋予一个预设的极大值。

步骤(15)、针对每一可添加子节点，确定可添加子节点与父节点之间的转向角变化方向，与父节点与父节点的上一级节点的转向角变化方向的一致性。

可选的，在不一致时，说明车辆在从父节点对应的位置点行驶至可添加子节点对应的位置点时的转向角变化方向与上一步不一致，则为其赋予一个预设的惩罚量(也就是预设值)，使得后续确定的目标子节点能够使车辆在沿着目标路径行驶时能连续的转向。

步骤(16)、针对每一可添加子节点，根据第四路径长度，第四路径不平滑程度，碰撞情况，一致性，计算第二可能性程度。

可选的，可以对第四路径长度，第四路径不平滑程度，碰撞情况，一致性进行加权求和，求和后的值即为第二可能性程度。

示例性的，可以通过下述公式确定第二可能性程度：

Cost2＝Dist+CollisionCost+SmoothCost

其中，Cost2为第二可能性程度，CostCollision为碰撞情况，SmoothCost为一致性。

可选的，在计算完每个可添加子节点的第二可能性后，还可以将该可添加子节点相对于父节点的转向角变化率在离散转向角变化率集合标注为已使用。

S403、将第二可能性程度最大的可添加子节点确定为父节点对应的目标子节点。

在本步骤中，在计算出车辆从父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度后，可以将其中第二可能性程度最大的节点确定为目标子节点。

S404、在RRT树中添加父节点对应的目标子节点。

上述实施例提供的方法，在确定目标子节点时，考虑了两个节点间的路径长度以及路径不平滑程度，同时还考虑了可添加子节点的碰撞情况以及平滑情况，以使确定的目标子节点所在的路径不会超过允许的牵引车前轮最大转向角且不会与障碍物发生碰撞，还提高了车辆转向的连续性。

基于上述任一所示的实施例，在确定出目标路径后，可以对其进行进一步的平滑处理。图5为本申请实施例提供的路径处理方法实施例四的流程示意图。如图5所示，在确定出目标路径后，该路径处理方法还可以包括步骤：

S501、通过三次样条插值生成目标路径的分段多项式。

在本步骤中，在确定出目标路径后，可以通过三次样条插值对其进行平滑处理，进一步提高路径质量。

可选的，可以将目标路径通过三次样条插值得到x(s)，y(s)分段多项式(s为路径长度)。

S502、对分段多项式进行二次规划，获取目标路径的优化后节点。

在本步骤中，需要构建路径光滑的二次规划问题，对目标路径中的节点进行优化，获取目标路径的优化后节点。

在实际应用中，目标路径的节点与在三次样条s_i处的横向偏差为优化变量：d₀，d₁，...，d_n-1，其一阶导数为d′₀，d′₁，...，d′_n-1，其二阶导数为d″₀，d″₁，...，d″_n-1。

二次规划问题可以通过下述公式表示：

d₀＝0，

d″₀＝0

d_n-1＝0，

d′_i+1＝d′_i+d″_i(s_i+1-s_i)+d″′_i-＞i+1(s_i+1-s_i)²/2

d_i+1＝d_i+d′_i(s_i+1-s_i)+d″_i(s_i+1-s_i)²/2+d″′_i-＞i+1(s_i+1-s_i)³/6

d′_min＜d′_i＜d′_max

d″_min＜d″_i＜d″_max

其中，J为代价函数，可以通过下述公式进行表示：

J＝J_d+J_k+J_dk+J_end

进一步的，在该代价函数中：

yaw_world＝yaw_ref+d′

其中，w_d代表优化变量d的优化权重，w_k代表笛卡尔坐标系曲率的优化权重，w_dk代表笛卡尔坐标系曲率变化率的优化权重，

代表终止位置点朝向角的优化权重，

代表终止位置点曲率变化率的优化权重。k_ref，yaw_ref分别指的是目标路径插值成的三次样条在对应s_i处的笛卡尔曲率和笛卡尔斜率。

其中，二次规划问题还需要满足下面约束：

(1)、初态边界值约束，要求光滑后路径初态的位置和曲率与光滑前一致，路径yaw与牵引车初态姿态角yaw_init一致。

(2)、终态边界值约束，要求光滑后路径终态的位置与光滑前一致，路径yaw与牵引车终态姿态角yaw_end的误差在

终态曲率半径在(-R_converg，R_converg)范围内。

(3)、d'连续性约束

(4)、d”连续性约束

(5)、d,d”,d”'范围约束，计算三次样条s_i点处的法线与约束多边形的左右交点，将s_i点到交点距离作为挂载点横向偏差d的约束范围

S503、根据允许的牵引车前轮最大转向角以及障碍物信息，判断每个优化后节点对应的位置点与道路边界是否发生碰撞。

在本步骤中，还需要验证优化后节点是否可用，优化后节点对应的位置点与道路边界发生碰撞时说明优化后节点不可用，需要对其进行再次优化；优化后节点对应的位置点与道路边界不发生碰撞时说明优化后节点可用。

可选的，将优化得到的d₀，d₁，...，d_n-1，d′₀，d′₁，...，d′_n-1，d″₀，d″₁，...，d″_n-1转到世界坐标系下，根据允许的牵引车前轮最大转向角以及障碍物信息，确定可添加子节点对应的位置点与道路边界的碰撞情况，具体确定方式可以参考步骤(14)，此处不再赘述。

S5041、若否，则根据优化后节点对目标路径进行更新，生成更新后的目标路径。

在本步骤中，在存在优化后节点对应的位置点与道路边界不发生碰撞时，说明优化后节点构成的路径能够满足行驶要求，则可以在笛卡尔坐标下输出优化后节点，根据优化后节点生成更新后的目标路径。

S5042、若是，则根据优化后节点构成的路径与道路边界发生碰撞的碰撞位置点，通过碰撞位置点的坐标从所有优化后节点中确定出需要再次优化的节点。

在本步骤中，在优化后节点对应的位置点与道路边界发生碰撞时，说明此时优化后节点构成的路径不满足行驶要求，还需要对其进行再次优化调整。

可选的，可以根据优化后节点构成的路径坐标，以及道路边界的坐标，确定碰撞坐标。将该碰撞坐标转到以三次样条为s轴的frenet坐标系，得到其投影点s，二分查找此s对应的优化变量，该优化变量就是需要再次优化的节点与在三次样条si处的横向偏差。

S5052、调小需要再次优化的节点的曲率权重和曲率变化率权重。

在本步骤中，在查找到需要再次优化的节点时，需要调小曲率权重和曲率变化率权重，减少其与道路边界碰撞的可能性。

可选的，可以将需要再次优化的节点的曲率权重和曲率变化率权重减小至1/2。

可选的，可以判断平滑处理次数是否达到预设平滑处理次数，若是，则停止，若否，则重复执行S501至S5052。

在上述实施例中，在生成目标路径后，可以根据三次样条插值对其进行进一步平滑处理，提高了更新后的目标路径的质量和车辆的自动驾驶性能。

基于上述任一实施例提供的路径处理方法，接下来通过一个具体的示例对其进行解释说明。

图6为本申请实施例提供的路径处理方法实施例五的流程示意图。如图6所示，该路径处理方法包括构建二维运动规划问题、改进RRT搜索以及后处理优化三部分。

构建二维运动规划问题

应理解，该路径处理方法可以通过车辆模型实现，构建二维运动规划问题即为构建用于确定起始位置点至终止位置点的目标路径的车辆模型。

首先获取用户输入的车辆在起始位置点和终止位置点的位姿，之后对车辆模型参数、车辆运行状态、车辆运行状态在相邻帧(i至i+1)的递推关系进行定义。

车辆模型参数：

模型递推单步时长为dt，模型递推单步路径长为ds，c_yaw1为牵引车动力学修正系数，c_yaw2为挂车动力学系数，牵引车前轴到挂载点距离l1，挂车后轴到挂载点距离l2，允许的牵引车前轮最大转向角steer_constraint，离散转向角变化率集合enum_dsteer。

车辆的运行状态：

x,y为参考线坐标系下以车辆后轴为中心点的坐标，yaw1为参考线坐标系下牵引车姿态角，yaw2为参考线坐标系下挂车姿态角，steer牵引车前轮转向角，dsteer为牵引车前轮转向角变化率。

递推关系：

进一步的，从高精地图中获取从起始位置点到终止位置点的多个第一路径，从而获取障碍物信息。

最后，输入RRT规划问题和后处理优化问题参数，从而完成构建过程。

改进RRT搜索

在规划开始后，将迭代次数+1，并判断此时迭代次数是否小于第一预设迭代次数，若否，则直接结束；若是，则进行随机采样，获取采样位置点的位姿，从RRT树中确定该采样位置点对应的父节点，并确定该父节点对应的可添加的目标子节点，将该目标子节点更新至RRT树中，并判断目标子节点是否满足预设条件(也就是判断目标子节点对应的位置点是否到达终止位置点)，若否则将迭代次数+1，重复执行上述步骤，直到规划结束；若是，则将目标子节点至根节点的路径确定为第五路径并进行记录，并判断第五路径的数量是否超过预设路径数量，若是，则结束规划，若否，则将迭代次数+1，重复执行上述步骤，直到规划结束。

后处理优化

从第五路径中确定出目标路径，为该目标路径构建优化问题，将优化权重迭代次数+1，判断当前的优化权重迭代次数是否小于第二预设迭代次数，若否，则结束优化；若是，则求解优化问题，并判断是否满足地图约束，若是，则结束优化；若否，则将约束位置光滑权重减半，并将优化权重迭代次数+1重复执行上述过程，直到优化结束。。

在一个具体的示例中，以半挂车为例，最大转向角为800度，对应最大前轮转角0.6rad,最大转向角速度400度/秒，对应最大前轮转角变化率0.3rad/s。现假设车辆以最低过弯速度1.5m/s匀速，可以通过如下过程进行路径处理：

1.获取需要搜索可行路径的局部高精地图。

2.根据车辆在起始位置点和终止位置点的位姿，给出构建该规划问题的参数。

3.使用考虑牵引车半挂车运动学约束的RRT树规划多条车辆能够到达终止位置点的路径。

4.平滑车辆能够到达终止位置点的目标路径，并检查是否满足所有约束，平滑后得到更新后的目标路径。

通过上述示例，可以看出路径处理方法具有以下有益效果：

1、基于道路中心线的采样方式有效地加快了结果收敛。

2、能够生成利于自动驾驶车辆执行的路径，满足牵引车半挂车运动学约束。

3、能够避免牵引车和半挂车与静态障碍发生碰撞。

4、保证了路径的平滑性。

为了验证本技术方案的效果，分别对现有技术以及本技术方案进行仿真模拟，仿真模拟的结果如下。

图7为本申请实施例提供的路径对比示意图。如图7所示，原始参考线即为通过现有技术确定的目标路径，搜索结果即为通过本技术方案确定的目标路径。通过图7可以发现，本技术方案确定的目标路径相比现有技术来说更平滑。

图8为本申请实施例提供的曲率对比示意图。如图8所示，现有技术生成的目标路径的曲率在整体上大于本技术方案，甚至在部分位置超过了车辆最大执行曲率，无法满足车辆的实际运行要求。而本技术方案生成的目标路径的曲率的变化不大，且均小于车辆最大执行曲率，能够满足牵引半挂车的特殊避障和拉直需求。

图9为本申请实施例提供的曲率变化率对比示意图。如图9所示，现有技术生成的目标路径的曲率变化率波动幅度远远大于本技术方案，也就是说明，本技术方案的目标轨迹的平滑性远远大于现有技术。

下述为本申请装置实施例，可以用于执行本申请方法实施例。对于本申请装置实施例中未披露的细节，请参照本申请方法实施例。

图10为本申请实施例提供的路径处理装置的结构示意图。如图10所示，该路径处理装置1000包括：

获取模块1001，用于获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，所述障碍物信息用于表示从所述起始位置点到所述终止位置点的多个第一路径两侧的障碍物；

确定模块1002，用于从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据所述采样位置点的位姿以及RRT树中各节点的节点信息，从所述RRT树中确定出所述采样位置点对应的父节点，所述RRT树包括根节点以及所述根节点下的用于表示位置的多级节点，每个节点的节点信息用于表示所述车辆在对应位置的运行状态，所述根节点的节点信息与所述起始位置点的位姿有关；

添加模块1003，用于根据所述父节点的节点信息以及所述障碍物信息，在所述RRT树中添加所述父节点对应的目标子节点；

所述确定模块1002，还用于若所述目标子节点的节点信息满足预设条件，则根据所述目标子节点和所述根节点，确定目标路径，所述目标路径为从所述起始位置点到所述终止位置点的路径，所述预设条件与所述终止位置点的位姿有关。

在本申请实施例的一种可能设计中，所述确定模块1002，具体用于：

从任一第一路径所处的道路中进行采样，获取所述采样位置点的位姿；

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及处于所述节点至所述根节点所在第二路径中的所有节点的节点信息，计算所述车辆从所述节点对应的位置点行驶至所述采样位置点的第一可能性程度；

将所述第一可能性程度最大的节点确定为所述采样位置点对应的父节点。

可选的，所述车辆为牵引车半挂拖车，所述节点信息包括坐标，牵引车姿态角，挂车姿态角，牵引车前轮的转向角和转向角变化率；

相应的，所述确定模块1002，具体用于：

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及所述节点的坐标，牵引车姿态角以及挂车姿态角，确定所述采样位置点与所述节点对应的位置点之间的第三路径长度以及第三路径不平滑程度；

针对RRT树中每个节点，根据所述第二路径中的所有节点的牵引车前轮的转向角和转向角变化率，计算所述第二路径中的所有节点的牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和；

针对RRT树中每个节点，确定所述节点的离散转向角变化率集合中已使用的转向角变化率的数量，所述已使用的离散转向角变化率已用于确定所述节点是否可以添加所述已使用的离散转向角变化率对应的下一级子节点；

针对RRT树中每个节点，根据所述第三路径长度，所述第三路径不平滑程度，所述已使用的转向角变化率的数量，牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和，确定所述第一可能性程度。

可选的，所述添加模块1003，具体用于：

根据所述父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，确定所述RRT树中所述父节点的可添加子节点；

针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及所述障碍物信息，计算所述车辆从所述父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度；

将所述第二可能性程度最大的可添加子节点确定为所述父节点对应的目标子节点；

在所述RRT树中添加所述父节点对应的所述目标子节点。

可选的，所述添加模块1003，具体用于：

针对每一可添加子节点，根据所述可添加子节点的坐标，牵引车的前轮转向角以及转向角变化率，以及所述父节点的坐标，牵引车的前轮转向角以及转向角变化率，确定所述可添加子节点对应位置点与所述父节点对应位置点之间的第四路径长度以及第四路径不平滑程度；

针对每一可添加子节点，根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，确定所述可添加子节点对应的位置点与道路边界的碰撞情况；

针对每一可添加子节点，确定所述可添加子节点与所述父节点之间的转向角变化方向，与所述父节点与所述父节点的上一级节点的转向角变化方向的一致性；

针对每一可添加子节点，根据所述第四路径长度，所述第四路径不平滑程度，所述碰撞情况，所述一致性，计算所述第二可能性程度。

可选的，所述确定模块1002，具体用于：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点相对于所述终止位置点的误差；

若所述误差小于预设误差，则将所述目标子节点至根节点的路径确定为第五路径；

根据每个第五路径的误差，以及所述第五路径中的所有节点的牵引车前轮的转向角变化率绝对值累加和，从所有第五路径中确定出所述目标路径；

其中，所述预设条件为所述目标子节点与所述终止位置点的误差小于预设误差。

可选的，该路径处理装置1000还包括：

生成模块，用于通过三次样条插值生成所述目标路径的分段多项式；

所述获取模块1001，还用于对所述分段多项式进行二次规划，获取所述目标路径的优化后节点；

判断模块，用于根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，判断每个优化后节点对应的位置点与道路边界是否发生碰撞；

所述生成模块，还用于若否，则根据优化后节点对所述目标路径进行更新，生成更新后的目标路径；

所述确定模块1002，还用于若是，则根据优化后节点构成的路径与所述道路边界发生碰撞的碰撞位置点，通过所述碰撞位置点的坐标从所有优化后节点中确定出需要再次优化的节点；

调小模块，用于调小所述需要再次优化的节点的曲率权重和曲率变化率权重。

可选的，所述确定模块1002，具体用于：

从预设概率范围内进行随机正态采样，得到将所述终止位置点确定为所述采样位置点的概率值；

若所述概率值大于预设概率值，则将所述终止位置点确定为所述采样位置点，所述预设概率值处于所述预设概率范围内；

否则，则将从任一第一路径所在道路的道路中心线上进行采样，获取初始采样位置点；

根据所述初始采样位置点以及预设距离，确定采样范围；

从所述采样范围内进行采样，获取所述采样位置点，所述采样位置点的牵引车姿态角和挂车姿态角与所述初始采样位置点相同。

可选的，所述确定模块1002，具体用于：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点与所述终止位置点的横坐标差值小于第一预设距离，所述目标子节点与所述终止位置点的纵坐标差值小于第二预设距离，所述目标子节点与所述终止位置点的牵引车姿态角差值小于第一姿态角差值，所述目标子节点与所述终止位置点的挂车姿态角差值小于第二姿态角差值；

其中，所述误差包括所述横坐标差值，所述纵坐标差值，所述牵引车姿态角差值以及所述挂车姿态角差值。

本申请实施例提供的路径处理装置，可用于执行上述任一实施例中的路径处理方法，其实现原理和技术效果类似，在此不再赘述。

需要说明的是，应理解以上装置的各个模块的划分仅仅是一种逻辑功能的划分，实际实现时可以全部或部分集成到一个物理实体上，也可以物理上分开。且这些模块可以全部以软件通过处理元件调用的形式实现；也可以全部以硬件的形式实现；还可以部分模块通过处理元件调用软件的形式实现，部分模块通过硬件的形式实现。此外，这些模块全部或部分可以集成在一起，也可以独立实现。这里所述的处理元件可以是一种集成电路，具有信号的处理能力。在实现过程中，上述方法的各步骤或以上各个模块可以通过处理器元件中的硬件的集成逻辑电路或者软件形式的指令完成。

图11为本申请实施例提供的电子设备的结构示意图。如图11所示，该电子设备1100可以包括：处理器1101、存储器1102及存储在所述存储器1102上并可在处理器1101上运行的计算机程序指令，所述处理器1101执行所述计算机程序指令时实现前述任一实施例提供的路径处理方法。

可选的，该电子设备1100的上述各个器件之间可以通过系统总线连接。

存储器1102可以是单独的存储单元，也可以是集成在处理器中的存储单元。处理器的数量为一个或者多个。

可选的，电子设备1100还可以包括与其他设备进行交互的接口。

应理解，处理器1101可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请所公开的方法的步骤可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。

系统总线可以是外设部件互连标准(peripheral component interconnect，PCI)总线或扩展工业标准结构(extended industry standard architecture，EISA)总线等。系统总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。存储器可能包括随机存取存储器(randomaccess memory，RAM)，也可能还包括非易失性存储器(non-volatile memory，NVM)，例如至少一个磁盘存储器。

实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一可读取存储器中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储器(存储介质)包括：只读存储器(read-only memory，ROM)、RAM、快闪存储器、硬盘、固态硬盘、磁带(magnetic tape)、软盘(floppy disk)、光盘(optical disc)及其任意组合。

本申请实施例提供的电子设备，可用于执行上述任一方法实施例提供的路径处理方法，其实现原理和技术效果类似，在此不再赘述。

本申请实施例提供一种计算机可读存储介质，该计算机可读存储介质中存储有计算机执行指令，当该计算机执行指令在计算机上运行时，使得计算机执行上述路径处理方法。

上述的计算机可读存储介质，上述可读存储介质可以是由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器，电可擦除可编程只读存储器，可擦除可编程只读存储器，可编程只读存储器，只读存储器，磁存储器，快闪存储器，磁盘或光盘。可读存储介质可以是通用或专用计算机能够存取的任何可用介质。

可选的，将可读存储介质耦合至处理器，从而使处理器能够从该可读存储介质读取信息，且可向该可读存储介质写入信息。当然，可读存储介质也可以是处理器的组成部分。处理器和可读存储介质可以位于专用集成电路(Application Specific IntegratedCircuits，ASIC)中。当然，处理器和可读存储介质也可以作为分立组件存在于设备中。

本申请实施例还提供一种计算机程序产品，该计算机程序产品包括计算机程序，该计算机程序存储在计算机可读存储介质中，至少一个处理器可以从该计算机可读存储介质中读取该计算机程序，所述至少一个处理器执行所述计算机程序时可实现上述路径处理方法。

应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求书来限制。

Claims (10)

1.一种路径处理方法，其特征在于，包括：

获取障碍物信息以及车辆在起始位置点和终止位置点的位姿，所述障碍物信息用于表示从所述起始位置点到所述终止位置点的多个第一路径两侧的障碍物；

从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据所述采样位置点的位姿以及快速搜索随机树算法RRT树中各节点的节点信息，从所述RRT树中确定出所述采样位置点对应的父节点，所述RRT树包括根节点以及所述根节点下的用于表示位置的多级节点，每个节点的节点信息用于表示所述车辆在对应位置的运行状态，所述根节点的节点信息与所述起始位置点的位姿有关；

根据所述父节点的节点信息以及所述障碍物信息，在所述RRT树中添加所述父节点对应的目标子节点；

若所述目标子节点的节点信息满足预设条件，则根据所述目标子节点和所述根节点，确定目标路径，所述目标路径为从所述起始位置点到所述终止位置点的路径，所述预设条件与所述终止位置点的位姿有关。

2.根据权利要求1所述的方法，其特征在于，所述从任一第一路径所处的道路中进行采样，在每次获取到采样位置点的位姿后，根据所述采样位置点的位姿以及RRT树中各节点的节点信息，从所述RRT树中确定出所述采样位置点对应的父节点，包括：

从任一第一路径所处的道路中进行采样，获取所述采样位置点的位姿；

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及处于所述节点至所述根节点所在第二路径中的所有节点的节点信息，计算所述车辆从所述节点对应的位置点行驶至所述采样位置点的第一可能性程度；

将所述第一可能性程度最大的节点确定为所述采样位置点对应的父节点。

3.根据权利要求2所述的方法，其特征在于，所述车辆为牵引车半挂拖车，所述节点信息包括坐标，牵引车姿态角，挂车姿态角，牵引车前轮的转向角和转向角变化率；

相应的，所述在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及处于所述节点至所述根节点所在第二路径中的所有节点的节点信息，计算所述车辆从所述节点对应的位置点行驶至所述采样位置点的第一可能性程度，包括：

在每次获取到采样位置点的位姿后，针对RRT树中每个节点，根据所述采样位置点的位姿以及所述节点的坐标，牵引车姿态角以及挂车姿态角，确定所述采样位置点与所述节点对应的位置点之间的第三路径长度以及第三路径不平滑程度；

针对RRT树中每个节点，根据所述第二路径中的所有节点的牵引车前轮的转向角和转向角变化率，计算所述第二路径中的所有节点的牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和；

针对RRT树中每个节点，确定所述节点的离散转向角变化率集合中已使用的转向角变化率的数量，所述已使用的离散转向角变化率已用于确定所述节点是否可以添加所述已使用的离散转向角变化率对应的下一级子节点；

针对RRT树中每个节点，根据所述第三路径长度，所述第三路径不平滑程度，所述已使用的转向角变化率的数量，牵引车前轮的转向角绝对值累加和以及转向角变化率绝对值累加和，确定所述第一可能性程度。

4.根据权利要求3所述的方法，其特征在于，所述根据所述父节点的节点信息以及所述障碍物信息，在所述RRT树中添加所述父节点对应的目标子节点，包括：

根据所述父节点的节点信息，牵引车动力学修正系数以及挂车动力学系数，确定所述RRT树中所述父节点的可添加子节点；

针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及所述障碍物信息，计算所述车辆从所述父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度；

将所述第二可能性程度最大的可添加子节点确定为所述父节点对应的目标子节点；

在所述RRT树中添加所述父节点对应的所述目标子节点。

5.根据权利要求4所述的方法，其特征在于，所述针对每一可添加子节点，根据允许的牵引车前轮最大转向角以及所述障碍物信息，计算所述车辆从所述父节点对应的位置点行驶至可添加子节点对应的位置点的第二可能性程度，包括：

针对每一可添加子节点，根据所述可添加子节点的坐标，牵引车的前轮转向角以及转向角变化率，以及所述父节点的坐标，牵引车的前轮转向角以及转向角变化率，确定所述可添加子节点对应位置点与所述父节点对应位置点之间的第四路径长度以及第四路径不平滑程度；

针对每一可添加子节点，根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，确定所述可添加子节点对应的位置点与道路边界的碰撞情况；

针对每一可添加子节点，确定所述可添加子节点与所述父节点之间的转向角变化方向，与所述父节点与所述父节点的上一级节点的转向角变化方向的一致性；

针对每一可添加子节点，根据所述第四路径长度，所述第四路径不平滑程度，所述碰撞情况，所述一致性，计算所述第二可能性程度。

6.根据权利要求2至5任一项所述的方法，其特征在于，所述若所述目标子节点的节点信息满足预设条件，则根据所述目标子节点和所述根节点，确定目标路径，所述目标路径为从所述起始位置点到所述终止位置点的路径，包括：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点相对于所述终止位置点的误差；

若所述误差小于预设误差，则将所述目标子节点至根节点的路径确定为第五路径；

根据每个第五路径的误差，以及所述第五路径中的所有节点的牵引车前轮的转向角变化率绝对值累加和，从所有第五路径中确定出所述目标路径；

其中，所述预设条件为所述目标子节点与所述终止位置点的误差小于预设误差。

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

通过三次样条插值生成所述目标路径的分段多项式；

对所述分段多项式进行二次规划，获取所述目标路径的优化后节点；

根据所述允许的牵引车前轮最大转向角以及所述障碍物信息，判断每个优化后节点对应的位置点与道路边界是否发生碰撞；

若否，则根据优化后节点对所述目标路径进行更新，生成更新后的目标路径；

若是，则根据优化后节点构成的路径与所述道路边界发生碰撞的碰撞位置点，通过所述碰撞位置点的坐标从所有优化后节点中确定出需要再次优化的节点；

调小所述需要再次优化的节点的曲率权重和曲率变化率权重。

8.根据权利要求2至5任一项所述的方法，其特征在于，所述从任一第一路径所处的道路中进行采样，获取所述采样位置点的位姿，包括：

从预设概率范围内进行随机正态采样，得到将所述终止位置点确定为所述采样位置点的概率值；

若所述概率值大于预设概率值，则将所述终止位置点确定为所述采样位置点，所述预设概率值处于所述预设概率范围内；

否则，则将从任一第一路径所在道路的道路中心线上进行采样，获取初始采样位置点；

根据所述初始采样位置点以及预设距离，确定采样范围；

从所述采样范围内进行采样，获取所述采样位置点，所述采样位置点的牵引车姿态角和挂车姿态角与所述初始采样位置点相同。

9.根据权利要求6所述的方法，其特征在于，所述根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点相对于所述终止位置点的误差，包括：

根据所述目标子节点的节点信息与所述终止位置点的位姿，计算所述目标子节点与所述终止位置点的横坐标差值小于第一预设距离，所述目标子节点与所述终止位置点的纵坐标差值小于第二预设距离，所述目标子节点与所述终止位置点的牵引车姿态角差值小于第一姿态角差值，所述目标子节点与所述终止位置点的挂车姿态角差值小于第二姿态角差值；

其中，所述误差包括所述横坐标差值，所述纵坐标差值，所述牵引车姿态角差值以及所述挂车姿态角差值。

10.一种电子设备，包括：处理器、存储器及存储在所述存储器上并可在处理器上运行的计算机程序指令，其特征在于，所述处理器执行所述计算机程序指令时用于实现如权利要求1至9任一项所述的路径处理方法。

Images