CN115390452A - 一种lqr横向控制器参数在线自适应方法及系统 - Google Patents

Abstract

本发明公开了一种LQR横向控制器参数在线自适应方法及系统，其包括：Step1，设置将会影响控制器的控制效果的条件；Step2，将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化；Step3，将优化后的LQR横向控制器参数作为训练神经网络的数据集；Step4，将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

Description

一种LQR横向控制器参数在线自适应方法及系统

技术领域

本发明涉及自动驾驶汽车控制技术领域，特别是关于一种基于神经网络和遗传算法的LQR(Linear Quadratic Regulator)横向控制器参数在线自适应方法。

背景技术

横向控制系统作为自动驾驶汽车的关键功能，是为了提高驾驶的舒适性和安全性。为了精确跟踪规划轨迹，横向控制系统应该对风阻、滚动摩擦和道路曲率的不确定性具有鲁棒性。LQR控制算法是自动驾驶汽车控制设计分析中较为常用的方法。在LQR算法中，权重矩阵Q和R的选择将直接影响车辆的行驶轨迹，进而影响车辆的燃油经济性和乘客的舒适度。如何确定Q和R的最优形式和取值，以获得整体最优控制仍是一个难题。LQR理论是现代控制理论中最早、最成熟的状态空间设计方法。LQR可以得到状态线性反馈的最优控制律，容易形成闭环最优控制。LQR优化设计是指状态反馈控制器K设计成最小化二次目标函数J，而K是由权重矩阵Q和R唯一确定的，所以Q和R的选择尤为重要。

然而，传统的传统LQR控制中的权重值Q和R是根据工程经验确定的，此方法非常耗时、随机性大且很依赖调试者的经验，因此，无法获得更好的控制性能。近年来，随着传统控制技术与现代计算机技术的结合，涌现出了一系列全新的组合式控制算法，可基于多种算法，如蚁群算法、遗传算法、神经网络、人工鱼群算法、退火算法、免疫算法和粒子群算法等优化算法，用于LQR横向控制器参数的优化。因为上述优化算法的计算量偏大且所需计算时间较长，而自动驾驶汽车对算法的实时性要求较高，所以很难直接将上述方法用于实车。

发明内容

本发明的目的在于提供一种基于神经网络和遗传算法的LQR横向控制器参数在线自适应方法，来解决自动驾驶汽车在不同工况下LQR横向控制器参数选择的技术问题。

为实现上述目的，本发明提供一种LQR横向控制器参数在线自适应方法，其包括：

Step1，设置将会影响控制器的控制效果的条件；

Step2，将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化；

Step3，将优化后的LQR横向控制器参数作为训练神经网络的数据集；

Step4，将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

进一步地，Step2具体包括：

Step21，对种群中待优化的LQR横向控制器参数进行解码，并将其传送至LQR控制器中；

Step22，在当前工况下，以由Step21解码后的LQR横向控制器参数进行控制，并得到该工况下的控制误差；

Step23，归一化处理步骤Step22获得的控制误差，并计算当前种群中当前个体的适应度函数，判断是否收敛于最优解或达到最大迭代次数，若是，则输出最优的LQR横向控制器参数；若否，则执行Step24；

Step24，再次对由Step21解码后的LQR横向控制器参数进行编码，并执行选择、交叉和变异操作，产生新种群并执行Step21。

进一步地，控制误差包括横向误差e_y和航向误差e_φ，Step23中的当前种群中当前个体的适应度函数表示为式(6)：

式中，f_i为种群中第i个个体的适应度函数，E_i为种群中第i个个体的评价函数，其表示为式(3)；

式中，e_y，i为种群中第i个个体作为LQR控制器参数时的横向控制误差，e_φ，i为种群中第i个个体作为LQR控制器参数时的航向控制误差，w₁+w₂＝1，且w₁、w₂∈(0,1)，w₁、w₂分别为横向误差e_y和航向误差e_φ的权重，n为种群中个体的数量。

进一步地，步骤step25中的交叉操作、变异操作分别通过计算如下式(1)、(2)实现：

