Mellinger Controller

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.

Mathematical Background

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

Implementation

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

 1void 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) {
 2
 3    // Ignore snap references
 4    (void) snap;
 5
 6    // Get the current state of the vehicle
 7    State state = get_vehicle_state();
 8
 9    // Get the current attitude in quaternion and generate a rotation matrix
10    Eigen::Matrix3d R = state.attitude.toRotationMatrix();
11
12    // Get the attitude but in euler angles (for debugging purposes only)
13    Eigen::Vector3d euler_angles = R.eulerAngles(2, 1, 0);
14
15    // Get the current yaw and yaw-rate from degrees to radians
16    double yaw_rad = Pegasus::Rotations::deg_to_rad(yaw);
17    double yaw_rate_rad = Pegasus::Rotations::deg_to_rad(yaw_rate);
18    
19    // Compute the position error and velocity error using the path desired position and velocity
20    Eigen::Vector3d pos_error = position - state.position;
21    Eigen::Vector3d vel_error = velocity - state.velocity;
22
23    Eigen::Vector3d external_force = Eigen::Vector3d(0.0, 0.0, 0.0);
24    external_force[0] = acceleration[0];
25    external_force[1] = acceleration[1];
26    external_force[2] = acceleration[2] - 9.81;
27
28    // Compute the desired force output using a PID scheme
29    Eigen::Vector3d F_des;
30    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);
31
32    // Compute the desired body-frame axis Z_b (b3d)
33    // Check [3-eq.12] for more details
34    Eigen::Vector3d Z_b_des = -F_des / F_des.norm();
35
36    // Compute Y_C
37    Eigen::Vector3d Y_C = Eigen::Vector3d(-sin(yaw_rad), cos(yaw_rad), 0.0);
38
39    // Compute X_B_des
40    Eigen::Vector3d X_b_des = Y_C.cross(Z_b_des);
41    X_b_des = X_b_des / X_b_des.norm();
42
43    // Compute Y_B_des
44    Eigen::Vector3d Y_b_des = Z_b_des.cross(X_b_des);
45    Y_b_des = Y_b_des / Y_b_des.norm();
46
47    // Compute the desired rotation R_des = [X_b_des | Y_b_des | Z_b_des]
48    Eigen::Matrix3d R_des;
49    R_des.col(0) = X_b_des;
50    R_des.col(1) = Y_b_des;
51    R_des.col(2) = Z_b_des;
52
53    // Get the desired Euler-angles (for debugging purposes only)
54    Eigen::Vector3d euler_angles_des = R_des.eulerAngles(2, 1, 0);
55
56    // Compute the rotation error
57    Eigen::Matrix3d R_error = (R_des.transpose() * R) - (R.transpose() * R_des);
58
59    // Compute the vee map of the rotation error and project into the coordinates of the manifold
60    Eigen::Vector3d e_R;
61    e_R << -R_error(1,2), R_error(0, 2), -R_error(0,1);
62    e_R = 0.5 * e_R;
63
64    // Get the desired total thrust (in Newtons) in Z_B direction (u_1)
65    Eigen::Vector3d Z_B = R.col(2);
66    double T = -F_des.dot(Z_B);
67
68    // Compute the desired angular velocity for the feed-forward terms
69    Eigen::Vector3d w_des;
70    w_des(0) =  mass_ / T * Y_b_des.dot(jerk);
71    w_des(1) = -mass_ / T * X_b_des.dot(jerk);
72    w_des(2) = yaw_rate_rad * Z_b_des[2];
73
74    // Compute the target attitude rate
75    Eigen::Vector3d attitude_rate = w_des - (kr_ * e_R);
76
77    // Convert the output to rad/s
78    attitude_rate = Eigen::Vector3d(
79        Pegasus::Rotations::rad_to_deg(attitude_rate[0]), 
80        Pegasus::Rotations::rad_to_deg(attitude_rate[1]), 
81        Pegasus::Rotations::rad_to_deg(attitude_rate[2]));
82
83    // Send the attitude rate and thrust to the attitude-rate controller
84    set_attitude_rate(attitude_rate, T);
85
86    // Update and publish the statistics
87    update_statistics(position, e_R, w_des, T, attitude_rate, euler_angles, euler_angles_des);
88    statistics_pub_->publish(statistics_msg_);
89}