# Notes-of-MPC-in-Self-Driving-Car

**Repository Path**: dashenyjn123/Notes-of-MPC-in-Self-Driving-Car

## Basic Information

- **Project Name**: Notes-of-MPC-in-Self-Driving-Car
- **Description**: 读书笔记《无人驾驶车辆模型预测控制》- 龚建伟
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 3
- **Created**: 2021-05-07
- **Last Updated**: 2021-05-07

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Self-Driving-Car-MPC-01-MPC-Basic
《无人驾驶车辆模型预测控制》- 龚建伟 - 第三章 - 模型预测控制算法基础与仿真分析

### chapter3-3-3.m

- 参考轨迹生成
  - Xout
    - 第一列为每个时刻`T, 2T, ..., nT`
    - 第二列为参考轨迹`y = 2`
    
- 控制系统基本情况
  - 3个状态量:`x速度，y速度，角速度`
  - 2个控制量:`线速度、角速度`
  
- 矩阵定义
  - `x_real` 记录车辆的观测坐标
    - `x_real`第1行填初始位置 `x_real(1,:)=X0;`（`X0 = [0 0 pi/3];`）
  - `x_piao` 记录车辆当前坐标与参考坐标的差
    - `x_piao(1,:)=x_real(1,:)-Xout(1,:);`
  - `u_real=zeros(Nr,2); u_piao=zeros(Nr,2)` 
  
- MPC主体
  - 每一个周期`for i=1:1:Nr`
    - 获取系统当前状态量、更新状态空间方程
      - 用系统当前状态（朝向）更新矩阵a、b
        ```matlab
        t_d =Xout(i,3);
        a=[1    0   -vd1*sin(t_d)*T;
           0    1   vd1*cos(t_d)*T;
           0    0   1;];
        b=[cos(t_d)*T   0;
           sin(t_d)*T   0;
           0            T;];    
        ```
    - 求解一个标准二次规划问题
      - Reference: 
        - [quadprog](https://www.mathworks.com/help/optim/ug/quadprog.html?s_tid=mwa_osa_a#d120e113143)
        - [Model Predictive Control of a Mobile Robot Using Linearization](http://www.ece.ufrgs.br/~fetter/mechrob04_553.pdf)
      - 解二次规划问题 `x = quadprog(H,f,A,b,Aeq,beq,lb,ub)`
        ```matlab
        H=2*(B'*Q*B+R);
        f=2*B'*Q*A*x_piao(i,:)';
        [X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
        ```
      - `XXX`保存每一时刻预测的所有状态值
        ```matlab
        X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';
        if i+j<Nr
           for j=1:1:Tsim
               XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
               XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
               XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
           end
        else
           for j=1:1:Tsim
               XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
               XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
               XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
           end
        end
        ```
    - 将第一个解施加在系统上
      - `u_piao`保存解出的第一个u
        ```matlab
        u_piao(i,1)=X(1,1);
        u_piao(i,2)=X(2,1);
        ```
      - 将第一个解施加在系统上
        ```matlab
        u_real(i,1)=vd1+u_piao(i,1);
        u_real(i,2)=vd2+u_piao(i,2);
        ```
      - 用解得的控制量更新下一时刻状态
        ```matlab
        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; 
        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
        ```