Main Content

interpolate

Interpolate poses along planned vehicle path

Description

poses = interpolate(refPath) interpolates along the length of a reference path, returning transition poses. For more information, see Transition Poses.

example

poses = interpolate(refPath,lengths) interpolates poses at specified points along the length of the path. In addition to including poses corresponding to specified lengths, poses also includes the transition poses.

example

[poses,directions] = interpolate(___) also returns the motion directions of the vehicle at each pose, using inputs from any of the preceding syntaxes.

Examples

collapse all

Plan a vehicle path through a parking lot by using the optimal rapidly exploring random tree (RRT*) algorithm. Check that the path is valid, and then plot the transition poses along the path.

Load a costmap of a parking lot. Plot the costmap to see the parking lot and inflated areas for the vehicle to avoid.

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

Define start and goal poses for the vehicle as [x, y, Θ] vectors. World units for the (x,y) locations are in meters. World units for the Θ orientation angles are in degrees.

startPose = [4, 4, 90]; % [meters, meters, degrees]
goalPose = [30, 13, 0];

Use a pathPlannerRRT object to plan a path from the start pose to the goal pose.

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

Check that the path is valid.

isPathValid = checkPathValidity(refPath,costmap)
isPathValid = logical
   1

Interpolate the transition poses along the path.

transitionPoses = interpolate(refPath);

Plot the planned path and the transition poses on the costmap.

hold on
plot(refPath,'DisplayName','Planned Path')
scatter(transitionPoses(:,1),transitionPoses(:,2),[],'filled', ...
    'DisplayName','Transition Poses')
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 13 objects of type image, patch, scatter, line, polygon. These objects represent Inflated Areas, Planned Path, Transition Poses.

Plan a vehicle path through a parking lot by using the rapidly exploring random tree (RRT*) algorithm. Interpolate the poses of the vehicle at points along the path.

Load a costmap of a parking lot. Plot the costmap to see the parking lot and inflated areas for the vehicle to avoid.

data = load('parkingLotCostmap.mat');
costmap = data.parkingLotCostmap;
plot(costmap)

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

Define start and goal poses for the vehicle as [x, y, Θ] vectors. World units for the (x,y) locations are in meters. World units for the Θ orientation angles are in degrees.

startPose = [4, 4, 90]; % [meters, meters, degrees]
goalPose = [30, 13, 0]; 

Use a pathPlannerRRT object to plan a path from the start pose to the goal pose.

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);

Interpolate the vehicle poses every 1 meter along the entire path.

lengths = 0 : 1 : refPath.Length;
poses = interpolate(refPath,lengths);

Plot the interpolated poses on the costmap.

plot(costmap)
hold on
scatter(poses(:,1),poses(:,2),'DisplayName','Interpolated Poses')
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 3 objects of type image, patch, scatter. These objects represent Inflated Areas, Interpolated Poses.

Input Arguments

collapse all

Planned vehicle path, specified as a driving.Path object.

Points along the length of the path, specified as a real-valued vector. Values must be in the range from 0 to the length of the path, as determined by the Length property of refPath. The interpolate function interpolates poses at these specified points. lengths is in world units, such as meters.

Example: poses = interpolate(refPath,0:0.1:refPath.Length) interpolates poses every 0.1 meter along the entire length of the path.

Output Arguments

collapse all

Vehicle poses along the path, returned as an m-by-3 matrix of [x, y, Θ] vectors. m is the number of returned poses.

x and y specify the location of the vehicle in world units, such as meters. Θ specifies the orientation angle of the vehicle in degrees.

poses always includes the transition poses, even if you interpolate only at specified points along the path. If you do not specify the lengths input argument, then poses includes only the transition poses.

Motion directions of vehicle poses, returned as an m-by-1 vector of 1s (forward motion) and –1s (reverse motion). m is the number of returned poses. Each element of directions corresponds to a row of poses.

More About

collapse all

Extended Capabilities

expand all

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2018b