Main Content

waypointTrajectory

Waypoint trajectory generator

expand all in page

Description

ThewaypointTrajectorySystem 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. SeeAlgorithmsfor more details.

To generate a trajectory from waypoints:

  1. Create thewaypointTrajectoryobject and set its properties.

  2. Call the object as if it were a function.

To learn more about how System objects work, seeWhat Are System Objects?.

Creation

Syntax

trajectory = waypointTrajectory
trajectory = waypointTrajectory(Waypoints,TimeOfArrival)
trajectory = waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)

Description

example

trajectory= waypointTrajectoryreturns a System object,trajectory, that generates a trajectory based on default stationary waypoints.

example

trajectory= waypointTrajectory(Waypoints,TimeOfArrival)specifies theWaypointsthat the generated trajectory passes through and theTimeOfArrivalat each waypoint.

example

trajectory= waypointTrajectory(Waypoints,TimeOfArrival,Name,Value)sets each creation argument or propertyNameto the specifiedValue. Unspecified properties and creation arguments have default or inferred values.

Example: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 theWaypointsandTimeOfArrivalcreation arguments. You can specifyWaypointsandTimeOfArrivalas value-only arguments or name-value pairs.

Properties

expand all

Unless otherwise indicated, properties arenontunable, which means you cannot change their values after calling the object. Objects lock when you call them, and thereleasefunction unlocks them.

If a property istunable, you can change its value at any time.

For more information on changing property values, seeSystem Design in MATLAB Using System Objects.

Sample rate of trajectory in Hz, specified as a positive scalar.

可调:是的

Data Types:double

Number of samples per output frame, specified as a positive scalar integer.

可调:是的

Data Types:double

Positions in the navigation coordinate system in meters, specified as anN3矩阵. The columns of the matrix correspond to the first, second, and third axes, respectively. The rows of the matrix,N, correspond to individual waypoints.

Dependencies

To set this property, you must also set valid values for theTimeOfArrival财产。

Data Types:double

Time corresponding to arrival at each waypoint in seconds, specified as anN-element column vector. The first element ofTimeOfArrivalmust be0. The number of samples,N, must be the same as the number of samples (rows) defined byWaypoints.

Dependencies

To set this property, you must also set valid values for theWaypoints财产。

Data Types:double

Velocity in the navigation coordinate system at each waypoint in meters per second, specified as anN3矩阵. The columns of the matrix correspond to the first, second, and third axes, respectively. The number of samples,N, must be the same as the number of samples (rows) defined byWaypoints.

If the velocity is specified as a non-zero value, the object automatically calculates the course of the trajectory. If the velocity is specified as zero, the object infers the course of the trajectory from adjacent waypoints.

Dependencies

To set this property, you must also set valid values for theWaypointsandTimeOfArrivalproperties.

Data Types:double

Horizontal direction of travel, specified as anN-element real vector in degrees. The number of samples,N, must be the same as the number of samples (rows) defined byWaypoints. If neitherVelocitiesnorCourseis specified, course is inferred from the waypoints.

Dependencies

To set this property, theVelocitiesproperty must not be specified in object creation.

Data Types:double

Groundspeed at each waypoint, specified as anN-element real vector in m/s. If the property is not specified, it is inferred from the waypoints. The number of samples,N, must be the same as the number of samples (rows) defined byWaypoints.

Dependencies

To set this property, theVelocitiesproperty must not be specified at object creation.

Data Types:double

Climb Rate at each waypoint, specified as anN-element real vector in degrees. The number of samples,N, must be the same as the number of samples (rows) defined byWaypoints. If neitherVelocitiesnorCourseis specified, climb rate is inferred from the waypoints.

Dependencies

To set this property, theVelocitiesproperty must not be specified at object creation.

Data Types:double

