Trajectory generation

General description

The mrs_uav_trajectory_generation package provides a method for generation a time-parametrized trajectory out of a path (a sequence of waypoints). The resulting trajectory satisfies the current dynamics constraints of the UAV and completes the path in minimum possible time. The maximum deviation of the resulting trajectory from the supplied path is a user-configurable parameter.

We built upon the work of ethz-asl/mav_trajectory_generation. The main differences are:

• This package provides ROS node that is meant to be used within the MRS UAV System. However, it can be easily modified for other purposes.
• This ROS node subscribes to the current control reference of the UAV and current allowed dynamic constraints. The resulting trajectory starts from the current reference state and satisfies the constraints.
• The utilized variant minimizes acceleration and uses the Mellinger's time allocation method.
• Improved Mellinger's time allocation: only the polynomial segments that violate constraints are stretched, instead of the whole trajectory.
• Purely Euclidean max_speed time estimate is used for initializing segment times. It provides lower bound, which is good since the constraints are met by prolonging the segment times (not shortening them).
• Iterative segment subsectioning is added to satisfy maximum distance from the original segmented waypoint path.
• Added mechanism to smoothly continue with the current motion when replanning (the incoming path needs to be from the future).
• Added checks for spatial and temporal validity of the resulting trajectory.
• Added check for nlopt results, that can, in fact, indicate the process has failed.
• Added the maxtime parameter for nlopt, therefore, timeouting is possible.
• Added fallback trajectory generation that is used in case of failure of the ETH trajectory generation.
• Removed dependency on the glog library and the ETH glog wrapper.
• Removed dependency on catkin_simple.
• Removed dependency on the yaml_cpp library.
• Removed dependency on the mav_msgs library.
• Changed dependency from nlopt ETH wrapper to MRS nlopt_ros wrapper.
• Added dependency on mrs_lib (just for convenience).
• Added dependency on mrs_msgs (for compatibility with the MRS UAV System).

Configuration and use

ROS interface

Input: service and topic address:

/uav*/trajectory_generation/path

Output: by default, the node calls /uav*/control_manager/trajectory_reference service to the ControlManager.

Minimum waypoint distance

The minimum distance between the waypoints is set to 0.05 m. A waypoint that violates the condition relative to its predecesor will be removed.

Segment subsectioning

The node allows to check and correct for the maximum allowed deviation from a segmented path supplied by the user. If enabled, the user-supplied segments will be subdivided to satisfy the maximum distance constraint. The config file provides the options to enable/disable this feature, to set the maximum allowed deviations, and the number of iterations. Each iteration allows the algorithm to subdivide any segments if the resulting trajectory violates the distance constraint within the segment. 6 iterations is enough to fall within 0.1 m tolerance. The first segment can be optionally excluded from this constraint.

check_trajectory_deviation:
  enabled: true
  max_deviation: 0.1 # [m]
  max_iterations: 6 # [-]
  first_segment: true

without subsectioning	1 iteration
2 iterations	3 iterations
4 iterations	5 iterations

Initial condition & First segment subsectioning

Subsectioning of the first segment is optional. The choice revolves around the initial conditions. When the UAV is in motion, the subsectioning of the first segment can create constraints that might be difficult to satisfy. On the other hand, when the UAV is stationary, the first segment might need to be subsectioned to minimize the deviation from the first waypoint, especially if the first waypoint is relatively far away.

The following images show the two situations with a dynamic initial condition.

the 1st segment unconstrained	the 1st segment subsectioned

Dynamic constraints

The dynamic constraints are automatically obtained from the ControlManager (/uav*/control_manager/current_constraints). Beware, this method does not distinguish between horizontal and vertical constraints, as it is with the rest of the MRS UAV system. Therefore, only the horizontal constraints values are pull out of ControlManager. For arbitrarily structured paths set the horizontal and vertical constraints equal in the ConstraintManager.

The input service/topic allows to override the maximum velocity and acceleration constraints. If overriden, the smaller values (between the user-overriden and the supplied by the ControlManager) will be used.

Dependencies

• nlopt_ros
• mrs_lib
• mrs_msgs

Acknowledgments

This package is based upon ethz-asl/mav_trajectory_generation. Please, cite the original publications:

C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in International Journal of Robotics Research, Springer, 2016.

@incollection{richter2016polynomial,
  title={Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments},
  author={Richter, Charles and Bry, Adam and Roy, Nicholas},
  booktitle={Robotics Research},
  pages={649--666},
  year={2016},
  publisher={Springer}
}

Michael Burri, Helen Oleynikova, Markus Achtelik, and Roland Siegwart, “Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Previously Unknown Environments”. In IEEE Int. Conf. on Intelligent Robots and Systems (IROS), September 2015.

@inproceedings{burri2015real-time,
  author={Burri, Michael and Oleynikova, Helen and  and Achtelik, Markus W. and Siegwart, Roland},
  booktitle={2015 IEEE/RSJ International Conference on Intelligent Robots and Systems},
  title={Real-Time Visual-Inertial Mapping, Re-localization and Planning Onboard MAVs in Unknown Environments},
  year={2015},
  month={Sept}
}