Nonlinear Model Predictive Control (NMPC) for Quadcopter 

 Introduction: Beyond Basic Control – Understanding Model Fidelity in Drone Flight

Our journey began with the fundamental question: "How can we accurately model a quadcopter's complex, nonlinear dynamics?" This led to our initial project, where we meticulously developed a high-fidelity nonlinear drone model, detailing its configuration, physical properties, and the intricate dynamic equations governing its flight.

Following this, our second project tackled the crucial question: "Can a classical controller provide stable flight for a nonlinear system, and what are the implications of simplifying the model?" In this phase, we implemented and rigorously tuned classical controllers, such as a cascaded PID system. While capable of achieving stable flight, we consistently observed a somewhat sluggish response and found that PID tuning required substantial, iterative effort to achieve desired agility or faster trajectory tracking. We then critically examined the implications of model fidelity by linearizing the drone's dynamics, analyzing its open-loop behavior, and comparing the performance of a controller designed for the linear model when applied to both its simplified representation and the full nonlinear system. This work highlighted the crucial differences between linear and nonlinear control design, reinforcing the necessity of considering true nonlinear dynamics for developing robust systems.

This third and culminating project introduces Nonlinear Model Predictive Control (NMPC) to explicitly address the limitations and questions raised by the previous two: "How can we achieve highly responsive, precise, and constrained-aware control for a nonlinear quadcopter, effectively overcoming the practical challenges of classical methods?" By leveraging the detailed nonlinear drone model from our initial project, NMPC fundamentally changes how we control the quadcopter.

1. Quadcopter Physical Model (Dynamics) 

The quadcopter's motion is described by a set of nonlinear differential equations, capturing its position, linear velocities, angular orientation (Euler angles), and angular velocities. The detailed configuration of the drone, including its physical properties and parameters, was established and discussed in our previous project focusing on drone design and kinematics/dynamics modeling. For completeness within this control context, the state vector x is 12x1 and is defined as:

x = [pos_x; pos_y; pos_z; uvw_x; uvw_y; uvw_z; phi; theta; psi; p; q; r]

Where:

• pos_x, pos_y, pos_z: Position in the inertial (world) frame (meters)

• uvw_x, uvw_y, uvw_z: Linear velocities in the body frame (meters/second)

• phi: Roll angle (radians)

• theta: Pitch angle (radians)

• psi: Yaw angle (radians)

• p, q, r: Angular velocities about the body-fixed x, y, z axes respectively (radians/second)

The control inputs u_rpms are the four motor angular velocities (RPMs): [omega1; omega2; omega3; omega4].

The core of the model is represented by the drone_dynamics function, which computes the time derivatives of the state vector (dxdt).

1.1. Forces and Torques and Physical Constraints

This section outlines the forces and torques acting on the quadcopter, incorporating the physical properties (mass m, gravity g, inertia matrix I, arm length L, thrust coefficient kF, moment coefficient kM, air density rho, reference area A, and drag coefficients Cd_u, Cd_v, Cd_w) that were defined based on our drone's configuration.

Motor Thrusts: Each motor produces a thrust force proportional to the square of its RPM: F_i = kF * omega_i^2

Total Thrust (Body Z-axis): The total thrust acts along the body's negative Z-axis (upwards): F_thrust_body = [0; 0; -(F1 + F2 + F3 + F4)]

Motor Torques:

• Roll Torque (tau_phi): Caused by differential thrust between motors 2 and 4. tau_phi = L * (F2 - F4)

• Pitch Torque (tau_theta): Caused by differential thrust between motors 3 and 1. tau_theta = L * (F3 - F1)

• Yaw Torque (tau_psi): Caused by drag moments of the propellers. tau_psi = kM * (omega1^2 - omega2^2 + omega3^2 - omega4^2)

• Combined Motor Torques (Body Frame): T_motor_body = [tau_phi; tau_theta; tau_psi]

Aerodynamic Drag Forces: Drag forces oppose the relative air velocity (v_air_body = uvw_body - wind_body). Fd_u = -0.5 * rho * A * Cd_u * u_air * abs(u_air) Fd_v = -0.5 * rho * A * Cd_v * v_air * abs(v_air) Fd_w = -0.5 * rho * A * Cd_w * w_air * abs(w_air) F_drag_body = [Fd_u; Fd_v; Fd_w]

Gravity Force (in Body Frame): Gravity, which always acts downwards in the inertial frame, must be rotated into the body frame: F_gravity_body = R_body_to_inertial' * [0; 0; m * g]

Motor RPM Limits (Hard Constraints): A crucial physical aspect is the operational limits of the motors. We have defined max_motor_rpm (20000 RPM) and min_motor_rpm (0 RPM). These are incorporated as hard constraints directly into the NMPC optimization problem. This is a significant advantage over classical controllers where actuator saturation often needs to be handled outside the controller, potentially leading to instability or performance degradation. The NMPC explicitly calculates control sequences that respect these physical boundaries, ensuring that the commanded RPMs are always achievable.

1.2. Equations of Motion

The dynamics are described by the following differential equations:

