CN114384915A - 基于lqr的改进路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种基于LQR的改进路径跟踪控制方法，基于无人驾驶车辆当前实际位置与规划位置的横向偏差值和航向偏差值，通过模糊控制器对LQR控制器原始加权矩阵中Q的元素进行实时修正优化，以得到优化后的加权矩阵Q，通过对LQR问题进行求解得出车辆前轮转角的控制量。本发明至少包括以下有益效果：为了解决实际应用中无人驾驶车辆路径跟踪的精度不足的问题，本发明在现有的LQR路径跟踪算法中加入一个模糊控制，动态调节LQR路径跟踪控制器中加权矩阵Q中的元素，然后再使用调整后的加权矩阵Q计算LQR控制器的最优解，计算出无人驾驶车辆需要的前轮转角，以达到增加无人驾驶车辆路径跟踪精度的效果。

Description

基于LQR的改进路径跟踪控制方法

技术领域

本发明涉及无人驾驶路径跟踪控制领域，更具体地说，本发明涉及一种基于LQR的改进路径跟踪控制方法。

背景技术

无人驾驶领域中的路径跟踪是指控制无人驾驶车辆的方向盘、油门和刹车踏板等机构，使得无人驾驶车辆能够按照规划出的路径行驶。现有的路径跟踪通常使用的是LQR路径跟踪控制算法，其优点在于：(1)计算简单，能快速求解出最优解；(2)参数较少，能快速调试出合适的参数，但也存在以下不足：(1)曲率快速变化时会发生超调，并且由于算法本身的原因会存在稳态误差；(2)LQR路径跟踪控制器中的加权矩阵Q中的元素使用的是固定值，无法改变，限制了路径跟踪控制器的性能。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

为了实现根据本发明的这些目的和其它优点，提供了一种基于LQR的改进路径跟踪控制方法，基于无人驾驶车辆当前实际位置与规划位置的横向偏差值和航向偏差值，通过模糊控制器对LQR控制器对原始加权矩阵中Q`的元素进行实时修正优化，以得到优化后的加权矩阵Q，通过对LQR问题进行求解得出车辆前轮转角的控制量。

优选的是，所述横向偏差值和航向偏差值的获取方式被配置为包括：

步骤一：LQR控制器接收规划层传输的路径信息，包括路径的位置信息和路径的航向信息；

步骤二：LQR控制器通过接收到的GPS信号获得无人接驳车的实际位置，以找出当前车辆位置与规划路径最近的点，并计算出车辆与最近点之间的横向偏差和航向偏差。

优选的是，所述模糊控制器的控制流程被配置为包括：

步骤三：将步骤三中得到的横向偏差值和航向偏差值送入模糊控制器中，对原有加权矩阵Q`中对应元素增量进行计算，以得到优化后的加权矩阵Q；

步骤四：根据车辆的模型参数和加权矩阵Q、加权矩阵R对LQR问题进行求解，求解出增益矩阵K，进而通过如下的公式一得到车辆前轮转角的控制量u：

u＝K*Err；

其中，Err为控制系统的误差状态向量；

步骤五：通过串口将求解出的控制量u下发给车辆底层执行，通过转动方向盘实现路径跟踪。

优选的是，在步骤三-四中，所述加权矩阵Q和加权矩阵R被配置为：

Q＝diag[q1，q2，q3，q4]，R＝[r]；

其中，q1，q2，q3，q4表示对横向偏差ed、横向偏差变化率

航向偏差

和航向偏差变化率

的重视程度，加权矩阵R表示对系统控制量的限制，且在加权矩阵Q中将q1，q3设定为实时变量，q2，q4设定为固定值，在加权矩阵R中设定元素r为固定值。

