CN116185014A - 一种基于动态规划的智能车全局最优轨迹规划方法与系统 - Google Patents
一种基于动态规划的智能车全局最优轨迹规划方法与系统 Download PDFInfo
- Publication number
- CN116185014A CN116185014A CN202310021535.6A CN202310021535A CN116185014A CN 116185014 A CN116185014 A CN 116185014A CN 202310021535 A CN202310021535 A CN 202310021535A CN 116185014 A CN116185014 A CN 116185014A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- coordinate system
- optimal
- global
- planning
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 88
- 239000006185 dispersion Substances 0.000 claims abstract description 33
- 230000007704 transition Effects 0.000 claims description 26
- 230000008859 change Effects 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000013507 mapping Methods 0.000 claims description 4
- DMSMPAJRVJJAGA-UHFFFAOYSA-N benzo[d]isothiazol-3-one Chemical compound C1=CC=C2C(=O)NSC2=C1 DMSMPAJRVJJAGA-UHFFFAOYSA-N 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 20
- 238000010586 diagram Methods 0.000 description 10
- 238000004422 calculation algorithm Methods 0.000 description 9
- 230000001133 acceleration Effects 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000005457 optimization Methods 0.000 description 4
- 238000011217 control strategy Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012067 mathematical method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
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/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- 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)
Abstract
本发明公开了一种基于动态规划的智能车辆全局最优轨迹规划方法与系统，其包括：步骤1，根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列；步骤2，根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹。
Description
技术领域
本发明涉及自动驾驶技术领域，特别是关于一种基于动态规划的智能车全局最优轨迹规划方法与系统。
背景技术
智能车辆是智能交通系统的重要部分，它能够有效减少交通事故，使社会交通更加高效运行，同时也能够起到节约能源，减少排放的作用，因此成为了近年来的研究热点。其中，轨迹规划是智能车辆的核心模块之一，起到衔接上层的建图定位、感知、决策和下层运动控制的作用。全局轨迹规划是指基于地图信息，根据起始点和目标点信息规划出一条符合目标需求、满足车辆运动学原理、包含速度信息的轨迹。全局轨迹规划包括路径规划和速度规划两部分，路径规划解决智能车辆会经过哪些坐标点从而到达目标位置的问题，速度规划解决智能车辆在每个坐标点以怎样的速度行驶的问题。
目前，智能驾驶车辆轨迹规划方法主要有：1)基于曲线的方法主要特征是参考曲线的特定要求或者表达式，根据一些控制点边界条件计算出曲线的参数。常见的主要有RS曲线、贝塞尔曲线和样条曲线等。2)基于图搜索的方法主要特征是将地图栅格化后进行路径搜索，并致力于达到路径最短、效率最优等目标，主要包括Dijkstra算法、A*算法和Hybrid A*算法等。3)基于采样的方法不需要遍历地图空间所有点，而是通过在空间中随机撒点，通过线段连点构成没有障碍的路图/树，来抽象的表达空间，主要包括RRT(Rapidly-exploring Random Tree，快速搜索随机树)、PRM(Probabilistic Road Map，概率路图)等。4)基于最优控制的方法需要综合考虑实际条件建立最优控制问题模型，包括目标函数、约束条件等。适用于求解最优控制问题的方法包括间接法、动态规划法以及直接法。间接法又称为解析法，通常利用代价函数取得极值的必要性条件，并结合充分性条件或命题的实际物理意义间接确定最优解，典型的间接方法包括变分法以及庞特里亚金极值原理。动态规划法将问题空间划分成多个子空间，通过确定各个区间上的控制变量序列形式来实现目标函数的最优化。直接法将最优控制问题的变量离散化，通过求解离散后形成的NLP(Nonlinear programming，非线性规划)问题直接得到数值最优解。
针对已知地图的全局最优轨迹规划问题，现有方法有一种使用直接法求解的基于最优控制的全局最优轨迹规划方法。这种方法是结合地图中障碍物以及道路边界信息、车辆模型和终端边值约束，设计目标函数，建立最优控制问题模型；通过对变量进行离散化将最优控制问题转换为NLP问题并使用求解器进行求解，从而得到连续空间下的变量的数值最优解。这种方法虽然能够进行全局最优轨迹的求解，但是该方法严重依赖于求解器的内置优化算法，并且在复杂地图或者多障碍物环境下使用数学方法表示车辆的障碍物约束较为困难，使用直接法进行最优控制问题的求解时，如果对于问题的约束没有准确的数学表达或者约束过于复杂很容易导致求解器的求解失败；另一方面，基于求解器的内置优化算法进行最优控制问题的求解时，对于初始解有很高的要求，质量较差的初始解很可能导致求解结果的次优性。
而针对已知地图的全局最优轨迹规划问题，现有技术通常是首先将地图建立为拓扑地图，将地图空间离散成多个拓扑点，然后使用动态规划求出到达每个拓扑点的满足目标函数的最优控制变量序列，从而得到整个问题的最优路线。但是，使用动态规划求出的拓扑地图下最优路线一般只考虑车辆的坐标点，并不考虑车辆的横摆角以及前轮转角，得到的结果是点到点之间的折线路线，并不满足车辆路径的最小转弯半径和曲率连续性约束，因此车辆无法进行直接控制跟踪。
发明内容
本发明的目的在于提供一种基于动态规划的智能车全局最优轨迹规划方法与系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种基于动态规划的智能车辆全局最优轨迹规划方法，其包括：
步骤1，根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列；进一步地，Frenet坐标系的设置方法包括：
根据预设的参考线Tref，将智能驾驶车辆在笛卡尔坐标系下的位置坐标设置为(x,y)，从车辆位置(x,y)向参考线Tref作投影，投影点为M，则点M与车辆位置(x,y)的距离为横向距离l，沿Tref从起点到投影点M的曲线距离为纵向距离s，用(s,l)表示Frenet坐标系下车辆的坐标值，构建如下式(1)表示Frenet坐标系与笛卡尔坐标系之间的映射关系：
其中，
为车辆位置在参考线上的投影点M在全局坐标系下的位置向量，/>
为投影点M在参考曲线上的法向量和切向量；
步骤2，根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹。
进一步地，第一最优控制问题模型设置为：
其中，式(2a)为目标函数，||xk+1-xk||表示两个阶段之间的路径点的距离，xk＝(xk,yk)＝f1(sk,lk)，||xk+1-xk||max表示两个阶段之间的路径点距离的最大值，|δk+1-δk|表示两个阶段之间车辆的前轮转角控制量的变化量，δmax表示前轮转角的最大值，κx,k表示第k阶段的路径曲率值，κx,max表示第k阶段的路径曲率最大值，ω1,ω2,ω3表示权值，式(2b)为不同阶段状态量之间的状态转移方程，式(2c)为控制量的边界限制范围，式(2d)为道路边界以及障碍物约束，lleft(sk)表示sk对应的l左边界，lright(sk)表示sk对应的l右边界。
进一步地，步骤1中的“求解基于动态规划方法进行最优控制问题”的方法包括：
在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散，再根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围，然后通过确定各个区间上的控制变量最优序列形式，来对目标函数进行最优化，以获得整个地图空间下的全局最优轨迹。
进一步地，在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散的方法具体包括：
根据建图模块所提供地图信息，采集道路中心线作为参考线建立Frenet坐标系并进行状态离散，道路地图的离散根据[s,l,θx]三个维度，s、l、θx分别为纵向距离、横向距离、笛卡尔坐标系下的车辆横摆角。
进一步地，“根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围”的方法具体包括：
先利用式(2)描述的最优控制问题以前轮转角δx为控制量，根据车辆前轮转角范围，按照dδx对前轮转角δx进行离散，由式(3)可计算出根据离散的前轮转角所得的离散曲率控制序列：
其中，κx表示车辆行驶路径曲率，Rx表示车辆转弯半径，Lx为车辆轴距，δx为车辆前轮转角；
再根据由Frenet坐标系与笛卡尔坐标系之间转换关系获得的状态转移方程计算得到车辆在下一阶段的可行驶范围。
本发明还提供一种基于动态规划的智能车辆全局最优轨迹规划系统，其包括建图定位单元、全局最优轨迹规划单元、运动控制单元和底层执行单元，其中：
建图定位单元用于获取环境信息，经处理后下发至全局最优轨迹规划单元；全局最优轨迹规划单元用于根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列；全局最优轨迹规划单元还用于根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹，进而得到全局最优轨迹，并输出给运动控制单元；运动控制单元用于根据全局最优轨迹，计算控制指令，下发给底层执行单元来控制车辆；
其中，进一步地，Frenet坐标系的设置方法包括：
根据预设的参考线Tref，将智能驾驶车辆在笛卡尔坐标系下的位置坐标设置为(x,y)，从车辆位置(x,y)向参考线Tref作投影，投影点为M，则点M与车辆位置(x,y)的距离为横向距离l，沿Tref从起点到投影点M的曲线距离为纵向距离s，用(s,l)表示Frenet坐标系下车辆的坐标值，构建如下式(1)表示Frenet坐标系与笛卡尔坐标系之间的映射关系：
其中，
为车辆位置在参考线上的投影点M在全局坐标系下的位置向量，/>
为投影点M在参考曲线上的法向量和切向量。
其中，第一最优控制问题模型设置为：
其中，式(2a)为目标函数，||xk+1-xk||表示两个阶段之间的路径点的距离，xk＝(xk,yk)＝f1(sk,lk)，||xk+1-xk||max表示两个阶段之间的路径点距离的最大值，|δk+1-δk|表示两个阶段之间车辆的前轮转角控制量的变化量，δmax表示前轮转角的最大值，κx,k表示第k阶段的路径曲率值，κx,max表示第k阶段的路径曲率最大值，ω1,ω2,ω3表示权值，式(2b)为不同阶段状态量之间的状态转移方程，式(2c)为控制量的边界限制范围，式(2d)为道路边界以及障碍物约束，lleft(sk)表示sk对应的l左边界，lright(sk)表示sk对应的l右边界。
进一步地，求解基于动态规划方法进行最优控制问题的方法包括：
在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散，再根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围，然后通过确定各个区间上的控制变量最优序列形式，来对目标函数进行最优化，以获得整个地图空间下的全局最优轨迹。
进一步地，在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散的方法具体包括：
根据建图模块所提供地图信息，采集道路中心线作为参考线建立Frenet坐标系并进行状态离散，道路地图的离散根据[s,l,θx]三个维度，s、l、θx分别为纵向距离、横向距离、笛卡尔坐标系下的车辆横摆角。
进一步地，“根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围”的方法具体包括：
先利用式(2)描述的最优控制问题以前轮转角δx为控制量，根据车辆前轮转角范围，按照dδx对前轮转角δx进行离散，由式(3)可计算出根据离散的前轮转角所得的离散曲率控制序列：
其中，κx表示车辆行驶路径曲率，Rx表示车辆转弯半径，Lx为车辆轴距，δx为车辆前轮转角；
再根据由Frenet坐标系与笛卡尔坐标系之间转换关系获得的状态转移方程计算得到车辆在下一阶段的可行驶范围。
本发明相比于拓扑地图基于动态规划的路线规划方法，本发明所求出的全局最优轨迹符合曲率连续性要求，满足车辆的最小转弯半径约束，进行轨迹点插值之后可以直接下发给运动控制单元进行跟踪控制；使用动态规划的方法来对最优控制问题进行求解，对整个配置空间进行三个维度状态量的离散，从而将轨迹规划问题划分为若干阶段，基于状态转移方程与目标函数求解各阶段之间的最优策略，可以准确求出到达每个离散状态量的最优决策以及对应目标函数的全局最优结果，进而，一方面可以避免由于约束过于复杂导致求解器内置算法的求解失败，另一方面可以避免因为较差的初始解导致求解陷入局部最优，本发明求得的决策序列是该离散条件下针对于目标函数的最优策略。
附图说明
图1为本发明实施例提供的基于动态规划的智能车辆全局最优轨迹规划方法的框架示意图；
图2为本发明实施例提供的Frenet坐标与笛卡尔坐标系的转换示意图；
图3为本发明实施例提供的Frenet坐标下车辆状态离散图；
图4为本发明实施例提供的车辆自行车模型示意图；
图5为本发明实施例提供的基于动态规划的全局最优路径求解过程图；
图6为本发明实施例提供的速度离散图；
图7为本实施例提供的距离与速度状态转移图；
图8为本实施例提供的最优速度动态规划求解过程图；
图9为本实施例提供的基于动态规划的智能车全局最优轨迹规划系统的结构示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
如图1所示，本发明实施例提供的基于动态规划的智能车辆全局最优轨迹规划方法包括：
步骤1，动态规划全局最优路径：根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列。其中，地图信息可以由建图定位模块输入获得。
步骤2，动态规划全局最优速度：根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹。
在一个实施林中，作为步骤1的一种实现方式，其具体包括：
步骤11，建立Frenet坐标系：Frenet坐标系的建立基于预设的参考线Tref，Tref可以是任意曲线，一般情况下定义为车道中心线。将智能驾驶车辆在笛卡尔坐标系下的坐标设置为(x,y)，从车辆的位置(x,y)向参考线Tref作投影，投影点为M，则点M与车辆位置(x,y)的距离即为横向距离l；沿Tref从起点到投影点M的曲线距离即为纵向距离s。用(s,l)表示Frenet坐标系下车辆的坐标值，构建如下式(1)表示的映射关系：
图2描述智能驾驶车辆在Frenet坐标系与笛卡尔坐标系之间的转换关系，车辆在任意时刻的运动状态可以描述为[x,θx,κx,νx,ax]；其中x为当前车辆位置坐标，可以由笛卡尔坐标系下位置信息(x,y)表示，也可以由Frenet坐标系下的纵向距离s和横向距离l表示，即x＝(x,y)＝f1(s,l)；θx为笛卡尔坐标系下的车辆横摆角，κx为曲率，νx为车辆速度，ax为加速度。
为车辆位置在参考线上的投影点M在全局坐标系下的位置向量，/>
和/>
分别为投影点M在参考曲线上的法向量和切向量。
步骤12，建立最优控制问题，其中，最优控制是指在给定的约束条件下，寻求一个控制，使给定的系统性能指标达到极大值(或极小值)。假定在ds长度范围内，车辆的前轮转角没有发生变化，即车辆沿固定曲率的曲线运动，以每个阶段中车辆前轮转角δx为控制量，ξ＝[x,s,l,θx,κx]为状态量，x＝(x,y)表示笛卡尔坐标系下车辆坐标，s和l分别表示Frenet坐标系下的纵向距离和横向距离，θx为笛卡尔坐标系下的车辆横摆角，κx为曲率；基于此，建立第一最优控制问题模型如下：
其中，式(2a)为目标函数，||xk+1-xk||表示两个阶段之间的路径点距离，xk＝(xk,yk)＝f1(sk,lk)，||xk+1-xk||max表示两个阶段之间的路径点距离的最大值；|δk+1-δk|表示两个阶段之间控制量的变化量，δmax表示前轮转角的最大值；κx,k表示第k阶段的路径曲率值，κx,max表示第k阶段的路径曲率最大值；ω1,ω2,ω3表示权值大小，本发明参照相关文献设置ω1,ω2,ω3权值大小分别为7、2和1；式(2b)描述的fds(ξk,δk)为不同阶段(例如第k阶段与第k+1阶段为不同的两个阶段)的状态量(ξk，δk)之间的状态转移方程，即下文中的公式(3)-(6)，ξk、δk分别为第k阶段的车辆状态量和控制量(前轮转角)，ξk+1为第k+1阶段的车辆状态量；式(2c)为控制量的边界限制范围；式(2d)为道路边界以及障碍物约束，lleft(sk)表示sk对应的l左边界，lright(sk)表示sk对应的l右边界。
由于上述实施例中所定义的一个阶段内，车辆沿圆弧运动，所以也可以用圆弧长予以表示，相比于其他距离形式，欧氏距离更方便计算，因此有：
当然，其他距离也可以适用于上述实施例中的第一最优控制问题模型。
与背景技术中的使用直接法求解的基于最优控制的全局最优轨迹规划方法比较而言，现有技术方法依赖于求解器的内置求解算法，约束复杂性的提高很容易导致问题的求解失败。本发明实施例不依赖于求解器的内置算法，基于动态规划算法的状态转移方程进行状态之间的迭代，求解较为稳定。
与背景技术中提到的针对已知地图的全局最优轨迹规划问题的方法相比，现有方法得到的路线虽然是最优路线，但是车辆无法进行直接控制跟踪。本发明实施例的路径规划模块考虑车辆的坐标点、横摆角以及前轮转角，得到的路径是一条曲率连续的、满足车辆运动学约束的全局最优路径，车辆可以进行直接的跟踪控制。
步骤13，基于动态规划的求解最优控制问题。
本实施例基于坐标系将地图信息所在的地图空间划分成多个子状态空间，根据车辆前轮转角约束确定下一阶段的可行驶范围，确定各个区间上的控制变量最优序列形式来实现目标函数的最优化，因此能够得到整个地图空间下的全局最优轨迹，因此，步骤13具体包括：
步骤131，在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间的离散：在Frenet坐标系中，车辆的运动状态可以描述为
其中：s为沿参考线纵向距离，/>
为纵向速度，/>
为纵向加速度，l为横向距离，l′为横向距离对s的一阶导数，l″为横向距离对s的二阶导数。本部分内容研究车辆的全局路径规划，根据建图模块所提供地图信息(包括道路宽度、障碍物位置等)，采集道路中心线作为参考线建立Frenet坐标系并进行状态离散，道路地图的离散根据[s,l,θx]三个维度，θx为笛卡尔坐标系下的车辆横摆角。对应按照ds,dl,dθx离散量进行子状态空间的划分，同时去除与障碍物或者道路边界发生碰撞的状态空间，如图3所示。步骤132，根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围：上述最优控制问题以前轮转角δx为控制量，根据车辆前轮转角范围，按照dδx对其进行离散。将车辆简化为自行车模型，如图4所示：
由车辆模型可得曲率计算公式(3)：
其中，κx表示车辆行驶路径曲率，Rx表示车辆转弯半径，Lx为车辆轴距，δx为车辆前轮转角。由式(3)可计算出根据离散的前轮转角所得的离散曲率控制序列。
根据Frenet坐标系与笛卡尔坐标系之间的转换关系可推导出以下公式(4)-(6)：
其中，κr为车辆在参考线上的投影点的曲率，κr′表示κr对纵向距离s的一阶导，θr为车辆在参考线上的投影点的切线方向与x轴的夹角。由公式(3)和(4)可知，如果车辆当前状态已知，车辆下一状态的值只与车辆下一阶段的曲率有关，即只与车辆下一阶段的前轮转角策略有关。上文已知，对Frenet坐标系状态的离散按照沿参考线的ds进行阶段划分，根据状态转移公式(3)-(6)，对车辆进行前轮转角的控制量离散，可以基于当前阶段状态量计算车辆在下一阶段的可行驶范围。
步骤133，通过确定各个区间上的控制变量最优序列形式，来对目标函数进行最优化，以获得整个地图空间下的全局最优轨迹：动态规划的思想是将一个问题分解为若干个阶段，根据状态转移方程以及代价函数对每个阶段求最优策略并进行存储，动态规划问题满足无后效性要求，即当前阶段决策不会影响后续决策，每阶段的最优策略为后续阶段提供了有效信息，对每个阶段依次进行状态求解直到目标阶段，最后进行决策回溯得到最优决策序列。
如图4所示，判定条件一保证两个阶段之间的路径曲率连续性，κx,dp,k+1和κx,dp,k分别表示通过动态规划求解得到的第k+1阶段和第k阶段的曲率值；判定条件二和三保证通过状态转移方程计算得到的下一阶段状态与离散状态量的偏差在一定范围内，其中ldp,s(k+1)和θx,dp,s(k+1)表示通过动态规划求解得到的k+1阶段所对应s(k+1)的横向距离以及车辆横摆角，ls(k+1)和θx,s(k+1)表示k+1阶段所对应s(k+1)的横向距离以及横摆角的离散状态量，ε1,ε2,ε3为偏差阈值；当三个判定条件同时满足，则记录到达该阶段状态量的代价值，当前阶段结束时，计算到达下一阶段各状态量的最小代价值以及最优控制策略；到达目标状态之后，计算到达目标状态对应目标函数的最小代价策略并进行回溯，得到该地图环境下的全局最优路径。
本实施例在Frenet坐标系下基于动态规划方法的全局最优路径规划，每个阶段的车辆状态量考虑横纵向坐标和车辆横摆角[s,l,θx]三个维度，控制量考虑前轮转角δx。不同于传统的基于动态规划的路径规划方法只考虑[x,y]或者[s,l]两个维度，三个维度的状态离散更精确的表达车辆的位姿状态；以前轮转角δx为控制量能够准确表述车辆行驶过程中的曲率信息。
显然，背景技术中的使用直接法求解的基于最优控制的全局最优轨迹规划方法依赖较优的初始解，没有初始解或者较差的初始解容易导致求解陷入局部最优。而本实施例提供的方法不需要初始解，能够得到每个子状态空间的最优策略，求得的全局轨迹一定是离散空间下的符合目标函数的最优解。
当然，上述实施例中，按照ds进行离散并将状态量设为[s,l,θx]三个维度，可以替换为按照dt进行离散同时将状态量设置为
或者/>
相应的步骤可以简要概括为包括：(1)根据坐标转换公式推导出每个变量的状态计算公式，如式(3)-(6)所示；(2)以加速度a和前轮转角δ为控制量进行离散；(3)基于当前状态量，根据式(1)中推导出的公式进行下个阶段状态的计算；(4)选取每个状态量对应的最优决策；(5)最终进行回溯得到最优轨迹。
步骤2，规划基于动态规划的全局最优速度，其具体包括：
步骤21，根据已知的全局路径，计算全局路径Sglobal，并沿全局路径Sglobal按照dSglobal进行路径离散，并建立最优控制问题：
预先设定车辆的加速度在dSglobal长度范围内没有发生变化，即车辆在dSglobal长度范围内做匀加速直线运动，并且，在每一个dSglobal长度范围内对应的阶段中车辆加速度ax作为控制量，ξ＝[Sx,νx]作为状态量，其中，Sx为沿全局路径Sglobal的距离值，即从全局路径的起点开始，沿全局路径的长度，νx为车辆速度值，由此建立如下(7)所示的最优控制问题模型：
其中，公式(7a)为目标函数，Sx,k+1-Sx,k表示两个阶段之间的路径距离，Sx,k为第k阶段沿全局路径Sglobal的距离值，Sx,k+1为第k+1阶段沿全局路径Sglobal的距离值，(νx,k+1+νx,k)/2表示两个阶段之间平均速度，νx,k为第k阶段车辆的速度值，νx,k+1为第k+1阶段车辆的速度值，
表示第k阶段到第k+1阶段所花费的时间；公式(7b)为不同阶段状态量之间的状态转移方程；公式(7c)为控制量的边界限制范围，adec,max,aacc,max分别表示加速度的上下限值；公式(7d)为速度的边界限制范围。
步骤22，根据已知的车辆所设定最大速度值νmax，针对每个路径阶段在0:νmax之间按照dνmax进行速度离散，由于本部分应用环境不允许倒车，因此得到的离散状态图如图5所示。
步骤23，根据动态规划，求解最优速度：为了加快计算速度，减小计算量，基于当前距离与速度的状态量，每个阶段计算前方Spre长度范围内满足约束(8)的可达状态量，记录到达该阶段状态量的代价值；当前阶段结束时，计算到达该阶段各状态量的最小代价值以及最优控制策略；到达目标状态之后，计算到达目标状态对应目标函数的最小代价策略并进行回溯，得到对应全局路径点的最优速度以及加速度控制策略。状态转移图以及动态规划求解过程图如图6和7所示：
adec,max≤ax,k+1≤aacc,max
本发明实施例在动态规划各阶段的求解过程考虑车辆路径的曲率连续性以及车辆状态与离散状态量之间的一致性。利用微分的思想，对环境空间进行[s,l,θx]多维子状态空间的划分之后，以前轮转角δx为控制量并进行离散，参考自行车模型计算曲率值，确保相邻阶段之间的曲率变化在一定阈值内来保证曲率连续性；基于离散控制量与状态转移方程计算车辆每阶段状态，并判断车辆状态与所离散状态量之间的一致性，以便得到各离散量所对应的最优策略。
需要说明的是，上述实施例中所述的路径速度解耦的轨迹规划可以替换为基于动态规划的路径速度耦合的最优轨迹规划方法。
如图8和图9所示，本发明实施例还提供一种基于动态规划的智能车辆全局最优轨迹规划系统，其包括建图定位单元、全局最优轨迹规划单元、运动控制单元和底层执行单元，其中：
建图定位单元用于获取环境信息，经处理后下发至全局最优轨迹规划单元。
全局最优轨迹规划单元用于根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列；全局最优轨迹规划单元还用于根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹，进而得到全局最优轨迹，并输出给运动控制单元。
运动控制单元用于根据全局最优轨迹，计算控制指令，下发给底层执行单元来控制车辆，实现无人车辆精确、平稳的运动控制。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种基于动态规划的智能车辆全局最优轨迹规划方法，其特征在于，包括：
步骤1，根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列；Frenet坐标系的设置方法包括：
根据预设的参考线Tref，将智能驾驶车辆在笛卡尔坐标系下的位置坐标设置为(x,y)，从车辆位置(x,y)向参考线Tref作投影，投影点为M，则点M与车辆位置(x,y)的距离为横向距离l，沿Tref从起点到投影点M的曲线距离为纵向距离s，用(s,l)表示Frenet坐标系下车辆的坐标值，构建如下式(1)表示Frenet坐标系与笛卡尔坐标系之间的映射关系：
其中，
为车辆位置在参考线上的投影点M在全局坐标系下的位置向量，/>
为投影点M在参考曲线上的法向量和切向量；
步骤2，根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹。
2.如权利要求1所述的基于动态规划的智能车辆全局最优轨迹规划方法，其特征在于，第一最优控制问题模型设置为：
其中，式(2a)为目标函数，||xk+1-xk||表示两个阶段之间的路径点的距离，xk＝(xk,yk)＝f1(sk,lk)，||xk+1-xk||max表示两个阶段之间的路径点距离的最大值，|δk+1-δk|表示两个阶段之间车辆的前轮转角控制量的变化量，δmax表示前轮转角的最大值，κx,k表示第k阶段的路径曲率值，κx,max表示第k阶段的路径曲率最大值，ω1,ω2,ω3表示权值，式(2b)为不同阶段状态量之间的状态转移方程，式(2c)为控制量的边界限制范围，式(2d)为道路边界以及障碍物约束，lleft(sk)表示sk对应的l左边界，lright(sk)表示sk对应的l右边界。
3.如权利要求1所述的基于动态规划的智能车辆全局最优轨迹规划方法，其特征在于，步骤1中的“求解基于动态规划方法进行最优控制问题”的方法包括：
在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散，再根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围，然后通过确定各个区间上的控制变量最优序列形式，来对目标函数进行最优化，以获得整个地图空间下的全局最优轨迹。
4.如权利要求3所述的基于动态规划的智能车辆全局最优轨迹规划方法，其特征在于，在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散的方法具体包括：
根据建图模块所提供地图信息，采集道路中心线作为参考线建立Frenet坐标系并进行状态离散，道路地图的离散根据[s,l,θx]三个维度，s、l、θx分别为纵向距离、横向距离、笛卡尔坐标系下的车辆横摆角。
5.如权利要求4所述的基于动态规划的智能车辆全局最优轨迹规划方法，其特征在于，“根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围”的方法具体包括：
先利用式(2)描述的最优控制问题以前轮转角δx为控制量，根据车辆前轮转角范围，按照dδx对前轮转角δx进行离散，由式(3)可计算出根据离散的前轮转角所得的离散曲率控制序列：
其中，κx表示车辆行驶路径曲率，Rx表示车辆转弯半径，Lx为车辆轴距，δx为车辆前轮转角；
再根据由Frenet坐标系与笛卡尔坐标系之间转换关系获得的状态转移方程计算得到车辆在下一阶段的可行驶范围。
6.一种基于动态规划的智能车辆全局最优轨迹规划系统，其特征在于，包括建图定位单元、全局最优轨迹规划单元、运动控制单元和底层执行单元，其中：
建图定位单元用于获取环境信息，经处理后下发至全局最优轨迹规划单元；全局最优轨迹规划单元用于根据地图信息、车辆底盘参数信息以及起点和终点位姿信息，将地图信息中的道路环境由笛卡尔坐标系转换到Frenet坐标系下，进行状态空间的离散，再根据轨迹规划任务建立第一最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的路径点序列；全局最优轨迹规划单元还用于根据路径点序列以及车辆的动力学参数信息进行状态空间的离散，参照轨迹规划任务建立第二最优控制问题模型，采用动态规划方法求解最优控制问题，获得全局最优路径的最优速度，从而获得全局最优轨迹，进而得到全局最优轨迹，并输出给运动控制单元；运动控制单元用于根据全局最优轨迹，计算控制指令，下发给底层执行单元来控制车辆；
其中，Frenet坐标系的设置方法包括：
根据预设的参考线Tref，将智能驾驶车辆在笛卡尔坐标系下的位置坐标设置为(x,y)，从车辆位置(x,y)向参考线Tref作投影，投影点为M，则点M与车辆位置(x,y)的距离为横向距离l，沿Tref从起点到投影点M的曲线距离为纵向距离s，用(s,l)表示Frenet坐标系下车辆的坐标值，构建如下式(1)表示Frenet坐标系与笛卡尔坐标系之间的映射关系：
其中，
为车辆位置在参考线上的投影点M在全局坐标系下的位置向量，/>
为投影点M在参考曲线上的法向量和切向量。
7.如权利要求6所述的基于动态规划的智能车辆全局最优轨迹规划系统，其特征在于，第一最优控制问题模型设置为：
其中，式(2a)为目标函数，||xk+1-xk||表示两个阶段之间的路径点的距离，xk＝(xk,yk)＝f1(sk,lk)，||xk+1-xk||max表示两个阶段之间的路径点距离的最大值，|δk+1-δk|表示两个阶段之间车辆的前轮转角控制量的变化量，δmax表示前轮转角的最大值，κx,k表示第k阶段的路径曲率值，κx,max表示第k阶段的路径曲率最大值，ω1,ω2,ω3表示权值，式(2b)为不同阶段状态量之间的状态转移方程，式(2c)为控制量的边界限制范围，式(2d)为道路边界以及障碍物约束，lleft(sk)表示sk对应的l左边界，lright(sk)表示sk对应的l右边界。
8.如权利要求6所述的基于动态规划的智能车辆全局最优轨迹规划系统，其特征在于，求解基于动态规划方法进行最优控制问题的方法包括：
在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散，再根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围，然后通过确定各个区间上的控制变量最优序列形式，来对目标函数进行最优化，以获得整个地图空间下的全局最优轨迹。
9.如权利要求8所述的基于动态规划的智能车辆全局最优轨迹规划系统，其特征在于，在Frenet坐标系将地图空间划分成多个子状态空间，并对子状态空间进行离散的方法具体包括：
根据建图模块所提供地图信息，采集道路中心线作为参考线建立Frenet坐标系并进行状态离散，道路地图的离散根据[s,l,θx]三个维度，s、l、θx分别为纵向距离、横向距离、笛卡尔坐标系下的车辆横摆角。
10.如权利要求4所述的基于动态规划的智能车辆全局最优轨迹规划系统，其特征在于，“根据车辆前轮转角约束，利用状态转移方程确定下一阶段的可行驶范围”的方法具体包括：
先利用式(2)描述的最优控制问题以前轮转角δx为控制量，根据车辆前轮转角范围，按照dδx对前轮转角δx进行离散，由式(3)可计算出根据离散的前轮转角所得的离散曲率控制序列：
其中，κx表示车辆行驶路径曲率，Rx表示车辆转弯半径，Lx为车辆轴距，δx为车辆前轮转角；
再根据由Frenet坐标系与笛卡尔坐标系之间转换关系获得的状态转移方程计算得到车辆在下一阶段的可行驶范围。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310021535.6A CN116185014A (zh) | 2023-01-07 | 2023-01-07 | 一种基于动态规划的智能车全局最优轨迹规划方法与系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310021535.6A CN116185014A (zh) | 2023-01-07 | 2023-01-07 | 一种基于动态规划的智能车全局最优轨迹规划方法与系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116185014A true CN116185014A (zh) | 2023-05-30 |
Family
ID=86437718
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310021535.6A Pending CN116185014A (zh) | 2023-01-07 | 2023-01-07 | 一种基于动态规划的智能车全局最优轨迹规划方法与系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116185014A (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116495013A (zh) * | 2023-06-21 | 2023-07-28 | 北京理工大学 | 面向任务需求的速度规划方法、系统、设备及存储介质 |
| CN116612654A (zh) * | 2023-07-21 | 2023-08-18 | 北京斯年智驾科技有限公司 | 一种无人驾驶车队调度方法、装置及电子设备 |
| CN116674594A (zh) * | 2023-07-28 | 2023-09-01 | 广汽埃安新能源汽车股份有限公司 | 一种基于路径规划的纵向控制方法及装置 |
| CN116817955A (zh) * | 2023-06-30 | 2023-09-29 | 北京易航远智科技有限公司 | 车辆路径规划方法、装置、电子设备及存储介质 |
| CN117112867A (zh) * | 2023-10-24 | 2023-11-24 | 四川国蓝中天环境科技集团有限公司 | 一种基于gps轨迹数据的重型车辆排放计算和可视化方法 |
| CN117234206A (zh) * | 2023-09-05 | 2023-12-15 | 酷哇科技有限公司 | 基于复杂障碍物场景下的避障路径规划方法 |
-
2023
- 2023-01-07 CN CN202310021535.6A patent/CN116185014A/zh active Pending
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116495013A (zh) * | 2023-06-21 | 2023-07-28 | 北京理工大学 | 面向任务需求的速度规划方法、系统、设备及存储介质 |
| CN116495013B (zh) * | 2023-06-21 | 2023-12-01 | 北京理工大学 | 面向任务需求的速度规划方法、系统、设备及存储介质 |
| CN116817955A (zh) * | 2023-06-30 | 2023-09-29 | 北京易航远智科技有限公司 | 车辆路径规划方法、装置、电子设备及存储介质 |
| CN116817955B (zh) * | 2023-06-30 | 2024-04-12 | 北京易航远智科技有限公司 | 车辆路径规划方法、装置、电子设备及存储介质 |
| CN116612654A (zh) * | 2023-07-21 | 2023-08-18 | 北京斯年智驾科技有限公司 | 一种无人驾驶车队调度方法、装置及电子设备 |
| CN116612654B (zh) * | 2023-07-21 | 2023-11-24 | 北京斯年智驾科技有限公司 | 一种无人驾驶车队调度方法、装置及电子设备 |
| CN116674594A (zh) * | 2023-07-28 | 2023-09-01 | 广汽埃安新能源汽车股份有限公司 | 一种基于路径规划的纵向控制方法及装置 |
| CN116674594B (zh) * | 2023-07-28 | 2023-10-27 | 广汽埃安新能源汽车股份有限公司 | 一种基于路径规划的纵向控制方法及装置 |
| CN117234206A (zh) * | 2023-09-05 | 2023-12-15 | 酷哇科技有限公司 | 基于复杂障碍物场景下的避障路径规划方法 |
| CN117234206B (zh) * | 2023-09-05 | 2024-05-14 | 酷哇科技有限公司 | 基于复杂障碍物场景下的避障路径规划方法 |
| CN117112867A (zh) * | 2023-10-24 | 2023-11-24 | 四川国蓝中天环境科技集团有限公司 | 一种基于gps轨迹数据的重型车辆排放计算和可视化方法 |
| CN117112867B (zh) * | 2023-10-24 | 2024-01-09 | 四川国蓝中天环境科技集团有限公司 | 一种基于gps轨迹数据的重型车辆排放计算和可视化方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116185014A (zh) | 一种基于动态规划的智能车全局最优轨迹规划方法与系统 | |
| CN110749333B (zh) | 基于多目标优化的无人驾驶车辆运动规划方法 | |
| CN107702716B (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
| CN112810630B (zh) | 一种自动驾驶车辆轨迹规划方法及系统 | |
| CN113204236B (zh) | 一种智能体路径跟踪控制方法 | |
| CN113721637B (zh) | 智能车动态避障路径连续规划方法、系统及存储介质 | |
| CN113895463B (zh) | 一种适用于自动驾驶车辆掉头的路径规划方法 | |
| CN113830079B (zh) | 任意起始位姿的连续曲率泊车路径在线规划方法及系统 | |
| CN110954122B (zh) | 高速场景下的自动驾驶轨迹生成方法 | |
| CN112965476B (zh) | 一种基于多窗口模型的高速无人车轨迹规划系统及方法 | |
| CN112937555A (zh) | 一种基于车辆运动学模型的平行泊车轨迹规划方法 | |
| CN113886764B (zh) | 一种基于Frenet坐标系的智能车辆多场景轨迹规划方法 | |
| CN111896004A (zh) | 一种狭窄通道车辆轨迹规划方法及系统 | |
| CN113978452B (zh) | 一种自动平行泊车路径规划方法 | |
| CN113515111B (zh) | 一种车辆避障路径规划方法及装置 | |
| CN115077553A (zh) | 基于栅格搜索轨迹规划方法、系统、汽车、设备及介质 | |
| CN112519783B (zh) | 一种智能驾驶的自底向上平滑轨迹生成方法及系统 | |
| Smit et al. | Informed sampling-based trajectory planner for automated driving in dynamic urban environments | |
| CN116429144A (zh) | 基于改进Astar和DWA融合算法的自主车辆路径规划方法 | |
| Chen et al. | A robust trajectory planning method based on historical information for autonomous vehicles | |
| CN111857112A (zh) | 一种汽车局部路径规划方法及电子设备 | |
| Li et al. | An efficient sampling-based hybrid a* algorithm for intelligent vehicles | |
| CN118192601B (zh) | 一种面向非结构化场景的自动驾驶拖拉机路径规划方法 | |
| CN114967710A (zh) | 基于控制点拟合多项式自动驾驶避障路径规划系统及方法 | |
| CN116858254A (zh) | 单舵轮agv路径规划方法 |
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 |