CN116691668A - 一种大型智能车辆的横向控制方法 - Google Patents

Abstract

本发明涉及一种大型智能车辆的横向控制方法，属于自动驾驶技术领域。方法包括：实时获取车辆的当前位置、当前航向角、以及当前车速，并且规划车辆的行驶轨迹，车辆的行驶轨迹包括每个轨迹点的位置和航向角；在车辆的行驶轨迹中结合当前位置确定若干个预瞄点，通过若干个预瞄点的位置和航向角、当前位置、当前航向角，结合加权平均的方式确定出目标横向偏差和目标航向角偏差；将当前车速、目标横向偏差、以及目标航向角偏差输入空间状态方程；求解空间状态方程，得到前轮转角，按照前轮转角进行转向控制。本发明的横向控制适用于大型车辆的泊车控制，有效保证大型智能车辆在泊车场景中大曲率S弯的跟踪精度。

Description

一种大型智能车辆的横向控制方法

技术领域

本发明涉及一种大型智能车辆的横向控制方法，属于自动驾驶技术领域。

背景技术

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

传统的路径跟踪方法是根据车辆运动学原理，基于横向偏差，采用横向控制的高阶控制器如LQR及MPC等得到车辆前轮的最优转角，进而对车辆进行方向盘转角控制，然而这种跟踪控制精度较低。

为了提高跟踪精度，有人提出以横向偏差和航向角偏差为状态量的控制方式，例如：申请公布号为CN 113608530 A的中国发明专利申请文件，该专利文件公开了一种带有PID转角补偿的参数自整定LQR路径跟踪方法，其中在进行前轮转角计算时通过车辆质心位置与车辆实际的质心参考点位置，计算车辆与参考点的横向偏差和航向偏差，通过横向偏差和航向偏差计算出前轮转角，并且确定车辆实际的预瞄参考点位置，计算参考点与预瞄参考点的横向偏差，通过横向偏差计算补偿前轮转角，结合前轮转角和补偿前轮转角确定最终的前轮转角控制量。

然而，对于大型智能公交(12米以上)的泊车场景，受限于空间，泊车轨迹会存在大曲率的S弯，并且轨迹形状多变，对跟踪算法的精度适应性和鲁棒性要求都很高，上述方案并不能看出针对大型商用车泊车场景的优化和适应，因此难以适用于大型智能公交(12米以上)的泊车场景，为此，需要提出一种适用于大型智能公交的高精度的横向控制的技术方案。

发明内容

本申请的目的在于提供一种大型智能车辆的横向控制方法，用以解决现有横向控制精度差的问题。

为实现上述目的，本申请提出了一种大型智能车辆的横向控制方法的技术方案，包括以下步骤：

1)实时获取车辆的当前位置、当前航向角、以及当前车速，并且规划车辆的行驶轨迹，车辆的行驶轨迹包括每个轨迹点的位置和航向角；

2)在车辆的行驶轨迹中结合当前位置确定若干个预瞄点，通过若干个预瞄点的位置和航向角、当前位置、当前航向角，结合加权平均的方式确定出目标横向偏差和目标航向角偏差；

3)将当前车速、目标横向偏差、以及目标航向角偏差输入空间状态方程；所述空间状态方程基于车辆动力学模型建立；

4)通过线性二次调节控制算法或者模型预测控制算法求解空间状态方程，得到前轮转角，按照前轮转角进行转向控制。

本发明的大型智能车辆的横向控制方法的技术方案的有益效果是：本发明通过若干个预瞄点的状态和当前状态确定出目标横向偏差和目标航向角偏差，进而将目标横向偏差和目标航向角偏差作为空间状态方程的输入，通过线性二次调节控制算法或者模型预测控制算法求解出前轮转角。其中，多个预瞄点能够更好地反应轨迹趋势，且有效避免轨迹不平滑对LQR算法造成的扰动。因此，本发明的横向控制适用于大型车辆的泊车控制，能有效保证大型智能车辆在泊车场景中大曲率S弯的跟踪精度。

进一步地，为了提高横向控制精度，若干个预瞄点通过预瞄距离确定，在车辆的行驶轨迹中找出最接近预瞄距离的点作为预瞄点，所述预瞄距离的计算过程为：

L_m＝v_xt_{pre_m}+L_{base_m}，

其中，L_m为第m个预瞄点的预瞄距离；v_x为当前车速；t_{pre_m}为第m个预瞄点的预瞄时间；L_{base_m}为第m个预瞄点的基础预瞄距离。

