CN113741450B - 一种车辆自动驾驶横向自适应控制方法 - Google Patents

Abstract

本发明公开了一种车辆自动驾驶横向自适应控制方法，包括如下步骤：(1)根据Frenet坐标系下的二自由度车辆动力学矩阵方程获取系数矩阵A和B，对系数矩阵A和B进行离散化；(2)根据离散化的系数矩阵和计算反馈矩阵；(3)计算车辆当前位置与匹配点之间的误差矩阵；(4)根据反馈矩阵和轨迹曲率，计算前馈控制输入；(5)根据反馈控制输入和前馈控制输入得到总控制输入，将总控制输入传递给控制器进行车辆的自动控制。本发明利用控制权重矩阵R设置参数自调整策略后，自动驾驶车辆根据不同的转弯半径工况，选择不同的控制矩阵R，从而使得LQR算法能够自适应输出不同的转弯角度，在满足跟踪轨迹精度的基础上保证了车辆转向舒适性的要求。

Description

一种车辆自动驾驶横向自适应控制方法

技术领域

本发明涉及自动驾驶横向运动控制领域，特别涉及一种车辆自动驾驶横向自适应控制方法。

背景技术

车辆自动驾驶控制系统根据上层决策、规划系统发出的规划目标路径信息，输出相应的控制参数，使车辆按照规划的目标路径行驶，控制系统的好坏不仅决定了车辆跟踪目标路径的精度，还会影响车辆的舒适性。

当前自动驾驶控制分为横向控制与纵向控制，现有的横线控制技术包括纯跟踪算法、Stanley算法、LQR算法、MPC算法等，其中LQR算法应用最为广泛，但现有技术无法实现LQR算法的自适应参数调整控制。

专利申请CN110568758A提出了一种自动驾驶汽车的参数自适应横向运动LQR控制方法，在车辆位置与规划路径的距离偏差与航向偏差不同时，选择不同的状态权重矩阵Q增益值；但该专利申请对Q矩阵中的横向距离偏差、横摆角偏差项对应的矩阵参数进行设计，忽略了横向速度偏差、横摆角速度偏差对车辆横向控制的影响，与实际应用有一定差异，且该专利申请并没有对控制权重矩阵R设置参数自调整策略，使得自动驾驶车辆在不同工况行驶过程中，因为转向盘转角过大影响了车辆舒适性。

发明内容

发明目的：针对以上问题，本发明目的是提供一种车辆自动驾驶横向自适应控制方法。

技术方案：本发明的一种车辆自动驾驶横向自适应控制方法，包括如下步骤：

(1)根据Frenet坐标系下的二自由度车辆动力学矩阵方程获取系数矩阵A和B，对系数矩阵A和B进行离散化；

(2)输入状态权重矩阵Q和控制权重矩阵R，根据离散化的系数矩阵和/>计算反馈矩阵K；

(3)根据当前车辆状态矩阵和规划轨迹矩阵，计算车辆当前位置与匹配点之间的误差矩阵e_rr，所述匹配点为集中规划离散轨迹点中距离当前车辆位置距离最近的规划点；

(4)根据反馈矩阵K和轨迹曲率，计算前馈控制输入δ_f；

(5)根据反馈控制输入和前馈控制输入得到总控制输入，将总控制输入传递给控制器进行车辆的自动控制；

所述步骤(2)中的控制权重矩阵R随车辆转弯半径进行调整，表达式为：

式中r为车辆转弯半径，k_R为控制增益，R_max为控制增益的最大值。

进一步，所述步骤(1)二自由度车辆动力学矩阵方程为：

其中表示误差矩阵的导数，e_rr表示误差矩阵，u为控制量，表示前轮转角；

获取车辆当前行驶数据，上式写成：

其中C_αf为车辆前轮侧偏刚度、C_αr为车辆后轮侧偏刚度、a为车辆质心到前轴的距离、b为车辆质心到后轴的距离、I_z为车辆绕Z轴的转动惯量、m为车辆的质量、v_x为车辆在纵向的车速、e_d为横向位置误差、为横向速度误差、/>为横摆角误差、/>为横摆角速度误差；

得到系数矩阵A的表达式为：

系数矩阵B的表达式为：

系数矩阵A和B方程为连续方程，将两个矩阵进行离散化处理，分别为：

其中E为单位矩阵，dt为采样时间；

则Frenet坐标系下的二自由度车辆动力学矩阵方程离散化为：

其中k表示当前离散矩阵方程的采样时刻。

进一步，所述步骤(2)状态权重矩阵Q为：

其中q₁至q₄分别表示横向位置误差e_d、横向速度误差横摆角误差/>横摆角速度误差/>的权重值；

根据黎卡提方程计算矩阵P，表达式为：

则计算得到反馈矩阵K的表达式为：

