Check vehicle costmap for collision-free poses or points
checkFree function checks whether vehicle poses or
points are free from obstacles on the vehicle costmap. Path planning algorithms use
checkFree to check whether candidate vehicle poses along a path
To simplify the collision check for a vehicle pose,
vehicleCostmap inflates obstacles according to the vehicle's
InflationRadius, as specified by the
CollisionChecker property of the costmap. The collision checker
calculates the inflation radius by enclosing the vehicle in a set of overlapping circles
of radius R, where the centers of these circles lie along the
longitudinal axis of the vehicle. The inflation radius is the minimum
R needed to fully enclose the vehicle in these circles.
A vehicle pose is collision-free when the following conditions apply:
None of the vehicle's circle centers lie on an inflated grid cell.
The cost value of each containing a circle center is less than the
FreeThresholdof the costmap.
For more details, see the algorithm on
vehicleCostmap reference page.
Check If Sequence of Poses Is Collision-Free
Load a costmap from a parking lot.
data = load('parkingLotCostmap.mat'); parkMap = data.parkingLotCostmap; plot(parkMap)
Create vehicle poses following a straight-line path.
y are the (x,y) coordinates of the rear axle of the vehicle.
theta is the angle of the rear axle with respect to the x-axis. Note that the dimensions of the vehicle are stored in the
CollisionChecker.VehicleDimensions property of the costmap, and that there is an offset between the rear axle of the vehicle and its center.
x = 4:0.25:6; y = 3:0.25:5; theta = repmat(45,size(x)); vehiclePoses = [x',y',theta']; hold on plot(x,y,'b.') hold off
The first few (x,y) coordinates of the rear axle are within the inflated area. However, this does not imply a collision because the center of the vehicle may be outside the inflated area. Check if the poses are collision-free.
free = checkFree(parkMap,vehiclePoses)
free = 9x1 logical array 1 1 1 1 1 1 1 1 1
All values of
free are 1 (
true), so all poses are collision-free. The center of the vehicle does not enter the inflated area at any pose.
freeMat — Costmap cell is free
Costmap cell is free, returned as a logical matrix of the same size as the
costmap grid. This size is specified by the
MapSize property of the
costmap. An element of
true) when the corresponding
costmap is unoccupied and the cost value of the
cell is below the
FreeThreshold of the costmap.
If you specify a small value of
InflationRadiusthat does not completely enclose the vehicle, then
checkFreemight report occupied poses as collision-free. To avoid this situation, the default value of
InflationRadiuscompletely encloses the vehicle.