CN113608530A - 一种带有pid转角补偿的参数自整定lqr路径跟踪方法 - Google Patents

Abstract

本发明公开了一种带有PID转角补偿的参数自整定LQR路径跟踪方法，包括以下步骤：接收参考路径信息、车辆当前的位姿信息与运动状态信息；根据接收的信息，确定目标参考点，计算车辆与目标参考点的偏差；根据车速信息，采用基于车速的LQR控制权重调整方法确定控制权重；采用LQR控制方法计算车辆的前轮转角；采用预瞄PID方法计算补偿前轮转角；计算车辆方向盘转角控制量，传递给转向执行机构。本发明对一般LQR控制方法进行了改进，采用预瞄PID控制方法对LQR控制方法进行转角补偿，能有效的提高跟踪精度；并加入基于车速的LQR控制权重调整方法，提高控制器对于车速的适应性。

Description

一种带有PID转角补偿的参数自整定LQR路径跟踪方法

技术领域

本发明涉及自动驾驶技术领域，具体地指一种带有PID转角补偿的参数自整定LQR路径跟踪方法。

背景技术

智能车辆的路径跟踪控制是实现自动驾驶的关键技术之一，其主要功能是控制车辆的前轮转角，使车辆能够沿着既定的参考路径行驶，尽量减少受控车辆与参考路径之间的偏差，跟踪精度是路径跟踪控制的主要目标。

目前，智能车辆的路径跟踪控制方法较多，其中，PID控制方法简单有效，但存在外部扰动时，无法保证在大范围内的渐进稳定；MPC控制方法可以有效的处理系统多约束，跟踪精度最好，但由于其需要在线求解序列二次规划问题，计算速度难以满足智能车辆实时控制的要求；LQR控制方法计算速度快，实时性好，但跟踪精度次于MPC控制方法，并且受车速变化影响大。

发明内容

针对上述背景技术存在的不足之处，本发明提供一种带有PID转角补偿的参数自整定LQR路径跟踪方法，能够提高一般LQR控制方法的跟踪精度及其对不同车速的适应性。

为实现上述目的，本发明所设计的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特殊之处在于，所述方法包括以下步骤：

步骤S1：实时采集参考路径信息、车辆当前的位姿信息与运动状态信息；

步骤S2：根据采集的参考路径信息、车辆当前的位姿信息与运动状态信息确定目标参考点，计算车辆与目标参考点的偏差；

步骤S3：根据车速信息，采用基于车速的LQR控制权重调整方法确定控制权重；

步骤S4：根据步骤S2中计算的车辆与目标参考点的偏差值及步骤S3中确定的控制权重计算车辆的前轮转角；

步骤S5：采用预瞄PID方法计算补偿前轮转角；

步骤S6：计算车辆方向盘转角控制量，传递给转向执行机构。

优选地，所述步骤S1中，所述参考路径信息为离散的参考路径点集，其中参考路径点包含位置、航向角与曲率信息；所述车辆当前的位姿信息包括车辆质心位置、车辆航向角，所述运动状态信息包括车辆横摆角速度、车辆纵向速度、车辆横向速度。

优选地，所述步骤S2的具体步骤包括：

S201：将参考路径信息中的参考路径点位置与车辆位置转换为UTM坐标；

S202：确定目标参考点，所述目标参考点包含质心参考点与预瞄参考点，分别为车辆质心参考点、车辆预瞄点参考点；

S203：遍历参考路径点集，查找距离车辆质心位置最近的两个相邻路径点，利用线性插值法确定车辆实际的质心参考点；

S204：根据车辆实际的质心参考点信息与车辆质心位置，计算车辆与参考点的横向位置偏差e_y，航向偏差

横向位置偏差变化率

航向偏差变化率

S205：遍历参考路径点集，查找距离车辆预瞄点位置最近的两个相邻路径点，利用线性插值法确定车辆实际的预瞄参考点；

S206：根据车辆实际的预瞄参考点位置与车辆预瞄点位置，计算预瞄点与参考点的横向位置偏差e_pre。

优选地，所述步骤S3的具体步骤包括：

S301：建立路径跟踪误差模型，设计LQR路径跟踪控制器针对离散系统进行控制，通过求解合适的控制量U，使目标函数的值最小，目标函数中，权重矩阵Q＝diag[q1,q2,q3,q4]，q1,q2,q3,q4分别为横向位置偏差e_y，横向位置偏差变化率

航向偏差

航向偏差变化率

控制量权重矩阵R＝[r]，r为前轮转角δ_f的权重系数，

