CN116520857A - 一种车辆轨迹跟踪方法 - Google Patents

Abstract

本发明提供一种车辆轨迹跟踪方法，通过获取的车辆当前位置坐标及其他参数信息，进行横向偏差与航向偏差的计算，根据Stanley控制算法，推导得到车辆的前轮转角，并通过粒子群算法对特定车速下的增益参数进行优化，同时建立基于消除航向偏差的LQR控制算法，计算由航向偏差引起的前轮转角，再建立基于权重占比的Stanley‑LQR策略，能够根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比，得到车辆的方向盘转角，对车辆进行横向控制，同时采用PID控制器对车辆进行纵向控制，实现轨迹跟踪，提高了在变道工况下车辆行驶的横向稳定性，轨迹跟踪效果更加平稳，根据实际工况进行权重分配，使车辆横向控制相对平滑，鲁棒性增强，收敛速度加快。

Description

一种车辆轨迹跟踪方法

技术领域

本发明涉及智能汽车技术领域，特别是涉及一种车辆轨迹跟踪方法。

背景技术

在社会要求汽车智能化、低碳化、轻量化的当下，智能电动汽车已经成为提高汽车安全性、减少环境污染、节约能源的重要手段。智能电动汽车的控制算法一般分为基于运动学模型的控制算法和基于动力学模型的控制算法。其中，在进行慢速行驶汽车的横向控制时，综合考虑控制算法和建模简便性等因素，通常选用基于运动学模型的Stanley控制算法进行车辆路径跟踪。

现有的Stanley控制算法在车辆行驶过程中横向稳定性有待提高，尤其是在高速工况下存在控制输入震荡及车头左右摆动的问题。同时，Stanley控制器对参数依赖性过强，这是目前多数运动控制器普遍存在的问题。当选取合适的增益参数后，Stanley控制器会在某种道路路线或者特定车速范围下运行良好。申请号为CN202210002683.9的中国专利公开了“基于改进Stanley控制的车辆路径跟踪方法”，该方法通过模糊控制算法对Stanley控制器的增益参数不断调优，具有良好的适应性，但是控制输入与控制目标无确定比例关系，具有一定的局限性，存在控制精度低、鲁棒性差的问题。

发明内容

本发明的目的在于提供一种车辆轨迹跟踪方法，以解决现有技术控制精度低、鲁棒性差的问题。

一种车辆轨迹跟踪方法，包括以下步骤：

步骤1，获取车辆当前位置坐标、当前时刻的航向角、当前时刻的质心侧偏角、当前时刻的速度、当前时刻的横摆角速度，并根据目标轨迹路径获取跟踪路径轨迹点坐标和每个轨迹点的航向角；

步骤2，基于车辆当前位置坐标、当前时刻的航向角及根据目标轨迹路径获取的跟踪路径轨迹点坐标和每个轨迹点的航向角，计算前轮中心的横向偏差与航向偏差，并通过Stanley控制算法计算出车辆前轮转角，其中，对于Stanley控制算法中的增益参数，以增益参数的差值平方和最小作为搜索求解的目标函数，通过粒子群算法对增益参数进行优化调整；

步骤3，基于当前时刻的航向角、每个轨迹点的航向角、当前时刻的速度、当前时刻的横摆角速度，建立基于消除航向角偏差的线性二次型调节器控制算法，计算由航向偏差引起的前轮转角；

步骤4：基于步骤2得到的车辆前轮转角和步骤3得到的由航向偏差引起的前轮转角，针对车辆变道场景，建立基于权重占比的控制策略，该控制策略以航向角为判别依据，根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比，得到最优前轮转角；

步骤5：基于最优前轮转角获取车辆当前时刻的方向盘转角，对车辆进行横向控制，同时基于当前时刻的速度，采用PID控制器对车辆进行纵向控制，从而对车辆进行轨迹跟踪。

根据本发明提供的车辆轨迹跟踪方法，通过获取的车辆当前位置坐标及其他参数信息，进行横向偏差与航向偏差的计算，根据Stanley控制算法，推导得到车辆的前轮转角，并通过粒子群算法对特定车速下的增益参数进行优化，同时建立基于消除航向偏差的LQR（线性二次型调节器）控制算法，计算由航向偏差引起的前轮转角，再建立一种基于权重占比的Stanley-LQR策略，能够根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比，得到车辆的方向盘转角，对车辆进行横向控制，同时采用PID控制器对车辆进行纵向控制，实现轨迹跟踪，本发明提高了在变道工况下车辆行驶的横向稳定性，轨迹跟踪效果更加平稳，根据实际工况进行权重分配，使车辆横向控制相对平滑，鲁棒性增强，收敛速度加快。

附图说明

图1为本发明的实施例提供的车辆轨迹跟踪方法的流程图；

