PID Controller

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].

Mathematical Background

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_d - p \\ \dot{e}_p &= e_v = v_d - v\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}\]

Implementation

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

 1void 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) {
 2
 3    // Ignore jerk, snap and yaw_rate references
 4    (void) jerk;
 5    (void) snap;
 6    (void) yaw_rate;
 7
 8    // Get the current state of the vehicle
 9    State state = get_vehicle_state();
10    
11    // Compute the position error and velocity error using the path desired position and velocity
12    Eigen::Vector3d pos_error = position - state.position;
13    Eigen::Vector3d vel_error = velocity - state.velocity;
14
15    // Compute the desired control output acceleration for each controller
16    Eigen::Vector3d u;
17    const Eigen::Vector3d g(0.0, 0.0, 9.81);
18    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_
19    
20    u[2] = u[2] - 9.81;
21
22    // Convert the acceleration to attitude and thrust
23    Eigen::Vector4d attitude_thrust = get_attitude_thrust_from_acceleration(u, mass_, Pegasus::Rotations::deg_to_rad(yaw));
24
25    // Set the control output
26    Eigen::Vector3d attitude_target = Eigen::Vector3d(
27        Pegasus::Rotations::rad_to_deg(attitude_thrust[0]), 
28        Pegasus::Rotations::rad_to_deg(attitude_thrust[1]), 
29        Pegasus::Rotations::rad_to_deg(attitude_thrust[2]));
30
31    // Send the attitude and thrust to the attitude controller
32    set_attitude(attitude_target, attitude_thrust[3]);
33
34    // Update and publish the PID statistics
35    update_statistics(position);
36    statistics_pub_->publish(pid_statistics_msg_);
37}

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

 1Eigen::Vector4d PIDController::get_attitude_thrust_from_acceleration(const Eigen::Vector3d & u, double mass, double yaw) {
 2
 3    Eigen::Matrix3d RzT;
 4    Eigen::Vector3d r3d;
 5    Eigen::Vector4d attitude_thrust;
 6
 7    /* Compute the normalized thrust and r3d vector */
 8    double T = mass * u.norm();
 9
10    /* Compute the rotation matrix about the Z-axis */
11    RzT << cos(yaw), sin(yaw), 0.0,
12          -sin(yaw), cos(yaw), 0.0,
13                0.0,      0.0, 1.0;
14
15    /* Compute the normalized rotation */
16    r3d = -RzT * u / u.norm();
17
18    /* Compute the actual attitude and setup the desired thrust to apply to the vehicle */
19    attitude_thrust << asin(-r3d[1]), atan2(r3d[0], r3d[2]), yaw, T;
20    return attitude_thrust;
21}

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).