优选的是，在步骤三中，对原有加权矩阵Q`中对应元素增量进行计算的方式，是通过在模糊控制器内制定对加权矩阵Q中q1，q3进行修正的控制策略得以实现，所述控制策略的制定方式包括：

S40，将步骤二中实时计算得到的横向偏差和航向偏差进行处理后作为模糊控制器的输入；

S41，利用调节因子Δq1、Δq3与横向偏差值、航向偏差值呈正相关的特性，对加权矩阵Q中的q1，q3进行实时修正，在模糊控制器中实现加权矩阵Q系数的自适应。

优选的是，所述横向偏差值和航向偏差值的处理公式为：

其中，

表示经处理后的横向偏差值和航向偏差值，

的论域设置为[0，1]，ed为车辆与道路最近点的横向偏差，

为车辆与道路最近点的航向偏差，e_{d max}、e_{d min}为横向偏差的最大值、最小值；

航向偏差的最大值、最小值。

优选的是，利用调节因子Δq1、Δq3对q1，q3进行修正的公式配置为

q1＝q₁₀*3^Δq1；

q3＝q₃₀*2^Δq3；

其中，q10和q30为原LQR控制器中加权矩阵Q`的元素，q1和q3为经过模糊控制优化后加权矩阵Q中的元素，Δq1、Δq3的论域设置为[-2，2]。

本发明至少包括以下有益效果：本发明为了保证无人驾驶车辆路径跟踪的准确性，加入模糊控制对LQR算法进行优化，使得无人驾驶车辆能更加准确的沿着规划路径行驶。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明的路径跟踪控制方法流程图；

图2为本发明实际路径跟踪效果图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

本发明的整体设计思路是：该发明在主流的路径跟踪控制方法-LQR控制算法的基础上进行改进，在传统的LQR控制算法加权矩阵Q为固定值无法改变，在横向偏差和航向偏差较大时加权矩阵Q的元素还是固定值，限制了LQR控制器的性能，导致了无人接驳车在跟踪规划路径时产生较大的偏差，尤其是在转弯时偏差更大，容易与行人或者过往的车辆发生碰撞，造成交通事故。本发明在加入模糊控制器动态调整LQR控制器中加权矩阵Q的元素后，能够有效的提高无人接驳车路径跟踪的精度，在无人接驳车与规划路径偏差较大时能快速的调整车辆，使其准确的跟踪目标路径。

如图1，本发明的实施步骤为：

步骤二：接收规划层传输的路径信息，包括路径的位置信息和路径的航向信息。

步骤三：通过接收GPS信号获得无人接驳车实际位置，找出当前车辆位置与规划路径最近的点，并计算出车辆与最近点之间的横向偏差和航向偏差等。

步骤四：将计算出的横向偏差和航向偏差等送入模糊控制中进行优化，得出加权矩阵Q中的元素的增量，计算出最终的加权矩阵Q。

步骤五：根据车辆的模型参数和加权矩阵Q、R对LQR问题进行求解，求解出增益矩阵K，最终的控制量也即前轮转角u＝K*Err；

其中，Err是控制系统的误差状态向量；

各个元素分别代表车与规划路径上最近点之间的横向偏差ed、横向偏差变化率

航向偏差

和航向偏差变化率

步骤六：通过串口将求解出的控制量下发给车辆底层执行，转动方向盘实现路径跟踪。

具体来说，本发明在利用无人驾驶车辆实际位置与规划位置的横向偏差和航向偏差值，优化求解出LQR控制器加权矩阵Q中的元素，使得车辆在横向偏差和航向偏差较大时能快速调整车的姿态，减小跟踪的偏差。

本发明为了解决在实际中接驳车跟踪精度不足的问题，采用模糊控制对LQR算法进行优化，LQR控制器中的加权矩阵Q和R为：

Q＝diag[q1，q2，q3，q4]，R＝[r]；

其中q1，q2，q3，q4分别表示对横向偏差ed，横向偏差变化率

航向偏差

和航向偏差变化率

的重视程度，Q矩阵中对应的元素越大表示对该目标的重视程度越大，即让该目标快速减小。在路径跟踪控制的过程中，横向偏差ed和航向偏差

是主要的控制目标，因此只针对q1和q3进行调整，设置q2和q4为固定值；

加权矩阵R表示对系统控制量的限制，其值越大对控制量的输出限制越大，而LQR控制器中控制量只有一个前轮转角，因此设置加权矩阵R中的元素r为固定值。

