gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/mrsDiffDriveNavigation.m
%% EXAMPLE: Differential Drive Path Planning and Navigation % In this example, a path is found in an occupancy grid using a probabilistic % roadmap (robotics.PRM) and followed using Pure Pursuit (robotics.PurePursuit) % % Copyright 2018-2019 The MathWorks, Inc. %% Define Vehicle R = 0.1; % Wheel radius [m] L = 0.5; % Wheelbase [m] dd = DifferentialDrive(R,L); %% Simulation parameters sampleTime = 0.1; % Sample time [s] tVec = 0:sampleTime:20; % Time array initPose = [2;2;0]; % Initial pose (x y theta) pose = zeros(3,numel(tVec)); % Pose matrix pose(:,1) = initPose; %% Path planning % Load map and inflate it by a safety distance close all load exampleMap inflate(map,R); % Create a Probabilistic Road Map (PRM) planner = robotics.PRM(map); planner.NumNodes = 75; planner.ConnectionDistance = 5; % Find a path from the start point to a specified goal point startPoint = initPose(1:2)'; goalPoint = [11, 11]; waypoints = findpath(planner,startPoint,goalPoint); show(planner) %% Pure Pursuit Controller controller = robotics.PurePursuit; controller.Waypoints = waypoints; controller.LookaheadDistance = 0.35; controller.DesiredLinearVelocity = 0.75; controller.MaxAngularVelocity = 1.5; %% Create visualizer load exampleMap % Reload original (uninflated) map for visualization viz = Visualizer2D; viz.hasWaypoints = true; viz.mapName = 'map'; %% Simulation loop 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)); [wL,wR] = inverseKinematics(dd,vRef,wRef); % Compute the velocities [v,w] = forwardKinematics(dd,wL,wR); velB = [v;0;w]; % Body velocities [vx;vy;w] 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) waitfor(r); end