• documentation
main content

automated parking valet -凯发k8网页登录

this example shows how to construct an automated parking valet system. in this example, you learn about tools and techniques in support of path planning, trajectory generation, and vehicle control. while this example focuses on a matlab®-oriented workflow, these tools are also available in simulink®. for a simulink version of this example, see automated parking valet in simulink.

overview

automatically parking a car that is left in front of a parking lot is a challenging problem. the vehicle's automated systems are expected to take over control and steer the vehicle to an available parking spot. such a function makes use of multiple on-board sensors. for example:

  • front and side cameras for detecting lane markings, road signs (stop signs, exit markings, etc.), other vehicles, and pedestrians

  • lidar and ultrasound sensors for detecting obstacles and calculating accurate distance measurements

  • ultrasound sensors for obstacle detection

  • imu and wheel encoders for dead reckoning

on-board sensors are used to perceive the environment around the vehicle. the perceived environment includes an understanding of road markings to interpret road rules and infer drivable regions, recognition of obstacles, and detection of available parking spots.

as the vehicle sensors perceive the world, the vehicle must plan a path through the environment towards a free parking spot and execute a sequence of control actions needed to drive to it. while doing so, it must respond to dynamic changes in the environment, such as pedestrians crossing its path, and readjust its plan.

this example implements a subset of features required to implement such a system. it focuses on planning a feasible path through the environment, and executing the actions needed to traverse the path. map creation and dynamic obstacle avoidance are excluded from this example.

environment model

the environment model represents a map of the environment. for a parking valet system, this map includes available and occupied parking spots, road markings, and obstacles such as pedestrians or other vehicles. occupancy maps are a common representation for this form of environment model. such a map is typically built using simultaneous localization and mapping (slam) by integrating observations from lidar and camera sensors. this example concentrates on a simpler scenario, where a map is already provided, for example, by a vehicle-to-infrastructure (v2x) system or a camera overlooking the entire parking space. it uses a static map of a parking lot and assumes that the self-localization of the vehicle is accurate.

the parking lot example used in this example is composed of three occupancy grid layers.

  • stationary obstacles: this layer contains stationary obstacles like walls, barriers, and bounds of the parking lot.

  • road markings: this layer contains occupancy information pertaining to road markings, including road markings for parking spaces.

  • parked cars: this layer contains information about which parking spots are already occupied.

each map layer contains different kinds of obstacles that represent different levels of danger for a car navigating through it. with this structure, each layer can be handled, updated, and maintained independently.

load and display the three map layers. in each layer, dark cells represent occupied cells, and light cells represent free cells.

maplayers = loadparkinglotmaplayers;
plotmaplayers(maplayers)

for simplicity, combine the three layers into a single costmap.

costmap = combinemaplayers(maplayers);
figure
plot(costmap, 'inflation', 'off')
legend off

the combined costmap is a object, which represents the vehicle environment as a 2-d occupancy grid. each grid in the cell has values between 0 and 1, representing the cost of navigating through the cell. obstacles have a higher cost, while free space has a lower cost. a cell is considered an obstacle if its cost is higher than the occupiedthreshold property, and free if its cost is lower than the freethreshold property.

the costmap covers the entire 75m-by-50m parking lot area, divided into 0.5m-by-0.5m square cells.

costmap.mapextent % [x, width, y, height] in meters
costmap.cellsize  % cell size in meters

ans =
     0    75     0    50
ans =
    0.5000

create a object for storing the dimensions of the vehicle that will park automatically. also define the maximum steering angle of the vehicle. this value determines the limits on the turning radius during motion planning and control.

vehicledims      = vehicledimensions;
maxsteeringangle = 35; % in degrees

update the vehicledimensions property of the costmap collision checker with the dimensions of the vehicle to park. this setting adjusts the extent of the inflation in the map around obstacles to correspond to the size of the vehicle being parked, ensuring that collision-free paths can be found through the parking lot.

