一种车辆参考轨迹跟踪方法及系统

摘要

本发明提供一种车辆参考轨迹跟踪方法及系统，其中，车辆参考轨迹跟踪方法包括：实时获取车辆当前车速、车辆横摆角速度、所述参考轨迹的曲率、车辆与所述参考轨迹的相对航向角偏差和相对横向位置偏差、以及车辆运动控制器的期望性能指标；根据所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差以及所述期望性能指标计算获得期望横摆角速度；根据所述期望横摆角速度计算获得所述车辆的最终方向盘转角，并控制车辆输出所述最终方向盘转角。本发明根据期望的运动控制器性能指标直接设计控制器，具有控制效果好、调整参数工作量小、可以缩短开发周期等优点。

说明书

技术领域

本发明涉及智能驾驶技术领域，具体涉及一种车辆参考轨迹跟踪方法及系统。

背景技术

车辆运动控制是自动驾驶汽车的关键技术之一，是以环境感知、决策规划的车辆轨迹为基础，通过控制油门、制动和转向等执行机构准确跟随规划出的目标轨迹，同时使汽车在行驶过程中能够实现车速调节、车距保持、换道、超车等基本操作，以期保证汽车的安全性、操纵性和稳定性。

如何控制车辆更好地跟随目标轨迹是运动控制要解决的核心问题。针对该问题，现阶段技术人员大都从控制算法层面去提高轨迹跟踪精度，如采用PID控制、最优控制、模型预测控制等。但无论采用上述何种算法，在算法设计之初是没有将控制器性能指标(如超调量、调整时间等)纳入控制器设计范围，只是在算法实施之后，通过效果评估不断调整参数直至满足系统性能指标，这一方面需要丰富的参数整定经验，也延长了开发周期。

此外，现有的关于轨迹跟踪的算法，很大一部分都是在大地坐标系和车辆坐标系之间进行坐标转化，相比于Frenet坐标系，存在公式多，计算量大的问题。

发明内容

本发明所要解决的技术问题在于，提供一种车辆参考轨迹跟踪方法及系统，以降低参数整定工作量，缩短开发周期，以及简化控制器设计，提高控制效果。

为了解决上述技术问题，本发明提供一种车辆参考轨迹跟踪方法，包括如下步骤：

步骤S1，实时获取车辆当前车速、车辆横摆角速度、所述参考轨迹的曲率、车辆与所述参考轨迹的相对航向角偏差和相对横向位置偏差、以及车辆运动控制器的期望性能指标；

步骤S2，根据所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差以及所述期望性能指标计算获得期望横摆角速度；

步骤S3，根据所述期望横摆角速度计算获得所述车辆的最终方向盘转角，并控制车辆输出所述最终方向盘转角。

其中，所述步骤S2具体包括：

根据车辆在Frenet坐标系下的运动学模型建立的所述运动控制器的二阶系统方程、所述车辆运动控制器的期望性能指标和所述期望横摆角速度的表达式计算获得所述期望横摆角速度；

其中，所述车辆在Frenet坐标系下的运动学模型基于所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差建立，所述期望横摆角速度的表达式基于所述运动学模型获得。

其中，所述运动学模型为：

其中，v为车辆当前车速，w为车辆当前横摆角速度，θ为车辆与所述参考轨迹的相对航向角偏差，r为车辆与所述参考轨迹的相对横向位置偏差，k为所述参考轨迹的曲率，

其中，所述计算获得所述期望横摆角速度的步骤具体包括：

根据调整时间与二阶系统的自然频率和阻尼比的对应关系、所述超调量与所述阻尼比的对应关系以及所述运动控制器的期望的调整时间和期望的超调量计算获得所述自然频率和阻尼比，其中，所述二阶系统基于所述车辆与参考轨迹的相对横向位置偏差为变量建立，所述二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式根据二阶系统特征方程分别获得；

根据计算获得的自然频率、阻尼比以及二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式计算获得所述二阶系统方程中的各比例系数的值；

根据所述各比例系数的值和所述期望横摆角速度的表达式计算获得所述期望横摆角速度。

其中，所述步骤S3具体包括：

步骤S31，根据当前车速获得车辆的当前运行模式，计算所述当前运行模式下的方向盘转角前馈控制量；

步骤S32，根据所述期望横摆角速度计算所述方向盘转角反馈控制量；

步骤S33，根据所述方向盘转角前馈控制量和所述方向盘转角反馈控制量计算获得所述车辆的最终方向盘转角，并控制输出所述最终方向盘转角。

其中，所述步骤S31具体包括：

