CN113946155B - 一种自动驾驶速度规划方法、装置及存储介质 - Google Patents

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

Info

Publication number
CN113946155B
CN113946155B CN202111558017.5A CN202111558017A CN113946155B CN 113946155 B CN113946155 B CN 113946155B CN 202111558017 A CN202111558017 A CN 202111558017A CN 113946155 B CN113946155 B CN 113946155B
Authority
CN
China
Prior art keywords
constraint
speed
coefficient
function
highest
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111558017.5A
Other languages
English (en)
Other versions
CN113946155A (zh
Inventor
王文聪
刘国清
杨广
王启程
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Youjia Innovation Technology Co.,Ltd.
Original Assignee
Shenzhen Minieye Innovation Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Minieye Innovation Technology Co Ltd filed Critical Shenzhen Minieye Innovation Technology Co Ltd
Priority to CN202111558017.5A priority Critical patent/CN113946155B/zh
Publication of CN113946155A publication Critical patent/CN113946155A/zh
Application granted granted Critical
Publication of CN113946155B publication Critical patent/CN113946155B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种自动驾驶速度规划方法、装置及存储介质，包括：获取速度规划的多种约束条件；设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式；根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围；将速度曲线函数转化为关于最高次项系数的二次函数，通过求解二次函数在约束范围内的最小值，得到最优最高次项系数；根据最优最高次项系数和线性表达式计算得到多项式的其余系数，根据所有系数求解速度曲线函数，得到最优速度曲线。本发明以线性计算实现速度规划，能够有效降低速度规划的计算量，从而能够快速、准确应对复杂的交通场景。

Description

一种自动驾驶速度规划方法、装置及存储介质
技术领域
本发明涉及自动驾驶技术领域，尤其是涉及一种自动驾驶速度规划方法、装置及存储介质。
背景技术
避让动态障碍物一直是自动驾驶领域的难题，因为交通环境的复杂以及汽车的运动要满足车辆动力学和交通规则的约束，为了简化问题业界通常的做法是先做路径规划，再做速度决策，然后做速度规划算法，路径规划决定汽车运动轨迹的几何形状，速度决策算法决定在该规划路径上汽车对动态障碍物的响应(避让或超车)，速度规划算法决定具体的速度大小，由于交通环境复杂多变，速度规划算法必须要有较快的计算速度才能应对复杂的交通场景。现有的速度规划方法通常采用二次规划算法，但是二次规划算法是非线性规划，计算量很大，对于复杂场景下的速度规划，若使用算力不强的处理器，将难以满足实时性要求，若使用算力很强的处理器则需要较高的成本。
发明内容
本发明提供一种自动驾驶速度规划方法、装置及存储介质，以解决现有速度规划方法难以应对复杂的交通场景的技术问题。
本发明的一个实施例提供了一种自动驾驶速度规划方法，包括：
获取速度规划的多种约束条件；
设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式；
根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围；
将所述速度曲线函数转化为关于所述最高次项系数的二次函数，通过求解所述二次函数在所述约束范围内的最小值，得到最优最高次项系数；
根据所述最优最高次项系数和所述线性表达式计算得到所述多项式的其余系数，根据所有系数求解所述速度曲线函数，得到最优速度曲线。
进一步的，所述约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束，所述获取速度规划的多种约束条件，包括：
获取速度规划的起点参数、终点参数和时间，根据所述起点参数、终点参数和时间建立始末点约束；
根据交通规则下限制的最小速度和最大速度，建立交通规则约束；
根据车辆的最小加速度和最大加速度建立动力学约束；
根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据所述障碍物轨迹建立碰撞约束。
进一步的，根据所述障碍物轨迹建立碰撞约束，包括：
在判断到所述障碍物轨迹与车辆的规划路径存在干涉时，计算所述障碍物的速度曲线；
若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：
Figure 939059DEST_PATH_IMAGE001
若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：
Figure 541259DEST_PATH_IMAGE004
其中，
Figure 800202DEST_PATH_IMAGE005
为速度曲线函数，
Figure 226635DEST_PATH_IMAGE006
为障碍物的速度曲线，
Figure 229226DEST_PATH_IMAGE007
为障碍物的切入时间，
Figure 752611DEST_PATH_IMAGE008
为障碍物的切除时间；
将所有的不等式联立后作为碰撞约束。
进一步的，所述设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式，包括：
设置速度曲线函数的六次多项式为：
Figure 854559DEST_PATH_IMAGE009
其中，b 0~b 6为系数；
以所述六次多项式中的最高次项系数b6作为基本未知量，建立所述多项式中所有系数的线性表达式为：
Figure 96185DEST_PATH_IMAGE010
其中，
Figure 636888DEST_PATH_IMAGE011
、
Figure 14779DEST_PATH_IMAGE012
为线性系数。
进一步的，所述根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围，包括：
将所述线性表达式代入到所述速度曲线函数、所述速度曲线函数的一阶导数和所述速度曲线函数的二阶导数中，分别得到关于时间和所述最高次项系数的线性函数：
Figure 553208DEST_PATH_IMAGE013
Figure 16550DEST_PATH_IMAGE014
Figure 298627DEST_PATH_IMAGE015
其中，
Figure 796605DEST_PATH_IMAGE016
为速度曲线函数，
Figure 302672DEST_PATH_IMAGE017
为速度曲线函数的一阶导数，
Figure 456573DEST_PATH_IMAGE018
为速度曲线函数的二阶导数，p 0(t),p 1(t),p 2(t),q 0(t),q 1(t),q 2(t)均为已知函数；
将所述交通规则约束和动力学约束代入到所述线性函数中，求解得到满足所述交通规则约束和所述动力学约束的最高次项系数的第一约束范围；
将所述碰撞约束代入至所述线性函数中，求解得到满足所述碰撞约束的最高次项系数的第二约束范围；
将所述第一约束范围和所述第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围。
进一步的，将所述速度曲线函数转化为关于所述最高次项系数的二次函数，包括：
采用离散化的方法，在[0,T]中均匀采样n个点，记为t1,t2,…tn，将所述速度曲线函数的积分转化为：
Figure 339078DEST_PATH_IMAGE019
进一步化简为关于所述最高次项系数的二次函数：
Figure 629245DEST_PATH_IMAGE020
。
本发明的一个实施例提供了一种自动驾驶速度规划装置，包括：
约束条件获取模块，用于获取速度规划的多种约束条件；
线性表达式建立模块，用于设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式；
第一计算模块，用于根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围；
第二计算模块，用于将所述速度曲线函数转化为关于所述最高次项系数的二次函数，通过求解所述二次函数在所述约束范围内的最小值，得到最优最高次项系数；
最优速度曲线获取模块，用于根据所述最优最高次项系数和所述线性表达式计算得到所述多项式的其余系数，根据所有系数求解所述速度曲线函数，得到最优速度曲线。
本发明的一个实施提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上述的自动驾驶速度规划方法的步骤。
本发明实施例通过建立满足速度规划的多个约束条件，并设定速度曲线函数的多项式，利用多项式中的多个系数之间的关系，建立以最高次项系数表达的系数的线性表达式，并结合该线性表达式和约束条件得到最高次项系数的约束范围，再将速度曲线函数转换为关于最高次项数的二次函数，以求得在约束范围内的最小最高次项数，从而确定最优的速度曲线。本发明实施例根据满足速度规划的约束条件设定速度曲线函数为多项式，以线性计算实现速度规划，能够有效降低速度规划的计算量，提高速度规划的响应速度，从而能够满足复杂交通环境下的自动驾驶车辆的避障要求。
附图说明
图1是本发明实施例提供的一种自动驾驶速度规划方法的流程示意图；
图2是本发明实施例提供的一种自动驾驶速度规划方法的另一流程示意图；
图3是本发明实施例提供的一种自动驾驶速度规划装置的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本申请的描述中，需要理解的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在本申请的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本申请中的具体含义。
请参阅图1，本发明的一个实施例提供了一种自动驾驶速度规划方法，包括：
S1、获取速度规划的多种约束条件；
在本发明实施例中，与速度规划有关的约束条件包括：始末点约束、动力学约束、交通规则约束和碰撞约束。
S2、设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式；
在本发明实施例中，可以根据始末点约束的未知数来设定速度曲线函数的多项式次数，例如，根据规划的起点位置
Figure 306214DEST_PATH_IMAGE021
、起点速度
Figure 696481DEST_PATH_IMAGE022
、起点加速度
Figure 382677DEST_PATH_IMAGE023
，终点位置
Figure 589667DEST_PATH_IMAGE024
、终点速度
Figure 703117DEST_PATH_IMAGE025
、终点加速度
Figure 566031DEST_PATH_IMAGE026
，以及从起点到达终点所需的时间T，设定速度曲线函数f(t)需要满足的始末点约束为：
Figure 790339DEST_PATH_IMAGE027
Figure 117415DEST_PATH_IMAGE028
Figure 339449DEST_PATH_IMAGE029
设速度曲线函数为一个六次多项式：
Figure 486396DEST_PATH_IMAGE030
其中b 0~b 6为待定系数。上述六次多项式有7个未知数，始末点约束有6个未知数，即可将b 0~b 5写成b 6的线性表达式，以便于后续求解。
S3、根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围；
在本发明实施中，将线性表达式代入到速度曲线函数及其一阶导数和二阶导数，得到速度曲线函数及其一阶导数和二阶导数的线性函数，根据多个线性函数，从而能够计算得到满足所有约束条件的最高次项系数的约束范围。
S4、将速度曲线函数转化为关于最高次项系数的二次函数，通过求解二次函数在约束范围内的最小值，得到最优最高次项系数；
S5、根据最优最高次项系数和线性表达式计算得到多项式的其余系数，根据所有系数求解速度曲线函数，得到最优速度曲线。
本发明实施例通过建立满足速度规划的多个约束条件，并设定速度曲线函数的多项式，利用多项式中的多个系数之间的关系，建立以最高次项系数表达的系数的线性表达式，并结合该线性表达式和约束条件得到最高次项系数的约束范围，再将速度曲线函数转换为关于最高次项数的二次函数，以求得在约束范围内的最小最高次项数，从而确定最优的速度曲线。本发明实施例根据满足速度规划的约束条件设定速度曲线函数为多项式，以线性计算实现速度规划，能够有效降低速度规划的计算量，提高速度规划的响应速度，从而能够满足复杂交通环境下的自动驾驶车辆的避障要求，且本发明实施例结合了多种约束条件进行速度规划，综合考虑了在复杂交通环境下对速度规划的影响因素，能够有效提高了速度规划的准确性和可靠性。
在一个实施例中，约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束，获取速度规划的多种约束条件，包括：
获取速度规划的起点参数、终点参数和时间，根据起点参数、终点参数和时间建立始末点约束；
可选地，起点参数包括起点位置
Figure 514395DEST_PATH_IMAGE031
、起点速度
Figure 633661DEST_PATH_IMAGE032
、起点加速度
Figure 88913DEST_PATH_IMAGE033
，终点参数包括终点位置
Figure 988736DEST_PATH_IMAGE034
、终点速度
Figure 492529DEST_PATH_IMAGE025
、终点加速度
Figure 528619DEST_PATH_IMAGE035
，设定速度曲线函数f(t)需要满足的始末点约束为：
Figure 154772DEST_PATH_IMAGE027
Figure 213995DEST_PATH_IMAGE028
Figure 583796DEST_PATH_IMAGE036
。
可以理解的是，起点参数可以根据车辆的定位模块获取，起点参数以及时间T可以通过车辆的路径规划和速度决策获取。
根据交通规则下限制的最小速度和最大速度，建立交通规则约束；
在一个实施例中，通过车辆中的感知模块和地图模块获得交通规则，设交通规则下限制的最小速度为vmin，最大速度为vmax，则f(t)需要满足交通规则约束
Figure 739971DEST_PATH_IMAGE037
。
根据车辆的最小加速度和最大加速度建立动力学约束；
在一个实施例中，动力学的主要约束为加速度，车辆的动力学约束可以事先通过车辆动力学标定实验得到标定表，再通过感知和定位模块计算的车辆实际车速，根据车速查表得到，假设车辆的最小加速度为amin，最大加速度为amax，则f(t)需要满足:
Figure 474709DEST_PATH_IMAGE038
。
根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据障碍物轨迹建立碰撞约束。
可选地，在根据障碍物轨迹建立碰撞约束时，需要判断车辆的规划路径和障碍物轨迹是否存在干涉，若存在，则根据不同的障碍物处理决策建立障碍物处理约束，将所有的障碍物处理约束结合作为碰撞约束。
在一个实施例中，根据障碍物轨迹建立碰撞约束，包括：
在判断到障碍物轨迹与车辆的规划路径存在干涉时，计算障碍物的速度曲线；
若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：
Figure 83545DEST_PATH_IMAGE039
若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：
Figure 991458DEST_PATH_IMAGE040
其中，
Figure 2139DEST_PATH_IMAGE005
为速度曲线函数，
Figure 907778DEST_PATH_IMAGE006
为障碍物的速度曲线，
Figure 3910DEST_PATH_IMAGE007
为障碍物的切入时间，
Figure 715514DEST_PATH_IMAGE008
为障碍物的切除时间；
将所有的不等式联立后作为碰撞约束。
在一个实施例中，设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式，包括：
设置速度曲线函数的六次多项式为：
Figure 781035DEST_PATH_IMAGE009
其中，b 0~b 6为系数；
以六次多项式中的最高次项系数b6作为基本未知量，建立多项式中所有系数的线性表达式为：
Figure 919892DEST_PATH_IMAGE010
其中，
Figure 237741DEST_PATH_IMAGE011
、
Figure 753036DEST_PATH_IMAGE012
为线性系数。
在本发明实施例中，由于始末点约束包括起点位置
Figure 675993DEST_PATH_IMAGE041
、起点速度
Figure 985751DEST_PATH_IMAGE022
、起点加速度
Figure 790896DEST_PATH_IMAGE023
、终点位置
Figure 844303DEST_PATH_IMAGE042
、终点速度
Figure 621766DEST_PATH_IMAGE043
、终点加速度
Figure 102426DEST_PATH_IMAGE035
对应的约束，即共有六个约束，而六次多项式的七个系数共有六个约束，本发明实施例通过以六次多项式中的最高次项系数b6作为基本未知量，建立多项式中所有系数的线性表达式，实现其他六个系数由一个系数线性表出，从而以线性计算实现速度规划，能够有效降低速度规划的计算量。
在一个实施例中，根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围，包括：
将线性表达式代入到速度曲线函数、速度曲线函数的一阶导数和速度曲线函数的二阶导数中，分别得到关于时间和最高次项系数的线性函数：
Figure 660446DEST_PATH_IMAGE044
Figure 189648DEST_PATH_IMAGE045
Figure 149513DEST_PATH_IMAGE046
其中，
Figure 801075DEST_PATH_IMAGE005
为速度曲线函数，
Figure 580812DEST_PATH_IMAGE047
为速度曲线函数的一阶导数，
Figure 913704DEST_PATH_IMAGE048
为速度曲线函数的二阶导数，p 0(t),p 1(t),p 2(t),q 0(t),q 1(t),q 2(t)均为已知函数；
将交通规则约束和动力学约束代入到线性函数中，求解得到满足交通规则约束和动力学约束的最高次项系数的第一约束范围；
在本发明实施例中，交通规则和动力学约束均为时间
Figure 728076DEST_PATH_IMAGE049
上的约束，将交通规则和动力学约束代入可得：
Figure 550539DEST_PATH_IMAGE050
Figure 817572DEST_PATH_IMAGE051
由于p 1(t),p 2(t), q 1(t),q 2(t)都不是线性函数，本发明实施例在[0,T]中均匀采样n个点，记为t1,t2,…tn，将上述不等式求解问题近似转化为：
Figure 954155DEST_PATH_IMAGE052
Figure 623034DEST_PATH_IMAGE053
Figure 616398DEST_PATH_IMAGE055
Figure 308410DEST_PATH_IMAGE056
以及
Figure 311002DEST_PATH_IMAGE057
Figure 834387DEST_PATH_IMAGE058
Figure 998652DEST_PATH_IMAGE055
Figure 177960DEST_PATH_IMAGE059
通过上述不等式的转换，使得p 1(ti),p 2(ti), q 1(ti),q 2(ti) (i=1,2,3…n)变成函数的值，为常数，即上述每个不等式均为关于最高次项系数b 6的线性函数，求解得到满足交通规则约束和动力学约束的最高次项系数b 6的第一约束范围。
将碰撞约束代入至线性函数中，求解得到满足碰撞约束的最高次项系数的第二约束范围；
在本发明实施中，设障碍物处理决策为避让障碍物，该障碍物的切入时间为tin,切出时间tout,以及障碍物在[tin,tout]的速度曲线为g(t)，在[tin,tout]上采样m个点，记为t1,t2,…tm，求解下列不等式组：
Figure 718663DEST_PATH_IMAGE060
Figure 96555DEST_PATH_IMAGE061
Figure 637913DEST_PATH_IMAGE055
Figure 101256DEST_PATH_IMAGE062
即每个不等式均为最高次项系数b 6的线性函数，容易快速求解，求解得到满足碰撞约束的最高次项系数的第二约束范围。
可以理解的是，可以根据实际情况的不同障碍物处理决策求解得到满足碰撞约束的最高次项系数的第二约束范围。
将第一约束范围和第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围，记为[b 6min,b 6max]。
在一个实施例中，将速度曲线函数转化为关于最高次项系数的二次函数，包括：
采用离散化的方法，在[0,T]中均匀采样n个点，记为t1,t2,…tn，将速度曲线函数的积分转化为：
Figure 445649DEST_PATH_IMAGE063
进一步化简为关于最高次项系数的二次函数：
Figure 615731DEST_PATH_IMAGE064
。
可以理解的是，最平顺的速度曲线即为速度规划的最优速度曲线，评价平顺性的指标是f(t)二阶导数的绝对值在[0,T]上要尽可能的小，由于绝对值不好处理，采用平方代替，即最平顺的曲线就是使得积分
Figure 387378DEST_PATH_IMAGE065
最小的曲线。
为了进一步降低积分处理的复杂度，本发明实施例使用离散化的方法，在[0,T]中均匀采样n个点，记为t1,t2,…tn，将上述积分近似为：
Figure 338016DEST_PATH_IMAGE066
进一步化简为关于最高次项系数的二次函数：
Figure 95888DEST_PATH_IMAGE067
基于该二次函数，在已经求解出最高次项系数的约束范围[b 6min,b 6max]的前提下，能够快捷求解得到二次函数在[b 6min,b 6max]上的取最小值时的
Figure 448372DEST_PATH_IMAGE069
，并根据以最高次项系数b 6表达的系数b 0~b 5的线性表达式，确定六次多项式的所有系数，从而确定f(t)，即为速度规划的最优速度曲线。
请参阅图2，为本发明实施例提供的一种自动驾驶速度规划方法的另一流程示意图。
实施本发明实施例，具有以下有益效果：
本发明实施例通过建立满足速度规划的多个约束条件，并设定速度曲线函数的多项式，利用多项式中的多个系数之间的关系，建立以最高次项系数表达的系数的线性表达式，并结合该线性表达式和约束条件得到最高次项系数的约束范围，再将速度曲线函数转换为关于最高次项数的二次函数，以求得在约束范围内的最小最高次项数，从而确定最优的速度曲线。本发明实施例根据满足速度规划的约束条件设定速度曲线函数为多项式，以线性计算实现速度规划，能够有效降低速度规划的计算量，提高速度规划的响应速度，从而能够满足复杂交通环境下的自动驾驶车辆的避障要求，且本发明实施例结合了多种约束条件进行速度规划，综合考虑了在复杂交通环境下对速度规划的影响因素，能够有效提高了速度规划的准确性和可靠性。
请参阅图3，基于与上述实施例相同的发明构思，本发明的一个实施例提供了一种自动驾驶速度规划装置，包括：
约束条件获取模块10，用于获取速度规划的多种约束条件；
线性表达式建立模块20，用于设置速度曲线函数的多项式，根据多项式中的最高次项系数建立多项式中所有系数的线性表达式；
第一计算模块30，用于根据所有约束条件和线性表达式，计算得到满足所有约束条件的最高次项系数的约束范围；
第二计算模块40，用于将速度曲线函数转化为关于最高次项系数的二次函数，通过求解二次函数在约束范围内的最小值，得到最优最高次项系数；
最优速度曲线获取模块50，用于根据最优最高次项系数和线性表达式计算得到多项式的其余系数，根据所有系数求解速度曲线函数，得到最优速度曲线。
在一个实施例中，约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束，约束条件获取模块10，具体用于：
获取速度规划的起点参数、终点参数和时间，根据起点参数、终点参数和时间建立始末点约束；
根据交通规则下限制的最小速度和最大速度，建立交通规则约束；
根据车辆的最小加速度和最大加速度建立动力学约束；
根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据障碍物轨迹建立碰撞约束。
在一个实施例中，根据障碍物轨迹建立碰撞约束，包括：
在判断到障碍物轨迹与车辆的规划路径存在干涉时，计算障碍物的速度曲线；
若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：
Figure 390920DEST_PATH_IMAGE070
若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：
Figure 766537DEST_PATH_IMAGE071
其中，
Figure 452734DEST_PATH_IMAGE005
为速度曲线函数，
Figure 659724DEST_PATH_IMAGE006
为障碍物的速度曲线，
Figure 710857DEST_PATH_IMAGE007
为障碍物的切入时间，
Figure 636087DEST_PATH_IMAGE008
为障碍物的切除时间；
将所有的不等式联立后作为碰撞约束。
线性表达式建立模块20，具体用于：
设置速度曲线函数的六次多项式为：
Figure 860395DEST_PATH_IMAGE009
其中，b 0~b 6为系数；
建立多项式中所有系数的线性表达式为：
Figure 187472DEST_PATH_IMAGE010
其中，
Figure 143926DEST_PATH_IMAGE011
、
Figure 556453DEST_PATH_IMAGE012
为线性系数。
第一计算模块，具体用于：
将线性表达式代入到速度曲线函数、速度曲线函数的一阶导数和速度曲线函数的二阶导数中，分别得到关于时间和最高次项系数的线性函数：
Figure 584452DEST_PATH_IMAGE072
Figure 703718DEST_PATH_IMAGE045
Figure 158970DEST_PATH_IMAGE073
其中，
Figure 793213DEST_PATH_IMAGE005
为速度曲线函数，
Figure 359324DEST_PATH_IMAGE074
为速度曲线函数的一阶导数，
Figure 595746DEST_PATH_IMAGE048
为速度曲线函数的二阶导数，p 0(t),p 1(t),p 2(t),q 0(t),q 1(t),q 2(t)均为已知函数；
将交通规则约束和动力学约束代入到线性函数中，求解得到满足交通规则约束和动力学约束的最高次项系数的第一约束范围；
将碰撞约束代入至线性函数中，求解得到满足碰撞约束的最高次项系数的第二约束范围；
将第一约束范围和第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围。
第二计算模块，具体用于：
采用离散化的方法，在[0,T]中均匀采样n个点，记为t1,t2,…tn，将速度曲线函数的积分转化为：
Figure 221899DEST_PATH_IMAGE063
进一步化简为关于最高次项系数的二次函数：
Figure 343439DEST_PATH_IMAGE064
。
本发明的一个实施提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如上述的自动驾驶速度规划方法的步骤。
以上是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (5)

1.一种自动驾驶速度规划方法，其特征在于，包括：
获取速度规划的多种约束条件，所述约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束；
设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式，包括：设置速度曲线函数的六次多项式为：
Figure 3778DEST_PATH_IMAGE001
其中，b 0~b 6为系数；
以所述六次多项式中的最高次项系数b6作为基本未知量，建立所述多项式中所有系数的线性表达式为：
Figure 887465DEST_PATH_IMAGE002
其中，
Figure 938598DEST_PATH_IMAGE003
、
Figure 443922DEST_PATH_IMAGE004
为线性系数；
根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围，包括：
将所述线性表达式代入到所述速度曲线函数、所述速度曲线函数的一阶导数和所述速度曲线函数的二阶导数中，分别得到关于时间和所述最高次项系数的线性函数：
Figure 340334DEST_PATH_IMAGE005
Figure 214880DEST_PATH_IMAGE006
Figure 702493DEST_PATH_IMAGE007
其中，
Figure 301971DEST_PATH_IMAGE008
为速度曲线函数，
Figure 267653DEST_PATH_IMAGE009
为速度曲线函数的一阶导数，
Figure 809755DEST_PATH_IMAGE010
为速度曲线函数的二阶导数，p 0(t),p 1(t),p 2(t),q 0(t),q 1(t),q 2(t)均为已知函数；
将所述交通规则约束和动力学约束代入到所述线性函数中，求解得到满足所述交通规则约束和所述动力学约束的最高次项系数的第一约束范围；
将所述碰撞约束代入至所述线性函数中，求解得到满足所述碰撞约束的最高次项系数的第二约束范围 ；
将所述第一约束范围和所述第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围；
将所述速度曲线函数转化为关于所述最高次项系数的二次函数，通过求解所述二次函数在所述约束范围内的最小值，得到最优最高次项系数，其中，将所述速度曲线函数转化为关于所述最高次项系数的二次函数，包括：
采用离散化的方法，在[0,T]中均匀采样n个点，记为t1,t2,…tn，将所述速度曲线函数的积分转化为：
Figure 999428DEST_PATH_IMAGE011
进一步化简为关于所述最高次项系数的二次函数：
Figure 289464DEST_PATH_IMAGE012
；
根据所述最优最高次项系数和所述线性表达式计算得到所述多项式的其余系数，根据所有系数求解所述速度曲线函数，得到最优速度曲线。
2.如权利要求1所述的自动驾驶速度规划方法，其特征在于，所述获取速度规划的多种约束条件，包括：
获取速度规划的起点参数、终点参数和时间，根据所述起点参数、终点参数和时间建立始末点约束；
根据交通规则下限制的最小速度和最大速度，建立交通规则约束；
根据车辆的最小加速度和最大加速度建立动力学约束；
根据障碍物的当前时刻和历史时刻的位置以及速度信号，拟合得到障碍物轨迹，根据所述障碍物轨迹建立碰撞约束。
3.如权利要求2所述的自动驾驶速度规划方法，其特征在于，根据所述障碍物轨迹建立碰撞约束，包括：
在判断到所述障碍物轨迹与车辆的规划路径存在干涉时，计算所述障碍物的速度曲线；
若障碍物处理决策为避让障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第一不等式：
Figure 543990DEST_PATH_IMAGE013
若障碍物处理决策为超过障碍物，则在障碍物切入切出时间段内，车辆的速度曲线函数满足第二不等式：
Figure 783341DEST_PATH_IMAGE014
其中，
Figure 330866DEST_PATH_IMAGE015
为速度曲线函数，
Figure 717985DEST_PATH_IMAGE016
为障碍物的速度曲线，
Figure 25470DEST_PATH_IMAGE017
为障碍物的切入时间，
Figure 604481DEST_PATH_IMAGE018
为障碍物的切除时间；
将所有的不等式联立后作为碰撞约束。
4.一种自动驾驶速度规划装置，其特征在于，包括：
约束条件获取模块，用于获取速度规划的多种约束条件，所述约束条件包括始末点约束、交通规则约束、动力学约束和碰撞约束；
线性表达式建立模块，用于设置速度曲线函数的多项式，根据所述多项式中的最高次项系数建立所述多项式中所有系数的线性表达式，具体用于：设置速度曲线函数的六次多项式为：
Figure 339218DEST_PATH_IMAGE001
其中，b 0~b 6为系数；
以所述六次多项式中的最高次项系数b6作为基本未知量，建立所述多项式中所有系数的线性表达式为：
Figure 72688DEST_PATH_IMAGE002
其中，
Figure 946315DEST_PATH_IMAGE003
、
Figure 894679DEST_PATH_IMAGE004
为线性系数；
第一计算模块，用于根据所有所述约束条件和所述线性表达式，计算得到满足所有所述约束条件的最高次项系数的约束范围，具体用于：
将所述线性表达式代入到所述速度曲线函数、所述速度曲线函数的一阶导数和所述速度曲线函数的二阶导数中，分别得到关于时间和所述最高次项系数的线性函数：
Figure 862635DEST_PATH_IMAGE005
Figure 83401DEST_PATH_IMAGE006
Figure 483421DEST_PATH_IMAGE007
其中，
Figure 286292DEST_PATH_IMAGE008
为速度曲线函数，
Figure 549783DEST_PATH_IMAGE009
为速度曲线函数的一阶导数，
Figure 821626DEST_PATH_IMAGE010
为速度曲线函数的二阶导数，p 0(t),p 1(t),p 2(t),q 0(t),q 1(t),q 2(t)均为已知函数；
将所述交通规则约束和动力学约束代入到所述线性函数中，求解得到满足所述交通规则约束和所述动力学约束的最高次项系数的第一约束范围；
将所述碰撞约束代入至所述线性函数中，求解得到满足所述碰撞约束的最高次项系数的第二约束范围 ；
将所述第一约束范围和所述第二约束范围的交集作为满足所有约束条件的最高次项数的约束范围；
第二计算模块，用于将所述速度曲线函数转化为关于所述最高次项系数的二次函数，通过求解所述二次函数在所述约束范围内的最小值，得到最优最高次项系数；其中，将所述速度曲线函数转化为关于所述最高次项系数的二次函数，具体为：
采用离散化的方法，在[0,T]中均匀采样n个点，记为t1,t2,…tn，将所述速度曲线函数的积分转化为：
Figure 9025DEST_PATH_IMAGE011
进一步化简为关于所述最高次项系数的二次函数：
Figure 181249DEST_PATH_IMAGE012
最优速度曲线获取模块，用于根据所述最优最高次项系数和所述线性表达式计算得到所述多项式的其余系数，根据所有系数求解所述速度曲线函数，得到最优速度曲线。
5.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至3中任一项所述的自动驾驶速度规划方法的步骤。
CN202111558017.5A 2021-12-20 2021-12-20 一种自动驾驶速度规划方法、装置及存储介质 Active CN113946155B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111558017.5A CN113946155B (zh) 2021-12-20 2021-12-20 一种自动驾驶速度规划方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111558017.5A CN113946155B (zh) 2021-12-20 2021-12-20 一种自动驾驶速度规划方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN113946155A CN113946155A (zh) 2022-01-18
CN113946155B true CN113946155B (zh) 2022-04-05

Family

ID=79339205

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111558017.5A Active CN113946155B (zh) 2021-12-20 2021-12-20 一种自动驾驶速度规划方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN113946155B (zh)

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10606277B2 (en) * 2017-09-18 2020-03-31 Baidu Usa Llc Speed optimization based on constrained smoothing spline for autonomous driving vehicles
CN114527737A (zh) * 2020-11-06 2022-05-24 百度在线网络技术（北京）有限公司 用于自动驾驶的速度规划方法、装置、设备、介质和车辆
CN112965501A (zh) * 2021-03-31 2021-06-15 东风商用车有限公司 一种自动驾驶速度规划方法及装置
CN113377112B (zh) * 2021-06-30 2023-05-05 东风商用车有限公司 一种自动驾驶速度规划及状态协调方法和装置

Also Published As

Publication number Publication date
CN113946155A (zh) 2022-01-18

Similar Documents

Publication Publication Date Title
WO2020187257A1 (zh) 车辆异常换道控制方法、装置及系统
CN110316193B (zh) 预瞄距离的设置方法、装置、设备及计算机可读存储介质
CN112249032B (zh) 一种自动驾驶决策方法、系统、设备及计算机存储介质
CN110789517A (zh) 自动驾驶横向控制方法、装置、设备及存储介质
CN110853399A (zh) 一种基于超声波传感器车位检测系统的车位识别补偿方法
CN108710370B (zh) 一种无人驾驶汽车的控制方法及系统
US10795357B2 (en) Method and device for controlling vehicle and autonomous driving vehicle
JP2020516880A (ja) ポリゴンにおける中間点を低減する方法および装置
CN113460033B (zh) 一种自动泊车方法以及装置
WO2022088104A1 (zh) 一种确定目标对象点云集的方法及装置
CN112319464B (zh) 自动泊车方法、装置、设备及存储介质
CN112124314B (zh) 车辆自动变道横向路径规划方法、系统、车辆及存储介质
CN113377112B (zh) 一种自动驾驶速度规划及状态协调方法和装置
CN113946155B (zh) 一种自动驾驶速度规划方法、装置及存储介质
CN110597257B (zh) 一种基于道路曲率的常规行驶车速规划策略
CN112415999A (zh) 自动驾驶路径点的处理方法、装置、设备及存储介质
WO2023241556A1 (zh) 泊车控制方法、装置、设备和存储介质
CN110398248B (zh) 一种基于网格优化的航海避障算法
CN109606467B (zh) 一种车辆转向方法和车辆
CN116559845A (zh) 一种激光雷达自标定方法、系统、设备及存储介质
CN112733778B (zh) 一种车辆前导车确定方法、装置及计算机设备
CN112092800A (zh) 一种自动泊车时的方向盘转角优化方法及系统
CN115862310B (zh) 交通信息不确定环境下网联自动车队稳定性分析方法
CN113345248B (zh) 非信控交叉口安全预警和管控方法和系统
CN116560219B (zh) 基于输电杆塔风速联合分析的自适应监测控制方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: Floor 25, Block A, Zhongzhou Binhai Commercial Center Phase II, No. 9285, Binhe Boulevard, Shangsha Community, Shatou Street, Futian District, Shenzhen, Guangdong 518000

Patentee after: Shenzhen Youjia Innovation Technology Co.,Ltd.

Address before: 518051 401, building 1, Shenzhen new generation industrial park, No. 136, Zhongkang Road, Meidu community, Meilin street, Futian District, Shenzhen, Guangdong Province

Patentee before: SHENZHEN MINIEYE INNOVATION TECHNOLOGY Co.,Ltd.

CP03 Change of name, title or address