costmap.collisionchecker.vehicledimensions = vehicledims;

define the starting pose of the vehicle. the pose is obtained through localization, which is left out of this example for simplicity. the vehicle pose is specified as $[x,y,\theta]$, in world coordinates. $(x,y)$ represents the position of the center of the vehicle's rear axle in world coordinate system. $\theta$ represents the orientation of the vehicle with respect to world x axis. for more details, see .

currentpose = [4 12 0]; % [x, y, theta]

behavioral layer

planning involves organizing all pertinent information into hierarchical layers. each successive layer is responsible for a more fine-grained task. the behavioral layer [1] sits at the top of this stack. it is responsible for activating and managing the different parts of the mission by supplying a sequence of navigation tasks. the behavioral layer assembles information from all relevant parts of the system, including:

  • localization: the behavioral layer inspects the localization module for an estimate of the current location of the vehicle.

  • environment model: perception and sensor fusion systems report a map of the environment around the vehicle.

  • determining a parking spot: the behavioral layer analyzes the map to determine the closest available parking spot.

  • finding a global route: a routing module calculates a global route through the road network obtained either from a mapping service or from a v2x infrastructure. decomposing the global route as a series of road links allows the trajectory for each link to be planned differently. for example, the final parking maneuver requires a different speed profile than the approach to the parking spot. in a more general setting, this becomes crucial for navigating through streets that involve different speed limits, numbers of lanes, and road signs.

rather than rely on vehicle sensors to build a map of the environment, this example uses a map that comes from a smart parking lot via v2x communication. for simplicity, assume that the map is in the form of an occupancy grid, with road links and locations of available parking spots provided by v2x.

the class mimics an interface of a behavioral planning layer. the is created using the map and the global route plan. this example uses a static global route plan stored in a matlab table, but typically a routing algorithm provided by the local parking infrastructure or a mapping service determines this plan. the global route plan is described as a sequence of lane segments to traverse to reach a parking spot.

load the mat-file containing a route plan that is stored in a table. the table has three variables: startpose, endpose, and attributes. startpose and endpose specify the start and end poses of the segment, expressed as $[x,y,\theta]$. attributes specifies properties of the segment such as the speed limit.

data = load('routeplan.mat');
routeplan = data.routeplan %#ok

routeplan =
  4×3 table
      startpose           endpose         attributes
    ______________    ________________    __________
     4    12     0    56     11      0    1×1 struct
    56    11     0    70     19     90    1×1 struct
    70    19    90    70     32     90    1×1 struct
    70    32    90    53     39    180    1×1 struct

plot a vehicle at the current pose, and along each goal in the route plan.

% plot vehicle at current pose
hold on
helperplotvehicle(currentpose, vehicledims, 'displayname', 'current pose')
legend
for n = 1 : height(routeplan)
    % extract the goal waypoint
    vehiclepose = routeplan{n, 'endpose'};
    % plot the pose
    legendentry = sprintf('goal %i', n);
    helperplotvehicle(vehiclepose, vehicledims, 'displayname', legendentry);
end
hold off

create the behavioral planner helper object. the requestmaneuver method requests a stream of navigation tasks from the behavioral planner until the destination is reached.

behavioralplanner = helperbehavioralplanner(routeplan, maxsteeringangle);

the vehicle navigates each path segment using these steps:

  1. motion planning: plan a feasible path through the environment map using the optimal rapidly exploring random tree (rrt*) algorithm ().

  2. path smoothing: smooth the reference path by fitting splines to it using .

  3. trajectory generation: convert the smoothed path into a trajectory by generating a speed profile using .

  4. vehicle control: given the smoothed reference path, calculates the reference pose and velocity based on the current pose and velocity of the vehicle. provided with the reference values, computes the steering angle to control the heading of the vehicle. computes the acceleration and deceleration commands to maintain the desired vehicle velocity.

  5. goal checking: check if the vehicle has reached the final pose of the segment using .

