Obstacle Avoidance

Obstacle Avoidance enables a vehicle to navigate around obstacles when following a preplanned path.

The feature requires a companion computer that is running computer vision software. This software provides a route for a given desired trajectory, mapping and navigating around obstacles to achieve the best path.

Obstacle avoidance is intended for automatic modes, and is currently supported for multicopter vehicles in Missions and Offboard mode.

This topic explains how the feature is set up and enabled in both modes.

Offboard Mode Avoidance

PX4 supports collision avoidance in Offboard mode.

The desired route comes from a ROS node running on a companion computer. This is passed into an obstacle avoidance module (another ROS node). The avoidance software sends the planned path to the flight stack as a stream of SET_POSITION_TARGET_LOCAL_NED messages.

The only required PX4-side setup is to put PX4 into Offboard mode. While the SET_POSITION_TARGET_LOCAL_NED setpoints come from a ROS collision avoidance node, to PX4 could be from any MAVLink system.

The tested hardware/software platform is Intel Aero running either the local_planner or global_planner. Object avoidance can also be tested using Gazebo. The set up for both is as described in the Intel Aero > Obstacle Avoidance and in the PX4/avoidance Github repo.

Mission Mode Avoidance

PX4 supports obstacle avoidance in Mission mode, using avoidance software running on a separate companion computer.

Obstacle avoidance is enabled within PX4 by setting the COM_OBS_AVOID to 1. PX4 communicates with the obstacle avoidance software using an implementation of the MAVLink Path Planning Protocol (Trajectory Interface) which is #described below. Provided an avoidance system complies with this interface it can be used with PX4.

The tested companion computer platform is Intel Aero running either the local_planner or global_planner avoidance software. Object avoidance can also be tested using Gazebo. The set up for both is as described in the Intel Aero > Obstacle Avoidance and in the PX4/avoidance Github repo.

Mission Progression

Mission behaviour with obstacle avoidance enabled is slightly different to the original plan.

The difference when avoidance is active are:

  • A waypoint is "reached" when the vehicle is within the acceptance radius, regardless of its heading.
    • This differs from normal missions, in which the vehicle must reach a waypoint with a certain heading (i.e. in a "close to" straight line from the previous waypoint). This constraint cannot be fulfilled when obstacle avoidance is active because the obstacle avoidance algorithm has full control of the vehicle heading, and the vehicle always moves in the current field of view.
  • PX4 starts emitting a new current/next waypoint once the previous waypoint is reached (i.e. as soon as vehicle enters its acceptance radius).
  • If a waypoint is inside an obstacle it may unreachable (and the mission will be stuck).
    • If the vehicle projection on the line previous-current waypoint passes the current waypoint, the acceptance radius is enlarged such that the current waypoint is set as reached
    • If the vehicle within the x-y acceptance radius, the altitude acceptance is modified such that the mission progresses (even if it is not in the altitude acceptance radius).
  • The original mission speed (as set in QGroundControl/PX4) is ignored. The speed will be determined by the avoidance software:
    • local planner mission speed is around 3 m/s.
    • global planner mission speed is around 1-1.5 m/s.

If PX4 stops receiving setpoint updates then obstacle avoidance will be disabled, and the mission continues under normal PX4 Mission mode control.

Mission Mode Avoidance Interface

Obstacle avoidance in mission mode is enabled on PX4 by setting COM_OBS_AVOID to 1.

PX4 sends the desired trajectory to the companion computer in TRAJECTORY_REPRESENTATION_WAYPOINTS messages at 5Hz.

The fields are set as shown:

  • time_usec: UNIX Epoch time.
  • valid_points: 3
  • Current vehicle information:
    • pos_x[0], pos_y[0], pos_z[0]: x-y-z NED vehicle local position
    • vel_x[0], vel_y[0], vel_z[0]: x-y-z NED velocity setpoint
    • acc_x[0], acc_y[0], acc_z[0]: x-y-z NED acceleration setpoint
    • pos_yaw[0]: Current yaw angle
    • vel_yaw[0]: NaN
  • Current waypoint:

    • pos_x[1], pos_y[1], pos_z[1]: x-y-z NED local position of current mission waypoint
    • vel_x[1], vel_y[1], vel_z[1]: NaN
    • acc_x[1], acc_y[1], acc_z[1]: NaN
    • pos_yaw[1]: Yaw setpoint
    • vel_yaw[1]: Yaw speed setpoint
  • Next waypoint:

    • pos_x[2], pos_y[2], pos_z[2]: x-y-z NED local position of next mission waypoint
    • vel_x[2], vel_y[2], vel_z[2]: NaN
    • acc_x[2], acc_y[2], acc_z[2]: NaN
    • pos_yaw[2]: Yaw setpoint
    • vel_yaw[2]: Yaw speed setpoint
  • All other indices/fields are set as NaN.

