Controllers

A controller computes the control inputs for the vehicle to track a desired trajectory given an estimated state of the vehicle. The controller can be used by all the autopilot modes to compute the control inputs for the vehicle.

If you want to prototype a new controller to be used only for following a specific trajectory, then it might make sense to implement it as a mode rather than a controller that is used by all the other available modes.

This page is structured as follows:

• Section 0. Controller Interface details the interface that a controller must implement to be used by the autopilot.

• Section 1. PID Controller - Mathematical Background and Implementation details the mathematical background for the PID controller that is implemented in the Pegasus Autopilot.

• Section 2. Mellinger Controller - Mathematical Background and Implementation details the mathematical background for the Mellinger controller that is implemented in the Pegasus Autopilot.

• Section 3. Adding a Custom Controller details how to add a custom controller to the autopilot.

0. Controller Interface

The interface for the autopilot controller is defined in the autopilot package under the pegasus_autopilot/autopilot/include/autopilot/controller.hpp header file. The Controller class is an abstract class that defines the interface for the autopilot controller.

Each autopilot controller must inherit from the Controller class and implement the following one or multiple methods highlighted in yellow in the code snippet below:

namespace autopilot {

class Controller {

public: 

    using SharedPtr = std::shared_ptr<Controller>;
    using UniquePtr = std::unique_ptr<Controller>;
    using WeakPtr = std::weak_ptr<Controller>;

    // Configuration for the operation mode
    struct Config {
        rclcpp::Node::SharedPtr node;                                           // ROS 2 node ptr (in case the mode needs to create publishers, subscribers, etc.)
        std::function<State()> get_vehicle_state;                               // Function pointer to get the current state of the vehicle      
        std::function<VehicleStatus()> get_vehicle_status;                      // Function pointer to get the current status of the vehicle  
        std::function<VehicleConstants()> get_vehicle_constants;                // Function pointer to get the current dynamical constants of the vehicle    
    };

    inline void initialize_controller(const Controller::Config & config) {

        // Initialize the base class
        node_ = config.node;
        get_vehicle_state = config.get_vehicle_state;
        get_vehicle_status = config.get_vehicle_status;
        get_vehicle_constants = config.get_vehicle_constants;

        // Initialize the derived class
        initialize();
    }

    /**
     * @brief Pure virtual function that must be implemented by the derived class to initialize the controller
    */
    virtual void initialize() = 0;

    /**
     * @brief Method that can be used to reset certain parameters of the controller.
     * This method is called by the autopilot when the vehicle is switched to the disarm mode. If not implemented, it does nothing.
     */
    virtual void reset_controller() {};

    /** 
     * @brief Sets the target position of the vehicle in the inertial frame and the target yaw (in degres)
     * @param position The target position in the inertial frame
     * @param yaw The target yaw in degrees
    */
   virtual void set_position(const Eigen::Vector3d & position, double yaw, double dt) {
        set_position(position, yaw, 0.0, dt);
    }

    virtual void set_position(const Eigen::Vector3d& position, double yaw, double yaw_rate, double dt) {
        set_position(position, Eigen::Vector3d::Zero(), yaw, yaw_rate, dt);
    }

    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, double yaw, double yaw_rate, double dt) {
        set_position(position, velocity, Eigen::Vector3d::Zero(), yaw, yaw_rate, dt);
    }

    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, double yaw, double yaw_rate, double dt) {
        set_position(position, velocity, acceleration, Eigen::Vector3d::Zero(3), yaw, yaw_rate, dt);
    }

    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, double yaw, double yaw_rate, double dt) {
        set_position(position, velocity, acceleration, jerk, Eigen::Vector3d::Zero(3), yaw, yaw_rate, dt);
    }

    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, const Eigen::Vector3d& snap, double yaw, double yaw_rate, double dt) {
        throw std::runtime_error("set_position() not implemented in derived class");
    }

    /**
     * @brief Sets the target velocity of the vehicle in the inertial frame and the target yaw rate (in degres/s)
     * @param velocity The target velocity in the inertial frame
     * @param yaw_rate The target yaw rate in degrees/s
    */
    virtual void set_velocity(const Eigen::Vector3d& velocity, double yaw_rate, double dt=0) {
        throw std::runtime_error("set_velocity() not implemented in derived class");
    }

    /**
     * @brief Sets the target position of the vehicle in the body frame (rotated to be vertically aligned with NED) and the target yaw (in degres)
     * @param position The target position in the body frame
     * @param yaw The target yaw in degrees
    */
    virtual void set_body_velocity(const Eigen::Vector3d& velocity, double yaw_rate, double dt=0) {
        throw std::runtime_error("set_body_velocity() not implemented in derived class");
    }

    /**
     * @brief Sets the target attitude of the vehicle in the body frame and the target thrust force
     * @param attitude The target attitude in the body frame (in degrees)
     * @param thrust_force The target thrust force (in Newton)
    */
    virtual void set_attitude(const Eigen::Vector3d& attitude, double thrust_force, double dt=0) {
        throw std::runtime_error("set_attitude() not implemented in derived class");
    }

    /**
     * @brief Sets the target attitude rate of the vehicle in the body frame and the target thrust force
     * @param attitude_rate The target attitude rate in the body frame (in degrees/s)
     * @param thrust_force The target thrust force (in Newton)
    */
    virtual void set_attitude_rate(const Eigen::Vector3d& attitude_rate, double thrust_force, double dt=0) {
        throw std::runtime_error("set_attitude_rate() not implemented in derived class");
    }

    /**
     * @brief Sets the target motor speed of the vehicle (from 0-100%)
    */
    virtual void set_motor_speed(const Eigen::VectorXd& motor_velocity, double dt=0) {
        throw std::runtime_error("set_motor_velocity() not implemented in derived class");
    }