式中，P_c为自适应遗传算法下个体的交叉概率，P_m为自适应遗传算法下个体的变异概率，P_c1和P_c2分别为交叉算子的上限值和下限值，f_avg和f_max分别为种群的平均适应度值和最大适应度值，f_c为种群需要交叉个体的适应度值，P_m1和P_m2分别为变异算子的上限值和下限值，f_m为种群需要变异个体的适应度值。

进一步地，横向控制误差e_y，i、航向控制误差e_φ，i分别通过下式计算获得：

e_y，i＝(y_vehicle-y_ref)cos(φ_ref)-(x_vehicle-x_ref)sin(φ_ref) (4)

e_φ，i＝φ_vehicle-φ_ref (5)

式中，y_vehicle是当前时刻车辆的纵坐标，y_ref表示参考路径中与当前时刻车辆距离最近的点的纵坐标，x_vehicle是当前时刻车辆的横坐标，x_ref表示参考路径中与当前时刻车辆距离最近的点的横坐标，φ_vehicle是当前时刻车辆的航向角，φ_ref表示参考路径中与当前时刻车辆距离最近的点的航向角。

本发明还提供一种LQR横向控制器参数在线自适应系统，其包括：

条件设置单元，其用于设置将会影响控制器的控制效果的条件；

参数优化单元，其用于将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化；

神经网络训练单元，其用于将优化后的LQR横向控制器参数作为训练神经网络的数据集；

参数自适应单元，其用于将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

进一步地，所述参数优化单元具体包括：

参数解码子单元，其用于对种群中待优化的LQR横向控制器参数进行解码，并将其传送至LQR控制器中；

控制误差获取子单元，其用于在当前工况下，以由参数解码子单元解码后的LQR横向控制器参数进行控制，并得到该工况下的控制误差；

收敛结果判断子单元，其用于归一化处理控制误差获取子单元获得的控制误差，并计算当前种群中当前个体的适应度函数，判断是否收敛于最优解或达到最大迭代次数，若是，则输出最优的LQR横向控制器参数；

种群生成子单元，其用于在收敛结果判断子单元判断的结果为否的情形下，再次对由参数解码子单元解码后的LQR横向控制器参数进行编码，并执行选择、交叉和变异操作，产生新种群并由参数解码子单元继续工作。

进一步地，控制误差包括横向误差e_y和航向误差e_φ，Step23中的当前种群中当前个体的适应度函数表示为式(6)：

式中，f_i为种群中第i个个体的适应度函数，E_i为种群中第i个个体的评价函数，其表示为式(3)；

式中，e_y，i为种群中第i个个体作为LQR控制器参数时的横向控制误差，e_φ，i为种群中第i个个体作为LQR控制器参数时的航向控制误差，w₁+w₂＝1，且w₁、w₂∈(0,1)，w₁、w₂分别为横向误差e_y和航向误差e_φ的权重，n为种群中个体的数量。

进一步地，种群生成子单元中的交叉操作、变异操作分别通过计算如下式(1)、(2)实现：

式中，P_c为自适应遗传算法下个体的交叉概率，P_m为自适应遗传算法下个体的变异概率，P_c1和P_c2分别为交叉算子的上限值和下限值，f_avg和f_max分别为种群的平均适应度值和最大适应度值，f_c为种群需要交叉个体的适应度值，P_m1和P_m2分别为变异算子的上限值和下限值，f_m为种群需要变异个体的适应度值。

进一步地，横向控制误差e_y，i、航向控制误差e_φ，i分别通过下式计算获得：

e_y，i＝(y_vehicle-y_ref)cos(φ_ref)-(x_vehicle-x_ref)sin(φ_ref) (4)

e_φ，i＝φ_vehicle-φ_ref (5)

式中，y_vehicle是当前时刻车辆的纵坐标，y_ref表示参考路径中与当前时刻车辆距离最近的点的纵坐标，x_vehicle是当前时刻车辆的横坐标，x_ref表示参考路径中与当前时刻车辆距离最近的点的横坐标，φ_vehicle是当前时刻车辆的航向角，φ_ref表示参考路径中与当前时刻车辆距离最近的点的航向角。