定位在每一个路标, specified as anN-elementquaternioncolumn vector or 3-by-3-by-Narray of real numbers. Eachquaternionmust have a norm of1. Each 3-by-3 rotation matrix must be an orthonormal matrix. The number of quaternions or rotation matrices,N, must be the same as the number of samples (rows) defined byWaypoints.

IfOrientationis specified by quaternions, the underlying class must bedouble.

Dependencies

To set this property, you must also set valid values for theWaypointsandTimeOfArrivalproperties.

Data Types:double

Align pitch angle with the direction of motion, specified astrueorfalse. When specified astrue, the pitch angle automatically aligns with the direction of motion. If specified asfalse, the pitch angle is set to zero (level orientation).

Dependencies

To set this property, theOrientationproperty must not be specified at object creation.

Align roll angle to counteract the centripetal force, specified astrueorfalse. When specified astrue, the roll angle automatically counteracts the centripetal force. If specified asfalse, the roll angle is set to zero (flat orientation).

Dependencies

To set this property, theOrientationproperty must not be specified at object creation.

Reference frame of the trajectory, specified as'NED'(North-East-Down) or'ENU'(East-North-Up).

Usage

Syntax

[position,orientation,velocity,acceleration,angularVelocity] = trajectory()

Description

example

[position,orientation,velocity,acceleration,angularVelocity] = trajectory()outputs a frame of trajectory data based on specified creation arguments and properties.

Output Arguments

expand all

Position in the local navigation coordinate system in meters, returned as anM3矩阵.

Mis specified by theSamplesPerFrame财产。

Data Types:double

Orientation in the local navigation coordinate system, returned as anM-by-1quaternioncolumn vector or a 3-by-3-by-Mreal array.

Each quaternion or 3-by-3 rotation matrix is a frame rotation from the local navigation coordinate system to the current body coordinate system.

Mis specified by theSamplesPerFrame财产。

Data Types:double

Velocity in the local navigation coordinate system in meters per second, returned as anM3矩阵.

Mis specified by theSamplesPerFrame财产。

Data Types:double

Acceleration in the local navigation coordinate system in meters per second squared, returned as anM3矩阵.

Mis specified by theSamplesPerFrame财产。

Data Types:double

Angular velocity in the local navigation coordinate system in radians per second, returned as anM3矩阵.

Mis specified by theSamplesPerFrame财产。

Data Types:double

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 namedobj, use this syntax:

release(obj)

expand all

waypointInfo 获取路径信息表
lookupPose Obtain pose information for certain time
perturbations Perturbation defined on object
perturb Apply perturbations to object
clone Create duplicateSystem object
step RunSystem objectalgorithm
release Release resources and allow changes toSystem objectproperty values and input characteristics
reset Reset internal states ofSystem object
isDone End-of-data status

Examples

collapse all

Open Live Script 
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 callingwaypointInfo. By default, the waypoints indicate a stationary position for one second.

waypointInfo(trajectory)
ans=2×2 tableTimeOfArrival Waypoints _____________ ___________ 0 0 0 0 1 0 0 0
Open Script

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 defaultSamplesPerFrameof 1.

waypoints = [0,0,0;...% Initial position0,1,0;...1,1,0;...1,0,0;...0,0,0];% Final positiontoa = 0:4;%的时间of arrivalorientation = 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]) axissquarexlabel('X') ylabel('Y') gridonholdon

In a loop, step through the trajectory to output the current position and current orientation. Plot the current position and log the orientation. Usepauseto 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;endholdoff

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)') gridon

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 thetrajectorysample rate to 100 Hz and callreset.

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. Usepauseto mimic real-time processing.

figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axissquarexlabel('X') ylabel('Y') gridonholdonorientationLog = 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;endholdoff

The trajectory output now appears circular. This is because thewaypointTrajectorySystem 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)') gridon

