一种基于Frenet坐标系的智能车辆多场景轨迹规划方法技术

一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，属于智能车辆控制、车路协同技术领域。本发明专利技术解决了现有智能车辆协同控制算法使用的坐标系存在适用场景受限，效率低的问题。本发明专利技术根据智能车辆的实际轨迹，获取当前智能车辆在笛卡尔坐标系下x、y参考方向的轨迹信息，再获取智能车辆在Frenet坐标系下的横、纵向坐标，横、纵向的速度和加速度信息；建立智能车辆在Frenet坐标系下的横、纵向轨迹规划集模型，推导获得智能车辆横向、纵向的运动轨迹集；合并智能车辆横、纵向运动的轨迹优化函数，利用所述总优化函数对所述智能车辆的实际应用场景轨迹进行优化，获得该应用场景下最优轨迹。本发明专利技术适用于智能车辆轨迹规划。本发明专利技术适用于智能车辆轨迹规划。本发明专利技术适用于智能车辆轨迹规划。

【技术实现步骤摘要】
一种基于Frenet坐标系的智能车辆多场景轨迹规划方法


[0001]本专利技术属于智能车辆控制、车路协同


技术介绍

[0002]车路协同通过感知、通信等技术的融合，实现“人－车－路－云－网”之间的信息交互与智能协同管控，是道路交通运输领域技术革新的重要发展方向。智能车辆技术是车路协同的重要组成。决策是智能车辆技术的核心环节，其包括行为决策和运动规划。运动规划中的轨迹规划，是决定车辆行驶安全性与舒适性的直接因素，也是智能车辆技术能力的重要体现。
[0003]目前智能车辆在完成行为决策时，使用的坐标系多是笛卡尔坐标系、里程计坐标系、极坐标系等。然而，在智能车辆决策算法实施的过程中，这些坐标系的使用均有一定弊端：(1)基于全局坐标系的算法，轨迹信息受限于局部地图，采样的时效性和模型的拟合受轨迹长度的影响。(2)基于笛卡尔坐标系的算法，效率制约决策系统优化的瓶颈。(3)基于里程计坐标系的算法计算累计误差较大。

技术实现思路

[0004]本专利技术是为了解决现有智能车辆协同控制算法使用的坐标系存在适用场景受限，效率低的问题，提出了一种基于Frenet坐标系的智能车辆多场景轨迹规划方法。
[0005]本专利技术所述的一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，该方法包括：
[0006]步骤一、根据智能车辆的实际轨迹，获取当前智能车辆在笛卡尔坐标系下x、y参考方向上的速度、运动曲率、加速度和方位角信息；
[0007]步骤二、利用笛卡尔坐标系向Frenet坐标系的转换公式和笛卡尔坐标系下x、y参考方向上的速度、运动曲率、加速度和方位角信息，获得智能车辆在Frenet坐标系下的横、纵向坐标，横、纵向的速度和加速度信息；
[0008]步骤三、根据智能车辆在Frenet坐标系下的横、纵向坐标，横、纵向的速度和加速度信息，分别建立智能车辆在Frenet坐标系下的横、纵向轨迹规划集模型；
[0009]步骤四、利用所述智能车辆的横、纵向轨迹规划集模型及智能车辆的横、纵向坐标及横、纵向速度和加速度信息，多次按照不同步长采样推导，获得智能车辆横向、纵向的运动轨迹集；
[0010]步骤五、根据智能车辆的横、纵向应用场景建立智能车辆横向运动的轨迹优化函数和纵向运动的轨迹优化函数，对所述横向运动的轨迹优化函数和纵向运动的轨迹优化函进行合并，获得适用于总应用场景的总优化函数；
[0011]步骤六、利用所述适用于总应用场景的总优化函数和步骤四所述的智能车辆横向、纵向的运动轨迹集，对当前智能车辆实际应用场景下的运动轨迹进行优化，获得智能车辆当前应用场景下的最优运动轨迹。进一步地，本专利技术中，步骤一中，笛卡尔坐标系向
Frenet坐标系的转换公式为：
[0012]s＝s
p
ꢀꢀ
公式一
[0013][0014][0015]d
′
＝(1
‑
dk
p
)tan(θ
x
‑
θ
p
)
ꢀꢀ
公式四
[0016][0017]其中，S为Frenet坐标系下轨迹点x的纵向坐标，s
p
是Frenet坐标系距离轨迹点x最近的一个点p的纵向坐标，为Frenet坐标系下轨迹点x纵向坐标对时间的导数，表示车辆沿参考线方向的速度，为Frenet坐标系下轨迹点x纵向坐标对时间的二阶导数，表示车辆沿参考线方向的加速度，d为Frenet坐标系下为轨迹点x的横向坐标，d
′
为Frenet坐标系下轨迹点x横向坐标对纵向坐标的一阶导数，d
″
为Frenet坐标系下轨迹点x横向坐标对纵向坐标的二阶导数，θ
x
是笛卡尔坐标系下轨迹点x的方位角，θ
p
是笛卡尔坐标系下点P的方位角，k
x
是笛卡尔坐标系下轨迹点x的曲率，k
p
是笛卡尔坐标系下点p的曲率，υ
x
是笛卡尔坐标系下轨迹点x的速度，a
x
是笛卡尔坐标系下点x的加速度,k
p
′
是笛卡尔坐标系下点p的曲率k
p
的导数。
[0018]进一步地，本专利技术中，步骤二中，步骤二中，建立智能车辆的横向轨迹规划集模型为：
[0019]在Frenet坐标系下根据车辆运动学模型确定的智能车辆的五次多项式横向轨迹规划模型：
[0020]d(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5ꢀꢀ
公式六
[0021]式中，a0＝d(t0)，)，d(t0)为规划轨迹初始时刻t0的横向轨迹，为规划轨迹初始时刻t0的横向速度，为规划轨迹初始时刻t0的横向加速，令规划轨迹终止时刻为t
f
，T＝t
f
‑
t0，则a3，a4，a5通过：
[0022][0023]计算获得；
[0024]在Frenet坐标系下根据车辆运动学模型确定的智能车辆的五次多项式纵向轨迹规划模型：
[0025]s(t)＝b0+b1t+b2t2+b3t3+b4t4+b5t5ꢀꢀ
公式八
[0026]式中，b0＝s(t0)，)，s(t0)为规划轨迹初始时刻t0的纵向偏移，为规划轨迹初始时刻t0的横向速度，为规划轨迹初始时刻t0的横向加速，则b3，
b4，b5通过：
[0027][0028]计算获得。
[0029]进一步地，本专利技术中，步骤四中，获得智能车辆横向的运动轨迹集的方法为：
[0030]根据智能车辆的横向轨迹规划集模型，对于时间t∈[t
i
，t
i+1
]，获取智能车辆始末状态横向采样模型：
[0031][0032]其中，d(t
i
)为t
i
时刻车辆的横向偏移，为t
i
时刻车辆的横向速度，为t
i
时刻车辆的横向加速度，由于t
i+1
时刻的车辆的横向偏移d(t
i+1
)、车辆的横向速度和车辆的横向加速度间相互独立，利用所述智能车辆的横向轨迹规划模型等时间步长取样，推导智能车辆的横向运动轨迹，选取不同的时间步长取样进行多次推导，获得横向的运动轨迹集；
[0033]d
set
＝Ψ(t,d0,d
f
),t＝{t0,t1…
t
N
‑1}
ꢀꢀ
公式十一
[0034]其中，d
set
表示生成的横向运动的轨迹集合；t表示采样的N个时间点；d0表示智能车辆于初始状态的横向坐标，d
f
表示由车辆始末状态的采样模型式及采样时间点依次推导出的末状态的横向坐标；Ψ(t
i
,d0,d
f
)表示t∈[t0，t
i
]时的一个确定本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，其特征在于，该方法包括：步骤一、根据智能车辆的实际轨迹，获取当前智能车辆在笛卡尔坐标系下x、y参考方向上的速度、运动曲率、加速度和方位角信息；步骤二、利用笛卡尔坐标系向Frenet坐标系的转换公式和笛卡尔坐标系下x、y参考方向上的速度、运动曲率、加速度和方位角信息，获得智能车辆在Frenet坐标系下的横、纵向坐标，横、纵向的速度和加速度信息；步骤三、根据智能车辆在Frenet坐标系下的横、纵向坐标，横、纵向的速度和加速度信息，分别建立智能车辆在Frenet坐标系下的横、纵向轨迹规划集模型；步骤四、利用所述智能车辆的横、纵向轨迹规划集模型及智能车辆的横、纵向坐标及横、纵向速度和加速度信息，多次按照不同步长采样推导，获得智能车辆横向、纵向的运动轨迹集；步骤五、根据智能车辆的横、纵向应用场景建立智能车辆横向运动的轨迹优化函数和纵向运动的轨迹优化函数，对所述横向运动的轨迹优化函数和纵向运动的轨迹优化函进行合并，获得适用于总应用场景的总优化函数；步骤六、利用所述适用于总应用场景的总优化函数和步骤四所述的智能车辆横向、纵向的运动轨迹集，对当前智能车辆实际应用场景下的运动轨迹进行优化，获得智能车辆当前应用场景下的最优运动轨迹。2.根据权利要求1所述的一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，其特征在于，步骤一中，笛卡尔坐标系向Frenet坐标系的转换公式为：s＝s
p
ꢀꢀꢀꢀ
公式一公式一d
′
＝(1
‑
dk
p
)tan(λ
x
‑
θ
p
)
ꢀꢀꢀꢀ
公式四其中，S为Frenet坐标系下轨迹点x的纵向坐标，s
p
是Frenet坐标系距离轨迹点x最近的一个点p的纵向坐标，为Frenet坐标系下轨迹点x纵向坐标对时间的导数，表示车辆沿参考线方向的速度，为Frenet坐标系下轨迹点x纵向坐标对时间的二阶导数，表示车辆沿参考线方向的加速度，d为Frenet坐标系下轨迹点x的横向坐标，d
′
为Frenet坐标系下轨迹点x横向坐标对纵向坐标的一阶导数，d
″
为Frenet坐标系下轨迹点x横向坐标对纵向坐标的二阶导数，θ
x
是笛卡尔坐标系下轨迹点x的方位角，θ
p
是笛卡尔坐标系下点P的方位角，k
x
是笛卡尔坐标系下轨迹点x的曲率，k
p
是笛卡尔坐标系下点p的曲率，v
x
是笛卡尔坐标系下轨迹点x的速度，a
x
是笛卡尔坐标系下点x的加速度，k
p
′
是笛卡尔坐标系下点p的曲率k
p
的导数。3.根据权利要求1或2所述的一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，其特征在于，步骤二中，建立智能车辆的横向轨迹规划集模型为：在Frenet坐标系下根据车辆运动学模型确定智能车辆的五次多项式横向轨迹规划模型：
d(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5ꢀꢀꢀ
公式六式中，a0＝d(t0)，d(t0)为规划轨迹初始时刻t0的横向轨迹，为规划轨迹初始时刻t0的横向速度，为规划轨迹初始时刻t0的横向加速，令规划轨迹终止时刻为t
f
，T＝t
f
‑
t0，则a3，a4，a5通过：计算获得；在Frenet坐标系下根据车辆运动学模型确定智能车辆的五次多项式纵向轨迹规划模型：s(t)＝b0+b1t+b2t2+b3t3+b4t4+b5t5ꢀꢀꢀꢀ
公式八式中，b0＝s(t0)，s(t0)为规划轨迹初始时刻t0的纵向偏移，为规划轨迹初始时刻t0的横向速度，为规划轨迹初始时刻t0的横向加速，则b3，b4，b5通过：计算获得。4.根据权利要求3所述的一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，其特征在于，步骤四中，获得智能车辆横向的运动轨迹集的方法为：根据智能车辆的横向轨迹规划集模型，对于时间t∈[t
i
，t
i+1
]，获取智能车辆始末状态横向采样模型：其中，d(t
i
)为t
i
时刻车辆的横向偏移，为t
i
时刻车辆的横向速度，为t
i
时刻车辆的横向加速度，由于t
i+1
时刻的车辆的横向偏移d(t
i+1
)、车辆的横向速度和车辆的横向加速度间相互独立，选取不同的时间步长取样进行多次推导，获得横向的运动轨迹集；d
set
＝Ψ(t，d0，d
f
)，t＝{t0，t1…
t
N
‑1}
ꢀꢀꢀꢀ
公式十一其中，d
set
表示生成的横向运动的轨迹集合；t表示采样的N个时间点；d0表示智能车辆于初始状态的横向坐标，d
f
表示由车辆始末状态的采样模型式及采样时间点依次推导出的末状态的横向坐标；Ψ(t
i
，d0，d
f
)表示t∈[t0，t
i
]时的一个确定的横运动的轨迹。5.根据权利要求3所述的一种基于Frenet坐标系的智能车辆多场景轨迹规划方法，其特征在于，步骤四中，获得智能车辆纵向的运动轨迹集的方法为：根据智能车辆的纵向轨迹规划集模型，对于时间t∈[t
i
，t
i+1
]，智能车辆始末状态纵向采样模型：
s(t
i
...

【专利技术属性】
技术研发人员：张瞫，崔建勋，田梦婷，李奎，张岩，刘昕，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：