vehicleCostmap

Costmap representing planning space around vehicle

Description

The vehicleCostmap object creates a costmap that represents the planning search space around a vehicle. The costmap holds information about the environment, such as obstacles or areas that the vehicle cannot traverse. To check for collisions, the costmap inflates obstacles using the inflation radius specified in the CollisionChecker property. The costmap is used by path planning algorithms, such as pathPlannerRRT, to find collision-free paths for the vehicle to follow.

The costmap is stored as a 2-D grid of cells, often called an occupancy grid or occupancy map. Each grid cell in the costmap has a value in the range [0, 1] representing the cost of navigating through that grid cell. The state of each grid cell is free, occupied, or unknown, as determined by the FreeThreshold and OccupiedThreshold properties.

The following figure shows a costmap with sample costs and grid cell states.

Creation

Syntax

costmap = vehicleCostmap(C)

costmap = vehicleCostmap(mapWidth,mapLength)

costmap = vehicleCostmap(mapWidth,mapLength,costVal)

costmap = vehicleCostmap(occMap)

costmap = vehicleCostmap(___,'MapLocation',mapLocation)

costmap = vehicleCostmap(___,Name,Value)

Description

costmap = vehicleCostmap(C) creates a vehicle costmap using the cost values in matrix C.

costmap = vehicleCostmap(mapWidth,mapLength) creates a vehicle costmap representing an area of width mapWidth and length mapLength in world units. By default, each grid cell is in the unknown state.

costmap = vehicleCostmap(mapWidth,mapLength,costVal) also assigns a default cost, costVal, to each cell in the grid.

costmap = vehicleCostmap(occMap) creates a vehicle costmap from the occupancy map occMap. Use of this syntax requires Navigation Toolbox™.

costmap = vehicleCostmap(___,'MapLocation',mapLocation) specifies in mapLocation the bottom-left corner coordinates of the costmap. Specify 'MapLocation',mapLocation after any of the preceding inputs and in any order among the Name,Value pair arguments.

costmap = vehicleCostmap(___,Name,Value) uses Name,Value pair arguments to specify the FreeThreshold, OccupiedThreshold, CollisionChecker, and CellSize properties. For example, vehicleCostmap(C,'CollisionChecker',ccConfig) uses an inflationCollisionChecker object, ccConfig, to represent the vehicle shape and check for collisions. After you create the object, you can update all of these properties except CellSize.

Input Arguments

C — Cost values

matrix of real values in the range [0, 1]

Cost values, specified as a matrix of real values that are in the range [0, 1].

When creating a vehicleCostmap object, if you do not specify C or a uniform cost value, costVal, then the default cost value of each grid cell is (FreeThreshold + OccupiedThreshold)/2.

Data Types: single | double

mapWidth — Width of costmap

positive real scalar

Width of costmap, in world units, specified as a positive real scalar.

mapLength — Length of costmap

positive real scalar

Length of costmap, in world units, specified as a positive real scalar.

costVal — Uniform cost value

real scalar in the range [0, 1]

Uniform cost value applied to all cells in the costmap, specified as a real scalar in the range [0, 1].

When creating a vehicleCostmap object, if you do not specify costVal or a cost value matrix, C, then the default cost value of each grid cell is (FreeThreshold + OccupiedThreshold)/2.

occMap — Occupancy map

occupancyMap object | binaryOccupancyMap object

Occupancy map, specified as an occupancyMap (Navigation Toolbox) or binaryOccupancyMap (Navigation Toolbox) object. Use of this argument requires Navigation Toolbox.

mapLocation — Costmap location

[0 0] (default) | two-element real-valued vector of form [mapX mapY]

Costmap location, specified as a two-element real-valued vector of the form [mapX mapY]. This vector specifies the coordinate location of the bottom-left corner of the costmap.

Example: 'MapLocation',[8 8]

Properties

FreeThreshold — Threshold below which grid cell is free

0.2 (default) | real scalar in the range [0, 1]

Threshold below which a grid cell is free, specified as a real scalar in the range [0, 1].

A grid cell with cost c can have one of these states:

• If c < FreeThreshold, the grid cell state is free.

• If c ≥ FreeThreshold and c ≤ OccupiedThreshold, the grid cell state is unknown.

• If c > OccupiedThreshold, the grid cell state is occupied.

OccupiedThreshold — Threshold above which grid cell is occupied

0.65 (default) | real scalar in the range [0, 1]

Threshold above which a grid cell is occupied, specified as a real scalar in the range [0, 1].

A grid cell with cost c can have one of these states:

• If c < FreeThreshold, the grid cell state is free.

