gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/mrsOmniwheelMPCInit.m
%% Initialization script for nonlinear MPC example % % Copyright 2018 The MathWorks, Inc. %% Sample time sampleTime = 0.1; %% Vehicle Parameters wheelRadius = 0.1; robotRadius = 0.25; wheelAngles = [0, 2*pi/3, -2*pi/3]; %% Define waypoints, times, and trajectory times = [0; 3; 6; 9; 12; 15]; waypoints = [0,0,0; 2,2,pi/4; 4,2,pi/2; 2,4,pi/2; 0.5,3,-pi/2; 0.5,3,-pi/2]; % [x y theta] %% Create the nonlinear MPC Controller nx = 3; % Number of states ny = 3; % Number of outputs nu = 3; % Number of inputs controller = nlmpc(nx,ny,nu); p = 5; % Prediction horizon m = 3; % Control horizon controller.Ts = sampleTime; controller.PredictionHorizon = p; controller.ControlHorizon = m; % Add dynamic model for nonlinear MPC controller.Model.StateFcn = @(xk,u)discreteDynamics(xk,u,sampleTime); controller.Model.IsContinuousTime = false; % Add weights controller.Weights.ManipulatedVariables = [1, 1, 1]; controller.Weights.ManipulatedVariablesRate = [10, 10, 10]; controller.Weights.OutputVariables = [100, 100, 50]; % Add constraints % X velocity controller.MV(1).Max = 0.9; controller.MV(1).RateMax = 0.2; controller.MV(1).Min = -0.9; controller.MV(1).RateMin = -0.2; % Y velocity controller.MV(2).Max = 0.9; controller.MV(2).RateMax = 0.2; controller.MV(2).Min = -0.9; controller.MV(2).RateMin = -0.2; % Z velocity controller.MV(3).Max = pi/2; controller.MV(3).RateMax = pi/4; controller.MV(3).Min = -pi/2; controller.MV(3).RateMin = -pi/4; %% Helper functions function xk1 = discreteDynamics(xk,u,dt) % x[k+1] = x[k] + B*u[k]*dt % Where: x[k] = [x;y;theta], u[k] = [vx;vy;w] theta = xk(3); M = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0 , 0, 1]; xk1 = xk + M*u*dt; end