本发明由于采取以上技术方案，其具有以下优点：

本发明将离线优化的结果作为数据集，并用神经网络学习优化后的非线性参数，与现有的LQR横向控制器参数选择方法相比计算量相差不大，但在任意工况下的权重选择更为准确，效率更高。

附图说明

图1为本发明实施例中使用的改进遗传算法的流程图。

图2为本发明实施例利用图1中的改进遗传算法对LQR横向控制器参数进行离线优化的示意图。

图3为本发明实施例用于学习最优LQR横向控制器参数的神经网络的示意图。

图4为本发明实施例的图3的神经网络的LQR横向控制器参数自适应方法。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

本发明实施例提供的LQR横向控制器参数在线自适应方法包括：

Step1，设置将会影响控制器的控制效果的条件。其中，会影响控制器的控制效果的参数包括但不限于车速范围、路面附着系数范围和道路曲率范围。

Step2，将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化。

Step3，将优化后的LQR横向控制器参数作为训练神经网络的数据集；

Step4，将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

本实施例针对自动驾驶汽车常见的运行工况，考虑道路摩擦系数、车速、道路曲率等因素，针对单一或复合工况将归一化后的横向误差、航向角误差等性能指标作为适应度函数，采用改进遗传算法对不同工况下的控制参数进行离线优化，进而得到最佳LQR横向控制器参数的数据集；使用神经网络来学习最佳LQR横向控制器参数，以满足在线自适应要求。

如图2所示，Step2是利用如图1所示的改进遗传算法对LQR横向控制器参数进行离线优化，具体包括：

Step21，对种群中待优化的LQR横向控制器参数进行解码，并将其传送至LQR控制器中。

Step22，在当前工况下，以由Step21解码后的LQR横向控制器参数进行控制，并得到该工况下的控制误差。其中，控制误差可以包括横向误差e_y和航向误差e_φ。

Step23，归一化处理步骤Step22获得的控制误差，并计算当前种群中当前个体的适应度函数，判断是否收敛于最优解或达到最大迭代次数，若是，则输出最优的LQR横向控制器参数；若否，则执行Step24。其中，当代种群的适应度值与上一代种群的适应度值相等或差别很小时，可认为收敛于最优解。最大迭代次数在step21中初始化遗传算法时设定。

Step24，再次对由Step21解码后的LQR横向控制器参数进行编码，并执行选择、交叉和变异操作，产生新种群并执行Step21。

遗传算法的实现主要是通过遗传算子，遗传算子包括：选择算子、交叉算子、变异算子3种。其中，选择算子通过选择使最优的个体进入下一代，交叉算子P_c和变异算子P_m是通过当代个体交叉和变异形成新的个体。交叉操作是产生新个体的主要操作，决定了遗传算法的全局搜索能力。变异操作是产生新个体的辅助性操作，决定遗传算法的局部搜索能力。本实施例是在一般遗传算法的交叉、变异过程中采用自适应的交叉算子和变异算子，其主要思想是：对于适应度高于群体平均适应度的个体，选择较低的交叉算子P_c和变异算子P_m，使个体被破坏的可能性尽可能减小；对于适应度低于种群平均值的个体，选择较高的交叉算子P_c和变异算子P_m，淘汰掉较差的个体产生新的个体，保证了算法的收敛性。

步骤step25中的交叉操作通过计算如下式(1)表示的线性自适应遗传算法中交叉概率实现，变异操作通过计算如下式(2)表示的线性自适应遗传算法中变异概率实现：

式中，P_c和P_m为自适应遗传算法下个体的交叉概率和变异概率；f_avg表示种群的平均适应度值，

f_max表示种群的最大适应度值，f_max＝max(f_i)；P_c1和P_c2分别表示交叉算子的上限值和下限值，交叉算子太大会导致高适应度的结构很快被破坏掉，太小搜索会停滞不前，一般取0.25至0.8，可以取P_C1＝0.8，P_C2＝0.5；P_m1和P_m2分别表示变异算子的上限值和下限值；f_c和f_m分别表示种群需要交叉和变异个体的适应度值，变异概率是增大种群多样性的第二个因素，起到保持和恢复染色体多样性的作用，太小会产生新的基因快，太大会使遗传算法变成随机搜索，一般取0.01至0.2，可以取P_m1＝0.1；P_m2＝0.01。