图2为Stanley控制算法的控制模型示意图；

图3为车辆的典型变道场景示意图；

图4增益参数优化前后的前轮转角对比图；

图5为轨迹跟踪效果对比示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1，本发明的实施例提供一种车辆轨迹跟踪方法，包括步骤1~步骤5：

步骤1，获取车辆当前位置坐标、当前时刻的航向角、当前时刻的质心侧偏角、当前时刻的速度、当前时刻的横摆角速度，并根据目标轨迹路径获取跟踪路径轨迹点坐标和每个轨迹点的航向角。

对于智能电动汽车，为实现汽车的横向闭环控制，一般需要独立的感知传感器进行辅助，包括摄像头、毫米波雷达等，车辆当前位置坐标、当前时刻的航向角、当前时刻的质心侧偏角、当前时刻的速度、当前时刻的横摆角速度、当前时刻的方向盘转角都可以通过车辆的感知传感器感知得到。

步骤2，基于车辆当前位置坐标、当前时刻的航向角及根据目标轨迹路径获取的跟踪路径轨迹点坐标和每个轨迹点的航向角，计算前轮中心的横向偏差与航向偏差，并通过Stanley控制算法计算出车辆前轮转角，其中，对于Stanley控制算法中的增益参数，以增益参数的差值平方和最小作为搜索求解的目标函数，通过粒子群算法对增益参数进行优化调整。

具体的，请参阅图2，首先基于步骤1中车辆当前位置坐标与给定路径的几何关系得到横向偏差e(t)，然后基于步骤1中当前时刻的航向角与目标路径最近点的航向角，根据目标路径轨迹以及车辆运动学模型，解算出航向偏差，最后基于横向偏差、航向偏差，推导Stanley控制算法，得到车辆前轮转角。

其中，航向偏差的表达式为：

其中，为当前时刻的航向角，/>为目标轨迹航向角。

在不考虑侧面跟踪误差的情况下，前轮偏转角与给定路径的切线方向一致。其中，航向偏差是车辆的航向与最近路线点的切线方向之间的夹角，在没有横向误差的情况下，前轮的方向与路径点的方向相同，得到不考虑横向偏差时前轮转角/>为:

在不考虑航向跟踪偏差的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预定轨迹在距离前轮d(t)处与给定路径上最近点切线相交，根据几何关系得到非线性比例函数如下式所示：

其中，为不考虑航向偏差时前轮转角，k为增益参数，v(t)为当前时刻的速度。

上式中，d(t)与车速有关，可以由车速v(t)和增益参数k表示。随着横向偏差的增加，arctan函数会产生所期望的前轮偏角，偏转直接指向所需路径，并且会受到车速v(t)的限制，从而得到：

其中，表示车辆前轮转角。

进而得到车辆前轮转角的表达式为：

其中，表示车辆前轮转角，/>为航向偏差，e(t)为横向偏差，k为增益参数，v(t)为当前时刻的速度。

Stanley控制算法的特点是跟踪效果主要受增益参数k的影响，并且拥有前馈控制，能够提前补足转向过程中的车辆转角。车辆的横向稳定能力是由车辆行驶速度和前轮偏角，即增益参数k决定的。

在航向偏差为0的条件下，基于车辆的不同时刻的横向偏差和纵向速度值，推导目标增益参数k _e，增益参数的差值平方和最小作为搜索求解的目标函数。为了加快收敛速度，提高识别精度，可通过粒子群算法对增益参数k进行优化调整。粒子群算法是一种群体智能的优化算法，它相对简单、容易实现，具有搜索效率高、可以局部和全局搜索的优点。

具体的，以增益参数的差值平方和最小作为搜索求解的目标函数的步骤中，满足以下条件式：

其中：

其中，k _e为目标增益参数，为期望转角，/>为期望质心侧偏角，l为车辆轴距，F为稳定性因数，a为车辆质心到车辆前轴的距离，b为车辆质心到车辆后轴的距离，m为车辆的整车质量，C _f、C _r分别为车辆前后轮侧偏刚度。

在推导增益参数的过程中，考虑了质心侧偏角因素，一定程度上提升了车辆在高速变道工况下的横向控制水平。

通过粒子群算法对增益参数进行优化调整的步骤中，满足以下条件式：

其中，h为参数信息的实际方差，为优化参数差值序列，T表示转置操作，/>为优化参数真实值，W _i为权重系数，n为迭代次数，N为大于0的自然数，为实际参数差值序列。

根据所选择的目标函数计算出每个粒子当前位置的适应度值，经过不断地迭代求值，得到最终的优化值。

步骤3，基于当前时刻的航向角、每个轨迹点的航向角、当前时刻的速度、当前时刻的横摆角速度，建立基于消除航向角偏差的线性二次型调节器控制算法，计算由航向偏差引起的前轮转角。