S302：根据测试实验，在保证车辆路径跟踪的精准性和行驶的舒适性、无左右摇摆现象的条件下确定速度阈值V_th的最优控制权重；

S303：根据车辆纵向速度V_x，调整权重参数q₁,r的值。

优选地，所述步骤S5中根据步骤S2计算得到的预瞄偏差e_pre，采用PID方法计算补偿前轮转角，计算公式为：

式中，k_p，k_i，k_δ分别表示比例、积分、微分常数，e_pre(k)表示k时刻的预瞄横向偏差。

优选地，所述步骤S6中，计算车辆方向盘转角控制量，传递给转向执行结构，车辆方向盘转角控制量δ的计算公式为：

δ＝i·(δ₁+δ₂)

式中，i表示车辆转向系统的传动比，δ₁为车辆跟踪路径所需的前轮转角，δ₂为前轮转角补偿前轮转角。

优选地，所述步骤S202中车辆预瞄点的计算公式如下：

式中，X_pre，Y_pre为车辆预瞄点位置，X，Y为车辆质心位置，d_pre为预瞄距离，

为车辆航向角。

优选地，所述步骤S301中LQR路径跟踪控制器目标函数的设定方法为：将路径跟踪误差模型表示为状态空间方程形式：

其中：

式中，m为整车质量；V_x分别为车辆的纵向车速；c_f，c_r分别为车辆前、后轴车轮的侧偏刚度；I_z为车辆绕垂线方向的转动惯量；l_f、l_r分别为车辆前、后轴与质心的距离；δ_f为前轮转角；

LQR路径跟踪控制器针对离散系统进行控制，将连续系统转化为离散系统，设定控制步长为T，选择双线性离散化的方法，将状态空间方程离散化，得到：

X(k+1)＝A_dX(k)+B_dU(k)

式中：A_d＝(I-TA/2)^-1(I+TA/2),B_d＝TB；

定义目标函数：

式中，Q为状态权重矩阵；R为控制量权重矩阵；

LQR控制方法通过求解控制量U，使目标函数的值最小。

优选地，其特征在于：所述步骤S302中最优控制权重为速度阈值V_th预设为2m/s时的控制权重。

优选地，所述步骤S303中权重参数q₁,r的计算公式为：

式中，q_1th，r_th为速度阈值内的最优控制权重，k₁，k₂为速度增益系数。

与现有技术相比，本发明的有益效果是：

本发明对一般LQR控制方法进行了改进，采用预瞄PID方法对LQR控制方法进行转角补偿，克服传统前馈转角补偿由于车辆延时和参数不确定性而无法精准消除稳态误差的问题，有效的提高跟踪精度；同时，本发明采用基于车速的LQR控制权重调整方法根据车速实时调整控制权重，能够有效提高控制器对于车速的适应性，在车速变化时也能保持良好的跟踪精度。

附图说明

图1为本发明的一种带有PID转角补偿的参数自整定LQR路径跟踪方法的控制系统示意图。

图2为路径跟踪误差示意图。

图3为路径跟踪实车测试效果图。

图4为路径跟踪实车测试横向位置偏差示意图。

具体实施方式

以下结合附图和具体实施例对本发明作进一步的详细描述。

如图1所示，本发明提出的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，所述方法包括以下步骤：

步骤S1：实时采集参考路径信息、车辆当前的位姿信息与运动状态信息；参考路径信息为离散的参考路径点集，其中参考路径点包含位置、航向角与曲率信息；车辆当前的位姿信息包括车辆质心位置、车辆航向角，所述运动状态信息包括车辆横摆角速度、车辆纵向速度、车辆横向速度。

步骤S2：根据采集的参考路径信息、车辆当前的位姿信息与运动状态信息确定目标参考点，计算车辆与目标参考点的偏差。具体步骤包括：

S201：将参考路径信息中的参考路径点位置与车辆位置转换为UTM坐标；

S202：确定目标参考点，所述目标参考点包含质心参考点与预瞄参考点，分别为车辆质心参考点、车辆预瞄点参考点；

车辆预瞄点的计算公式如下：

式中，X_pre，Y_pre为车辆预瞄点位置，X，Y为车辆质心位置，d_pre为预瞄距离，

为车辆航向角；

S203：遍历参考路径点集，查找距离车辆质心位置最近的两个相邻路径点，利用线性插值法确定车辆实际的质心参考点；

S204：根据车辆实际的质心参考点信息与车辆质心位置，计算车辆与参考点的横向位置偏差e_y，航向偏差

横向位置偏差变化率

航向偏差变化率

S205：遍历参考路径点集，查找距离车辆预瞄点位置最近的两个相邻路径点，利用线性插值法确定车辆实际的预瞄参考点；

