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

    %% EXAMPLE: Four-wheel steering discrete simulation
% Copyright 2018 The MathWorks, Inc.

%% Define vehicle
wheelRadius = 0.1;          % Wheel radius [m]
wheelDists = [0.3, 0.25];   % Front and rear wheel distances to CG [m]
vehicle = FourWheelSteering(wheelRadius,wheelDists);

%% Simulation parameters
sampleTime = 0.01;       % Sample time [s]
initPose = [0; 0; pi/4]; % Initial pose (x y theta)
ikMode = 1;     % Inverse kinematics mode:
                % 1: Front steering
                % 2: Zero sideslip
                % 3: Parallel steering

% Initialize time, input, and pose arrays
tVec = 0:sampleTime:10;         % Time array
vxRef = 0.2*ones(size(tVec));   % Reference x speed
vyRef = 0.1*ones(size(tVec));   % Reference y speed
wRef = zeros(size(tVec));       % Reference angular speed
wRef(tVec < 5) = -0.5;
wRef(tVec >=5) = 0.5;
pose = zeros(3,numel(tVec));    % Pose matrix
pose(:,1) = initPose;

%% Simulation loop
for idx = 2:numel(tVec) 
    % Solve inverse kinematics to find wheel speeds and steer angles
    if ikMode == 1
        [wheelSpeeds,steerAngles] = ...
            inverseKinematicsFrontSteer(vehicle,vxRef(idx-1),wRef(idx-1));
    elseif ikMode == 2
        [wheelSpeeds,steerAngles] = ...
            inverseKinematicsZeroSideslip(vehicle,vxRef(idx-1),wRef(idx-1));
    else
        [wheelSpeeds,steerAngles] = ...
            inverseKinematicsParallelSteer(vehicle,vxRef(idx-1),vyRef(idx-1)); 
    end
    
    % Compute the velocities
    velB = forwardKinematics(vehicle,wheelSpeeds,steerAngles);
    vel = bodyToWorld(velB,pose(:,idx-1));
    
    % Perform forward discrete integration step
    pose(:,idx) = pose(:,idx-1) + vel*sampleTime;
end

%% Display results
close all
figure
hold on
plot(pose(1,1),pose(2,1),'ro', ...
     pose(1,end),pose(2,end),'go', ...
     pose(1,:),pose(2,:),'b-');
axis equal
title('Vehicle Trajectory');
xlabel('X [m]')
ylabel('Y [m]')
legend('Start','End','Trajectory')