1. Translational Velocity in Inertial Frame (Change in Position): dpos_dt_inertial = R_body_to_inertial * uvw_body

2. Linear Acceleration in Body Frame (Change in Body Linear Velocity): This is derived from Newton's second law: F = m * a. In the rotating body frame, we must also consider the Coriolis term (omega_body x uvw_body). dv_dt_body = (F_thrust_body + F_drag_body + F_gravity_body) / m - cross(omega_body, uvw_body)

3. Euler Angle Rates (Change in Orientation): This relates the angular velocities in the body frame (p, q, r) to the rates of change of Euler angles (phi, theta, psi). deul_dt = W_eul * omega_body where W_eul is the Euler angle rate transformation matrix: W_eul = [1 sin(phi)*tan(theta) cos(phi)*tan(theta); 0 cos(phi) -sin(phi); 0 sin(phi)/cos(theta) cos(phi)/cos(theta)]

4. Angular Acceleration in Body Frame (Change in Body Angular Velocity): This is derived from Euler's rotational equations of motion: T = I * d(omega)/dt + omega x (I * omega). domega_dt_body = inv(I) * (T_motor_body - cross(omega_body, I * omega_body))

1.3. Rotation Matrix from Euler Angles

The rotation matrix R_body_to_inertial converts vectors from the body frame to the inertial frame using a ZYX (yaw-pitch-roll) convention:

Rz (Yaw): [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]

Ry (Pitch): [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)]

Rx (Roll): [1 0 0; 0 cos(phi) -sin(phi); 0 sin(phi) cos(phi)]