S206：根据车辆实际的预瞄参考点位置与车辆预瞄点位置，计算预瞄点与参考点的横向位置偏差e_pre。

步骤S3：根据车速信息，采用基于车速的LQR控制权重调整方法确定控制权重；具体步骤包括：

S301：建立路径跟踪误差模型，设计LQR路径跟踪控制器针对离散系统进行控制，通过求解合适的控制量U，使目标函数的值最小，目标函数中，权重矩阵Q＝diag[q1,q2,q3,q4]，q1,q2,q3,q4分别为横向位置偏差e_y，横向位置偏差变化率

航向偏差

航向偏差变化率

控制量权重矩阵R＝[r]，r为前轮转角δ_f的权重系数；

S302：根据测试实验，在保证车辆路径跟踪的精准性和行驶的舒适性、无左右摇摆现象的条件下确定速度阈值V_th(预设为2m/s)的最优控制权重；

S303：根据车辆纵向速度V_x，调整权重参数q₁,r的值，计算公式为：

式中，q_1th，r_th为速度阈值内的最优控制权重，k₁，k₂为速度增益系数。

智能汽车跟踪参考路径时，控制系统主要针对横向位置误差e_y、航向角误差

和前轮转角δ_f三个变量进行控制，兼顾准确性与舒适性。控制权重系数越大，其对相应变量的限制作用越强，权重系数的比例大小决定了控制系统对各变量的控制程度。基于此，保持权重参数q₂,q₃,q₄的值不变，只调整权重参数q₁,r。

步骤S4：根据步骤S2中计算的车辆与目标参考点的偏差值(横向位置偏差e_y，航向偏差

横向位置偏差变化率

航向偏差变化率

)及步骤S3中确定的控制权重(q1,q2,q3,q4,r)计算车辆的前轮转角δ₁；

步骤S5：采用预瞄PID方法计算补偿前轮转角δ₂，补偿前轮转角δ₂的计算公式为：

式中，k_p，k_i，k_δ分别表示比例、积分、微分常数，e_pre(k)表示k时刻的预瞄横向偏差。

步骤S6：计算车辆方向盘转角控制量，传递给转向执行机构。车辆方向盘转角控制量δ的计算公式为：

δ＝i·(δ₁+δ₂)

式中，i表示车辆转向系统的传动比，δ₁为车辆跟踪路径所需的前轮转角，δ₂为前轮转角补偿前轮转角。

图3和图4为变速工况下，车辆进行路径跟踪测试的结果，车辆的行驶速度在1～6m/s的区间内变化，速度阈值设为2m/s，阈值速度内的最优控制权重为Q＝diag[10,0,17,0]，R＝[5]。图3为路径跟踪效果图，图4为横向位置偏差示意图。

除上述实施例外，本发明还可以有其他实施方式。凡任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化均应为等效的置换方式，都包含在本发明要求的保护范围。

Claims (10)

1.一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述方法包括以下步骤：

步骤S1：实时采集参考路径信息、车辆当前的位姿信息与运动状态信息；

步骤S2：根据采集的参考路径信息、车辆当前的位姿信息与运动状态信息确定目标参考点，计算车辆与目标参考点的偏差；

步骤S3：根据车速信息，采用基于车速的LQR控制权重调整方法确定控制权重；

步骤S4：根据步骤S2中计算的车辆与目标参考点的偏差值及步骤S3中确定的控制权重计算车辆的前轮转角；

步骤S5：采用预瞄PID方法计算补偿前轮转角；

步骤S6：计算车辆方向盘转角控制量，传递给转向执行机构。

2.根据权利要求1所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S1中，所述参考路径信息为离散的参考路径点集，其中参考路径点包含位置、航向角与曲率信息；所述车辆当前的位姿信息包括车辆质心位置、车辆航向角，所述运动状态信息包括车辆横摆角速度、车辆纵向速度、车辆横向速度。

3.根据权利要求2所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S2的具体步骤包括：

S201：将参考路径信息中的参考路径点位置与车辆位置转换为UTM坐标；

S202：确定目标参考点，所述目标参考点包含质心参考点与预瞄参考点，分别为车辆质心参考点、车辆预瞄点参考点；

S203：遍历参考路径点集，查找距离车辆质心位置最近的两个相邻路径点，利用线性插值法确定车辆实际的质心参考点；

S204：根据车辆实际的质心参考点信息与车辆质心位置，计算车辆与参考点的横向位置偏差e_y，航向偏差

横向位置偏差变化率

航向偏差变化率