为避免跟踪的状态量e_y和e_φ偏差较大，影响跟踪效果和行驶安全性，考虑到横向误差e_y和航向误差e_φ数量级不同，会导致目标评价函数中数量级较高的项的占比过高，进而影响遗传算法对LQR控制器待优化参数的优化，因此本发明实施例将其进行归一化处理，最终将目标评价函数定义为：

式中，E_i为种群中第i个个体的评价函数，w₁、w₂为横向误差e_y和航向误差e_φ的权重，w₁、w₂∈(0,1)且w₁+w₂＝1；下标i表示种群中第i个个体，n表示种群中个体的数量，n是种群数目,在设置遗传算法参数时设置,种群数目影响算法的有效性，n太小，遗传算法会因为采样点太少而找不出问题的解；n太大，会增加计算量，使收敛时间增长，一般设置在30至160之间比较合适；e_y，i为种群中第i个个体作为LQR控制器参数时的横向控制误差，如下式(4)所示，e_φ，i为种群中第i个个体作为LQR控制器参数时的航向控制误差，如下式(5)所示。

e_y，i＝(y_vehicle-y_ref)cos(φ_ref)-(x_vehicle-x_ref)sin(φ_ref) (4)

e_φ，i＝φ_vehicle-φ_ref (5)

式中，y_vehicle是当前时刻车辆的纵坐标，y_ref表示参考路径中与当前时刻车辆距离最近的点的纵坐标，x_vehicle是当前时刻车辆的横坐标，x_ref表示参考路径中与当前时刻车辆距离最近的点的横坐标，φ_vehicle是当前时刻车辆的航向角，φ_ref表示参考路径中与当前时刻车辆距离最近的点的航向角。

上述实施例中，本发明中使用遗传算法进行控制器参数离线优化，在优化参数的时候也可以采用其他的优化方法，如粒子群算法、退火算法、蚁群算法等。

种群中个体的目标评价函数E_i越小控制效果越好；而种群中个体的适应度值f_i越大越好，因此需要对评价函数取倒数得到该个体的适应度函数，即上述种群中第i个个体的适应度函数可以表示为下式(6)：

当然，式(6)也可以替换成其他的f_i与E_i呈反比例关系的形式。

在上述离线优化阶段完成之后，生成最佳LQR横向控制器参数的数据集。为了在线适应，使用神经网络来学习最佳的LQR横向控制器参数。所提出的神经网络由四层组成；第一层包含相同的四个输入，第一个隐藏层由16个神经元组成，第二个隐藏层包含8个隐藏神经元，输出层对应于LQR的两个参数：Q和R，如图3所示。由于这是一个正值的回归问题，所以隐藏层采用Sigmoid激活函数，输出层采用Relu函数。

神经网络中的前向传递由下式(7)至式(9)表示：

式中，f和g分别是Sigmoid函数和Relu函数；H_j ¹，H_j ²表示分别表示第一个隐藏层和第二个隐藏层中第j个神经元的输出；O_j表示输出层的输出；

表示输入层第i个神经元和第一个隐藏层第j个神经元的权重；

表示第一个隐藏层第i个神经元和第二个隐藏层第j个神经元的权重；

表示第二个隐藏层第i个神经元和输出层第j个神经元的权重。

将上述学习完毕的神经网络用于LQR控制器的参数自适应问题，如图4所示，其主要包括两个模块，神经网络模块主要根据当前测试车辆的行驶条件在线输出LQR控制器控制参数，而LQR控制器模块则根据当前控制参数出计算出期望前轮转角控制量。

本发明方法针对自动驾驶汽车常见的运行工况，考虑道路摩擦系数、车速、道路曲率等因素，针对单一或复合工况将归一化后的横向误差、航向角误差等性能指标作为适应度函数，采用改进遗传算法对不同工况下的控制参数进行离线优化，进而得到最佳LQR横向控制器参数的数据集；使用神经网络来学习最佳LQR横向控制器参数，已满足在任意工况下控制参数自适应的需求，再使用同等计算量的情况下，使选择的权重更加准确，达到更优的控制效果。