PX4 expects to receive target setpoints in a stream of TRAJECTORY_REPRESENTATION_WAYPOINTS messages for the duration of the mission (not just when there are obstacles).

The fields for the message from the avoidance software are set as shown:

  • time_usec: UNIX Epoch time.
  • valid_points: 1
  • Current vehicle information:
    • pos_x[0], pos_y[0], pos_z[0]: x-y-z NED vehicle local position setpoint
    • vel_x[0], vel_y[0], vel_z[0]: x-y-z NED velocity setpoint
    • acc_x[0], acc_y[0], acc_z[0]: NaN
    • pos_yaw[0]: Yaw angle setpoint
    • vel_yaw[0]: Yaw speed setpoint
  • All other indices/fields are set as NaN.

The rate at which target setpoints are sent depends on the capabilities of the planning software and the desired speed.

Currently the local planner emits messages at ~30Hz and can move at around 3 m/s. The global planner emits messages at ~10Hz and mission speed is around 1-1.5 m/s.

The paragraphs below describe the behaviour in greater detail, covering the internal PX4 behaviour and message flow through ROS.

Mission Mode Detailed Behaviour

When a mission is uploaded from QGC and the parameter COM_OBS_AVOID is set to 1, PX4 fills the uORB message vehicle_trajectory_waypoint_desired as described below:

Array waypoints:

  • index 0 :

    • position: x-y-z NED vehicle local position
    • velocity: x-y-z NED velocity setpoint generated by the active FlightTask
    • Acceleration: vehicle acceleration
    • Yaw: vehicle yaw
    • Yaw_speed: NaN
  • index 1:

    • position: x-y-z NED local coordinates of the current mission waypoint
    • Velocity: NaN
    • Acceleration: NaN
    • Yaw: yaw setpoint
    • Yaw_speed: yaw speed setpoint
  • Index2:

    • position: x-y-z NED local coordinates of the next mission waypoint
    • Velocity: NaN
    • Acceleration: NaN
    • Yaw: yaw setpoint
    • Yaw_speed: yaw speed setpoint

The remaining indices are filled with NaN.

The message vehicle_trajectory_waypoint_desired is mapped into the MAVLink message TRAJECTORY_REPRESENTATION_WAYPOINTS (see avoidance interface above). The messages are sent at 5Hz.

MAVROS translates the MAVLink message into a ROS message called mavros_msgs::trajectory and does the conversion from NED to ENU frames. Messages are published on the ROS topic /mavros/trajectory/desired

On the avoidance side, the algorithm plans a path to the waypoint.

The position or velocity setpoints generated by the obstacle avoidance to get collision free to the waypoint can be sent to the Firmware with two ROS messages: mavros_msgs::trajectory (both velocity and position set points) on ROS topic /mavros/trajectory/generated nav_msgs::Path (only position setpoints) on ROS topic /mavros/trajectory/path

MAVROS converts the set points from ENU to NED frame and translates the ROS messages into a MAVLink message TRAJECTORY_REPRESENTATION_WAYPOINTS.

On the PX4 side, incoming TRAJECTORY_REPRESENTATION_WAYPOINTS are translated into uORB vehicle_trajectory_waypoint messages. The array waypoints contains all NAN expect for index 0:

  • Position: position setpoint
  • Velocity: velocity setpoint
  • acceleration: NaN (acceleration setpoints are not supported by the Firmware)
  • Yaw: yaw setpoint
  • Yaw_speed: yaw speed setpoint

The setpoints are tracked by the multicopter position controller.

© PX4 Dev Team. License: CC BY 4.0            Updated: 2020-10-28 22:06:47

results matching ""

    No results matching ""