Load the Map and Simulink Model

Load map in MATLAB workspace

load exampleMaps.mat

Enter start and goal locations

startLoc = [5 5];
goalLoc = [12 3];

The imported maps are : simpleMap, complexMap and ternaryMap.

Open the Simulink Model

open_system('pathPlanningBicycleSimulinkModel.slx')

Model Overview

The model is composed of four primary operations :

Planning

This blocks takes a start location, a goal location and map as inputs and outputs an array of wapoints which robot will follow. The planned waypoints are used downstream by the Pure pursuit controller which outputs the angular and linear velocities given the current pose of the robot and the planned waypoints as inputs.

Check if Goal is Reached

If robot has reached the goal location, the simulaion is stopped.

Controller

Controller outputs the linear velocity and angular velocity based on the waypoints and the robot's current pose. The Pure Pursuit Controller block is used for the same.

Bicycle Kinematic Model

Bicycle Kinematic Model creates a vehicle model to simulate simplified vehicle dynamics. It takes linear and angular velocities as input from the Pure Pursuit Controller block, and outputs i.e. state and stateDot which are the robot's state and the robot's state's time derivative, respectively. The robot' state is also used to calculate the the distance to goal and check if the robot has reached the goal location.

Run the Model

To simulate the model

simulation = sim('pathPlanningBicycleSimulinkModel.slx');

Visualize The Motion of Robot

To see the poses :

map = binaryOccupancyMap(simpleMap)

map = 
  binaryOccupancyMap with properties:

    GridLocationInWorld: [0 0]
           XWorldLimits: [0 27]
           YWorldLimits: [0 26]
               DataType: 'logical'
           DefaultValue: 0
             Resolution: 1
               GridSize: [26 27]
           XLocalLimits: [0 27]
           YLocalLimits: [0 26]
      GridOriginInLocal: [0 0]
     LocalOriginInWorld: [0 0]

robotPose = simulation.BicyclePose

robotPose = 304×3

    5.0000    5.0000         0
    5.0002    5.0000   -0.0002
    5.0012    5.0000   -0.0012
    5.0062    5.0000   -0.0062
    5.0313    4.9995   -0.0313
    5.1563    4.9877   -0.1569
    5.7068    4.7074   -0.7849
    5.8197    4.6015   -0.6638
    5.9427    4.5193   -0.5157
    6.6589    4.4144    0.2249
      ⋮

numRobots = size(robotPose, 2) / 3;
thetaIdx = 3;

% Translation
xyz = robotPose;
xyz(:, thetaIdx) = 0;

% Rotation in XYZ euler angles
theta = robotPose(:,thetaIdx);
thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2));
thetaEuler(:, end) = theta;

for k = 1:size(xyz, 1)
    show(map)
    hold on;
    
    % Plot Start Location
    plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
    text(startLoc(1), startLoc(2), 2, 'Start');
    
    % Plot Goal Location
    plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
    text(goalLoc(1), goalLoc(2), 2, 'Goal');
    
    % Plot Robot's XY locations
    plot(robotPose(:, 1), robotPose(:, 2), '-b')
    
    % Plot Robot's pose as it traverses the path
    quat = eul2quat(thetaEuler(k, :), 'xyz');
    plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
        'groundvehicle.stl');
    
    pause(0.01)
    hold off;
end

© Copyright 2019 The MathWorks, Inc.