通过优化Stanley控制算法中的增益参数k，提高了其消除横向偏差的速度与精度，为提高航向的前轮转角计算精度与收敛速度，本发明还提出一种基于消除航向偏差的LQR（线性二次型调节器）控制算法，计算由航向偏差引起的前轮转角。

LQR控制算法稳态误差小，能够弥补Stanley控制算法在高速驾驶跟踪场景下鲁棒性差的缺点，补偿优化后的Stanley控制算法的减小航向偏差方面，提高车辆变道行驶的横向运行稳定性，能够更好的通过控制车辆方向盘，使车辆在道路上平稳行驶，达到需要的控制效果。

其中，步骤3具体包括：

步骤301，建立基于跟踪误差的动力学模型；

步骤302，通过动力学模型推导得到基于航向偏差的状态空间方程；

步骤303，通过基于航向偏差的状态空间方程设定控制性能目标函数；

步骤304，基于控制性能目标函数提高加权矩阵中航向偏差的权重占比，得到由航向偏差引起的前轮转角。

进一步的，所述步骤301中，基于跟踪误差的动力学模型中，在特定车速的条件下，由车辆在侧向和横摆两个方向进行受力分析，得到下示方程：

其中，m为车辆的整车质量，a _y为汽车侧向加速度，F _f、F _r为前、后轴侧向力，I _z为绕z轴转动量，为横摆角速度，a为车辆质心到车辆前轴的距离，b为车辆质心到车辆后轴的距离。

根据航向偏差、横向偏差与车速进行推导，能够得到基于跟踪误差的动力学方程。

进而，由航向偏差引起的前轮转角通过下式计算得到：

其中，J表示控制性能目标函数，t表示t时刻，为状态变量，/>表示由航向偏差引起的前轮转角，Q、R为加权矩阵，T表示转置操作。

通过上式的极小值可得到前轮转角控制规律如下式所示：

其中，K、B ₁、P、A均为中间向量，P满足Riccati方程，T表示转置操作。

步骤4：基于步骤2得到的车辆前轮转角和步骤3得到的由航向偏差引起的前轮转角，针对车辆变道场景，建立基于权重占比的控制策略，该控制策略以航向角为判别依据，根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比，得到最优前轮转角。

其中，最优前轮转角的表达式为：

其中，表示最优前轮转角，m _a表示最优前轮转角的权重因子，m _b表示由航向偏差引起的前轮转角的权重因子，且m _a+m _b=1。

根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比的具体过程中，以航向角为判别依据，将车辆典型变道过程分为两个过程：

在图3中的过程①中，航向角不断变大，此时为加快收敛速度，提高跟踪精度，需要减小横向偏差，因此加大Stanley控制算法的权重占比，此时满足关系：m _a>m _b；

在图3中的过程②中，航向角从最大航向角开始减小，车辆的主要目标为调整车头方向到目标路径轨迹，为减小航向偏差，加大LQR算法的权重，此时满足关系：m _a<m _b。

m _a、m _b根据不同车速与变道所处阶段进行实时调整，达到最优前轮转向角控制，满足不同阶段的行驶需求，根据实际车况提高收敛速度，使跟踪效果保持在相对平稳的水平上。

步骤5：基于最优前轮转角获取车辆当前时刻的方向盘转角，对车辆进行横向控制，同时基于当前时刻的速度，采用PID控制器对车辆进行纵向控制，从而对车辆进行轨迹跟踪。

其中，具体可以根据基于前轮转角和方向盘转角的预设关系获取车辆当前时刻的方向盘转角，输入实车进行控制。本实施例中，预设前轮转角和方向盘转角的比例关系为8:1。

对上述方法进行仿真测试，仿真实验使用的是MatlabR2021b版本，在仿真中，根据控制效果调整参数，在特定速度90km/h下，由粒子群算法优化该特定速度下的增益参数k，得到优化前后的前轮转角图，如图4所示，图4为增益参数优化前后的前轮转角对比图，通过对比得出结论：增益参数k优化后的结果平稳性更强。

在特定车速90km/h下，过程①中，在航向角=17°时，m _a=1,此时只有Stanley控制器起作用，当m _a=0.82、m _b=0.18时，横向偏差振荡微小，轨迹跟踪效果最佳；过程②中，在航向角/>=21°时，m _a=1，此时LQR控制器起主要作用，且当m _a=0.27、m _b=0.73时，到达目标轨迹的偏差最小，轨迹跟踪效果最佳。

进一步的，如图5所示，图5为轨迹跟踪效果对比示意图，通过仿真验证，发现传统Stanley控制算法所得轨迹与目标轨迹的横向位移为0.032米，而本发明所得轨迹与目标轨迹的横向位移仅0.0086米，通过对比得到结论：本发明的整体轨迹跟踪效果相比于传统Stanley控制算法的跟踪效果更佳，跟踪精度更高。

