CN110377039B - 一种车辆避障轨迹规划与跟踪控制方法 - Google Patents

Abstract

本发明属于车辆避障控制方法技术领域,公开一种车辆避障轨迹规划与跟踪控制方法，将避障过程分解为基于优化的轨迹规划和基于模型预测控制的轨迹跟踪控制两部分,以侧向加速度的三段式正弦型曲线为基础，建立了一种以时间最优，包含多种约束的轨迹优化问题，通过优化求解获得避障的最优轨迹；建立二自由度车辆控制模型，以路径跟踪性能和最优转向角为代价函数，设计基于模型预测控制思想的最优轨迹跟踪控制器，实现有效避障。

Description

一种车辆避障轨迹规划与跟踪控制方法

技术领域

本发明属于车辆避障控制方法技术领域，特别涉及一种车辆避障轨迹规划与跟踪控制方法。

背景技术

目前，汽车智能化已经成为汽车工业和车辆工程领域研究的热点，车辆避障控制技术已经受到学术界的重点关注和广泛研究。在现有的车辆避障控制技术研究中，通常采用避障路径规划与跟踪的控制方法，这也是目前车辆避障最有效的控制方案。对于避障路径的规划，通常包括人工势场法、智能优化算法等；其中人工势场法是一种虚拟力法，把车辆在周围环境中的运动视为车辆在人工建立的虚拟力场中的运动，应用人工势场法规划出来的路径一般比较平滑且安全，算法简单，实时性良好，但智能车容易陷入局部最优点；而对于智能优化算法，常采用的是模糊逻辑算法，是根据人的经验，设计出一个模糊控制规则库，将传感器获得的信息作为输入，经过模糊推理得出车辆的所需输出，但模糊规则往往是人们通过经验预先设定的，所以灵活性差，无法学习。

对于避障路径的跟踪控制，通常采用LQR方法，但是该方法未考虑预瞄前方目标路径，易出现跟踪误差较大的问题。

发明内容

为了克服上述问题，本发明提供一种车辆避障轨迹规划与跟踪控制方法，是基于优化控制思想的车辆避障轨迹规划与跟踪控制方法，采用一种具有三段式正弦型侧向加速度的曲线规划方法，通过优化求解获得避障时间最优情况下的车辆轨迹，再设计基于模型预测控制的跟踪控制器，驱动方向盘转向实现避障。

为了实现上述目的，本发明提供如下技术方案：

一种车辆避障轨迹规划与跟踪控制方法，将避障过程分解为基于优化思想的轨迹规划和基于模型预测控制思想的轨迹跟踪控制两部分，该方法具体包括以下步骤：

步骤一、基于优化思想的轨迹规划：

提出一种基于三段正弦型优化思想的避障路径规划，即采用具有加速段、匀速段和减速段的三段式正弦型侧向加速度的曲线规划，在考虑车辆侧向加速度限制及速度限制的约束下，通过优化求解获得车辆所需总转向避障时间最优情况下的车辆期望轨迹，该期望轨迹作为后续跟踪控制器的期望输入路径；

步骤二、基于模型预测控制思想的轨迹跟踪控制：

为了描述车辆的侧向和横摆运动，根据车辆的运动学及动力学关系，建立二自由度车辆动力学模型，基于该模型设计基于模型预测控制思想的轨迹跟踪控制器去跟踪步骤一中规划出的车辆期望轨迹，实现有效避障。

所述步骤一中基于优化思想的轨迹规划具体包括：

首先设定三段式正弦型侧向加速度的曲线形式：定义T₁为车辆在避障过程中加速段的时长，T₂为车辆在避障过程中匀速段的时长，T₃为车辆避障过程中减速段的时长，其中T₁＝T₃；a_yp为规划出的车辆最大侧向加速度，v_yp为规划出的车辆最大侧向速度，y_hope为期望的车辆侧向位移，t为车辆所需总转向避障时间，t＝T₁+T₂+T₃；车辆在加速段、匀速段和减速段的侧向加速度曲线公式分别为：

0，

通过对车辆侧向加速度曲线的二次积分，可以获得加速段、匀速段和减速段的车辆侧向位移曲线公式，分别为：

