CN114647246A - 一种时间空间耦合搜索的局部路径规划方法及*** - Google Patents
一种时间空间耦合搜索的局部路径规划方法及*** Download PDFInfo
- Publication number
- CN114647246A CN114647246A CN202210296397.8A CN202210296397A CN114647246A CN 114647246 A CN114647246 A CN 114647246A CN 202210296397 A CN202210296397 A CN 202210296397A CN 114647246 A CN114647246 A CN 114647246A
- Authority
- CN
- China
- Prior art keywords
- search
- information
- time
- track
- space
- 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 46
- 230000008878 coupling Effects 0.000 title claims abstract description 24
- 238000010168 coupling process Methods 0.000 title claims abstract description 24
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 24
- 238000005457 optimization Methods 0.000 claims abstract description 24
- 238000009499 grossing Methods 0.000 claims abstract description 15
- 230000004888 barrier function Effects 0.000 claims abstract description 11
- 238000010845 search algorithm Methods 0.000 claims abstract description 11
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 12
- 238000011478 gradient descent method Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 3
- 230000002123 temporal effect Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000008901 benefit Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000006467 substitution reaction 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种时间空间耦合搜索的局部路径规划方法及***，包括S1、获取主车定位信息、道路信息及障碍物预测信息；S2、根据所述主车定位信息、道路信息及障碍物预测信息构建时间空间耦合的三维占用栅格地图；S3、获取主车在所述三维占用栅格地图中的搜索起点和终点，并通过搜索算法获取初始轨迹；S4、对所述初始轨迹按照预设的优化方法进行平滑处理。本发明针对自动驾驶局部路径规划，采用时间空间耦合搜索的横纵向耦合式路径规划方法，来使规划轨迹在复杂环境场景中得到全局最优解，确保在动态场景下主车行驶轨迹的安全性。
Description
技术领域
本发明属于无人驾驶汽车的决策控制***和运动规划技术领域，更具体涉及一种时间空间耦合搜索的局部路径规划方法及***。
背景技术
无人驾驶技术在国防和国民经济领域具有广阔的应用前景。无人驾驶技术主要有：1)环境感知技术；2)定位技术；3)高精度地图技术；4)决策规划与控制。而决策规划技术可以分为三层：全局路径规划、行为决策、局部路径规划。其中，局部路径规划作为无人驾驶车辆运动控制的关键技术之一，它利用全局参考路径或者全局坐标点的引导信息，并结合实时更新的局部环境感知信息，计算得到车辆在未来有限时域内需要执行的路径或者轨迹，以保证无人驾驶车辆行驶的平稳性和安全性。如我国专利 CN201711387100.4公开了如下专利技术：提出一种用于无人驾驶汽车局部路径运动规划方法和装置，该方案基于无人驾驶汽车期望驾驶路径和感知***检测到的当前车辆周边障碍物信息，根据预设代价函数，获取局部路径轨迹簇中各局部路径轨迹的代价函数值；选取代价函数值最小的局部路径轨迹，最终规划出一条可行驶的最佳局部运动路径。
现有的目前在无人驾驶轨迹规划中，由于种种原因绝大多数公司或研究机构都是采用横向规划和纵向规划解耦的方案，也就是不考虑时间维度的信息先规划出车辆行驶路径，然后在横向规划模块生成路径的基础上进行纵向规划，得到一系列含有速度和时间信息的轨迹点，构成一条完整的轨迹。
解耦的规划方法带来了计算简单的好处，同样也带来了一些不能解决的痛点：轨迹规划结果可能陷入局部最优解，对车辆行驶的安全性带来隐患。比如我们面对一个横向突然出现的障碍物这种场景，对于人来说通常会采取减速并让道去规避最危险的情况，但是对于横向纵向解耦的方案很难实现。
发明内容
为解决上述问题，本发明提供了一种时间空间耦合搜索的局部路径规划方法及***，针对自动驾驶局部路径规划，采用时间空间耦合搜索的横纵向耦合式路径规划方法，来使规划轨迹在复杂环境场景中得到全局最优解，确保在动态场景下主车行驶轨迹的安全性。
为了解决上述技术问题，本发明采用的技术方案是这样的，一种时间空间耦合搜索的局部路径规划方法，包括以下步骤，
S1、获取主车定位信息、道路信息及障碍物预测信息；
S2、根据所述主车定位信息、道路信息及障碍物预测信息构建时间空间耦合的三维占用栅格地图；
S3、获取主车在所述三维占用栅格地图中的搜索起点和终点，并通过搜索算法获取初始轨迹；
S4、对所述初始轨迹按照预设的优化方法进行平滑处理。
作为优化，所述主车定位信息包括主车的定位坐标、车速、加速度；
所述障碍物预测信息包括障碍物在预设时间范围内的运动轨迹信息；
所述道路信息包括道路中心线及路宽信息。
作为优化，所述时间空间耦合的三维占用栅格地图的构建包括，
S201、可行驶时间离散化；
将预设的规划轨迹时长进行等分作为三维占用栅格地图的时间维度，并根据障碍物预测信息记录障碍物各时刻的boundingbox状态；
S202、frenet坐标系转换；
将障碍物boundingbox的四个顶点进行frenet坐标系转换，得到sl坐标系下，各时刻障碍物的boundingbox顶点；
S203、可行驶空间离散化；
将空间分辨率resolution_x和resolution_y对sl坐标系进行离散化处理，并根据每个空间栅格在当前时刻是否被障碍物覆盖生成占用栅格。
作为优化，所述搜索算法为A*搜索算法，包括，
S301、以主车当前时刻的定位信息所在的时间空间栅格作为搜索起点，并确定目标点所在栅格；
S302、采用动作空间加速度和偏航角作为搜索空间，基于选定的车辆运动学模型计算待拓展栅格；
S303、为每一个待拓展栅格计算代价值。
作为优化，所述预设的优化方法采用梯度下降法。
作为优化，所述梯度下降法对初始轨迹的平滑处理包括，
S401、以非参数化的方式建模轨迹，优化变量为轨迹点序列，输入所述初始轨迹；
S402、目标函数结合初始轨迹总时长，轨迹点距离障碍物的距离及预设的约束条件，获取一条无碰撞风险的平滑轨迹
作为优化，所述约束条件包括每个轨迹点与环境之间的碰撞约束、轨迹点之间的曲率约束、速度加速度约束。
基于上述方法，本发明还提供了一种时间空间耦合搜索的局部路径规划***，以实现上述方法，包括，
输入信息模块，用于输入主车定位信息、道路信息及障碍物预测信息；
轨迹计算单元，根据从输入信息模块获取的信息构建时间空间耦合的三维占用栅格地图，并采用A*搜索和预设的优化方法获取无碰撞风险的平滑轨迹。
作为优化，所述轨迹计算单元包括，
构建地图模块，根据从输入信息模块获取的信息通过时间离散化、frenet坐标转换、空间离散化构建时间空间耦合的三维占用栅格地图；
路径搜索模块，采用A*搜索方法在所述三维占用栅格地图中搜索初始轨迹；
轨迹平滑模块，采用预设的优化方法对所述初始轨迹进行平滑处理。
与现有技术相比，本发明具有如下优点：
本发明包含输入信息模块和轨迹计算单元模块。其中输入信息包含道路信息、障碍物预测信息和主车定位信息，该模块主要提供道路中心线及路宽、一定范围内障碍物未来几秒钟的运动轨迹信息、主车实际定位（包含定位坐标、车速、加速度）等信息。轨迹计算单元模块根据从输入信息模块获取的信息构建时间空间耦合的三维搜索地图，采用A*搜索和基于优化的平滑方法得到一条无碰撞风险的平滑轨迹。
本发明针对自动驾驶局部路径规划，采用时间空间耦合搜索的横纵向耦合式路径规划方法，来使规划轨迹在复杂环境场景中得到全局最优解。该发明能够针对在局部路径规划时，环境中存在的对主车安全有影响的动态障碍物会使解耦合式轨迹规划陷入局部优化解的问题，提出一种基于时间空间耦合搜索的轨迹规划方法，进而确保在动态场景下主车行驶轨迹的安全性。
附图说明
图1为本发明***图；
图2为路径搜索算法流程图。
具体实施方式
下面将结合附图及实施例对本发明作进一步说明。
实施例：参见图1-图2，
一种时间空间耦合搜索的局部路径规划方法，包括以下步骤，
S1、获取主车定位信息、道路信息及障碍物预测信息；所述主车定位信息包括主车的定位坐标、车速、加速度；
所述障碍物预测信息包括障碍物在预设时间范围内的运动轨迹信息；
所述道路信息包括道路中心线及路宽信息。
S2、根据所述主车定位信息、道路信息及障碍物预测信息构建时间空间耦合的三维占用栅格地图；所述时间空间耦合的三维占用栅格地图的构建包括，
S201、可行驶时间离散化；
将预设的规划轨迹时长进行等分作为三维占用栅格地图的时间维度，并根据障碍物预测信息记录障碍物各时刻的boundingbox状态。具体的，将预设的规划轨迹时长T按照ΔT进行等分为[T1、T2……、Tn]共计n个时刻，三维栅格地图的时间维度维数即为n。选取车道范围内的所有障碍物的预测轨迹，记录[T1、T2……、Tn]每个时刻各个障碍物的boundingbox状态。
S202、frenet坐标系转换；
将障碍物boundingbox的四个顶点进行frenet坐标系转换，得到sl坐标系下，各时刻障碍物的boundingbox顶点。具体的，将障碍物boundingbox四个顶点，进行frenet坐标系（即SL坐标系）转换，得到sl坐标系下，[T1、T2……、Tn]每个时刻所有障碍物的boundingbox顶点。
S203、可行驶空间离散化；
将空间分辨率resolution_x和resolution_y对sl坐标系进行离散化处理，并根据每个空间栅格在当前时刻是否被障碍物覆盖生成占用栅格。具体的，对空间分辨率resolution_x和resolution_y对sl坐标系进行离散化处理，根据每个空间栅格在当前时刻Tn是否被障碍物覆盖来生成占用栅格。若第Tn时刻在（Xn,Yn）坐标出现障碍物，则该占用栅格为1，表示不可通行。
S3、获取主车在所述三维占用栅格地图中的搜索起点和终点，并通过搜索算法获取初始轨迹；所述搜索算法为A*搜索算法，包括，
S301、以主车当前时刻的定位信息所在的时间空间栅格作为搜索起点，并确定目标点所在栅格；
S302、采用动作空间加速度和偏航角作为搜索空间，基于选定的车辆运动学模型计算待拓展栅格；
S303、为每一个待拓展栅格计算代价值。
过程如附图2所示。关键步骤包括：以主车当前时刻的定位状态所在的时间空间栅格作为搜索起点，并加入open_set中；采用动作空间加速度和偏航角作为搜索空间，基于选定的车辆运动学模型来计算待拓展栅格；为每一个拓展点计算代价值，以当前点的代价值g_cost加上启发代价h_cost，启发代价估计了从当前位置行驶到终点需要的时长。
S4、对所述初始轨迹按照预设的优化方法进行平滑处理。所述预设的优化方法采用梯度下降法。
所述梯度下降法对初始轨迹的平滑处理包括，
S401、以非参数化的方式建模轨迹，优化变量为轨迹点序列，输入所述初始轨迹；
S402、目标函数结合初始轨迹总时长，轨迹点距离障碍物的距离及预设的约束条件，获取一条无碰撞风险的平滑轨迹。所述约束条件包括每个轨迹点与环境之间的碰撞约束、轨迹点之间的曲率约束、速度加速度约束。
基于上述方法，本发明还提供了一种时间空间耦合搜索的局部路径规划***，以实现上述方法，包括，
输入信息模块，用于输入主车定位信息、道路信息及障碍物预测信息；
轨迹计算单元，根据从输入信息模块获取的信息构建时间空间耦合的三维占用栅格地图，并采用A*搜索和预设的优化方法获取无碰撞风险的平滑轨迹。
所述轨迹计算单元包括，
构建地图模块，根据从输入信息模块获取的信息通过时间离散化、frenet坐标转换、空间离散化构建时间空间耦合的三维占用栅格地图；
路径搜索模块，采用A*搜索方法在所述三维占用栅格地图中搜索初始轨迹；
轨迹平滑模块，采用预设的优化方法对所述初始轨迹进行平滑处理。
本发明包含输入信息模块和轨迹计算单元模块。其中输入信息包含道路信息、障碍物预测信息和主车定位信息，该模块主要提供道路中心线及路宽、一定范围内障碍物未来几秒钟的运动轨迹信息、主车实际定位（包含定位坐标、车速、加速度）等信息。轨迹计算单元模块根据从输入信息模块获取的信息构建时间空间耦合的三维搜索地图，采用A*搜索和基于优化的平滑方法得到一条无碰撞风险的平滑轨迹。
本发明针对自动驾驶局部路径规划，采用时间空间耦合搜索的横纵向耦合式路径规划方法，来使规划轨迹在复杂环境场景中得到全局最优解。该发明能够针对在局部路径规划时，环境中存在的对主车安全有影响的动态障碍物会使解耦合式轨迹规划陷入局部优化解的问题，提出一种基于时间空间耦合搜索的轨迹规划方法，进而确保在动态场景下主车行驶轨迹的安全性。
最后需要说明的是，以上实施例仅用以说明本发明的技术方案而非限制技术方案，本领域的普通技术人员应当理解，那些对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，均应涵盖在本发明的权利要求范围当中。
Claims (9)
1.一种时间空间耦合搜索的局部路径规划方法，其特征在于，包括以下步骤，
S1、获取主车定位信息、道路信息及障碍物预测信息；
S2、根据所述主车定位信息、道路信息及障碍物预测信息构建时间空间耦合的三维占用栅格地图；
S3、获取主车在所述三维占用栅格地图中的搜索起点和终点，并通过搜索算法获取初始轨迹；
S4、对所述初始轨迹按照预设的优化方法进行平滑处理。
2.根据权利要求1所述的一种时间空间耦合搜索的局部路径规划方法，其特征在于，所述主车定位信息包括主车的定位坐标、车速、加速度；
所述障碍物预测信息包括障碍物在预设时间范围内的运动轨迹信息；
所述道路信息包括道路中心线及路宽信息。
3.根据权利要求1所述的一种时间空间耦合搜索的局部路径规划方法，其特征在于，所述时间空间耦合的三维占用栅格地图的构建包括，
S201、可行驶时间离散化；
将预设的规划轨迹时长进行等分作为三维占用栅格地图的时间维度，并根据障碍物预测信息记录障碍物各时刻的boundingbox状态；
S202、frenet坐标系转换；
将障碍物boundingbox的四个顶点进行frenet坐标系转换，得到sl坐标系下，各时刻障碍物的boundingbox顶点；
S203、可行驶空间离散化；
将空间分辨率resolution_x和resolution_y对sl坐标系进行离散化处理，并根据每个空间栅格在当前时刻是否被障碍物覆盖生成占用栅格。
4.根据权利要求1所述的一种时间空间耦合搜索的局部路径规划方法，其特征在于，所述搜索算法为A*搜索算法，包括，
S301、以主车当前时刻的定位信息所在的时间空间栅格作为搜索起点，并确定目标点所在栅格；
S302、采用动作空间加速度和偏航角作为搜索空间，基于选定的车辆运动学模型计算待拓展栅格；
S303、为每一个待拓展栅格计算代价值。
5.根据权利要求1所述的一种时间空间耦合搜索的局部路径规划方法，其特征在于，所述预设的优化方法采用梯度下降法。
6.根据权利要求5所述的一种时间空间耦合搜索的局部路径规划方法，其特征在于，所述梯度下降法对初始轨迹的平滑处理包括，
S401、以非参数化的方式建模轨迹，优化变量为轨迹点序列，输入所述初始轨迹；
S402、目标函数结合初始轨迹总时长，轨迹点距离障碍物的距离及预设的约束条件，获取一条无碰撞风险的平滑轨迹。
7.根据权利要求6所述的一种时间空间耦合搜索的局部路径规划方法，其特征在于，所述约束条件包括每个轨迹点与环境之间的碰撞约束、轨迹点之间的曲率约束、速度加速度约束。
8.一种时间空间耦合搜索的局部路径规划***，其特征在于，包括，
输入信息模块，用于输入主车定位信息、道路信息及障碍物预测信息；
轨迹计算单元，根据从输入信息模块获取的信息构建时间空间耦合的三维占用栅格地图，并采用A*搜索和预设的优化方法获取无碰撞风险的平滑轨迹。
9.根据权利要求8所述的一种时间空间耦合搜索的局部路径规划***，其特征在于，所述轨迹计算单元包括，
构建地图模块，根据从输入信息模块获取的信息通过时间离散化、frenet坐标转换、空间离散化构建时间空间耦合的三维占用栅格地图；
路径搜索模块，采用A*搜索方法在所述三维占用栅格地图中搜索初始轨迹；
轨迹平滑模块，采用预设的优化方法对所述初始轨迹进行平滑处理。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210296397.8A CN114647246A (zh) | 2022-03-24 | 2022-03-24 | 一种时间空间耦合搜索的局部路径规划方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210296397.8A CN114647246A (zh) | 2022-03-24 | 2022-03-24 | 一种时间空间耦合搜索的局部路径规划方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114647246A true CN114647246A (zh) | 2022-06-21 |
Family
ID=81994625
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210296397.8A Pending CN114647246A (zh) | 2022-03-24 | 2022-03-24 | 一种时间空间耦合搜索的局部路径规划方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114647246A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115079702A (zh) * | 2022-07-18 | 2022-09-20 | 江苏集萃清联智控科技有限公司 | 一种混合道路场景下的智能车辆规划方法和*** |
| CN115328174A (zh) * | 2022-10-14 | 2022-11-11 | 杭州华橙软件技术有限公司 | 轨迹规划方法、电子设备及存储介质 |
| CN117885764A (zh) * | 2024-03-14 | 2024-04-16 | 中国第一汽车股份有限公司 | 车辆的轨迹规划方法、装置、车辆及存储介质 |
-
2022
- 2022-03-24 CN CN202210296397.8A patent/CN114647246A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115079702A (zh) * | 2022-07-18 | 2022-09-20 | 江苏集萃清联智控科技有限公司 | 一种混合道路场景下的智能车辆规划方法和*** |
| CN115328174A (zh) * | 2022-10-14 | 2022-11-11 | 杭州华橙软件技术有限公司 | 轨迹规划方法、电子设备及存储介质 |
| CN117885764A (zh) * | 2024-03-14 | 2024-04-16 | 中国第一汽车股份有限公司 | 车辆的轨迹规划方法、装置、车辆及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114647246A (zh) | 一种时间空间耦合搜索的局部路径规划方法及*** | |
| US11938926B2 (en) | Polyline contour representations for autonomous vehicles | |
| US11680820B2 (en) | Map information system | |
| Miller et al. | Team Cornell's Skynet: Robust perception and planning in an urban environment | |
| US10571916B2 (en) | Control method for autonomous vehicles | |
| CN113359757A (zh) | 一种改进型混合a*算法的无人驾驶车辆路径规划与轨迹跟踪方法 | |
| CN111258316B (zh) | 一种动态环境下趋势感知的机器人轨迹规划方法 | |
| CN113960996A (zh) | 行驶装置的避障路径的规划方法和装置 | |
| CN112327888B (zh) | 路径规划方法、装置、电子设备和存储介质 | |
| US20230056589A1 (en) | Systems and methods for generating multilevel occupancy and occlusion grids for controlling navigation of vehicles | |
| Park et al. | Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles | |
| Benenson et al. | Achievable safety of driverless ground vehicles | |
| CN116257058A (zh) | 一种园区自动驾驶物流小车智能局部规划方法 | |
| CN115246394A (zh) | 一种智能车超车避障方法 | |
| Brumitt et al. | Dynamic trajectory planning for a cross-country navigator | |
| US11783178B2 (en) | Systems and methods for corridor intent prediction | |
| CN115140048A (zh) | 一种自动驾驶行为决策与轨迹规划模型与方法 | |
| CN117916682A (zh) | 使用时空凸形走廊的运动规划 | |
| RU2790105C2 (ru) | Способ и электронное устройство для управления беспилотным автомобилем | |
| Miller et al. | Team cornell’s skynet: Robust perception and planning in an urban environment | |
| Murthy et al. | Human Behaviour-Based Trajectory Planning for Autonomous Overtaking Maneuver | |
| CN117826825B (zh) | 基于人工势场算法的无人矿卡局部路径规划方法及*** | |
| US20240190452A1 (en) | Methods and systems for handling occlusions in operation of autonomous vehicle | |
| Bajić et al. | Trajectory planning for autonomous vehicle using digital map | |
| CN116880496A (zh) | 一种移动机器人停障方法、装置、电子设备及存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |