一种基于轨迹优化算法的自动驾驶规划控制方法技术

本发明专利技术属于汽车控制技术领域，公开了一种基于轨迹优化算法的自动驾驶规划控制方法：通过车辆传感器获取车辆当前在世界坐标系下位置信息和速度信息；通过矢量地图获取车辆行驶目标与车辆当前在地图坐标系下位置信息；建立车辆系统动力学模型；根据车辆的速度选取轨迹迭代次数；建立系统动力学迭代代价函数；添加代价函数的求解约束；求解完整轨迹代价函数，获取期望规划轨迹和对应的期望控制列表；将期望控制列表发送给车辆控制器。本发明专利技术解决了实时规划与控制互相影响的问题。时规划与控制互相影响的问题。时规划与控制互相影响的问题。

【技术实现步骤摘要】
一种基于轨迹优化算法的自动驾驶规划控制方法


[0001]本专利技术属于汽车控制
，具体涉及一种基于轨迹优化算法的自动驾驶规划控制方法。

技术介绍

[0002]随着人工智能，控制算法等技术的的不断发展，自动驾驶相关技术的也得到了质的提升。在辅助驾驶，自动泊车等方面得到了广泛的应用，辅助提高了有人驾驶的安全性，减少了部分有人驾驶存在的驾驶失误风险。
[0003]由于辅助驾驶等技术智能性不足，因此虽然在一定程度解决了有人驾驶存在的问题，但还需人为介入。所以为了实现狭义的自动驾驶，更为智能的技术非常必要。其中特别是如果在没有人为干预时，运行车辆能在动态合理的轨迹上行驶成为自动驾驶的基础。本专利技术将实现车辆自动驾驶的规划与控制中，更加智能与更强的鲁棒性。
[0004]现有的自动驾驶规划控制方法基本分为两部分，规划与控制。通过规划算法规划出的轨迹发送给控制器作为车辆期望状态进行控制。往往存在规划频率与控制频率差别过大，从而引起控制之后导致车辆控制不够平稳，规划不考虑车辆运动学或动力学约束导致的轨迹阶跃等缺点。
[0005]鉴于上述缺陷，提供一种基于轨迹优化算法的自动驾驶规划控制方法，使得规划与控制在相同周期完成。

技术实现思路

