MPC控制器学习-NMPC

趁着刚讲明白马上记录一下，不然以后又忘了_(:з」∠)_是一位老师给的现成的mpc小项目，代码写的很仔细能够帮助理解mpc的原理。
场景是一个二维平面的小车(看成一个质点)，要运动到目标点，途中避开圆形范围的障碍物。
小车的矩形边框可以无视，只用作画图（方便展示旋转角度）。
MPC可以看做是一个算法，大概就是仿真系统运行K步，然后取第1步作为真实要走的步数。仿真通过常微分方程来计算，比如要仿真下一步step[k+1]=step[k]+ODE
第一步，因为有了ODE，又有了离散化的时间，我们就可以仿真出K步的位置。
第二步，设计目标函数。目标函数要最小，比如在这个例子中，就是距离goal的距离最短。一般是把K的距离都加起来再乘以一些参数啥的作为目标函数。
第三步，设置限制条件。比如速度不能超过多少，角速度不能超过多少，以及距离障碍物的距离要大于的半径等等。
这三样东西都有了呢，我们就可以把它们全部送进优化器里面，优化器会计算出来，优化的结果就放在state_predict数组里面，至于这个感觉还要研究一下语法的问题。

这个例子最后的效果如图：

%% Code for Nonlinear Model Predictive Control
% Status of cars and Navigation horizon can be modified in section
% "Set Robot Parameters" and "Set System Parameters".
% YALMIP required. Visit _yalmip.github.io_ for more information

%% ============= Initialization =============
yalmip('clear'); clear ; close all; clc;

%% ============= Render Obstacles =============
% Set number and size of obstacles
choice_size = 4;   
choice_num = 1;
horizon = 15; % 仿真步数 15步

if choice_num ~= 0
    obs = obs_render(choice_num,choice_size);
end

%% ============= Set Robot Parameters =============
% Initialize robot status
robot = struct(...
    'length',4,...          % Length of the robot 画图用 后面三个点是换行用的，这样写好看
    'width',1.8,...         % Width of the robot 画图用
    'x_init',[-15;-15;0],...% Starting point of the robot 没用到
    'x_curr',[-15;-15;0],...% Current point of the robot 理论上初始化：x_curr=x_init，初始坐标和角度θ
    'x_goal',[15;15;0],...  % Goal point of the robot 目标坐标和角度θ
    'sample_freq',0.2,...   % Sampling frequency of the robot 步长，也可以说是时间间隔δt
    'flag',0,...            % Denote whether the robot has reached the goal point 标记是否到达目标
    'state_predict',0,...   % Predict sequence of state 预测的状态，里面应该存的是方程
    'control_predict',0);   % Predict sequence of control 控制的状态，里面应该存的是方程

%%