ThewaypointTrajectoryalgorithm 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, OrientationtrajectoryInfo = [0, 0,0,0, 0,0,0;...% Initial position0.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 positiontrajectory = 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. Usepauseto mimic real-time processing.

figure(1) plot(waypoints(1,1),waypoints(1,2),'b*') title('Position') axis([-1,2,-1,2]) axissquarexlabel('X') ylabel('Y') gridonholdonorientationLog = 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;endholdoff

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)') gridon

Open Script

This example shows how to create an arc trajectory using thewaypointTrajectorySystem object™.waypointTrajectorycreates 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. ThewaypointTrajectorySystem object requires orientation to be specified using quaternions or rotation matrices. Convert the Euler angles saved in the constraints matrix to quaternions when specifying theOrientation财产。

% Arrival, Waypoints, Orientation约束=[0 0,20日20日,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'));

CallwaypointInfoontrajectoryto return a table of your specified constraints. The creation propertiesWaypoints,TimeOfArrival, andOrientationare 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. Calltrajectoryin 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') gridondaspect([1 1 1]) holdonorient = 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. ThewaypointTrajectorySystem 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)') gridonfigure(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)') gridonfigure(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)') gridonfigure(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)') gridon

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);情节(xLowerBound,yLowerBound,'r','LineWidth',2)

To create a trajectory within the bounds, add additional waypoints. Create a newwaypointTrajectorySystem object™, and then call it in a loop to plot the generated trajectory. Cache the orientation, velocity, acceleration, and angular velocity output from thetrajectoryobject.

% Time, Waypoint, Orientation约束=[0 0,20日20日,90年,0,0;0 1.5, 35岁,20日,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)') gridonfigure(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)') gridonfigure(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)') gridonfigure(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)') gridon

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.

Algorithms

ThewaypointTrajectorySystem object defines a trajectory that smoothly passes through waypoints. The trajectory connects the waypoints through an interpolation that assumes the gravity direction expressed in the trajectory reference frame is constant. Generally, you can usewaypointTrajectoryto model platform or vehicle trajectories within a hundreds of kilometers distance span.

The planar path of the trajectory (thex-yplane projection) consists of piecewise, clothoid curves. The curvature of the curve between two consecutive waypoints varies linearly with the curve length between them. The tangent direction of the path at each waypoint is chosen to minimize discontinuities in the curvature, unless the course is specified explicitly via theCourseproperty or implicitly via theVelocities财产。一旦建立了路径,对象uses cubic Hermite interpolation to compute the location of the vehicle throughout the path as a function of time and the planar distance traveled.

The normal component (z-component) of the trajectory is subsequently chosen to satisfy a shape-preserving piecewise spline (PCHIP) unless the climb rate is specified explicitly via theClimbRateproperty or the third column of theVelocities财产。Choose the sign of the climb rate based on the selectedReferenceFrame:

  • When an 'ENU' reference frame is selected, specifying a positive climb rate results in an increasing value ofz.

  • When an 'NED' reference frame is selected, specifying a positive climb rate results in a decreasing value ofz.

You can define the orientation of the vehicle through the path in two primary ways:

  • If theOrientationproperty is specified, then the object uses a piecewise-cubic, quaternion spline to compute the orientation along the path as a function of time.

  • If theOrientationproperty is not specified, then the yaw of the vehicle is always aligned with the path. The roll and pitch are then governed by theAutoBankandAutoPitchproperty values, respectively.

    AutoBank AutoPitch Description
    false false The vehicle is always level (zero pitch and roll). This is typically used for large marine vessels.
    false true The vehicle pitch is aligned with the path, and its roll is always zero. This is typically used for ground vehicles.
    true false The vehicle pitch and roll are chosen so that its localz-axis is aligned with the net acceleration (including gravity). This is typically used for rotary-wing craft.
    true true The vehicle roll is chosen so that its local transverse plane aligns with the net acceleration (including gravity). The vehicle pitch is aligned with the path. This is typically used for two-wheeled vehicles and fixed-wing aircraft.

Extended Capabilities

Version History

Introduced in R2018b

See Also

Objects