本发明实施例还提供一种LQR横向控制器参数在线自适应系统，其包括条件设置单元、参数优化单元、神经网络训练单元和参数自适应单元，其中：

条件设置单元用于设置将会影响控制器的控制效果的条件。

参数优化单元用于将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化。

神经网络训练单元用于将优化后的LQR横向控制器参数作为训练神经网络的数据集。

参数自适应单元用于将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

在一个实施例中，所述参数优化单元具体包括参数解码子单元、控制误差获取子单元、收敛结果判断子单元和种群生成子单元，其中：

参数解码子单元用于对种群中待优化的LQR横向控制器参数进行解码，并将其传送至LQR控制器中。

控制误差获取子单元用于在当前工况下，以由参数解码子单元解码后的LQR横向控制器参数进行控制，并得到该工况下的控制误差。

收敛结果判断子单元用于归一化处理控制误差获取子单元获得的控制误差，并计算当前种群中当前个体的适应度函数，判断是否收敛于最优解或达到最大迭代次数，若是，则输出最优的LQR横向控制器参数。

种群生成子单元用于在收敛结果判断子单元判断的结果为否的情形下，再次对由参数解码子单元解码后的LQR横向控制器参数进行编码，并执行选择、交叉和变异操作，产生新种群并由参数解码子单元继续工作。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种LQR横向控制器参数在线自适应方法，其特征在于，包括：

Step1，设置将会影响控制器的控制效果的条件；

Step2，将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化；

Step3，将优化后的LQR横向控制器参数作为训练神经网络的数据集；

Step4，将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

2.如权利要求1所述的LQR横向控制器参数在线自适应方法，其特征在于，Step2具体包括：

Step21，对种群中待优化的LQR横向控制器参数进行解码，并将其传送至LQR控制器中；

Step22，在当前工况下，以由Step21解码后的LQR横向控制器参数进行控制，并得到该工况下的控制误差；

Step23，归一化处理步骤Step22获得的控制误差，并计算当前种群中当前个体的适应度函数，判断是否收敛于最优解或达到最大迭代次数，若是，则输出最优的LQR横向控制器参数；若否，则执行Step24；

Step24，再次对由Step21解码后的LQR横向控制器参数进行编码，并执行选择、交叉和变异操作，产生新种群并执行Step21。

3.如权利要求2所述的LQR横向控制器参数在线自适应方法，其特征在于，控制误差包括横向误差e_y和航向误差e_φ，Step23中的当前种群中当前个体的适应度函数表示为式(6)：

式中，f_i为种群中第i个个体的适应度函数，E_i为种群中第i个个体的评价函数，其表示为式(3)；

式中，e_y，i为种群中第i个个体作为LQR控制器参数时的横向控制误差，e_φ，i为种群中第i个个体作为LQR控制器参数时的航向控制误差，w₁+w₂＝1且w₁、w₂∈(0，1)，w₁、w₂分别为横向误差e_y和航向误差e_φ的权重，n为种群中个体的数量。

4.如权利要求2或3所述的LQR横向控制器参数在线自适应方法，其特征在于，步骤step25中的交叉操作、变异操作分别通过计算如下式(1)、(2)实现：

式中，P_c为自适应遗传算法下个体的交叉概率，P_m为自适应遗传算法下个体的变异概率，P_c1和P_c2分别为交叉算子的上限值和下限值，f_avg和f_max分别为种群的平均适应度值和最大适应度值，f_c为种群需要交叉个体的适应度值，P_m1和P_m2分别为变异算子的上限值和下限值，f_m为种群需要变异个体的适应度值。

5.如权利要求3所述的LQR横向控制器参数在线自适应方法，其特征在于，横向控制误差e_y，i、航向控制误差e_φ，i分别通过下式计算获得：

e_y，i＝(y_vehicle-y_ref)cos(φ_ref)-(x_vehicle-x_ref)sin(φ_ref) (4)