判断当前车速是否大于一预设车速，若大于，则判断车辆运行在中高速模式下，进一步根据车辆动力学模型计算所述中高速模式下的方向盘转角前馈控制量；若小于，则判断车辆运行在低速模式下，进一步根据车辆运动学模型计算所述低速模式下的方向盘转角前馈控制量。

其中，所述中高速模式下的方向盘转角前馈控制量为：

其中，δ

其中，所述低速模式下的方向盘转角前馈控制量为：

其中，δ

其中，所述方向盘转角的反馈控制量为：

δ

其中，δ

其中，所述步骤S33具体包括：

当车辆运行在中高速模式下时，计算所述中高速模式下的方向盘转角的前馈控制量和所述方向盘转角的反馈控制量的和获得所述中高速模式下的最终方向盘转角，并控制输出所述中高速模式下的最终方向盘转角；

当车辆运行在低速模式下时，计算所述低速模式下的方向盘转角的前馈控制量和所述方向盘转角的反馈控制量的和获得所述低速模式下的最终方向盘转角，并控制输出所述低速模式下的最终方向盘转角。

本发明还提供一种车辆参考轨迹跟踪系统，包括：

获取单元，用于实时获取车辆当前车速、车辆横摆角速度、所述参考轨迹的曲率、车辆与所述参考轨迹的相对航向角偏差和相对横向位置偏差、以及车辆运动控制器的期望性能指标；

期望横摆角速度计算单元，用于根据所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差以及所述期望性能指标计算获得期望横摆角速度；

最终方向盘转角计算单元，用于根据所述期望横摆角速度计算获得所述车辆的最终方向盘转角，并控制车辆输出所述最终方向盘转角。

其中，所述期望横摆角速度计算单元具体包括：

第一计算模块，用于根据车辆在Frenet坐标系下的运动学模型建立的所述车辆运动控制器的二阶系统方程、所述车辆运动控制器的期望性能指标和所述期望横摆角速度的表达式计算获得所述期望横摆角速度；

存储模块，用于存储基于所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差建立的所述车辆在Frenet坐标系下的运动学模型，以及基于所述运动学模型获得的所述期望横摆角速度的表达式。

其中，所述运动学模型为：

其中，v为车辆当前车速，w为车辆当前横摆角速度，θ为车辆与所述参考轨迹的相对航向角偏差，r为车辆与所述参考轨迹的相对横向位置偏差，k为所述参考轨迹的曲率，

其中，所述第一计算模块具体用于：

根据调整时间与二阶系统的自然频率和阻尼比的对应关系、所述超调量与所述阻尼比的对应关系以及所述运动控制器的期望的调整时间和期望的超调量计算获得所述自然频率和阻尼比，其中，所述二阶系统基于所述车辆与参考轨迹的相对横向位置偏差为变量建立，所述二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式根据二阶系统特征方程分别获得；

根据计算获得的自然频率、阻尼比以及二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式计算获得所述二阶系统方程中的各比例系数的值。

其中，所述最终方向盘转角计算单元进一步包括：

第二计算模块，用于根据当前车速获得车辆的当前运行模式，计算所述当前运行模式下的方向盘转角前馈控制量；

第三计算模块，用于根据所述期望横摆角速度计算所述方向盘转角反馈控制量；

第四计算模块，用于根据所述方向盘转角前馈控制量和所述方向盘转角反馈控制量计算获得所述车辆的最终方向盘转角，并控制输出所述最终方向盘转角。

其中，第二计算模块具体用于判断当前车速是否大于一预设车速，若大于，则判断车辆运行在中高速模式下，进一步根据车辆动力学模型计算所述中高速模式下的方向盘转角前馈控制量；若小于，则判断车辆运行在低速模式下，进一步根据车辆运动学模型计算所述低速模式下的方向盘转角前馈控制量。

实施本发明实施例具有以下有益效果：

根据期望的运动控制器性能指标直接设计控制器，具有控制效果好、调整参数工作量小、可以缩短开发周期等优点；

将满足性能指标的期望横摆角速度作为虚拟控制可实现车辆相对于轨迹的横向位置误差的渐进稳定；

为保证车辆航向角的稳定，结合车辆运动学和动力学模型的特点及适用范围，根据不同的车速，设计中高速和低速控制器，可以很好的兼顾模型复杂度和精度的需求，从而降低整个控制算法的实施难度以及对搭载平台计算能力的需求。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一一种车辆参考轨迹跟踪方法的流程示意图。

图2是本发明实施例一中笛卡尔坐标系示意图。

图3是本发明实施例一中Frenet坐标系流程示意图。

图4是本发明实施例一中步骤S3的流程示意图。

图5是本发明实施例一中车辆运动学模型示意图。

具体实施方式