进一步地，为了更加准确的确定目标横向偏差，目标横向偏差的计算过程为：

其中，e_cg为目标横向偏差；为第i个预瞄点的位置与当前位置的横向偏差；n为预瞄点的数量；k_window为权重窗。

进一步地，为了更加准确的确定目标航向角偏差，目标航向角偏差的计算过程为：

其中，θ_e为目标航向角偏差；为第i个预瞄点的航向角与当前航向角的航向角偏差；n为预瞄点的数量；k_window为权重窗。

进一步地，空间状态方程为：

其中，e_cg为目标横向偏差；θ_e为目标航向角偏差；δ_f为前轮转角；为横摆角速率；C_f为前轴轮胎侧偏刚度；C_r为后轴轮胎侧偏刚度；l_f为前轴到质心距离；I_z为转动惯量；l_r为后轴到质心距离；V_x为车速；a为加速度；m为整车质量。

进一步地，为了提高前轮转角的跟踪精度，所述步骤1)中车辆的行驶轨迹还包括每个轨迹点的曲率，所述步骤2)中还通过若干个预瞄点的曲率，结合加权平均的方式确定出目标曲率，通过目标曲率计算补偿前轮转角，通过补偿前轮转角对步骤4)得到的前轮转角进行补偿，进而通过补偿后的前轮转角进行转向控制，补偿前轮转角的计算过程为：

其中，δ_f′为补偿前轮转角；L为轴距；R为转弯半径；ρ_e为目标曲率；a_y为侧向加速度；k₃为目标航向角偏差权重系数；C_f为前轴轮胎侧偏刚度；C_r为后轴轮胎侧偏刚度；l_f为前轴到质心距离；l_r为后轴到质心距离；V_x为车速；m为整车质量。

进一步地，为了更加准确的确定目标曲率，目标曲率的计算过程为：

其中，ρ_e为目标曲率；为第i个预瞄点的曲率；n为预瞄点的数量；k_window为权重窗。

进一步地，为了保证跟踪精度，还包括确定纵向车速的步骤：

当目标横向偏差超出横向偏差阈值、或者目标航向角偏差超出航向角偏差阈值、或者目标曲率超出曲率阈值，纵向车速不超过车速低值。

进一步地，为了保证大型车辆泊车入位的最终姿态精度，线性二次调节控制算法中各状态量的权重系数根据泊车时的横向偏差精度和航向角偏差精度进行确定，所述状态量包括横向偏差、航向角偏差、横向偏差变化率、航向角偏差变化率。

进一步地，为了保证预瞄点确定的准确性，预瞄时间与车速正相关，基础预瞄距离根据曲率确定。

附图说明

图1是本发明大型智能车辆的横向控制方法的流程图；

图2是本发明预瞄点与当前位置的横向偏差和航向角偏差的计算示意图。

具体实施方式

大型智能车辆的横向控制方法实施例：

本发明的主要构思在于，基于现有横向控制不适用于大型车辆的问题，本发明通过在规划的行驶轨迹中确定若干个预瞄点，通过若干个预瞄点的位置与当前位置确定出目标横向偏差，并且通过若干个预瞄点的航向角与当前航向角确定出目标航向角偏差，进而通过目标横向偏差、目标航向角偏差结合空间状态方程，采用线性二次调节控制算法求解出前轮转角，进而实现大型智能车辆的横向控制。

具体地，大型智能车辆的横向控制方法如图1所示，包括以下步骤：

1)实时获取车辆的当前状态，并且规划车辆的行驶轨迹。

车辆的当前状态包括当前位置、当前航向角、以及当前车速，车辆的当前状态可以通过设置在车辆上的各种传感器获取。

车辆的行驶轨迹包括每个轨迹点的位置、航向角以及曲率。

车辆的行驶轨迹的规划属于现有技术，本实施例中不再赘述，具体行驶轨迹规划的方法例如可以参考现有技术文献：《智能车辆局部轨迹规划与跟踪控制算法研究》何慧玲，哈尔滨工业大学，2021年6月。

2)将规划的行驶轨迹以及车辆的当前位置输入轨迹预瞄模型确定出若干个预瞄点的位置，结合行驶轨迹确定每个预瞄点的位置、航向角以及曲率。

轨迹预瞄模型用于在车辆的行驶轨迹中确定若干个预瞄点，若干个预瞄点的位置确定过程如下：