v_yp(t-T₁)+a_ypT₁ ²/π，

根据各阶段规划曲线在各时间点处的正弦函数幅值关系计算出车辆所需总转向避障时间为

为了使车辆所需总转向避障时间t最短，通过以车辆所需总转向避障时间t为待优化目标，以规划出的车辆最大侧向加速度a_yp和规划出的车辆最大侧向速度v_yp为待优化变量，形成如下的优化问题，并且在求解最小的车辆所需总转向避障时间t的过程中，必须满足约束条件：

其中，v_ymax和a_ymax分别为车辆控制***能够允许的最大侧向速度和侧向加速度，v_x为车辆纵向行驶速度，X_safe为预先给定的安全距离，即车辆与障碍物之间的纵向位置差，也就是车辆的中心点与障碍物对着车辆那端的端面在纵向方向之间的垂直距离；

将得到的最小车辆所需总转向避障时间t代入加速段、匀速段和减速段的车辆侧向位移曲线公式，得到车辆所需总转向避障时间t最优情况下的车辆期望轨迹。

所述建立步骤二中的二自由度车辆动力学模型过程为:

首先车辆动力学状态空间方程可描述为：

其中，符号m表示车身质量，w(t)表示横摆角速度，v_y(t)表示车辆在车身坐标系下的侧向速度，a,b分别表示质心与车轮前后轴的距离，I_z表示横摆转动惯量，F_yf,F_yr分别表示前轮和后轮的侧向轮胎力，

为v_y(t)的导数，

为w(t)的导数，α_f(t),α_r(t)分别表示轮胎前后轮的轮胎侧偏角，采用分式轮胎模型得到线性化后的车辆动力学状态空间方程为：

其中：δ_f(t)表示车辆的前轮转角，C_f，C_r分别为前后轮胎的侧偏刚度；

结合车辆运动学方程：

其中：x(t)和y(t)分别表示车辆在大地坐标系下的纵向位移和侧向位移，

为x(t)的导数，

为y(t)的导数，ψ(t)为横摆角，即车身坐标系下x轴与大地坐标系下x轴之间的夹角，v_x(t)为车辆在车身坐标系下的纵向速度；

结合上述线性化后的车辆动力学状态空间方程与车辆运动学方程，得到连续时间的四阶车辆动力学及运动学状态空间方程为:

其中

***的输出方程为:Y(t)＝CX(t)＝(0 0 1 0)X(t)

该***是以X(t)＝(v_y(t) ω(t) y(t) ψ(t))^T为状态，以方向盘转角δ(t)为输入的四阶线性***，其中G为方向盘转角与前轮转角的比值，δ(t)为方向盘转角，X(t)为状态变量，A为***的状态矩阵，B为***的输入矩阵，C为***的输出矩阵，Y(t)为***的输出；

在采样周期为T的情况下，通过零阶保持器离散化方法，将连续时间的四阶车辆动力学及运动学状态空间方程离散化，得到二自由度车辆动力学模型：

其中，k是当前时刻，k+1表示下一时刻，X(k)指车辆在k时刻的状态，Y(k)指k时刻***的输出，δ(k)为k时刻的方向盘转角，

为离散***的状态矩阵，

为离散***的输入矩阵，

为离散***的输出矩阵。

所述步骤二中基于模型预测控制思想的轨迹跟踪控制器设计包括以下步骤:

通过定义如下向量和矩阵：

可以获得P步车辆未来状态的预测输出方程：Y_p(k)＝S_xX(k)+S_uU(k)

其中，P是预测时域，N是控制时域，Y_p(k)为预测输出的车辆侧向位移序列，U(k)为控制输入，S_x为状态变量X对输出Y的系数矩阵，S_u为控制输入U(k)对输出Y_p(k)的系数矩阵；

由于在轨迹跟踪的过程中，保证跟踪路径侧向位移偏差满足要求的同时，还要限制转向控制输入，这些需求通过目标函数得以体现，因此提出优化问题：

目标函数

其中，Y(k+i)，i＝1，2，…，P为k+i时刻预测的控制输出的侧向位移序列，r_g(k+i)，i＝1，2，…，P为k+i时刻参考的侧向位移，R(k)＝(r_g(k+1)，r_g(k+2)，…，r_g(k+P))^T，δ(k+i-1)，i＝1，2，…，N为输入向量，即未来N步的方向盘转角输入，U^T(k)为向量U(k)的转置；

