CN114415694B - 一种面向自动驾驶车辆的实时轨迹规划方法及系统 - Google Patents

一种面向自动驾驶车辆的实时轨迹规划方法及系统 Download PDF

Info

Publication number
CN114415694B
CN114415694B CN202210308275.6A CN202210308275A CN114415694B CN 114415694 B CN114415694 B CN 114415694B CN 202210308275 A CN202210308275 A CN 202210308275A CN 114415694 B CN114415694 B CN 114415694B
Authority
CN
China
Prior art keywords
vehicle
control
model
variable
variables
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
CN202210308275.6A
Other languages
English (en)
Other versions
CN114415694A (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210308275.6A priority Critical patent/CN114415694B/zh
Publication of CN114415694A publication Critical patent/CN114415694A/zh
Application granted granted Critical
Publication of CN114415694B publication Critical patent/CN114415694B/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

本发明涉及自动驾驶车辆轨迹规划领域，具体涉及一种面向自动驾驶车辆的实时轨迹规划方法及系统。该方法包括根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；获取自动驾驶车辆的多边形函数；并根据多边形函数确定避障的约束条件；对车辆非线性运动学模型和避障的约束条件进行线性化处理；根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划。本发明能够提高轨迹规划和避障的准确性和实时性。

Description

一种面向自动驾驶车辆的实时轨迹规划方法及系统
技术领域
本发明涉及自动驾驶车辆轨迹规划领域，特别是涉及一种面向自动驾驶车辆的实时轨迹规划方法及系统。
背景技术
安全高效轨迹规划问题是自动驾驶和移动机器人研究领域的核心问题之一，然而现有技术难以实现避撞约束下运动规划的实时解算，主要原因是：传统建模方法将车辆近似为圆形或椭圆形，使得最终形成的运动规划问题隶属于非线性最优规划问题范畴，依赖于专业商用非线性规划求解软件解算，极大增加了运算复杂度和硬件资源，不利于实时运动规划的实现。
发明内容
本发明的目的是提供一种面向自动驾驶车辆的实时轨迹规划方法及系统，能够提高轨迹规划和避障的准确性和实时性。
为实现上述目的，本发明提供了如下方案：
一种面向自动驾驶车辆的实时轨迹规划方法，包括：
确定自动驾驶车辆的车辆非线性运动学模型；
根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；
获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件；
对车辆非线性运动学模型和避障的约束条件进行线性化处理；
根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；
根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划。
可选地，所述确定自动驾驶车辆的车辆非线性运动学模型，具体包括：
利用公式
Figure 100002_DEST_PATH_IMAGE002
确定自动驾驶车辆的车辆非线性运动学模型；
其中，p x 和p y 分别为车辆后轴中心点在惯性坐标系下的横纵坐标，
Figure 100002_DEST_PATH_IMAGE004
和
Figure 100002_DEST_PATH_IMAGE006
为p x 和p y 的导数，v为车辆速度，
Figure 100002_DEST_PATH_IMAGE008
为车辆加速度，a为车辆加速度，
Figure 100002_DEST_PATH_IMAGE010
为偏航角，
Figure 100002_DEST_PATH_IMAGE012
为
Figure DEST_PATH_IMAGE010A
的导数，
Figure 100002_DEST_PATH_IMAGE014
表示航向角，L表示轴距，e为后轴中心点距离对应参考轨迹的横向距离，
Figure 100002_DEST_PATH_IMAGE016
为e导数，
Figure 100002_DEST_PATH_IMAGE018
为相对偏航角，
Figure 100002_DEST_PATH_IMAGE020
为
Figure DEST_PATH_IMAGE018A
导数，
Figure 100002_DEST_PATH_IMAGE022
为参考轨迹的偏航角速率。
可选地，所述根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件，具体包括：
根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率；
根据状态变量、控制变量以及控制变量的变化率确定在连续时间空间下的动力学模型；
基于欧拉近似方法对动力学模型进行离散化处理。
可选地，所述获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件，具体包括以下公式：
Figure 100002_DEST_PATH_IMAGE024
；
Figure 100002_DEST_PATH_IMAGE026
；
其中，
Figure 100002_DEST_PATH_IMAGE028
为自动驾驶车辆设定范围内第m个物体所占的空间，m为物体的编号，m=0为自动驾驶车辆，M为自动驾驶车辆设定范围内的所有物体，
Figure 100002_DEST_PATH_IMAGE030
为自动驾驶车辆的形状，
Figure 100002_DEST_PATH_IMAGE032
为自动驾驶车辆设定范围内物体所占的空间，
Figure 100002_DEST_PATH_IMAGE034
为自动驾驶车辆与设定范围内物体的最小距离，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，z为多边形物体每条边对应线性函数自变量，
Figure 100002_DEST_PATH_IMAGE036
为自变量的实数域范围，
Figure DEST_PATH_IMAGE038
为多边形每条边的斜率系数，
Figure DEST_PATH_IMAGE040
为多边形每条边的截距系数。
可选地，所述对车辆非线性运动学模型和避障的约束条件进行线性化处理，具体包括：
对车辆非线性运动学模型进行泰勒近似展开；并基于对偶法对避障的约束条件进行凸优化近似，完成车辆非线性运动学模型和避障的约束条件的线性化处理。
一种面向自动驾驶车辆的实时轨迹规划系统，包括：
车辆非线性运动学模型确定模块，用于确定自动驾驶车辆的车辆非线性运动学模型；
约束条件第一确定模块，用于根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；
约束条件第二确定模块，用于获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件；
线性化处理模块，用于对车辆非线性运动学模型和避障的约束条件进行线性化处理；
运动控制目标函数及问题模型确定模块，用于根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；
实时轨迹规划完成模块，用于根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划。
可选地，所述车辆非线性运动学模型确定模块具体包括：
利用公式
Figure DEST_PATH_IMAGE002A
确定自动驾驶车辆的车辆非线性运动学模型；
其中，p x 和p y 分别为车辆后轴中心点在惯性坐标系下的横纵坐标，
Figure DEST_PATH_IMAGE004A
和
Figure DEST_PATH_IMAGE006A
为p x 和p y 的导数，v为车辆速度，
Figure DEST_PATH_IMAGE008A
为车辆加速度，a为车辆加速度，
Figure DEST_PATH_IMAGE010AA
为偏航角，
Figure DEST_PATH_IMAGE012A
为
Figure DEST_PATH_IMAGE010AAA
的导数，
Figure DEST_PATH_IMAGE014A
表示航向角，L表示轴距，e为后轴中心点距离对应参考轨迹的横向距离，
Figure DEST_PATH_IMAGE016A
为e导数，
Figure DEST_PATH_IMAGE018AA
为相对偏航角，
Figure DEST_PATH_IMAGE020A
为
Figure DEST_PATH_IMAGE018AAA
导数，
Figure DEST_PATH_IMAGE022A
为参考轨迹的偏航角速率。
可选地，所述约束条件第一确定模块具体包括：
变量确定单元，用于根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率；
在连续时间空间下的动力学模型确定单元，用于根据状态变量、控制变量以及控制变量的变化率确定在连续时间空间下的动力学模型；
离散化处理单元，用于基于欧拉近似方法对动力学模型进行离散化处理。
可选地，所述约束条件第二确定模块具体包括以下公式：
Figure DEST_PATH_IMAGE024A
；
Figure DEST_PATH_IMAGE026A
；
其中，
Figure DEST_PATH_IMAGE028A
为自动驾驶车辆设定范围内第m个物体所占的空间，m为物体的编号，m=0为自动驾驶车辆，M为自动驾驶车辆设定范围内的所有物体，
Figure DEST_PATH_IMAGE030A
为自动驾驶车辆的形状，
Figure DEST_PATH_IMAGE032A
为自动驾驶车辆设定范围内物体所占的空间，
Figure DEST_PATH_IMAGE034A
为自动驾驶车辆与设定范围内物体的最小距离，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，z为多边形物体每条边对应线性函数自变量，
Figure DEST_PATH_IMAGE036A
为自变量的实数域范围，
Figure DEST_PATH_IMAGE038A
为多边形每条边的斜率系数，
Figure DEST_PATH_IMAGE040A
为多边形每条边的截距系数。
可选地，所述线性化处理模块具体包括：
线性化处理单元，用于对车辆非线性运动学模型进行泰勒近似展开；并基于对偶法对避障的约束条件进行凸优化近似，完成车辆非线性运动学模型和避障的约束条件的线性化处理。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明所提供的一种面向自动驾驶车辆的实时轨迹规划方法及系统，根据自动驾驶车辆的多边形函数确定避障的约束条件；对车辆非线性运动学模型和避障的约束条件进行线性化处理；根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型，不依赖于专业商用非线性规划求解软件解算，极大降低了运算复杂度和硬件资源，利于实时运动规划的实现，进而提高轨迹规划和避障的准确性和实时性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的一种面向自动驾驶车辆的实时轨迹规划方法流程示意图；
图2为车辆非线性运动学模型原理示意图；
图3为本发明所提供的一种面向自动驾驶车辆的实时轨迹规划系统结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种面向自动驾驶车辆的实时轨迹规划方法及系统，能够提高轨迹规划和避障的准确性和实时性。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明所提供的一种面向自动驾驶车辆的实时轨迹规划方法流程示意图，如图1所示，本发明所提供的一种面向自动驾驶车辆的实时轨迹规划方法，包括：
S101，如图2所示，确定自动驾驶车辆的车辆非线性运动学模型；
S101具体包括：
利用公式
Figure DEST_PATH_IMAGE002AA
确定自动驾驶车辆的车辆非线性运动学模型；
其中，p x 和p y 分别为车辆后轴中心点在惯性坐标系下的横纵坐标，
Figure DEST_PATH_IMAGE004AA
和
Figure DEST_PATH_IMAGE006AA
为p x 和p y 的导数，v为车辆速度，
Figure DEST_PATH_IMAGE008AA
为车辆加速度，a为车辆加速度，
Figure DEST_PATH_IMAGE010AAAA
为偏航角，
Figure DEST_PATH_IMAGE012AA
为
Figure DEST_PATH_IMAGE010_5A
的导数，
Figure DEST_PATH_IMAGE014AA
表示航向角，L表示轴距，e为后轴中心点距离对应参考轨迹的横向距离，
Figure DEST_PATH_IMAGE016AA
为e导数，
Figure DEST_PATH_IMAGE018AAAA
为相对偏航角，
Figure DEST_PATH_IMAGE020AA
为
Figure DEST_PATH_IMAGE018_5A
导数，
Figure DEST_PATH_IMAGE022AA
为参考轨迹的偏航角速率。
S102，根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；
S102具体包括：
根据车辆非线性运动学模型确定状态变量
Figure DEST_PATH_IMAGE042
、控制变量
Figure DEST_PATH_IMAGE044
以及控制变量的变化率；
根据状态变量、控制变量以及控制变量的变化率确定在连续时间空间下的动力学模型
Figure DEST_PATH_IMAGE046
；
基于欧拉近似方法对动力学模型进行离散化处理：
Figure DEST_PATH_IMAGE048
；
其中：
Figure DEST_PATH_IMAGE050
表示采样周期。
考虑到系统物理量存在边界可行域，对任意k时刻的状态变量x k 、控制变量u k 以及控制变量的变化率
Figure DEST_PATH_IMAGE052
定义如下的上下限边界约束：
Figure DEST_PATH_IMAGE054
；
S103，获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件；
S103具体包括以下公式：
Figure DEST_PATH_IMAGE024AA
；
Figure DEST_PATH_IMAGE056
；
Figure DEST_PATH_IMAGE026AA
；
Figure DEST_PATH_IMAGE058
；
其中，
Figure DEST_PATH_IMAGE028AA
为自动驾驶车辆设定范围内第m个物体所占的空间，m为物体的编号，m=0为自动驾驶车辆，M为自动驾驶车辆设定范围内的所有物体，
Figure DEST_PATH_IMAGE030AA
为自动驾驶车辆的形状，
Figure DEST_PATH_IMAGE032AA
为自动驾驶车辆设定范围内物体所占的空间，
Figure DEST_PATH_IMAGE034AA
为自动驾驶车辆与设定范围内物体的最小距离，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，z为多边形物体每条边对应线性函数自变量，
Figure DEST_PATH_IMAGE036AA
为自变量的实数域范围，
Figure DEST_PATH_IMAGE038AA
为多边形每条边的斜率系数，
Figure DEST_PATH_IMAGE040AA
为多边形每条边的截距系数。
S104，对车辆非线性运动学模型和避障的约束条件进行线性化处理；
S104具体包括：
对车辆非线性运动学模型进行泰勒近似展开；并基于对偶法对避障的约束条件进行凸优化近似，完成车辆非线性运动学模型和避障的约束条件的线性化处理。
进行泰勒展开，可以获得如下的时变状态空间模型：
Figure DEST_PATH_IMAGE060
；
其中：A k 和B k 分别表示状态变量和控制变量的雅可比矩阵，C k 是残差项，可以分别写为：
Figure DEST_PATH_IMAGE062
；
Figure DEST_PATH_IMAGE064
；
Figure DEST_PATH_IMAGE066
；
自动驾驶车辆与周围m个车辆或物体的最小距离可以表示为：
Figure DEST_PATH_IMAGE068
；
上式中最小化距离d的对偶问题是：
Figure DEST_PATH_IMAGE070
；
其中：
Figure DEST_PATH_IMAGE072
是对偶问题的对偶变量，
Figure DEST_PATH_IMAGE074
表示对偶范数，这里采用L2范数。可以根据上式求解自动驾驶车辆到与周围第m个物体的最小距离。因此，避撞约束可以表示为：
Figure DEST_PATH_IMAGE076
；
Figure DEST_PATH_IMAGE078
；
S105，根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；
以模型预测控制为框架的运动规划包括两个子步骤：
（1）通过对偶变量
Figure DEST_PATH_IMAGE080
求解自动驾驶车辆与周围物体的最小距离，即：
Figure DEST_PATH_IMAGE082
；
其中：
Figure DEST_PATH_IMAGE084
表示自动驾驶车辆在预测时间窗口N步内的初始既定占据空间。
（2）求解目标函数获得最优控制序列。在驾驶过程中，自动驾驶车辆应在避撞的前提下遵循既定的路径及速度轨迹。为了提高驾驶效率，避免自动驾驶车辆与既定轨迹偏离过大，横向偏离量e和相对偏航角
Figure DEST_PATH_IMAGE086
应该接近于0，因此，定义如下的优化控制问题：
Figure DEST_PATH_IMAGE088
；
Figure DEST_PATH_IMAGE090
；
Figure DEST_PATH_IMAGE092
；
Figure DEST_PATH_IMAGE094
；
其中：s k,m 表示软约束对应的松弛变量，
Figure DEST_PATH_IMAGE096
表示对应的惩罚系数，x ref 包含需要跟随的既定路线及速度轨迹，Q和R分别表示对状态变量和控制变量的加权系数矩阵，P是末点时刻的加权系数矩阵，本专利中
Figure DEST_PATH_IMAGE098
表示
Figure DEST_PATH_IMAGE100
。
利用QP求解器求解未来时间窗N内的最优控制序列u*=[u 0,u 1,u 2,…,u N-1]T，在每一时刻，只将第一个控制量u 0施加于系统，并在滚动时间窗内重复上述过程。
S106，根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划。
图3为本发明所提供的一种面向自动驾驶车辆的实时轨迹规划系统结构示意图，如图3所示，本发明所提供的一种面向自动驾驶车辆的实时轨迹规划系统，包括：
车辆非线性运动学模型确定模块301，用于确定自动驾驶车辆的车辆非线性运动学模型；
约束条件第一确定模块302，用于根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；
约束条件第二确定模块303，用于获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件；
线性化处理模块304，用于对车辆非线性运动学模型和避障的约束条件进行线性化处理；
运动控制目标函数及问题模型确定模块305，用于根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；
实时轨迹规划完成模块306，用于根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划。
所述车辆非线性运动学模型确定模块301具体包括：
利用公式
Figure DEST_PATH_IMAGE002AAA
确定自动驾驶车辆的车辆非线性运动学模型；
其中，p x 和p y 分别为车辆后轴中心点在惯性坐标系下的横纵坐标，
Figure DEST_PATH_IMAGE004AAA
和
Figure DEST_PATH_IMAGE006AAA
为p x 和p y 的导数，v为车辆速度，
Figure DEST_PATH_IMAGE008AAA
为车辆加速度，a为车辆加速度，
Figure DEST_PATH_IMAGE010_6A
为偏航角，
Figure DEST_PATH_IMAGE012AAA
为
Figure DEST_PATH_IMAGE010_7A
的导数，
Figure DEST_PATH_IMAGE014AAA
表示航向角，L表示轴距，e为后轴中心点距离对应参考轨迹的横向距离，
Figure DEST_PATH_IMAGE016AAA
为e导数，
Figure DEST_PATH_IMAGE018_6A
为相对偏航角，
Figure DEST_PATH_IMAGE020AAA
为
Figure DEST_PATH_IMAGE018_7A
导数，
Figure DEST_PATH_IMAGE022AAA
为参考轨迹的偏航角速率。
所述约束条件第一确定模块302具体包括：
变量确定单元，用于根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率；
在连续时间空间下的动力学模型确定单元，用于根据状态变量、控制变量以及控制变量的变化率确定在连续时间空间下的动力学模型；
离散化处理单元，用于基于欧拉近似方法对动力学模型进行离散化处理。
所述约束条件第二确定模块303具体包括以下公式：
Figure DEST_PATH_IMAGE024AAA
；
Figure DEST_PATH_IMAGE026AAA
；
其中，
Figure DEST_PATH_IMAGE028AAA
为自动驾驶车辆设定范围内第m个物体所占的空间，m为物体的编号，m=0为自动驾驶车辆，M为自动驾驶车辆设定范围内的所有物体，
Figure DEST_PATH_IMAGE030AAA
为自动驾驶车辆的形状，
Figure DEST_PATH_IMAGE032AAA
为自动驾驶车辆设定范围内物体所占的空间，
Figure DEST_PATH_IMAGE034AAA
为自动驾驶车辆与设定范围内物体的最小距离，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，z为多边形物体每条边对应线性函数自变量，
Figure DEST_PATH_IMAGE036AAA
为自变量的实数域范围，
Figure DEST_PATH_IMAGE038AAA
为多边形每条边的斜率系数，
Figure DEST_PATH_IMAGE040AAA
为多边形每条边的截距系数。
所述线性化处理模块304具体包括：
线性化处理单元，用于对车辆非线性运动学模型进行泰勒近似展开；并基于对偶法对避障的约束条件进行凸优化近似，完成车辆非线性运动学模型和避障的约束条件的线性化处理。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种面向自动驾驶车辆的实时轨迹规划方法，其特征在于，包括：
确定自动驾驶车辆的车辆非线性运动学模型；
根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；
获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件；
对车辆非线性运动学模型和避障的约束条件进行线性化处理；
根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；
根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划；
所述获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件，具体包括以下公式：
Figure DEST_PATH_IMAGE001
；
Figure DEST_PATH_IMAGE002
；
避撞约束为：
Figure DEST_PATH_IMAGE003
；
Figure DEST_PATH_IMAGE004
；
以模型预测控制为框架的运动规划包括两个子步骤：
通过对偶变量
Figure DEST_PATH_IMAGE005
求解自动驾驶车辆与周围物体的最小距离：
Figure DEST_PATH_IMAGE006
；
求解目标函数获得最优控制序列：
Figure DEST_PATH_IMAGE007
；
Figure DEST_PATH_IMAGE008
；
Figure DEST_PATH_IMAGE009
；
Figure DEST_PATH_IMAGE010
；
其中，
Figure DEST_PATH_IMAGE011
为自动驾驶车辆设定范围内第m个物体所占的空间，m为物体的编号，m=0为自动驾驶车辆，M为自动驾驶车辆设定范围内的所有物体，
Figure DEST_PATH_IMAGE012
为自动驾驶车辆的形状，
Figure DEST_PATH_IMAGE013
为自动驾驶车辆设定范围内物体所占的空间，
Figure DEST_PATH_IMAGE014
为自动驾驶车辆与设定范围内物体的最小距离，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，z为多边形物体每条边对应线性函数自变量，
Figure DEST_PATH_IMAGE015
为自变量的实数域范围，
Figure DEST_PATH_IMAGE016
为多边形每条边的斜率系数，
Figure DEST_PATH_IMAGE017
为多边形每条边的截距系数，
Figure DEST_PATH_IMAGE018
为对偶问题的对偶变量，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，x k 为k时刻的状态变量，u k 为k时刻的控制变量，
Figure DEST_PATH_IMAGE019
为k时刻的控制变量的变化率，x min 为状态变量最小值，x max 为状态变量最大值，u min 为控制变量最小值，u max 为控制变量最大值，
Figure DEST_PATH_IMAGE020
为控制变量的变化率最小值，
Figure DEST_PATH_IMAGE021
为控制变量的变化率最大值，
Figure DEST_PATH_IMAGE022
为自动驾驶车辆在预测时间窗口N步内的初始既定占据空间，
Figure DEST_PATH_IMAGE023
表示对偶范数，x为状态变量，u为控制变量，Q和R分别表示对状态变量和控制变量的加权系数矩阵，P是末点时刻的加权系数矩阵，x ref 为需要跟随的既定路线及速度轨迹，
Figure DEST_PATH_IMAGE024
为对应的惩罚系数，s k,m 为软约束对应的松弛变量，A k 和B k 分别为状态变量和控制变量的雅可比矩阵，C k 为残差项。
2.根据权利要求1所述的一种面向自动驾驶车辆的实时轨迹规划方法，其特征在于，所述确定自动驾驶车辆的车辆非线性运动学模型，具体包括：
利用公式
Figure DEST_PATH_IMAGE025
确定自动驾驶车辆的车辆非线性运动学模型；
其中，p x 和p y 分别为车辆后轴中心点在惯性坐标系下的横纵坐标，
Figure DEST_PATH_IMAGE026
和
Figure DEST_PATH_IMAGE027
为p x 和p y 的导数，v为车辆速度，
Figure DEST_PATH_IMAGE028
为车辆加速度，a为车辆加速度，
Figure DEST_PATH_IMAGE029
为偏航角，
Figure DEST_PATH_IMAGE030
为
Figure 478222DEST_PATH_IMAGE029
的导数，
Figure DEST_PATH_IMAGE031
表示航向角，L表示轴距，e为后轴中心点距离对应参考轨迹的横向距离，
Figure DEST_PATH_IMAGE032
为e的导数，
Figure DEST_PATH_IMAGE033
为相对偏航角，
Figure DEST_PATH_IMAGE034
为
Figure 644630DEST_PATH_IMAGE033
导数，
Figure DEST_PATH_IMAGE035
为参考轨迹的偏航角速率。
3.根据权利要求1所述的一种面向自动驾驶车辆的实时轨迹规划方法，其特征在于，所述根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件，具体包括：
根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率；
根据状态变量以及控制变量确定在连续时间空间下的动力学模型；
基于欧拉近似方法对动力学模型进行离散化处理。
4.根据权利要求1所述的一种面向自动驾驶车辆的实时轨迹规划方法，其特征在于，所述对车辆非线性运动学模型和避障的约束条件进行线性化处理，具体包括：
对车辆非线性运动学模型进行泰勒近似展开；并基于对偶法对避障的约束条件进行凸优化近似，完成车辆非线性运动学模型和避障的约束条件的线性化处理。
5.一种面向自动驾驶车辆的实时轨迹规划系统，其特征在于，包括：
车辆非线性运动学模型确定模块，用于确定自动驾驶车辆的车辆非线性运动学模型；
约束条件第一确定模块，用于根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率的约束条件；
约束条件第二确定模块，用于获取自动驾驶车辆的多边形函数；并根据自动驾驶车辆的多边形函数确定避障的约束条件；
线性化处理模块，用于对车辆非线性运动学模型和避障的约束条件进行线性化处理；
运动控制目标函数及问题模型确定模块，用于根据线性化处理后的车辆非线性运动学模型和线性化处理后的避障的约束条件以及状态变量、控制变量以及控制变量的变化率的约束条件建立以模型预测控制为框架的运动控制目标函数及问题模型；
实时轨迹规划完成模块，用于根据以模型预测控制为框架的运动控制目标函数及问题模型完成实时轨迹规划；
所述约束条件第二确定模块具体包括以下公式：
Figure 532689DEST_PATH_IMAGE001
；
Figure 600003DEST_PATH_IMAGE002
；
避撞约束为：
Figure DEST_PATH_IMAGE036
；
Figure 883216DEST_PATH_IMAGE004
；
以模型预测控制为框架的运动规划包括两个子步骤：
通过对偶变量
Figure 377783DEST_PATH_IMAGE005
求解自动驾驶车辆与周围物体的最小距离：
Figure 815455DEST_PATH_IMAGE006
；
求解目标函数获得最优控制序列：
Figure 811093DEST_PATH_IMAGE007
；
Figure 152075DEST_PATH_IMAGE008
；
Figure DEST_PATH_IMAGE037
；
Figure 755226DEST_PATH_IMAGE010
；
其中，
Figure 680195DEST_PATH_IMAGE011
为自动驾驶车辆设定范围内第m个物体所占的空间，m为物体的编号，m=0为自动驾驶车辆，M为自动驾驶车辆设定范围内的所有物体，
Figure 213944DEST_PATH_IMAGE012
为自动驾驶车辆的形状，
Figure 409433DEST_PATH_IMAGE013
为自动驾驶车辆设定范围内物体所占的空间，
Figure 511382DEST_PATH_IMAGE014
为自动驾驶车辆与设定范围内物体的最小距离，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，z为多边形物体每条边对应线性函数自变量，
Figure 690690DEST_PATH_IMAGE015
为自变量的实数域范围，
Figure 136453DEST_PATH_IMAGE016
为多边形每条边的斜率系数，
Figure 576661DEST_PATH_IMAGE017
为多边形每条边的截距系数，
Figure 849511DEST_PATH_IMAGE018
为对偶问题的对偶变量，d min 为自动驾驶车辆与设定范围内物体允许的最小距离，d min >0，x k 为k时刻的状态变量，u k 为k时刻的控制变量，
Figure 250536DEST_PATH_IMAGE019
为k时刻的控制变量的变化率，x min 为状态变量最小值，x max 为状态变量最大值，u min 为控制变量最小值，u max 为控制变量最大值，
Figure 1455DEST_PATH_IMAGE020
为控制变量的变化率最小值，
Figure 935650DEST_PATH_IMAGE021
为控制变量的变化率最大值，
Figure 379401DEST_PATH_IMAGE022
为自动驾驶车辆在预测时间窗口N步内的初始既定占据空间，
Figure 126777DEST_PATH_IMAGE023
表示对偶范数，x为状态变量，u为控制变量，Q和R分别表示对状态变量和控制变量的加权系数矩阵，P是末点时刻的加权系数矩阵，x ref 为需要跟随的既定路线及速度轨迹，
Figure 681386DEST_PATH_IMAGE024
为对应的惩罚系数，s k,m 为软约束对应的松弛变量，A k 和B k 分别为状态变量和控制变量的雅可比矩阵，C k 为残差项。
6.根据权利要求5所述的一种面向自动驾驶车辆的实时轨迹规划系统，其特征在于，所述车辆非线性运动学模型确定模块具体包括：
利用公式
Figure 705974DEST_PATH_IMAGE025
确定自动驾驶车辆的车辆非线性运动学模型；
其中，p x 和p y 分别为车辆后轴中心点在惯性坐标系下的横纵坐标，
Figure 84741DEST_PATH_IMAGE026
和
Figure 194779DEST_PATH_IMAGE027
为p x 和p y 的导数，v为车辆速度，
Figure 412134DEST_PATH_IMAGE028
为车辆加速度，a为车辆加速度，
Figure 556807DEST_PATH_IMAGE029
为偏航角，
Figure 342361DEST_PATH_IMAGE030
为
Figure 703810DEST_PATH_IMAGE029
的导数，
Figure 600222DEST_PATH_IMAGE031
表示航向角，L表示轴距，e为后轴中心点距离对应参考轨迹的横向距离，
Figure 458456DEST_PATH_IMAGE032
为e的导数，
Figure 414911DEST_PATH_IMAGE033
为相对偏航角，
Figure 765121DEST_PATH_IMAGE034
为
Figure 963759DEST_PATH_IMAGE033
导数，
Figure 817445DEST_PATH_IMAGE035
为参考轨迹的偏航角速率。
7.根据权利要求5所述的一种面向自动驾驶车辆的实时轨迹规划系统，其特征在于，所述约束条件第一确定模块具体包括：
变量确定单元，用于根据车辆非线性运动学模型确定状态变量、控制变量以及控制变量的变化率；
在连续时间空间下的动力学模型确定单元，用于根据状态变量以及控制变量确定在连续时间空间下的动力学模型；
离散化处理单元，用于基于欧拉近似方法对动力学模型进行离散化处理。
8.根据权利要求5所述的一种面向自动驾驶车辆的实时轨迹规划系统，其特征在于，所述线性化处理模块具体包括：
线性化处理单元，用于对车辆非线性运动学模型进行泰勒近似展开；并基于对偶法对避障的约束条件进行凸优化近似，完成车辆非线性运动学模型和避障的约束条件的线性化处理。
CN202210308275.6A 2022-03-28 2022-03-28 一种面向自动驾驶车辆的实时轨迹规划方法及系统 Active CN114415694B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210308275.6A CN114415694B (zh) 2022-03-28 2022-03-28 一种面向自动驾驶车辆的实时轨迹规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210308275.6A CN114415694B (zh) 2022-03-28 2022-03-28 一种面向自动驾驶车辆的实时轨迹规划方法及系统

Publications (2)

Publication Number Publication Date
CN114415694A CN114415694A (zh) 2022-04-29
CN114415694B true CN114415694B (zh) 2022-08-19

Family

ID=81264156

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210308275.6A Active CN114415694B (zh) 2022-03-28 2022-03-28 一种面向自动驾驶车辆的实时轨迹规划方法及系统

Country Status (1)

Country Link
CN (1) CN114415694B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114995465B (zh) * 2022-08-02 2022-11-15 北京理工大学 一种考虑车辆运动能力的多无人车运动规划方法和系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SE539429C2 (en) * 2015-12-15 2017-09-19 Greater Than S A Method and system for assessing the trip performance of a driver
CN109508007A (zh) * 2018-12-11 2019-03-22 东南大学 一种基于多源信息融合的农机轨迹跟踪、避障系统及方法
CN113419521A (zh) * 2020-03-05 2021-09-21 广州市香港科大霍英东研究院 一种自动驾驶车辆局部避障路径的规划及跟踪方法
CN111413966B (zh) * 2020-03-12 2022-04-29 天津大学 一种递进式模型预测无人驾驶规划跟踪协同控制方法
CN113238563B (zh) * 2021-06-04 2022-07-22 重庆大学 一种高实时性的自动驾驶运动规划方法

Also Published As

Publication number Publication date
CN114415694A (zh) 2022-04-29

Similar Documents

Publication Publication Date Title
CN113276848B (zh) 一种智能驾驶换道避障轨迹规划、跟踪控制方法及系统
CN111845774B (zh) 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
CN108227491B (zh) 一种基于滑模神经网络的智能车轨迹跟踪控制方法
CN114379583B (zh) 一种基于神经网络动力学模型的自动驾驶车辆轨迹跟踪系统及方法
Kosecka et al. A comparative study of vision-based lateral control strategies for autonomous highway driving
Zhou et al. Event-triggered model predictive control for autonomous vehicle path tracking: Validation using CARLA simulator
Xu et al. Model predictive control for lane keeping system in autonomous vehicle
CN110780594A (zh) 一种智能车的路径跟踪方法及系统
CN113581201B (zh) 一种基于势场模型的无人汽车避撞控制方法及系统
CN114415694B (zh) 一种面向自动驾驶车辆的实时轨迹规划方法及系统
CN116465427B (zh) 一种基于时空风险量化的智能车辆换道避障路径规划方法
Na et al. Experimental evaluation of a game-theoretic human driver steering control model
Hegedüs et al. Motion planning for highly automated road vehicles with a hybrid approach using nonlinear optimization and artificial neural networks
CN113465625A (zh) 局部路径规划方法和装置
CN115303289A (zh) 一种基于深度高斯车辆动力学模型、训练方法、智能汽车轨迹跟踪控制方法及终端设备
CN114442630B (zh) 一种基于强化学习和模型预测的智能车规划控制方法
CN116048081A (zh) 一种考虑安全边界约束的自动驾驶车辆决策与规控方法
Choi et al. Waypoint tracking predictive control with vehicle speed variation
Ko et al. Integrated path planning and tracking control of autonomous vehicle for collision avoidance based on model predictive control and potential field
CN113184040B (zh) 一种基于驾驶人转向意图的无人车线控转向控制方法及系统
Xu et al. Autonomous vehicle active safety system based on path planning and predictive control
CN110654386B (zh) 弯道下多智能电动汽车协作式巡航纵横向综合控制方法
CN111857112B (zh) 一种汽车局部路径规划方法及电子设备
CN115782904A (zh) 一种基于非合作博弈的智能汽车换道轨迹规划方法
CN112596516B (zh) 基于Dubins曲线的多车队形切换方法

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