以下各实施例的说明是参考附图，用以示例本发明可以用以实施的特定实施例。

本发明采用控制器逆向设计的思路，通过设定的系统性能指标直接进行控制器设计，相比于正向设计思路开发的控制器，具有效果好、调参工作量小、节约开发时间的特点。

请参照图1所示，本发明实施例一提供一种车辆参考轨迹跟踪方法，该方法包括如下步骤：

步骤S1，实时获取车辆当前车速、车辆横摆角速度、所述参考轨迹的曲率、车辆与所述参考轨迹的相对航向角偏差和相对横向位置偏差、以及车辆运动控制器的期望性能指标；

步骤S2，根据所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差以及所述期望性能指标计算获得期望横摆角速度；

步骤S3，根据所述期望横摆角速度计算获得所述车辆的最终方向盘转角，并控制车辆输出所述最终方向盘转角。

具体地，步骤S1中，车辆视觉系统采集车辆坐标系下的车辆横摆角ψ、车辆质心侧偏角β、车辆当前车速v，车辆横摆角速度ω，车辆视觉系统将车辆坐标系下的相关参量经过坐标转换，转换到笛卡尔坐标系下，如图2所示，为笛卡尔坐标系，根据车辆运动学模型，获得笛卡尔坐标系下的运动学方程为：

其中，x

由于正交投影不随车辆旋转而变化，只要车速为正，投影点沿路径单调运动，因而采用正交投影的方式将参考点投影到参考轨迹上，从而获得参考轨迹的曲率k、车辆与参考轨迹的相对航向角偏差θ和相对横向位置偏差r。

具体地，车辆运动控制器的期望性能指标包括调整时间t

在一具体实施方式中，步骤S2具体包括：

根据车辆在Frenet坐标系下的运动学模型建立的所述车辆运动控制器的二阶系统方程、所述车辆运动控制器的期望性能指标和所述期望横摆角速度的表达式计算获得所述期望横摆角速度；

其中，所述车辆在Frenet坐标系下的运动学模型基于所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差建立；所述期望横摆角速度的表达式基于所述运动学模型获得。

如图3所示，所述车辆在Frenet坐标系下的运动学模型为：

其中，s为所述参考轨迹的长度，

具体地，基于所述车辆在Frenet坐标下的运动学模型，同时考虑质心侧偏角通常很小，可视为0，做如下定义：

z＝r

基于上述第一等式，可以推导出期望的横摆角速度：

其中，

计算获得期望横摆角速度的步骤具体如下：

根据调整时间与二阶系统的自然频率和阻尼比的对应关系、所述超调量与所述阻尼比的对应关系以及所述运动控制器的期望的调整时间和期望的超调量计算获得所述自然频率和阻尼比，其中，所述二阶系统基于所述车辆与参考轨迹的相对横向位置偏差为变量建立，所述二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式根据二阶系统特征方程分别获得；

根据计算获得的自然频率、阻尼比以及二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式计算获得所述二阶系统方程中的各比例系数的值；

根据所述各比例系数的值和所述期望横摆角速度的表达式计算获得所述期望横摆角速度。

具体地，本实施例做如下定义并设计二阶系统：

则建立的二阶系统的方程为：

由于二阶系统的特征方程为：

将建立的二阶系统方程与二阶系统的特征方程进行对比可知：

k

其中，ω

其中，∈为实际响应和稳态输出之间的误差。

在实际的应用过程中，如果要求二阶系统达到期望的性能指标，即已知调整时间t

当0<ζ<1：

当ζ＝1：

基于上式，在已知调整时间t

在一具体实施方式中，如图4所示，步骤S3具体包括：

步骤S31，根据车辆当前车速获得车辆的当前运行模式，计算所述当前运行模式下的方向盘转角前馈控制量。

具体地，判断当前车速是否大于一预设车速，若大于，则判断车辆运行在中高速模式下，进一步根据车辆动力学模型计算所述中高速模式下的方向盘转角前馈控制量，若小于，则判断车辆运行在低速模式下，进一步根据车辆运动学模式计算所述低速模式下的方向盘转角前馈控制量。其中，所述预设车速为36千米每小时。

当车辆运行在中高速模式下时，搭建的车辆动力学模型为：

其中，m为整车质量，l

当车辆处于稳态时，则

其中，β

因而，方向盘转角的前馈控制量为：

其中，δ

当车辆运行于低速模式下时，车辆运动学模型如图5所示，根据车辆运动学模型可得：

其中，β

基于上式，可以得出低速模式下方向盘转角的前馈控制量δ

步骤S32，根据所述期望横摆角速度计算所述方向盘转角反馈控制量。

具体地，考虑到外界干扰以及模型的不确定性，引入反馈控制，其中，方向盘转角反馈控制量为：

