Waypoint Follower

Follow waypoints for UAV

• 

Libraries:
UAV Toolbox / Algorithms

Description

The Waypoint Follower block calculates a lookahead point based on the current UAV pose, a desired lookahead distance, and a specified set of waypoints. The block also returns the desired course and yaw of the UAV at the current position [1].

Specify the UAV type parameter to select your UAV type. Use the Start from parameter to specify how the block determines which waypoint to start from. Specify the Transition radius source parameter to select whether to use a uniform or waypoint-specific transition radius.

Examples

Approximate High-Fidelity UAV Model with UAV Guidance Model Block

Prototype and tune a waypoint following navigation system by using a reduced-order model for UAV.

• Open Live Script

Tuning Waypoint Following Controller for Fixed-Wing UAV

Design and tune a waypoint following controller for a fixed-wing UAV by using the Guidance Model and Waypoint Follower blocks.

• Open Live Script

Ports

Input

Pose — Current UAV pose

vector

Current pose of the UAV, specified as a vector of the form [x y z chi].

x, y, and z specify the current xyz-position of the UAV in the north-east-down coordinate system, in meters.

chi specifies the current course of the UAV, which is the angle of the velocity vector relative to north. Specify the current course in the range of (-pi, pi]. Units are in radians.

Example: [0.5 1.75 -2.5 pi]

Data Types: single | double

Waypoints — Waypoints to follow

matrix

Waypoints to follow, specified as a matrix with a number of rows equal to the number of waypoints. The form of each row depends on the values of Show Yaw input variable and Transition radius source parameters.

• Show Yaw input variable is off and Transition radius source is internal

  Specify each row in the form [x y z], where x, y, and z specify the xyz-position of the corresponding waypoint in the north-east-down coordinate system. Units are in meters.

• Show Yaw input variable is on and Transition radius source is internal

  Specify each row in the form [x y z yaw]. yaw is the desired yaw angle for the UAV at the corresponding waypoint, which is the angle of the x-axis of the UAV body relative to north. Specify the desired yaw in the range of (-pi, pi], in radians.

• Show Yaw input variable is off and Transition radius source is external

  Specify each row in the form [x y z r]. r is the transition radius of the corresponding waypoint, in meters.

• Show Yaw input variable is on and Transition radius source is external

  Specify each row as [x y z yaw r].

Data Types: single | double

LookaheadDistance — Straight-line distance to lookahead point

scalar

Straight-line distance from the UAV to the lookahead point, specified as a positive scalar, in meters.

Data Types: single | double

Output

LookaheadPoint — xyz-position of lookahead point

three-element vector

xyz-position of the lookahead point in the north-east-down coordinate system, returned as a vector of the form [x y z]. Units are in meters.

Data Types: single | double

DesiredCourse — Desired course

numeric scalar

Desired course, returned as numeric scalar in the range (-pi, pi]. This value indicates the desired angle of the velocity vector relative to north. Units are in radians.

If the UAV type parameter is fixed-wing, then the value of DesiredCourse is equal to the value of DesiredYaw.

Data Types: single | double

DesiredYaw — Desired yaw

numeric scalar

Desired yaw, returned as numeric scalar in radians in the range (-pi, pi]. This value indicates the desired angle of the x-axis of the UAV body relative to north. Units are in radians.

If the UAV type parameter is fixed-wing, then the value of DesiredYaw is equal to the value of DesiredCourse.

Data Types: single | double

LookaheadDistFlag — Lookahead distance saturation flag

0 | 1

Lookahead distance saturation flag, returned as 0 or 1.

A value of 1 indicates that the specified lookahead distance LookAheadDistance is less than the minimum lookahead distance specified in the Minimum lookahead distance parameter. When this occurs, the waypoint follower object saturates the straight-line distance between the UAV and the lookahead point to the minimum lookahead distance.

Data Types: uint8

CrossTrackError — Cross track error from UAV position to path

positive scalar

Cross track error from the UAV position to the path, returned as a positive numeric scalar, in meters. The error measures the perpendicular distance from the UAV position to the closest point on the path.

Dependencies

To enable this port, select the Show CrossTrackError output port parameter.

Data Types: single | double

Status — Status of waypoint navigation

0 | 1

Status of waypoint navigation, returned as 0 or 1. Value of 1 indicates that the Waypoint Follower block has navigated all waypoints.

Status of waypoint navigation, returned as 0 or 1. A value of 1 indicates that the Waypoint Follower block has navigated to the final waypoint specified in the Waypoints input.

Dependencies

To enable this port, select the Show UAV Status output port parameter.

Data Types: uint8

WaypointIndex — Waypoint index of current straight-line segment

two-element column vector

Waypoint index of the current straight-line segment, returned as a two-element column vector of the form [FromWP; ToWP]. FromWP is the index of the waypoint that the UAV is traveling from, and ToWP is the index of waypoint that the UAV is traveling to in the current straight-line segment.

Dependencies

To enable this port, select the Show Waypoint index output port parameter.

Data Types: single | double

Parameters

UAV type — Type of UAV

multirotor (default) | fixed-wing

Specify the type of UAV as multirotor or fixed-wing.

Start from — Waypoint start behavior

