原项目：
    控制小车在车辆变道场景下的路径跟踪控制

    1.对象参数：
    image.png
    对象参数如上所示，其中
    控制变量：前轮转向角
    被控变量：横向位置与偏航角
    即前轮转向时会导致横向位置与偏航角的改变，以此来进行路径的跟踪
    图中绿色的线条是参考轨迹，提供参考横向位置以及参考偏航角

    2.动力学模型 + 全局Y位置
    image.png、
    在本例中，纵向速度被设置为恒定的15m/s，我们仅研究车辆的横向动力学表现
    对于动力学模型的搭建，并没有特别深入的研究，如若后续要自行搭建，比较重要的是找到对应的公式+构建S-function，但Carsim中应该可以提供现成的车辆模型以供使用。现在重点还是把握模型的输入输出关系，即提供转向角后对应的车辆横向位置与偏航角。（在论文中是否需要体现出来？）

    3.控制器设计流程
    标准MPC控制图
    image.png
    （1）导入车辆模型，这里是直接使用了Matlab官方提供的车辆动力学状态空间模型和变道运行参考轨迹
    image.png
    模型参数如下所示：
    image.png
    参考轨迹导出后利用s-function或者matlab function均能制作需要的block
    更高版本的matlab中也可以直接导出simulink模块

    （2）引入控制器，这里使用的是Model Predictive Control Toolbox来生成控制器
    image.png
    输入：
    测量输出（mo）
    参考值（ref）
    测量扰动（md）此处不需要故删除

    （3）利用驾驶场景设计器，也可以生成自定义的参考轨迹，将其导出后形成用于输出参考横向位置和偏航角值的块

    最终完成的结构如图所示：
    控制器输入：参考轨迹、测量输出
    控制器输出：控制变量Steering angle
    被控对象：车辆
    对象输出：横向位置、偏航角
    image.png
    （4）设计控制器参数
    选择采样时间、确定控制变量和输出变量等
    确定默认场景参考输入输出类型
    调节控制区间、预测区间、闭环性能（由综合权重确定）
    设定输入输出约束（默认输入硬约束、输出软约束，可自行调节）
    设定权重：取决于某一因素在代价函数当中的重要程度，权重的调节是需要结合代价函数来进行考虑的
    【疑问：此控制器中没有涉及代价函数的设计，控制器1是如何生产的呢，求解过程又是怎样进行的？如果运用到更复杂的设计中要怎么办？参考线性时变模型的MPC设计教程】

    4.仿真运行结果：
    蓝线参考值，黄线输出值
    横坐标时间，纵坐标是偏移位置与弧度
    横向位置
    image.png
    偏航角
    image.png
    5.自适应MPC：需要获得一个能够自动更新的模型
    image.png
    如图所示，对象更新模型是由Matlab函数构建的

    1. function [A,B,C,D,U,Y,X,DX] = fcn(Vx,u,x)
    2. % Vx：Longitudinal Velocity
    3. % u:  Steering angle
    4. % x:  Startes
    6. % 计算状态空间矩阵--------------------------------------------------------------
    7. % Sample time
    8. Ts = 0.1;
    10. % Model parameters 模型参数
    11. m = 1575;
    12. Iz = 2875;
    13. lf = 1.2;
    14. lr = 1.6;
    15. Cf = 19000;
    16. Cr = 33000;
    18. % Continuous-time model 给定连续时间模型
    19. Ac = [-(2*Cf+2*Cr)/m/Vx, 0, -Vx-(2*Cf*lf-2*Cr*lr)/m/Vx, 0;
    20.      0, 0, 1, 0;
    21.      -(2*Cf*lf-2*Cr*lr)/Iz/Vx, 0, -(2*Cf*lf^2+2*Cr*lr^2)/Iz/Vx, 0;
    22.      1, Vx, 0, 0];
    23. Bc = [2*Cf/m 0 2*Cf*lf/Iz 0]';   
    24. Cc = [0 0 0 1; 0 1 0 0];
    25. Dc = zeros(2,1);
    27. % 计算离散模型--------------------------------------------------------------
    28. % Generate discrete-time model 生成离散时间模型
    29. nx = size(Ac,1);
    30. nu = size(Bc,2);
    31. M = expm([[Ac Bc]*Ts; zeros(nu,nx+nu)]);
    32. A = M(1:nx,1:nx);
    33. B = M(1:nx,nx+1:nx+nu);
    34. C = Cc;
    35. D = Dc;
    37. % 更新额定条件--------------------------------------------------------------
    38. % Nominal conditions for discrete-time plant 
    39. X = x;
    40. U = u;
    41. Y = C*x + D*u;
    42. DX = A*x+B*u-x;

    运行结果如下所示
    image.png
    我们甚至可以将信号源改为不断变化的正弦波，设置幅度为5，偏差为+10的正弦波信号，运行结果如下所示
    image.png
    可以看到横向位置跟踪效果良好，但偏航角效果则一般，实际偏航角变化小于参考值，说明是通过前期较快调整角度来达到在速度较快时进行路径跟踪的情形