《无人驾驶车辆模型预测控制》3.3.3工程实例

主要针对《无人驾驶车辆模型预测控制》这本书的第三章3.3.3的工程实例进行详细的代码分析，结合自己在学习以及推导的MPC算法的一些理解。

书中源代码

以无人驾驶车辆的轨迹跟踪问题作为应用的背景，无人驾驶车辆在一个给定的位置出发，通过离散轨迹点（本工程实例使用的是离散的轨迹点）或者连续轨迹函数的指引，最终跟上期望的轨迹。
轨迹跟踪仿真基本设定：车辆从坐标原点出发，以期望纵向速度v=1m/s跟踪一条直线轨迹y=2。采样时间为0.05s（50ms），仿真的总时间设定为5s，在Matab中进行仿真。控制的目标是无人驾驶车辆在低速情况下的跟踪控制，因此考虑使用车辆的运动学方程作为预测模型。通过将车辆的运动学方程进行线性化（还有离散化的处理），得到线性时变模型。

%% 生成目标轨迹
N = 100; % 目标轨迹点的数量
T = 0.05; % 采样周期
Xout = zeros(N,3); % Xout用于存储目标轨迹点上的车辆状态；车辆共有3个状态变量：X轴坐标、Y轴坐标、航向角φ；Xout就是用来表示期望轨迹上车辆的状态
Tout = zeros(N,1); % Tout用于存储离散的时间序列
for k = 1:1:N
    Xout(k,1) = k*T; % 设置目标轨迹点的X轴坐标
    Xout(k,2) = 2; % 设置目标轨迹点的Y轴坐标；此仿真实例轨迹是一条直线y=2
    Xout(k,3) = 0; % 设置目标轨迹点的航向角;此仿真实例中由于轨迹是一条直线，所以车辆的航向角也是固定的，为0
    Tout(k,1) = (k-1)*T; % 由此可见，目标轨迹点对应的时刻，和采样时刻保持一致；整个仿真时长为100*0.05共计5秒（默认单位时长为1秒）
    % 由上可知，车辆在目标轨迹上，每两个轨迹点间的X轴的坐标相差0.05，每两个轨迹点对应的时刻相差0.05，因此车辆在X轴上的速度为1m/s
end

代码第一部分主要是根据仿真要求设定的一些基本的情况，参考轨迹后面可以自己进行修改，比如圆形轨迹或者8字形的的轨迹	，本文的后面还有对轨迹进行修改的一个测试
%% 描述系统的基本情况
Nx = 3; % 状态量共有3个：X轴坐标、Y轴坐标、航向角φ；
Nu = 2; % 控制量共有2个：车辆的纵向速度v（后轴速度），前轮偏角δ

[Nr,Nc] = size(Xout); % 获取目标轨迹点车辆状态矩阵的维度，Nr代表目标点的数目100，Nc代表车辆状态的数目3；即100行3列
Tsim = 20; % MPC的预测时域，即当车辆处于时刻k时，对k+1,k+2,k+3,...,k+Tsim时刻的车辆状态进行预测
X0 = [0 0 pi/3]; % 车辆初始状态，X轴坐标为0，Y坐标为0，航向角pi/3
L = 1; % 车辆轴距
vd1 = 1; % 车辆的目标纵向速度v=1m/s
vd2 = 0; % 车辆的目标前轮偏角δ=0

