CN116300478B - 基于变量分离微分动态规划的轨迹生成方法、介质及设备 - Google Patents
基于变量分离微分动态规划的轨迹生成方法、介质及设备 Download PDFInfo
- Publication number
- CN116300478B CN116300478B CN202310572245.0A CN202310572245A CN116300478B CN 116300478 B CN116300478 B CN 116300478B CN 202310572245 A CN202310572245 A CN 202310572245A CN 116300478 B CN116300478 B CN 116300478B
- Authority
- CN
- China
- Prior art keywords
- dynamic programming
- track
- differential dynamic
- variable separation
- automatic driving
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 78
- 238000000926 separation method Methods 0.000 title claims abstract description 56
- 238000005457 optimization Methods 0.000 claims abstract description 45
- 230000008569 process Effects 0.000 claims abstract description 12
- 230000006870 function Effects 0.000 claims description 38
- 238000004891 communication Methods 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 12
- 230000001351 cycling effect Effects 0.000 claims description 10
- 230000003416 augmentation Effects 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 7
- 238000009795 derivation Methods 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000014509 gene expression Effects 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 3
- 239000012976 trial formulation Substances 0.000 claims description 3
- 230000003190 augmentative effect Effects 0.000 claims 2
- 230000004069 differentiation Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 17
- 238000004422 calculation algorithm Methods 0.000 abstract description 6
- 238000013461 design Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000007429 general method Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- 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)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
- Liquid Crystal (AREA)
Abstract
本发明公开一种基于变量分离微分动态规划的轨迹生成方法、介质及设备,该方法通过构建有约束的轨迹优化问题,并采用变量分离法将不等式约束处理为等式约束,结合更新拉格朗日乘子和更新惩罚项因子对轨迹优化问题进行求解,即可生成车体动力学光滑的轨迹,采用变量分离法对微分动态规划进行改造计算效率极高,且不需要部署优化算法包,在空间存储上具有明显优势。
Description
技术领域
本发明属于自动驾驶车辆轨迹规划与控制技术领域,具体涉及到一种基于变量分离微分动态规划的自动驾驶轨迹生成方法、介质及设备。
背景技术
随着车辆信息化与智能化技术的飞速发展,自动驾驶成为最炙手可热的汽车应用方向。通常,车辆均行驶在结构化的道路上,根据结构化道路的中心点、道路边界等信息可以构建一系列车辆轨迹的参考点,进而生成车辆行驶的参考轨迹,有效降低车辆规划路径的查找时间和复杂度,进而降低整个规划算法的负载,意义十分突出。
现有的参考轨迹的生成大致可以分为两大类:直接求解法和间接求解法。直接求解法通常使用非线性求解器,比如SNOPT或者IPOPT进行求解,比如直接配置法(DirectCollocation, DIRCOL),使用Hermite-Simpson方法来对系统动态和代价函数同时进行积分;相关细节可以参考Matthew Kelly的论文“An Introduction to TrajectoryOptimization: How to Do Your Own Direct Collocation”。百度的知名项目Apollo中即采用的直接配置法,其部署了IPOPT优化算法包,对空间存储有一定的要求,且求解效率低。不同于直接求解法,间接配置法会利用系统动力学模型的马尔可夫传递特性,并将该特性在参考轨迹生成问题求解的全域内推行,包含代价函数和约束函数。现存的最先进的相关方法有,基于barrier函数的有约束微分动态规划方法,详见J. Chen等人的论文“Autonomous driving motion planning with constrained iterative lqr”, IEEETIV, 2018;基于Karush-KuhnTucker (KKT)条件的有约束微分动态规划方法,详见Y.Aoyama等人的论文“Constrained differential dynamic programming revisited”,IEEEICRA,2021;基于交替方向乘子法(Alternating Direction Methods of Multipliers)的有约束微分动态规划方法,详见J. Ma等人的论文“Alternating direction method ofmultipliers for constrained iterative lqr in autonomous driving”,IEEE TITS,2022。这些方法存在着一些不足之处,基于barrier函数的方法计算速度慢,效率低下,基于KKT条件的方法理论完备,但实用性很低,难于展开工程实践;基于ADMM的方法基本解决了计算效率和工程实践可用性的问题,但其引入的双重优化变量,优化步骤多了一个步骤,故而其计算效率仍然有提升的空间,最重要的,上述这些方法都是通用方法,并未面向自动驾驶的轨迹生成应用进行定向设计及优化。
发明内容
针对上述问题,本发明的主要目的在于设计一种基于变量分离微分动态规划的自动驾驶轨迹生成方法、介质及设备,采用变量分离法对微分动态规划进行改造,以进一步提升计算效率,并面向轨迹生成应用进行定向设计,形成生成效率远胜百度Apollo的轨迹生成方法。
为了实现上述目的本发明采用如下技术方案:
一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,该方法包括如下步骤:
步骤1:获取结构化道路信息,并对道路空间信息进行预处理,使道路空间信息具有笛卡尔坐标系下的具体坐标值;所述的道路信息包括路沿、车道、车道中心点坐标;
其中,以车道中心点坐标为轨迹生成追踪目标,以路沿及车体动力学模型为约束信息;
步骤2:结合车体动力学模型、轨迹追踪目标函数、约束函数,构建有约束的轨迹优化问题;所述约束包含等式约束和不等式约束;
步骤3:针对步骤2中构建的轨迹优化问题采用变量分离法进行约束处理的迭代优化,在每一轮迭代优化中使用微分动态规划进行优化轨迹的求解,最终得到车体动力学光滑的生成轨迹。
作为本发明的进一步描述,所述的车体动力学模型构建如下:
,
其中,和/>分别表示车体轨迹的横向偏移和纵向偏移量,/>表示车速,/>表示车辆偏航角,/>表示曲率,/>表示曲率变化率;/>表示车体动力学系统状态,表示相应的系统控制,则所述的车体动力学模型可以表示为:
,
针对上述的车体动力学模型使用四阶龙格库塔法进行离散化,可得离散化的车体动力学模型如下:
,
其中,和/>分别表示离散化之后/>时刻的系统状态和控制变量。
作为本发明的进一步描述,步骤1中,待追踪目标中心点为N个,则待追踪目标中心点的坐标数据集;
相应的,所述的目标函数如下:
,
其中,表示终点代价(cost),/>表示第/>步的过程代价;
式中的与/>分别表示如下:
,
,
其中,表示状态权重矩阵,且为半正定矩阵,/>表示控制权重矩阵,且为正定矩阵,/>和/>分别表示状态权重梯度向量和控制权重梯度向量,/>表示常数。
作为本发明的进一步描述,所述的约束函数构建如下:
,
其中,表示第/>个时间步的约束函数,/>表示终点约束函数。
作为本发明的进一步描述,所述有约束的轨迹优化问题构建如下:
,
,
,
构建有约束的轨迹优化问题后,即可针对轨迹优化问题进行求解。
作为本发明的进一步描述,步骤3中,对构建的轨迹优化问题进行求解,得到车体动力学光滑的生成轨迹,包括如下步骤:
步骤3.1:采用变量分离法将不等式约束处理为等式约束;
定义一个辅助变量,然后定义一个函数/>,其形式如下,
,
则目标函数可以增广为如下形式:
,
其中,和/>分别表示第/>步的拉格朗日乘子和惩罚因子;然后通过微分动态规划进行轨迹求解,得到反馈增益/>和前馈增益/>,并进行微分动态规划收敛判断;
步骤3.2:更新拉格朗日乘子;
,
,
其中,表示变量分离法迭代步,根据新的拉格朗日乘子更新/>,得到/>,当与/>的相对差异小于设定的变量分析法收敛阈值/>时,整个求解流程结束;若/>与/>的相对差异未小于设定的变量分析法收敛阈值/>时,则进入步骤3.3;
步骤3.3:更新惩罚项因子,并循环步骤3.1至步骤3.2,求解流程结束循环或者循环达到变量分离法设定的最大次数,求解结束。
作为本发明的进一步描述,步骤3.1中,通过微分动态规划进行轨迹求解,所述的微分动态规划包括反向传播、前向传播和微分动态规划收敛判断,具体步骤如下:
步骤3.1.1:进入反向传播步骤,对车体动力学模型进行线性化,保留一阶雅可比项,即:
,
记,/>,上式可以化为,
,
对增广代价函数进行线性化,保留一阶雅可比项和二阶海森项,即:
,
等同于,
,
,
其中,,/>,/>,/>,,/>中的相关变量计算方式如下,
,
,
,
,
,
其中,分别表示总代价函数/>对/>进行序列求导,/>和/>表示第/>步代价函数/>对/>进行序列求导,/>和/>即为/>和/>;
如此,对进行极小值求取等同于对/>进行极小值求取,可得,
,
并用表示反馈增益,/>表示前馈增益;
步骤3.1.2:进入前向传播步骤,前馈增益调节变量,对控制律/>进行调整,则:
,
进行试配,寻找/>下降最大的相应值,即可得到本次前向滚出的最优控制律;由车体动力学模型/>递推出轨迹序列;
步骤3.1.3:微分动态规划收敛判断,重复步骤3.1.1至步骤3.1.2,直至的下降幅度小于设定的微分动态规划收敛阈值/>,微分动态规划求解结束;若/>的下降幅度未小于设定的微分动态规划收敛阈值/>,则进入步骤3.2,更新拉格朗日乘子。
作为本发明的进一步描述,步骤3.3中,更新惩罚项因子,公式如下:
,
,
其中,表示惩罚项因子的伸缩系数,根据如下算式更新,
,
其中,表示惩罚项因子伸缩系数,循环步骤3.1至步骤3.3直至步骤3.2求解成功结束循环或者循环达到变量分离法设定的最大次数,求解结束,即得到车体动力学光滑的生成轨迹。
一种基于变量分离微分动态规划的自动驾驶轨迹生成设备,包括处理器、通信接口、存储器和通信总线,其中,所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信,所述存储器,用于存储计算机程序;
所述处理器,用于通过运行所述存储器上所存储的所述计算机程序来执行上述变量分离微分动态规划的自动驾驶轨迹生成方法步骤。
一种计算机可读的存储介质,所述存储介质中存储有计算机程序,其中,所述计算机程序被处理器执行时实现上述变量分离微分动态规划的自动驾驶轨迹生成方法步骤。
相对于现有技术,本发明的技术效果为:
本发明提供了一种基于变量分离微分动态规划的自动驾驶轨迹生成方法、介质及设备,该方法通过构建有约束的轨迹优化问题,并对轨迹优化问题进行求解,即可生成车体动力学光滑的轨迹,采用变量分离法对微分动态规划进行改造计算效率极高,且不需要部署优化算法包,在空间存储上具有明显优势。
附图说明
图1为本发明的整体流程示意图;
图2为本发明的方法应用在实际路段及生成轨迹示意图;
图3为本发明的方法与现有技术方案Apollo计算效率对比示意图。
具体实施方式
下面结合附图对本发明进行详细描述:
在本发明的一种实施例中,公开了一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,参考图1所示,该方法包括如下步骤:
步骤1:获取结构化道路信息,并对道路空间信息进行预处理,使道路空间信息具有笛卡尔坐标系下的具体坐标值;所述的道路信息包括路沿、车道、车道中心点坐标;
其中,以车道中心点坐标为轨迹生成追踪目标,以路沿及车体动力学模型为约束信息;
步骤2:结合车体动力学模型、轨迹追踪目标函数、约束函数,构建有约束的轨迹优化问题;所述约束包含等式约束和不等式约束;
步骤3:针对步骤2中构建的轨迹优化问题采用变量分离法进行约束处理的迭代优化,在每一轮迭代优化中使用微分动态规划进行优化轨迹的求解,最终得到车体动力学光滑的生成轨迹。
具体的,本实施例,针对上述的轨迹优化问题构建和求解进行详细说明,具体如下:
需要说明的是,本实施例的目的是为了生成车体动力学光滑的轨迹,然而在生成过程中,会存在一些约束,因此,本实施例构建有约束的轨迹优化问题,并针对有约束的轨迹优化问题进行求解,即可得到车体动力学光滑的轨迹。
具体的,一个有约束的优化的轨迹优化问题,需要的组件包括:运载体动力学模型,(即车体动力学模型)、轨迹追踪目标函数、约束函数;因此,结合车体动力学模型、轨迹追踪目标函数、约束函数,构建有约束的轨迹优化问题。
具体的,本实施例在步骤1中,将待追踪目标中心点设定为N个,则待追踪目标中心点的坐标数据集;
一、所述的车体动力学模型构建如下:
,
其中,和/>分别表示车体轨迹的横向偏移和纵向偏移量,/>表示车速,/>表示车辆偏航角,/>表示曲率,/>表示曲率变化率;/>表示车体动力学系统状态,表示相应的系统控制,则所述的车体动力学模型可以表示为:
,
针对上述的车体动力学模型使用四阶龙格库塔法进行离散化,可得离散化的车体动力学模型如下:
,
其中,和/>分别表示离散化之后/>时刻的系统状态和控制变量。
二、所述的目标函数如下:
,
其中,表示终点代价(cost),/>表示第/>步的过程代价;
式中的与/>分别表示如下:
,
,
其中,表示状态权重矩阵,且为半正定矩阵,/>表示控制权重矩阵,且为正定矩阵,/>和/>分别表示状态权重梯度向量和控制权重梯度向量,/>表示常数。
三、所述的约束函数构建如下:
,
其中,表示第/>个时间步的约束函数,/>表示终点约束函数。
四、所述的有约束的轨迹优化问题构建如下:
,
,
,
至此,通过上述公开的内容,构建了有约束的轨迹优化问题,进而即可针对轨迹优化问题进行求解。
具体的,本实施例,在步骤3中对构建的轨迹优化问题进行求解,得到车体动力学光滑的生成轨迹,包括如下步骤:
步骤3.1:采用变量分离法将不等式约束处理为等式约束;
定义一个辅助变量,然后定义一个函数/>,其形式如下,
,
则目标函数可以增广为如下形式:
,
其中,和/>分别表示第/>步的拉格朗日乘子和惩罚因子;然后通过微分动态规划进行轨迹求解,得到反馈增益/>和前馈增益/>,并进行微分动态规划收敛判断;
本实施例所述的微分动态规划包括反向传播、前向传播和微分动态规划收敛判断,具体步骤如下:
步骤3.1.1:进入反向传播步骤,对车体动力学模型进行线性化,保留一阶雅可比项,即:
,
记,/>,上式可以化为,
,
对增广代价函数进行线性化,保留一阶雅可比项和二阶海森项,即:
,
等同于,
,
,
其中,,/>,/>,/>,,/>中的相关变量计算方式如下,
,
,
,
,
,
其中,分别表示总代价函数/>对/>进行序列求导,/>和/>表示第/>步代价函数/>对/>进行序列求导,/>和/>即为/>和/>;
如此,对进行极小值求取等同于对/>进行极小值求取,可得,
,
并用表示反馈增益,/>表示前馈增益;
步骤3.1.2:进入前向传播步骤,前馈增益调节变量,对控制律/>进行调整,则:
,
进行试配,寻找/>下降最大的相应值,即可得到本次前向滚出的最优控制律;由车体动力学模型/>递推出轨迹序列;
步骤3.1.3:微分动态规划收敛判断,重复步骤3.1.1至步骤3.1.2,直至的下降幅度小于设定的微分动态规划收敛阈值/>,微分动态规划求解结束;若/>的下降幅度未小于设定的微分动态规划收敛阈值/>,则进入步骤3.2。
步骤3.2:更新拉格朗日乘子;
,
,
其中,表示变量分离法迭代步,根据新的拉格朗日乘子更新/>,得到/>,当与/>的相对差异小于设定的变量分析法收敛阈值/>时,整个求解流程结束;若/>与/>的相对差异未小于设定的变量分析法收敛阈值/>时,则进入步骤3.3。
步骤3.3:更新惩罚项因子,并循环步骤3.1至步骤3.2,求解流程结束循环或者循环达到变量分离法设定的最大次数,求解结束;
具体的,更新惩罚项因子,公式如下:
,
,
其中,表示惩罚项因子的伸缩系数,根据如下算式更新,
,
其中,表示惩罚项因子伸缩系数,循环步骤3.1至步骤3.3直至步骤3.2求解成功结束循环或者循环达到变量分离法设定的最大次数,求解结束,即得到车体动力学光滑的生成轨迹。
需要说明的是,本实施例上述的收敛包括变量分离法的收敛和微分动态规划的收敛;上述的变量分离的收敛是每次拉格朗日乘子和惩罚因子更新以后计算约束突破的值,如果约束突破的值小于设定的变量分离法收敛阈值,则满足了约束的需求,即满足收敛;上述的微分动态规划的收敛以满足代价函数cost下降变化小于设定的微分动态规划收敛阈值/>。
通过上述公开的内容可以得知,本实施例实现了反向传播计算控制增益,和正向传播计算轨迹,通过设置变量分离法收敛阈值,进行判定收敛结束后,约束突破是否小于设定的变量分离法收敛阈值/>,当小于设定的变量分离法收敛阈值/>即得到求解/>和/>;当约束突破不小于设定的变量分离法收敛阈值/>则更新惩罚项因子和惩罚项伸缩参数,重新进入微分动态规划求解。
综合上述公开的内容,上述公开的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,通过构建有约束的轨迹优化问题,并对轨迹优化问题进行求解,即可生成车体动力学光滑的轨迹,采用变量分离法对微分动态规划进行改造,以进一步提升计算效率,计算效率极高,且不需要部署优化算法包,在空间存储上具有明显优势,并面向自动驾驶轨迹生成应用进行定向设计,形成生成效率远胜百度Apollo的轨迹生成方法。
通过本实施例公开的方法,应用在实际的轨迹生成过程中,实验环境构建如下:
一台笔记本电脑,CPU配置为i7-11800H,内存为32G,部署ubuntu 22.04操作系统,ros2框架,权重矩阵统一设置为:
,
所设置约束为边界约束,即,
,
,
其中, ,/>,/>,。进行实验的路段及生成轨迹如图2所示,道路包含一个U型弯和一个曲率较大的弯道,点状虚线标识道路拓扑结构,圆点处表示信号灯,需要计算的三根轨迹线长度分别为549m,552m和449m。实线表示的即为所述变量分离微分动态规划方法生成的轨迹线,在相同实验条件下,复现百度apollo基于IPOPT的直接配置法方案,进行计算效率对比,如图3所示,三根轨迹的两种方法计算时效均以柱状图表示,可以明显看出,计算效率提升上百倍。
本发明的通过上述公开的实施例,相对于现有技术具有以下优势:
1.本发明的技术方案计算效率极高,在相同实验条件下与百度Apollo的相近方案进行对比,计算效率提升上百倍;
2.本发明的技术方案相较于直接配置法方案,在空间存储上同样具有明显优势,不需要部署优化算法包,这一优势对于空间关键型(rom-critical)系统至关重要;
3.本发明的技术方案与同期最先进的学术方案相比,在实用性和算法计算流程上均具有优势。
在本发明的另一种实施例中,还包括一种基于变量分离微分动态规划的自动驾驶轨迹生成设备,该轨迹生成设备可以包括处理器以及存储由计算机程序指令的存储器。
具体的,本实施例,上述处理器可以包括中央处理器(CPU),或者特定集成电路,或者可以被配置成本实施例的一个或多个集成电路;上述的存储器可以包括用于数据或指令的大容量存储器,针对该存储器,包括但不限于硬盘驱动器(Hard Disk Drive,HDD)、软盘驱动器、闪存、光盘、磁光盘、磁带或通用串行总线(Universal Serial Bus,USB)驱动器或者两个或更多个以上这些的组合;在适当的情况下,存储器可包括可移除或不可移除(或固定)的介质;在特定实施例中,存储器是非易失性固态存储器。在特定实施例中,存储器包括只读存储器(ROM)。在适当的情况下,该ROM可以是掩模编程的ROM、可编程ROM(PROM)、可擦除PROM(EPROM)、电可擦除PROM(EEPROM)、电可改写ROM(EAROM)或闪存或者两个或更多个以上这些的组合。
上述处理器通过读取并执行存储器中存储的计算机程序指令,实现本发明上述公开的基于变量分离微分动态规划的自动驾驶轨迹生成方法。
还需要说明的是,本实施例的电子设备还可以包括通信接口和通信总线。其中,处理器、存储器、通信接口通过通信总线连接并完成相互间的通信。通信接口,主要用于实现本发明实施例中各单元、各模块、装置或设备之间的通信。
上述的通信总线包括硬件、软件或者软硬件两者的结合,将在线数据流量设备的部件彼此耦接在一起。在适当的情况下,通信总线可以包括一个或多个总线。
另外,结合上述实施例中变量分离微分动态规划的自动驾驶轨迹生成方法,本发明的实施例可提供一种计算机存储介质来实现,该计算机存储介质上存储有计算机程序指令;该计算机程序指令被处理器执行上述基于变量分离微分动态规划的自动驾驶轨迹生成方法。
需要明确的是,本发明并不限于上述公开的方法、系统、设备,还包括本领域技术人员基于本发明的思路后作出的各种改变、修改和添加,或者改变步骤之间的顺序。
本发明当以硬件的方式实现时,其可以是电子电路、专用集成电路,适当的固件、插件、功能卡等;当以软件方式实现时,本发明的元素是被用于执行所需任务的程序或者代码段,程序或者代码段可以存储在机器可读介质中,或者通过载波中携带的数据信号在传输介质或者通信链路上传,“机器可读介质”可以包括能够存储或传输信息的任何介质,例如:电子电路、半导体存储器设备、ROM、闪存、可擦除ROM(EROM)、软盘、光盘、硬盘、光纤介质、射频链路等待。代码段可以经由诸如因特网、内联网等的计算机网络被下载。
以上实施例仅用以说明本发明的技术方案而非限制,本领域普通技术人员对本发明的技术方案所做的其他修改或者等同替换,只要不脱离本发明技术方案的精神和范围,均应涵盖在本发明的权利要求范围当中。
Claims (10)
1.一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于,该方法包括如下步骤:
步骤1:获取结构化道路信息,并对道路空间信息进行预处理,使道路空间信息具有笛卡尔坐标系下的具体坐标值;所述的道路信息包括路沿、车道、车道中心点坐标;
其中,以车道中心点坐标为轨迹生成追踪目标,以路沿及车体动力学模型为约束信息;
步骤2:结合车体动力学模型、轨迹追踪目标函数、约束函数,构建有约束的轨迹优化问题;所述约束包含等式约束和不等式约束;
步骤3:针对步骤2中构建的轨迹优化问题,采用变量分离法进行约束处理的迭代优化,所述变量分离法包含以下步骤,首先辅助变量将所有不等式约束转换为等式约束;然后将等式约束扩增至目标优化函数,得到包含了拉格朗日增广项和惩罚项的增广目标优化函数;在每一轮迭代优化中使用微分动态规划进行针对增广目标优化函数的轨迹优化问题的求解;然后依次更新拉格朗日乘子和惩罚项因子,直到变量分离法优化流程结束,最终得到车体动力学光滑的生成轨迹。
2.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:所述的车体动力学模型构建如下:
,
其中,和/>分别表示车体轨迹的横向偏移和纵向偏移量,/>表示车速,/>表示车辆偏航角,/>表示曲率,/>表示曲率变化率;/>表示车体动力学系统状态,表示相应的系统控制,则所述的车体动力学模型可以表示为:
,
针对上述的车体动力学模型使用四阶龙格库塔法进行离散化,可得离散化的车体动力学模型如下:
,
其中,和/>分别表示离散化之后/>时刻的系统状态和控制变量。
3.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:步骤1中,待追踪目标中心点为N个,则待追踪目标中心点的坐标数据集;
相应的,所述的目标函数如下:
,
其中,表示终点代价(cost),/>表示第/>步的过程代价;
式中的与/>分别表示如下:
,
,
其中,表示状态权重矩阵,且为半正定矩阵,/>表示控制权重矩阵,且为正定矩阵,/>和/>分别表示状态权重梯度向量和控制权重梯度向量,/>表示常数。
4.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:所述的约束函数构建如下:
,
其中,表示第/>个时间步的约束函数,/>表示终点约束函数。
5.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:所述有约束的轨迹优化问题构建如下:
,
,
,
构建有约束的轨迹优化问题后,即可针对轨迹优化问题进行求解。
6.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:步骤3中,对构建的轨迹优化问题进行求解,得到车体动力学光滑的生成轨迹,包括如下步骤:
步骤3.1:采用变量分离法将不等式约束处理为等式约束;
定义一个辅助变量,然后定义一个函数/>,其形式如下,
,
则目标函数可以增广为如下形式:
,
其中,和/>分别表示第/>步的拉格朗日乘子和惩罚因子;然后通过微分动态规划进行轨迹求解,得到反馈增益/>和前馈增益/>,并进行微分动态规划收敛判断;
步骤3.2:更新拉格朗日乘子;
,
,
其中,表示变量分离法迭代步,根据新的拉格朗日乘子更新/>,得到/>,当/>与的相对差异小于设定的变量分析法收敛阈值/>时,整个求解流程结束;若/>与/>的相对差异未小于设定的变量分析法收敛阈值/>时,则进入步骤3.3;
步骤3.3:更新惩罚项因子,并循环步骤3.1至步骤3.2,求解流程结束循环或者循环达到变量分离法设定的最大次数,求解结束。
7.根据权利要求6所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:步骤3.1中,通过微分动态规划进行轨迹求解,所述的微分动态规划包括反向传播、前向传播和微分动态规划收敛判断,具体步骤如下:
步骤3.1.1:进入反向传播步骤,对车体动力学模型进行线性化,保留一阶雅可比项,即:
,
记,/>,上式可以化为,
,
对增广代价函数进行线性化,保留一阶雅可比项和二阶海森项,即:
,
等同于,
,
,
其中,,/>,/>,/>,;/>中的相关变量计算方式如下,
,
,
,
,
,
其中,分别表示总代价函数/>对/>进行序列求导,和/>表示第/>步代价函数/>对/>进行序列求导,/>和/>即为/>和/>;
如此,对进行极小值求取等同于对/>进行极小值求取,可得,
,
并用表示反馈增益,/>表示前馈增益;
步骤3.1.2:进入前向传播步骤,前馈增益调节变量,对控制律/>进行调整,则:
,
进行试配,寻找/>下降最大的相应值,即可得到本次前向滚出的最优控制律;由动力学模型/>递推出轨迹序列/>;
步骤3.1.3:微分动态规划收敛判断,重复步骤3.1.1至步骤3.1.2,直至的下降幅度小于设定的微分动态规划收敛阈值/>,微分动态规划求解结束;若/>的下降幅度未小于设定的微分动态规划收敛阈值/>,则进入步骤3.2,更新拉格朗日乘子。
8.根据权利要求6所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法,其特征在于:步骤3.3中,更新惩罚项因子,公式如下:
,
,
其中,表示惩罚项因子的伸缩系数,根据如下算式更新,
,
其中,表示惩罚项因子伸缩系数,循环步骤3.1至步骤3.3直至步骤3.2求解成功结束循环或者循环达到变量分离法设定的最大次数,求解结束,即得到车体动力学光滑的生成轨迹。
9.一种基于变量分离微分动态规划的自动驾驶轨迹生成设备,包括处理器、通信接口、存储器和通信总线,其中,所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信,其特征在于,所述存储器,用于存储计算机程序;
所述处理器,用于通过运行所述存储器上所存储的所述计算机程序来执行权利要求1-8任一项中所述的基于变量分离微分动态规划的自动驾驶轨迹生成方法。
10.一种计算机可读的存储介质,其特征在于,所述存储介质中存储有计算机程序,其中,所述计算机程序被处理器执行时实现权利要求1-8任一项中所述的基于变量分离微分动态规划的自动驾驶轨迹生成方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310572245.0A CN116300478B (zh) | 2023-05-22 | 2023-05-22 | 基于变量分离微分动态规划的轨迹生成方法、介质及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310572245.0A CN116300478B (zh) | 2023-05-22 | 2023-05-22 | 基于变量分离微分动态规划的轨迹生成方法、介质及设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116300478A CN116300478A (zh) | 2023-06-23 |
CN116300478B true CN116300478B (zh) | 2023-08-18 |
Family
ID=86798210
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310572245.0A Active CN116300478B (zh) | 2023-05-22 | 2023-05-22 | 基于变量分离微分动态规划的轨迹生成方法、介质及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116300478B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104965517A (zh) * | 2015-07-07 | 2015-10-07 | 张耀伦 | 一种机器人笛卡尔空间轨迹的规划方法 |
CN105652664A (zh) * | 2016-02-29 | 2016-06-08 | 北京航空航天大学 | 一种基于鸽群优化的四旋翼无人机显式预测控制方法 |
CN110275434A (zh) * | 2019-05-20 | 2019-09-24 | 江苏大学 | 一种基于条件触发无偏mpc算法的独立悬架结构车辆控制方法 |
CN111258323A (zh) * | 2020-03-30 | 2020-06-09 | 华南理工大学 | 一种智能车辆轨迹规划与跟踪的联合控制方法 |
CN114312926A (zh) * | 2021-12-03 | 2022-04-12 | 北京交通大学 | 一种城市轨道交通列车运行调整方案优化方法和系统 |
CN114879508A (zh) * | 2022-05-31 | 2022-08-09 | 天津理工大学 | 一种基于模型预测控制的磨削机器人路径跟踪控制方法 |
CN115840451A (zh) * | 2022-12-12 | 2023-03-24 | 畅加风行(苏州)智能科技有限公司 | 狭窄通道无人驾驶拖挂避障转弯轨迹规划方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108445750B (zh) * | 2017-02-16 | 2022-04-08 | 法拉第未来公司 | 用于车辆运动规划的方法和系统 |
FR3080447B1 (fr) * | 2018-04-23 | 2020-04-17 | Inria Institut National De Recherche En Informatique Et En Automatique | Dispositif de localisation |
-
2023
- 2023-05-22 CN CN202310572245.0A patent/CN116300478B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104965517A (zh) * | 2015-07-07 | 2015-10-07 | 张耀伦 | 一种机器人笛卡尔空间轨迹的规划方法 |
CN105652664A (zh) * | 2016-02-29 | 2016-06-08 | 北京航空航天大学 | 一种基于鸽群优化的四旋翼无人机显式预测控制方法 |
CN110275434A (zh) * | 2019-05-20 | 2019-09-24 | 江苏大学 | 一种基于条件触发无偏mpc算法的独立悬架结构车辆控制方法 |
CN111258323A (zh) * | 2020-03-30 | 2020-06-09 | 华南理工大学 | 一种智能车辆轨迹规划与跟踪的联合控制方法 |
CN114312926A (zh) * | 2021-12-03 | 2022-04-12 | 北京交通大学 | 一种城市轨道交通列车运行调整方案优化方法和系统 |
CN114879508A (zh) * | 2022-05-31 | 2022-08-09 | 天津理工大学 | 一种基于模型预测控制的磨削机器人路径跟踪控制方法 |
CN115840451A (zh) * | 2022-12-12 | 2023-03-24 | 畅加风行(苏州)智能科技有限公司 | 狭窄通道无人驾驶拖挂避障转弯轨迹规划方法 |
Non-Patent Citations (1)
Title |
---|
易云飞 ; 蔡永乐 ; 董文永 ; 林郭隆 ; .求解带容量约束的车辆路径问题的改进伊藤算法.计算机科学.2013,(第05期),全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN116300478A (zh) | 2023-06-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Zhang et al. | Trajectory planning and tracking for autonomous vehicle based on state lattice and model predictive control | |
Rosolia et al. | Autonomous racing using learning model predictive control | |
Issa et al. | ASCA-PSO: Adaptive sine cosine optimization algorithm integrated with particle swarm for pairwise local sequence alignment | |
Okada et al. | Path integral networks: End-to-end differentiable optimal control | |
Ge et al. | Safe Q-learning method based on constrained Markov decision processes | |
CN110530373B (zh) | 一种机器人路径规划方法、控制器及系统 | |
Kapoor et al. | Model-based reinforcement learning from signal temporal logic specifications | |
Zhu et al. | A gaussian process model for opponent prediction in autonomous racing | |
Wang | Reinforcement learning for combinatorial optimization | |
CN116300478B (zh) | 基于变量分离微分动态规划的轨迹生成方法、介质及设备 | |
He et al. | Personalized robotic control via constrained multi-objective reinforcement learning | |
Afzali et al. | A Modified Convergence DDPG Algorithm for Robotic Manipulation | |
Fridovich-Keil et al. | Approximate solutions to a class of reachability games | |
Chauhan et al. | Competitive Swarm Optimizer: A decade survey | |
Liu et al. | Delay-Aware Multi-Agent Reinforcement Learning for Cooperative Adaptive Cruise Control with Model-based Stability Enhancement | |
Lin et al. | Policy-iteration-based finite-horizon approximate dynamic programming for continuous-time nonlinear optimal control | |
Prasad et al. | Multi-agent Polygon Formation using Reinforcement Learning. | |
Handoko et al. | Using classification for constrained memetic algorithm: A new paradigm | |
Xiao et al. | Variation encoded large-scale swarm optimizers for path planning of unmanned aerial vehicle | |
Lee et al. | Cooperative particle swarm optimization-based predictive controller for multi-robot formation | |
Shi et al. | Local path planning of unmanned vehicles based on improved RRT algorithm | |
Zarei et al. | Experimental study on optimal motion planning of wheeled mobile robot using convex optimization and receding horizon concept | |
CN117666594B (zh) | 基于分块高斯消元的运动规划方法、系统、设备及介质 | |
HomChaudhuri et al. | Distributed model predictive control for connected and automated vehicles in the presence of uncertainty | |
Li et al. | Dynamic path planning for mobile robots based on artificial potential field enhanced improved multiobjective snake optimization (APF‐IMOSO) |
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 |