权重Γ_y，i≥0为第i个预测控制输出误差的加权因子，该加权因子越大，表明期望对应的跟踪路径侧向位移偏差越小，即控制输出的侧向位移越接近参考的侧向位移；权重Γ_u，i≥0为第i个控制输入的加权因子，该加权因子越大，表明期望的控制输入变化越小；

用第一项

表示对跟踪路径的侧向位移偏差的要求，即用侧向位移偏差的平方来描述路径跟踪能力，第二项

表示对执行机构即方向盘转角的限制，权重Γ_y,i，Γ_u,i用来描述两者之间的侧重或者倾向程度；

由于U(k)为使得目标函数J(k)达到最小的控制输入序列，该优化问题为无约束优化问题，通过对J(k)求偏导，再令偏导为零即可获得极值点U^*(k)，即

整理可得

通过该优化问题的求解，求出控制输入，就实现了对车辆期望轨迹的跟踪。

本发明的有益效果为：

本发明针对车辆转向避障控制问题，设计了一种基于优化控制思想的车辆避障轨迹规划与跟踪控制方法，采用一种具有三段式正弦型侧向加速度的曲线规划方法，在考虑侧向加速度限制及速度限制的约束下，通过优化求解获得避障时间最优情况下的车辆轨迹。

本发明以二自由度车辆动力学模型为依据，基于滚动优化控制思想，设计基于模型预测控制的轨迹跟踪控制器，本发明采用的模型预测控制方法具有算法设计简单，鲁棒性强，并且能够处理优化问题中的多个控制目标和多约束的特点，通过滚动寻优以及反馈校正的思想实现对期望输入的跟踪控制，驱动方向盘转向实现避障。

本发明采用的轨迹规划方案是在车辆纵向位移到达障碍物前，其侧向位移就已经超过障碍物的侧向位置，实现安全避障，该避障控制策略选取的避障轨迹是安全和高效的。

本发明引入车辆与障碍物间安全距离的概念，即车辆与障碍物之间的纵向位置差；在所规划路径的终端纵向位移小于安全距离约束情况下，又充分考虑了避障曲线对驾驶员舒适性的影响，即对侧向加速度和侧向速度的限幅约束。

附图说明

图1为本发明方法的流程图；

图2为本发明方法避障路径规划策略示意图；

图3为本发明方法规划的侧向加速度和位移曲线示意图；

图4为本发明方法规划的侧向加速度、速度和位移曲线示意图；

图5为本发明方法车辆避障规划位移曲线示意图；

图6为本发明方法基于模型预测控制的跟踪控制效果示意图；

其中：1第一路径，2第二路径，3障碍物，X_safe为预先给定的安全距离。

具体实施方式

以下结合附图详细介绍本发明的技术方案：

本发明提供一种车辆避障轨迹规划与跟踪控制方法，该方法包括以下几个步骤：

步骤一、提出了一种基于三段正弦型优化思想的避障路径规划：基于车载传感器***，当车辆检测到前方车道上有障碍物时，其采取方向盘转向避开障碍物的方式有多种，如图2中的第一路径1和第二路径2，但这两种路径对障碍物避让的效果是不同的，第一路径1是在车辆纵向位移达到障碍物前，其侧向位移已经超过障碍物的侧向位置，已实现安全避障，第二路径2是在车辆纵向位移达到障碍物后，其侧向位移才超过障碍物的侧向位置，虽也可实现避障，但从安全角度来说，避障第一路径1的效率和安全度要高；

本发明在满足规划路径的纵向位移小于安全距离的约束下，设计车辆所需总转向避障时间最优的路径曲线，以此作为后续跟踪控制器的期望输入路径；

采用三段式正弦型侧向加速度的曲线形式，即分为加速段、匀速段和减速段，且加速段和减速段设计为具有对称形式的正弦形式，通过对侧向加速度曲线的二次积分，可以获得侧向位移曲线，如图3所示；