the rest of this example describes these steps in detail, before assembling them into a complete solution.

motion planning

given a global route, motion planning can be used to plan a path through the environment to reach each intermediate waypoint, until the vehicle reaches the final destination. the planned path for each link must be feasible and collision-free. a feasible path is one that can be realized by the vehicle given the motion and dynamic constraints imposed on it. a parking valet system involves low velocities and low accelerations. this allows us to safely ignore dynamic constraints arising from inertial effects.

create a object to configure a path planner using an optimal rapidly exploring random tree (rrt*) approach. the rrt family of planning algorithms find a path by constructing a tree of connected, collision-free vehicle poses. poses are connected using dubins or reeds-shepp steering, ensuring that the generated path is kinematically feasible.

motionplanner = pathplannerrrt(costmap, 'miniterations', 1000, ...
    'connectiondistance', 10, 'minturningradius', 20);

plan a path from the current pose to the first goal by using the plan function. the returned object, refpath, is a feasible and collision-free reference path.

goalpose = routeplan{1, 'endpose'};
refpath = plan(motionplanner, currentpose, goalpose);

the reference path consists of a sequence of path segments. each path segment describes the set of dubins or reeds-shepp maneuvers used to connect to the next segment. inspect the path segments.

refpath.pathsegments

ans = 
  1×6 dubinspathsegment array with properties:
    startpose
    goalpose
    minturningradius
    motionlengths
    motiontypes
    length

the reference path contains transition poses along the way, representing points along the path corresponding to a transition from one maneuver to the next. they can also represent changes in direction, for example, from forward to reverse motion along a reeds-shepp path.

retrieve transition poses and directions from the planned path.

[transitionposes, directions] = interpolate(refpath);
% visualize the planned path.
plot(motionplanner)

in addition to the planned reference path, notice the red areas on the plot. these areas represent areas of the costmap where the origin of the vehicle (center of the rear axle) must not cross in order to avoid hitting any obstacles. finds paths that avoid obstacles by checking to ensure that vehicle poses generated do not lie on these areas.

path smoothing and trajectory generation

the reference path generated by the path planner is composed either of dubins or reeds-shepp segments. the curvature at the junctions of two such segments is not continuous and can result in abrupt changes to the steering angle. to avoid such unnatural motion and to ensure passenger comfort, the path needs to be continuously differentiable and therefore smooth [2]. one approach to smoothing a path involves fitting a parametric cubic spline. spline fitting enables you to generate a smooth path that a controller can execute.

use to fit a parametric cubic spline that passes through all the transition points in the reference path. the spline approximately matches the starting and ending directions with the starting and ending heading angle of the vehicle.

% specify number of poses to return using a separation of approximately 0.1
% m.
approxseparation = 0.1; % meters
numsmoothposes   = round(refpath.length / approxseparation);
% return discretized poses along the smooth path.
[refposes, directions, cumlengths, curvatures] = smoothpathspline(transitionposes, directions, numsmoothposes);
% plot the smoothed path.
hold on
hsmoothpath = plot(refposes(:, 1), refposes(:, 2), 'r', 'linewidth', 2, ...
    'displayname', 'smoothed path');
hold off

next, convert the generated smooth path to a trajectory that can be executed using a speed profile. compute a speed profile for each path as a sequence of three phases: accelerating to a set maximum speed, maintaining the maximum speed and decelerating to a terminal speed. the function generates such a speed profile.

specify initial, maximum, and terminal speeds so that the vehicle starts stationary, accelerates to a speed of 5 meters/second, and comes to a stop.

maxspeed   = 5; % in meters/second
startspeed = 0; % in meters/second
endspeed   = 0; % in meters/second

generate a velocity profile.

refvelocities = helpergeneratevelocityprofile(directions, cumlengths, curvatures, startspeed, endspeed, maxspeed);

refvelocities contains reference velocities for each point along the smoothed path. plot the generated velocity profile.

plotvelocityprofile(cumlengths, refvelocities, maxspeed)

