一种分阶段的自动驾驶车辆轨迹规划方法

1.本发明属于自动驾驶技术领域，尤其涉及一种分阶段的自动驾驶车辆轨迹规划方法。


背景技术：

2.规划模块是自动驾驶的关键模块，需要综合考虑感知、预测、地图信息以及车辆状态，输出安全且舒适的轨迹。目前有很多关于自动驾驶车辆轨迹规划的研究工作，根据侧重点的不同，也会有不同的分类方式。
3.根据轨迹生成方式的不同，轨迹规划可以分为基于采样的方法和基于优化的方法，基于采样的方法通过对状态空间进行采样生成大量候选轨迹，然后根据预先设计的评价指标选择一条最优的轨迹。基于采样的方法实现简单，实时性高，但规划的结果容易受到采样间隔的影响，采样间隔过大则生成的轨迹过于粗糙，采样间隔过过小，则计算量显著增加，不利于满足实时性要求。与之相对应的是基于优化的方法，该方法通过建立具有约束条件的最小代价问题，通过对该问题的求解得到满足约束条件的轨迹。基于优化的方法不受采样方式的限制，代价函数和约束条件可以根据场景的不同而调整，具有更高的灵活性，所以更适合应对复杂场景。然而，基于优化的方法往往需要求解复杂的非线性规划问题，需要更多的计算资源才能得到最优解，所以当问题的规模较大时，很难保证满足实时性的要求。
4.根据坐标系的不同，轨迹规划可以分为基于frenet坐标系的方法和基于笛卡尔坐标系的方法。frenet坐标系是一种曲线坐标系，frenet坐标系能够很好地简化道路曲率，从而使表述更为简介、直观。由于frenet忽略了道路曲率，在曲率较大的场景下无法准确描述车辆的运动学模型。而在笛卡尔坐标系下虽然可以准确描述车辆的运动学模型，得到更符合车辆与运动学模型的轨迹，但车道边界和障碍物在笛卡尔坐标系下的描述复杂，不够简洁，使得规划的约束求解困难，计算量显著增加。


技术实现要素：