• If c ≥ FreeThreshold and c ≤ OccupiedThreshold, the grid cell state is unknown.

• If c > OccupiedThreshold, the grid cell state is occupied.

CollisionChecker — Collision-checking configuration

inflationCollisionChecker() (default) | InflationCollisionChecker object

Collision-checking configuration, specified as an InflationCollisionChecker object. To create this object, use the inflationCollisionChecker function. Using the properties of the InflationCollisionChecker object, you can configure:

• The inflation radius used to inflate obstacles in the costmap

• The number of circles used to enclose the vehicle when calculating the inflation radius

• The placement of each circle along the longitudinal axis of the vehicle

• The dimensions of the vehicle

By default, CollisionChecker uses the default InflationCollisionChecker object, which is created using the syntax inflationCollisionChecker(). This collision-checking configuration encloses the vehicle in one circle.

MapExtent — Extent of costmap

four-element, nonnegative integer vector of form [xmin xmax ymin ymax]

This property is read-only.

Extent of costmap around the vehicle, specified as a four-element, nonnegative integer vector of the form [xmin xmax ymin ymax].

• xmin and xmax describe the length of the map in world coordinates.

• ymin and ymax describe the width of the map in world coordinates.

CellSize — Side length of each square cell

1 (default) | positive real scalar

Side length of each square cell, in world units, specified as a positive real scalar. For example, a side length of 1 implies a grid where each cell is a square of size 1-by-1 meters. Smaller values improve the resolution of the search space at the cost of increased memory consumption.

You can specify CellSize when you create the vehicleCostmap object. However, after you create the object, CellSize becomes read-only.

MapSize — Size of costmap grid

two-element, positive integer vector of form [nrows ncols]

This property is read-only.

Size of costmap grid, specified as a two-element, positive integer vector of the form [nrows ncols].

• nrows is the number of grid cell rows in the costmap.

• ncols is the number of grid cell columns in the costmap.

Object Functions

checkFree	Check vehicle costmap for collision-free poses or points
checkOccupied	Check vehicle costmap for occupied poses or points
getCosts	Get cost value of cells in vehicle costmap
setCosts	Set cost value of cells in vehicle costmap
plot	Plot vehicle costmap

Examples

Create and Populate a Vehicle Costmap

Create a 10-by-20 meter costmap that is divided into square cells of size 0.5-by-0.5 meters. Specify a default cost value of 0.5 for all cells.

mapWidth = 10;
mapLength = 20;
costVal = 0.5;
cellSize = 0.5;

costmap = vehicleCostmap(mapWidth,mapLength,costVal,'CellSize',cellSize)

costmap = 
  vehicleCostmap with properties:

        FreeThreshold: 0.2000
    OccupiedThreshold: 0.6500
     CollisionChecker: [1x1 driving.costmap.InflationCollisionChecker]
             CellSize: 0.5000
              MapSize: [40 20]
            MapExtent: [0 10 0 20]

Mark an obstacle on the costmap. Display the costmap.

occupiedVal = 0.9;
xyPoint = [2,4]; 
setCosts(costmap,xyPoint,occupiedVal)

plot(costmap)

Mark an obstacle-free area on the costmap. Display the costmap again.

freeVal = 0.15; 
[X,Y] = meshgrid(3.5:cellSize:5,0.5:cellSize:1.5); 
setCosts(costmap,[X(:),Y(:)],freeVal)
plot(costmap)

Plan Path Using A-Star Path Planners

Plan the shortest vehicle path to a parking spot using the A* grid algorithm. Then impose nonholonomic constraints on the vehicle and replan the path using the Hybrid A* algorithm.

Create Occpancy Map

Load a costmap of a parking lot. Create an occupancyMap (Navigation Toolbox) object using the properties of the costmap object. Visualize the occupancy map.

data = load('parkingLotCostmapReducedInflation.mat');
costmapObj = data.parkingLotCostmapReducedInflation;
resolution = 1/costmapObj.CellSize;
oMap = occupancyMap(costmapObj.Costmap,resolution);
oMap.FreeThreshold = costmapObj.FreeThreshold;
oMap.OccupiedThreshold = costmapObj.OccupiedThreshold;
show(oMap)

Plan Path Using A* Grid Planner

Use the occupancy map to create a plannerAStarGrid (Navigation Toolbox) object.

gridPlanner = plannerAStarGrid(oMap);

Define the start and goal positions in world coordinate frame. The origin of this coordinate frame is at the bottom-left corner of the map.

startPos = [11,10];
goalPos  = [31.5,18];

Plan a path from the start point to the goal point in world coordinates.

path = plan(gridPlanner,startPos,goalPos,"world");

