Costmap representing planning space around vehicle
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.
costmap = vehicleCostmap(
creates
a vehicle costmap using the cost values in matrix C
)C
.
costmap = vehicleCostmap(
creates a vehicle costmap from the occupancy map occMap
)occMap
.
Use of this syntax requires Navigation Toolbox™.
costmap = vehicleCostmap(___,'MapLocation',
specifies in mapLocation
)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(___,
uses Name,Value
)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
.
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 |
To simplify checking for whether a vehicle pose is in collision,
vehicleCostmap
inflates the size of obstacles. The
collision-checking algorithm follows these steps:
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 |
---|---|
|
|
Convert the inflation radius to a number of grid cells, R. Round up noninteger values of R to the next largest integer.
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 |
---|---|
|
|
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 |
---|---|
|
|