路径轨迹规划方法、终端及计算机可读存储介质与流程

1.本发明涉及自动驾驶技术领域，尤其涉及一种路径轨迹规划方法、终端及计算机可读存储介质。


背景技术：

2.目标车辆辆可以减轻驾驶员的驾驶压力，以及相关责任，依靠自身的传感器和处理器获取驾驶信息，将车辆导航到目标位置；决策规划是目标车辆辆系统的关键部分，为了处理驾驶任务，需要依靠决策规划给出安全，舒适，合理的轨迹，但决策规划需要依靠一些复杂的算法计算规划轨迹，给处理器实时处理，因此决策规划执行的时间至关重要。


技术实现要素：

3.有鉴于此，本发明提供了一种路径轨迹规划方法、终端及计算机可读存储介质，加快了求解非线性规划问题的速度，缩短了路径轨迹规划的时间。
4.本技术提供了一种路径轨迹规划方法，包括以下步骤：
5.确定目标车辆的预设轨迹点序列，所述预设轨迹点序列用于输入双循环迭代算法作为初始热启动项；
6.确定sv限速的非线性函数，所述sv限速的非线性函数用于计算所述双循环迭代算法的速度约束条件；
7.启动双循环迭代算法对所述预设轨迹点序列优化求解，以输出目标路径轨迹点序列。
8.可选地，所述确定目标车辆的预设轨迹点序列，包括：
9.根据目标车辆的位置信息、出行任务以及地图信息确定至少一路径参考线；
10.根据障碍物信息对所述参考线进行横向规划和/或纵向规划，以生成所述目标车辆从当前位置到期望位置的时空线路；
11.对所述时空线路进行优化求解，得到预设轨迹点序列。
12.可选地，所述对所述时空线路进行优化求解，包括：
13.根据第一目标函数和预设约束条件对所述时空线路进行优化求解；
14.所述第一目标函数为：
15.总cost＝位置cost 速度cost 加速度cost 加加速度cost 跟车项cost
16.其中，位置cost为求解的每个位置与sl图中纵向位移线路上每个位置sr差值的平方乘以权重w_s；速度cost为求解的每个速度与sl图中纵向位移线路上每个速度vr差值的平方乘以权重w_v；加速度cost为求解的每个加速度的平方乘以权重w_a；加加速度cost为求解的每个加加速度的平方乘以权重w_jerk；跟车项cost为跟车目标的每个sl图中纵向坐标位置减去求解的每个位置与跟停距离d_stop，再减去目标车辆与跟车目标碰撞所需的时间与速度差，速度差为求解的每个速度乘以2减去跟车目标的速度。
17.可选地，所述确定sv限速的非线性函数，包括：
18.根据预设限速信息和目标车辆的位置信息确定sv限速，所述预设限速信息包括地图信息、场景信息和风险信息中至少一项；
19.对所述sv限速进行平滑处理，得到sv限速的非线性函数。
20.可选地，所述对所述sv限速进行平滑处理，包括：
21.根据第二目标函数对所述sv限速进行平滑处理；
22.所述第二目标函数为：
23.sv目标函数＝速度成本 速度对位置一阶导的成本 速度对位置二阶导的成本 速度对位置三阶导的成本
24.其中，速度成本为求解速度v_o与sv限速上的每个v的差的平方乘以权重w_0；速度对位置一阶导的成本为求解的速度v_o对sv限速中的每个位置一阶导的平方乘以权重w_1；速度对位置二阶导的成本为求解的速度v_o对sv限速中的每个位置二阶导的平方乘以权重w_2；速度对位置三阶导的成本为求解的速度v_o对sv限速中的每个位置三阶导的平方乘以权重w_3。
25.可选地，所述启动双循环迭代算法对所述预设轨迹点序列优化求解，包括：
26.启动外循环，在所述外循环中运行内循环；
27.在所述内循环中迭代更新所述预设轨迹点序列；
28.在所述外循环中根据内循环中相邻两次的求解结果进行收敛判断以及松弛变量调整；
29.外循环结束时，将求解结果输出作为目标路径轨迹点序列。
30.可选地，所述迭代更新所述预设轨迹点序列，包括：
31.根据第三目标函数和预设约束条件对所述预设轨迹点序列进行优化求解，所述第三目标函数用于对位置代价、速度代价、加速度代价、加加速度代价、跟车项代价、位置松弛项代价和速度松弛项代价中一项或多项求解；
32.若求解成功，则将求解结果序列作为热启动项，并设定对偶变量，进行下一次迭代；
33.若内循环中相邻两次的求解结果序列满足内循环收敛条件阈值，则跳出内循环。
34.可选地，所述根据内循环中相邻两次的求解结果进行收敛判断以及松弛变量调整，包括：
35.若相邻两次内循环的求解结果序列满足外循环收敛条件阈值，则跳出外循环；
36.若内循环输出的松弛变量超出预设阈值，则增大所述第三目标函数中松弛变量的权重。
37.本技术还提供一种终端，包括：存储器、处理器，其中，存储器上存储有速度规划程序，速度规划程序被处理器执行时实现如上所述的路径轨迹规划方法的步骤。
38.本技术还提供一种计算机存储介质，计算机存储介质上存储有计算机程序指令；计算机程序指令被处理器执行时实现如上所述的路径轨迹规划方法。
39.综上所述，本发明提供的路径轨迹规划方法、终端及计算机可读存储介质，方法包括：确定目标车辆的预设轨迹点序列，预设轨迹点序列用于输入双循环迭代算法作为初始热启动项；确定sv限速的非线性函数，sv限速的非线性函数用于计算双循环迭代算法的速度约束条件；启动双循环迭代算法对预设轨迹点序列优化求解，以输出目标路径轨迹点序
列。本技术通过构建双循环迭代算法框架，加快了求解非线性规划问题的速度，缩短了路径轨迹规划的时间。
40.上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
附图说明
41.图1为本发明实施例示出的路径轨迹规划方法的流程示意图；
42.图2为本发明实施例示出的sv限速的关系示意图；
43.图3为本发明实施例示出的双循环迭代算法的逻辑示意图；
44.图4为本发明实施例示出的路径轨迹规划方法的具体流程示意图；
45.图5为本发明实施例示出的路径轨迹规划系统的结构示意图。
具体实施方式
46.为更进一步阐述本发明为达成预定发明目的所采取的技术手段及功效，以下结合附图及较佳实施例，对本发明详细说明如下。
47.第一实施例
48.图1为根据本发明实施例示出的路径轨迹规划方法的流程示意图。
49.请参考图1，本发明实施例提供了一种路径轨迹规划方法，包括：
50.步骤201，确定目标车辆的预设轨迹点序列，预设轨迹点序列用于输入双循环迭代算法作为初始热启动项。
51.在一实施方式中，确定目标车辆的预设轨迹点序列，包括：
52.根据目标车辆的位置信息、出行任务以及地图信息确定至少一路径参考线；
53.根据障碍物信息对参考线进行横向规划和/或纵向规划，以生成目标车辆从当前位置到期望位置的时空线路；
54.对时空线路进行优化求解，得到预设轨迹点序列。
55.本实施例中，路径规划系统用于实现针对目标车辆的路径轨迹规划方法。路径轨迹规划系统包括但不限于感知模块，定位模块，地图模块，预测模块，路由模块，决策和规划模块，控制模块。其中，感知模块确定目标车辆周围的环境信息，包括但不限于周围的障碍物信息(如车辆，行人，锥桶，非机动车等)，结合定位信息和地图信息能确定交通信号灯，交通标志，车道信息等；定位模块确定目标车辆当前的位置，管理目标车辆的行程数据；地图模块存储地图信息，包括但不限于道路形状，车道信息，合流和分流信息，交叉口信息，人行横道信息，减速带等；路由模块基于定位模块和地图模块，以及目标车辆的任务，生成全局规划的路径信息；预测模块根据感知模块中的障碍物信息以及地图模块中的地图信息，生成障碍物的预测轨迹；决策规划模块根据预测模块的障碍物预测轨迹，定位模块的自车定位以及路由模块的目标车辆的出行任务，生成满足车辆动力学约束，安全，舒适的轨迹；控制模块根据决策规划模块的轨迹，输出相应的方向盘转角，驱动或制动力矩，进行车辆的轨迹控制。
56.当决策规划模块接收到路由模块的路径信息以及pncmap(plan and cotrol map，
规划与控制地图)，根据定位模块给出的当前位置生成一条或多条运动参考线，该参考线为路径信息的部分，或路径临车道可通行的局部路径。决策规划模块接收预测模块的障碍物预测轨迹和自身定位后，横向决策和纵向决策对各障碍物做出决策，不限于横纵向决策的先后顺序。当对障碍物的决策形成后，横向规划规划出目标车辆当前位置到期望位置的路径，该路径在参考线基础上进行横向的拉升，能够对障碍物进行避让；纵向规划模块根据横向规划的路径以及障碍物的运动轨迹，生成st图，并根据对障碍物的决策形成关于时间t的s上下边界，即每个时间t下的s上边界s_max以及每个时间t下的下边界s_min；在关于时间t的上下边界内，不限于决策方法和动态规划方法，生成目标车辆从当前位置到期望位置的时空路线。
57.在一实施方式中，当纵向规划模块位于横向规划模块前面时，纵向规划模块根据横向决策的的粗略路径或参考线路径以及障碍物的运动轨迹，生成st图，并根据对障碍物的决策形成关于时间t的s上下边界，即每个时间t下的s上边界s_max以及每个时间t下的下边界s_min；在关于时间t的上下边界内，不限于决策方法和动态规划方法，生成目标车辆从当前位置到期望位置的时空路线。
58.在一实施方式中，对时空线路进行优化求解，包括：
59.根据第一目标函数和预设约束条件对时空线路进行优化求解；
60.第一目标函数为：
61.总cost＝位置cost 速度cost 加速度cost 加加速度cost 跟车项cost
62.其中，位置cost为求解的每个位置与sl图中纵向位移线路上每个位置sr差值的平方乘以权重w_s；速度cost为求解的每个速度与sl图中纵向位移线路上每个速度vr差值的平方乘以权重w_v；加速度cost为求解的每个加速度的平方乘以权重w_a；加加速度cost为求解的每个加加速度的平方乘以权重w_jerk；跟车项cost为跟车目标的每个sl图中纵向坐标位置减去求解的每个位置与跟停距离d_stop，再减去目标车辆与跟车目标碰撞所需的时间与速度差，速度差为求解的每个速度乘以2减去跟车目标的速度。
63.本实施例中，根据得到的目标车辆从当前位置到期望位置的时空路线，利用目标函数根据二次规划优化，以产生路径轨迹。考虑的约束包括加加速度约束，加速度约束，速度约束，跟车项约束。其中加加速度约束，加速度约束，速度约束为求解的每个加加速度，加速度，速度都应在车辆的动力性能范围内；跟车项约束为求解的位置与跟车目标的距离在安全距离d_safe范围内。根据目标函数和约束计算出轨迹点序列，输入到双循环作为内循环的初始热启动项。这样，第一目标函数对时空路线优化时增加跟车项，解决现有技术中跟车时与前车间距难以自适应调整的问题，能适应不同速度下跟车时距和不同习惯驾驶的驾驶员的跟车习惯。通过在轨迹优化的目标函数中增加了跟车项，考虑了车辆在跟车中的安全距离和跟停距离，能自适应调整跟车距离，同时保证了跟车的安全性。
64.步骤202，确定sv限速的非线性函数，sv限速的非线性函数用于计算双循环迭代算法的速度约束条件。
65.在一实施方式中，确定sv限速的非线性函数，包括：
66.根据预设限速信息和目标车辆的位置信息确定sv限速，预设限速信息包括地图信息、场景信息和风险信息中至少一项；
67.对sv限速进行平滑处理，得到sv限速的非线性函数。
68.本实施例中，目标车辆根据所处位置，考虑但不限于基于地图信息的限速，基于场景识别的限速以及基于风险信息的限速。如表1所示，基于地图信息的限速包括但不限于道路限速，斑马线限速，减速带限速，路口限速，坡道限速，曲率限速；基于场景识别的限速包括但不限于临车道拥堵限速，斑马线上行人限速，上一周期的运动趋势限速，openspace信息限速，斑马线前静止车辆限速；基于风险信息的限速包括但不限于感知盲区限速，临车道减速的车辆限速，车辆系统故障限速，功能开启的环境限速。考虑基于地图信息，场景识别，风险信息等限速，并融合为关于位置和速度的非线性约束，有助于在各种场景下完成基于位置的限速，保证行驶安全性。本实施例中，还可对odd(operational design domain，设计运行区域)内感兴趣位置做相应的限速，然后求解关于速度约束的非线性问题，以解决现有技术无法在感兴趣位置做出限速的问题。
69.表1
[0070][0071]
值得一提的是，根据上述限速条件最终生成的限速为，上述各个限速信息对应的限速值在每个取样位置限速中的最小值，取样点数记为n，那么每个取样位置s_point对应一个速度上限v_limit，s_point的取样数n根据选取的间距delta_s决定，n为目标车辆当前位置到期望位置的s差值与delta_s的比例，最终输出为如图2所示的一系列sv限速。
[0072]
在一实施方式中，对sv限速进行平滑处理，包括：
[0073]
根据第二目标函数对sv限速进行平滑处理；
[0074]
第二目标函数为：
[0075]
sv目标函数＝速度成本 速度对位置一阶导的成本 速度对位置二阶导的成本 速度对位置三阶导的成本
[0076]
其中，速度成本为求解速度v_o与sv限速上的每个v的差的平方乘以权重w_0；速度对位置一阶导的成本为求解的速度v_o对sv限速中的每个位置一阶导的平方乘以权重w_1；速度对位置二阶导的成本为求解的速度v_o对sv限速中的每个位置二阶导的平方乘以权重w_2；速度对位置三阶导的成本为求解的速度v_o对sv限速中的每个位置三阶导的平方乘以权重w_3。
[0077]
本实施例中，对于sv限速，为获取每个点的高阶导数，并用非线性函数表示，利用目标函数根据二次规划优化，生成平滑的sv函数。如图2，由于sv限速中的s和v是非线性的关系，为用一个连续的函数表示该函数，用二次规划优化平滑处理。
[0078]
步骤203，启动双循环迭代算法对预设轨迹点序列优化求解，以输出目标路径轨迹点序列。
[0079]
请参考图3，本实施例中，启动双循环，对目标车辆从当前位置到期望位置的时空路线进行最终的优化，求出平滑满足车辆运行的轨迹。在启动双循环前，设置外循环的迭代次数iters_out和内循环迭代次数iters_in，位置松弛变量阈值s_threshold和速度松弛变量阈值v_threshold，以及外循环收敛条件阈值violate_out和内循环收敛条件阈值violate_in。启动双循环时先启动外循环，外循环里主要运行内循环，以及收敛判断和松弛约束判断。这样，通过建立了轻量级的双循环迭代框架，在双循环里设立收敛条件，热启动项以及对对偶变量的更改，加快了求解非线性规划问题的速度。
[0080]
在一实施方式中，启动双循环迭代算法对预设轨迹点序列优化求解，包括：
[0081]
启动外循环，在外循环中运行内循环；
[0082]
在内循环中迭代更新预设轨迹点序列；
[0083]
在外循环中根据内循环中相邻两次的求解结果进行收敛判断以及松弛变量调整；
[0084]
外循环结束时，将求解结果输出作为目标路径轨迹点序列。
[0085]
在一实施方式中，迭代更新预设轨迹点序列，包括：
[0086]
根据第三目标函数和预设约束条件对预设轨迹点序列进行优化求解，第三目标函数用于对位置代价、速度代价、加速度代价、加加速度代价、跟车项代价、位置松弛项代价和速度松弛项代价中一项或多项求解；
[0087]
若求解成功，则将求解结果序列作为热启动项，并设定对偶变量，进行下一次迭代；
[0088]
若内循环中相邻两次的求解结果序列满足内循环收敛条件阈值，则跳出内循环。
[0089]
本实施例中，在内循环中主要将非线性问题在各个位置进行线性展开，解决多个线性化的问题，使线性化问题的解逼近非线性问题的解。第一目标函数中求解的预设轨迹点序列作为初始热启动项，根据初始热启动项的位置、速度、加速度更新二次规划的约束矩阵和约束项。
[0090]
值得一提的是，在内循环中引入了松弛变量，包括速度松弛变量和位置松弛变量，在求解非线性问题时，设置位置和速度的松弛变量，使优化求解结果的位置不会因为超出位置上限导致求解失败，也不会因为优化结果的速度超出速度上限导致求解失败，增加求解成功率，能够应对车速超限速情况和位置上限非硬性需求的情况。
[0091]
具体地，第三目标函数为：
[0092]
总cost’＝位置cost’ 速度cost’ 加速度cost’ 加加速度cost’ 跟车项cost’ 位置松弛项cost’ 速度松弛项cost’[0093]
其中，位置cost’为求解的每个位置与sl图中纵向位移线路上每个位置sr差值的平方乘以权重w_s；速度cost’为求解的每个速度与sl图中纵向位移线路上每个速度vr差值的平方乘以权重w_v；加速度cost’为求解的每个加速度的平方乘以权重w_a；加加速度cost’为求解的每个加加速度的平方乘以权重w_jerk；跟车项cost’为跟车目标的每个sl图中纵向坐标位置减去求解的每个位置与跟停距离d_stop，再减去目标车辆与跟车目标碰撞所需的时间与速度差，速度差为求解的每个速度乘以2减去跟车目标的速度；位置松弛项cost’为位置松弛变量的平方乘以权重w_slack_s；速度松弛项cost’为速度松弛变量的平
方乘以权重w_slack_v。
[0094]
考虑的约束包括加加速度约束，加速度约束，速度约束，位置约束，变量等式约束和跟车项约束。其中加加速度约束，加速度约束为求解的每个加加速度，加速度都应在车辆的动力性能范围内；速度约束有两项：一项是速度大于0，一项是速度与速度松弛变量的差值小于在热启动点每个位置的一阶泰勒展开式；位置约束也包括两部分，一部分是求解的位置大于位置下限smin，另一部分是求解的位置与位置松弛变量的差值小于位置上限smax与安全距离d_safe的差值；变量等式约束包括速度与加加速度的等式关系，以及位置与加加速度的等式关系，见下式：
[0095]vt t
＝v
t
a
t
*t 0.5*j
t
*t2[0096]s t
＝s
t
v
t
0.5*t2 1/6*j
t
*t3[0097]
其中，v
t t，vt
，s
t t
，s
t
，a
t
，j
t
，t分别为t t时刻速度，t时刻速度，t t时刻位置，t时刻位置，t时刻加速度，t时刻加加速度，时间间隔。
[0098]
若求解成功，将求解结果作为热启动项，并设定对偶变量，进行下一次迭代。在内循环的收敛判断为，相邻两次内循环的求解结果序列中，每个位置差，速度差累计值小于阈值violate_in，即判定收敛，跳出内循环。对于优化器报错，如出现越界、无解等情形，则同时跳出内外循环，结束双循环迭代，并将此时的求解结果输出作为目标路径轨迹点序列。
[0099]
在一实施方式中，根据内循环中相邻两次的求解结果进行收敛判断以及松弛变量调整，包括：
[0100]
若相邻两次内循环的求解结果序列满足外循环收敛条件阈值，则跳出外循环；
[0101]
若内循环输出的松弛变量超出预设阈值，则增大第三目标函数中松弛变量的权重。
[0102]
本实施例中，外循环中收敛性判断为：对于相邻两次非线性求解结果序列，每个位置差，速度差累计值小于阈值violate_out，即判断收敛，跳出外循环，否则继续外循环。
[0103]
外循环松弛变量判断：位置松弛变量超出阈值s_threshold时，增加位置松弛变量权重w_slack_s；速度松弛变量超出阈值v_threshold时，增加速度松弛变量权重w_slack_v。
[0104]
当外循环结束时，其求解结果作为最终的路径轨迹点，将目标车辆从当前位置导航到目标位置。
[0105]
本发明实施例的路径轨迹规划方法，包括：确定目标车辆的预设轨迹点序列，预设轨迹点序列用于输入双循环迭代算法作为初始热启动项；确定sv限速的非线性函数，sv限速的非线性函数用于计算双循环迭代算法的速度约束条件；启动双循环迭代算法对预设轨迹点序列优化求解，以输出目标路径轨迹点序列。本技术的路径轨迹规划方法通过构建双循环迭代算法框架，加快了求解非线性规划问题的速度，缩短了路径轨迹规划的时间。
[0106]
第二实施例
[0107]
请参考图4，本实施例的路径轨迹规划方法中，在odd区域内确定目标车辆从当前位置到期望位置的一条时空路线。对于该路线，利用目标函数根据二次规划优化，以产生路径轨迹，作为热启动值输入到双循环中；目标车辆根据所处位置进行当前位置到期望位置的sv限速；对于sv限速，利用目标函数根据二次规划优化，生成平滑的sv函数，输入到双循环中；启动双循环，外循环里执行内循环，根据内循环结果进行收敛判断和松弛约束判断，
内循环中利用目标函数进行非线性二次规划优化，以产生满足车辆运动和约束的路径轨迹；利用双循环框架优化出路径轨迹将目标车辆从当前位置导航到期望位置。
[0108]
本实施例执行上述方法步骤的具体过程，详见上述实施例的相关描述，在此不再赘述。
[0109]
第三实施例
[0110]
请参考图5，本发明实施例还提供一种路径轨迹规划系统，其特征在于，包括感知模块、定位模块、地图模块、预测模块、路由模块、决策规划模块和控制模块；其中，
[0111]
感知模块，用于获取输入信息，包括障碍物信息、目标车辆的定位信息、地图信息；
[0112]
路由模块，用于根据目标车辆的出行任务生成全局规划的路径信息；
[0113]
预测模块，用于根据障碍物信息和地图信息生成障碍物的预测轨迹；
[0114]
决策规划模块，用于根据预测轨迹、位置信息和出行任务，生成目标轨迹；
[0115]
控制模块，用于根据目标轨迹控制目标车辆行驶。
[0116]
目标车辆的路径轨迹规划系统，包括但不限于感知模块，定位模块，地图模块，预测模块，路由模块，决策和规划模块，控制模块。其中，感知模块确定目标车辆周围的环境信息，包括但不限于周围的障碍物信息(如车辆，行人，锥桶，非机动车等)，结合定位信息和地图信息能确定交通信号灯，交通标志，车道信息等；定位模块确定目标车辆当前的位置，管理目标车辆的行程数据；地图模块存储地图信息，包括但不限于道路形状，车道信息，合流和分流信息，交叉口信息，人行横道信息，减速带等；路由模块基于定位模块和地图模块，以及目标车辆的任务，生成全局规划的路径信息；预测模块根据感知模块中的障碍物信息以及地图模块中的地图信息，生成障碍物的预测轨迹；决策规划模块根据预测模块的障碍物预测轨迹，定位模块的自车定位以及路由模块的目标车辆的出行任务，生成满足车辆动力学约束，安全，舒适的轨迹；控制模块根据决策规划模块的轨迹，输出相应的方向盘转角，驱动或制动力矩，进行车辆的轨迹控制。
[0117]
本实施例执行上述方法步骤的具体过程，详见上述实施例的相关描述，在此不再赘述。
[0118]
本技术还提供一种终端，包括：存储器、处理器，其中，存储器上存储有速度规划程序，速度规划程序被处理器执行时实现如上所述的路径轨迹规划方法的步骤。
[0119]
本实施例执行上述方法步骤的具体过程，详见上述实施例的相关描述，在此不再赘述。
[0120]
本技术还提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序指令；计算机程序指令被处理器执行时实现如上所述的路径轨迹规划方法。
[0121]
本实施例执行上述方法步骤的具体过程，详见上述实施例的相关描述，在此不再赘述。
[0122]
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0123]
在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，除了包含所列的那些要素，而且还可包含没有明确列出的其他要素。
[0124]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何
熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。