定义T₁为车辆在避障过程中加速段的时长，T₂为车辆在避障过程中匀速段的时长，T₃为车辆避障过程中减速段的时长，其中T₁＝T₃；a_yp为规划出的车辆最大侧向加速度，v_yp为规划出的车辆最大侧向速度，y_hope为期望的车辆侧向位移，t为车辆所需总转向避障时间，t＝T₁+T₂+T₃；对于规划的车辆侧向加速度和车辆侧向位移均可用具体公式进行描述，加速段、匀速段和减速段的车辆侧向加速度曲线公式分别为：

0，

通过对侧向加速度曲线的二次积分，可以获得加速段、匀速段和减速段的车辆侧向位移曲线公式分别为：

v_yp(t-T₁)+a_ypT₁ ²/π，

根据各阶段规划曲线在各时间点处的正弦函数幅值关系计算出车辆所需总转向避障时间为：

且有式(2)成立：

通过以车辆所需总转向避障时间t为待优化目标，以规划出的车辆最大侧向加速度a_yp和规划出的车辆最大侧向速度v_yp为待优化变量，在满足避障曲线对驾驶员舒适性(即对侧向加速度和侧向速度的限幅约束)，以及规划路径终端纵向位移小于安全距离约束情况下，可以建立如下的优化问题如式(3)，即通过优化a_yp和v_yp使车辆所需总转向避障时间t最小，且在求解最小的车辆所需总转向避障时间t的过程中，必须满足约束条件s.t.：

其中，v_ymax和a_ymax分别为车辆控制***能够允许的最大侧向速度和侧向加速度，v_x为车辆纵向行驶速度，X_safe为预先给定的安全距离，即车辆与障碍物之间的纵向位置差，也就是车辆的中心点与障碍物对着车辆那端的端面在纵向方向之间的垂直距离，在本实施例的仿真实验中，预先给定的安全距离X_safe为62m；

将得到的最小车辆所需总转向避障时间t代入加速段、匀速段和减速段的车辆侧向位移曲线公式，得到车辆所需总转向避障时间最优情况下的车辆期望轨迹。

接下来就是通过模型预测控制算法跟踪上车辆期望轨迹。

假设车辆行驶在同向双车道的右车道，且以纵向速度为常值行驶。当车载传感器检测到障碍物后，开始车辆左车道转向轨迹规划，并以模型预测跟踪控制器实现转向轨迹跟踪及避障，仿真工况为车道宽。车辆纵向速度为3.5m，安全距离预先给定为62m，障碍物尺寸为长10m，宽1.75m，为安全避开此障碍物，以车辆所需总转向避障时间最短为目标规划出一条最优路径，作为后续跟踪控制器的期望输入路径。通过优化计算得出，最优避障轨迹的最大侧向加速度为1.2m/s²，最大侧向速度为1.6351m/s，转向避障的最优时间为4.2809s。

步骤二、

为了描述车辆的侧向和横摆运动，根据车辆的运动学及动力学关系，二自由度车辆控制建模：

引入二自由度自行车模型来描述车辆的动力学特性，考虑车辆前轮转角为小角度的情况下，首先车辆动力学状态空间方程可描述为：

其中，符号m表示车身质量，w(t)表示横摆角速度，v_x(t)为车辆在车身坐标系下的纵向速度，v_y(t)表示车辆在车身坐标系下的侧向速度，a,b分别表示质心与车轮前后轴的距离，I_z表示横摆转动惯量，F_yf,F_yr分别表示前轮和后轮的侧向轮胎力，

为v_y(t)的导数，α_f(t),α_r(t)分别表示轮胎前后轮的轮胎侧偏角，采用分式轮胎模型得到线性化后的车辆动力学状态空间方程为：

其中，

为v_y(t)的导数，表示侧向加速度，

为w(t)导数，表示横摆角加速度，δ_f(t)表示车辆的前轮转角，C_f，C_r分别为前后轮胎的侧偏刚度；

结合车辆的运动学方程

其中：x(t)和y(t)分别表示车辆在大地坐标系下的纵向位移和侧向位移，

为x(t)导数，表示车辆在大地坐标系下的纵向速度，