进一步，步骤(3)中根据车辆自动驾驶定位模块与底层信息可知当前车辆状态矩阵为其中x为当前车辆纵向位移，y为当前车辆横向位移，v_x为当前车辆纵向速度，v_y为当前车辆横向速度，/>为当前车辆横摆角，/>为当前车辆横摆角速度；

由车辆自动驾驶规划模块提供规划轨迹矩阵为[x_r,y_r,θ_r,k_r]^T，其中x_r为当前规划的车辆纵向位移，y_r为当前规划的车辆横向位移，θ_r为当前规划的车辆横摆角，k_r为当前规划的轨迹曲率；

计算车辆当前位置与集中规划离散轨迹点每个点的距离，得到距离当前车辆位置距离最近的规划点，将该规划点称为匹配点，下标记为dmin，则匹配点的规划信息矩阵为[x_{d min},y_{d min},θ_{d min},k_{d min}]^T，当前车辆位置的误差矩阵各项表达式为：

e_d＝[-sin(θ_{d min}),cos(θ_{d min})][x-x_{d min},y-y_{d min}]^T

其中k_{d min}＝k_r，θ_{d min}＝θ_r。

进一步，将4×4反馈矩阵K的第3行记为K(3)，令K₃＝K(3)，所述步骤(4)中的前馈控制输入δ_f表达式为：

进一步，所述步骤(5)反馈控制输入u_k包括反馈矩阵和误差矩阵，表达式为：

u_k＝-Ke_rr

所述总控制输入δ表达式为：δ＝u_k+δ_f＝-Ke_rr+δ_f。

有益效果：本发明与现有技术相比，其显著优点是：本发明利用控制权重矩阵R设置参数自调整策略后，自动驾驶车辆根据不同的转弯半径工况，选择不同的控制矩阵R，从而使得LQR算法能够自适应输出不同的转弯角度，在满足跟踪轨迹精度的基础上保证了车辆转向舒适性的要求。

附图说明

图1为本发明流程图；

图2为二自由度车辆动力学模型示意图；

图3为坐标系转换示意图；

图4为全状态反馈控制示意图。

具体实施方式

如图1所示，本实施例所述的一种车辆自动驾驶横向自适应控制方法，包括如下步骤：

(1)根据Frenet坐标系下的二自由度车辆动力学矩阵方程获取系数矩阵A和B，对系数矩阵A和B进行离散化。

如图2所示，二自由度车辆动力学矩阵方程为：

其中表示误差矩阵的导数，e_rr表示误差矩阵，u为控制量，表示前轮转角；

获取车辆当前行驶数据，上式写成：

其中C_αf为车辆前轮侧偏刚度、C_αr为车辆后轮侧偏刚度、a为车辆质心到前轴的距离、b为车辆质心到后轴的距离、I_z为车辆绕Z轴的转动惯量、m为车辆的质量、v_x为车辆在纵向的车速、e_d为横向位置误差、为横向速度误差、/>为横摆角误差、/>为横摆角速度误差；

得到系数矩阵A的表达式为：

系数矩阵B的表达式为：

系数矩阵A和B方程为连续方程，将两个矩阵进行离散化处理，分别为：

其中E为单位矩阵，dt为采样时间；

则Frenet坐标系下的二自由度车辆动力学矩阵方程离散化为：

其中k表示当前离散矩阵方程的采样时刻。

(2)输入状态权重矩阵Q和控制权重矩阵R，根据离散化的系数矩阵和/>计算反馈矩阵K；

控制权重矩阵R随车辆转弯半径进行调整，表达式为：

式中r为车辆转弯半径，k_R为控制增益，R_max为控制增益的最大值，当车辆处于不同的转弯半径时，控制权重矩阵的取值不同。

状态权重矩阵Q为：

其中q₁至q₄分别表示横向位置误差e_d、横向速度误差横摆角误差/>横摆角速度误差/>的权重值；

根据黎卡提方程计算矩阵P，表达式为：

则计算得到反馈矩阵K的表达式为：

(3)根据当前车辆状态矩阵和规划轨迹矩阵，计算车辆当前位置与匹配点之间的误差矩阵e_rr，其中匹配点为集中规划离散轨迹点中距离当前车辆位置距离最近的规划点；

根据车辆自动驾驶定位模块与底层信息可知当前车辆状态矩阵为其中x为当前车辆纵向位移，y为当前车辆横向位移，v_x为当前车辆纵向速度，v_y为当前车辆横向速度，/>为当前车辆横摆角，/>为当前车辆横摆角速度；

如图3所示，由车辆自动驾驶规划模块提供规划轨迹矩阵为[x_r,y_r,θ_r,k_r]^T，其中x_r为当前规划的车辆纵向位移，y_r为当前规划的车辆横向位移，θ_r为当前规划的车辆横摆角，k_r为当前规划的轨迹曲率；

计算车辆当前位置与集中规划离散轨迹点每个点的距离，得到距离当前车辆位置距离最近的规划点，将该规划点称为匹配点，下标记为dmin，则匹配点的规划信息矩阵为[x_{d min},y_{d min},θ_{d min},k_{d min}]^T，当前车辆位置的误差矩阵各项表达式为：