首先，确定预瞄点的数量、每个预瞄点的预瞄距离；

一般情况下，预瞄点的数量为8个左右，每个预瞄点的预瞄距离和每个预瞄点的预瞄时间和每个预瞄点的基础预瞄距离有关；

预瞄距离的计算过程为：

L_m＝v_xt_{pre_m}+L_{base_m}，

其中，L_m为第m个预瞄点的预瞄距离；v_x为当前车速；t_{pre_m}为第m个预瞄点的预瞄时间；L_{base_m}为第m个预瞄点的基础预瞄距离。t_{pre_m}的设定跟当前车速正相关，车速越快，预瞄时间越大，由标定获取，例如：车速从0到70km/h，步长为10km/h，0到70km/h的速度值按设定步长的对应速度值均标定一个预瞄时间，然后采用线性差值的方法，获取每个速度值的预瞄时间。基础预瞄距离的确定和一段轨迹的曲率以及预瞄点的序列有关，曲率可以取整条轨迹的曲率的最大值，基础预瞄距离根据曲率分段给出。

其次，根据车辆的当前位置，在车辆的行驶轨迹中找出最接近预瞄距离的点作为预瞄点，车辆的当前位置(x₀，y₀)和行驶轨迹的轨迹点(x_n，y_n)之间的距离s_n计算公式为：s_n的值最接近预瞄距离的轨迹点选择为相应的预瞄点。

当然，无需每个预瞄点的预瞄距离均采用上述公式计算，例如：以6个预瞄点为例，第1个预瞄点的预瞄距离为固定距离，例如为通过标定确定的1米，第2个预瞄点和第6个预瞄点的预瞄距离采用上述公式计算，并且第2个预瞄点的L_{base_2}和t_{pre_2}取值为：当一段轨迹的曲率大于0.03时(可以通过标定确定)，L_{base_2}取值为1.5m，否则L_{base_2}取值为2m，t_{pre_2}为固定值0.1s；第6个预瞄点L_{base_6}和t_{pre_6}的取值为：当一段轨迹的曲率大于0.03时，L_{base_6}＝5m，否则L_{base_6}取值为8m，t_{pre_6}取值为0.3s；第3个预瞄点、第4个预瞄点和第5个预瞄点的预瞄距离通过第2个预瞄点和第6个预瞄点的预瞄距离进行线性插值得到。

为了减小计算量，提高控制的速度，这里若干个预瞄点的确定并不需要根据实时的当前位置进行实时更新，只需要车辆的当前位置与上一次确定的第1个预瞄点的位置非常接近时再结合当前位置重新确定若干个预瞄点即可。

3)根据每个预瞄点的位置与当前位置确定每个预瞄点对应的横向偏差，根据每个预瞄点的航向角和当前航向角确定每个预瞄点对应的航向角偏差，将若干个预瞄点对应的横向偏差取加权平均得到目标横向偏差，将若干个预瞄点对应的航向角偏差取加权平均得到目标横向偏差，同时根据每个预瞄点的曲率确定目标曲率。

目标横向偏差的计算过程为：

其中，e_cg为目标横向偏差；为第i个预瞄点的位置与当前位置的横向偏差；n为预瞄点的数量；k_window为权重窗。

目标航向角偏差的计算过程为：

其中，θ_e为目标航向角偏差；为第i个预瞄点的航向角与当前航向角的航向角偏差；n为预瞄点的数量；k_window为权重窗。

以某个预瞄点为例详细说明每个预瞄点对应的横向偏差以及航向角偏差的计算过程：

如图2所示，设第i个预瞄点的位置为(x_des，y_des)，第i个预瞄点的航向角为当前位置为(x，y)，当前航向角为/>根据三角函数关系得到：

其中，d_x为第i个预瞄点的位置与当前位置的x轴的差值；d_y为第i个预瞄点的位置与当前位置的y轴的差值；为第i个预瞄点的位置与当前位置的横向偏差；e_y为第i个预瞄点的位置与当前位置的纵向偏差；/>为第i个预瞄点的航向角与当前航向角的航向角偏差。

目标曲率的计算过程为：

其中，ρ_e为目标曲率；为第i个预瞄点的曲率；n为预瞄点的数量；k_window为权重窗。

4)将当前车速、目标横向偏差、以及目标航向角偏差输入空间状态方程，求解出前轮转角。

