一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法技术

本发明专利技术公开了一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，包括：获取自车车辆位置及状态

【技术实现步骤摘要】
一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法


[0001]本专利技术属于自动驾驶车辆轨迹规划
，具体指代一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法
。

技术介绍

[0002]近年来，自动驾驶一直是高校科研人员和企业研发工程师的研究热点，其技术架构主要分为感知
、
决策
、
规划
、
控制四个部分，其中规划算法部分的任务是为车辆找到一条遵循决策层结果且无碰撞
、
符合车辆运动学约束的最优轨迹，其规划得到轨迹是否足够安全
、
舒适且平滑，直接影响到车辆行驶安全性和乘员舒适性
。
[0003]现有的基于优化的轨迹规划算法大多基于五次多项式对车辆横纵向轨迹进行拟合，然而五次多项式的表达能力有限，不适合时空联合规划下具有复杂构型时空障碍和动态约束的问题
。
此外，在目前关于多项式轨迹的研究中，约束仅仅在有限的采样点上进行检查，这种方案无法检测到样本点之间的碰撞，因此无法保证安全性和可行性
。

技术实现思路

[0004]针对于上述现有技术的不足，本专利技术的目的在于提供一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其借助于贝塞尔曲线的凸包特性和凸形图特性，可以方便的对轨迹的高阶量进行约束并通过约束控制点在凸空间内来保证轨迹的安全性
。
[0005]为达到上述目的，本专利技术采用的技术方案如下：
[0006]本专利技术的一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，步骤如下：
[0007]1)
获取自车车辆位置及状态
、
决策粗轨迹
、
动静态障碍物信息
、
车道中心线信息；
[0008]2)
将步骤
1)
中获取到的自车车辆位置及状态
、
决策粗轨迹
、
动静态障碍物信息从笛卡尔坐标系转换到
Frenet
坐标系下，并填充到栅格地图中；
[0009]3)
以转换后的当前车辆位置为规划起点，基于填充了动静态障碍物信息和决策粗轨迹后的栅格地图进行分段时空三维凸空间的构建；
[0010]4)
基于步骤
3)
中构建的时空三维凸空间信息和步骤
1)
中获取的决策粗轨迹信息，采用分段贝塞尔曲线进行轨迹优化，将代价函数
、
约束条件转换成准二次规划问题数学表达式中各个矩阵的形式，并调用求解器库函数进行最优轨迹求解
。
[0011]进一步地，所述步骤
1)
中自车车辆位置及状态包括：笛卡尔坐标系下车辆位置
、
横纵向速度
、
横纵向加速度
、
车辆横摆角
、
车辆转向角以及车辆参数；决策粗轨迹为自动驾驶车辆决策算法输出的车辆未来一段时间的决策轨迹，其由一系列的车辆离散状态点组成，每一个车辆离散状态点包含了对应时间戳的车辆位置及状态；动静态障碍物信息为自动驾驶车辆感知到的周围障碍物信息，其中静态障碍物为在规划周期内状态不会发生改变的障碍物
(
如路边静止的车辆
)
，动态障碍物为在规划周期内状态会随时间变化的障碍物
(
如自车周围不断运动的车辆
、
行人
)
；车道中心线信息为自动驾驶车辆通过高精地图或感知得到的车辆当前行驶车道以及附近车道的中心线信息，存储方式包括但不限于：一系列的车道
中心线离散点
、
以样条曲线拟合的车道中心线函数表达式
。
[0012]进一步地，所述步骤
2)
中栅格地图的分辨率设置为
0.2m。
[0013]进一步地，所述步骤
2)
中的笛卡尔坐标系转换到
Frenet
坐标系分为坐标点位置转换和运动状态转换；在进行坐标点位置转换时，将待转换点
P
向参考线
(
即车道中心线
)
进行投影，在确定待转换点
P
在
Frenet
坐标系下的纵向坐标
s
时，在参考线上寻找与待转换点
P
匹配的投影点
M
＝
(x(s
*
),y(s
*
))
，
s
*
为投影点对应的纵向坐标，使得线段
PM
长度最短，即：
[0014][0015]其中，
D(s)
为线段
PM
的长度：
[0016][0017]符合条件的投影点
M
能够在其切线方向与
PM
垂直，
PM
长度为
|L
*
|
，投影点
M
位于参考轨迹
s
＝
s
*
处，则待转换点
P
在
Frenet
坐标系中的坐标值可确定为
(s
*
,l
*
)
；在计算
s
*
时，使用二分法获取距离较短的初始解
s
init
，在初始解
s
init
的微小邻域利用牛顿法迭代求解使得
D
′
(s)
＝0的精确解
s
＝
s
*
，迭代公式如下：
[0018][0019]式中，为每次迭代的纵向坐标；
[0020]使用对
s
init
赋值并继续重复计算，直至取值收敛时将收敛值记为
s
*
；在计算
l
*
时，求解公式如下：
[0021][0022]式中，
l
为
Frenet
坐标系下的横向坐标，
(x
x
,y
x
)
为待转换点在笛卡尔坐标系下的坐标，
(x
r
,y
r
)
为投影点在笛卡尔坐标系下的坐标，
θ
r
为投影点向量与笛卡尔坐标系
x
轴的夹角；
[0023]上述过程完成了坐标点从笛卡尔坐标系到
Frenet
坐标系的转换，对于车辆和动态障碍物，需进行运动状态
(
速度，加速度等
)
的转换，从笛卡尔坐标系向
Frenet
坐标系转换依据以下公式进行：
[0024][0025]式中，
s、
分别为转换后纵向坐标对时间的
0、1、2
阶本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，步骤如下：
1)
获取自车车辆位置及状态
、
决策粗轨迹
、
动静态障碍物信息
、
车道中心线信息；
2)
将步骤
1)
中获取到的自车车辆位置及状态
、
决策粗轨迹
、
动静态障碍物信息从笛卡尔坐标系转换到
Frenet
坐标系下，并填充到栅格地图中；
3)
以转换后的当前车辆位置为规划起点，基于填充了动静态障碍物信息和决策粗轨迹后的栅格地图进行分段时空三维凸空间的构建；
4)
基于步骤
3)
中构建的时空三维凸空间信息和步骤
1)
中获取的决策粗轨迹信息，采用分段贝塞尔曲线进行轨迹优化，将代价函数
、
约束条件转换成准二次规划问题数学表达式中各个矩阵的形式，并调用求解器库函数进行最优轨迹求解
。2.
根据权利要求1所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤
1)
中自车车辆位置及状态包括：笛卡尔坐标系下车辆位置
、
横纵向速度
、
横纵向加速度
、
车辆横摆角
、
车辆转向角以及车辆参数；决策粗轨迹为自动驾驶车辆决策算法输出的车辆未来一段时间的决策轨迹，其由一系列的车辆离散状态点组成，每一个车辆离散状态点包含了对应时间戳的车辆位置及状态；动静态障碍物信息为自动驾驶车辆感知到的周围障碍物信息，其中静态障碍物为在规划周期内状态不会发生改变的障碍物，动态障碍物为在规划周期内状态会随时间变化的障碍物；车道中心线信息为自动驾驶车辆通过高精地图或感知得到的车辆当前行驶车道以及附近车道的中心线信息
。3.
根据权利要求1所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤
2)
中栅格地图的分辨率设置为
0.2m。4.
根据权利要求1所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤
2)
中的笛卡尔坐标系转换到
Frenet
坐标系分为坐标点位置转换和运动状态转换；在进行坐标点位置转换时，将待转换点
P
向参考线进行投影，在确定待转换点
P
在
Frenet
坐标系下的纵向坐标
s
时，在参考线上寻找与待转换点
P
匹配的投影点
M
＝
(x(s
*
),y(s
*
))
，
s
*
为投影点对应的纵向坐标，使得线段
PM
长度最短，即：其中，
D(s)
为线段
PM
的长度：符合条件的投影点
M
能够在其切线方向与
PM
垂直，
PM
长度为
|L
*
|
，投影点
M
位于参考轨迹
s
＝
s
*
处，则待转换点
P
在
Frenet
坐标系中的坐标值可确定为
(s
*
,l
*
)
；在计算
s
*
时，使用二分法获取距离较短的初始解
s
init
，在初始解
s
init
的微小邻域利用牛顿法迭代求解使得
D
′
(s)
＝0的精确解
s
＝
s
*
，迭代公式如下：式中，为每次迭代的纵向坐标；使用对
s
init
赋值并继续重复计算，直至取值收敛时将收敛值记为
s
*
；在计算
l
*
时，求解公式如下：
式中，
l
为
Frenet
坐标系下的横向坐标，
(x
x
,y
x
)
为待转换点在笛卡尔坐标系下的坐标，
(x
r
,y
r
)
为投影点在笛卡尔坐标系下的坐标，
θ
r
为投影点向量与笛卡尔坐标系
x
轴的夹角；上述过程完成了坐标点从笛卡尔坐标系到
Frenet
坐标系的转换，对于车辆和动态障碍物，需进行运动状态的转换，从笛卡尔坐标系向
Frenet
坐标系转换依据以下公式进行：式中，
s、
分别为转换后纵向坐标对时间的
0、1、2
阶次导数，
l、l
′
、l
″
分别为转换后横向坐标对纵向坐标的
0、1、2
阶次导数，
v
x
为车辆在笛卡尔坐标系下的车速，
θ
x
为待转换点向量与笛卡尔坐标系
x
轴的夹角，
k
r
、k<...

【专利技术属性】
技术研发人员：赵万忠，王睿，周小川，王春燕，章波，张雨杰，
申请(专利权)人：南京航空航天大学秦淮创新研究院，
类型：发明
国别省市：