protected:

    // The ROS 2 node
    rclcpp::Node::SharedPtr node_{nullptr};

    // Function pointer which will be instantiated with the function pointers passed in the configuration
    std::function<State()> get_vehicle_state{nullptr};
    std::function<VehicleStatus()> get_vehicle_status{nullptr};
    std::function<VehicleConstants()> get_vehicle_constants{nullptr};
};

} // namespace autopilot

The methods that can be implemented are:

• initialize: This method is called once when the controller is initialized. It is used to set the controller parameters.

• reset_controller: This method is called when the controller is reset. It is used to reset the controller internal state (for example the integral term).

• set_position: Set the desired position + yaw and yaw-rate (in deg, deg/s) to track (in the inertial frame in NED).

• set_velocity: Set the desired velocity to track (in the inertial frame in NED).

• set_body_velocity: Set the desired velocity to track (in the body frame in FRD).

• set_attitude: Set the desired attitude (in deg for a FRD body frame relative to a NED inertial frame) and total thrust (in Newton) to track.

• set_attitude_rate: Set the desired angular velocity to track (in deg/s for a FRD body frame relative to a NED inertial frame) and total thrust (in Newton) to track.

• set_motor_speed: Set the individual desired motor speed (0-100%)

Note that you do not need to implement all of these methods, only the ones that are necessary for your controller. However, keep in mind that all the autopilot modes can call any of these methods. If an autopilot mode calls a method that is not implemented:

1. A runtime exception will be thrown

2. The autopilot will switch to the failsafe mode (if available)

3. If the failsafe mode also calls a method that is not implemented, you are screwed.

In practice, most operation modes will call the set_position method to set the desired position to track. As a good rule of thumb you should always implement the most generic version of this method that receives references up to the snap (even if you do not make use these higher-order derivatives in your controller).

1. PID Controller - Mathematical Background and Implementation

In this section we detail the mathematical background for the PID controller that is implemented in the Pegasus Autopilot, which sends attitude and total thrust commands for the inner-loops of the vehicle to track. This is the same algorithm used in [Jac21, JCP22].

Consider a multirotor vehicle given by the double-integrator model:

\[\begin{split}\ddot{p} &= g e_3 \underbrace{- \frac{T}{m}R e_3}_{u} \\ &= g e_3 + u\end{split}\]

where \(u \in \mathbb{R}^3\) is the acceleration input of the system. Consider also the tracking error system:

\[\begin{split}e_p &= p - p_d \\ \dot{e}_p &= e_v = v - v_d\end{split}\]

The controller can then be given by the Proportional-Derivative (PD) controller with an acceleration feed-forward term

\[u = -K_p e_p -K_d e_v + a_d -ge_3,\]

can render the system globally assymptotically stable (GAS). From the equality

\[\begin{split}u &= - \frac{T}{m}R e_3 \\ &= - \frac{T}{m}R_z({\psi_{des}})R_y({\theta_{des}})R_x({\phi_{des}}) e_3 \\ & = - \frac{T}{m}R_z({\psi_{des}})r_{3d} \\\end{split}\]

we can take the total thrust (in Newton) to be given by \(T = m|| u ||\). Replacing in the previous equations yields

\[r_{3d} = -R_z^\top({\psi_{des}})\frac{u}{||u||}\]

But we also know that