δ

其中，δ

步骤S33，根据所述方向盘转角前馈控制量和所述方向盘转角反馈控制量计算获得所述车辆的最终方向盘转角，并控制输出所述最终方向盘转角。

具体地，当车辆运行在中高速模式下时，最终方向盘转角δ

当车辆运行在低速模式下时，最终方向盘转角δ

在获得了最终方向盘转角后，控制车辆输出最终方向盘转角以跟踪所述参考轨迹。

本发明实施例的参考轨迹跟踪方法，根据车辆在Frenet坐标系下的运动学模型建立的车辆运动控制器的二阶系统方程、所述车辆运动控制器的期望性能指标和期望横摆角速度的表达式计算获得所述期望横摆角速度，基于横摆角速度采用前馈控制和反馈控制计算最终的方向盘转角，以跟踪所述参考轨迹。

本实施例的方法根据期望的运动控制器性能指标直接设计控制器，具有控制效果好、调整参数工作量小、可以缩短开发周期等优点；其次将满足性能指标的期望横摆角速度作为虚拟控制可实现车辆相对于轨迹的横向位置误差的渐进稳定；最后为保证车辆航向角的稳定，结合车辆运动学和动力学模型的特点及适用范围，根据不同的车速，设计中高速模式方向盘前馈控制量和低速模式方向盘前馈控制量，可以很好的兼顾模型复杂度和精度的需求，从而降低整个控制算法的实施难度以及对呆在平台计算能力的需求。

基于本发明实施例一，本发明实施例二提供一种车辆参考轨迹跟踪系统，包括：

获取单元，用于实时获取车辆当前车速、车辆横摆角速度、所述参考轨迹的曲率、车辆与所述参考轨迹的相对航向角偏差和相对横向位置偏差、以及车辆运动控制器的期望性能指标；

期望横摆角速度计算单元，用于根据所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差以及所述期望性能指标计算获得期望横摆角速度；

最终方向盘转角计算单元，用于根据所述期望横摆角速度计算获得所述车辆的最终方向盘转角，并控制车辆输出所述最终方向盘转角。

其中，期望横摆角速度计算单元具体包括：

第一计算模块，用于根据车辆在Frenet坐标系下的运动学模型建立的所述运动控制器的二阶系统方程、所述运动控制器的期望性能指标和所述期望横摆角速度的表达式计算获得所述期望横摆角速度；

存储模块，用于存储基于所述当前车速、所述车辆横摆角速度、所述曲率、所述相对航向角偏差、所述相对横向位置偏差建立的所述车辆在Frenet坐标系下的运动学模型，以及基于所述运动学模型获得的所述期望横摆角速度的表达式。

其中，所述运动学模型为：

其中，v为车辆当前车速，w为车辆当前横摆角速度，θ为车辆与所述参考轨迹的相对航向角偏差，r为车辆与所述参考轨迹的相对横向位置偏差，k为所述参考轨迹的曲率，

其中，所述第一计算模块具体用于：

以所述车辆与参考轨迹的相对横向位置偏差为变量建立所述二阶系统方程；

根据二阶系统特征方程分别获得所述二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式；

根据调整时间与所述二阶系统的自然频率和阻尼比的对应关系、所述超调量与所述阻尼比的对应关系以及所述运动控制器的期望的调整时间和期望的超调量计算获得所述自然频率和阻尼比；

根据计算获得的自然频率、阻尼比以及二阶系统方程中的各比例系数与二阶系统的自然频率和阻尼比的关系表达式计算获得所述二阶系统方程中的各比例系数的值；

根据所述各比例系数的值和所述期望横摆角速度的表达式计算获得所述期望横摆角速度。

其中，所述最终方向盘转角计算单元进一步包括：

第二计算模块，用于根据当前车速获得车辆的当前运行模式，计算所述当前运行模式下的方向盘转角前馈控制量；

第三计算模块，用于根据所述期望横摆角速度计算所述方向盘转角反馈控制量；

第四计算模块，用于根据所述方向盘转角前馈控制量和所述方向盘转角反馈控制量计算获得所述车辆的最终方向盘转角，并控制输出所述最终方向盘转角。

其中，第二计算模块具体用于判断当前车速是否大于一预设车速，若大于，则判断车辆运行在中高速模式下，进一步根据车辆动力学模型计算所述中高速模式下的方向盘转角前馈控制量；若小于，则判断车辆运行在低速模式下，进一步根据车辆运动学模型计算所述低速模式下的方向盘转角前馈控制量。

有关本实施例的工作原理以及所带来的有益效果请参照本发明实施例一的说明，此处不再赘述。

以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。