空间状态方程基于车辆动力学模型进行建立，由于泊车场景属于低速运行场景，因此选择二轮自行车模型即可满足要求，空间状态方程为：

其中，e_cg为目标横向偏差；θ_e为目标航向角偏差；δ_f为前轮转角；为横摆角速率；C_f为前轴轮胎侧偏刚度；C_r为后轴轮胎侧偏刚度；l_f为前轴到质心距离；I_z为转动惯量；l_r为后轴到质心距离；V_x为车速；a为加速度；m为整车质量。

空间状态方程中，令x为状态量矩阵，令/>u为控制量，A为状态量系数矩阵，B为控制量系数矩阵。

通过线性二次调节控制算法LQR或者模型预测控制算法MPC求解空间状态方程，得到前轮转角，本实施例采用LQR，LQR的目标是找出一组控制量使得***的状态量和控制量最小，定义如下代价函数J：

其中，Q为状态量权重矩阵；R为控制量权重矩阵；Q_f为最终的状态量权重矩阵；N为到达最终状态的控制序列数。

上述状态量权重矩阵Q定义如下：

k₁为横向偏差权重系数；k₂为横向偏差变化率权重系数；k₃为航向角偏差权重系数；k₄为航向角偏差变化率权重系数。

状态量权重矩阵Q根据泊车时的横向偏差精度和航向角偏差精度进行确定，根据控制精度的要求，横向偏差在0.12m左右，航向角偏差在0.03(弧度)左右，根据权重系数设置原则：权重系数越大对应的偏差越小，横向偏差权系数应小于航向角偏差权重系数，且两者的比例为1:4。本实施例中，横向偏差变化率权重系数和航向角偏差变化率权重系数设为0。

关于LQR控制算法的具体求解过程为现有技术，这里不做赘述。

5)将目标曲率输入轨迹曲率补偿模型得到补偿前轮转角，通过补偿前轮转角对步骤4)得到的前轮转角进行补偿，进而通过补偿后的前轮转角进行转向控制。

轨迹曲率补偿模型用于计算补偿前轮转角，补偿前轮转角的计算过程为：

其中，δ_f′为补偿前轮转角；L为轴距；R为转弯半径；ρ_e为目标曲率；a_y为侧向加速度；k₃为航向角偏差权重系数；C_f为前轴轮胎侧偏刚度；C_r为后轴轮胎侧偏刚度；l_f为前轴到质心距离；l_r为后轴到质心距离；V_x为车速；m为整车质量。

补偿后的前轮转角＝δ_fδ+δ_f。

6)将目标横向偏差、目标航向角偏差、以及目标曲率输入车速控制模型确定纵向车速。

车速控制模型用于确定车辆的纵向车速，当目标横向偏差超出横向偏差阈值、或者目标航向角偏差超出航向角偏差阈值、或者目标曲率超出曲率阈值，纵向车速不超过车速低值，车速低值为2km/h，为横向控制提供更多的调整时间，保证跟踪精度。当然，在大型车辆泊车时纵向车速不高的情况下，也可以不进行纵向车速的控制。本实施例中，横向偏差阈值设为泊车精度要求的横向偏差的3倍；航向角偏差阈值设为泊车精度要求的航向角偏差的2倍。

曲率阈值一般选取0.07，曲率阈值与车辆参数(最小转弯半径和最大侧向加速度)和车辆响应性能(方向盘转角的响应时间)有关，为根据车辆参数和车辆响应性能的标定量。

上述实施例中，目标横向偏差以及目标航向角偏差的计算过程为首先计算每个预瞄点与车辆当前状态的偏差，接着根据各偏差加权平均计算得出目标横向偏差和目标航向角偏差，作为其他实施方式，也可以首先对各预瞄点的状态进行加权平均，得到加权平均的结构再与车辆当前状态比较进而得到目标横向偏差和目标航向角偏差。

上述实施例中，通过轨迹曲率补偿模型对前轮转角进行补偿，并且将目标曲率作为输入量计算补偿前轮转角，作为其他实施方式，在保证前轮转角精度的情况下，也可以不进行补偿，或者直接选用若干预瞄点中的其中一个预瞄点的曲率作为输入计算补偿前轮转角。

上述实施例中，为了提高横向控制的精度，若干预瞄点是通过预瞄距离进行确定的，作为其他实施方式，也可以直接在行驶轨迹上当前位置的前方设定距离处选若干个轨迹点作为预瞄点。