为y(t)的导数，表示车辆在大地坐标系下的侧向速度，横摆角ψ(t)表示车身坐标系下x轴与大地坐标系下x轴的夹角，当横摆角较小时，即|ψ|小于1.5度时，大地坐标系的运动学方程式(5)可描述为

结合公式(5)和公式(7)得到连续时间的四阶车辆动力学及运动学状态空间方程为:

其中

***的输出方程为:Y(t)＝CX(t)＝(0 0 1 0)X(t)＝y(t)；

***是以X(t)＝(v_y(t) ω(t) y(t) ψ(t))^T为状态，以方向盘转角δ(t)为输入的四阶线性***，其中G为方向盘转角与前轮转角的比值，C_f，C_r分别为前后轮胎的侧偏刚度，δ(t)为方向盘转角，X(t)为状态变量，A为***的状态矩阵，B为***的输入矩阵，C为***的输出矩阵，Y(t)为***的输出；

在采样周期为T的情况下，通过零阶保持器离散化方法，将连续时间的四阶车辆动力学及运动学状态空间方程离散化，得到二自由度车辆动力学模型：

其中，k是当前时刻，k+1表示下一时刻，X(k)指车辆在k时刻的状态，Y(k)指k时刻***的输出，δ(k)为k时刻的方向盘转角，

为离散***的状态矩阵，

为离散***的输入矩阵，

为离散***的输出矩阵。

基于模型预测控制思想的轨迹跟踪控制器设计：采用四阶车辆动力学及运动学的离散状态空间方程进行车辆未来状态的预测，预测时域从k+1至k+P，当超出控制时域N时控制输入为常值，进而有

δ(k+N-1)＝δ(k+N)＝δ(k+N+1)＝…＝δ(k+N-1) (10)

通过定义如下向量和矩阵：

可以获得P步车辆未来状态的预测输出方程：Y_p(k)＝S_xX(k)+S_uU(k)

其中，P是预测时域，N是控制时域，Y_p(k)为预测输出的车辆侧向位移序列，U(k)为控制输入，在本方法中即驾驶员输入的方向盘转角，S_x为状态变量X对输出Y的系数矩阵，S_u为控制输入U(k)对输出Y_p(k)的系数矩阵；

在保证跟踪路径侧向位移偏差满足要求的同时，还要限制转向控制输入，因此定义跟踪路径的侧向位移偏差及驾驶员转向动作的加权为优化目标，具体地提出优化问题：

目标函数

其中，Y(k+i),i＝1,2，…，P为k+i时刻预测的控制输出的侧向位移序列，r_g(A+i)，i＝1,2，…，P为k+i时刻参考的侧向位移,A(k)＝(r_g(k+1)，r_g(k+2)，…，r_g(k+P))^T，A(k+i-1)，i＝1,2，…，N为输入向量，即未来N步的方向盘转角输入，U^T(k)为向量U(k)的转置；

权重Γ_y，i≥0为第i个预测控制输出误差的加权因子，该加权因子越大，表明期望对应的跟踪路径侧向位移偏差越小，即控制输出的侧向位移越接近参考的侧向位移；权重Γ_u，i≥0为第i个控制输入的加权因子，该加权因子越大，表明期望的控制输入变化越小；

用第一项

表示对跟踪路径的侧向位移偏差的要求，即用侧向位移偏差的平方来描述路径跟踪能力，第二项

表示对执行机构即方向盘转角的限制，权重Γ_y,i，Γ_u,i用来描述两者之间的侧重或者倾向程度；

由于U(k)为使得目标函数J(k)达到最小的控制输入序列，该优化问题为无约束优化问题，通过对J(k)求偏导，再令偏导为零即可获得极值点U^*(k)，即

整理可得

通过求解优化问题，求出控制输入，就实现了对车辆期望轨迹的跟踪，因为在目标函数的第一项就是预测轨迹与期望轨迹的偏差。

以步骤一中规划出的最优路径作为跟踪目标，仿真结果表明，所设计的模型预测控制跟踪控制器能够较好的跟踪所规划的避障路径，侧向位移的跟踪误差最大值小于0.23m。

下面给出本方法的仿真验证，通过高保真仿真软件veDYNA进行：

(1)轨迹规划实验结果