R_body_to_inertial = Rz * Ry * Rx (Matrix multiplication order matter

2. Nonlinear Model Predictive Control (NMPC) Design 

NMPC is an advanced control strategy that works by solving an optimal control problem at each time step. This capability is particularly vital for overcoming the sluggish responses and extensive PID tuning efforts encountered in our earlier projects. Unlike PID, NMPC uses a predictive model and optimization to inherently achieve desired performance under complex dynamics and constraints.

2.1. NMPC Core Idea

1. Prediction: At the current time t, the controller uses the nonlinear drone dynamics model to predict the drone's future behavior over a finite "prediction horizon" (Np steps into the future). This predictive capability allows NMPC to anticipate future states and actively counteract disturbances like wind, which was modeled as a constant [2; 0; 0] m/s (2 m/s in X-direction) for robustness testing.

2. Optimization: It then calculates an optimal sequence of control inputs (motor RPMs) over this prediction horizon. This is done by minimizing a "cost function" that quantifies the desired performance (e.g., how close the drone is to its target, how much control effort is used, and how smooth the angular movements are). The fmincon function in MATLAB is used for this nonlinear optimization.

3. Receding Horizon: Only the first control input from this optimal sequence is applied to the actual drone. At the next time step, the process is repeated with new measurements, in a "receding horizon" fashion. This continuous re-optimization provides robust feedback control.

2.2. Cost Function (nmpc_objective)

The nmpc_objective function is the heart of the NMPC, guiding the fmincon optimizer. It calculates a scalar cost based on the predicted states and control inputs over the horizon. The cost function consists of two main parts:

• Running Cost: Applied at each intermediate step (i < Np) within the prediction horizon. It serves primarily as a regularizer, ensuring smoother transitions and preventing numerical issues.

  • Q_running: Penalizes deviations from the desired state.

  • R_running: Penalizes the magnitude of the control inputs (motor RPMs), encouraging less aggressive control.

• Terminal Cost: Applied only at the final step (i = Np) of the prediction horizon. This is crucial for settling time as it strongly penalizes any remaining error at the end of the predicted trajectory. This emphasis on the terminal state helps ensure the drone quickly reaches and holds its target, a direct improvement over the sluggishness often seen with classical methods.

The total cost is a sum of these weighted terms: Cost = SUM ( (State_Deviation_i)' * Q_running * (State_Deviation_i) + (Control_Input_i)' * R_running * (Control_Input_i) ) for i = 1 to Np-1 + (State_Deviation_Np)' * P_terminal * (State_Deviation_Np)

Where:

• State_Deviation = Predicted_State - Desired_State

• Predicted_State: The drone's state predicted at step i.

• Desired_State: The constant target state for the drone.

• Control_Input_i: The motor RPMs applied at step i.

Current Weighting Matrices (tuned for settling time and lower angular rates):

• Q_running (Running State Deviation Penalties): Q_running = diag([5; 5; 5; % Position (x,y,z) 0.1; 0.1; 0.1; % Body Linear Velocities (u,v,w) 0.1; 0.1; 0.1; % Euler Angles (phi,theta,psi) 100; 100; 100]) % Body Angular Velocities (p,q,r) - Heavily increased to smooth rotations

• R_running (Running Control Effort Penalties): R_running = diag([1e-6; 1e-6; 1e-6; 1e-6])

• P_terminal (Terminal State Deviation Penalties):P_terminal = diag([10000; 10000; 10000; % Position (x,y,z) - High emphasis for settling 100; 100; 100; % Body Linear Velocities (u,v,w) 5000; 5000; 2000; % Euler Angles (phi,theta,psi) - High emphasis on roll/pitch 10000; 20000; 4000]) % Body Angular Velocities (p,q,r) - Very high emphasis to drive to zero at horizon end 

2.3. Optimization with fmincon

fmincon is used to find the optimal sequence of control inputs (optimal_u_seq) that minimizes the nmpc_cost_function.

• Initial Guess (last_optimal_u_seq): A crucial aspect is "warm-starting" fmincon. Instead of starting from scratch, the optimal control sequence from the previous time step is shifted and used as the initial guess for the current step. This significantly speeds up convergence, crucial for real-time performance. last_optimal_u_seq = [optimal_u_seq(5:end); repmat(u_hover_rpm, 4, 1)]

• Bounds: lb_fmincon and ub_fmincon enforce the minimum (0 RPM) and maximum (20000 RPM) motor speeds as hard constraints. This is where the NMPC explicitly ensures that the calculated control commands are physically realizable.

• Options: MaxIterations = 100 and MaxFunctionEvaluations = 500 provide fmincon sufficient computational budget at each time step.

3. Simulation Setup 

The simulation setup is configured to test the NMPC's performance under various conditions:

• Initial Conditions (pos0_cl): The drone starts at [2; 1; 0] meters (x, y, z). This initial condition, starting from the ground and requiring vertical ascent, provides a robust test of the NMPC's ability to handle significant initial errors.

• Desired State (pos_des_cl): The target is to reach [0; 0; 5] meters (x, y, z). The desired Euler angles are [0; 0; 0] (level flight) and desired angular velocities are [0; 0; 0] (stable hover).

• External Disturbances (wind_for_simulation): A constant wind of [2; 0; 0] m/s (2 m/s in X-direction) is applied to test robustness. The NMPC's predictive nature helps it anticipate and compensate for such known disturbances.

• Simulation Time:

  • T_sim = 50 seconds: Total simulation duration (Updated to 50 seconds).

  • dt_sim = 0.05 seconds: Time step for the main simulation.

  • Np = 30: Number of prediction steps for NMPC (Updated to 30 steps).

  • dt_mpc = dt_sim: Time step for NMPC's internal prediction model (0.05 seconds).

  • This means the NMPC has a prediction horizon of Np * dt_mpc = 30 * 0.05 = 1.5 seconds

4. Performance Metrics 

The simulation evaluates performance using:

• Tracking RMSE (Root Mean Square Error): Calculated for X, Y, and Z positions. It quantifies the average deviation from the desired position over the entire simulation. Lower values indicate better tracking. RMSE_axis = sqrt(mean(Error_axis.^2))

• Settling Time (X-axis, 2% tolerance): Measures the time it takes for the X-position to reach and remain within 2% of its initial deviation from the desired X-position. This directly assesses how quickly the controller drives the drone to the target, a key performance indicator that this NMPC design prioritizes.

• Plots: Visual representations of position, Euler angles, and body angular velocities over time are generated to provide a qualitative assessment of the drone's behavior and stability, helping to identify and fine-tune aspects like transient angular rates.

This NMPC project, with its robust handling of nonlinear dynamics, explicit constraint management, and predictive capabilities, effectively completes and significantly enhances the control system developed in our previous drone-related projects. It moves beyond the limitations of classical controllers, providing a more powerful and adaptable solution for complex drone flight control.

5. Conclusion and Future Work 

This NMPC project represents a successful culmination of our drone control development efforts. By transitioning from classical PID methods to a Nonlinear Model Predictive Control framework, we have demonstrably overcome the limitations of sluggish responses and the arduous, iterative process of PID tuning.

The NMPC controller, utilizing a detailed nonlinear dynamic model (whose configuration and properties were established in our initial drone mechanics project), has proven highly effective. It consistently achieves rapid settling times (e.g., 5.40 seconds for the X-axis) and significantly reduced RMSE in position tracking, even when starting from challenging initial conditions or facing external wind disturbances. A key advantage of NMPC, and one of our primary goals, is its inherent ability to directly incorporate physical constraints such as motor RPM limits, ensuring that the commanded controls are always realistic and safe for the drone. Through careful tuning of the cost function, particularly the running and terminal penalties on angular velocities, we've also managed to achieve smoother transient behaviors, making the drone's movements more controlled and physically plausible.

This project effectively completes and significantly enhances the control system developed in our previous drone-related projects, offering a more robust, adaptable, and high-performance solution for complex drone flight control.

Future Work: Further research could explore:

• Implementing state constraints (e.g., maximum allowed roll/pitch angles) as hard constraints within fmincon to guarantee strict adherence to operational envelopes.

• Investigating real-time optimization techniques or more computationally efficient NMPC formulations for deployment on actual drone hardware.

• Including disturbance observers to actively estimate and compensate for unmodeled wind gusts or other external forces, further enhancing robustness.

• Exploring trajectory tracking for dynamic paths rather than just set-point regulation, which would involve providing a time-varying desired state to the NMPC.