vehicle control and simulation

the reference speeds, together with the smoothed path, comprise a feasible trajectory that the vehicle can follow. a feedback controller is used to follow this trajectory. the controller corrects errors in tracking the trajectory that arise from tire slippage and other sources of noise, such as inaccuracies in localization. in particular, the controller consists of two components:

  • lateral control: adjust the steering angle such that the vehicle follows the reference path.

  • longitudinal control: while following the reference path, maintain the desired speed by controlling the throttle and the brake.

since this scenario involves slow speeds, you can simplify the controller to take into account only a kinematic model. in this example, lateral control is realized by the function. the longitudinal control is realized by a helper system object™ , that computes acceleration and deceleration commands based on the proportional-integral law.

the feedback controller requires a simulator that can execute the desired controller commands using a suitable vehicle model. the class simulates such a vehicle using the following kinematic bicycle model:

$$\dot x_r = v_r*\cos(\theta) $$

$$\dot y_r = v_r*\sin(\theta) $$

$$\dot \theta = \frac{v_r}{l}*\tan(\delta) $$

$$\dot v_r = a_r $$

in the above equations, $(x_r,y_r,\theta)$ represents the vehicle pose in world coordinates. $v_r$, $a_r$, $l$, and $\delta$ represent the rear-wheel speed, rear-wheel acceleration, wheelbase, and steering angle, respectively. the position and speed of the front wheel can be obtained by:

$$ x_f = x_r l cos(\theta)$$

$$ y_f = y_r l sin(\theta)$$

$$ v_f = \frac{v_r} {cos(\delta)}$$

% close all the figures.
closefigures;
% create the vehicle simulator.
vehiclesim = helpervehiclesimulator(costmap, vehicledims);
% set the vehicle pose and velocity.
vehiclesim.setvehiclepose(currentpose);
currentvel = 0;
vehiclesim.setvehiclevelocity(currentvel);
% configure the simulator to show the trajectory.
vehiclesim.showtrajectory(true);
% hide vehicle simulation figure.
hidefigure(vehiclesim);

create a object to compute reference pose, reference velocity and driving direction for the controller.

pathanalyzer = helperpathanalyzer(refposes, refvelocities, directions, ...
    'wheelbase', vehicledims.wheelbase);

create a object to control the velocity of the vehicle and specify the sample time.

sampletime = 0.05;
loncontroller = helperlongitudinalcontroller('sampletime', sampletime);

use the object to ensure fixed-rate execution of the feedback controller. use a control rate to be consistent with the longitudinal controller.

controlrate = helperfixedrate(1/sampletime); % in hertz

until the goal is reached, do the following:

  • compute steering and acceleration/deceleration commands required to track the planned trajectory.

  • feed control commands to the simulator.

  • record the returned vehicle pose and velocity to feed into the controller in the next iteration.

reachgoal = false;
while ~reachgoal
    % find the reference pose on the path and the corresponding velocity.
    [refpose, refvel, direction] = pathanalyzer(currentpose, currentvel);
    % update driving direction for the simulator.
    updatedrivingdirection(vehiclesim, direction);
    % compute steering command.
    steeringangle = lateralcontrollerstanley(refpose, currentpose, currentvel, ...
        'direction', direction, 'wheelbase', vehicledims.wheelbase);
    % compute acceleration and deceleration commands.
    loncontroller.direction = direction;
    [accelcmd, decelcmd] = loncontroller(refvel, currentvel);
    % simulate the vehicle using the controller outputs.
    drive(vehiclesim, accelcmd, decelcmd, steeringangle);
    % check if the vehicle reaches the goal.
    reachgoal = helpergoalchecker(goalpose, currentpose, currentvel, endspeed, direction);
    % wait for fixed-rate execution.
    waitfor(controlrate);
    % get current pose and velocity of the vehicle.
    currentpose  = getvehiclepose(vehiclesim);
    currentvel   = getvehiclevelocity(vehiclesim);