S205：遍历参考路径点集，查找距离车辆预瞄点位置最近的两个相邻路径点，利用线性插值法确定车辆实际的预瞄参考点；

S206：根据车辆实际的预瞄参考点位置与车辆预瞄点位置，计算预瞄点与参考点的横向位置偏差e_pre。

4.根据权利要求1所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S3的具体步骤包括：

S301：建立路径跟踪误差模型，设计LQR路径跟踪控制器针对离散系统进行控制，通过求解合适的控制量U，使目标函数的值最小，目标函数中，权重矩阵Q＝diag[q1,q2,q3,q4]，q1,q2,q3,q4分别为横向位置偏差e_y，横向位置偏差变化率

航向偏差

航向偏差变化率

控制量权重矩阵R＝[r]，r为前轮转角δ_f的权重系数：

LQR路径跟踪控制器目标函数的设定方法为：将路径跟踪误差模型表示为状态空间方程形式：

其中：

U＝[δ_f]，

式中，m为整车质量；V_x分别为车辆的纵向车速；c_f，c_r分别为车辆前、后轴车轮的侧偏刚度；I_z为车辆绕垂线方向的转动惯量；l_f、l_r分别为车辆前、后轴与质心的距离；δ_f为前轮转角；

LQR路径跟踪控制器针对离散系统进行控制，在每一个控制时域输出一个最优控制量。所以，需要将连续系统转化为离散系统。设定控制步长为T，选择双线性离散化的方法，将状态空间方程离散化，可以得到：

X(k+1)＝A_dX(k)+B_dU(k)

式中：

A_d＝(I-TA/2)^-1(I+TA/2),B_d＝TB

路径跟踪控制器的控制目的不仅需要减小跟踪误差，同时，为了保证智能汽车的行驶稳定性，还需要使控制量较小。由此，定义如下目标函数：

式中，Q为状态权重矩阵；R为控制量权重矩阵。

LQR控制方法通过求解合适的控制量U，使目标函数的值最小。目标函数中，权重矩阵Q＝diag[q1,q2,q3,q4],分别为e_y、

和

的权重系数；R＝[r],为δ_f的权重系数，一共5个权重参数；

S302：根据测试实验，在保证车辆路径跟踪的精准性和行驶的舒适性、无左右摇摆现象的条件下确定速度阈值V_th的最优控制权重；

S303：根据车辆纵向速度V_x，调整权重参数q₁,r的值。

5.根据权利要求1所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S5中根据步骤S2计算得到的预瞄偏差e_pre，采用PID方法计算补偿前轮转角，计算公式为：

式中，k_p，k_i，k_δ分别表示比例、积分、微分常数，e_pre(k)表示k时刻的预瞄横向偏差。

6.根据权利要求1所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S6中，计算车辆方向盘转角控制量，传递给转向执行结构，车辆方向盘转角控制量δ的计算公式为：

δ＝i·(δ₁+δ₂)

式中，i表示车辆转向系统的传动比，δ₁为车辆跟踪路径所需的前轮转角，δ₂为前轮转角补偿前轮转角。

7.根据权利要求3所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S202中车辆预瞄点的计算公式如下：

式中，X_pre，Y_pre为车辆预瞄点位置，X，Y为车辆质心位置，d_pre为预瞄距离，

为车辆航向角。

8.根据权利要求4所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S301中LQR路径跟踪控制器目标函数的设定方法为：将路径跟踪误差模型表示为状态空间方程形式：

其中：

U＝[δ_f]，

式中，m为整车质量；V_x分别为车辆的纵向车速；c_f，c_r分别为车辆前、后轴车轮的侧偏刚度；I_z为车辆绕垂线方向的转动惯量；l_f、l_r分别为车辆前、后轴与质心的距离；δ_f为前轮转角；

LQR路径跟踪控制器针对离散系统进行控制，将连续系统转化为离散系统，设定控制步长为T，选择双线性离散化的方法，将状态空间方程离散化，得到：

X(k+1)＝A_dX(k)+B_dU(k)

式中：A_d＝(I-TA/2)^-1(I+TA/2),B_d＝TB；

定义目标函数：

式中，Q为状态权重矩阵；R为控制量权重矩阵；

LQR控制方法通过求解控制量U，使目标函数的值最小。

9.根据权利要求4所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S302中最优控制权重为速度阈值V_th预设为2m/s时的控制权重。

10.根据权利要求4所述的一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其特征在于：所述步骤S303中权重参数q₁,r的计算公式为：

式中，q_1th，r_th为速度阈值内的最优控制权重，k₁，k₂为速度增益系数。

Images