路径规划方法和路径规划装置与流程

1.本申请涉及智能驾驶领域，尤其涉及路径规划方法和路径规划装置。


背景技术：

2.无人驾驶是智能交通系统中的重要组成部分。无人驾驶车辆(下面简称无人车)接收到传感器的各种感知信息之后，对当前环境做出分析，然后对底层控制模块下达指令，这一过程就是决策规划模块的主要任务。避障路径规划是决策规划模块的重要部分，即结合实时交通信息对预先规划好的行车路线进行调整，达到避免与障碍物发生碰撞的目的。
3.避障路径规划中，通常运用单源最短路径(dijkstra)算法，a*算法，快速扩展随机树(rrt)。以rrt算法为例，从起始点开始向外拓展一个树状结构，树状结构的拓展方向通过在规划空间内随机采点确定。
4.现有技术中，基于rrt算法实现避障路径规划时，由于路径采样点的随机分布，无法保证规划路径的光滑性，因此，采用rrt算法生成的路径质量较低。


技术实现要素：

5.本申请实施例提供了一种路径规划方法，用于实现避障路径规划，可以提高规划的路径质量。
6.本申请实施例的第一方面提供一种路径规划方法，包括：获取道路信息、目标对象的初始位置和目的位置；根据所述道路信息确定笛卡尔坐标系、frenet坐标系以及所述笛卡尔坐标系与所述frenet坐标系的映射关系；基于所述映射关系获取基于frenet坐标系的路径质量指标函数，所述路径质量指标函数为二次型函数；根据所述基于frenet坐标系的路径质量指标函数、所述目标对象的初始位置和所述目的位置，利用二次最优规划确定基于所述frenet坐标系的目标路径；根据所述映射关系，将所述目标路径转换到所述笛卡尔坐标系中，所述目标路径用于所述目标对象的路径规划控制。
7.本申请实施例提供的路径规划方法，通过基于笛卡尔坐标系与frenet坐标系的映射关系确定路径质量指标函数，利用二次规划解法可以获取基于frenet坐标系的目标路径，根据该映射关系，可以将该目标路径转换到笛卡尔坐标系中用于路径规划控制，本申请实施例提供的路径规划方法可以提升目标路径的路径质量。由于通过笛卡尔坐标系与frenet坐标系的映射关系确定二次型的路径质量指标函数，利用二次最优规划确定目标路径，可以简化运算，提升路径规划速度，满足实时运算需求。
8.可选地，在确定所述映射关系之后，所述路径规划装置获取目标对象的初始位置，以及获取目的位置。
9.在第一方面的一种可能的实现方式中，所述根据所述基于frenet坐标系的路径质量指标函数、所述目标对象的初始位置和所述目的位置，利用二次最优规划确定基于所述frenet坐标系的目标路径包括：根据所述道路信息确定道路边界约束条件；在所述道路边界约束条件的约束下，利用二次规划解法确定基于所述frenet坐标系的目标路径。
10.本申请实施例提供的路径规划方法，在利用二次规划解法确定目标路径时，可以根据道路信息确定的道路边界约束条件下计算。
11.在第一方面的一种可能的实现方式中，所述方法还包括：获取障碍物信息；所述根据所述基于frenet坐标系的路径质量指标函数、所述目标对象的初始位置和所述目的位置，利用二次最优规划确定基于所述frenet坐标系的目标路径包括：在根据所述障碍物信息确定的障碍物约束条件下，利用二次规划解法确定基于所述frenet坐标系的目标路径。
12.所述障碍物约束条件为目标路径上的路径点在沿道路纵向方向，与障碍物的距离小于或等于预设长度的范围内，目标路径上的路径点与障碍物的横向距离需大于或等于预设阈值，即在一段纵向区间内需保证距离障碍物的横向安全距离，该安全距离为预设值，由此确定的目标路径的障碍物碰撞风险低，提高了目标路径的安全程度。
13.本申请实施例提供的路径规划方法，若道路中存在障碍物，在利用二次规划解法确定目标路径时，可以根据障碍物信息确定的障碍物约束条件下进行计算，确定的目标路径不仅路径质量较高，而且可以满足避障要求。
14.在第一方面的一种可能的实现方式中，所述根据所述基于frenet坐标系的路径质量指标函数、所述目标对象的初始位置和所述目的位置，利用二次最优规划确定基于所述frenet坐标系的目标路径包括：利用二次规划解法确定所述目标路径上的多个控制节点；根据所述多个控制节点，以插值方式确定所述目标路径上除所述多个控制节点之外的任一路径点。目标路径上的控制节点即目标路径上的采样点，采样点的数量不做限定，
15.本申请实施例提供的路径规划方法，在确定目标路径时，可以先确定目标路径中的多个控制节点的位置，然后通过插值的方式确定控制节点之间的任一路径点。
16.在第一方面的一种可能的实现方式中，所述插值方式包括自然三次样条曲线插值。
17.本申请实施例提供的路径规划方法，在通过插值的方法确定控制节点之间的路径点时，可以根据自然三次样条曲线插值。本申请实施例提供的路径规划方法，根据自然三次样条曲线插值计算路径点时，由于自然三次样条曲线的性质，相邻样条段曲线在控制节点处斜率和曲率都相同，可以简化计算，将路径质量指标的都表示为二次型，即优化目标表示为二次型。
18.在第一方面的一种可能的实现方式中，所述路径质量指标函数包括路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数和航向角误差变化率的函数中的一个或多个的组合。
19.本申请实施例提供的路径规划方法，路径质量指标函数可以有多种组合构成形式，可以根据实际需求灵活设定，针对性改善目标路径质量，使得获取的目标路径更符合运动目标对象的实际运动场景。
20.在第一方面的一种可能的实现方式中，所述路径质量指标函数为路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数或航向角误差变化率的函数。
21.在第一方面的一种可能的实现方式中，所述路径质量指标函数由路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数和航向角误差变化率的函数中的至少两个组成。
22.在第一方面的一种可能的实现方式中，所述基于所述映射关系获取基于frenet坐
标系的路径质量指标函数包括：根据预设的权重确定所述路径质量指标函数。
23.本申请实施例提供的路径规划方法，根据多个路径质量指标确定优化目标时，可以根据不同质量指标的重要程度设定权值。
24.在第一方面的一种可能的实现方式中，所述笛卡尔坐标系包括：车身坐标系、站心坐标系或世界坐标系。
25.本申请实施例的第二方面提供了一种路径规划装置，包括：获取模块，用于获取道路信息、目标对象的初始位置和目的位置；确定模块，用于根据所述道路信息确定笛卡尔坐标系、frenet坐标系以及所述笛卡尔坐标系与所述frenet坐标系的映射关系；所述获取模块，还用于基于所述映射关系获取基于frenet坐标系的路径质量指标函数，所述路径质量指标函数为二次型函数；所述确定模块，还用于根据所述基于frenet坐标系的路径质量指标函数、所述目标对象的初始位置和所述目的位置，利用二次最优规划确定基于所述frenet坐标系的目标路径；转换模块，用于根据所述映射关系，将所述目标路径转换到所述笛卡尔坐标系中，所述目标路径用于所述目标对象的路径规划控制。
26.在第二方面的一种可能的实现方式中，所述确定模块具体用于：根据所述道路信息确定道路边界约束条件；在所述道路边界约束条件的约束下，利用二次规划解法确定基于所述frenet坐标系的目标路径。
27.在第二方面的一种可能的实现方式中，所述获取模块还用于：获取障碍物信息；所述确定模块具体用于：在根据所述障碍物信息确定的障碍物约束条件下，利用二次规划解法确定基于所述frenet坐标系的目标路径。
28.在第二方面的一种可能的实现方式中，所述确定模块具体用于：利用二次规划解法确定所述目标路径上的多个控制节点；根据所述多个控制节点，以插值方式确定所述目标路径上除所述多个控制节点之外的任一路径点。
29.在第二方面的一种可能的实现方式中，所述插值方式包括自然三次样条曲线插值。
30.在第二方面的一种可能的实现方式中，所述路径质量指标函数包括路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数和航向角误差变化率的函数中的一个或多个的组合。
31.在第二方面的一种可能的实现方式中，所述路径质量指标函数为路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数或航向角误差变化率的函数。
32.在第二方面的一种可能的实现方式中，所述路径质量指标函数由路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数和航向角误差变化率的函数中的至少两个组成。
33.在第二方面的一种可能的实现方式中，根据预设的多个所述路径质量指标的权重确定所述路径质量指标函数。
34.在第二方面的一种可能的实现方式中，所述笛卡尔坐标系包括：车身坐标系、站心坐标系或世界坐标系。
35.本申请实施例第三方面提供了一种路径规划装置，包括处理器和存储器，所述处理器和所述存储器相互连接，其中，所述存储器用于存储计算机程序，所述计算机程序包括程序指令，所述处理器用于调用所述程序指令，执行如上述第一方面以及各种可能的实现
方式中任一项所述的方法。
36.本申请实施例第四方面提供了一种包含指令的计算机程序产品，其特征在于，当其在计算机上运行时，使得所述计算机执行如上述第一方面以及各种可能的实现方式中任一项所述的方法。
37.本申请实施例第五方面提供了一种计算机可读存储介质，包括指令，其特征在于，当所述指令在计算机上运行时，使得计算机执行如上述第一方面以及各种可能的实现方式中任一项所述的方法。
38.本申请实施例第六方面提供了一种一种芯片，包括处理器。处理器用于读取并执行存储器中存储的计算机程序，以执行上述任一方面任意可能的实现方式中的方法。可选地，该芯片该包括存储器，该存储器与该处理器通过电路或电线与存储器连接。进一步可选地，该芯片还包括通信接口，处理器与该通信接口连接。通信接口用于接收需要处理的数据和/或信息，处理器从该通信接口获取该数据和/或信息，并对该数据和/或信息进行处理，并通过该通信接口输出处理结果。该通信接口可以是输入输出接口。
39.本申请实施例提供的路径规划方法，通过笛卡尔坐标系与frenet坐标系的映射关系，构建二次型的路径质量指标函数，进而利用二次规划解法确定基于所述frenet坐标系的目标路径，最后转换到所述笛卡尔坐标系中，所述目标路径用于所述目标对象的路径规划控制。利用frenet坐标系方便地描述路径，并根据该映射关系在基于笛卡尔坐标系进行计算，利用二次规划解法确定目标路径，相较现有rrt算法，可以减少计算量，提升路径规划速度。
附图说明
40.图1为基于笛卡尔坐标系描述道路中车辆位置的示意图；
41.图2为基于frenet坐标系描述道路中车辆位置的示意图；
42.图3为本申请实施例中路径规划方法的系统架构示意图；
43.图4为本申请实施例中路径规划方法的一个实施例示意图；
44.图5为本申请实施例中笛卡尔坐标系与frenet坐标系的映射关系示意图；
45.图6为本申请实施例中避障约束的实施例示意图；
46.图7为本申请实施例提供的目标路径的一个示意图；
47.图8为本申请实施例中路径规划装置的一个实施例示意图；
48.图9为本申请实施例中路径规划装置的一个实施例示意图。
具体实施方式
49.本申请实施例提供了一种路径规划方法，用于实现避障路径规划，可以提高规划的路径质量。
50.本发明实施例可应用于各种运动目标对象(包括机动车、非机动车、行人或机器人等)在各种形式的路径(包括高速公路、城市道路、乡村道路或室内路径等)上的运动，后续实施例以车辆或车道为例做描述，但本领域技术人员可以将其扩展至其他目标对象的路径规划领域，具体此处不做限定。
51.本申请实施例中，假设无人车在结构化道路上行驶，实际应用场景可能包括多车
道，在多车道场景中，由于无论无人车处于哪条车道，根据交通规则行驶过程中，只能选择相邻车道变道，即无人车只能向左变道或者向右变道，由此，可以将无人车在多车道中的行驶简化为双车道模型，本申请实施例中，将以双车道模型为例进行介绍，可以理解的是，这并不会对方案实施的场景构成限定。
52.为了便于理解，下面对本申请涉及的部分技术术语进行简要介绍：
53.笛卡尔坐标系：笛卡尔坐标系通常用于描述物体的位置，请参阅图1，基于笛卡尔坐标系描述道路中车辆位置的示意图。不难发现，笛卡尔坐标系对于道路中车辆路径的位置描述来说并不是最佳选择，因为即使给出了车辆位置(x，y)，还是无法直观确定车辆行驶距离，以及偏离车道中心的距离。
54.frenet坐标系：frenet坐标系则可以笛卡尔坐标系在描述道路中车辆位置时的一些问题，请参阅图2，基于frenet坐标系描述道路中车辆位置的示意图。在frenet坐标系中，纵坐标s为道路中心线长度，道路中心线也称道路参考线，s代表frenet坐标中的沿道路中心线方向的纵向距离，也称为路程，横坐标d表示与道路中心线的法向位移。虽然frenet坐标系可以方便地描述道路中车辆位置，但是在评价路径质量时，会涉及复杂的求导运算，描述路径质量时会涉及复杂的一阶或者二阶求导运算，在frenet坐标系下难以构建关于路径质量指标，例如曲率和航向角的二次优化项，包含长度、曲率、路径航向误差的综合路径质量指标不高，frenet坐标系中进行路径质量评价计算非常复杂。
55.自然三次样条曲线：两个相邻曲线段在公共边界(控制点)处有相同的一阶和二阶导数，即自然三次样条具有二阶导数连续性。
56.路径质量：现有的路径规划方法中，一般都基于给定的障碍物信息进行避障规划，但是较少考虑路径质量指标，路径质量对于无人车避障换道时乘客的舒适感，以及道路行驶效率的影响较大，本申请中，用于评价路径质量的路径质量指标包括但不限于：路径长度、曲率、航向角误差、曲率变化率和航向角误差变化率。
57.二次规划(quadratic programming)是一种特殊类型的最佳化问题。
58.下面结合附图，对本申请的实施例进行描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。本领域普通技术人员可知，随着技术的发展和新场景的出现，本申请实施例提供的技术方案对于类似的技术问题，同样适用。
59.本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或模块的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或模块，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或模块。在本申请中出现的对步骤进行的命名或者编号，并不意味着必须按照命名或者编号所指示的时间/逻辑先后顺序执行方法流程中的步骤，已经命名或者编号的流程步骤可以根据要实现的技术目的变更执行次序，只要能达到相同或者相类似的技术效果即可。
60.请参阅图3，为本申请实施例中路径规划方法的系统架构示意图；
61.本申请针对智能车在道路中的自主避障场景，该系统以经车辆传感器融合后得到的道路中心线位置以及障碍物位置信息为输入，以车辆最优避障路径为输出，联合笛卡尔
坐标与frenet道路坐标，采用二次最优规划求解得到，该系统的架构如图3所示。
62.请参阅图4，为本申请实施例中路径规划方法的一个实施例示意图；
63.401、获取道路信息、目标对象的初始位置和目的位置；
64.路径规划装置获取道路信息，道路信息可以通过高精地图以及车辆的精确定位获取，获取道路信息的具体方式此处不做限定。
65.可选的，路径规划装置还获取障碍物信息，障碍物信息可以通过车辆传感器捕捉，例如激光雷达或惯性导航(imu)等，获取障碍物信息的具体方式此处不做限定。
66.此外，路径规划装置还获取目标对象的初始位置和目的位置；
67.需要说明的是，路径规划装置获取道路信息，与获取目标对象的初始位置，与获取目标对象的目的位置，可以是同时获取，也可以是分别获取。若分别获取，对获取道路信息、目标对象的初始位置和目的位置的执行顺序不做限定。
68.402、获取基于笛卡尔坐标系的道路中心线；
69.frenet坐标系基于道路参考线(reference line)构建，道路参考线例如可以是道路中心线。
70.可选的，请参阅图5，虚线为道路中心线，基于道路中心线上的n个采样点，可以通过插值的方式获得关于道路中心线(x
r
，y
r
)的函数式其中(x
r
，y
r
)为笛卡尔坐标，f
rx
和f
ry
是关于中心线长度s的函数，即frenet坐标中的沿参考线的纵向距离。
71.可选的，确定障碍物在笛卡尔坐标系中的坐标。
72.403、构建笛卡尔坐标系中的路径点与frenet坐标系中路径点的映射关系；
73.请继续参阅图5，可选的，为便于计算，可以将道路中心线上的o点定义为frenet坐标系原点和笛卡尔坐标系原点，o点可以是车辆获取道路信息的当前时刻所在位置沿道路中心线法向投影至道路中心线上的点，frenet坐标系原点的位置具体此处不做限定。
74.点a代表道路中任一路径点，a点在笛卡尔坐标系中的坐标为(x，y)，a点在frenet坐标系中的坐标为(s，l)，a点沿道路中心线法向投影至道路中心线上的o
′
点，e
x
为笛卡尔坐标系中的横向单位向量，e
y
为笛卡尔坐标系中的纵向单位向量。
75.根据道路中心线在笛卡尔坐标系中的函数式，求得路径点在frenet坐标系下的法向单位向量e
n
，e
n
可以表示为关于s的函数e
n
(s)，表达式如下：
[0076][0077]
然后根据图5所示的向量关系，可以得到a点的frenet坐标(s，l)和笛卡尔坐标(x，y)相互转化关系：
[0078]
[0079]
其中，道路中心线(x
r
，y
r
)的x
r
可以表示为关于s的函数x
r
(s)，x
r
(s)＝f
rx
(s)；类似地，y
r
可以表示为关于s的函数y
r
(s)，y
r
(s)＝f
ry
(s)。
[0080]
可选的，可以获取障碍物的frenet坐标。
[0081]
404、获取路径质量指标函数；
[0082]
路径优化的目标是提升路径质量，具体的，可以是提升一个或多个路径质量指标，路径质量指标包括路径长度、航向角误差、曲率、曲率变化率或航向角误差变化率等待，具体此处不做限定。可选的，下面以路径质量指标函数包括路径长度函数、曲率函数和航向角误差函数为例进行介绍。
[0083]
联合笛卡尔坐标和frenet坐标描述路径长度、曲率和航向角。
[0084]
可选的，假设路径曲线ε(s)＝[x(s)，y(s)]由n+1个控制节点构成，且各控制节点之间的纵向距离保持均匀分布，即各控制节点的frenet纵坐标等差，各控制节点间的任一路径点(x,y)位置可以基于各个控制节点，根据样条曲线等求得，具体方法此处不做限定。下面以路径点(x,y)位置用关于道路中心线长度s基于三次自然样条曲线插值求得为例进行介绍。
[0085]
自然三次样条曲线，是在三次样条曲线的基础上，满足相邻样条段曲线在控制节点处斜率和曲率都相同。也就是说，自然样条曲线z在各控制节点处的一阶、二阶导数可表示成关于控制节点坐标的线性关系，即
[0086][0087]
其中，z是关于s的函数，可表示为z(s)，g、h均为常数矩阵。
[0088]
基于frenet坐标与笛卡尔坐标的转换关系，笛卡尔坐标系下的路径点位置(x,y)可以写成如下的矩阵形式，其中横坐标x可以表示为关于s的函数x(s)，纵坐标y可以表示为关于s的函数y(s)，x(s)和y(s)的表达式如下：
[0089][0090]
其中，l＝[l
1 l2ꢀ…ꢀ
l
n l
n+1
]
t
称为路径的决策向量，l1代表第1个控制节点与道路中心线的法向位移，类似地，ln代表第n个控制节点与道路中心线的法向位移，不再一一赘述；
[0091]
s＝[s
1 s2ꢀ…
s
n s
n+1
]，大写s为路径决策向量l对应的纵向距离向量，i代表第i个控制节点；s
i
代表控制节点i对应的道路中心线长度；
[0092]
x
r
(s)＝[x
r
(s1) x
r
(s2)
ꢀ…ꢀ
x
r
(s
n
) x
r
(s
n+1
)]
[0093]
类似地，y
r
(s)＝[y
r
(s1) y
r
(s2)
ꢀ…ꢀ
y
r
(s
n
) y
r
(s
n+1
)]；
[0094]
其中x
r
(s)为路径点对应的道路中心线上的点的笛卡尔坐标系中的横坐标，可以表示为关于s的函数，即x
r
(s)＝f
rx
(s)；y
r
(s)为路径点对应的道路中心线上的点的笛卡尔坐标系中的纵坐标，可以表示为关于s的函数，y
r
(s)＝f
ry
(s)。
[0095]
diag[
·
]表示根据括号中的与元素生成一对角矩阵，且
[0096][0097]
基于上述三次自然样条曲线的性质，可以将路径曲线ε(s)＝[x(s)，y(s)]的各路径质量指标在frenet坐标系下表示成二次型函数。下面分别进行介绍：
[0098]
1)、路径长度
[0099]
相邻控制节点之间的间距，基于frenet坐标与笛卡尔坐标的转换关系有
[0100][0101]
其中，，α
i
＝[
‑
χ
i χ
i+1
]，β
i
＝[
‑
σ
i σ
i+1
]，
[0102]
δx
i
为相邻控制节点之间的笛卡尔横坐标间距，δy
i
为相邻控制节点之间的笛卡尔横坐标间距，l
i
为控制节点i的frenet横坐标，正值表示参考线左侧，负值表示在参考线右侧，代表相邻控制节点的frenet横坐标的均值，δx
ri
为相邻控制节点之间的frenet横坐标间距，δy
ri
为相邻控制节点之间的frenet纵坐标间距。
[0103]
相邻路径控制点的路径长度平方项为，
[0104][0105]
对于整条路径的路径长度平方项的函数表达式为：
[0106][0107]
其中常数矩阵满足(具体形式参考实例)，cost代表与路径决策向量l无关的常数项。且
[0108][0109]
2)、路径曲率
[0110]
根据曲率定义，某一路径控制点处曲率平方项κ2，可以表示为s的函数κ2(s)，计算公式为：
[0111][0112]
根据自然三次样条曲线的性质，对于整条路径节点有：
[0113][0114][0115]
同理可得，
[0116][0117]
最终有，对于整条路径的路径曲率平方项的函数表达式为：
[0118][0119]
其中，
[0120]
n＝h
t
h
[0121]
t＝2(x
rt
h
t
h)diag[χ
i
]+2(y
rt
h
t
h)diag[σ
i
]
[0122]
cost代表与路径决策向量l无关的常数项。
[0123]
3)、路径航向角误差
[0124]
某一控制节点处的路径航向角可近似为相邻路径控制点连线的斜率，它与道路中心线的航向角的误差可表示为s的函数计算公式近似表示为：
[0125][0126]
因此，整条路径的航向角误差二次项的函数表达式为
[0127][0128]
其中q＝g
t
g，
[0129]
cost代表与路径决策向量l无关的常数项。
[0130]
405、根据路径质量指标函数利用二次最优规划确定基于frenet坐标系的目标路径；
[0131]
为了生成适合车辆行驶的路径，分别考虑路径长度、曲率和航向角误差，应满足：
[0132]
(1)最短行驶路径：即路径长度尽可能小，有利于节省时间和油耗。
[0133]
(2)最优曲率：所求路径的曲率尽可能地小，避免大曲率路径导致危险工况的发生。
[0134]
(3)最优路径航向角：路径节点处航向角与道路中心线的航向角的偏差尽可能小，路径节点处航向角是指车辆沿候选路径行驶至路径节点i时的航向角，道路中心线的航向角是指车辆沿道路参考线行驶的航向角。
[0135]
为了达到上述目的，根据路径质量指标函数构建优化目标，即如下所示的二次最优规划qp(quadratic program)问题：
[0136][0137]
其中p1，p2，p3为对应的权重项，具体数值此处不做限定。
[0138]
满足15
‑
1式优化目标的路径即为目标路径。
[0139]
可选的，若存在障碍物，为了保障避障过程中主车与障碍物不会发生碰撞，还需增
加避障约束。请参阅图6，为本申请实施例中避障约束的实施例示意图。根据当前的位置关系大致估算与障碍物最近的控制点p
j
，j≈[d/δs]，其中d为当前主车与障碍车的距离，δs表示路径控制点frenet纵向距离间距，加上该点纵向位移前后安全距离d内附近m≈[d/δs]个控制点，它们的侧向位移至少要比障碍物的侧向位移多出一安全距离阈值r
safety
，从而保证避障路径不会与障碍物发生碰撞。(其中[
·
]为取整符号)。最终，将最优避障路径规划问题描述成如下所示的二次最优规划问题(以左避障为例)，即：
[0140][0141]
s.t.d
r
≤l
k
≤d
l
，k＝1，2，...，j
‑
m
‑
1，j+m+1，...，n+1
[0142]
l
obst
+r
safety
≤l
k
≤d
l
，k＝j
‑
m，...，j，...j+m
[0143]
其中l
k
表示某个路径控制点的frenet横向坐标值，l
obst
表示障碍物的frenet横向坐标值，d
l
和d
r
分别表示道路左边界和右边界的frenet横向坐标值。
[0144]
根据障碍物约束条件和道路边界约束，确定使得，路径长度、曲率、和航向角误差的二次形式最小的目标路径的l矩阵。
[0145]
406、将基于frenet坐标系的目标路径转换为笛卡尔坐标系中；
[0146]
最后，依据式(4)将求解得到的最优路径从frenet坐标转换回笛卡尔坐标，将其传递给路径跟随模块，用于对底层控制模块下达指令。
[0147]
示例性的，下面举例介绍本申请实施例提供的路径规划方法：
[0148]
步骤1：构建笛卡尔坐标与frenet坐标的映射关系；
[0149]
假设道路中心线为三次多项式曲线，道路长度约l＝250m，道路开始处的曲率为0，终止处的曲率为c
f
＝1/800，道路曲率变化率c＝(c
f
‑
c0)/l=1/200000，道路中心线侧向位移方程设为：
[0150][0151]
在该中心线上以间距为1采样笛卡尔横向x坐标，即x
r
＝0∶1∶250，共计n＝251个采样点，根据式(16)计算得到笛卡尔纵向y坐标的值，通过勾股定理近似计算出相邻采样点之间的距离，并累计得到参考路径向量s，初始值s[0]＝0。
[0152]
s[i]＝s[i
‑
1]+sqrt((x
r
[i]
‑
x
r
[i
‑
1])2+(y
r
[i]
‑
y
r
[i
‑
1])2)，i＝1，2，...n
ꢀꢀ
(17)
[0153]
以及道路航向角(初始值)
[0154][0155]
依据三次样条曲线插值方法获得笛卡尔横向x坐标与路径向量s的函数关系式，笛卡尔纵向y坐标与路径向量s的函数关系式，以及道路中心线航向角θ
r
与路径向量s的关系，
[0156][0157]
计算道路中心线上对应采样点的法向向量e
n
＝(χ，σ)，其中
[0158][0159]
类似的，同样构建道路中心线法向向量分量与路程的函数关系，即
[0160][0161]
任意frenet坐标(s，l)和相应的笛卡尔坐标(x，y)的转化关系为：
[0162][0163]
步骤2：联合笛卡尔坐标和frenet坐标描述路径
[0164]
假定路径由n+1个控制节点构成n段连续短路径，且各节点之间的纵向距离保持均匀分布，假若在此案例中s＝[0，2.5，5，...，120]
t
，n＝48，依据式(22)将路径节点(x，y)表述成向量形式：
[0165][0166]
其中依据(21)计算
[0167]
k
1，i
＝f
rχ
(s[i])，k
2，i
＝f
rσ
(s[i])
[0168]
diag[
·
]表示根据括号中的元素生成对角矩阵，l＝[l
1 l2ꢀ…ꢀ
l
n l
n+1
]
t
是每个路径节点在frenet坐标系下的横坐标，l为路径的决策向量。
[0169]
步骤3：将路径长度、曲率、航向角误差表示成二次形式；
[0170]
将路径长度、曲率、航向角误差表示为二次型，即变量l的最高次为二次。
[0171]
依据式(7)把路径平方项表示为
[0172][0173]
其中，
[0174][0175]
α
i
＝[
‑
k
1i k
1，i+1
]，β
i
＝[
‑
k
2，i k
2，i+1
]，γ
i
＝[α
i β
i
]
t
，
[0176][0177]
依据式(12)把曲率平方项表示为
[0178]
k2＝l
t
nl+tl+cost
ꢀꢀꢀ
(25)
[0179]
其中，
[0180]
n＝h
t
h
[0181]
t＝2(x
rt
h
t
h)diag[χ
i
]+2(y
rt
h
t
h)diag[σ
i
]
[0182][0183][0184]
依据式(14)把路径航向角误差平方项表示为
[0185]
p2＝l
t
ql+pl+cost
ꢀꢀꢀ
(26)
[0186]
其中，
[0187][0188][0189]
步骤4：采用二次最优规划方法进行避障路径规划
[0190]
如图7所示，假定障碍物处于主车同一车道距离前方50m处，其中心的侧向位移距离道路中心线为l
obst
＝
‑
2m，路径的目标点设置为前方120m的距离，根据当前的位置关系大致估算与障碍物最近的控制点序号(其中ceil为向上取整函数)，加上该点纵向位移前后安全距离d＝20m内附近个控制点，它们的侧向位移至少要比障碍物的侧向位移多出一安全距离阈值r
safety
＝2m，从而保证避障路径不会与障碍物发生碰撞。d
r
和d
l
分别设为
‑
2.5m和2.5m，最终避障路径规划问题可表述为如下的二次形式：
[0191][0192]
s.t.d
r
≤l
k
≤d
l
，k＝1，2，...，11，29，...，49
[0193]
0＝l
obst
+r
safety
≤l
k
≤d
l
，k＝12，...，20，...28
[0194]
对应的权重分别取p1＝0.1，p2＝0.8，p3＝0.1，在matlab中采用qp问题求解二次规划优化函数(quadprog)得到最优路径，然后再次利用式(23)将路径frenet坐标转换为笛卡尔坐标。
[0195]
请参阅图7，为本申请实施例中曲线道路上的避障路径规划示意图；
[0196]
本申请提出的路径规划算法能成功地规划一条安全路径避过障碍，并返回原车道到达目的点，整个避障路径形状也非常自然。与考虑路径航向约束的改进rrt算法进行对比，从整体规划效果视觉而言，本申请方案的规划效果是最为自然的，路径过渡平滑。
[0197]
如表1所示，在路径长度、路径曲率均值、侧向安全(与障碍物中心的侧向距离均值)路径质量指标值分布进行对比可知，本申请方案的路径质量也是最优的，在路径长度最短、路径曲率更优且侧向安全值更高。
[0198]
表1、路径指标对比
[0199]
规划方法本申请方案改进rrt路径长度120.37120.57曲率均值0.00570.0202侧向安全2.16351.7737
[0200]
此外，如表2所示，两种方法的整体规划的求解速度进行对比，经测试本申请规划方法仅耗时22ms，远低于改进rrt的规划方法耗时。
[0201]
表2、规划求解耗时对比
[0202]
规划方法本申请方案改进rrt求解耗时22ms125ms
[0203]
本申请实施例提供的路径规划方法所生成的避障路径形状自然，各项路径质量指标占优，且求解时间最短。
[0204]
上面介绍了本申请提供的路径规划方法，下面对实现该路径规划方法的路径规划装置进行介绍，请参阅图8，为本申请实施例中路径规划装置的一个实施例示意图。
[0205]
该路径规划装置包括：
[0206]
获取模块801，用于获取道路信息、目标对象的初始位置和目的位置；
[0207]
确定模块802，用于根据所述道路信息确定笛卡尔坐标系、frenet坐标系以及所述笛卡尔坐标系与所述frenet坐标系的映射关系；
[0208]
所述获取模块801，还用于基于所述映射关系获取基于frenet坐标系的路径质量指标函数，所述路径质量指标函数为二次型函数；
[0209]
所述确定模块802，还用于根据所述基于frenet坐标系的路径质量指标函数、所述目标对象的初始位置和所述目的位置，利用二次最优规划确定基于所述frenet坐标系的目标路径；
[0210]
转换模块803，用于根据所述映射关系，将所述目标路径转换到所述笛卡尔坐标系中，所述目标路径用于所述目标对象的路径规划控制。
[0211]
可选地，所述确定模块802具体用于：根据所述道路信息确定道路边界约束条件；在所述道路边界约束条件的约束下，利用二次规划解法确定基于所述frenet坐标系的目标路径。
[0212]
可选地，所述获取模块801还用于：获取障碍物信息；所述确定模块802具体用于：
在根据所述障碍物信息确定的障碍物约束条件下，利用二次规划解法确定基于所述frenet坐标系的目标路径。
[0213]
可选地，所述确定模块802具体用于：利用二次规划解法确定所述目标路径上的多个控制节点；根据所述多个控制节点，以插值方式确定所述目标路径上除所述多个控制节点之外的任一路径点。
[0214]
可选地，所述插值方式包括自然三次样条曲线插值。
[0215]
可选地，所述路径质量指标函数包括路径长度的函数、曲率的函数、航向角误差的函数、曲率变化率的函数和航向角误差变化率的函数中的一个或多个的组合。
[0216]
可选地，所述获取模块801具体用于：根据预设的多个所述路径质量指标的权重确定所述路径质量指标函数。
[0217]
可选地，所述笛卡尔坐标系包括：车身坐标系、站心坐标系或世界坐标系。
[0218]
图8中的各个模块的只一个或多个可以软件、硬件、固件或其结合实现。所述软件或固件包括但不限于计算机程序指令或代码，并可以被硬件处理器所执行。所述硬件包括但不限于各类集成电路，如中央处理单元(cpu)、数字信号处理器(dsp)、现场可编程门阵列(fpga)或专用集成电路(asic)。
[0219]
请参阅图9，本申请实施例中路径规划装置的一个实施例示意图。
[0220]
如图9所示，路径规划装置900包括存储器901和处理器902。所述存储器901存储计算机程序指令，所述处理器902运行所述计算机程序指令以执行上述实施例描述的路径规划相关操作。所述处理器902还与路径规划装置900外界的一个或多个传感器相连接，接收所述传感器探测的自车周围环境的原始数据。所述传感器包括但不限于如摄像头、激光雷达、超声波雷达或毫米波雷达。路径规划装置900输出的目标路径一般发送给智能驾驶车辆的底层控制模块，提供控车参考信息。底层控制模块也可以是由处理器902执行的一个软件模块或集成于处理器902中，本实施例不做限定。处理器902包括但不限于各类cpu、dsp、微控制器、微处理器或人工智能处理器。
[0221]
上述图8、图9所示的路径规划装置，通过构建笛卡尔坐标系和frenet坐标系的映射关系，将路径质量指标的优化目标确定为二次规划问题，可以简便的获取基于frenet坐标系的目标路径，最后，还可以根据该映射关系将目标路径转换回笛卡尔坐标系，便于底层控制模块的执行控制操作。本方案通过二次规划解法可以获取路径质量指标最优的目标路径，可以改善目标路径质量，获取的目标路径更符合运动目标对象尤其是车辆的实际运动场景。