end
% show vehicle simulation figure.
showfigure(vehiclesim);

this completes the first leg of the route plan and demonstrates each step of the process. the next sections run the simulator for the entire route, which takes the vehicle close to the parking spot, and finally executes a parking maneuver to place the vehicle into the parking spot.

execute a complete plan

now combine all the previous steps in the planning process and run the simulation for the complete route plan. this process involves incorporating the behavioral planner.

% set the vehicle pose back to the initial starting point.
currentpose = [4 12 0]; % [x, y, theta]
vehiclesim.setvehiclepose(currentpose);
% reset velocity.
currentvel  = 0; % meters/second
vehiclesim.setvehiclevelocity(currentvel);
while ~reacheddestination(behavioralplanner)
    % request next maneuver from behavioral layer.
    [nextgoal, plannerconfig, speedconfig] = requestmaneuver(behavioralplanner, ...
        currentpose, currentvel);
    % configure the motion planner.
    configureplanner(motionplanner, plannerconfig);
    % plan a reference path using rrt* planner to the next goal pose.
    refpath = plan(motionplanner, currentpose, nextgoal);
    % check if the path is valid. if the planner fails to compute a path,
    % or the path is not collision-free because of updates to the map, the
    % system needs to re-plan. this scenario uses a static map, so the path
    % will always be collision-free.
    isreplanneeded = ~checkpathvalidity(refpath, costmap);
    if isreplanneeded
        warning('unable to find a valid path. attempting to re-plan.')
        % request behavioral planner to re-plan
        replanneeded(behavioralplanner);
        continue;
    end
    % retrieve transition poses and directions from the planned path.
    [transitionposes, directions] = interpolate(refpath);
    % smooth the path.
    numsmoothposes   = round(refpath.length / approxseparation);
    [refposes, directions, cumlengths, curvatures] = smoothpathspline(transitionposes, directions, numsmoothposes);
    % generate a velocity profile.
    refvelocities = helpergeneratevelocityprofile(directions, cumlengths, curvatures, startspeed, endspeed, maxspeed);
    % configure path analyzer.
    pathanalyzer.refposes     = refposes;
    pathanalyzer.directions   = directions;
    pathanalyzer.velocityprofile = refvelocities;
    % reset longitudinal controller.
    reset(loncontroller);
    reachgoal = false;
    % execute control loop.
    while ~reachgoal
        % find the reference pose on the path and the corresponding
        % velocity.
        [refpose, refvel, direction] = pathanalyzer(currentpose, currentvel);
        % update driving direction for the simulator.
        updatedrivingdirection(vehiclesim, direction);
        % compute steering command.
        steeringangle = lateralcontrollerstanley(refpose, currentpose, currentvel, ...
            'direction', direction, 'wheelbase', vehicledims.wheelbase);
        % compute acceleration and deceleration commands.
        loncontroller.direction = direction;
        [accelcmd, decelcmd] = loncontroller(refvel, currentvel);
        % simulate the vehicle using the controller outputs.
        drive(vehiclesim, accelcmd, decelcmd, steeringangle);
        % check if the vehicle reaches the goal.
        reachgoal = helpergoalchecker(nextgoal, currentpose, currentvel, speedconfig.endspeed, direction);
        % wait for fixed-rate execution.
        waitfor(controlrate);
        % get current pose and velocity of the vehicle.
        currentpose  = getvehiclepose(vehiclesim);
        currentvel   = getvehiclevelocity(vehiclesim);
    end
end
% show vehicle simulation figure.
showfigure(vehiclesim);

parking maneuver

now that the vehicle is near the parking spot, a specialized parking maneuver is used to park the vehicle in the final parking spot. this maneuver requires passing through a narrow corridor flanked by the edges of the parking spot on both ends. such a maneuver is typically accompanied with ultrasound sensors or laser scanners continuously checking for obstacles.

% hide vehicle simulation figure
hidefigure(vehiclesim);