代码的第二部分也没有什么好说的，也是一些车辆基本情况和MPC预测时域的设定
%% 定义相关矩阵
% x_real矩阵，用于存储每一个仿真时刻，车辆的实际位置状态，初始状态为上面定义的X0
x_real = zeros(Nr,Nc); %生成一个用来存储每一个仿真时刻车辆实际位置的空矩阵
x_real(1,:) = X0;%将车辆初始时刻的状态加入x_real
% x_piao矩阵，用于存储每一个仿真时刻，车辆的实际位置状态与目标位置状态之间的偏差
x_piao = zeros(Nr,Nc); %同样地，生成一个用来存储每一个仿真时刻车辆的实际位置状态与目标位置状态的偏差
x_piao(1,:) = x_real(1,:)-Xout(1,:);%车辆初始时刻所在的位置状态与目标位置状态之间的偏差
% X_PIAO的每一行代表一个仿真时刻（对应一个轨迹点）
% 在每一个仿真时刻，都需要对车辆未来20（Tsim）个时刻的实际位置状态与目标位置状态的偏差进行预测，所以X_PIAO每一行的数据可以分为20（Tsim）个组
% 每一组有3（Nx）列，分别对应车辆3个状态的偏差；100行，20X3列
X_PIAO = zeros(Nr,Nx*Tsim);%生成一个用来存储在每一个仿真时刻预测未来20（Tsim）个时域车辆与目标轨迹之间的状态偏差
% u_real矩阵，用于存储每一个仿真时刻，车辆的实际控制量（实际运动状态）（纵向速度、前轮偏角）
u_real = zeros(Nr,Nu);%100行2列
% u_piao矩阵，用于存储每一个仿真时刻，车辆的实际控制量（实际运动状态）与目标控制量（运动状态）之间的偏差
u_piao = zeros(Nr,Nu);%同样100行2列
% XXX的每一行代表一个仿真时刻（对应一个轨迹点）
% 在每一个仿真时刻，都需要对未来20个时刻的状态值进行预测，所以XXX每一行的数据可以分为20（Tsim）个组
% 每一组有3（Nx）列，分别对应车辆3个状态
XXX = zeros(Nr,Nx*Tsim);% 用于保存每个时刻预测的所有状态值100行60列
% q为加权矩阵，用于调节每个状态量（X轴坐标、Y轴坐标、航向角）在目标函数中比重（权重系数）
q = [1 0 0;0 1 0;0 0 0.5];
Q_cell = cell(Tsim,Tsim); % 元组Q_cell的作用是，将q作用于预测的每一个点上（共计Tsim个点）
for i = 1:1:Tsim
    for j = 1:1:Tsim
        if i == j
            Q_cell{i,j}=q;%将加权矩阵q赋给二维元胞数组行列索引位置相等处的数组元素
        else
            Q_cell{i,j}=zeros(Nx,Nx);%其他的位置都赋给0加权矩阵
        end
    end
end
Q = cell2mat(Q_cell); % 通过cell2mat()函数将元胞数组转化成最终的状态加权矩阵Q，调节每个预测点上，三个状态量各自在目标函数中比重；20X3行20X3列
R = 0.1*eye(Nu*Tsim,Nu*Tsim); % 最终的控制量加权矩阵，用于调节控制量偏差在目标函数中的比重