模糊控制算法能够对复杂的系统进行控制，而且对模型的准确性要求不高，只需要根据现有知识和规则便可以进行优化求解。因此采用模糊控制对LQR控制器的加权矩阵Q中的q1和q3元素进行优化。在优化的过程中设置q2、q4和r为固定值，通过调节q1和q3获得最佳的加权矩阵Q。由于横向偏差和航向偏差是路径跟踪的主要影响因素，所以将横向偏差和航向偏差作为模糊控制器的输入，q1和q3的调节因子Δq1、Δq3作为模糊控制器的输出，设计加权矩阵系数自适应模糊控制器。

首先将车辆的横向偏差和航向偏差进行处理：

其中，

表示经处理后的横向偏差值和航向偏差值，其论域设置为[0，1]，ed为车辆与道路最近点的横向偏差，

为车辆与道路最近点的航向偏差，e_{d max}、e_{d min}为横向偏差的最大值、最小值，在应用时e_{d max}的值设定为1.5m，e_{d min}的值设定为0m；

航向偏差的最大值、最小值，在应用时

的值设定为0.2rad，

的值设定为0rad。

为了使车辆控制平滑，模糊控制器的输出不直接作用于LQR控制器的加权矩阵系数，而是作为LQR控制器加权矩阵Q的元素的修正量，q1和q3的修正策略为：

q1＝q₁₀*3^Δq1；

q3＝q₃₀*2^Δq3；

式中，q10和q30为原LQR控制器中加权矩阵Q`的元素，q1和q3为经过模糊控制优化后加权矩阵Q中的元素，Δq1、Δq3的论域设置为[-2，2]。

制定模糊控制器规则(策略)的原则是控制车辆的横向偏差和航向偏差尽可能的小，分析LQR控制器的能量函数J：

其中，Xk为被控制系统的状态向量，x^Tk为Xk的转置矩阵，Uk为被控制系统的输入量，U^Tk为Uk的转置矩阵。

当加权矩阵Q中的元素增大时，要让能量函数J尽可能的小，那么Xk就需要更小。即加权矩阵Q中的元素增大时，Xk中对应的状态就以更快的速度衰减到0。所以制定的模糊控制规则如下：当横向偏差或者航向偏差较大时，应该适当增加加权矩阵Q中的q1和q3元素，提高LQR控制器对横向偏差或者航向偏差的重视程度，以此来提高车辆路径跟踪的精度；当横向偏差或者航向偏差较小时，适当的减小加权矩阵Q中的q1和q3元素，减小控制器的能量消耗。

在进行加权矩阵系数的优化时，要确定模糊控制的输入和输出的范围，再将其模糊化。模糊控制器的输入，

的论域设置为[0，1]，模糊子集选择五个，模糊控制子集表示为{HS(很小)，S(小)，M(中)，L(大)，HL(很大)}，选择三角形隶属度函数；输出Δq1、Δq3的论域设置为[-2，2]，模糊子集表示为{HS(很小)，S(小)，M(中)，L(大)，HL(很大)}，选择高斯隶属度函数。基于LQR控制器的能量函数，可以得到调节因子Δq1、Δq3随

的变化的控制规则如下表1和表2所示。

表1Δq1模糊控制规则表

表2Δq3模糊控制规则表

即在

的论域[0，1]设置范围内，先行制备5个第一模糊控制子集，在Δq1、Δq3的论域[-2，2]设置范围内，也制备5个第二模糊控制子集，当

作为模糊控制器的输入数据时，基于输入值选择对应的第一模糊子集，在基于表1-2得出与第一模糊控制子集对应的第二模糊控制子集Δq1、Δq3的值，将Δq1、Δq3代入至q1和q3的修正策略公式中：

q1＝q₁₀*3^Δq1；

q3＝q₃₀*2^Δq3；

完成对加权矩阵Q中q1和q3的修正。

将本发明的路径跟踪算法应用在实际的路径跟踪中，如图2，运用在校园无人接驳车上，可以有效提高了校园无人接驳车路径跟踪的精度。

故本发明为了解决实际应用中无人驾驶车辆路径跟踪的精度不足的问题，在现有的LQR路径跟踪算法中加入一个模糊控制，动态调节LQR路径跟踪控制器中加权矩阵Q中的元素，然后再使用调整后的加权矩阵Q计算LQR控制器的最优解，计算出无人驾驶车辆需要的前轮转角，以达到增加无人驾驶车辆路径跟踪精度的效果。

以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (7)

1.一种基于LQR的改进路径跟踪控制方法，其特征在于，基于无人驾驶车辆当前实际位置与规划位置的横向偏差值和航向偏差值，通过模糊控制器对LQR控制器原始加权矩阵中Q的元素进行实时修正优化，以得到优化后的加权矩阵Q，通过对LQR问题进行求解得出车辆前轮转角的控制量。

2.如权利要求1所述的基于LQR的改进路径跟踪控制方法，其特征在于，所述横向偏差值和航向偏差值的获取方式被配置为包括：

步骤一：LQR控制器接收规划层传输的路径信息，包括路径的位置信息和路径的航向信息；

步骤二：LQR控制器通过接收到的GPS信号获得无人接驳车的实际位置，以找出当前车辆位置与规划路径最近的点，并计算出车辆与最近点之间的横向偏差和航向偏差。

3.如权利要求2所述的基于LQR的改进路径跟踪控制方法，其特征在于，所述模糊控制器的控制流程被配置为包括：

步骤三：将步骤三中得到的横向偏差值和航向偏差值送入模糊控制器中，对原有加权矩阵Q`中对应元素增量进行计算，以得到优化后的加权矩阵Q；