\[\begin{split}r_{3d} = R_y({\theta_{des}})R_x({\phi_{des}}) e_3 = \begin{bmatrix} \cos(\phi_{des})\sin(\theta_{des}) \\ -\sin(\phi_{des}) \\ \cos(\theta_{des})\cos(\phi_{des}) \end{bmatrix} = \begin{bmatrix} r_{31} \\ r_{32} \\ r_{33} \end{bmatrix}\end{split}\]

Reference Frames

Reminder that the reference frames are defined as follows: - The inertial frame \(\mathcal{I}\) is defined according to the North-East-Down (NED) convention. - The body frame: \(\mathcal{B}\) is defined according to the Forward-Right-Down (FRD) convention.

Therefore, a minus sign appears in the \(r_{3d}\) as the body z-axis is pointing downwards, but the acceleration vector is pointing upwards.

Finally, we can compute the desired roll \(\phi_{des}\), pitch \(\theta_{des}\) angles as follows

\[\begin{split}\phi_{des} &= \arcsin(-r_{32}) \\ \theta_{des} &= \arctan \left(\frac{r_{31}}{r_{33}}\right)\end{split}\]

The corresponding code for computing the desired acceleration is implemented in pegasus_autopilot/autopilot_controllers/src/pid_controller.cpp. The code is shown below:

void PIDController::set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, const Eigen::Vector3d& snap, double yaw, double yaw_rate, double dt) {

    // Ignore jerk, snap and yaw_rate references
    (void) jerk;
    (void) snap;
    (void) yaw_rate;

    // Get the current state of the vehicle
    State state = get_vehicle_state();
    
    // Compute the position error and velocity error using the path desired position and velocity
    Eigen::Vector3d pos_error = position - state.position;
    Eigen::Vector3d vel_error = velocity - state.velocity;

    // Compute the desired control output acceleration for each controller
    Eigen::Vector3d u;
    const Eigen::Vector3d g(0.0, 0.0, 9.81);
    for(unsigned int i=0; i < 3; i++) u[i] = controllers_[i]->compute_output(pos_error[i], vel_error[i], acceleration[i], dt); // (acceleration[i] - g[i])* mass_
    
    u[2] = u[2] - 9.81;

    // Convert the acceleration to attitude and thrust
    Eigen::Vector4d attitude_thrust = get_attitude_thrust_from_acceleration(u, mass_, Pegasus::Rotations::deg_to_rad(yaw));

    // Set the control output
    Eigen::Vector3d attitude_target = Eigen::Vector3d(
        Pegasus::Rotations::rad_to_deg(attitude_thrust[0]), 
        Pegasus::Rotations::rad_to_deg(attitude_thrust[1]), 
        Pegasus::Rotations::rad_to_deg(attitude_thrust[2]));

    // Send the attitude and thrust to the attitude controller
    set_attitude(attitude_target, attitude_thrust[3]);

    // Update and publish the PID statistics
    update_statistics(position);
    statistics_pub_->publish(pid_statistics_msg_);
}

The code for converting the desired acceleration into a set of desired roll and pitch angles + total thrust is shown below:

Eigen::Vector4d PIDController::get_attitude_thrust_from_acceleration(const Eigen::Vector3d & u, double mass, double yaw) {

    Eigen::Matrix3d RzT;
    Eigen::Vector3d r3d;
    Eigen::Vector4d attitude_thrust;

    /* Compute the normalized thrust and r3d vector */
    double T = mass * u.norm();

    /* Compute the rotation matrix about the Z-axis */
    RzT << cos(yaw), sin(yaw), 0.0,
          -sin(yaw), cos(yaw), 0.0,
                0.0,      0.0, 1.0;

    /* Compute the normalized rotation */
    r3d = -RzT * u / u.norm();

    /* Compute the actual attitude and setup the desired thrust to apply to the vehicle */
    attitude_thrust << asin(-r3d[1]), atan2(r3d[0], r3d[2]), yaw, T;
    return attitude_thrust;
}

Integral Action

Since the quadrotor can be viewed as a double integrator, the integral action is not necessary for the system to be stable, and the natural position controller that emerges is a Proportional-Derivative (PD). However, in practice a little bit of integral action can be used to improve the tracking performance of the system and accomodade for model uncertainties (such as the mass).

2. Mellinger Controller - Mathematical Background and Implementation

In this section we detail the mathematical background for the Mellinger controller that is implemented in the Pegasus Autopilot. It sends angular rates and total thrust commands to the inner-loops of the vehicle to be tracked. This is an adapted version of the algorithm proposed in [MK11, PGC21]. Reference trajectories \(\{p_{des},~v_{des},~a_{des},~j_{des}\}\) should be three times continuously differentiable.

The vehicle translational dynamics derive from Newton’s 2nd law of motion and are expressed as

