【模糊预测控制】线性MPC低曲率道路车道保持和避障【含Matlab源码 3702期】

💥💥💥💥💥💥💥💥💞💞💞💞💞💞💞💞💞Matlab领域博客之家💞💞💞💞💞💞💞💞💞💥💥💥💥💥💥💥💥
🚀🚀🚀🚀🚀🚀🚀🚀🚀🚀🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚀🚀🚀🚀🚀🚀🚀🚀🚀🚀

🔊博主简介：985研究生，Matlab领域科研开发者；
🏫个人主页：Matlab领域
🏆代码获取方式：
CSDN Matlab领域—代码获取方式

🚅座右铭：路漫漫其修远兮，吾将上下而求索。
更多Matlab信号处理仿真内容点击👇
①Matlab信号处理（高阶版）
②付费专栏Matlab信号处理（进阶版）
③付费专栏Matlab信号处理（初级版）

⛳️关注CSDN Matlab领域，更多资源等你来！！

⛄一、无人机圆轨迹MPC控制器设计简介

1 MPC简介
模型预测控制（以下简称 MPC）是一种依赖于系统模型进行数学优化的复杂控制器。它利用优化算法计算有限时间范围内一系列的控制输入序列，并优化该序列，但控制器仅执行序列中的第一组控制输入，然后再次重复该循环。MPC 主要分为 3 个关键步骤：模型预测、滚动优化、反馈校正。

2 MPC在轨迹跟踪过程中的应用
在轨迹跟踪应用领域，通常 MPC 建模可根据机器人的控制方式选择基于运动学运动状态方程建模或者基于动力学运动状态方程建模。前者是根据车辆转向的几何学角度关系和速度位置关系来建立描述车辆的运动的预测模型，一般只适用于低速运动场景；后者是对被控对象进行综合受力分析，从受力平衡的角度建模，一般应用在高速运动场景，如汽车无人驾驶。本文基于双轮差速运动学模型，推导实现差速运动的MPC轨迹跟踪控制。 双轮差速运动学线性 MPC 轨迹跟踪实现的基本思路是状态方程–线性化–离散化–预测方程–约束线性化–非线性目标函数转为二次规划–求解最优问题。其中线性化可采用泰勒级数展开忽略高次项的方式，离散化可采用后向差分法，这两个部分都有一定的模型精度丢失。

3 作用机理
MPC作用机理描述为：在每一个采用时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻，重复上述过程：用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解 。
即MPC算法包括三个步骤：
（1）预测系统未来动态；
（2）（数值）求解开环优化问题；
（3）将优化解的第一个元素（或者说第一部分）作用于系统
这三步是在每个采样时刻重复进行的，且无论采用什么样的模型，每个采样时刻得到的测量值都作为当前时刻预测系统未来动态的初始条件
在线求解开环优化问题获得开环优化序列是MPC和传统控制方法的主要区别，因为后者通常是离线求解一个反馈控制律，并将得到的反馈控制律一直作用于系统。

⛄二、部分源代码

%% Prepare
clc
clear
close all

%% Constants
g = 9.81; % m/s^2
m = 2050; % kg
I = 3344; % kg-m^2
% mu = .3; % Slipery
rho = Inf; % m
W_line = 3; % m
x_dot_ref = 125/9; % m/s

% Geometry
a = 2;
b = 2;
c = 1.8;

C_fu = (4200-(-4200))/(4-(-4)); % ? +
C_rL = (3800-(-3800))/(5-(-5)*8); % ? +
C_ru = (2000-(-2000))/(2-(-2))*8; % ?

%% Variables
T_sampling = 0.1;
T_duration = 20;
N_horizon = 30;
R_rho = .01;
x_0 = [0 0 0 0 0]‘;
umin = [-pi/3 1]’;
umax = [pi/3 1]';
xmin = [];
xmax = [];
f_x = [
0 0 0 1 0; % e_y
0 0 0 -1 0;
0 0 0 0 1; % delta
0 0 0 0 -1;
1/x_dot_ref a/x_dot_ref 0 0 -1; % alpha_f,lim
-1/x_dot_ref -a/x_dot_ref 0 0 1;
1/x_dot_ref -b/x_dot_ref 0 0 0; % alpha_r,lim
-1/x_dot_ref b/x_dot_ref 0 0 0;
];
f_u = [];
f_u_eq = [
0 1
];
v_x = [
W_line/2; % e_y
W_line/2;
pi/3; % delta
pi/3;
pi/6; % alpha_f_lim
pi/6; % alpha_f_lim
pi/6; % alpha_r_lim
pi/6; % alpha_r_lim
];
v_u = [];
v_u_eq = [
1
];
obstacle1.duration = [5 6];
obstacle1.width = [1 -0.5];
obstacle2.duration = [15 16];
obstacle2.width = [.2 -0.5];
obstacle3.duration = [10 12];
obstacle3.width = [.2 -0.1];
Obstacles = [obstacle1 obstacle2 obstacle3];
[f_x_TV, v_x_TV, TV_x] = CreateObstacle(Obstacles, T_sampling);

showPlots = true;
Psi_r = 1/rho;

%% State Space Modeling
States = [“y_dot”, “Phi_dot”, “e_Phi”, “e_y”, “delta”];
A_c = [ (C_fu+C_ru)/(x_dot_refm) , (-x_dot_ref + (C_fua-bC_ru)/(x_dot_refm)), 0, 0, -C_fu/m ;
(aC_fu-bC_rL)/(Ix_dot_ref), (aaC_fu+bbC_rL)/(Ix_dot_ref), 0, 0, -aC_fu/I ;
0, 1, 0, 0, 0; % Constant Term
1, 0, x_dot_ref, 0, 0;
0, 0, 0, 0, 0 ];
B_c = [0 0 0 0 1; 0 0 -x_dot_refPsi_r 0 0]';
C_c = [0 0 0 1 0];
D_c = [0 0];

%% Discritize
[A, B, C, D] = DiscritizeStateSpace(A_c, B_c, C_c, D_c, T_sampling);

[X, U, Y] = MPC_DualMode(A, B, C, D, N_horizon, T_sampling, T_duration, R_rho, x_0, umin, umax, xmin, xmax, f_x, f_u, f_u_eq, f_x_TV, v_x_TV, TV_x, v_x, v_u, v_u_eq, showPlots);

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]季冬冬.MPC控制器的FPGA实现及其应用[J].吉林大学学报（信息科学版）. 2013,21(11)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除