gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/mrsOmniwheelMPC.m

    %% EXAMPLE: Vehicle following waypoints using nonlinear Model Predictive Control (MPC)
%
% Copyright 2018 The MathWorks, Inc.

%% Define Vehicle
wheelRadius = 0.1;                      % Wheel radius [m]
robotRadius = 0.25;                     % Robot radius [m]
wheelAngles = [0, 2*pi/3, -2*pi/3];     % Wheel angles [rad]
vehicle = TripleOmniwheel(wheelRadius,robotRadius,wheelAngles);

%% Simulation parameters
sampleTime = 0.1;               % Sample time [s]
tVec = 0:sampleTime:15;         % Time array
initPose = [0;0;0];             % Initial pose (x y theta)
pose = zeros(3,numel(tVec));    % Pose matrix
pose(:,1) = initPose;

% 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]
ref = interp1(times,waypoints,tVec,'linear'); % Try other interpolation methods!

% Create visualizer
viz = Visualizer2D;
viz.hasWaypoints = true;

%% 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;

%% Simulation loop
close all
r = robotics.Rate(1/sampleTime);
u = zeros(3,numel(tVec));
wheelSpeeds = zeros(3,numel(tVec));

ref(end+1:end+p,:) = repmat(ref(end,:),[p 1]); % Extend reference by prediction horizon

for idx = 2:numel(tVec)  
    % Run the MPC controller
    [u(:,idx),~,mpcinfo] = nlmpcmove(controller,pose(:,idx-1), ...
                                     u(:,idx-1),ref(idx:idx+p-1,:));
    
    % Convert velocities to wheel speeds
    wheelSpeeds(:,idx) = inverseKinematics(vehicle,u(:,idx));
    
    % Compute the velocities
    velB = forwardKinematics(vehicle,wheelSpeeds(:,idx));
    vel = bodyToWorld(velB,pose(:,idx-1));
    
    % Perform forward discrete integration step
    pose(:,idx) = pose(:,idx-1) + vel*sampleTime; 
    
    % Update visualization, including MPC prediction overlays
    viz(pose(:,idx),waypoints)
    hold on
    if exist('hmpc','var')
        delete(hmpc)
    end
    hmpc = plot(mpcinfo.Yopt(2:end,1),mpcinfo.Yopt(2:end,2),'bs');
    hold off
    waitfor(r);
end

%% Control plots
figure
subplot(311), plot(tVec, u(1,:)), ylabel('X Vel [m/s]');
title('World Velocity Trajectory');
subplot(312), plot(tVec, u(2,:)), ylabel('Y Vel[m/s]');
subplot(313), plot(tVec, u(3,:)), ylabel('Angular Vel [rad/s]');
xlabel('Time [s]');
figure
plot(tVec, wheelSpeeds)
title('Wheel Speed Trajectory')
xlabel('Time [s]');
ylabel('Wheel Speeds [rad/s]');
legend('Wheel 1','Wheel 2','Wheel 3');


%% 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