e_φ，i＝φ_vehicle-φ_ref (5)

式中，y_vehicle是当前时刻车辆的纵坐标，y_ref表示参考路径中与当前时刻车辆距离最近的点的纵坐标，x_vehicle是当前时刻车辆的横坐标，x_ref表示参考路径中与当前时刻车辆距离最近的点的横坐标，φ_vehicle是当前时刻车辆的航向角，φ_ref表示参考路径中与当前时刻车辆距离最近的点的航向角。

6.一种LQR横向控制器参数在线自适应系统，其特征在于，包括：

条件设置单元，其用于设置将会影响控制器的控制效果的条件；

参数优化单元，其用于将归一化后的控制误差作为评价函数，采用改进遗传算法对该条件下的LQR横向控制器参数进行优化；

神经网络训练单元，其用于将优化后的LQR横向控制器参数作为训练神经网络的数据集；

参数自适应单元，其用于将训练后的神经网络用于LQR控制器中，实现不同工况下的LQR横向控制器参数在线自适应。

7.如权利要求6所述的LQR横向控制器参数在线自适应系统，其特征在于，所述参数优化单元具体包括：

参数解码子单元，其用于对种群中待优化的LQR横向控制器参数进行解码，并将其传送至LQR控制器中；

控制误差获取子单元，其用于在当前工况下，以由参数解码子单元解码后的LQR横向控制器参数进行控制，并得到该工况下的控制误差；

收敛结果判断子单元，其用于归一化处理控制误差获取子单元获得的控制误差，并计算当前种群中当前个体的适应度函数，判断是否收敛于最优解或达到最大迭代次数，若是，则输出最优的LQR横向控制器参数；

种群生成子单元，其用于在收敛结果判断子单元判断的结果为否的情形下，再次对由参数解码子单元解码后的LQR横向控制器参数进行编码，并执行选择、交叉和变异操作，产生新种群并由参数解码子单元继续工作。

8.如权利要求7所述的LQR横向控制器参数在线自适应系统，其特征在于，控制误差包括横向误差e_y和航向误差e_φ，Step23中的当前种群中当前个体的适应度函数表示为式(6)：

式中，f_i为种群中第i个个体的适应度函数，E_i为种群中第i个个体的评价函数，其表示为式(3)；

式中，e_y，i为种群中第i个个体作为LQR控制器参数时的横向控制误差，e_φ，i为种群中第i个个体作为LQR控制器参数时的航向控制误差，w₁+w₂＝1，且w₁、w₂∈(0,1)，w₁、w₂分别为横向误差e_y和航向误差e_φ的权重，n为种群中个体的数量。

9.如权利要求7或8所述的LQR横向控制器参数在线自适应系统，其特征在于，种群生成子单元中的交叉操作、变异操作分别通过计算如下式(1)、(2)实现：

式中，P_c为自适应遗传算法下个体的交叉概率，P_m为自适应遗传算法下个体的变异概率，P_c1和P_c2分别为交叉算子的上限值和下限值，f_avg和f_max分别为种群的平均适应度值和最大适应度值，f_c为种群需要交叉个体的适应度值，P_m1和P_m2分别为变异算子的上限值和下限值，f_m为种群需要变异个体的适应度值。

10.如权利要求7所述的LQR横向控制器参数在线自适应系统，其特征在于，横向控制误差e_y，i、航向控制误差e_φ，i分别通过下式计算获得：

e_y，i＝(y_vehicle-y_ref)cos(φ_ref)-(x_vehicle-x_ref)sin(φ_ref) (4)

e_φ，i＝φ_vehicle-φ_ref (5)

式中，y_vehicle是当前时刻车辆的纵坐标，y_ref表示参考路径中与当前时刻车辆距离最近的点的纵坐标，x_vehicle是当前时刻车辆的横坐标，x_ref表示参考路径中与当前时刻车辆距离最近的点的横坐标，φ_vehicle是当前时刻车辆的航向角，φ_ref表示参考路径中与当前时刻车辆距离最近的点的航向角。

Images