waypointTrajectory
Waypoint trajectory generator
Description
The waypointTrajectory
System object™ generates trajectories using specified waypoints. When you create the System object, you can optionally specify the time of arrival, velocity, and orientation at
each waypoint.
To generate a trajectory from waypoints:
Create the
waypointTrajectoryobject and set its properties.Call the object as if it were a function.
To learn more about how System objects work, see What Are System Objects?.
Creation
Syntax
Description
trajectory = waypointTrajectorytrajectory, that generates a trajectory based on default
stationary waypoints.
trajectory = waypointTrajectory(Waypoints,TimeOfArrival)Waypoints that the generated trajectory passes through
and the TimeOfArrival at each waypoint.
trajectory = waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)Name to the specified
Value. Unspecified properties and creation arguments have default or
inferred values.
trajectory =
waypointTrajectory([10,10,0;20,20,0;20,20,10],[0,0.5,10]) creates a waypoint
trajectory System object, trajectory, that starts at waypoint
[10,10,0], and then passes through [20,20,0] after
0.5 seconds and [20,20,10] after 10 seconds.Creation Arguments
Creation arguments are properties which are set during creation of the System object and cannot be modified later. If you do not explicitly set a creation argument value, the property value is inferred.
If you specify any creation argument, then you must specify both the Waypoints and TimeOfArrival creation
arguments. You can specify Waypoints and
TimeOfArrival as value-only arguments or name-value pairs.
Properties
Usage
Description
[
outputs a frame of trajectory data based on specified creation arguments and
properties.position,orientation,velocity,acceleration,angularVelocity] = trajectory()
Output Arguments
Object Functions
To use an object function, specify the
System object as the first input argument. For
example, to release system resources of a System object named obj, use
this syntax:
release(obj)
Examples
Create Default waypointTrajectory
trajectory = waypointTrajectory
trajectory =
waypointTrajectory with properties:
SampleRate: 100
SamplesPerFrame: 1
Waypoints: [2x3 double]
TimeOfArrival: [2x1 double]
Velocities: [2x3 double]
Course: [2x1 double]
GroundSpeed: [2x1 double]
ClimbRate: [2x1 double]
Orientation: [2x1 quaternion]
AutoPitch: 0
AutoBank: 0
ReferenceFrame: 'NED'
Inspect the default waypoints and times of arrival by calling waypointInfo. By default, the waypoints indicate a stationary position for one second.
waypointInfo(trajectory)
ans=2×2 table
TimeOfArrival Waypoints
_____________ ___________
0 0 0 0
1 0 0 0
Create Square Trajectory
Create a square trajectory and examine the relationship between waypoint constraints, sample rate, and the generated trajectory.
Create a square trajectory by defining the vertices of the square. Define the orientation at each waypoint as pointing in the direction of motion. Specify a 1 Hz sample rate and use the default SamplesPerFrame of 1.
waypoints = [0,0,0; ... % Initial position 0,1,0; ... 1,1,0; ... 1,0,0; ... 0,0,0]; % Final position toa = 0:4; % time of arrival orientation = quaternion([0,0,0; ... 45,0,0; ... 135,0,0; ... 225,0,0; ... 0,0,0], ... 'eulerd','ZYX','frame'); trajectory = waypointTrajectory(waypoints, ... 'TimeOfArrival',toa, ... 'Orientation',orientation, ... 'SampleRate',1);
Create a figure and plot the initial position of the platform.
figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axis square xlabel('X') ylabel('Y') grid on hold on
In a loop, step through the trajectory to output the current position and current orientation. Plot the current position and log the orientation. Use pause to mimic real-time processing.
orientationLog = zeros(toa(end)*trajectory.SampleRate,1,'quaternion'); count = 1; while ~isDone(trajectory) [currentPosition,orientationLog(count)] = trajectory(); plot(currentPosition(1),currentPosition(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end hold off
Convert the orientation quaternions to Euler angles for easy interpretation, and then plot orientation over time.
figure(2) eulerAngles = eulerd([orientation(1);orientationLog],'ZYX','frame'); plot(toa,eulerAngles(:,1),'ko', ... toa,eulerAngles(:,2),'bd', ... toa,eulerAngles(:,3),'r.'); title('Orientation Over Time') legend('Rotation around Z-axis','Rotation around Y-axis','Rotation around X-axis') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on
So far, the trajectory object has only output the waypoints that were specified during construction. To interpolate between waypoints, increase the sample rate to a rate faster than the time of arrivals of the waypoints. Set the trajectory sample rate to 100 Hz and call reset.
trajectory.SampleRate = 100; reset(trajectory)
Create a figure and plot the initial position of the platform. In a loop, step through the trajectory to output the current position and current orientation. Plot the current position and log the orientation. Use pause to mimic real-time processing.
figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axis square xlabel('X') ylabel('Y') grid on hold on orientationLog = zeros(toa(end)*trajectory.SampleRate,1,'quaternion'); count = 1; while ~isDone(trajectory) [currentPosition,orientationLog(count)] = trajectory(); plot(currentPosition(1),currentPosition(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end hold off
The trajectory output now appears circular. This is because the waypointTrajectory System object™ minimizes the acceleration and angular velocity when interpolating, which results in smoother, more realistic motions in most scenarios.
Convert the orientation quaternions to Euler angles for easy interpretation, and then plot orientation over time. The orientation is also interpolated.
figure(2) eulerAngles = eulerd([orientation(1);orientationLog],'ZYX','frame'); t = 0:1/trajectory.SampleRate:4; plot(t,eulerAngles(:,1),'ko', ... t,eulerAngles(:,2),'bd', ... t,eulerAngles(:,3),'r.'); title('Orientation Over Time') legend('Rotation around Z-axis','Rotation around Y-axis','Rotation around X-axis') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on
The waypointTrajectory algorithm interpolates the waypoints to create a smooth trajectory. To return to the square trajectory, provide more waypoints, especially around sharp changes. To track corresponding times, waypoints, and orientation, specify all the trajectory info in a single matrix.
% Time, Waypoint, Orientation trajectoryInfo = [0, 0,0,0, 0,0,0; ... % Initial position 0.1, 0,0.1,0, 0,0,0; ... 0.9, 0,0.9,0, 0,0,0; ... 1, 0,1,0, 45,0,0; ... 1.1, 0.1,1,0, 90,0,0; ... 1.9, 0.9,1,0, 90,0,0; ... 2, 1,1,0, 135,0,0; ... 2.1, 1,0.9,0, 180,0,0; ... 2.9, 1,0.1,0, 180,0,0; ... 3, 1,0,0, 225,0,0; ... 3.1, 0.9,0,0, 270,0,0; ... 3.9, 0.1,0,0, 270,0,0; ... 4, 0,0,0, 270,0,0]; % Final position trajectory = waypointTrajectory(trajectoryInfo(:,2:4), ... 'TimeOfArrival',trajectoryInfo(:,1), ... 'Orientation',quaternion(trajectoryInfo(:,5:end),'eulerd','ZYX','frame'), ... 'SampleRate',100);
Create a figure and plot the initial position of the platform. In a loop, step through the trajectory to output the current position and current orientation. Plot the current position and log the orientation. Use pause to mimic real-time processing.
figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axis square xlabel('X') ylabel('Y') grid on hold on orientationLog = zeros(toa(end)*trajectory.SampleRate,1,'quaternion'); count = 1; while ~isDone(trajectory) [currentPosition,orientationLog(count)] = trajectory(); plot(currentPosition(1),currentPosition(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count+1; end hold off
The trajectory output now appears more square-like, especially around the vertices with waypoints.
Convert the orientation quaternions to Euler angles for easy interpretation, and then plot orientation over time.
figure(2) eulerAngles = eulerd([orientation(1);orientationLog],'ZYX','frame'); t = 0:1/trajectory.SampleRate:4; eulerAngles = plot(t,eulerAngles(:,1),'ko', ... t,eulerAngles(:,2),'bd', ... t,eulerAngles(:,3),'r.'); title('Orientation Over Time') legend('Rotation around Z-axis', ... 'Rotation around Y-axis', ... 'Rotation around X-axis', ... 'Location', 'SouthWest') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on
Create Arc Trajectory
This example shows how to create an arc trajectory using the waypointTrajectory System object™. waypointTrajectory creates a path through specified waypoints that minimizes acceleration and angular velocity. After creating an arc trajectory, you restrict the trajectory to be within preset bounds.
Create an Arc Trajectory
Define a constraints matrix consisting of waypoints, times of arrival, and orientation for an arc trajectory. The generated trajectory passes through the waypoints at the specified times with the specified orientation. The waypointTrajectory System object requires orientation to be specified using quaternions or rotation matrices. Convert the Euler angles saved in the constrains matrix to quaternions when specifying the Orientation property.
% Arrival, Waypoints, Orientation constraints = [0, 20,20,0, 90,0,0; 3, 50,20,0, 90,0,0; 4, 58,15.5,0, 162,0,0; 5.5, 59.5,0,0 180,0,0]; trajectory = waypointTrajectory(constraints(:,2:4), ... 'TimeOfArrival',constraints(:,1), ... 'Orientation',quaternion(constraints(:,5:7),'eulerd','ZYX','frame'));
Call waypointInfo on trajectory to return a table of your specified constraints. The creation properties Waypoints, TimeOfArrival, and Orientation are variables of the table. The table is convenient for indexing while plotting.
tInfo = waypointInfo(trajectory)
tInfo =
4x3 table
TimeOfArrival Waypoints Orientation
_____________ ____________________ ________________
0 20 20 0 {1x1 quaternion}
3 50 20 0 {1x1 quaternion}
4 58 15.5 0 {1x1 quaternion}
5.5 59.5 0 0 {1x1 quaternion}
The trajectory object outputs the current position, velocity, acceleration, and angular velocity at each call. Call trajectory in a loop and plot the position over time. Cache the other outputs.
figure(1) plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),'b*') title('Position') axis([20,65,0,25]) xlabel('North') ylabel('East') grid on daspect([1 1 1]) hold on orient = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,1,'quaternion'); vel = zeros(tInfo.TimeOfArrival(end)*trajectory.SampleRate,3); acc = vel; angVel = vel; count = 1; while ~isDone(trajectory) [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory(); plot(pos(1),pos(2),'bo') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end
Inspect the orientation, velocity, acceleration, and angular velocity over time. The waypointTrajectory System object™ creates a path through the specified constraints that minimized acceleration and angular velocity.
figure(2)
timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end);
eulerAngles = eulerd([tInfo.Orientation{1};orient],'ZYX','frame');
plot(timeVector,eulerAngles(:,1), ...
timeVector,eulerAngles(:,2), ...
timeVector,eulerAngles(:,3));
title('Orientation Over Time')
legend('Rotation around Z-axis', ...
'Rotation around Y-axis', ...
'Rotation around X-axis', ...
'Location','southwest')
xlabel('Time (seconds)')
ylabel('Rotation (degrees)')
grid on
figure(3)
plot(timeVector(2:end),vel(:,1), ...
timeVector(2:end),vel(:,2), ...
timeVector(2:end),vel(:,3));
title('Velocity Over Time')
legend('North','East','Down')
xlabel('Time (seconds)')
ylabel('Velocity (m/s)')
grid on
figure(4)
plot(timeVector(2:end),acc(:,1), ...
timeVector(2:end),acc(:,2), ...
timeVector(2:end),acc(:,3));
title('Acceleration Over Time')
legend('North','East','Down','Location','southwest')
xlabel('Time (seconds)')
ylabel('Acceleration (m/s^2)')
grid on
figure(5)
plot(timeVector(2:end),angVel(:,1), ...
timeVector(2:end),angVel(:,2), ...
timeVector(2:end),angVel(:,3));
title('Angular Velocity Over Time')
legend('North','East','Down')
xlabel('Time (seconds)')
ylabel('Angular Velocity (rad/s)')
grid on
Restrict Arc Trajectory Within Preset Bounds
You can specify additional waypoints to create trajectories within given bounds. Create upper and lower bounds for the arc trajectory.
figure(1) xUpperBound = [(20:50)';50+10*sin(0:0.1:pi/2)';60*ones(11,1)]; yUpperBound = [20.5.*ones(31,1);10.5+10*cos(0:0.1:pi/2)';(10:-1:0)']; xLowerBound = [(20:49)';50+9*sin(0:0.1:pi/2)';59*ones(11,1)]; yLowerBound = [19.5.*ones(30,1);10.5+9*cos(0:0.1:pi/2)';(10:-1:0)']; plot(xUpperBound,yUpperBound,'r','LineWidth',2); plot(xLowerBound,yLowerBound,'r','LineWidth',2)
To create a trajectory within the bounds, add additional waypoints. Create a new waypointTrajectory System object™, and then call it in a loop to plot the generated trajectory. Cache the orientation, velocity, acceleration, and angular velocity output from the trajectory object.
% Time, Waypoint, Orientation constraints = [0, 20,20,0, 90,0,0; 1.5, 35,20,0, 90,0,0; 2.5 45,20,0, 90,0,0; 3, 50,20,0, 90,0,0; 3.3, 53,19.5,0, 108,0,0; 3.6, 55.5,18.25,0, 126,0,0; 3.9, 57.5,16,0, 144,0,0; 4.2, 59,14,0, 162,0,0; 4.5, 59.5,10,0 180,0,0; 5, 59.5,5,0 180,0,0; 5.5, 59.5,0,0 180,0,0]; trajectory = waypointTrajectory(constraints(:,2:4), ... 'TimeOfArrival',constraints(:,1), ... 'Orientation',quaternion(constraints(:,5:7),'eulerd','ZYX','frame')); tInfo = waypointInfo(trajectory); figure(1) plot(tInfo.Waypoints(1,1),tInfo.Waypoints(1,2),'b*') count = 1; while ~isDone(trajectory) [pos,orient(count),vel(count,:),acc(count,:),angVel(count,:)] = trajectory(); plot(pos(1),pos(2),'gd') pause(trajectory.SamplesPerFrame/trajectory.SampleRate) count = count + 1; end
The generated trajectory now fits within the specified boundaries. Visualize the orientation, velocity, acceleration, and angular velocity of the generated trajectory.
figure(2) timeVector = 0:(1/trajectory.SampleRate):tInfo.TimeOfArrival(end); eulerAngles = eulerd(orient,'ZYX','frame'); plot(timeVector(2:end),eulerAngles(:,1), ... timeVector(2:end),eulerAngles(:,2), ... timeVector(2:end),eulerAngles(:,3)); title('Orientation Over Time') legend('Rotation around Z-axis', ... 'Rotation around Y-axis', ... 'Rotation around X-axis', ... 'Location','southwest') xlabel('Time (seconds)') ylabel('Rotation (degrees)') grid on figure(3) plot(timeVector(2:end),vel(:,1), ... timeVector(2:end),vel(:,2), ... timeVector(2:end),vel(:,3)); title('Velocity Over Time') legend('North','East','Down') xlabel('Time (seconds)') ylabel('Velocity (m/s)') grid on figure(4) plot(timeVector(2:end),acc(:,1), ... timeVector(2:end),acc(:,2), ... timeVector(2:end),acc(:,3)); title('Acceleration Over Time') legend('North','East','Down') xlabel('Time (seconds)') ylabel('Acceleration (m/s^2)') grid on figure(5) plot(timeVector(2:end),angVel(:,1), ... timeVector(2:end),angVel(:,2), ... timeVector(2:end),angVel(:,3)); title('Angular Velocity Over Time') legend('North','East','Down') xlabel('Time (seconds)') ylabel('Angular Velocity (rad/s)') grid on
Note that while the generated trajectory now fits within the spatial boundaries, the acceleration and angular velocity of the trajectory are somewhat erratic. This is due to over-specifying waypoints.