\[m\dot{v} = mge_3 - TRe_3\]

where \(R = \left[ \begin{matrix} x_b & y_b & z_b \end{matrix} \right]\) denotes the vehicle orientation. Define the position and velocity tracking error as

\[\begin{split}e_p &= p - p_{des} \\ \dot{e}_p &= e_v = v - v_{des}\end{split}\]

The total force to be applied to the vehicle body is given by

\[F_{des} = -m (K_p e_p + K_d e_v + ge_3 - a_{des})\]

Next, compute the desired \(z_b\) axis from the desired total force

\[z_b^{des} = -\frac{F_{des}}{||F_{des}||}\]

Reference Frames

The reference frames are defined as follows: - The inertial frame \(\mathcal{I}\) is defined according to the North-East-Down (NED) convention. - The body frame: \(\mathcal{B}\) is defined according to the Forward-Right-Down (FRD) convention.

As the body z-axis is pointing downwards, but the force vector is pointing upwards, a minus sign appears in the expression of \(z_b^{des}\).

Get the desired total thrust to apply to the vehicle by projecting \(F_{des}\) onto the actual body z-axis (not the desired one that we computed before),

\[T = -F_{des}^\top z_b\]

The orientation kinematics are governed by the following equation

\[\dot{R} = R (\omega)_{\times}\]

where \((\omega)_{\times}\) is the skew-symmetric matrix of the angular velocity vector. To drive the vehicle orientation to a desired reference \(R_{des}\), we design a control law that provides angular rate setpoints. It is defined as

\[\omega = -K_R e_R + \omega_{des}\]

where the rotation error \(e_R\) is computed according to

\[e_R = \frac{1}{2}(R_{des}^\top R - R^\top R_{des})^\vee\]

The operator \(\vee\) maps a skew-symmetric matrix to a vector as follows

\[\begin{split}\begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}^\vee = \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix}\end{split}\]

We now present the steps to compute \(R_{des}\) and \(\omega_{des}\). To obtain \(R_{des} = \left[ \begin{matrix} x_b^{des} & y_b^{des} & z_b^{des} \end{matrix} \right]\), start by defining

\[y_c = \begin{bmatrix} -\sin(\psi_{des}) & \cos(\psi_{des}) & 0 \end{bmatrix}^\top\]

from the desired yaw angle \(\psi_{des}\). With this definition, the desired \(x_b^{des}\) and \(y_b^{des}\) axes are given by simple expressions,

\[\begin{split}x_b^{des} &= \frac{y_c^{des} \times z_b^{des}}{||y_c^{des} \times z_b^{des}||} \\ y_b^{des} &= z_b^{des} \times x_b^{des}\end{split}\]

As for \(\omega_{des} = \left[ \begin{matrix} p_{des} & q_{des} & r_{des} \end{matrix} \right]^\top\), let \(j\) denote the jerk of the vehicle and take the first time-derivative of the translational dynamics to get

\[\begin{split}\frac{d}{dt}(ma) &= \frac{d}{dt}(mge_3) - \frac{d}{dt}(TRe_3) \\ \Leftrightarrow m j &= -\dot{T}Re_3 - T\frac{d}{dt}\left(Re_3 \right) \\ &= -\dot{T}Re_3 - TR\omega \times Re_3 \\ &= -\dot{T}z_b - TR \left( \omega \times e_3 \right) \\ &= -\dot{T}z_b + T R \left(e_3 \times \omega \right) \\ &= -\dot{T}z_b + T (-q x_b + p y_b)\end{split}\]

• If we left multiply the jerk equation by \(x_b^{\top}\) we get

\[\begin{split}m x_b^{\top}j &= \underbrace{-\dot{T}x_b^{\top}z_b}_{0} + T (\underbrace{-q x_b^\top x_b}_{-q} + \underbrace{p y_b^\top x_b}_{0}) \\ \Leftrightarrow q &= -\frac{m}{T}x_b^{\top}j\end{split}\]

• If we left multiply the jerk equation by \(y_b^{\top}\) we get

\[\begin{split}m y_b^{\top}j &= \underbrace{-\dot{T}y_b^{\top}z_b}_{0} + T (\underbrace{-q x_b^\top y_b}_{0} + \underbrace{p y_b^\top y_b}_{p}) \\ \Leftrightarrow p &= \frac{m}{T}y_b^{\top}j\end{split}\]

• The angular velocity about the z-axis can be obtained from the first time derivative of the yaw angle

\[r = \dot{\psi} e_3^\top z_b\]

Then given orientation \(R_{des}\) and jerk \(j_{des}\) references, the desired angular rates are given by

