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

    %% EXAMPLE: Four-wheel steering continuous 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);

%% Run a continuous simulation using ODE45
initPose = [0; 0; pi/4];    % Initial pose (x y theta)
ikMode = 1;     % Inverse kinematics mode
                % 1: Front steering
                % 2: Zero sideslip
                % 3: Parallel steering;
tspan = [0 10];
[t,pose] = ode45(@(t,y)fourWheelSteerDynamics(t,y,vehicle,ikMode), ...
                  tspan,initPose);
pose = pose';

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


%% Continuous dynamics
function dy = fourWheelSteerDynamics(t,y,vehicle,ikMode)   
    
    % Set desired velocities
    vxRef = 0.2;
    vyRef = 0.1;
    if t < 5
       wRef = -0.5; 
    else
       wRef = 0.5;
    end
    
    % Solve inverse kinematics to find wheel speeds and steer angles
    if ikMode == 1
        [wheelSpeeds,steerAngles] = ...
            inverseKinematicsFrontSteer(vehicle,vxRef,wRef);
    elseif ikMode == 2
        [wheelSpeeds,steerAngles] = ...
            inverseKinematicsZeroSideslip(vehicle,vxRef,wRef);
    else
        [wheelSpeeds,steerAngles] = ...
            inverseKinematicsParallelSteer(vehicle,vxRef,vyRef); 
    end
    
    % Calculate forward kinematics in world coordinates
    velB = forwardKinematics(vehicle,wheelSpeeds,steerAngles);
    dy = bodyToWorld(velB,y);
end