一种考虑动力学和避障安全性约束的全局路径规划方法技术

本发明专利技术涉及一种考虑动力学和避障安全性约束的全局路径规划方法，首先以全局路径规划方法规划出从起点到终点的初始全局路径；然后提取高精地图中的静态障碍物位置信息，以曲率和与障碍物的距离作为代价函数构造优化问题，基于梯度下降法对规划出的初始全局路径进行首次优化求解，获得预处理的全局路径；最后再以平滑度和长度为代价函数构造凸优化问题，通过QP算法对预处理的全局路径进行二次优化，最终输出满足车辆动力学和避障安全要求的全局路径，以提高自动驾驶车辆在窄通道场景的通过率，提升自动驾驶车辆的平顺性和舒适性。提升自动驾驶车辆的平顺性和舒适性。提升自动驾驶车辆的平顺性和舒适性。

【技术实现步骤摘要】
一种考虑动力学和避障安全性约束的全局路径规划方法


[0001]本专利技术涉及智能驾驶车辆的路径规划
，具体涉及考虑动力学和避障安全性约束的全局路径规划方法。

技术介绍

[0002]路径规划是智能驾驶汽车的核心技术之一，其主要功能是提供车辆行驶路径，指引智能驾驶汽车安全行驶。智能驾驶汽车的路径规划分为全局路径规划和局部路径规划两个层面的工作。全局路径规划基于已知地图信息规划出一条从起点到终点的最短路径。局部路径规划则是以全局路径作为参考路径，根据实时感知到的障碍物信息，规划出安全舒适的路径引导汽车行驶。目前业内在进行全局路径规划时并未考虑车辆动力学和避障安全性约束，而主要是在局部路径规划模块中予以考虑。这加大了在狭窄通道场景局部路径规划的难度，规划出的局部路径很难使主车顺利通行，只能通过脱困、掉头或倒车等辅助方式通行，严重影响自动驾驶流畅性和舒适性。
[0003]现有技术公开了一种自动驾驶车辆的路径规划方法、装置、车辆及介质，其中，方法包括：识别目标区域内所有路段的路段信息；根据路段信息匹配每个路段的实际通行难度等级；根据每个路段的实际通行难度等级和路段之间的连接关系构建拓扑地图，并对拓扑地图进行全局路径搜索，得到自动驾驶车辆的最优全局规划路径，控制车辆自动驾驶沿着最优全局规划行驶。该专利对全局路径规划的搜索算法进行了改进，在计算搜索算法cost(路径权重，指在通过这条路径的时候会耗费多少的代价)项时，考虑了碰撞检测安全性cost，从而在全局上考虑狭窄环境带来的风险。但这种方法容易使搜索路径陷入局部最优解，且并未考虑车辆动力学约束。

技术实现思路