the vehiclecostmap uses inflation-based collision checking. first, visually inspect the current collision checker in use.

ccconfig = costmap.collisionchecker;
figure
plot(ccconfig)
title('current collision checker')

collision checking is performed by inflating obstacles in the costmap by the inflation radius, and checking whether the center of the circle shown above lies on an inflated grid cell. the final parking maneuver requires a more precise, less conservative collision-checking mechanism. this is commonly solved by representing the shape of the vehicle using multiple (3-5) overlapping circles instead of a single circle.

use a larger number of circles in the collision checker and visually inspect the collision checker. this allows planning through narrow passages.

ccconfig.numcircles = 4;
figure
plot(ccconfig)
title('new collision checker')

update the costmap to use this collision checker.

costmap.collisionchecker = ccconfig;

notice that the inflation radius has reduced, allowing the planner to find an unobstructed path to the parking spot.

figure
plot(costmap)
title('costmap with updated collision checker')
% set up the pathplannerrrt to use the updated costmap.
parkmotionplanner = pathplannerrrt(costmap, 'miniterations', 1000);
% define desired pose for the parking spot, returned by the v2x system.
parkpose = [36 44 90];
preparkpose = currentpose;
% compute the required parking maneuver.
refpath = plan(parkmotionplanner, preparkpose, parkpose);
% plot the resulting parking maneuver.
figure
plotparkingmaneuver(costmap, refpath, preparkpose, parkpose)

once the maneuver is found, repeat the previous process to determine a complete plan: smooth the path, generate a speed profile and follow the trajectory using the feedback controller.

% retrieve transition poses and directions from the planned path.
[transitionposes, directions] = interpolate(refpath);
% smooth the path.
numsmoothposes   = round(refpath.length / approxseparation);
[refposes, directions, cumlengths, curvatures] = smoothpathspline(transitionposes, directions, numsmoothposes);
% set up the velocity profile generator to stop at the end of the trajectory,
% with a speed limit of 5 mph.
refvelocities = helpergeneratevelocityprofile(directions, cumlengths, curvatures, currentvel, 0, 2.2352);
pathanalyzer.refposes     = refposes;
pathanalyzer.directions   = directions;
pathanalyzer.velocityprofile = refvelocities;
% reset longitudinal controller.
reset(loncontroller);
reachgoal = false;
while ~reachgoal
    % find the reference pose on the path and the corresponding velocity.
    [refpose, refvel, direction] = pathanalyzer(currentpose, currentvel);
    % update driving direction for the simulator.
    updatedrivingdirection(vehiclesim, direction);
    % compute steering command.
    steeringangle = lateralcontrollerstanley(refpose, currentpose, currentvel, ...
        'direction', direction, 'wheelbase', vehicledims.wheelbase);
    % compute acceleration and deceleration commands.
    loncontroller.direction = direction;
    [accelcmd, decelcmd] = loncontroller(refvel, currentvel);
    % simulate the vehicle using the controller outputs.
    drive(vehiclesim, accelcmd, decelcmd, steeringangle);
    % check if the vehicle reaches the goal.
    reachgoal = helpergoalchecker(parkpose, currentpose, currentvel, 0, direction);
    % wait for fixed-rate execution.
    waitfor(controlrate);
    % get current pose and velocity of the vehicle.
    currentpose  = getvehiclepose(vehiclesim);
    currentvel   = getvehiclevelocity(vehiclesim);
end
% show vehicle simulation figure.
closefigures;
showfigure(vehiclesim);

an alternative way to park the vehicle is to back into the parking spot. when the vehicle needs to back up into a spot, the motion planner needs to use the reeds-shepp connection method to search for a feasible path. the reeds-shepp connection allows for reverse motions during planning.

% specify a parking pose corresponding to a back-in parking maneuver.
parkpose = [49 47.2 -90];
% change the connection method to allow for reverse motions.
parkmotionplanner.connectionmethod = 'reeds-shepp';

