一种自动驾驶速度规划方法、装置及存储介质与流程

1.本发明涉及自动驾驶技术领域，尤其是涉及一种自动驾驶速度规划方法、装置及存储介质。


背景技术：

2.避让动态障碍物一直是自动驾驶领域的难题，因为交通环境的复杂以及汽车的运动要满足车辆动力学和交通规则的约束，为了简化问题业界通常的做法是先做路径规划，再做速度决策，然后做速度规划算法，路径规划决定汽车运动轨迹的几何形状，速度决策算法决定在该规划路径上汽车对动态障碍物的响应(避让或超车)，速度规划算法决定具体的速度大小，由于交通环境复杂多变，速度规划算法必须要有较快的计算速度才能应对复杂的交通场景。现有的速度规划方法通常采用二次规划算法，但是二次规划算法是非线性规划，计算量很大，对于复杂场景下的速度规划，若使用算力不强的处理器，将难以满足实时性要求，若使用算力很强的处理器则需要较高的成本。


技术实现要素：

3.本发明提供一种自动驾驶速度规划方法、装置及存储介质，以解决现有速度规划方法难以应对复杂的交通场景的技术问题。
4.本发明的一个实施例提供了一种自动驾驶速度规划方法，包括：获取速度规划的多种约束条件；设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式；根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围；将所述速度曲线函数转化为关于所述最高次项系数的二次函数，通过求解所述二次函数在所述约束范围内的最小值，得到最优最高次项系数；根据所述最优最高次项系数和所述线性表达式计算得到所述多项式的其余系数，根据所有系数求解所述速度曲线函数，得到最优速度曲线。
5.进一步的，所述约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束，所述获取速度规划的多种约束条件，包括：获取速度规划的起点参数、终点参数和时间，根据所述起点参数、终点参数和时间建立始末点约束；根据交通规则下限制的最小速度和最大速度，建立交通规则约束；根据车辆的最小加速度和最大加速度建立动力学约束；根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据所述障碍物轨迹建立碰撞约束。
6.进一步的，根据所述障碍物轨迹建立碰撞约束，包括：
在判断到所述障碍物轨迹与车辆的规划路径存在干涉时，计算所述障碍物的速度曲线；若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：其中，为速度曲线函数，为障碍物的速度曲线，为障碍物的切入时间，为障碍物的切除时间；将所有的不等式联立后作为碰撞约束。
7.进一步的，所述设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式，包括：设置速度曲线函数的六次多项式为：其中，b0~b6为系数；以所述六次多项式中的最高次项系数b6作为基本未知量，建立所述多项式中所有系数的线性表达式为：其中，、为线性系数。
8.进一步的，所述根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围，包括：将所述线性表达式带入到所述速度曲线函数、所述速度曲线函数的一阶导数和所述速度曲线函数的二阶导数中，分别得到关于时间和所述最高次项系数的线性函数：述速度曲线函数的二阶导数中，分别得到关于时间和所述最高次项系数的线性函数：述速度曲线函数的二阶导数中，分别得到关于时间和所述最高次项系数的线性函数：其中，为速度曲线函数，为速度曲线函数的一阶导数，为速度曲线函数的二阶导数，p0(t),p1(t),p2(t),q0(t),q1(t),q2(t)均为已知函数；将所述交通规则约束和动力学约束带入到所述线性函数中，求解得到满足所述交通规则约束和所述动力学约束的最高次项系数的第一约束范围；将所述碰撞约束带入至所述线性函数中，求解得到满足所述碰撞约束的最高次项系数的第二约束范围；将所述第一约束范围和所述第二约束范围的交集作为满足所有约束条件的最高
次项数的约束范围。
9.进一步的，将所述速度曲线函数转化为关于所述最高次项系数的二次函数，包括：采用离散化的方法，在[0,t]中均匀采样n个点，记为t1,t2,
…
tn，将所述速度曲线函数的积分转化为：进一步化简为关于所述最高次项系数的二次函数：。
[0010]
本发明的一个实施例提供了一种自动驾驶速度规划装置，包括：约束条件获取模块，用于获取速度规划的多种约束条件；线性表达式建立模块，用于设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式；第一计算模块，用于根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围；第二计算模块，用于将所述速度曲线函数转化为关于所述最高次项系数的二次函数，通过求解所述二次函数在所述约束范围内的最小值，得到最优最高次项系数；最优速度曲线获取模块，用于根据所述最优最高次项系数和所述线性表达式计算得到所述多项式的其余系数，根据所有系数求解所述速度曲线函数，得到最优速度曲线。
[0011]
本发明的一个实施提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上述的自动驾驶速度规划方法的步骤。
[0012]
本发明实施例通过建立满足速度规划的多个约束条件，并设定速度曲线函数的多项式，利用多项式中的多个系数之间的关系，建立以最高次项系数表达的系数的线性表达式，并结合该线性表达式和约束条件得到最高次项系数的约束范围，再将速度曲线函数转换为关于最高次项数的二次函数，以求得在约束范围内的最小最高次项数，从而确定最优的速度曲线。本发明实施例根据满足速度规划的约束条件设定速度曲线函数为多项式，以线性计算实现速度规划，能够有效降低速度规划的计算量，提高速度规划的响应速度，从而能够满足复杂交通环境下的自动驾驶车辆的避障要求。
附图说明
[0013]
图1是本发明实施例提供的一种自动驾驶速度规划方法的流程示意图；图2是本发明实施例提供的一种自动驾驶速度规划方法的另一流程示意图；图3是本发明实施例提供的一种自动驾驶速度规划装置的结构示意图。
具体实施方式
[0014]
下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于
本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0015]
在本技术的描述中，需要理解的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本技术的描述中，除非另有说明，“多个”的含义是两个或两个以上。
[0016]
在本技术的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本技术中的具体含义。
[0017]
请参阅图1，本发明的一个实施例提供了一种自动驾驶速度规划方法，包括：s1、获取速度规划的多种约束条件；在本发明实施例中，与速度规划有关的约束条件包括：始末点约束、动力学约束、交通规则约束和碰撞约束。
[0018]
s2、设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式；在本发明实施例中，可以根据始末点约束的未知数来设定速度曲线函数的多项式次数，例如，根据规划的起点位置、起点速度、起点加速度，终点位置、终点速度、终点加速度，以及从起点到达终点所需的时间t，设定速度曲线函数f(t)需要满足的始末点约束为：末点约束为：末点约束为：设速度曲线函数为一个六次多项式：其中b0~b6为待定系数。上述六次多项式有7个未知数，始末点约束有6个未知数，即可将b0~b5写成b6的线性表达式，以便于后续求解。
[0019]
s3、根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围；在本发明实施中，将线性表达式带入到速度曲线函数及其一阶导数和二阶导数，得到速度曲线函数及其一阶导数和二阶导数的线性函数，根据多个线性函数，从而能够计算得到满足所有约束条件的最高次项系数的约束范围。
[0020]
s4、将速度曲线函数转化为关于最高次项系数的二次函数，通过求解二次函数在约束范围内的最小值，得到最优最高次项系数；s5、根据最优最高次项系数和线性表达式计算得到多项式的其余系数，根据所有
系数求解速度曲线函数，得到最优速度曲线。
[0021]
本发明实施例通过建立满足速度规划的多个约束条件，并设定速度曲线函数的多项式，利用多项式中的多个系数之间的关系，建立以最高次项系数表达的系数的线性表达式，并结合该线性表达式和约束条件得到最高次项系数的约束范围，再将速度曲线函数转换为关于最高次项数的二次函数，以求得在约束范围内的最小最高次项数，从而确定最优的速度曲线。本发明实施例根据满足速度规划的约束条件设定速度曲线函数为多项式，以线性计算实现速度规划，能够有效降低速度规划的计算量，提高速度规划的响应速度，从而能够满足复杂交通环境下的自动驾驶车辆的避障要求，且本发明实施例结合了多种约束条件进行速度规划，综合考虑了在复杂交通环境下对速度规划的影响因素，能够有效提高了速度规划的准确性和可靠性。
[0022]
在一个实施例中，约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束，获取速度规划的多种约束条件，包括：获取速度规划的起点参数、终点参数和时间，根据起点参数、终点参数和时间建立始末点约束；可选地，起点参数包括起点位置、起点速度、起点加速度，终点参数包括终点位置、终点速度、终点加速度，设定速度曲线函数f(t)需要满足的始末点约束为：，设定速度曲线函数f(t)需要满足的始末点约束为：，设定速度曲线函数f(t)需要满足的始末点约束为：。
[0023]
可以理解的是，起点参数可以根据车辆的定位模块获取，起点参数以及时间t可以通过车辆的路径规划和速度决策获取。
[0024]
根据交通规则下限制的最小速度和最大速度，建立交通规则约束；在一个实施例中，通过车辆中的感知模块和地图模块获得交通规则，设交通规则下限制的最小速度为v
min
，最大速度为v
max
，则f(t)需要满足交通规则约束。
[0025]
根据车辆的最小加速度和最大加速度建立动力学约束；在一个实施例中，动力学的主要约束为加速度，车辆的动力学约束可以事先通过车辆动力学标定实验得到标定表，再通过感知和定位模块计算的车辆实际车速，根据车速查表得到，假设车辆的最小加速度为a
min
，最大加速度为a
max
，则f(t)需要满足:。
[0026]
根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据障碍物轨迹建立碰撞约束。
[0027]
可选地，在根据障碍物轨迹建立碰撞约束时，需要判断车辆的规划路径和障碍物轨迹是否存在干涉，若存在，则根据不同的障碍物处理决策建立障碍物处理约束，将所有的障碍物处理约束结合作为碰撞约束。
[0028]
在一个实施例中，根据障碍物轨迹建立碰撞约束，包括：
在判断到障碍物轨迹与车辆的规划路径存在干涉时，计算障碍物的速度曲线；若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：其中，为速度曲线函数，为障碍物的速度曲线，为障碍物的切入时间，为障碍物的切除时间；将所有的不等式联立后作为碰撞约束。
[0029]
在一个实施例中，设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式，包括：设置速度曲线函数的六次多项式为：其中，b0~b6为系数；以六次多项式中的最高次项系数b6作为基本未知量，建立多项式中所有系数的线性表达式为：其中，、为线性系数。
[0030]
在本发明实施例中，由于始末点约束包括起点位置、起点速度、起点加速度、终点位置、终点速度、终点加速度对应的约束，即共有六个约束，而六次多项式的七个系数共有六个约束，本发明实施例通过以六次多项式中的最高次项系数b6作为基本未知量，建立多项式中所有系数的线性表达式，实现其他六个系数由一个系数线性表出，从而以线性计算实现速度规划，能够有效降低速度规划的计算量。
[0031]
在一个实施例中，根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围，包括：将线性表达式带入到速度曲线函数、速度曲线函数的一阶导数和速度曲线函数的二阶导数中，分别得到关于时间和最高次项系数的线性函数：二阶导数中，分别得到关于时间和最高次项系数的线性函数：二阶导数中，分别得到关于时间和最高次项系数的线性函数：其中，为速度曲线函数，为速度曲线函数的一阶导数，为速度曲线函数的二阶导数，p0(t),p1(t),p2(t),q0(t),q1(t),q2(t)均为已知函数；将交通规则约束和动力学约束带入到线性函数中，求解得到满足交通规则约束和
动力学约束的最高次项系数的第一约束范围；在本发明实施例中，交通规则和动力学约束均为时间上的约束，将交通规则和动力学约束带入可得：则和动力学约束带入可得：由于p1(t),p2(t), q1(t),q2(t)都不是线性函数，本发明实施例在[0,t]中均匀采样n个点，记为t1,t2,
…
tn，将上述不等式求解问题近似转化为：，将上述不等式求解问题近似转化为：，将上述不等式求解问题近似转化为：，将上述不等式求解问题近似转化为：以及以及以及以及通过上述不等式的转换，使得p1(ti),p2(ti), q1(ti),q2(ti) (i=1,2,3
…
n)变成函数的值，为常数，即上述每个不等式均为关于最高次项系数b6的线性函数，求解得到满足交通规则约束和动力学约束的最高次项系数b6的第一约束范围。
[0032]
将碰撞约束带入至线性函数中，求解得到满足碰撞约束的最高次项系数的第二约束范围；在本发明实施中，设障碍物处理决策为避让障碍物，该障碍物的切入时间为t
in
,切出时间t
out
,以及障碍物在[t
in
,t
out
]的速度曲线为g(t)，在[t
in
,t
out
]上采样m个点，记为t1,t2,
…
tm，求解下列不等式组：解下列不等式组：解下列不等式组：解下列不等式组：即每个不等式均为最高次项系数b6的线性函数，容易快速求解，求解得到满足碰撞约束的最高次项系数的第二约束范围。
[0033]
可以理解的是，可以根据实际情况的不同障碍物处理决策求解得到满足碰撞约束的最高次项系数的第二约束范围。
[0034]
将第一约束范围和第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围，记为[b
6min
,b
6max
]。
[0035]
在一个实施例中，将速度曲线函数转化为关于最高次项系数的二次函数，包括：采用离散化的方法，在[0,t]中均匀采样n个点，记为t1,t2,
…
tn，将速度曲线函数的积分转化为：进一步化简为关于最高次项系数的二次函数：。
[0036]
可以理解的是，最平顺的速度曲线即为速度规划的最优速度曲线，评价平顺性的指标是f(t)二阶导数的绝对值在[0,t]上要尽可能的小，由于绝对值不好处理，采用平方代替，即最平顺的曲线就是使得积分最小的曲线。
[0037]
为了进一步降低积分处理的复杂度，本发明实施例使用离散化的方法，在[0,t]中均匀采样n个点，记为t1,t2,
…
tn，将上述积分近似为：进一步化简为关于最高次项系数的二次函数：基于该二次函数，在已经求解出最高次项系数的约束范围[b
6min
,b
6max
]的前提下，能够快捷求解得到二次函数在[b
6min
,b
6max
]上的取最小值时的，并根据以最高次项系数b6表达的系数b0~b5的线性表达式，确定六次多项式的所有系数，从而确定f(t)，即为速度规划的最优速度曲线。
[0038]
请参阅图2，为本发明实施例提供的一种自动驾驶速度规划方法的另一流程示意图。
[0039]
实施本发明实施例，具有以下有益效果：本发明实施例通过建立满足速度规划的多个约束条件，并设定速度曲线函数的多项式，利用多项式中的多个系数之间的关系，建立以最高次项系数表达的系数的线性表达式，并结合该线性表达式和约束条件得到最高次项系数的约束范围，再将速度曲线函数转换为关于最高次项数的二次函数，以求得在约束范围内的最小最高次项数，从而确定最优的速度曲线。本发明实施例根据满足速度规划的约束条件设定速度曲线函数为多项式，以线性计算实现速度规划，能够有效降低速度规划的计算量，提高速度规划的响应速度，从而能够满足复杂交通环境下的自动驾驶车辆的避障要求，且本发明实施例结合了多种约束条件进行速度规划，综合考虑了在复杂交通环境下对速度规划的影响因素，能够有效提高了速度规划的准确性和可靠性。
[0040]
请参阅图3，基于与上述实施例相同的发明构思，本发明的一个实施例提供了一种自动驾驶速度规划装置，包括：约束条件获取模块10，用于获取速度规划的多种约束条件；线性表达式建立模块20，用于设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式；第一计算模块30，用于根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围；第二计算模块40，用于将速度曲线函数转化为关于最高次项系数的二次函数，通过求解二次函数在约束范围内的最小值，得到最优最高次项系数；最优速度曲线获取模块50，用于根据最优最高次项系数和线性表达式计算得到多项式的其余系数，根据所有系数求解速度曲线函数，得到最优速度曲线。
[0041]
在一个实施例中，约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束，约束条件获取模块10，具体用于：获取速度规划的起点参数、终点参数和时间，根据起点参数、终点参数和时间建立始末点约束；根据交通规则下限制的最小速度和最大速度，建立交通规则约束；根据车辆的最小加速度和最大加速度建立动力学约束；根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据障碍物轨迹建立碰撞约束。
[0042]
在一个实施例中，根据障碍物轨迹建立碰撞约束，包括：在判断到障碍物轨迹与车辆的规划路径存在干涉时，计算障碍物的速度曲线；若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：其中，为速度曲线函数，为障碍物的速度曲线，为障碍物的切入时间，为障碍物的切除时间；将所有的不等式联立后作为碰撞约束。
[0043]
线性表达式建立模块20，具体用于：设置速度曲线函数的六次多项式为：其中，b0~b6为系数；建立多项式中所有系数的线性表达式为：
其中，、为线性系数。
[0044]
第一计算模块，具体用于：将线性表达式带入到速度曲线函数、速度曲线函数的一阶导数和速度曲线函数的二阶导数中，分别得到关于时间和最高次项系数的线性函数：二阶导数中，分别得到关于时间和最高次项系数的线性函数：二阶导数中，分别得到关于时间和最高次项系数的线性函数：其中，为速度曲线函数，为速度曲线函数的一阶导数，为速度曲线函数的二阶导数，p0(t),p1(t),p2(t),q0(t),q1(t),q2(t)均为已知函数；将交通规则约束和动力学约束带入到线性函数中，求解得到满足交通规则约束和动力学约束的最高次项系数的第一约束范围；将碰撞约束带入至线性函数中，求解得到满足碰撞约束的最高次项系数的第二约束范围；将第一约束范围和第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围。
[0045]
第二计算模块，具体用于：采用离散化的方法，在[0,t]中均匀采样n个点，记为t1,t2,
…
tn，将速度曲线函数的积分转化为：进一步化简为关于最高次项系数的二次函数：。
[0046]
本发明的一个实施提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如上述的自动驾驶速度规划方法的步骤。
[0047]
以上是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。