代码的第三部分是对相关矩阵的定义，其中XXX表示存储在每个仿真时刻预测的未来20个时刻的状态值这部分表示的含义需要进一步理解
%% 模型预测控制的主体函数
%在每一个仿真时刻，模型预测控制是如何进行预测、滚动优化以及反馈矫校正
for i = 1:1:Nr
    t_d = Xout(i,3); % t_d为i时刻的期望航向角
    % 下一预测点的状态偏差 = a*当前点的状态偏差 + b*当前点的控制量偏差   （状态偏差，即车辆位置偏差；控制量偏差，即车辆运动偏差）
    a = [1 0 -vd1*sin(t_d)*T;   % 矩阵a，是关于目标车速和目标航向角的函数；
        0 1 vd1*cos(t_d)*T; % 由于目标车速和目标航向角保持恒定
        0 0 1;]; % 该仿真中给出的期望车速和航向都是不变的，所以，矩阵a恒定不变
    b = [cos(t_d)*T 0; % 与矩阵a相似，矩阵b也保持恒定
        sin(t_d)*T 0;
        tan(vd2)*T/L vd1*T/(L*cos(vd2)^2);];% 此处已经对书中的错误进行了更正，原书中给出的代码为：vd2*T/L vd1*T/(cos(vd2)^2);使用了近似
    % 目标函数，是预测时域内预测点与目标点的方差之和；  预测时域为Tsim，即存在20个预测点；  预测点与目标点方差之和为“状态方差（位置方差）” + “控制量方差（运动方差）”；
    A_cell = cell(Tsim,1);%A_cell为预测模型状态方差的系数矩阵
    B_cell = cell(Tsim,Tsim);B_cell为预测模型控制量方差的系数矩阵
    for j = 1:1:Tsim
         % 因为目标速度vd1、目标航向角φ保持不变，所以A(k|k)、A(k+1|k)、...A(k+Tsim|k)全都相同
        A_cell{j,1} = a^j;
        for k = 1:1:Tsim
            if k<=j
                B_cell{j,k} = (a^(j-k))*b;
            else
                B_cell{j,k} = zeros(Nx,Nu);
            end
        end
    end
    %通过cell2mat()函数将A_cell和B_cell分别转化成矩阵形式，得到预测模型的两个重要系数矩阵
    A = cell2mat(A_cell);%60行3列
    B = cell2mat(B_cell);%60行40列
    %接下来将预测模型带入到给定的目标函数（目标函数的形式是别人给出的）并通过转化为标准的二次规划问题（quadratic programming）
    %按照标准形式1/2x'Hx+f'x来求解；这里求解使用matlab中这种[x,fval,exitflag,output] = quadprog(H,f,A,b,Aeq,beq,lb,ub)形式的方法来求解
    H = 2*(B'*Q*B + R);%根据二次规划的标准型，定义相关的矩阵
    f = 2*B'*Q*A*x_piao(i,:)';
    A_cons = [];
    b_cons = [];
    %对两个控制量偏差进行约束，分别定义速度偏差和前轮转向偏差的上下限
    % 每一步， -2.2 <= v - vr <= 0.2
    % 每一步， -0.64 <= 前轮偏角 - 目标前轮偏角 <= 0.64
    ub = [0.2; 0.64];    lb = [-2.2; -0.64]; 
    % 通过二次规划求解，得到的X，为最优控制偏差矩阵[Dlt_u,Dlt_u1,Dlt_u2,Dlt_u3,...,Dlt_u19]
    [X,fval(i,1),exitflag(i,1),output(i,1)] = quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
    % 通过运动学公式，计算得到在控制偏差矩阵[Dlt_u,Dlt_u1,Dlt_u2,Dlt_u3,...,Dlt_u19]作用下的状态偏差矩阵
    % [Dlt_x_1,Dlt_x_2,Dlt_x_3,Dlt_x1_1,Dlt_x2_1,Dlt_x3_1, Dlt_x1_2,Dlt_x2_2,Dlt_x3_2, ..., Dlt_x1_19,Dlt_x2_19,Dlt_x3_19]
    % 并将其存储在 X_PIAO(i,:) 中
    X_PIAO(i,:) = (A*x_piao(i,:)'+B*X)';%1行60列
    
    % 下面的j，虽然是上面for循环的变量，但是MATLAB不会将j自动清零；所以，j的大小即为Tsim(20)
    if i+j < Nr % i代表当前时刻，j代表相对于当前时刻向前预测的Tsim时刻；此句代表预测的时刻没有超出整个仿真时长
        for k = 1:1:Tsim
            % 存储预测到的未来Tsim（20）个时域内状态值（预测值 = 在最优控制偏差下得到的状态偏差值 + 目标状态值
            XXX(i,1+3*(k-1)) = X_PIAO(i,1+3*(k-1)) + Xout(i+k,1); 
            XXX(i,2+3*(k-1)) = X_PIAO(i,2+3*(k-1)) + Xout(i+k,2);
            XXX(i,3+3*(k-1)) = X_PIAO(i,3+3*(k-1)) + Xout(i+k,3);
        end
    else
    %如果预测时域超出了整个的仿真的时长，预测的状态都是相对目标点的
        for k = 1:1:Tsim
            XXX(i,1+3*(k-1)) = X_PIAO(i,1+3*(k-1)) + Xout(Nr,1);
            XXX(i,2+3*(k-1)) = X_PIAO(i,2+3*(k-1)) + Xout(Nr,2);
            XXX(i,3+3*(k-1)) = X_PIAO(i,3+3*(k-1)) + Xout(Nr,3);
        end
    end
    %分别取最优控制序列的第一个元素，包括纵向速度和前轮转向偏角；将他们分别赋给u_piao，
    %即将这个最优解作为当前时刻车辆实际的控制量与目标控制量之间的控制量偏差
    u_piao(i,1) = X(1,1); 
    u_piao(i,2) = X(2,1); 
    
    Tvec = [0:0.05:4];%定义的这个矩阵是何意？
    x00 = x_real(i,:); % x00为当前时刻车辆的位置状态
    vd11 = vd1 + u_piao(i,1); % 依据MPC求得的最优控制偏移u_piao(i,1)，以及当前的控制量vd1，计算出下一时刻的控制量vd11
    vd22 = vd2 + u_piao(i,2); % 依据MPC求得的最优控制偏移u_piao(i,2)，以及当前的控制量vd2，计算出下一时刻的控制量vd22
    % 知道了当前时刻的位置状态、运动状态（控制量）
    % 下面根据运动学方程，求解下一时刻车辆的位置状态
    %下面是求解的微分方程
    XOUT = dsolve('Dx - vd11*cos(z) = 0',...
        'Dy - vd11*sin(z) = 0',...
        'Dz - vd22 = 0',...
        'x(0) = x00(1)',...
        'y(0) = x00(2)',...
        'z(0) = x00(3)');
    t = T; % T为采样周期
    x_real(i+1,1) = eval(XOUT.x);
    x_real(i+1,2) = eval(XOUT.y);
    x_real(i+1,3) = eval(XOUT.z);
    if (i<Nr)
        x_piao(i+1,:) = x_real(i+1,:) - Xout(i+1,:);
    end
    % 根据目标运动状态（目标速度和目标前轮转角），以及上述求得的最优控制偏差（运动量偏差）
    % 计算当前最佳的控制量（当前速度、当前转向角）
    u_real(i,1) = vd1 + u_piao(i,1); % vd1为目标控制量：目标速度
    u_real(i,2) = vd2 + u_piao(i,2); % vd1为目标控制量：目标前轮转向角
    
    figure(1); 
    plot(Xout(1:Nr,1),Xout(1:Nr,2));    
    hold on;
    plot(x_real(i,1),x_real(i,2),'r *')
    xlabel('X[m]'); 
    axis([-1 5 -1 3]);
    ylabel('Y[m]');
    hold on;
    %绘制未来Tsim时域内预测的车辆的状态（黄线）
    for k = 1:1:Tsim
        X(i,k+1) = XXX(i,1+3*(k-1));
        Y(i,k+1) = XXX(i,2+3*(k-1));
    end
    X(i,1) = x_real(i,1);
    Y(i,1) = x_real(i,2);
    plot(X(i,:),Y(i,:),'y')
    hold on    
end

第四部分的代码也是代码的主要核心的部分，需要多次反复的来阅读理解。。。最后绘制了figure(1)将车辆在MPC作用下跟踪参考轨迹的过程表现了出来
%系统状态量随时间变化
figure(2)
subplot(3,1,1);
plot(Tout(1:Nr),Xout(1:Nr,1));
hold on;
plot(Tout(1:Nr),x_real(1:Nr,1),'r');
xlabel('采样时间T');
ylabel('横向位置X')%绘制了状态量X随着时间的变化（红线是车辆实际的状态量X）
subplot(3,1,2);
plot(Tout(1:Nr),Xout(1:Nr,2));
hold on;
plot(Tout(1:Nr),x_real(1:Nr,2),'r');
xlabel('采样时间T');
ylabel('纵向位置Y')%绘制了状态量Y随时间的变化（红线是车辆实际的状态量Y）
subplot(3,1,3);
plot(Tout(1:Nr),Xout(1:Nr,3));
hold on;
plot(Tout(1:Nr),x_real(1:Nr,3),'r');
hold on;
xlabel('采样时间T');
ylabel('\theta')%绘制了状态量φ随时间的变化（红线是车辆的实际状态量φ）

第五部分的代码主要是绘制了车辆的实际状态量和目标状态量随着时间的变化
figure(3)
subplot(2,1,1);
plot(Tout(1:Nr),u_real(1:Nr,1),'r');%绘制车辆实际速度随时间变化的曲线
xlabel('采样时间T');
ylabel('纵向速度')
subplot(2,1,2)
plot(Tout(1:Nr),u_real(1:Nr,2),'r');绘制车辆前轮偏角随时间变化的曲线
xlabel('采样时间T');
ylabel('角速度')%#####这里应该不是指的角速度而是指前轮（方向盘）转角吧

第六部分的代码绘制了车辆的两个控制量（速度v和前轮转角δ）随着时间的变化曲线
figure(4)
subplot(3,1,1);
plot(Tout(1:Nr),x_piao(1:Nr,1));
xlabel('采样时间T');
ylabel('e(x)');%绘制车辆实际状态量X与参考状态量X之间的偏差曲线
subplot(3,1,2);
plot(Tout(1:Nr),x_piao(1:Nr,2));
xlabel('采样时间T');
ylabel('e(y)');%绘制车辆实际状态量Y与参考状态量Y之间的偏差曲线
subplot(3,1,3);
plot(Tout(1:Nr),x_piao(1:Nr,3));
xlabel('采样时间T');
ylabel('e(\theta)');%绘制车辆实际状态量φ与参考状态量φ之间的偏差曲线

第七部分的代码绘制了车辆当前实际的三个状态量与目标状态量之间的偏差，至此，整个的程序结束。

前言：模型预测控制这一系列的文章是根据“Model Predictive Control: Theory, Computation, and Design”这本书来写的，展现的是这本书的主要内容以及模型预测控制的基本方法。自己学习的同时也与大家一起分享。详细资料参考网址：
www.chemengr.ucsb.edu/~jbraw/mpc​www.chemengr.ucsb.edu
这里开始介绍这本书的第一章内容，这一篇主要讲绪论的introduction以及建模部分，主要的目录如下图所示，由于篇幅原因，剩下的内容将继续分几次写完。这里开始介绍这本书的第一章内容，这一篇主要讲绪论的introduction以及建模部分，主要的目录如下图所示，由于篇幅原因，剩下的内容将继续分几次写完。
1.1. Introduction
本章的主要目的是为模型预测控制(MPC)的基本要素提供一个简洁的、可理解的概述。我们介绍了确定性和随机模型、调节、状态估计、动态规划(DP)、跟踪、扰动以及一些重要的性能特性，比如闭环稳定性、对扰动的零偏移。
1.2. Models and Modeling
模型预测控制源于最优控制。MPC的基本概念是利用动态模型对系统行为进行预测，并对预测进行优化来产生当前控制运动的最佳决策。因此，模型是MPC的核心。由于最优控制动作取决于动态系统的初始状态，MPC的第二个基本概念是利用过去的测量记录来确定系统最可能的初始状态。状态估计问题是检查过去数据的记录，并将这些测量结果与模型进行协调，以确定当前状态的最可能值。调节问题和估计问题都涉及动态模型和优化。调节问题是用模型预测来产生最优控制动作，而估计问题是用测量的过去记录来产生最优状态估计。
我们首先讨论本文中使用的动态模型。我们从熟悉的微分方程模型开始：
 其中 
 是状态， 
 是输入， 
 是输出， 
 表示时间，
 表示n个实向量的集合。一般的，我们定义初始时间为零，并有相应的初始条件 
 。
1.2.1 Linear Dynamic Models
Time-varying model:最一般的线性状态空间模型
 其中， 
 是状态转移矩阵， 
 是输入矩阵， 
 为输出矩阵， 
 允许u和y之间直接耦合，在许多应用中取D=0。
Time-invariant model：当A,B,C,D都是时不变的，线性模型可以写成
 使用线性模型来近似物理系统的主要原因之一是线性模型的容易解决和分析。上式可以通过求解得到：
1.2.2 Input-Output Models
如果我们对系统的内部结构知之甚少，可以采取另一种方便的方法，即抑制状态变量，只关注可操作的输入和可测量的输出。
如图1所示，我们考虑一个系统由u与y组成，我们通常进行系统识别实验，在实验中我们操纵u和测量y，并为G建立简单的线性模型。
为了利用简单级数和反馈连接的常用框图操作，考虑信号的拉普拉斯变换而不是时间函数是很方便的。其中 
 是复值拉普拉斯变换变量，然后从数据中识别出传递函数矩阵，该框图表示如下输入与输出之间的数学关系： 
其中 
 是转移函数矩阵，状态并没有出现在这个输入-输出描述中。
1.2.3 Distributed Models
当我们考虑非空间均匀的系统时，就会出现分布式模型，这里不细讲。
1.2.4 Discrete Time Models
如果所关心的系统在离散时间采样，离散时间模型通常是更方便的。如果选择适当的采样率，样本之间的行为可以被安全地忽略，并且模型只描述样本时刻的行为。该有限线性定常离散时间模型如下所示：
一般的，为了减少符号的复杂，我们写成如下形式:
 上标+表示下一采样时刻的状态，线性离散时间模型便于在尽可能简单的数学设置中表达MPC的思想和概念。因为模型是线性的，解析解很容易推导出来。通过定义信号的z变换，得到了连续时间输入输出模型的离散时间模型： 
离散传递函数矩阵G(z)表示离散输入-输出模型： 
1.2.5 Constraints
对大多数物理系统的操纵输入(比如阀门位置、电压、力矩等)是有限制的，我们把这些约束包括在线性不等式中：
其中 
 是用来简单描述边界的，例如 
 。我们有时希望为安全、可操作性、产品质量等方面的考虑对状态或输出加以限制，这些可以表述为
 。一些实践者发现，在某些应用中限制输入的变化率更加方便，为了获得模型的状态空间形式，我们将状态增强为
 因此增强的系统模型变为
这里的 
 的具体形式为 
 变化率约束 
 ，则可以写成
 为了简化分析，在建立线性动态模型时，需要保持线性约束。如果我们想考虑一个线性系统的一般约束条件，我们选择它的形式
而当考虑非线性系统时，保持线性不等式并不能显著地简化控制器的分析，而推广了约束集: 
 。
这里需要注意，输入约束和输出或状态约束之间的一般区别。输入约束通常代表物理限制，如果控制器不遵守输入约束，物理系统就会强制执行它们。而输出或状态约束通常是可取的，它们可能无法实现，这取决于影响系统的干扰。MPC控制器的功能通常是实时确定输出或状态约束无法实现，并以某种令人满意的方式放松它们。下面也会讨论这些问题，MPC经常使用硬约束作为输入约束，使用某种形式的软约束作为输出约束或状态约束来建立优化问题。
Soft state or output constraints
先考虑一组硬输入及状态约束： 
为了软化状态约束引入松弛变量 
 ，通过如下方法松弛状态约束并且引入新约束 
 。考虑增强输入为 
 ，软状态约束可以这么表达 
 。其中
 随后人们制定了一个阶段代价惩罚来衡量人们对状态x、输入u以及对硬状态约束的违反的关注程度，这个约束是由给定的。硬状态约束被混合状态输入约束所取代。这样做的好处是，状态约束在控制问题中不会造成不可行，因为可以通过选择对状态约束进行放松；用阶段成本函数来衡量，较大的ε值可能是不可取的，但也不是不可行的。
Discrete actuators and integrality constraints
在许多工业应用中，执行器或决策变量的子集可能是整数值或离散的。一种常见的情况是，当过程中有一组类似的装置同时运行（如熔炉、加热器、冷却器、压缩机等），在这类过程中控制问题的一部分是决定在过程运行期间这些离散单元中的多少和哪些应该开或关，以满足设定值或抑制干扰。离散决策也出现在许多调度问题中，这里不再赘述。定义离散值执行器可以添加约束 
 。
1.2.6 Deterministic and Stochastic
如果检查来自任何复杂物理过程的测量，图2所示的数据波动总是存在。对于小尺度的应用，波动可能是由小分子数量的随机行为引起的。随着科学家和工程师研究纳米技术的应用，这种应用越来越普遍。当模拟一些病毒粒子或蛋白质分子与活细胞的相互作用时，这种类型的系统也出现在生命科学的应用中。在这些应用中没有确定性的模拟模型，唯一可用的系统模型是随机的。
Linear time-invariant models
在主流的经典过程控制问题中，我们通常关注的是宏观系统的建模、监控和控制。我们没有考虑由少量分子组成的系统。因此，人们自然会问，在这个领域中随机模型的动机是什么?随机模型的动机是为了解释未建模的环境(干扰)对被研究系统的影响。如果我们从任何感兴趣的过程控制系统检查测量，无论多么宏观，我们面临的物理现实是，测量仍然很像图2，如果对观测到的测量波动建模是重要的，我们转向随机模型。
数据中观测到的一些波动可分配给测量装置。这种波动源称为测量噪声。某些观测到的数据波动可归因于影响系统状态的环境的未建模干扰，表示这两种可能的扰动源的最简单的随机模型是带有附加随机变量的线性模型：
 变量v用来描述测量噪声，w描述过程扰动。

本文是《无人驾驶汽车路径规划学习路线》中的一部分。
做路径规划的前提之一是必须对动态目标进行轨迹预测。由于预测是规划的输入，所以我们必须知道预测的输出结果是什么，对应的性能指标，优缺点等等，暂时不用深入了解预测算法。
2. 这里根据百度apollo的预测模块简单介绍下轨迹预测。预测技术分为两大类。
基于模型
       Apollo使用的基于车道序列方法进行预测，这种方法根据某种原则将车道划分为若干个车道，这种方法将预测问题转为选择问题，根据动态物体的状态，选择其最大概率的车道选择序列。
基于数据
       使用RNN这类处理时间序列数据的神经网络，利用时间序列数据特征进行预测。每个MLP(多层感知)根据输入序列和前一个MLP单元的预测结果输出自己的预测结果。
	预测动态物体的运动轨迹，才能对应的做出规划决策。并且两种预测方式都要有学习能力，否则无法处理未知情况，基于模型的方法也会根据已有数据进行自动更新。
3. 这两种预测方式输出的都是离散点坐标，不是轨迹。还需要进行轨迹生成。
       轨迹生成，是预测的最后一步。
利用先验知识(如交通规则，车辆运动特性)去掉大部分不可能的轨迹，然后根据实际当前位置和预测的终点位置，用五次多项式（五次保证车道曲率连续）进行曲线拟合，效果足够好。
4. 本文非常粗略的介绍了百度apollo的预测模块，预测部分是先根据两种预测技术计算车辆的下一时刻位置的预测点，然后将这个问题转换为根据当前位置和下一时刻位置以及约束条件求解两点边值问题，百度apollo利用五次多项式（五次多项式包含了相应的约束条件）来求解（实际是曲线拟合），从而形成轨迹。
TODO：如果要深入了解预测对规划的影响的话，这里应该推荐预测技术的视频，文章，论文等全面了解下预测技术，然后着重从预测的输出结果对规划的影响进行深层次的分析。

车辆运动学模型是根据车辆转向的几何学的角度关系来描述车辆的运动的。主要关注车辆的轴长、转弯半径、和前轮转角。车辆运动学只需要用较少的车辆参数就可以基本描述车辆的运动情况，但是一般只适合在低速下有一定的应用价值。

以下内容作为自己的学习笔记，方便日后复习。参考文献

[1] Rajamani R. Vehicle dynamics and control[M]. Springer Science & Business Media, 2011

[2] 无人驾驶车辆模型预测控制，龚建伟等，2014

下面我们来试着分析和建立一下车辆的运动学模型。

低速下，车辆在平面内的运动一般关注它在全局平面坐标系下的xy坐标和航向，如下图



                                      图1 车辆运动学模型

 上三角用正弦定理表示为：



下三角用正弦定理表示为：



对式（2-1）化简得



对式（2-2）化简得





我们假设 车道半径由于低速是缓慢变化的，则车辆方向变化率dot_phi为





 

这里，忽略后轮转向，只关注前轮转向，整理运动学模型公式为：



由于是低速下，忽略质心侧偏角的影响，车轴L=l_f + l_r，有



至此，建立好车辆运动学模型。

为表达方便，我们将它表示为一下形式



f：为状态转移函数，X状态量，u控制输入量，如果把它看成一个系统，显然，它是一个非线性的系统。