to find a feasible path, the motion planner needs to be adjusted. use a larger turning radius and connection distance to allow for a smooth back-in.

parkmotionplanner.minturningradius   = 10; % meters
parkmotionplanner.connectiondistance = 15;
% reset vehicle pose and velocity.
currentvel = 0;
vehiclesim.setvehiclepose(preparkpose);
vehiclesim.setvehiclevelocity(currentvel);
% compute the parking maneuver.
replan = true;
while replan
    refpath = plan(parkmotionplanner, preparkpose, parkpose);
    % the path corresponding to the parking maneuver is small and requires
    % precise maneuvering. instead of interpolating only at transition poses,
    % interpolate more finely along the length of the path.
    numsamples = 10;
    stepsize   = refpath.length / numsamples;
    lengths    = 0 : stepsize : refpath.length;
    [transitionposes, directions] = interpolate(refpath, lengths);
    % replan if the path contains more than one direction switching poses
    % or if the path is too long.
    replan = sum(abs(diff(directions)))~=2 || refpath.length > 20;
end
% visualize the parking maneuver.
figure
plotparkingmaneuver(costmap, refpath, preparkpose, parkpose)


% smooth the path.
numsmoothposes   = round(refpath.length / approxseparation);
[refposes, directions, cumlengths, curvatures] = smoothpathspline(transitionposes, directions, numsmoothposes, 0.5);
% generate velocity profile.
refvelocities = helpergeneratevelocityprofile(directions, cumlengths, curvatures, currentvel, 0, 1);
pathanalyzer.refposes     = refposes;
pathanalyzer.directions   = directions;
pathanalyzer.velocityprofile = refvelocities;
% reset longitudinal controller.
reset(loncontroller);
reachgoal = false;
while ~reachgoal
    % get current driving direction.
    currentdir = getdrivingdirection(vehiclesim);
    % find the reference pose on the path and the corresponding velocity.
    [refpose, refvel, direction] = pathanalyzer(currentpose, currentvel);
    % if the vehicle changes driving direction, reset vehicle velocity in
    % the simulator and reset longitudinal controller.
    if currentdir ~= direction
        currentvel = 0;
        setvehiclevelocity(vehiclesim, currentvel);
        reset(loncontroller);
    end
    % update driving direction for the simulator. if the vehicle changes
    % driving direction, reset and return the current vehicle velocity as zero.
    currentvel = updatedrivingdirection(vehiclesim, direction, currentdir);
    % compute steering command.
    steeringangle = lateralcontrollerstanley(refpose, currentpose, currentvel, ...
        'direction', direction, 'wheelbase', vehicledims.wheelbase);
    % compute acceleration and deceleration commands.
    loncontroller.direction = direction;
    [accelcmd, decelcmd] = loncontroller(refvel, currentvel);
    % simulate the vehicle using the controller outputs.
    drive(vehiclesim, accelcmd, decelcmd, steeringangle);
    % check if the vehicle reaches the goal.
    reachgoal = helpergoalchecker(parkpose, currentpose, currentvel, 0, direction);
    % wait for fixed-rate execution.
    waitfor(controlrate);
    % get current pose and velocity of the vehicle.
    currentpose  = getvehiclepose(vehiclesim);
    currentvel   = getvehiclevelocity(vehiclesim);
end
% take a snapshot for the example.
closefigures;
snapnow;
% delete the simulator.
delete(vehiclesim);

conclusion

this example showed how to:

  1. plan a feasible path in a semi-structured environment, such as a parking lot, using an rrt* path planning algorithm.

  2. smooth the path using splines and generate a speed profile along the smoothed path.

  3. control the vehicle to follow the reference path at the desired speed.

  4. realize different parking behaviors by using different motion planner settings.

references

[1] buehler, martin, karl iagnemma, and sanjiv singh. the darpa urban challenge: autonomous vehicles in city traffic (1st ed.). springer publishing company, incorporated, 2009.