在本方法中，对于车辆行驶前方突然出现的障碍物，采取车辆纵向速度保持不变，仅通过方向盘转向实现避障的策略，且在避障后保持与原道路平行的路径上行驶。通过优化计算，最优避障轨迹的最大侧向加速度为1.2m/s²，最大侧向速度为1.6351m/s，转向避障的最优时间为4.2809s。横向加速度、侧向速度和侧向位移曲线如图4所示，图5为整个场景中车辆、障碍物与规划轨迹的关系。

(2)轨迹跟踪实验结果

基于模型预测控制的跟踪控制效果如图6，从基于高保真仿真软件veDYNA的仿真结果可以看出，所设计的模型预测控制器能够很好地跟踪所规划的避障路径，且侧向位移的跟踪误差最大值小于0.23m。

如图1所示，基于车载传感器***来检测前方车道上的道路状况及障碍信息，根据所得的信息进行基于三段式优化思想的避障路径规划，根据二自由度车辆模型来设计基于模型预测控制的跟踪控制器，从而控制方向盘转向，进而控制车辆走向，从而避障，基于车载传感器***，将车辆走向信息反馈给基于模型预测控制的跟踪控制器。

Claims (2)

1.一种车辆避障轨迹规划与跟踪控制方法，其特征在于，将避障过程分解为基于优化思想的轨迹规划和基于模型预测控制思想的轨迹跟踪控制两部分，该方法具体包括以下步骤：

步骤一、基于优化思想的轨迹规划：

提出一种基于三段正弦型优化思想的避障路径规划，即采用具有加速段、匀速段和减速段的三段式正弦型侧向加速度的曲线规划，在考虑车辆侧向加速度限制及速度限制的约束下，通过优化求解获得车辆所需总转向避障时间最优情况下的车辆期望轨迹，该期望轨迹作为后续跟踪控制器的期望输入路径；

所述步骤一中基于优化思想的轨迹规划具体包括：

首先设定三段式正弦型侧向加速度的曲线形式：定义T₁为车辆在避障过程中加速段的时长，T₂为车辆在避障过程中匀速段的时长，T₃为车辆避障过程中减速段的时长，其中T₁＝T₃；a_yp为规划出的车辆最大侧向加速度，v_yp为规划出的车辆最大侧向速度，y_hope为期望的车辆侧向位移，t为车辆所需总转向避障时间，t＝T₁+T₂+T₃；车辆在加速段、匀速段和减速段的侧向加速度曲线公式分别为：

通过对车辆侧向加速度曲线的二次积分，可以获得加速段、匀速段和减速段的车辆侧向位移曲线公式，分别为：

v_yp(t-T₁)+a_ypT₁ ²/π，

根据各阶段规划曲线在各时间点处的正弦函数幅值关系计算出车辆所需总转向避障时间为

为了使车辆所需总转向避障时间t最短，通过以车辆所需总转向避障时间t为待优化目标，以规划出的车辆最大侧向加速度a_yp和规划出的车辆最大侧向速度v_yp为待优化变量，形成如下的优化问题，并且在求解最小的车辆所需总转向避障时间t的过程中，必须满足约束条件：

其中，v_ymax和a_ymax分别为车辆控制***能够允许的最大侧向速度和侧向加速度，v_x为车辆纵向行驶速度，X_safe为预先给定的安全距离，即车辆与障碍物之间的纵向位置差，也就是车辆的中心点与障碍物对着车辆那端的端面在纵向方向之间的垂直距离；

将得到的最小车辆所需总转向避障时间t代入加速段、匀速段和减速段的车辆侧向位移曲线公式，得到车辆所需总转向避障时间t最优情况下的车辆期望轨迹；

步骤二、基于模型预测控制思想的轨迹跟踪控制：

为了描述车辆的侧向和横摆运动，根据车辆的运动学及动力学关系，建立二自由度车辆动力学模型，基于该模型设计基于模型预测控制思想的轨迹跟踪控制器去跟踪步骤一中规划出的车辆期望轨迹，实现有效避障；

所述建立步骤二中的二自由度车辆动力学模型过程为:

首先车辆动力学状态空间方程可描述为：

