[发明专利]一种自动驾驶车辆轨迹跟踪方法

权利要求书

1.一种自动驾驶车辆轨迹跟踪方法，其特征在于，包括以下步骤：

S1：建立地面坐标系和机体坐标系，对车辆进行动力学建模；车辆的动力学模型如下：

其中，x、y分别为地面坐标系下自动驾驶车辆质心的x轴、y轴坐标，ψ为地面坐标系下自动驾驶车辆的航向角，v为自动驾驶车辆的前进速度，a为自动驾驶车辆的前进加速度，t表示当前时刻的量，t+1表示下一时刻的量，L_f为前轮和后轮之间的轴距；δ_true为虚拟量，表示当前真实前轮倾角，是车辆在当前行驶方向下假设前轮没有打滑状态下对应的前轮倾角；

S2：获取机体自身状态向量X_t以及上一次输出向量U_t-1；其中，

X_t＝[x，y，ψ，v]^T (2)

U_t-1＝[a_t-1，δ_t-1] (3)

δ为自动驾驶车辆的前轮倾角；当需要计算下一时刻的机体自身状态向量X_t+1时，将该时刻的X_t代入到公式(1)中，即可算出下一时刻的X_t+1的相关参数x_t+1、y_t+1、ψ_t+1以及v_t+1；

S3：获取虚拟量，该虚拟量为当前真实前轮倾角δ_true；

S4：通过采样获取离散的航点路径P，并对该航点路径P进行插值计算，以长度l分别得出x_ref，y_ref，ψ_ref，v_ref，即获得连续轨迹f(P)；其中，

代入下列公式，通过设定参考速度v获得离散型的航点路径P′，其中P′包含N个点的状态；

P′＝[X₁，X₂，…，X_N]^T (5)

X_n＝[f₁(l_n) f₂(l_n) f₃(l_n) f₄(l_n)] (6)

l_n＝(n+1)×v×dt (7)

其中，n为取的点的标号；

S5：根据自身状态X_t和航点路径P′，通过计算模型预测代价函数J的最小值获取当前时刻的加速度a_t和当前时刻的前轮倾角δ_t，从而获得最优输出向量为当前输出向量U_t；

其中，模型预测代价函数J的计算公式如下：

其中，N为模型预测控制计算步长，w表示各个指标的权重，其中每个指标的权重可根据实际情况赋值；

S6：输出向量U_t作用于自动驾驶车辆，重复S2到S6。

2.根据权利要求1所述的自动驾驶车辆轨迹跟踪方法，其特征在于，在S3中，获取当前真实前轮倾角δ_true的方法如下：

首先，通过几何关系可得：

故：

其中R为前轮倾角为δ_true时的转弯半径，L_f为前轮和后轮之间的轴距，dψ表示两个时间间隔的航向角差，dx表示两个时间间隔x坐标差，dy表示两个时间间隔y坐标差，dt表示时间间隔，ω为车辆的角速度。