The rapid development of new materials, micro-electro-mechanical systems (MEMS), and flight control technologies has propelled multi-rotor unmanned aerial vehicles (UAVs) into the spotlight of international research. The quadrotor drone, a prominent member of this family, is distinguished by its simple mechanical structure, exceptional maneuverability, and unique flight mechanics. As a nonlinear, under-actuated system with six degrees of freedom and only four control inputs, designing and validating robust control algorithms for the quadrotor drone remains a central challenge. Simulation plays a pivotal role in this process, offering a safe and cost-effective environment for algorithm testing before risky and expensive physical flight tests. However, traditional numerical simulations often lack an intuitive, visual representation of the drone’s real-time state, limiting the designer’s ability to fully assess performance. This gap underscores the significant value of developing a high-fidelity, three-dimensional visual simulation system. Such a system allows researchers to observe the dynamic behavior of the quadrotor drone in a virtual space, facilitating more efficient controller tuning, performance evaluation, and ultimately, accelerating the development cycle while ensuring flight safety.
Kinematic and Dynamic Modeling of the Quadrotor Drone
Accurate modeling is the foundation of any effective simulation system. The quadrotor drone’s motion is governed by the forces and moments generated by its four rotors. We consider an X-configuration quadrotor drone, where motors 1 and 3 are positioned forward. Motors 1 and 2 rotate counter-clockwise, while motors 3 and 4 rotate clockwise (positive direction defined as clockwise). Several standard assumptions are made: the quadrotor drone is a rigid and symmetrical body; the body-fixed coordinate frame originates at its center of mass; and the ground (inertial) frame is considered inertial.
The thrust $F_i$ and torque $T_i$ produced by the $i$-th motor are given by:
$$ F_i = k_b \omega_i^2, \quad T_i = k_d \omega_i^2, \quad i = 1, 2, 3, 4 $$
where $k_b$ is the thrust coefficient, $k_d$ is the drag (torque) coefficient, and $\omega_i$ is the angular speed of the $i$-th motor.
Let the Euler angles be $\boldsymbol{\eta} = [\phi, \theta, \psi]^T$ representing roll, pitch, and yaw, respectively. The angular velocity in the body frame is $\boldsymbol{\Omega} = [p, q, r]^T$. Ignoring aerodynamic drag, the total thrust vector in the body frame $\mathcal{F}_b$ is:
$$ \mathbf{F}_b = \begin{bmatrix} 0 & 0 & \sum_{i=1}^{4} F_i \end{bmatrix}^T $$
This thrust is transformed to the inertial frame $\mathcal{F}_g$ via the rotation matrix $\mathbf{R}$:
$$ \mathbf{F}_g = \mathbf{R} \mathbf{F}_b – m g \mathbf{E}_z $$
where $m$ is the mass, $g$ is gravity, and $\mathbf{E}_z = [0,0,1]^T$. The rotation matrix $\mathbf{R}$ from the body to the inertial frame is:
$$
\mathbf{R} = \begin{bmatrix}
c_\theta c_\psi & s_\theta c_\psi s_\phi – s_\psi c_\phi & s_\theta c_\psi c_\phi + s_\psi s_\phi \\
c_\theta s_\psi & s_\theta s_\psi s_\phi + c_\psi c_\phi & s_\theta s_\psi c_\phi – c_\psi s_\phi \\
-s_\theta & c_\theta s_\phi & c_\theta c_\phi
\end{bmatrix}
$$
where $c_x = \cos(x)$ and $s_x = \sin(x)$.
Applying Newton’s second law in the inertial frame yields the translational dynamics:
$$
\begin{aligned}
\ddot{x} &= \frac{1}{m} \sum_{i=1}^{4} F_i (\sin\theta \cos\psi \cos\phi + \sin\psi \sin\phi) \\
\ddot{y} &= \frac{1}{m} \sum_{i=1}^{4} F_i (\sin\theta \sin\psi \cos\phi – \cos\psi \sin\phi) \\
\ddot{z} &= \frac{1}{m} \sum_{i=1}^{4} F_i (\cos\theta \cos\phi) – g
\end{aligned}
$$
The rotational dynamics in the body frame are derived from Euler’s rotation equation:
$$ \mathbf{M}_b = \mathbf{J}_b \dot{\boldsymbol{\Omega}} + \boldsymbol{\Omega} \times \mathbf{J}_b \boldsymbol{\Omega} $$
where $\mathbf{J}_b = \text{diag}(J_x, J_y, J_z)$ is the inertia tensor. For small attitude angles, the relationship between Euler angle rates and body angular rates is approximately $\dot{\boldsymbol{\eta}} \approx \boldsymbol{\Omega}$. This leads to the following rotational equations:
$$
\begin{aligned}
M_x &= J_x \ddot{\phi} + (J_z – J_y) \dot{\theta} \dot{\psi} \\
M_y &= J_y \ddot{\theta} + (J_x – J_z) \dot{\phi} \dot{\psi} \\
M_z &= J_z \ddot{\psi} + (J_y – J_x) \dot{\phi} \dot{\theta}
\end{aligned}
$$
To simplify control design, we define the following virtual control inputs based on the motor speeds, where $l$ is the distance from each motor to the center of mass:
$$
\begin{aligned}
u_1 &= \sum_{i=1}^{4} F_i = k_b (\omega_1^2 + \omega_2^2 + \omega_3^2 + \omega_4^2) \quad &\text{(Total Thrust)} \\
u_2 &= l (F_2 + F_3 – F_1 – F_4) = l k_b (\omega_2^2 + \omega_3^2 – \omega_1^2 – \omega_4^2) \quad &\text{(Roll Moment)} \\
u_3 &= l (F_1 + F_3 – F_2 – F_4) = l k_b (\omega_1^2 + \omega_3^2 – \omega_2^2 – \omega_4^2) \quad &\text{(Pitch Moment)} \\
u_4 &= T_3 + T_4 – T_1 – T_2 = k_d (\omega_3^2 + \omega_4^2 – \omega_1^2 – \omega_2^2) \quad &\text{(Yaw Moment)}
\end{aligned}
$$
The body moments are directly related to these inputs: $M_x = u_2$, $M_y = u_3$, $M_z = u_4$.
Combining the translational and rotational equations, the complete nonlinear dynamic model for the quadrotor drone is:
$$
\boxed{
\begin{aligned}
\ddot{x} &= \frac{u_1}{m} (\sin\theta \cos\psi \cos\phi + \sin\psi \sin\phi) \\
\ddot{y} &= \frac{u_1}{m} (\sin\theta \sin\psi \cos\phi – \cos\psi \sin\phi) \\
\ddot{z} &= \frac{u_1}{m} \cos\theta \cos\phi – g \\
\ddot{\phi} &= \frac{1}{J_x} [ u_2 – (J_z – J_y) \dot{\theta} \dot{\psi} ] \\
\ddot{\theta} &= \frac{1}{J_y} [ u_3 – (J_x – J_z) \dot{\phi} \dot{\psi} ] \\
\ddot{\psi} &= \frac{1}{J_z} [ u_4 – (J_y – J_x) \dot{\phi} \dot{\theta} ]
\end{aligned}}
$$
This model forms the theoretical basis for the control system design implemented within the visual simulation platform.
| Parameter | Symbol | Value | Unit |
|---|---|---|---|
| Mass | $m$ | 1.2 | kg |
| Inertia (X-axis) | $J_x$ | 0.023 | kg·m² |
| Inertia (Y-axis) | $J_y$ | 0.023 | kg·m² |
| Inertia (Z-axis) | $J_z$ | 0.046 | kg·m² |
| Thrust Coefficient | $k_b$ | 1.5e-5 | N/(rad/s)² |
| Drag Coefficient | $k_d$ | 2.5e-7 | Nm/(rad/s)² |
| Arm Length | $l$ | 0.25 | m |
Design of the Visual Co-Simulation System
The core of this work is the integration of powerful modeling and simulation tools to create a realistic and interactive virtual testbed. The process leverages the complementary strengths of SolidWorks for high-fidelity 3D CAD modeling and MATLAB/Simulink with its SimMechanics toolbox for multi-body dynamics simulation and control system design.
3D Physical Modeling in SolidWorks
The first step involves constructing a detailed and physically accurate 3D model of the quadrotor drone. SolidWorks is the ideal tool for this task due to its intuitive interface and robust parametric modeling capabilities. The model is built based on the actual dimensions and geometry of a target quadrotor drone platform. To facilitate a modular and realistic simulation in SimMechanics, the assembly is logically divided into five distinct parts:
- The central Body/Hub: Contains the main frame, electronics bay, and landing gear.
- Four Motor-Propeller assemblies: Each consists of a motor model rigidly attached to a propeller.
This separation is crucial because the primary control actuation of the quadrotor drone comes from varying the speeds of these four independent motor-propeller assemblies. The parts are fully constrained within the SolidWorks assembly environment to reflect the real-world mechanical connections.
Importing into MATLAB/SimMechanics
To bridge the gap between the CAD model and the dynamics simulator, the SimMechanics Link plugin for SolidWorks is utilized. This plugin acts as a conversion interface. With the quadrotor drone assembly open in SolidWorks, the plugin is used to export the model into a Second Generation SimMechanics XML file (*.xml). This file encapsulates the geometry, mass properties, inertia, and assembly constraints of the model.
Within the MATLAB environment, the model is imported using the `smimport` command:
quadrotor_model = smimport('quadrotor_assembly.xml');
This command automatically generates a corresponding Simulink/SimMechanics block diagram model. The imported model in SimMechanics consists of several key blocks:
- A World Frame block representing the inertial (ground) coordinate system.
- A Body Block for the main chassis.
- Four Body Blocks for the motor-propeller assemblies.
- Joint Blocks connecting the propellers to the main body. These are configured as Revolute joints with a single degree of freedom (rotation about the motor axis).
- A 6-DOF Joint block connecting the main body to the World Frame. This joint provides the full translational and rotational motion of the quadrotor drone relative to the ground.
SimMechanics provides a built-in Mechanics Explorer, a 3D visualization window that renders the imported geometry. This allows for an immediate visual verification of the imported quadrotor drone model.
Integrating Actuation, Sensing, and Control
The raw SimMechanics model is a passive mechanical system. To simulate flight, actuation forces and feedback sensors must be added, and the model must be connected to a control algorithm.
- Actuation: A Joint Actuator block is added to each Revolute Joint (motor axis). This block applies a torque signal, which will ultimately be derived from the controller’s calculated motor inputs ($u_1$ to $u_4$). A Joint Sensor on each Revolute Joint measures the propeller’s angular velocity, which can be used for feedback if needed.
- Sensing: A Transform Sensor is attached to the 6-DOF Joint. This sensor provides the full state feedback required by the controller: the position $(x, y, z)$ and orientation $(\phi, \theta, \psi)$ of the quadrotor drone’s body in the inertial frame, as well as their linear and angular velocities.
- Gravity: The gravity vector in the World Frame block is configured to align with the negative Z-axis of the inertial frame, matching the dynamics model.
- Control System Integration: The completed SimMechanics plant model is saved as a subsystem. Its input ports are the four motor torque commands. Its output ports are the measured states (position, attitude, velocities). This subsystem is then placed within a Simulink model, where it can be easily connected to controller blocks developed in Simulink. This seamless integration is a key advantage, enabling rapid prototyping and testing of various control algorithms for the quadrotor drone.
Trajectory Tracking Control Design for the Quadrotor Drone
To demonstrate the utility of the visual simulation platform, a standard and widely used control strategy is implemented: a nested (cascaded) PID controller. This approach is chosen for its simplicity, reliability, and effectiveness in stabilizing the inherently unstable quadrotor drone.
The control architecture follows a hierarchical structure:
- Outer Loop (Position Control): This loop takes the desired trajectory $\mathbf{r}_{des} = [x_{des}, y_{des}, z_{des}]^T$ and the current position $\mathbf{r}$ and attitude $\psi$ as inputs. It generates desired setpoints for the roll ($\phi_{des}$) and pitch ($\theta_{des}$) angles, as well as the total thrust command ($u_{1,cmd}$).
- Inner Loop (Attitude Control): This loop takes the desired attitude $\boldsymbol{\eta}_{des} = [\phi_{des}, \theta_{des}, \psi_{des}]^T$ and the current attitude $\boldsymbol{\eta}$ as inputs. It generates the moment commands $u_2$, $u_3$, and $u_4$ to drive the attitude error to zero.
The control law for each channel is given by the PID formula:
$$ u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt} $$
where $e(t)$ is the error signal (e.g., $z_{des} – z$ for altitude, $\phi_{des} – \phi$ for roll).
The specific implementation for the quadrotor drone is as follows:
1. Altitude and Yaw Control:
These are directly controlled by dedicated PID controllers.
$$ u_1 = m \frac{(g + \ddot{z}_{des} + K_{p,z} e_z + K_{i,z} \int e_z + K_{d,z} \dot{e}_z)}{\cos\phi \cos\theta} $$
$$ u_4 = K_{p,\psi} e_\psi + K_{i,\psi} \int e_\psi + K_{d,\psi} \dot{e}_psi $$
Note: The calculation for $u_1$ includes a feedforward term and a nonlinear compensation ($\cos\phi \cos\theta$) to decouple the thrust from attitude.
2. Horizontal Position Control:
The desired horizontal accelerations are computed by PD controllers:
$$
\begin{aligned}
\ddot{x}_{cmd} &= K_{p,x} e_x + K_{d,x} \dot{e}_x \\
\ddot{y}_{cmd} &= K_{p,y} e_y + K_{d,y} \dot{e}_y
\end{aligned}
$$
These desired accelerations are then translated into desired roll and pitch angles through the following transformation, derived from the translational dynamics under small-angle approximations:
$$
\boxed{
\begin{aligned}
\phi_{des} &= \frac{1}{g} (\ddot{x}_{cmd} \sin\psi_{des} – \ddot{y}_{cmd} \cos\psi_{des}) \\
\theta_{des} &= \frac{1}{g} (\ddot{x}_{cmd} \cos\psi_{des} + \ddot{y}_{cmd} \sin\psi_{des})
\end{aligned}}
$$
3. Attitude Control:
Finally, the desired roll and pitch angles from the position loop, along with the desired yaw angle, are tracked by the inner-loop PID controllers.
$$
\begin{aligned}
u_2 &= K_{p,\phi} e_\phi + K_{i,\phi} \int e_\phi + K_{d,\phi} \dot{e}_\phi \\
u_3 &= K_{p,\theta} e_\theta + K_{i,\theta} \int e_\theta + K_{d,\theta} \dot{e}_\theta
\end{aligned}
$$
The four control signals $(u_1, u_2, u_3, u_4)$ are then converted into individual motor speed commands $\omega_i$ by inverting the relationship defined earlier:
$$
\begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix}
=
\begin{bmatrix}
1 & -1/(2l) & 1/(2l) & -1/(2k_d/k_b) \\
1 & -1/(2l) & -1/(2l) & 1/(2k_d/k_b) \\
1 & 1/(2l) & 1/(2l) & 1/(2k_d/k_b) \\
1 & 1/(2l) & -1/(2l) & -1/(2k_d/k_b)
\end{bmatrix}
\begin{bmatrix} u_1/(4k_b) \\ u_2/(k_b l) \\ u_3/(k_b l) \\ u_4/(4k_d) \end{bmatrix}
$$
These squared speeds are then used to calculate the required torque for each motor-propeller assembly in the SimMechanics model, typically assuming a linear relationship between squared speed and torque for simulation purposes.
| Control Loop | $K_p$ | $K_i$ | $K_d$ |
|---|---|---|---|
| Position – X | 2.5 | 0.05 | 1.8 |
| Position – Y | 2.5 | 0.05 | 1.8 |
| Position – Z | 25 | 5.0 | 8.0 |
| Attitude – $\phi$ | 8.0 | 1.5 | 0.8 |
| Attitude – $\theta$ | 8.0 | 1.5 | 0.8 |
| Attitude – $\psi$ | 6.0 | 1.0 | 0.6 |
Simulation Results and Analysis
The integrated visual simulation system was tested with a demanding trajectory tracking scenario to evaluate both the controller’s performance and the system’s visualization capabilities. The quadrotor drone was commanded to take off and follow a horizontal circular path while maintaining a constant altitude.
Simulation Parameters:
- Initial State: $[x, y, z, \phi, \theta, \psi] = [0, 0, 0, 0, 0, 0]$
- Desired Trajectory: $x_{des}(t) = \cos(t)$, $y_{des}(t) = \sin(t)$, $z_{des}(t) = 1$ meter, $\psi_{des}(t) = 0$ rad.
- Simulation Time: 10 seconds.
The Simulink model executed the simulation, solving the combined dynamics of the SimMechanics quadrotor drone model and the PID control algorithm. The results are presented both graphically and quantitatively.
Visual Simulation Output
The Mechanics Explorer window provided a real-time, three-dimensional view of the simulation. Observing the visualization confirmed the expected behavior:
- Take-off and Climb (0-2s): The quadrotor drone applied collective thrust to climb to the commanded altitude of 1 meter. A slight overshoot was visible, consistent with the transient response seen in the data plots.
- Circular Path Tracking (2-10s): The quadrotor drone successfully entered and maintained the circular path. The visualization clearly showed the coordinated bank (roll) and tilt (pitch) motions required to generate the centripetal force for turning, while the yaw angle remained steadily pointed forward. The motion was smooth and stable throughout the simulation.
The ability to watch this complex, coupled motion in 3D provides an immediate, intuitive assessment of control performance that is difficult to glean from plots alone.
Quantitative Trajectory Tracking Performance
The data logged from the simulation allows for a precise analysis of tracking error and controller stability.
Position Tracking:
- X-Position: The controller brought the quadrotor drone onto the cosine-wave reference path within approximately 2 seconds. The steady-state tracking error was minimal, demonstrating effective rejection of the coupling disturbances from lateral motion.
- Y-Position: Similar performance was observed for the sine-wave tracking. The slight phase lag visible in the initial seconds is attributable to the finite response time of the inner attitude loop.
- Altitude (Z-Position): A moderate overshoot of about 15% occurred during the aggressive take-off maneuver, settling to the desired 1-meter altitude within 3 seconds. The steady-state altitude error was negligible, showing the integral action’s effectiveness in eliminating bias.
Attitude Tracking:
- Yaw Angle ($\psi$): The yaw angle exhibited minor oscillations during the initial transient but was regulated to zero as commanded, with deviations of less than 0.05 radians during steady circular flight.
- Roll and Pitch Angles ($\phi, \theta$): These angles varied continuously to generate the forces needed for circular motion. Their profiles were sinusoidal and 90 degrees out of phase, which is the theoretically expected relationship for tracking a circle while maintaining a constant heading. The inner-loop PID controllers effectively tracked these time-varying attitude commands.
| Metric | X-Axis (RMS Error) | Y-Axis (RMS Error) | Z-Axis (RMS Error) | Yaw (Max Abs Error) |
|---|---|---|---|---|
| Value (t=3-10s) | 0.032 m | 0.028 m | 0.008 m | 0.046 rad |
The simulation results conclusively demonstrate that the developed visual trajectory tracking simulation system for the quadrotor drone is both functional and reliable. The system successfully integrates a high-fidelity physical model with a complex control algorithm and presents the outcomes in an intuitive visual format. The quantitative data confirms that the nested PID controller provides stable and accurate tracking for the given trajectory. More importantly, the platform allows a researcher to not only see the numerical results but also to visually inspect the realism of the quadrotor drone’s motion—checking for correct coordination, reasonable actuator activity, and the absence of anomalous behaviors. This combination is invaluable for control algorithm development.
Conclusion and Future Work
This paper presented the design and implementation of a comprehensive visual trajectory tracking simulation system for quadrotor drones. The system synergistically combines the 3D CAD modeling prowess of SolidWorks with the dynamic simulation and control design capabilities of MATLAB, Simulink, and SimMechanics. A detailed nonlinear dynamic model of the quadrotor drone was derived and served as the basis for a nested PID trajectory tracking controller. The core achievement is the seamless workflow: from creating a realistic 3D model of the quadrotor drone, to importing it into a physics-based simulation environment, and finally to connecting it with a control algorithm for interactive testing.
The primary advantages of this system are:
- High Visual Fidelity: The use of an actual CAD model, as opposed to simple primitives, greatly enhances the realism and immersion of the simulation, aiding in spatial awareness and qualitative assessment.
- Integrated Development Environment: The entire process—modeling, control design, simulation, and data analysis—occurs within a tightly coupled software ecosystem (SolidWorks & MATLAB), streamlining the workflow.
- Safety and Cost-Efficiency: It provides a risk-free virtual sandbox for exhaustively testing and tuning control algorithms for the quadrotor drone before any physical prototype is built or flown, preventing potential crashes and damage.
- Rapid Prototyping: Different control strategies (e.g., sliding mode control, backstepping, LQR, model predictive control) can be implemented in Simulink and tested on the same visual model with minimal setup time, enabling comparative studies.
The developed platform establishes a strong foundation for future research and development in quadrotor drone autonomy. Immediate extensions of this work could involve:
- Incorporating more sophisticated aerodynamic effects, such as blade flapping, ground effect, and detailed propeller aerodynamics.
- Modeling and simulating sensor suites (e.g., IMU noise, GPS latency) and state estimation algorithms (e.g., Kalman filters) within the loop.
- Expanding the virtual environment to include obstacles, wind disturbances, and other unmanned aerial vehicles for testing advanced guidance, navigation, and control (GNC) algorithms like obstacle avoidance and swarm coordination.
- Utilizing the system for hardware-in-the-loop (HIL) simulation, where the SimMechanics model is replaced by communication with the actual flight controller hardware, providing the final stage of validation before full flight testing.
In summary, this visual simulation system represents a powerful and practical tool for researchers and engineers working on quadrotor drone technology. It effectively bridges the gap between theoretical control design and real-world implementation, accelerating innovation while enhancing safety and reducing development costs for advanced quadrotor drone applications.