其中，符号m表示车身质量，w(t)表示横摆角速度，v_y(t)表示车辆在车身坐标系下的侧向速度，a,b分别表示质心与车轮前后轴的距离，I_z表示横摆转动惯量，F_yf,F_yr分别表示前轮和后轮的侧向轮胎力，

为v_y(t)的导数，

为w(t)的导数，α_f(t),α_r(t)分别表示轮胎前后轮的轮胎侧偏角，采用分式轮胎模型得到线性化后的车辆动力学状态空间方程为：

其中：δ_f(t)表示车辆的前轮转角，C_f，C_r分别为前后轮胎的侧偏刚度；

结合车辆运动学方程：

其中：x(t)和y(t)分别表示车辆在大地坐标系下的纵向位移和侧向位移，

为x(t)的导数，

为y(t)的导数，ψ(t)为横摆角，即车身坐标系下x轴与大地坐标系下x轴之间的夹角，v_x(t)为车辆在车身坐标系下的纵向速度；

结合上述线性化后的车辆动力学状态空间方程与车辆运动学方程，得到连续时间的四阶车辆动力学及运动学状态空间方程为:

其中

***的输出方程为:Y(t)＝CX(t)＝(0 0 1 0)X(t)

该***是以X(t)＝(v_y(t) ω(t) y(t) ψ(t))^T为状态，以方向盘转角δ(t)为输入的四阶线性***，其中G为方向盘转角与前轮转角的比值，δ(t)为方向盘转角，X(t)为状态变量，A为***的状态矩阵，B为***的输入矩阵，C为***的输出矩阵，Y(t)为***的输出；

在采样周期为T的情况下，通过零阶保持器离散化方法，将连续时间的四阶车辆动力学及运动学状态空间方程离散化，得到二自由度车辆动力学模型：

其中，k是当前时刻，k+1表示下一时刻，X(k)指车辆在k时刻的状态，Y(k)指k时刻***的输出，δ(k)为k时刻的方向盘转角，

为离散***的状态矩阵，

为离散***的输入矩阵，

为离散***的输出矩阵。

2.如权利要求1所述的一种车辆避障轨迹规划与跟踪控制方法，其特征在于，所述步骤二中基于模型预测控制思想的轨迹跟踪控制器设计包括以下步骤:

通过定义如下向量和矩阵：

可以获得P步车辆未来状态的预测输出方程：Y_p(k)＝S_xX(k)+S_uU(k)

其中，P是预测时域，N是控制时域，Y_p(k)为预测输出的车辆侧向位移序列，U(k)为控制输入，S_x为状态变量X对输出Y的系数矩阵，S_u为控制输入U(k)对输出Y_p(k)的系数矩阵；

由于在轨迹跟踪的过程中，保证跟踪路径侧向位移偏差满足要求的同时，还要限制转向控制输入，这些需求通过目标函数得以体现，因此提出优化问题：

目标函数

其中，Y(k+i),i＝1,2，…，P为k+i时刻预测的控制输出的侧向位移序列，r_g(k+i),i＝1,2，…，P为k+i时刻参考的侧向位移,R(k)＝(r_g，(k+1)，r_g(k+2)，…，r_g(k+P))^T，δ(k+i-1),i＝1,2，…，N为输入向量，即未来N步的方向盘转角输入，U^T(k)为向量U(k)的转置；

权重Γ_y，i≥0为第i个预测控制输出误差的加权因子，该加权因子越大，表明期望对应的跟踪路径侧向位移偏差越小，即控制输出的侧向位移越接近参考的侧向位移；权重Γ_u，i≥0为第i个控制输入的加权因子，该加权因子越大，表明期望的控制输入变化越小；

用第一项

表示对跟踪路径的侧向位移偏差的要求，即用侧向位移偏差的平方来描述路径跟踪能力，第二项

表示对执行机构即方向盘转角的限制，权重Γ_y,i，Γ_u,i用来描述两者之间的侧重或者倾向程度；

由于U(k)为使得目标函数J(k)达到最小的控制输入序列，该优化问题为无约束优化问题，通过对J(k)求偏导，再令偏导为零即可获得极值点U^*(k)，即

整理可得

通过该优化问题的求解，求出控制输入，就实现了对车辆期望轨迹的跟踪。

Images