5.本发明实施例的目的在于提供一种分阶段的自动驾驶车辆轨迹规划方法，旨在解决背景技术中提出的问题。
6.为实现上述目的，本发明实施例提供如下技术方案：
7.一种分阶段的自动驾驶车辆轨迹规划方法，所述方法具体包括以下步骤：
8.搜索阶段，在frenet坐标系下进行采样，使用五次多项式连结采样点，形成有向无环图，动态规划搜索得到参考轨迹，构建可行驶通道；
9.优化阶段，建立车辆运动学模型，根据所述可行驶通道，构建代价函数，建立边界约束，使用ilqr算法进行时空联动优化，输出最优轨迹。
10.作为本发明实施例技术方案进一步的限定，所述搜索阶段，在frenet坐标系下进行采样，使用五次多项式连结采样点，形成有向无环图，动态规划搜索得到参考轨迹，构建可行驶通道具体包括以下步骤：
11.获取自动驾驶车辆的处地图信息，构建frenet坐标系，获取障碍物信息和自动驾驶车辆信息，并投影至所述frenet坐标系；
12.在所述frenet坐标系进行采样，得到采样点，使用五次多项式连结采样点，并根据预设的代价函数，计算相应的代价；
13.进行动态规划搜索，得到搜索阶段的最优路径，进行膨胀得到可行驶通道，并转化为笛卡尔坐标系。
14.作为本发明实施例技术方案进一步的限定，所述使用五次多项式连结采样点的公式为：
15.s∈[0,δs]
[0016]
其中，是五次多项式的参数，是五次多项式的自变量。
[0017]
作为本发明实施例技术方案进一步的限定，所述代价函数为：
[0018]cpath
＝c
ref
+c
smooth
+c
obs
[0019]
其中，c
path
为路径代价，c
ref
为参考线偏移代价，c
smooth
为平滑性代价，c
obs
为障碍物代价。
[0020]
作为本发明实施例技术方案进一步的限定，所述参考线偏移代价的公式为：
[0021][0022]
其中，w0为权重系数，l(s)为偏移程度。
[0023]
作为本发明实施例技术方案进一步的限定，所述平滑性代价的公式为：
[0024][0025]
其中，w1,w2,w3均为权重系数，分别是l(s)的一阶，二阶，三阶导数。
[0026]
作为本发明实施例技术方案进一步的限定，所述障碍物代价的公式为：
[0027][0028]
作为本发明实施例技术方案进一步的限定，所述车辆运动学模型中车辆的运动状态方程为：
[0029]vi+1
＝vi+aiδ
t
[0030]
θ
i+1
＝θi+ωiδ
t
[0031][0032][0033]
其中，为ti时刻的状态量，为笛卡尔坐标系下的坐标点，
vi和θi分别为车速和航向角，ai为加速度。
[0034]
作为本发明实施例技术方案进一步的限定，所述代价函数包括控制代价函数和状态代价函数，具体的：
[0035]
控制代价函数为：
[0036][0037]
其中，ju为时控制代价，ω
acc
,ω
ω
均为权重系数，ai,ωi分别为加速度和横摆角速度；
[0038]
状态代价函数为：
[0039][0040]
其中，(r
ix
,r
iy
)为点在参考路径上的投影点，vr为自动驾驶车辆的参考速度，w
ref
,w
vel
均为权重系数，j
x
为状态代价。
[0041]
作为本发明实施例技术方案进一步的限定，所述边界约束的表达式为：
[0042][0043]
与现有技术相比，本发明的有益效果是：
[0044]
1、采用分阶段的规划方法，分为搜索阶段和优化阶段，结合了基于采样和基于优化两种方法的优点，同时也兼顾到frenet坐标系和笛卡尔坐标系的优势；
[0045]
2、解决了时空联合轨迹规划中计算复杂，难以满足实时性的问题；
[0046]
3、优化方式是时空耦合的，所以更容易满足动态场景的要求，解决了解耦类方法容易陷入局部最优解，甚至无解的问题；
[0047]
4、优化阶段是在笛卡尔坐标系下进行的，解决了frenet坐标系下无法建立车辆运动学模型的问题。
附图说明
[0048]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例。
[0049]
图1示出了本发明实施例提供的方法的流程示意图。
[0050]
图2示出了本发明实施例提供的方法中车辆运动学模型的示意图。
[0051]
图3示出了本发明实施例提供的方法中建立边界约束的示意图。
具体实施方式
[0052]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0053]
可以理解的是，现有的自动驾驶车辆轨迹规划技术中，具有以下缺点：(1)基于采样的方法，规划的结果容易受到采样间隔的影响，采样间隔过大则生成的轨迹过于粗糙，采
样间隔过过小，则计算量显著增加，不利于满足实时性要求；(2)基于优化的方法，往往需要求解复杂的非线性规划问题，需要更多的计算资源才能得到最优解，所以当问题的规模较大时，很难保证满足实时性的要求；(3)基于frenet坐标系的轨迹规划，忽略了道路曲率，在曲率较大的场景下无法准确描述车辆的运动学模型；(4)基于笛卡尔坐标系的轨迹规划，车道边界和障碍物的描述复杂，不够简洁，使得规划的约束求解困难，计算量显著增加。
[0054]
为解决上述问题，本发明实施例通过在搜索阶段，进行frenet坐标系下的采样，使用五次多项式连结采样点，动态规划搜索得到参考轨迹，构建可行驶通道；在优化阶段，建立车辆运动学模型，构建代价函数和边界约束，使用ilqr算法进行时空联动优化，输出最优轨迹。结合了基于采样和基于优化两种方法的优点，同时也兼顾到frenet坐标系和笛卡尔坐标系的优势，解决了时空联合轨迹规划中计算复杂，难以满足实时性的问题，更容易满足动态场景的要求，解决了解耦类方法容易陷入局部最优解，甚至无解的问题，且解决了frenet坐标系下无法建立车辆运动学模型的问题。
[0055]
图1示出了本发明实施例提供的方法的流程示意图。
[0056]
具体的，在本发明提供的一个优选实施方式中，一种分阶段的自动驾驶车辆轨迹规划方法，所述方法具体包括以下步骤：
[0057]
步骤一、搜索阶段，在frenet坐标系下进行采样，使用五次多项式连结采样点，形成有向无环图，动态规划搜索得到参考轨迹，构建可行驶通道；
[0058]
步骤二、优化阶段，建立车辆运动学模型，根据所述可行驶通道，构建代价函数，建立边界约束，使用ilqr算法进行时空联动优化，输出最优轨迹。
[0059]
在本发明实施例中，自动驾驶车辆轨迹规划的过程分为搜索阶段和优化阶段两个阶段，搜索阶段的目的是为了能获得可行驶通道，使用采样的方法，在极少计算量的情况下，完成搜索过程，为接下来的优化阶段提供参考依据；优化阶段的目的是为了能得到更加完善的轨迹，搜索阶段得到的结果太粗糙，在优化阶段做时空联合规划，在满足各项约束的情况下，计算得到时空平滑的轨迹。具体的：在搜索阶段中，首先获取自动驾驶车辆所处地图信息，根据目标车道构建frenet坐标系，将障碍物信息和自动驾驶车辆信息投影到该坐标系下(之所以向frenet投影，目的是简化道路结构和障碍物轨迹的几何信息，简化约束，减少计算量)；然后在frenet坐标系下进行采样，得到采样点，并用五次多项式连接。根据预设好的代价函数计算其代价，得到每条边的代价；最后进行动态规划搜索，得到搜索阶段的最优路径，进行膨胀得到可行驶通道，并将其转化为笛卡尔坐标系；在优化阶段中，通过建立车辆运动学模型，根据搜索阶段得到的可行驶通道，建立轨迹边界建立限制条件，再使用障碍函数将带约束的优化问题，转化为无约束问题，进而使用ilqr算法进行问题求解，得到优化后的最优轨迹，并输出。
[0060]
在本发明实施例中，在frenet坐标系下进行采样，使用五次多项式连结采样点，形成有向无环图(dag),通过动态规划(dp)搜索得到采样路径，并构建可行驶通道，五次多项式是轨迹规划常用的曲线，其公式为：
[0061]
s∈[0,δs]
[0062]
其中，是五次多项式的参数，是五次多项式的自变量，采样的目的是为了确定这些五次多项式的表达形式，由于
五次多项式需要六个参数，对初始状态和结束状态可以得到六个方程，也就是边界条件，这样就能解出参数，再制定标准进行最优评价，这个标准为路径代价c
path
，由参考线偏移代价c
ref
,平滑性代价c
smooth
和障碍物代价c
obs
之和组成，形成的代价函数的表达式为：
[0063]cpath
＝c
ref
+c
smooth
+c
obs
[0064]
参考线偏移代价的公式为：
[0065][0066]
其中，w0为权重系数，l(s)为偏移程度；
[0067]
平滑性代价的公式为：
[0068][0069]
其中，w1,w2,w3均为权重系数，分别是l(s)的一阶，二阶，三阶导数；
[0070]
障碍物代价的公式为：
[0071][0072]
可以理解的是，参考线偏移代价c
ref
是用来评价曲线与车道中线线的偏移，偏移越小越好，自动驾驶的车辆越靠近车道中心线；平滑性代价c
smooth
是用来评价曲线的平滑程度的，c
smooth
的值越小曲线约平滑，汽车越舒适；障碍物代价c
obs
是用来评价曲线与障碍物的原理程度，远离障碍物有利于保障车辆的安全，c
obs
的值越小，表示越远离障碍物。
[0073]
在本发明实施例中，计算完所有曲线的代价后，由于不存在明确的目标位置，很难使用启发式的搜索的得到最有路径，因此可以使用穷举搜索，通过采用动态规划(dp)算法来寻找最优路径，在非凸空间中生成相对平滑,无碰撞的代价最小的路径，将搜索得到的最优路径进行膨胀，从而得到可行驶通道，再将可行驶通道从frenet坐标系向笛卡尔坐标系转换，为优化阶段的边界限制提供依据。
[0074]
在本发明实施例中，将frenet坐标系下的可行驶通道转化为笛卡尔坐标系下的约束限制，在笛卡尔坐标系下基于带约束限制的ilqr(cilqr)进行时空耦合的轨迹优化，如图2示出了本发明实施例提供的方法中车辆运动学模型的示意图，使用车辆-自行车运动模型描述车辆运动，为ti时刻的状态量，其中是笛卡尔坐标系下的坐标点，vi和θi分别是车速和航向角，ui＝[ai,ωi]
t
为ti时刻的控制量，ai为加速度，ωi为横摆角速度，可以通过控制车辆的加速度和横摆角速度，完成对车辆整体运动轨迹的控制，假设控制量在[ti,t
i+1
]内保持不变，则车辆的运动状态方程可以近似表述为下式：
[0075]vi+1
＝vi+aiδ
t
[0076]
θ
i+1
＝θi+ωiδ
t
[0077][0078]
[0079]
其中，为ti时刻的状态量，为笛卡尔坐标系下的坐标点，vi和θi分别为车速和航向角，ai为加速度，δ
t
＝t
i+1-ti表示一个极短的时间，在这个时间内，假设控制量不变，自然由ti时刻的状态可以推到出t
i+1
的状态
[0080]
在本发明实施例中，优化过程的代价分为控制代价和状态代价，控制代价的目的是为了使自动驾驶车辆便于控制。控制代价包括加速度代价和横摆角速度代价，如下式:
[0081][0082]
其中，ju为时控制代价，ω
acc
,ω
ω
均为权重系数，ai,ωi分别为加速度和横摆角速度；
[0083]
状态代价的目的是使优化阶段得到的轨迹与搜索阶段得到的路径偏差不要太大，同时还要尽可能满足搜索阶段的可行驶通道的边界约束，状态代价定义为与搜索阶段路径的偏移和与参考速度的误差如下式：
[0084][0085]
其中，(r
ix
,r
iy
)为点在参考路径上的投影点，vr为自动驾驶车辆的参考速度，w
ref
,w
vel
均为权重系数，j
x
为状态代价；
[0086]
根据搜索阶段得到的可行驶通道，可以建立轨迹的边界约束，如图3示出了本发明实施例提供的方法中建立边界约束的示意图，点为ti时刻车辆位置，车辆位置在可行驶通道边界上的投影点。在投影点建立约束，函数g(x,y)描述经过投影点并于可行使通道边界相切的直线，建立如下约束.
[0087][0088]
该约束的含义是为了使车辆远离可行使通道的边界，使用切线的目的是为了简化约束的形式。
[0089]
应该理解的是，虽然本发明各实施例的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，各实施例中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
[0090]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(rom)、可编程rom(prom)、电可编程rom(eprom)、电可擦除可编程rom(eeprom)或闪存。易失性存储器可包括随机存取存储器(ram)或者外部高速缓冲存储器。作为说明而非局限，ram以多种形式可得，诸如静态ram
(sram)、动态ram(dram)、同步dram(sdram)、双数据率sdram(ddrsdram)、增强型sdram(esdram)、同步链路(synchlink)dram(sldram)、存储器总线(rambus)直接ram(rdram)、直接存储器总线动态ram(drdram)、以及存储器总线动态ram(rdram)等。
[0091]
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0092]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。
[0093]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。