\[\begin{split}\omega_{des} = \begin{bmatrix} \frac{m}{T}{y^{des}_b}^{\top}j^{des} \\ -\frac{m}{T}{x^{des}_b}^{\top}j^{des} \\ \dot{\psi}_{des} e_3^\top z_b^{des} \end{bmatrix}\end{split}\]

This controller is implemented in pegasus_autopilot/autopilot_controllers/src/mellinger_controller.cpp. The code is shown below:

void MellingerController::set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, const Eigen::Vector3d& snap, double yaw, double yaw_rate, double dt) {

    // Ignore snap references
    (void) snap;

    // Get the current state of the vehicle
    State state = get_vehicle_state();

    // Get the current attitude in quaternion and generate a rotation matrix
    Eigen::Matrix3d R = state.attitude.toRotationMatrix();

    // Get the attitude but in euler angles (for debugging purposes only)
    Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0);

    // Get the current yaw and yaw-rate from degrees to radians
    double yaw_rad = Pegasus::Rotations::deg_to_rad(yaw);
    double yaw_rate_rad = Pegasus::Rotations::deg_to_rad(yaw_rate);
    
    // Compute the position error and velocity error using the path desired position and velocity
    Eigen::Vector3d pos_error = position - state.position;
    Eigen::Vector3d vel_error = velocity - state.velocity;

    Eigen::Vector3d external_force = Eigen::Vector3d(0.0, 0.0, 0.0);
    external_force[0] = acceleration[0];
    external_force[1] = acceleration[1];
    external_force[2] = acceleration[2] - 9.81;

    // Compute the desired force output using a PID scheme
    Eigen::Vector3d F_des;
    for(unsigned int i=0; i < 3; i++) F_des[i] = mass_ * controllers_[i]->compute_output(pos_error[i], vel_error[i], external_force[i], dt);

    // Compute the desired body-frame axis Z_b (b3d)
    // Check [3-eq.12] for more details
    Eigen::Vector3d Z_b_des = -F_des / F_des.norm();

    // Compute Y_C
    Eigen::Vector3d Y_C = Eigen::Vector3d(-sin(yaw_rad), cos(yaw_rad), 0.0);

    // Compute X_B_des
    Eigen::Vector3d X_b_des = Y_C.cross(Z_b_des);
    X_b_des = X_b_des / X_b_des.norm();

    // Compute Y_B_des
    Eigen::Vector3d Y_b_des = Z_b_des.cross(X_b_des);
    Y_b_des = Y_b_des / Y_b_des.norm();

    // Compute the desired rotation R_des = [X_b_des | Y_b_des | Z_b_des]
    Eigen::Matrix3d R_des;
    R_des.col(0) = X_b_des;
    R_des.col(1) = Y_b_des;
    R_des.col(2) = Z_b_des;

    // Get the desired Euler-angles (for debugging purposes only)
    Eigen::Vector3d euler_angles_des = R_des.eulerAngles(2, 1, 0);

    // Compute the rotation error
    Eigen::Matrix3d R_error = (R_des.transpose() * R) - (R.transpose() * R_des);

    // Compute the vee map of the rotation error and project into the coordinates of the manifold
    Eigen::Vector3d e_R;
    e_R << -R_error(1,2), R_error(0, 2), -R_error(0,1);
    e_R = 0.5 * e_R;

    // Get the desired total thrust (in Newtons) in Z_B direction (u_1)
    Eigen::Vector3d Z_B = R.col(2);
    double T = -F_des.dot(Z_B);

    // Compute the desired angular velocity for the feed-forward terms
    Eigen::Vector3d w_des;
    w_des(0) =  mass_ / T * Y_b_des.dot(jerk);
    w_des(1) = -mass_ / T * X_b_des.dot(jerk);
    w_des(2) = yaw_rate_rad * Z_b_des[2];

    // Compute the target attitude rate
    Eigen::Vector3d attitude_rate = w_des - (kr_ * e_R);

    // Convert the output to rad/s
    attitude_rate = Eigen::Vector3d(
        Pegasus::Rotations::rad_to_deg(attitude_rate[0]), 
        Pegasus::Rotations::rad_to_deg(attitude_rate[1]), 
        Pegasus::Rotations::rad_to_deg(attitude_rate[2]));

    // Send the attitude rate and thrust to the attitude-rate controller
    set_attitude_rate(attitude_rate, T);

    // Update and publish the statistics
    update_statistics(position, e_R, w_des, T, attitude_rate, euler_angles, euler_angles_des);
    statistics_pub_->publish(statistics_msg_);
}

3. Adding a Custom Controller

TODO