CN116300478A - 基于变量分离微分动态规划的轨迹生成方法、介质及设备 - Google Patents

基于变量分离微分动态规划的轨迹生成方法、介质及设备 Download PDF

Info

Publication number
CN116300478A
CN116300478A CN202310572245.0A CN202310572245A CN116300478A CN 116300478 A CN116300478 A CN 116300478A CN 202310572245 A CN202310572245 A CN 202310572245A CN 116300478 A CN116300478 A CN 116300478A
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.)
Granted
Application number
CN202310572245.0A
Other languages
English (en)
Other versions
CN116300478B (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.)
Shanghai Youdao Zhitu Technology Co Ltd
Original Assignee
Shanghai Youdao Zhitu 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 Shanghai Youdao Zhitu Technology Co Ltd filed Critical Shanghai Youdao Zhitu Technology Co Ltd
Priority to CN202310572245.0A priority Critical patent/CN116300478B/zh
Publication of CN116300478A publication Critical patent/CN116300478A/zh
Application granted granted Critical
Publication of CN116300478B publication Critical patent/CN116300478B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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中构建的轨迹优化问题采用变量分离法进行约束处理的迭代优化，在每一轮迭代优化中使用微分动态规划进行优化轨迹的求解，最终得到车体动力学光滑的生成轨迹。
作为本发明的进一步描述，所述的车体动力学模型构建如下：
Figure SMS_1
，
其中，
Figure SMS_4
和/>
Figure SMS_6
分别表示车体轨迹的横向偏移和纵向偏移量，/>
Figure SMS_8
表示车速，/>
Figure SMS_3
表示车辆偏航角，/>
Figure SMS_5
表示曲率，/>
Figure SMS_7
表示曲率变化率；/>
Figure SMS_9
表示车体动力学系统状态，
Figure SMS_2
表示相应的系统控制，则所述的车体动力学模型可以表示为：
Figure SMS_10
，
针对上述的车体动力学模型使用四阶龙格库塔法进行离散化，可得离散化的车体动力学模型如下：
Figure SMS_11
，
其中，
Figure SMS_12
和/>
Figure SMS_13
分别表示离散化之后/>
Figure SMS_14
时刻的系统状态和控制变量。
作为本发明的进一步描述，步骤1中，待追踪目标中心点为N个，则待追踪目标中心点的坐标数据集
Figure SMS_15
；
相应的，所述的目标函数如下：
Figure SMS_16
，
其中，
Figure SMS_17
表示终点代价(cost)，/>
Figure SMS_18
表示第/>
Figure SMS_19
步的过程代价；
式中的
Figure SMS_20
与/>
Figure SMS_21
分别表示如下：
Figure SMS_22
，
Figure SMS_23
，
其中，
Figure SMS_24
表示状态权重矩阵，且为半正定矩阵，/>
Figure SMS_25
表示控制权重矩阵，且为正定矩阵，/>
Figure SMS_26
和/>
Figure SMS_27
分别表示状态权重梯度向量和控制权重梯度向量，/>
Figure SMS_28
表示常数。
作为本发明的进一步描述，所述的约束函数构建如下：
Figure SMS_29
，
其中，
Figure SMS_30
表示第/>
Figure SMS_31
个时间步的约束函数，/>
Figure SMS_32
表示终点约束函数。
作为本发明的进一步描述，所述有约束的轨迹优化问题构建如下：
Figure SMS_33
，
Figure SMS_34
，
Figure SMS_35
，
构建有约束的轨迹优化问题后，即可针对轨迹优化问题进行求解。
作为本发明的进一步描述，步骤3中，对构建的轨迹优化问题进行求解，得到车体动力学光滑的生成轨迹，包括如下步骤：
步骤3.1：采用变量分离法将不等式约束处理为等式约束；
定义一个辅助变量
Figure SMS_36
，然后定义一个函数/>
Figure SMS_37
，其形式如下，
Figure SMS_38
，
则目标函数
Figure SMS_39
可以增广为如下形式：
Figure SMS_40
，
其中，
Figure SMS_41
和/>
Figure SMS_42
分别表示第/>
Figure SMS_43
步的拉格朗日乘子和惩罚因子；然后通过微分动态规划进行轨迹求解，得到反馈增益/>
Figure SMS_44
和前馈增益/>
Figure SMS_45
，并进行微分动态规划收敛判断；
步骤3.2：更新拉格朗日乘子；
Figure SMS_46
，
Figure SMS_47
，
其中，
Figure SMS_49
表示变量分离法迭代步，根据新的拉格朗日乘子更新/>
Figure SMS_53
，得到/>
Figure SMS_54
，当
Figure SMS_50
与/>
Figure SMS_52
的相对差异小于设定的变量分析法收敛阈值/>
Figure SMS_55
时，整个求解流程结束；若/>
Figure SMS_56
与/>
Figure SMS_48
的相对差异未小于设定的变量分析法收敛阈值/>
Figure SMS_51
时，则进入步骤3.3；
步骤3.3：更新惩罚项因子，并循环步骤3.1至步骤3.2，求解流程结束循环或者循环达到变量分离法设定的最大次数，求解结束。
作为本发明的进一步描述，步骤3.1中，通过微分动态规划进行轨迹求解，所述的微分动态规划包括反向传播、前向传播和微分动态规划收敛判断，具体步骤如下：
步骤3.1.1：进入反向传播步骤，对车体动力学模型进行线性化，保留一阶雅可比项，即：
Figure SMS_57
，
记
Figure SMS_58
，/>
Figure SMS_59
，上式可以化为，
Figure SMS_60
，
对增广代价函数进行线性化，保留一阶雅可比项和二阶海森项，即：
Figure SMS_61
，
等同于，
Figure SMS_62
，
Figure SMS_63
，
其中，
Figure SMS_64
，/>
Figure SMS_65
，/>
Figure SMS_66
，/>
Figure SMS_67
，
Figure SMS_68
，/>
Figure SMS_69
中的相关变量计算方式如下，
Figure SMS_70
，
Figure SMS_71
，
Figure SMS_72
，
Figure SMS_73
，
Figure SMS_74
，
其中，
Figure SMS_76
分别表示总代价函数/>
Figure SMS_81
对/>
Figure SMS_83
进行序列求导，/>
Figure SMS_78
和/>
Figure SMS_80
表示第/>
Figure SMS_84
步代价函数/>
Figure SMS_86
对/>
Figure SMS_75
进行序列求导，/>
Figure SMS_79
和/>
Figure SMS_82
即为/>
Figure SMS_85
和/>
Figure SMS_77
；
如此，对
Figure SMS_87
进行极小值求取等同于对/>
Figure SMS_88
进行极小值求取，可得，
Figure SMS_89
，
并用
Figure SMS_90
表示反馈增益，/>
Figure SMS_91
表示前馈增益；
步骤3.1.2：进入前向传播步骤，前馈增益调节变量
Figure SMS_92
，对控制律/>
Figure SMS_93
进行调整，则：
Figure SMS_94
，
Figure SMS_95
进行试配，寻找/>
Figure SMS_96
下降最大的相应值，即可得到本次前向滚出的最优控制律；由车体动力学模型/>
Figure SMS_97
递推出轨迹序列/>
Figure SMS_98
；
步骤3.1.3：微分动态规划收敛判断，重复步骤3.1.1至步骤3.1.2，直至
Figure SMS_99
的下降幅度小于设定的微分动态规划收敛阈值/>
Figure SMS_100
，微分动态规划求解结束；若/>
Figure SMS_101
的下降幅度未小于设定的微分动态规划收敛阈值/>
Figure SMS_102
，则进入步骤3.2，更新拉格朗日乘子。
作为本发明的进一步描述，步骤3.3中，更新惩罚项因子，公式如下：
Figure SMS_103
，
Figure SMS_104
，
其中，
Figure SMS_105
表示惩罚项因子的伸缩系数，根据如下算式更新，
Figure SMS_106
，
其中，
Figure SMS_107
表示惩罚项因子伸缩系数，循环步骤3.1至步骤3.3直至步骤3.2求解成功结束循环或者循环达到变量分离法设定的最大次数，求解结束，即得到车体动力学光滑的生成轨迹。
一种基于变量分离微分动态规划的自动驾驶轨迹生成设备，包括处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信，所述存储器，用于存储计算机程序；
所述处理器，用于通过运行所述存储器上所存储的所述计算机程序来执行上述变量分离微分动态规划的自动驾驶轨迹生成方法步骤。
一种计算机可读的存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被处理器执行时实现上述变量分离微分动态规划的自动驾驶轨迹生成方法步骤。
相对于现有技术，本发明的技术效果为：
本发明提供了一种基于变量分离微分动态规划的自动驾驶轨迹生成方法、介质及设备，该方法通过构建有约束的轨迹优化问题，并对轨迹优化问题进行求解，即可生成车体动力学光滑的轨迹，采用变量分离法对微分动态规划进行改造计算效率极高，且不需要部署优化算法包，在空间存储上具有明显优势。
附图说明
图1为本发明的整体流程示意图；
图2为本发明的方法应用在实际路段及生成轨迹示意图；
图3为本发明的方法与现有技术方案Apollo计算效率对比示意图。
具体实施方式
下面结合附图对本发明进行详细描述：
在本发明的一种实施例中，公开了一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，参考图1所示，该方法包括如下步骤：
步骤1：获取结构化道路信息，并对道路空间信息进行预处理，使道路空间信息具有笛卡尔坐标系下的具体坐标值；所述的道路信息包括路沿、车道、车道中心点坐标；
其中，以车道中心点坐标为轨迹生成追踪目标，以路沿及车体动力学模型为约束信息；
步骤2：结合车体动力学模型、轨迹追踪目标函数、约束函数，构建有约束的轨迹优化问题；所述约束包含等式约束和不等式约束；
步骤3：针对步骤2中构建的轨迹优化问题采用变量分离法进行约束处理的迭代优化，在每一轮迭代优化中使用微分动态规划进行优化轨迹的求解，最终得到车体动力学光滑的生成轨迹。
具体的，本实施例，针对上述的轨迹优化问题构建和求解进行详细说明，具体如下：
需要说明的是，本实施例的目的是为了生成车体动力学光滑的轨迹，然而在生成过程中，会存在一些约束，因此，本实施例构建有约束的轨迹优化问题，并针对有约束的轨迹优化问题进行求解，即可得到车体动力学光滑的轨迹。
具体的，一个有约束的优化的轨迹优化问题，需要的组件包括：运载体动力学模型，（即车体动力学模型）、轨迹追踪目标函数、约束函数；因此，结合车体动力学模型、轨迹追踪目标函数、约束函数，构建有约束的轨迹优化问题。
具体的，本实施例在步骤1中，将待追踪目标中心点设定为N个，则待追踪目标中心点的坐标数据集
Figure SMS_108
；
一、所述的车体动力学模型构建如下：
Figure SMS_109
，
其中，
Figure SMS_111
和/>
Figure SMS_114
分别表示车体轨迹的横向偏移和纵向偏移量，/>
Figure SMS_116
表示车速，/>
Figure SMS_112
表示车辆偏航角，/>
Figure SMS_113
表示曲率，/>
Figure SMS_115
表示曲率变化率；/>
Figure SMS_117
表示车体动力学系统状态，
Figure SMS_110
表示相应的系统控制，则所述的车体动力学模型可以表示为：
Figure SMS_118
，
针对上述的车体动力学模型使用四阶龙格库塔法进行离散化，可得离散化的车体动力学模型如下：
Figure SMS_119
，
其中，
Figure SMS_120
和/>
Figure SMS_121
分别表示离散化之后/>
Figure SMS_122
时刻的系统状态和控制变量。
二、所述的目标函数如下：
Figure SMS_123
，
其中，
Figure SMS_124
表示终点代价(cost)，/>
Figure SMS_125
表示第/>
Figure SMS_126
步的过程代价；
式中的
Figure SMS_127
与/>
Figure SMS_128
分别表示如下：
Figure SMS_129
，
Figure SMS_130
，
其中，
Figure SMS_131
表示状态权重矩阵，且为半正定矩阵，/>
Figure SMS_132
表示控制权重矩阵，且为正定矩阵，/>
Figure SMS_133
和/>
Figure SMS_134
分别表示状态权重梯度向量和控制权重梯度向量，/>
Figure SMS_135
表示常数。
三、所述的约束函数构建如下：
Figure SMS_136
，
其中，
Figure SMS_137
表示第/>
Figure SMS_138
个时间步的约束函数，/>
Figure SMS_139
表示终点约束函数。
四、所述的有约束的轨迹优化问题构建如下：
Figure SMS_140
，
Figure SMS_141
，
Figure SMS_142
，
至此，通过上述公开的内容，构建了有约束的轨迹优化问题，进而即可针对轨迹优化问题进行求解。
具体的，本实施例，在步骤3中对构建的轨迹优化问题进行求解，得到车体动力学光滑的生成轨迹，包括如下步骤：
步骤3.1：采用变量分离法将不等式约束处理为等式约束；
定义一个辅助变量
Figure SMS_143
，然后定义一个函数/>
Figure SMS_144
，其形式如下，
Figure SMS_145
，
则目标函数
Figure SMS_146
可以增广为如下形式：
Figure SMS_147
，
其中，
Figure SMS_148
和/>
Figure SMS_149
分别表示第/>
Figure SMS_150
步的拉格朗日乘子和惩罚因子；然后通过微分动态规划进行轨迹求解，得到反馈增益/>
Figure SMS_151
和前馈增益/>
Figure SMS_152
，并进行微分动态规划收敛判断；
本实施例所述的微分动态规划包括反向传播、前向传播和微分动态规划收敛判断，具体步骤如下：
步骤3.1.1：进入反向传播步骤，对车体动力学模型进行线性化，保留一阶雅可比项，即：
Figure SMS_153
，
记
Figure SMS_154
，/>
Figure SMS_155
，上式可以化为，
Figure SMS_156
，
对增广代价函数进行线性化，保留一阶雅可比项和二阶海森项，即：
Figure SMS_157
，
等同于，
Figure SMS_158
，
Figure SMS_159
，
其中，
Figure SMS_160
，/>
Figure SMS_161
，/>
Figure SMS_162
，/>
Figure SMS_163
，
Figure SMS_164
，/>
Figure SMS_165
中的相关变量计算方式如下，
Figure SMS_166
，
Figure SMS_167
，
Figure SMS_168
，
Figure SMS_169
，
Figure SMS_170
，
其中，
Figure SMS_173
分别表示总代价函数/>
Figure SMS_177
对/>
Figure SMS_180
进行序列求导，/>
Figure SMS_174
和/>
Figure SMS_176
表示第/>
Figure SMS_179
步代价函数/>
Figure SMS_182
对/>
Figure SMS_171
进行序列求导，/>
Figure SMS_175
和/>
Figure SMS_178
即为/>
Figure SMS_181
和/>
Figure SMS_172
；
如此，对
Figure SMS_183
进行极小值求取等同于对/>
Figure SMS_184
进行极小值求取，可得，
Figure SMS_185
，
并用
Figure SMS_186
表示反馈增益，/>
Figure SMS_187
表示前馈增益；
步骤3.1.2：进入前向传播步骤，前馈增益调节变量
Figure SMS_188
，对控制律/>
Figure SMS_189
进行调整，则：
Figure SMS_190
，
Figure SMS_191
进行试配，寻找/>
Figure SMS_192
下降最大的相应值，即可得到本次前向滚出的最优控制律；由车体动力学模型/>
Figure SMS_193
递推出轨迹序列/>
Figure SMS_194
；
步骤3.1.3：微分动态规划收敛判断，重复步骤3.1.1至步骤3.1.2，直至
Figure SMS_195
的下降幅度小于设定的微分动态规划收敛阈值/>
Figure SMS_196
，微分动态规划求解结束；若/>
Figure SMS_197
的下降幅度未小于设定的微分动态规划收敛阈值/>
Figure SMS_198
，则进入步骤3.2。
步骤3.2：更新拉格朗日乘子；
Figure SMS_199
，
Figure SMS_200
，
其中，
Figure SMS_203
表示变量分离法迭代步，根据新的拉格朗日乘子更新/>
Figure SMS_205
，得到/>
Figure SMS_208
，当
Figure SMS_202
与/>
Figure SMS_206
的相对差异小于设定的变量分析法收敛阈值/>
Figure SMS_207
时，整个求解流程结束；若/>
Figure SMS_209
与/>
Figure SMS_201
的相对差异未小于设定的变量分析法收敛阈值/>
Figure SMS_204
时，则进入步骤3.3。
步骤3.3：更新惩罚项因子，并循环步骤3.1至步骤3.2，求解流程结束循环或者循环达到变量分离法设定的最大次数，求解结束；
具体的，更新惩罚项因子，公式如下：
Figure SMS_210
，
Figure SMS_211
，
其中，
Figure SMS_212
表示惩罚项因子的伸缩系数，根据如下算式更新，
Figure SMS_213
，
其中，
Figure SMS_214
表示惩罚项因子伸缩系数，循环步骤3.1至步骤3.3直至步骤3.2求解成功结束循环或者循环达到变量分离法设定的最大次数，求解结束，即得到车体动力学光滑的生成轨迹。
需要说明的是，本实施例上述的收敛包括变量分离法的收敛和微分动态规划的收敛；上述的变量分离的收敛是每次拉格朗日乘子和惩罚因子更新以后计算约束突破的值，如果约束突破的值小于设定的变量分离法收敛阈值
Figure SMS_215
，则满足了约束的需求，即满足收敛；上述的微分动态规划的收敛以满足代价函数cost下降变化小于设定的微分动态规划收敛阈值/>
Figure SMS_216
。
通过上述公开的内容可以得知，本实施例实现了反向传播计算控制增益，和正向传播计算轨迹，通过设置变量分离法收敛阈值
Figure SMS_217
，进行判定收敛结束后，约束突破是否小于设定的变量分离法收敛阈值/>
Figure SMS_218
，当小于设定的变量分离法收敛阈值/>
Figure SMS_219
即得到求解/>
Figure SMS_220
和/>
Figure SMS_221
；当约束突破不小于设定的变量分离法收敛阈值/>
Figure SMS_222
则更新惩罚项因子和惩罚项伸缩参数，重新进入微分动态规划求解。
综合上述公开的内容，上述公开的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，通过构建有约束的轨迹优化问题，并对轨迹优化问题进行求解，即可生成车体动力学光滑的轨迹，采用变量分离法对微分动态规划进行改造，以进一步提升计算效率，计算效率极高，且不需要部署优化算法包，在空间存储上具有明显优势，并面向自动驾驶轨迹生成应用进行定向设计，形成生成效率远胜百度Apollo的轨迹生成方法。
通过本实施例公开的方法，应用在实际的轨迹生成过程中，实验环境构建如下：
一台笔记本电脑，CPU配置为i7-11800H，内存为32G，部署ubuntu 22.04操作系统，ros2框架，权重矩阵统一设置为：
Figure SMS_223
，
所设置约束为边界约束，即，
Figure SMS_224
，
Figure SMS_225
，
其中，
Figure SMS_226
，/>
Figure SMS_227
，/>
Figure SMS_228
，
Figure SMS_229
。进行实验的路段及生成轨迹如图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所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：所述的车体动力学模型构建如下：
Figure QLYQS_1
，
其中，
Figure QLYQS_3
和/>
Figure QLYQS_6
分别表示车体轨迹的横向偏移和纵向偏移量，/>
Figure QLYQS_8
表示车速，/>
Figure QLYQS_4
表示车辆偏航角，/>
Figure QLYQS_5
表示曲率，/>
Figure QLYQS_7
表示曲率变化率；/>
Figure QLYQS_9
表示车体动力学系统状态，
Figure QLYQS_2
表示相应的系统控制，则所述的车体动力学模型可以表示为：
Figure QLYQS_10
，
针对上述的车体动力学模型使用四阶龙格库塔法进行离散化，可得离散化的车体动力学模型如下：
Figure QLYQS_11
，
其中，
Figure QLYQS_12
和/>
Figure QLYQS_13
分别表示离散化之后/>
Figure QLYQS_14
时刻的系统状态和控制变量。
3.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：步骤1中，待追踪目标中心点为N个，则待追踪目标中心点的坐标数据集
Figure QLYQS_15
；
相应的，所述的目标函数如下：
Figure QLYQS_16
，
其中，
Figure QLYQS_17
表示终点代价(cost)，/>
Figure QLYQS_18
表示第/>
Figure QLYQS_19
步的过程代价；
式中的
Figure QLYQS_20
与/>
Figure QLYQS_21
分别表示如下：
Figure QLYQS_22
，
Figure QLYQS_23
，
其中，
Figure QLYQS_24
表示状态权重矩阵，且为半正定矩阵，/>
Figure QLYQS_25
表示控制权重矩阵，且为正定矩阵，/>
Figure QLYQS_26
和/>
Figure QLYQS_27
分别表示状态权重梯度向量和控制权重梯度向量，/>
Figure QLYQS_28
表示常数。
4.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：所述的约束函数构建如下：
Figure QLYQS_29
，
其中，
Figure QLYQS_30
表示第/>
Figure QLYQS_31
个时间步的约束函数，/>
Figure QLYQS_32
表示终点约束函数。
5.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：所述有约束的轨迹优化问题构建如下：
Figure QLYQS_33
，
Figure QLYQS_34
，
Figure QLYQS_35
，
构建有约束的轨迹优化问题后，即可针对轨迹优化问题进行求解。
6.根据权利要求1所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：步骤3中，对构建的轨迹优化问题进行求解，得到车体动力学光滑的生成轨迹，包括如下步骤：
步骤3.1：采用变量分离法将不等式约束处理为等式约束；
定义一个辅助变量
Figure QLYQS_36
，然后定义一个函数/>
Figure QLYQS_37
，其形式如下，
Figure QLYQS_38
，
则目标函数
Figure QLYQS_39
可以增广为如下形式：
Figure QLYQS_40
，
其中，
Figure QLYQS_41
和/>
Figure QLYQS_42
分别表示第/>
Figure QLYQS_43
步的拉格朗日乘子和惩罚因子；然后通过微分动态规划进行轨迹求解，得到反馈增益/>
Figure QLYQS_44
和前馈增益/>
Figure QLYQS_45
，并进行微分动态规划收敛判断；
步骤3.2：更新拉格朗日乘子；
Figure QLYQS_46
，
Figure QLYQS_47
，
其中，
Figure QLYQS_49
表示变量分离法迭代步，根据新的拉格朗日乘子更新/>
Figure QLYQS_51
，得到/>
Figure QLYQS_54
，当/>
Figure QLYQS_50
与
Figure QLYQS_53
的相对差异小于设定的变量分析法收敛阈值/>
Figure QLYQS_55
时，整个求解流程结束；若/>
Figure QLYQS_56
与/>
Figure QLYQS_48
的相对差异未小于设定的变量分析法收敛阈值/>
Figure QLYQS_52
时，则进入步骤3.3；
步骤3.3：更新惩罚项因子，并循环步骤3.1至步骤3.2，求解流程结束循环或者循环达到变量分离法设定的最大次数，求解结束。
7.根据权利要求6所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：步骤3.1中，通过微分动态规划进行轨迹求解，所述的微分动态规划包括反向传播、前向传播和微分动态规划收敛判断，具体步骤如下：
步骤3.1.1：进入反向传播步骤，对车体动力学模型进行线性化，保留一阶雅可比项，即：
Figure QLYQS_57
，
记
Figure QLYQS_58
，/>
Figure QLYQS_59
，上式可以化为，
Figure QLYQS_60
，
对增广代价函数进行线性化，保留一阶雅可比项和二阶海森项，即：
Figure QLYQS_61
，
等同于，
Figure QLYQS_62
，
Figure QLYQS_63
，
其中，
Figure QLYQS_64
，/>
Figure QLYQS_65
，/>
Figure QLYQS_66
，/>
Figure QLYQS_67
，
Figure QLYQS_68
；/>
Figure QLYQS_69
中的相关变量计算方式如下，
Figure QLYQS_70
，
Figure QLYQS_71
，
Figure QLYQS_72
，
Figure QLYQS_73
，
Figure QLYQS_74
，
其中，
Figure QLYQS_76
分别表示总代价函数/>
Figure QLYQS_81
对/>
Figure QLYQS_83
进行序列求导，
Figure QLYQS_77
和/>
Figure QLYQS_80
表示第/>
Figure QLYQS_84
步代价函数/>
Figure QLYQS_85
对/>
Figure QLYQS_75
进行序列求导，/>
Figure QLYQS_79
和/>
Figure QLYQS_82
即为/>
Figure QLYQS_86
和/>
Figure QLYQS_78
；
如此，对
Figure QLYQS_87
进行极小值求取等同于对/>
Figure QLYQS_88
进行极小值求取，可得，
Figure QLYQS_89
，
并用
Figure QLYQS_90
表示反馈增益，/>
Figure QLYQS_91
表示前馈增益；
步骤3.1.2：进入前向传播步骤，前馈增益调节变量
Figure QLYQS_92
，对控制律/>
Figure QLYQS_93
进行调整，则：
Figure QLYQS_94
，
Figure QLYQS_95
进行试配，寻找/>
Figure QLYQS_96
下降最大的相应值，即可得到本次前向滚出的最优控制律；由动力学模型/>
Figure QLYQS_97
递推出轨迹序列/>
Figure QLYQS_98
；
步骤3.1.3：微分动态规划收敛判断，重复步骤3.1.1至步骤3.1.2，直至
Figure QLYQS_99
的下降幅度小于设定的微分动态规划收敛阈值/>
Figure QLYQS_100
，微分动态规划求解结束；若/>
Figure QLYQS_101
的下降幅度未小于设定的微分动态规划收敛阈值/>
Figure QLYQS_102
，则进入步骤3.2，更新拉格朗日乘子。
8.根据权利要求6所述的一种基于变量分离微分动态规划的自动驾驶轨迹生成方法，其特征在于：步骤3.3中，更新惩罚项因子，公式如下：
Figure QLYQS_103
，
Figure QLYQS_104
，
其中，
Figure QLYQS_105
表示惩罚项因子的伸缩系数，根据如下算式更新，
Figure QLYQS_106
，
其中，
Figure QLYQS_107
表示惩罚项因子伸缩系数，循环步骤3.1至步骤3.3直至步骤3.2求解成功结束循环或者循环达到变量分离法设定的最大次数，求解结束，即得到车体动力学光滑的生成轨迹。
9.一种基于变量分离微分动态规划的自动驾驶轨迹生成设备，包括处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信，其特征在于，所述存储器，用于存储计算机程序；
所述处理器，用于通过运行所述存储器上所存储的所述计算机程序来执行权利要求1-8任一项中所述的基于变量分离微分动态规划的自动驾驶轨迹生成方法。
10.一种计算机可读的存储介质，其特征在于，所述存储介质中存储有计算机程序，其中，所述计算机程序被处理器执行时实现权利要求1-8任一项中所述的基于变量分离微分动态规划的自动驾驶轨迹生成方法。
CN202310572245.0A 2023-05-22 2023-05-22 基于变量分离微分动态规划的轨迹生成方法、介质及设备 Active CN116300478B (zh)

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 true CN116300478A (zh) 2023-06-23
CN116300478B 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 (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104965517A (zh) * 2015-07-07 2015-10-07 张耀伦 一种机器人笛卡尔空间轨迹的规划方法
CN105652664A (zh) * 2016-02-29 2016-06-08 北京航空航天大学 一种基于鸽群优化的四旋翼无人机显式预测控制方法
US20190146498A1 (en) * 2017-02-16 2019-05-16 Faraday&Future Inc. Methods and systems for vehicle motion planning
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 北京交通大学 一种城市轨道交通列车运行调整方案优化方法和系统
US20220113367A1 (en) * 2018-04-23 2022-04-14 Inria Institut National De Recherche En Informatique Et En Automatique Locating device
CN114879508A (zh) * 2022-05-31 2022-08-09 天津理工大学 一种基于模型预测控制的磨削机器人路径跟踪控制方法
CN115840451A (zh) * 2022-12-12 2023-03-24 畅加风行(苏州)智能科技有限公司 狭窄通道无人驾驶拖挂避障转弯轨迹规划方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104965517A (zh) * 2015-07-07 2015-10-07 张耀伦 一种机器人笛卡尔空间轨迹的规划方法
CN105652664A (zh) * 2016-02-29 2016-06-08 北京航空航天大学 一种基于鸽群优化的四旋翼无人机显式预测控制方法
US20190146498A1 (en) * 2017-02-16 2019-05-16 Faraday&Future Inc. Methods and systems for vehicle motion planning
US20220113367A1 (en) * 2018-04-23 2022-04-14 Inria Institut National De Recherche En Informatique Et En Automatique Locating device
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 (2)

* Cited by examiner, † Cited by third party
Title
易云飞;蔡永乐;董文永;林郭隆;: "求解带容量约束的车辆路径问题的改进伊藤算法", 计算机科学, no. 05 *
梁宵;王宏伦;李大伟;吕文涛;: "基于流水避石原理的无人机三维航路规划方法", 航空学报, no. 07 *

Also Published As

Publication number Publication date
CN116300478B (zh) 2023-08-18

Similar Documents

Publication Publication Date Title
Issa et al. ASCA-PSO: Adaptive sine cosine optimization algorithm integrated with particle swarm for pairwise local sequence alignment
Rosolia et al. Autonomous racing using learning model predictive control
Rosolia et al. Learning how to autonomously race a car: a predictive control approach
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
Duan et al. Optimal formation reconfiguration control of multiple UCAVs using improved particle swarm optimization
CN110530373B (zh) 一种机器人路径规划方法、控制器及系统
Kapoor et al. Model-based reinforcement learning from signal temporal logic specifications
Pal et al. Brief survey of model-based reinforcement learning techniques
CN116399363A (zh) 狭窄道路的轨迹规划方法、装置、电子设备及存储介质
Vallon et al. Task decomposition for iterative learning model predictive control
Zhu et al. A gaussian process model for opponent prediction in autonomous racing
Wang Reinforcement learning for combinatorial optimization
CN116300478B (zh) 基于变量分离微分动态规划的轨迹生成方法、介质及设备
Fridovich-Keil et al. Approximate solutions to a class of reachability games
Power et al. Constrained stein variational trajectory optimization
He et al. Personalized robotic control via constrained multi-objective reinforcement learning
Lei et al. Kb-tree: Learnable and continuous monte-carlo tree search for autonomous driving planning
Prasad et al. Multi-agent Polygon Formation using Reinforcement Learning.
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
Lin et al. Policy-iteration-based finite-horizon approximate dynamic programming for continuous-time nonlinear optimal control
Xiao et al. Variation encoded large-scale swarm optimizers for path planning of unmanned aerial vehicle
CN117666594B (zh) 基于分块高斯消元的运动规划方法、系统、设备及介质
HomChaudhuri et al. Distributed model predictive control for connected and automated vehicles in the presence of uncertainty

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