e_d＝[-sin(θ_{d min}),cos(θ_{d min})][x-x_{d min},y-y_{d min}]^T

其中k_{d min}＝k_r，θ_{d min}＝θ_r，表示法向向量。

(4)根据反馈矩阵K和轨迹曲率，计算前馈控制输入δ_f；

将4×4反馈矩阵K的第3行记为K(3)，令K₃＝K(3)，则前馈控制输入δ_f表达式为：

(5)根据反馈控制输入和前馈控制输入得到总控制输入，将总控制输入传递给控制器进行车辆的自动控制，其中图4为全状态反馈控制示意图。

反馈控制输入u_k包括反馈矩阵和误差矩阵，表达式为：

u_k＝-Ke_rr

总控制输入δ表达式为：δ＝u_k+δ_f＝-Ke_rr+δ_f。

Claims (2)

1.一种车辆自动驾驶横向自适应控制方法，其特征在于，包括如下步骤：

(1)根据Frenet坐标系下的二自由度车辆动力学矩阵方程获取系数矩阵A和B，对系数矩阵A和B进行离散化；

(2)输入状态权重矩阵Q和控制权重矩阵R，根据离散化的系数矩阵A和B计算反馈矩阵K；

(3)根据当前车辆状态矩阵和规划轨迹矩阵，计算车辆当前位置与匹配点之间的误差矩阵，所述匹配点为集中规划离散轨迹点中距离当前车辆位置距离最近的规划点；

(4)根据反馈矩阵K和轨迹曲率，计算前馈控制输入；

(5)根据反馈控制输入和前馈控制输入得到总控制输入，将总控制输入传递给控制器进行车辆的自动控制；

所述步骤(1)二自由度车辆动力学矩阵方程为：

其中表示误差矩阵的导数，e_rr表示误差矩阵，u为控制量，表示前轮转角；

获取车辆当前行驶数据，上式写成：

其中C_αf为车辆前轮侧偏刚度、C_αr为车辆后轮侧偏刚度、a为车辆质心到前轴的距离、b为车辆质心到后轴的距离、I_z为车辆绕Z轴的转动惯量、m为车辆的质量、v_x为车辆在纵向的车速、e_d为横向位置误差、为横向速度误差、/>为横摆角误差、/>为横摆角速度误差，δ_f为前馈控制输入；

得到系数矩阵A的表达式为：

系数矩阵B的表达式为：

系数矩阵A和B方程为连续方程，将两个矩阵进行离散化处理，分别为：

其中E为单位矩阵，dt为采样时间；

则Frenet坐标系下的二自由度车辆动力学矩阵方程离散化为：

其中k表示当前离散矩阵方程的采样时刻；

所述步骤(2)中的控制权重矩阵R随车辆转弯半径进行调整，表达式为：

式中r为车辆转弯半径，k_R为控制增益，R_max为控制增益的最大值；

所述步骤(2)状态权重矩阵Q为：

其中q₁至q₄分别表示横向位置误差e_d、横向速度误差横摆角误差/>横摆角速度误差/>的权重值；

根据黎卡提方程计算矩阵P，表达式为：

则计算得到反馈矩阵K的表达式为：

将4×4反馈矩阵K的第3行记为K(3)，令K₃＝K(3)，所述步骤(4)中的前馈控制输入δ_f表达式为：

所述步骤(5)反馈控制输入u_k包括反馈矩阵和误差矩阵，表达式为：

u_k＝-Ke_rr

所述总控制输入δ表达式为：δ＝u_k+δ_f＝-Ke_rr+δ_f。

2.根据权利要求1所述的横向自适应控制方法，其特征在于，步骤(3)中根据车辆自动驾驶定位模块与底层信息得知当前车辆状态矩阵为其中x为当前车辆纵向位移，y为当前车辆横向位移，v_x为当前车辆纵向速度，v_y为当前车辆横向速度，/>为当前车辆横摆角，/>为当前车辆横摆角速度；

由车辆自动驾驶规划模块提供规划轨迹矩阵为[x_r,y_r,θ_r,k_r]^T，其中x_r为当前规划的车辆纵向位移，y_r为当前规划的车辆横向位移，θ_r为当前规划的车辆横摆角，k_r为当前规划的轨迹曲率；

计算车辆当前位置与集中规划离散轨迹点每个点的距离，得到距离当前车辆位置距离最近的规划点，将该规划点称为匹配点，下标记为dmin，则匹配点的规划信息矩阵为[x_dmin,y_dmin,θ_dmin,k_dmin]^T，当前车辆位置的误差矩阵各项表达式为：

e_d＝[-sin(θ_dmin),cos(θ_dmin)][x-x_dmin,y-y_dmin]^T

其中k_dmin＝k_r，θ_dmin＝θ_r。