first (default) | closest

Specify the waypoint as one of these options:

• first — The Waypoint Follower block starts waypoint navigation from the first path segment, which is the path segment formed by the first and second waypoints specified to the Waypoints input port.

  If you change the input waypoints during simulation, the Waypoint Follower block restarts navigation from the updated first path segment.

• closest — The Waypoint Follower block starts waypoint navigation from the path segment that is closest to the UAV position at the start of simulation.

  If you change the input waypoints during simulation, the Waypoint Follower block recalculates the closest path segment, then restarts navigation from the updated closest path segment.

Transition radius source — Source of transition radius

internal (default) | external

Specify the transition radius source as either internal or external.

• internal — Specify a uniform transition radius for all waypoint using the Transition radius (r) parameter.

• external — Specify different transition radius for each waypoint using the Waypoints input.

Transition radius (r) — Transition radius for all waypoints

10 (default) | positive scalar

Specify a uniform transition radius for all waypoints as a positive scalar, in meters.

When the UAV is within the transition radius, the block transitions to follow the next path segment.

Dependencies

To enable this parameter, set the Transition radius source parameter to internal.

Minimum lookahead distance (m) — Minimum lookahead distance

0.1 (default) | positive scalar

Specify the minimum lookahead distance as a positive numeric scalar, in meters.

Hover at last waypoint — Lookahead point behavior at last waypoint

off (default) | on

If you select this parameter, the lookahead point remains stationary at the last waypoint. Otherwise, the lookahead point continues to traverse the straight-line path connecting the last two waypoints.

Dependencies

To enable this parameter, set the UAV type parameter to multirotor.

Yaw Interpolation — Interpolate desired yaw between waypoints

on (default) | off

If you select this parameter, the Waypoint Follower block computes the desired yaw between each pair of waypoints by linearly interpolating between the desired yaw angles at those waypoint. Otherwise, the desired yaw angle remains constant from the most recently passed waypoint to the next waypoint.

Dependencies

To enable this parameter, set the UAV type parameter to multirotor.

Show Yaw input variable — Accept yaw input for waypoints

off (default) | on

If you select this parameter, the Waypoints input port accepts a desired yaw angle at each waypoint.

Show CrossTrackError output port — Output cross track error

off (default) | on

Output cross track error from the CrossTrackError port.

Show UAV Status output port — Output UAV waypoint status

off (default) | on

If you select this parameter, the block outputs the UAV waypoint status from the Status port.

Show Waypoint Index output port — Output waypoint index of current straight-line segment

off (default) | on

If you select this parameter, the block outputs waypoint index of the current straight-line segment from the WaypointIndex port.

Simulate using — Type of simulation to run

Interpreted execution (default) | Code generation

• Interpreted execution — Simulate model using the MATLAB^® interpreter. This option shortens startup time, but has a slower simulation speed than Code generation. In this mode, you can debug the source code of the block.

• Code generation — Simulate model using generated C code. The first time you run a simulation, Simulink^® generates C code for the block. The C code is reused for subsequent simulations, as long as the model does not change. This option requires additional startup time, but the speed of the subsequent simulations is comparable to Interpreted execution.

More About

Waypoint Hyperplane Condition

When following a set of waypoints, the waypoint follower might ignore the first waypoint depending on the pose of the UAV. Each consecutive pair of waypoints forms a path segment, and the waypoint follower transitions to the next path segment if the UAV position satisfies at least one of these conditions:

1. The UAV is inside the transition region of the waypoint at the end of a segment.

2. The UAV is inside the 3D hyperplane region of the waypoint at the end of a segment

This behavior helps to ensure the UAV follows an achievable path.

The hyperplane condition is satisfied if:

$${(p-w2)}^T(w2-w1)\ge 0$$

p is the current UAV position. w1 is location of waypoint that starts the current path segment, and w2 is location of waypoint that ends the current path segment.

If you find this behavior limiting, consider adding more waypoints based on your initial pose to force the follower to navigate towards your initial waypoint.

References

[1] Park, Sanghyuk, John Deyst, and Jonathan How. “A New Nonlinear Guidance Logic for Trajectory Tracking.” Paper presented at AIAA Guidance, Navigation, and Control Conference and Exhibit, Providence, Rhode Island. AIAA Guidance, Navigation, and Control Conference and Exhibit, American Institute of Aeronautics and Astronautics, August 16, 2004. https://doi.org/10.2514/6.2004-4900.

Extended Capabilities

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

Version History

Introduced in R2018b

R2026a: Configure yaw interpolation for multirotor UAV

You can now choose whether to enable interpolation of the desired yaw angle between waypoints by using the Yaw Interpolation parameter. In previous version, the yaw angle interpolation is always enabled.

To enable this parameter, you must set the UAV type parameter to multirotor.

R2026a: Output waypoint index of current straight-line segment

The Waypoint Follower block now has the optional WaypointIndex output port, which outputs the waypoint index of the current straight-line segment.

To enable this port, select the Show Waypoint Index output port parameter.

R2024b: Hover at last waypoint

You can now configure the lookahead point to remain stationary at the final waypoint by selecting the Hover at last waypoint parameter. To enable this parameter, set the UAV type parameter to multirotor.