[0004]本专利技术的目的在于提供一种提升驾驶流畅性和舒适性的考虑动力学和避障安全性约束的全局路径规划方法，以解决现有技术中的狭窄通道场景通过难的技术问题。
[0005]为了实现上述目的，本专利技术提供了一种考虑动力学和避障安全性约束的全局路径规划方法，包括如下步骤：
[0006]S1：根据地图和输入的起点、终点信息，生成初始全局路径；
[0007]S2：根据初始全局路径的曲率和曲率差分，提取出初始全局路径中的弯道信息和弯道间的直道信息；
[0008]S3：提取地图中的静态障碍物位置信息，根据初始全局路径中的弯道信息和弯道间的直道信息，对初始全局路径进行碰撞检测和曲率计算，提取出不满足避障和车辆动力学要求的弯道和直道；
[0009]S4：对不满足避障和车辆动力学要求的弯道和直道进行数值优化，获得预处理后的弯道和直道的路径点；再根据路径点两侧固定距离范围内的障碍物信息，得到各路径点的左右边界；
[0010]S5：以平滑度、长度和与原车道线的偏移距离为代价函数构造凸优化问题，凸优化问题的优化变量是预处理后的弯道和直道的路径点的坐标，同时对步骤S4得到的各路径点的左右边界采用硬约束的形式进行施加，通过QP算法对凸优化问题进行求解，得到二次优化后的弯道和直道；
[0011]S6：将二次优化后的弯道和直道与初始全局路径进行匹配和拼接，组成优化后全局路径。
[0012]进一步地，步骤S1包括：
[0013]S101：解析地图，以路段为节点提取加权有向图，根据道路属性，计算各节点cost；
[0014]S102：采用搜索算法，搜索出一条连接起点节点和终点节点的初始全局路径；
[0015]S103：从地图的数据中，解析得到路段中的车道参考线，车道参考线以离散点数组的形式给出，各离散点包括二维及以上的坐标信息，得到初始全局路径中各全局路径点的id和坐标；
[0016]S104：计算全局路径点的曲率和朝向角，并作为信息赋值给全局路径点数组。
[0017]进一步地，在步骤S2中，将初始全局路径中的弯道信息和弯道间的直道信息赋值给全局路径点；
[0018]弯道信息包括弯道id、弯道类型、入弯出弯朝向角、弯道起止点id；弯道间的直道信息包括直道id、直道朝向角、直道起止点id；
[0019]全局路径点信息包括该路径点id、坐标、曲率、朝向角、直道/弯道标识符和所在直道/弯道的id，其中，直道/弯道标识符用以表示该路径点的参考点在直道还是在弯道。
[0020]进一步地，在步骤S3中，对初始全局路径中的各全局路径点依次进行判断，若某一个全局路径点不满足避障和车辆动力学要求，则该全局路径点所在的弯道或直道不满足避障和车辆动力学要求。
[0021]进一步地，在步骤S3中，进行碰撞检测，是以全局路径点坐标作为自车的后轴中心点坐标，以全局路径点的朝向角作为自车朝向的朝向角，计算得到自车的四个角点坐标，计算四个角点坐标与障碍物点的距离，若小于给定的安全距离，则该全局路径点不满足避障安全性要求，若四个角点与障碍物的距离均大于安全距离，则该全局路径点满足避障安全性要求。
[0022]进一步地，在步骤S3中，是通过全局路径点的曲率来判断是否该全局路径点符合车辆动力学要求，若该全局路径点的曲率大于临界曲率，则该全局路径点符合车辆动力学要求；若该全局路径点的曲率小于或等于临界曲率，则该全局路径点不符合车辆动力学要求。
[0023]进一步地，在步骤S4中，对不满足避障和车辆动力学要求的弯道和直道分别构造不同的非凸优化问题，并基于梯度下降法进行求解；其中，
[0024]对于弯道，优化变量为弯道圆弧半径和圆心位置，代价函数包括弯道圆弧半径的倒数Cost_curve、圆弧与障碍物的距离Cost_obs_c、圆弧相对车道线的偏移距离Cost_deviation_c；
[0025]对于直道，优化变量为直线段方程参数，代价函数包括直线段与障碍物的距离Cost_obs_s、直线段路径点相对车道线的偏移距离Cos_deviation_s。
[0026]进一步地，设弯道的代价函数为Cost1，则：
[0027]Cost1＝Cost_curve+Cost_obs_c+Cost_deviation_c；
[0028]其中，w
curve
为动力学权重系数，R为圆弧半径；
[0029]w
obs_c
为避障权重系数，dci为圆弧距离各障碍物点的距离，n为所有的障碍物点数；
[0030](x
r
，y
r
)为圆心坐标，(x
i
，y
i
)为第i个障碍物点的坐标，w为半车宽；
[0031]Cost_deviation_c＝w
deviation_c
l
c
，w
deviation_c
为与原路径靠近的权重系数，l
c
为圆弧距离原路径点距离之和；
[0032]设直道的代价函数为Cost2，则：
[0033]Cost2＝Cost_obs_s+Cos_deviation_s；
[0034]其中，w
obs_s
为避障权重系数，dsi为各障碍物点到直线距离；本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种考虑动力学和避障安全性约束的全局路径规划方法，其特征在于，包括如下步骤：S1：根据地图和输入的起点、终点信息，生成初始全局路径；S2：根据初始全局路径的曲率和曲率差分，提取出初始全局路径中的弯道信息和弯道间的直道信息；S3：提取地图中的静态障碍物位置信息，根据初始全局路径中的弯道信息和弯道间的直道信息，对初始全局路径进行碰撞检测和曲率计算，提取出不满足避障和车辆动力学要求的弯道和直道；S4：对不满足避障和车辆动力学要求的弯道和直道进行数值优化，获得预处理后的弯道和直道的路径点；再根据路径点两侧固定距离范围内的障碍物信息，得到各路径点的左右边界；S5：以平滑度、长度和与原车道线的偏移距离为代价函数构造凸优化问题，凸优化问题的优化变量是预处理后的弯道和直道的路径点的坐标，同时对步骤S4得到的各路径点的左右边界采用硬约束的形式进行施加，通过QP算法对凸优化问题进行求解，得到二次优化后的弯道和直道；S6：将二次优化后的弯道和直道与初始全局路径进行匹配和拼接，组成优化后全局路径。2.根据权利要求1所述的考虑动力学和避障安全性约束的全局路径规划方法，其特征在于，步骤S1包括：S101：解析地图，以路段为节点提取加权有向图，根据道路属性，计算各节点cost；S102：采用搜索算法，搜索出一条连接起点节点和终点节点的初始全局路径；S103：从地图的数据中，解析得到路段中的车道参考线，车道参考线以离散点数组的形式给出，各离散点包括二维及以上的坐标信息，得到初始全局路径中各全局路径点的id和坐标；S104：计算全局路径点的曲率和朝向角，并作为信息赋值给全局路径点数组。3.根据权利要求2所述的考虑动力学和避障安全性约束的全局路径规划方法，其特征在于，在步骤S2中，将初始全局路径中的弯道信息和弯道间的直道信息赋值给全局路径点；弯道信息包括弯道id、弯道类型、入弯出弯朝向角、弯道起止点id；弯道间的直道信息包括直道id、直道朝向角、直道起止点id；全局路径点信息包括该路径点id、坐标、曲率、朝向角、直道/弯道标识符和所在直道/弯道的id，其中，直道/弯道标识符用以表示该路径点的参考点在直道还是在弯道。4.根据权利要求3所述的考虑动力学和避障安全性约束的全局路径规划方法，其特征在于，在步骤S3中，对初始全局路径中的各全局路径点依次进行判断，若某一个全局路径点不满足避障和车辆动力学要求，则该全局路径点所在的弯道或直道不满足避障和车辆动力学要求。5.根据权利要求4所述的考虑动力学和避障安全性约束的全局路径规划方法，其特征在于，在步骤S3中，进行碰撞检测，是以全局路径点坐标作为自车的后轴中心点坐标，以全局路径点的朝向角作为自车朝向的朝向角，计算得到自车的四个角点坐标，计算四个角点坐标与障碍物点的距离，若小于给定的安全距离，则该全局路径点不满足避障安全性要求，
若四个角点与障碍物的距离均大于安全距离，则该全局路径点满足避障安全性要求。6.根据权利要求5所述的考虑动力学和避障安全性约束的全局路径规划方法，其特征在于，在步骤S3中，是通过全局路径点的曲率来判断是否该全局路径点符合车辆动力学要求，若该全局路径点的曲率大于临界曲率，则该全局路径点符合车辆动力学要求；若该全局路径点的曲率小于或等于临界曲率，则该全局路径点不符合车辆动力学要求。7.根据权利要求6所述的考虑动...

【专利技术属性】
技术研发人员：王婷英，徐敏杰，贺勇，文滔，
申请(专利权)人：重庆长安汽车股份有限公司，
类型：发明
国别省市：