[0006]针对上述提出的技术问题，本专利技术提供一种基于轨迹优化算法的自动驾驶规划控制方法，旨在进行自动驾驶规划控制时使规划与控制在相同周期完成。
[0007]本专利技术提供了一种基于轨迹优化算法的自动驾驶规划控制方法，该方法包括如下的步骤：/>[0008]步骤1、通过车辆传感器获取车辆当前在世界坐标系下位置信息和速度信息；
[0009]步骤2、通过矢量地图获取车辆行驶目标与车辆当前在地图坐标系下位置信息；
[0010]步骤3、建立车辆系统动力学模型；
[0011]步骤4、根据车辆的速度选取轨迹迭代次数；
[0012]步骤5、建立系统动力学迭代代价函数；
[0013]步骤6、添加迭代代价函数的求解约束；
[0014]步骤7、求解迭代代价函数，获取期望规划轨迹和对应的期望控制列表；
[0015]步骤8、将期望控制列表发送给车辆控制器。
[0016]具体地，在步骤1中，通过定位系统rtk读取车辆当前在世界系统下的位姿P
w
。
[0017]具体地，在步骤2中，通过坐标转换矩阵
w
R
m
将位置信息转换到地图坐标系，通过
w
P
m
＝
w
R
m
P
w
得到车辆当前在地图坐标系下的初始位姿矢量并确定目标位置在地
图坐标系下的目标位姿矢量
[0018]具体地，在步骤3中，建立车辆的动力学方程
[0019]将车辆初始状态设置为位姿矢量为初始位姿矢量P
m0
，初始速度矢量为
[0020]具体地，在步骤5中，动力学迭代代价函数为：
[0021]cost＝(x(1,N)
‑
x
t
(1))2+x(2,N)
‑
x
t
(2))2+x(3,N)
‑
x
t
(3))2+x(4,N)
‑
x
t
(4))2[0022]+x(5,N)
‑
x
t
(5))2+x(6,N)
‑
x
t
(6))2[0023]其中，x(i,N)表示在第N个时刻位置的第i个状态分量，x
t
(i)表示目标点的第i个状态分量，N为迭代次数。
[0024]具体地，在步骤6中，求解约束包括等式约束和不等式约束。
[0025]具体地，等式约束包括：
[0026]系统动力学约束
[0027]初始状态约束x
t_0
＝x0；
[0028]最终状态x方向约束
[0029]最终状态y方向约束
[0030]其中，
[0031][0032]δ为车辆前轮转角，为车辆转角，为车辆转角速度，C
cf
为车辆前轮横向刚度，C
cr
为车辆后轮横向刚度，l
f
为前轮到车辆质心距离，l
r
为后轮到车辆质心距离，I
z
为车辆转动惯量。
[0033]具体地，不等式约束包括：
[0034]每个周期内x方向距离在
±
0.3m内
[0035]每个周期内y方向距离在
±
0.15m内
[0036]每个周期内控制量角度在
±
360度内
[0037]每个周期内车辆纵向速度在7.2m/s内
[0038]每个周期内横向速度变化量在
±
0.5m/s2内
[0039]每个周期内纵向速度变化量在
±
0.5m/s2内
[0040]每个周期内车辆姿态角变化量在
±
15度内
[0041]具体地，在步骤7中，期望控制列表包括系统状态列表和控制量列表；
[0042]通过第三方非线性求解器casadi，将迭代代价函数和求解约束放入求解器中求解，得到N个周期对应的系统状态列表x_list和控制量列表u_list。
[0043]具体地，在步骤8中，将控制量列表u_list中的前轮转角和系统状态列表x_list中的速度按照队列同时发送给车辆控制器，车辆按照规划出的轨迹行驶。
[0044]本专利技术公开一种基于轨迹优化算法的自动驾驶规划控制方法，减少了自动驾驶中的技术复杂程度，加快了规划到控制的时间，使传统控制对于规划跟随的方式变为控制与规划一一对应，解决了现有实时规划与控制互相影响的问题。
附图说明
[0045]图1为本专利技术的一种基于轨迹优化算法的自动驾驶规划控制方法的流程图。
具体实施方式
[0046]为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本专利技术经行进一步的详细说明。显然，此处所描述的具体实施例仅仅用于解释本专利技术，是本专利技术的一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域技术普通人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本专利技术保护的范围。
[0047]图1所示是本专利技术提供的一种基于轨迹优化算法的自动驾驶规划控制方法的一个实施例的流程图，该流程图具体包括如下的步骤：
[0048]步骤1、通过车辆传感器获取车辆当前在世界坐标系下位置信息和速度信息。
[0049]具体地，在步骤1中，通过定位系统rtk读取车辆当前在世界系统下的位姿P
w
。
[0050]步骤2、通过矢量地图获取车辆行驶目标与车辆当前在地图坐标系下位置信息。
[0051]本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于轨迹优化算法的自动驾驶规划控制方法，其特征在于，包括如下的步骤：步骤1、通过车辆传感器获取车辆当前在世界坐标系下位置信息和速度信息；步骤2、通过矢量地图获取车辆行驶目标与所述车辆当前在地图坐标系下位置信息；步骤3、建立车辆系统动力学模型；步骤4、根据所述车辆的速度选取轨迹迭代次数；步骤5、建立系统动力学迭代代价函数；步骤6、添加所迭代述代价函数的求解约束；步骤7、求解所述迭代代价函数，获取期望规划轨迹和对应的期望控制列表；步骤8、将所述期望控制列表发送给车辆控制器。2.根据权利要求1所述的一种基于轨迹优化算法的自动驾驶规划控制方法，其特征在于，在所述步骤1中，通过定位系统rtk读取所述车辆当前在世界系统下的位姿P
w
。3.根据权利要求1所述的一种基于轨迹优化算法的自动驾驶规划控制方法，其特征在于，在所述步骤2中，通过坐标转换矩阵
w
R
m
将所述位置信息转换到地图坐标系，通过
w
P
m
＝
w
R
m
P
w
得到所述车辆当前在地图坐标系下的初始位姿矢量并确定目标位置在地图坐标系下的目标位姿矢量4.根据权利要求1所述的一种基于轨迹优化算法的自动驾驶规划控制方法，其特征在于，在所述步骤3中，建立所述车辆的动力学方程将所述车辆初始状态设置为位姿矢量为所述初始位姿矢量P
m0
，初始速度矢量为5.根据权利要求1所述的一种基于轨迹优化算法的自动驾驶规划控制方法，其特征在于，在所述步骤5中，所述动力学迭代代价函数为：cost＝(x(1,N)
‑
x
t
(1))2+x(2,N)
‑
x
t
(2))2+x(3,N)
‑
x
t
(3))2+x(4,N)
‑
x
t
(4))2+x(5,N)
‑
x
t
(5))2+x(6,N)
‑
x
t
(6))2其中，x(i,N)表示在第N个时刻位置的第i个状态分量，x
t<...

【专利技术属性】
技术研发人员：江辉，骆嫚，熊胜健，鲁若宇，赵子实，
申请(专利权)人：东风悦享科技有限公司，
类型：发明
国别省市：