步骤四：根据车辆的模型参数和加权矩阵Q、加权矩阵R对LQR问题进行求解，求解出增益矩阵K，进而通过如下的公式一得到车辆前轮转角的控制量u：

u＝K*Err；

其中，Err为控制系统的误差状态向量；

步骤五：通过串口将求解出的控制量u下发给车辆底层执行，通过转动方向盘实现路径跟踪。

4.如权利要求3所述的基于LQR的改进路径跟踪控制方法，其特征在于，在步骤三-四中，所述加权矩阵Q和加权矩阵R被配置为：

Q＝diag[q1，q2，q3，q4]，R＝[r]；

其中，q1，q2，q3，q4表示对横向偏差e_d、横向偏差变化率

航向偏差

和航向偏差变化率

的重视程度，加权矩阵R表示对系统控制量的限制，且在加权矩阵Q中将q1，q3设定为实时变量，q2，q4设定为固定值，在加权矩阵R中设定元素r为固定值。

5.如权利要求4所述的基于LQR的改进路径跟踪控制方法，其特征在于，在步骤三中，对原有加权矩阵Q`中对应元素增量进行计算的方式，是通过在模糊控制器内制定对加权矩阵Q中q1，q3进行修正的控制策略得以实现，所述控制策略的制定方式包括：

S40，将步骤二中实时计算得到的横向偏差和航向偏差进行处理后作为模糊控制器的输入；

S41，利用调节因子Δq1、Δq3与横向偏差值、航向偏差值呈正相关的特性，对加权矩阵Q中的q1，q3进行实时修正，在模糊控制器中实现加权矩阵Q系数的自适应。

6.如权利要求5所述的基于LQR的改进路径跟踪控制方法，其特征在于，所述横向偏差值和航向偏差值的处理公式为：

其中，

表示经处理后的横向偏差值和航向偏差值，

的论域设置为[0，1]，e_d为车辆与道路最近点的横向偏差，

为车辆与道路最近点的航向偏差，e_{d max}、e_{d min}为横向偏差的最大值、最小值；

航向偏差的最大值、最小值。

7.如权利要求5所述的基于LQR的改进路径跟踪控制方法，其特征在于，利用调节因子Δq1、Δq3对q1，q3进行修正的公式配置为

q1＝q₁₀*3^Δq1；

q3＝q₃₀*2^Δq3；

其中，q10和q30为原LQR控制器中加权矩阵Q`的元素，q1和q3为经过模糊控制优化后加权矩阵Q中的元素，Δq1、Δq3的论域设置为[-2，2]。

Images