Visualize the path and the explored nodes using the show object function.

show(gridPlanner)

Impose Nonholonomic Constraints and Replan Using Hybrid A* Planner

Create a state validator object for validating planned path using collision checking. Assign the occupancy map to the state validator object.

validator = validatorOccupancyMap;
validator.Map = oMap;

Initialize a plannerHybridAStar (Navigation Toolbox) object with the state validator object. Impose the nonholonomic constraints of minimum turning radius and motion primitive length by specifying the MinTurningRadius and MotionPrimitiveLength properties of the planner.

hybridPlanner = plannerHybridAStar(validator,MinTurningRadius=4,MotionPrimitiveLength=6);

Define start and goal poses for the vehicle as [x, y, theta] vectors. x and y specify the position in meters, and theta specifies the orientation angle in radians.

startPose = [4 4 pi/2]; % [meters, meters, radians]
goalPose = [45 27 -pi/2];

Plan a path from the start pose to the goal pose.

refpath = plan(hybridPlanner,startPose,goalPose);

Visualize the path using show object function.

show(hybridPlanner)

Algorithms

To simplify checking for whether a vehicle pose is in collision, vehicleCostmap inflates the size of obstacles. The collision-checking algorithm follows these steps:

1. Calculate the inflation radius, in world units, from the vehicle dimensions. The default inflation radius is equal to the radius of the smallest set of overlapping circles required to completely enclose the vehicle. The center points of the circles lie along the longitudinal axis of the vehicle. Increasing the number of circles decreases the inflation radius, which enables more precise collision checking.

   Inflation Radius, One Center	Inflation Radius, Three Centers

2. Convert the inflation radius to a number of grid cells, R. Round up noninteger values of R to the next largest integer.

3. Inflate the size of obstacles using R. Label all cells in the inflated area as occupied.

   The diagrams show occupied cells in dark red. Cells in the inflated area are colored in light red. The solid black line shows the original inflation radius. In the diagram on the left, R is 3. In the diagram on the right, R is 2.

   Inflated Grid Cells, One Center	Inflated Grid Cells, Three Centers

4. Check whether the center points of the vehicle lie on inflated grid cells.

   • If any center point lies on an inflated grid cell, then the vehicle pose is occupied. The checkOccupied function returns true. An occupied pose does not necessarily mean a collision. For example, the vehicle might lie on an inflated grid cell but not on the grid cell that is actually occupied.

   • If no center points lie on inflated grid cells, and the cost value of each cell containing a center point is less than FreeThreshold, then the vehicle pose is free. The checkFree function returns true.

   • If no center points lie on inflated grid cells, and the cost value of any cell containing a center point is greater than FreeThreshold, then the vehicle pose is unknown. Both checkFree and checkOccupied return false.

The following poses are considered in collision because at least one center point is on an inflated area.

Pose in Collision, One Center	Pose in Collision, Three Centers

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

• occupancyMap and binaryOccupancyMap inputs are not supported.

• The collision-checking configuration stored in the CollisionChecker property must be a compile-time constant.

• The mapLocation input argument must be a compile-time constant.

Version History

Introduced in R2018a

R2020b: InflationRadius and VehicleDimensions properties have been removed

The InflationRadius and VehicleDimensions properties of the vehicleCostmap object have been removed. Follow this process instead:

1. Use the inflationCollisionChecker function to create an InflationCollisionChecker object, which has the InflationRadius and VehicleDimensions properties.

2. Specify this object as the value of the CollisionChecker property of vehicleCostmap.

If you do specify these properties for vehicleCostmap, the object returns an error.

When the vehicleCostmap object was introduced in R2018a, this object inflated obstacles based on the specified inflation radius and vehicle dimensions only. The InflationCollisionChecker object, which is specified in the CollisionChecker property of vehicleCostmap, provides additional configuration options for inflating obstacles. For example, you can specify the number of circles used to compute the inflation radius, enabling more precise collision checking.

Update Code

The table shows a typical usage of the InflationRadius and VehicleDimensions properties of vehicleCostmap. It also shows how to update your code using the corresponding properties of an InflationCollisionChecker object.

Invalid Usage	Recommended Replacement
vehicleDims = vehicleDimensions(5,2); inflationRadius = 1.2; costmap = vehicleCostmap(C, ... 'VehicleDimensions',vehicleDims, ... 'InflationRadius',inflationRadius);	vehicleDims = vehicleDimensions(5,2); inflationRadius = 1.2; ccConfig = inflationCollisionChecker(vehicleDims, ... 'InflationRadius',inflationRadius); costmap = vehicleCostmap(C, ... 'CollisionChecker',ccConfig);