综上，根据上述的车辆轨迹跟踪方法，通过获取的车辆当前位置坐标及其他参数信息，进行横向偏差与航向偏差的计算，根据Stanley控制算法，推导得到车辆的前轮转角，并通过粒子群算法对特定车速下的增益参数进行优化，同时建立基于消除航向偏差的LQR控制算法，计算由航向偏差引起的前轮转角，再建立一种基于权重占比的Stanley-LQR策略，能够根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比，得到车辆的方向盘转角，对车辆进行横向控制，同时采用PID控制器对车辆进行纵向控制，实现轨迹跟踪，本发明提高了在变道工况下车辆行驶的横向稳定性，轨迹跟踪效果更加平稳，根据实际工况进行权重分配，使车辆横向控制相对平滑，鲁棒性增强，收敛速度加快。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、 “示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (7)

1.一种车辆轨迹跟踪方法，其特征在于，包括以下步骤：

步骤1，获取车辆当前位置坐标、当前时刻的航向角、当前时刻的质心侧偏角、当前时刻的速度、当前时刻的横摆角速度，并根据目标轨迹路径获取跟踪路径轨迹点坐标和每个轨迹点的航向角；

步骤2，基于车辆当前位置坐标、当前时刻的航向角及根据目标轨迹路径获取的跟踪路径轨迹点坐标和每个轨迹点的航向角，计算前轮中心的横向偏差与航向偏差，并通过Stanley控制算法计算出车辆前轮转角，其中，对于Stanley控制算法中的增益参数，以增益参数的差值平方和最小作为搜索求解的目标函数，通过粒子群算法对增益参数进行优化调整；

步骤3，基于当前时刻的航向角、每个轨迹点的航向角、当前时刻的速度、当前时刻的横摆角速度，建立基于消除航向角偏差的线性二次型调节器控制算法，计算由航向偏差引起的前轮转角；

步骤4：基于步骤2得到的车辆前轮转角和步骤3得到的由航向偏差引起的前轮转角，针对车辆变道场景，建立基于权重占比的控制策略，该控制策略以航向角为判别依据，根据不同车速与变道所处阶段的不同行驶需求，实时调整权重占比，得到最优前轮转角；

步骤5：基于最优前轮转角获取车辆当前时刻的方向盘转角，对车辆进行横向控制，同时基于当前时刻的速度，采用PID控制器对车辆进行纵向控制，从而对车辆进行轨迹跟踪。

2.根据权利要求1所述的车辆轨迹跟踪方法，其特征在于，步骤2中，车辆前轮转角的表达式为：

其中，表示车辆前轮转角，/>为航向偏差，e(t)为横向偏差，k为增益参数，v(t)为当前时刻的速度。

3.根据权利要求2所述的车辆轨迹跟踪方法，其特征在于，以增益参数的差值平方和最小作为搜索求解的目标函数的步骤中，满足以下条件式：

其中，k _e为目标增益参数，为期望转角，/>为期望质心侧偏角，l为车辆轴距，F为稳定性因数，a为车辆质心到车辆前轴的距离，b为车辆质心到车辆后轴的距离，m为车辆的整车质量，C _f、C _r分别为车辆前后轮侧偏刚度。

4.根据权利要求1所述的车辆轨迹跟踪方法，其特征在于，通过粒子群算法对增益参数进行优化调整的步骤中，满足以下条件式：

其中，h为参数信息的实际方差，为优化参数差值序列，T表示转置操作，/>为优化参数真实值，W _i为权重系数，n为迭代次数，N为大于0的自然数，为实际参数差值序列。

5.根据权利要求3所述的车辆轨迹跟踪方法，其特征在于，步骤3具体包括：

步骤301，建立基于跟踪误差的动力学模型；

步骤302，通过动力学模型推导得到基于航向偏差的状态空间方程；

步骤303，通过基于航向偏差的状态空间方程设定控制性能目标函数；

步骤304，基于控制性能目标函数提高加权矩阵中航向偏差的权重占比，得到由航向偏差引起的前轮转角。

6.根据权利要求5所述的车辆轨迹跟踪方法，其特征在于，由航向偏差引起的前轮转角通过下式计算得到：

其中，J表示控制性能目标函数，t表示t时刻，为状态变量，/>表示由航向偏差引起的前轮转角，Q、R为加权矩阵，T表示转置操作。

7.根据权利要求6所述的车辆轨迹跟踪方法，其特征在于，步骤4中，最优前轮转角的表达式为：

其中，表示最优前轮转角，m _a表示最优前轮转角的权重因子，m _b表示由航向偏差引起的前轮转角的权重因子，且m _a+m _b=1。