本发明的横向控制适用于大型车辆的泊车控制，有效保证大型智能车辆在泊车场景中大曲率S弯的跟踪精度。

Claims (10)

1.一种大型智能车辆的横向控制方法，其特征在于，包括以下步骤：

1)实时获取车辆的当前位置、当前航向角、以及当前车速，并且规划车辆的行驶轨迹，车辆的行驶轨迹包括每个轨迹点的位置和航向角；

2)在车辆的行驶轨迹中结合当前位置确定若干个预瞄点，通过若干个预瞄点的位置和航向角、当前位置、当前航向角，结合加权平均的方式确定出目标横向偏差和目标航向角偏差；

3)将当前车速、目标横向偏差、以及目标航向角偏差输入空间状态方程；所述空间状态方程基于车辆动力学模型建立；

4)通过线性二次调节控制算法或者模型预测控制算法求解空间状态方程，得到前轮转角，按照前轮转角进行转向控制。

2.根据权利要求1所述的大型智能车辆的横向控制方法，其特征在于，若干个预瞄点通过预瞄距离确定，在车辆的行驶轨迹中找出最接近预瞄距离的点作为预瞄点，所述预瞄距离的计算过程为：

L_m＝v_xt_{pre_m}+L_{base_m}；

其中，L_m为第m个预瞄点的预瞄距离；v_x为当前车速；t_{pre_m}为第m个预瞄点的预瞄时间；L_{base_m}为第m个预瞄点的基础预瞄距离。

3.根据权利要求1所述的大型智能车辆的横向控制方法，其特征在于，目标横向偏差的计算过程为：

其中，e_cg为目标横向偏差；为第i个预瞄点的位置与当前位置的横向偏差；n为预瞄点的数量；k_window为权重窗。

4.根据权利要求1所述的大型智能车辆的横向控制方法，其特征在于，目标航向角偏差的计算过程为：

其中，θ_e为目标航向角偏差；为第i个预瞄点的航向角与当前航向角的航向角偏差；n为预瞄点的数量；k_window为权重窗。

5.根据权利要求1所述的大型智能车辆的横向控制方法，其特征在于，空间状态方程为：

其中，e_cg为目标横向偏差；θ_e为目标航向角偏差；δ_f为前轮转角；为横摆角速率；C_f为前轴轮胎侧偏刚度；C_r为后轴轮胎侧偏刚度；l_f为前轴到质心距离；I_z为转动惯量；l_r为后轴到质心距离；V_x为车速；a为加速度；m为整车质量。

6.根据权利要求1所述的大型智能车辆的横向控制方法，其特征在于，所述步骤1)中车辆的行驶轨迹还包括每个轨迹点的曲率，所述步骤2)中还通过若干个预瞄点的曲率，结合加权平均的方式确定出目标曲率，通过目标曲率计算补偿前轮转角，通过补偿前轮转角对步骤4)得到的前轮转角进行补偿，进而通过补偿后的前轮转角进行转向控制，补偿前轮转角的计算过程为：

其中，δ_f′为补偿前轮转角；L为轴距；R为转弯半径；ρ_e为目标曲率；a_y为侧向加速度；k₃为目标航向角偏差权重系数；C_f为前轴轮胎侧偏刚度；C_r为后轴轮胎侧偏刚度；l_f为前轴到质心距离；_r为后轴到质心距离；_x为车速；m为整车质量。

7.根据权利要求6所述的大型智能车辆的横向控制方法，其特征在于，目标曲率的计算过程为：

其中，ρ_e为目标曲率；为第i个预瞄点的曲率；n为预瞄点的数量；k_window为权重窗。

8.根据权利要求6所述的大型智能车辆的横向控制方法，其特征在于，还包括确定纵向车速的步骤：

当目标横向偏差超出横向偏差阈值、或者目标航向角偏差超出航向角偏差阈值、或者目标曲率超出曲率阈值，纵向车速不超过车速低值。

9.根据权利要求1所述的大型智能车辆的横向控制方法，其特征在于，线性二次调节控制算法中各状态量的权重系数根据泊车时的横向偏差精度和航向角偏差精度进行确定，所述状态量包括横向偏差、航向角偏差、横向偏差变化率、航向角偏差变化率。

10.根据权利要求2所述的大型智能车辆的横向控制方法，其特征在于，预瞄时间与车速正相关，基础预瞄距离根据曲率确定。