[2] lepetic, marko, gregor klancar, igor skrjanc, drago matko, bostjan potocnik, "time optimal path planning considering acceleration limits." robotics and autonomous systems. volume 45, issues 3-4, 2003, pp. 199-210.

supporting functions

loadparkinglotmaplayers load environment map layers for parking lot

function maplayers = loadparkinglotmaplayers()
%loadparkinglotmaplayers
%   load occupancy maps corresponding to 3 layers - obstacles, road
%   markings, and used spots.
maplayers.stationaryobstacles = imread('stationary.bmp');
maplayers.roadmarkings        = imread('road_markings.bmp');
maplayers.parkedcars          = imread('parked_cars.bmp');
end

plotmaplayers plot struct containing map layers

function plotmaplayers(maplayers)
%plotmaplayers
%   plot the multiple map layers on a figure window.
figure
cellofmaps = cellfun(@imcomplement, struct2cell(maplayers), 'uniformoutput', false);
montage( cellofmaps, 'size', [1 numel(cellofmaps)], 'border', [5 5], 'thumbnailsize', [300 nan] )
title('map layers - stationary obstacles, road markings, and parked cars')
end

combinemaplayers combine map layers into a single costmap

function costmap = combinemaplayers(maplayers)
%combinemaplayers
%   combine map layers struct into a single vehiclecostmap.
combinedmap = maplayers.stationaryobstacles   maplayers.roadmarkings   ...
    maplayers.parkedcars;
combinedmap = im2single(combinedmap);
res = 0.5; % meters
costmap = vehiclecostmap(combinedmap, 'cellsize', res);
end

configureplanner configure path planner with specified settings

function configureplanner(pathplanner, config)
%configureplanner
% configure the path planner object, pathplanner, with settings specified
% in struct config.
fieldnames = fields(config);
for n = 1 : numel(fieldnames)
    if ~strcmpi(fieldnames{n}, 'isparkmaneuver')
        pathplanner.(fieldnames{n}) = config.(fieldnames{n});
    end
end
end

plotvelocityprofile plot speed profile

function plotvelocityprofile(cumpathlength, refvelocities, maxspeed)
%plotvelocityprofile
% plot the generated velocity profile.
% plot reference velocity along length of the path.
plot(cumpathlength, refvelocities, 'linewidth', 2);
% plot a line to display maximum speed.
hold on
line([0;cumpathlength(end)], [maxspeed;maxspeed], 'color', 'r')
hold off
% set axes limits.
buffer = 2;
xlim([0 cumpathlength(end)]);
ylim([0 maxspeed   buffer])
% add labels.
xlabel('cumulative path length (m)');
ylabel('velocity (m/s)');
% add legend and title.
legend('velocity profile', 'max speed')
title('generated velocity profile')
end

closefigures

function closefigures()
% close all the figures except the simulator visualization.
% find all the figure objects.
fighandles = findobj('type', 'figure');
for i = 1: length(fighandles)
    if ~strcmp(fighandles(i).name, 'automated valet parking')
        close(fighandles(i));
    end
end
end

plotparkingmaneuver display the generated parking maneuver on a costmap.

function plotparkingmaneuver(costmap, refpath, currentpose, parkpose)
%plotparkingmaneuver
% plot the generated parking maneuver on a costmap.
% plot the costmap, without inflated areas.
plot(costmap, 'inflation', 'off')
% plot reference parking maneuver on the costmap.
hold on
plot(refpath, 'displayname', 'parking maneuver')
title('parking maneuver')
% zoom into parking maneuver by setting axes limits.
lo = min([currentpose(1:2); parkpose(1:2)]);
hi = max([currentpose(1:2); parkpose(1:2)]);
buffer = 6; % meters
xlim([lo(1)-buffer hi(1) buffer])
ylim([lo(2)-buffer hi(2) buffer])
end

see also

functions

  • | | |

objects

  • | | |

related topics

网站地图