www.gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/mrsFourWheelPurePursuit.m
%% EXAMPLE: Four-wheel vehicle with zero sideslip steering % (equal and opposite front and rear steering), following waypoints % using the Pure Pursuit algorithm (robotics.PurePursuit) % % Copyright 2018 The MathWorks, Inc. %% Define Vehicle wheelRadius = 0.1; % Wheel radius [m] frontLen = 0.25; % Distance from CG to front wheels [m] rearLen = 0.2; % Distance from CG to rear wheels [m] vehicle = FourWheelSteering(wheelRadius,[frontLen rearLen]); %% Simulation parameters sampleTime = 0.05; % 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 waypoints = [0,0; 2,2; 4,2; 2,4; 0.5,3]; % Create visualizer viz = Visualizer2D; viz.hasWaypoints = true; % Create arrays wArray = zeros(2,numel(tVec)-1); % Wheel speeds phiArray = zeros(2,numel(tVec)-1); % Steer angles %% Pure Pursuit Controller controller = robotics.PurePursuit; controller.Waypoints = waypoints; controller.LookaheadDistance = 0.3; controller.DesiredLinearVelocity = 0.75; controller.MaxAngularVelocity = 0.75; %% Simulation loop close all r = robotics.Rate(1/sampleTime); for idx = 2:numel(tVec) % Run the Pure Pursuit controller and convert output to wheel speeds [vRef,wRef] = controller(pose(:,idx-1)); [wheelSpeeds,steerAngles] = ... inverseKinematicsZeroSideslip(vehicle,vRef,wRef); %inverseKinematicsFrontSteer(vehicle,vRef,wRef); % Compute the velocities velB = forwardKinematics(vehicle,wheelSpeeds,steerAngles); vel = bodyToWorld(velB,pose(:,idx-1)); % Convert from body to world % Perform forward discrete integration step pose(:,idx) = pose(:,idx-1) + vel*sampleTime; % Update visualization viz(pose(:,idx),waypoints) % Update control input arrays wArray(:,idx-1) = wheelSpeeds; phiArray(:,idx-1) = steerAngles; waitfor(r); end %% Visualize the control inputs figure subplot(2,1,1) plot(tVec(1:end-1),wArray(1,:)*30/pi, ... tVec(1:end-1),wArray(2,:)*30/pi); title('Control Inputs') xlabel('Time [s]'), ylabel('Wheel Speeds [rpm]') legend('Front','Rear') subplot(2,1,2) plot(tVec(1:end-1),phiArray(1,:)*180/pi, ... tVec(1:end-1),phiArray(2,:)*180/pi); xlabel('Time [s]'), ylabel('Steer Angle [deg]') legend('Front','Rear')