Nodes
Joystick Node
The Joystick Node enables interaction between the user and the drone by processing input from a joystick.
It translates joystick commands into drone control messages and publishes these commands to a topic for further use by
other nodes in the control system.
Subscriptions:
Publications:
/quad_ctrl: Publishes joystick commands as a QuadCmd message containing roll, pitch, throttle, yaw, armed status, and mode.
Communication with the Joystick:
- Reads joystick inputs using a compatible library, capturing the state of axes and buttons.
- Maps joystick inputs to drone control commands, such as roll, pitch, throttle, and yaw.
- Includes functionality for arming the drone and switching modes based on button presses.
Key Features:
- Maps joystick axes to roll, pitch, throttle, and yaw values, ensuring precise user control over the drone's movements.
- Processes button presses to toggle the drone's armed state and change flight modes.
- Publishes user commands to the
/quad_ctrl topic in real time, enabling other nodes to execute the desired actions.
- Implements a safety mechanism by monitoring the state of the armed flag to ensure safe operation.
Node Workflow:
- Starts by initializing the joystick interface and configuring input mappings for axes and buttons.
- Continuously reads input data from the joystick at a predefined rate.
- Processes the joystick data to compute control values for roll, pitch, throttle, and yaw.
- Determines the drone's armed state and flight mode based on specific button presses.
- Publishes the computed control commands to the
/quad_ctrl topic in the form of a QuadCmd message.
Drone Control Node
The Drone Control Node is responsible for controlling the drone's behavior based on received control commands.
It subscribes to relevant topics that provide the current state of the drone, processes these inputs, and publishes
commands to actuate the drone's motors, adjusting its flight dynamics accordingly.
Subscriptions:
/quad_pose: Subscribes to the drone's pose (position and orientation) using a PoseStamped message./quad_ctrl: Subscribes to control commands for the drone (roll, pitch, throttle, yaw, armed state, mode)
using a QuadCmd message to initiate control actions.
Publications:
/drone_control_feedback: Publishes feedback on the drone's current control status, position, and orientation.
Communication with Other Nodes:
- Receives control commands from the joystick node through the
/quad_ctrl topic to determine the desired drone
movements (e.g., roll, pitch, throttle, yaw, armed status, and mode).
- Receives the current pose of the drone from the
/quad_pose topic to assess the drone's position and orientation.
- Processes both control and pose data to compute necessary actions for motor commands and flight adjustments.
- Publishes feedback about the drone's current state to the
/drone_control_feedback topic for monitoring purposes.
Key Features:
- Receives control input (roll, pitch, throttle, yaw) and applies the necessary transformations to actuate the drone’s flight.
- Monitors the armed state and mode of the drone to ensure it responds only when it is safe and in the correct mode.
- Uses PID or other control strategies to smoothen and stabilize the drone’s movements based on current pose and desired commands.
- Provides real-time feedback to other systems about the drone's position, orientation, and control status.
Node Workflow:
- Subscribes to the
/quad_pose topic to get the current pose (position and orientation) of the drone.
- Subscribes to the
/quad_ctrl topic to receive control commands for roll, pitch, throttle, yaw, and the armed status.
- Based on the control commands, adjusts the motor outputs to achieve the desired flight dynamics (e.g., stabilize pitch, roll, and yaw).
- Applies a control algorithm (e.g., PID) to minimize error between the desired and actual states (e.g., position and orientation).
- Publishes feedback about the drone's status (current position, orientation, etc.) on the
/drone_control_feedback topic for monitoring.
Quad Listener Node
The Quad Listener Node facilitates communication between the drone's position tracking system
(OptiTrack) and the ROS environment. It receives UDP packets containing the drone's position and orientation data,
applies necessary transformations to correct offsets, and publishes the corrected pose to the ROS network.
Subscriptions:
Publications:
/quad_pose: Publishes the corrected position and orientation of the drone as a PoseStamped message.
Communication with OptiTrack:
- Uses a UDP socket to receive real-time tracking data from OptiTrack. The data includes the drone's position (x, y, z) and orientation (quaternion qx, qy, qz, qw).
- Processes the received data to remove offsets specific to each drone ("bird") based on parameters defined in a YAML file.
- Corrects the received position and orientation using transformation matrices that adjust for the drone's predefined offsets in both position and orientation.
TF Broadcasting:
- Broadcasts a transform named
drone relative to the map frame, providing real-time positional and orientation updates of the drone in the ROS TF tree.
- The transform is continuously updated at 100Hz, ensuring smooth and accurate representation of the drone's pose in the world frame.
Key Features:
- Loads drone-specific position and orientation offsets from a YAML file.
- Applies mathematical transformations to adjust the received pose data, ensuring that the published pose accurately reflects the drone's corrected position and orientation in the world frame.
- Handles exceptions such as missing data or invalid configurations, ensuring robust communication with the tracking system.
Node Workflow:
- Starts a UDP server to listen for incoming tracking data.
- Loads bird-specific offsets from the YAML file and verifies their correctness.
- Processes incoming UDP messages to extract positional and orientation data.
- Applies the offset transformations using a combination of translation and rotation matrices.
- Publishes the corrected pose to the
/quad_pose topic.
- Broadcasts the corrected pose as a transform in the TF tree, enabling other nodes to access real-time drone position and orientation information.
QuadSim Node
The QuadSim Node simulates the behavior of a quadrotor drone by processing control inputs and updating the drone's pose
(position and orientation) over time. It interfaces with a quadrotor model to compute the drone's state based on the control commands,
publishes the updated pose, and visualizes the drone's parts using markers.
Subscriptions:
/quad_ctrl: Subscribes to control commands for the drone (throttle, roll, pitch, yaw, armed state) using a QuadCmd message.
Publications:
/quad_pose: Publishes the drone's updated pose (position and orientation) as a PoseStamped message.visualization_marker_array: Publishes a visualization of the drone's parts using a MarkerArray message./tf: Publishes the transform between the "map" and "base_link" frames of the drone.
Communication with Other Nodes:
- Receives control commands from the
/quad_ctrl topic, including throttle, roll, pitch, yaw, and armed state.
- Uses the quadrotor model to compute the new state of the drone based on these control inputs.
- Publishes the drone's current pose to the
/quad_pose topic.
- Publishes markers representing the drone's parts and the drone's current state for visualization in RViz.
- Publishes a transform (TF) to define the position and orientation of the drone in the "map" frame.
Key Features:
- Simulates the behavior of a quadrotor drone based on control inputs (throttle, roll, pitch, yaw) from a
QuadCmd message.
- Converts the drone's position and orientation from Euler angles to quaternion format for consistency with ROS conventions.
- Visualizes the drone's body parts (e.g., arms, motors) using RViz markers.
- Calculates the new state of the drone (position, orientation) based on the quadrotor dynamics and control inputs.
- Publishes the updated pose and transform data for external systems or visualization.
Node Workflow:
- The node initializes parameters such as initial position, frame ID, and sets up communication channels (subscription and publication).
- Subscribes to the
/quad_ctrl topic to receive control commands (throttle, roll, pitch, yaw, armed state).
- Uses the quadrotor model (in the
quadrotor class) to compute the new drone state based on the control inputs.
- Converts Euler angles (roll, pitch, yaw) into a quaternion for proper ROS integration.
- Publishes the drone's updated pose (position and orientation) to the
/quad_pose topic.
- Publishes the transform between the "map" and "base_link" frames to the
/tf topic for accurate positioning in 3D space.
- Generates a visualization of the drone's body using
MarkerArray messages, displaying parts such as arms and motors in RViz.
Controller PID Node
Node Overview:
The Controller_pid node controls the quadcopter's position using PID controllers for altitude, roll, pitch, and yaw. It subscribes to the quadcopter's current position and publishes control commands to adjust its position toward a desired target in 3D space.
Key Features:
- PID control for altitude (Z axis), roll and pitch (X and Y axes), and yaw adjustment.
- Real-time path tracking by publishing the quadcopter’s actual position over time.
- Customizable desired position parameters for dynamic control.
- Support for both simulation and real drone with different PID gain configurations.
- Automatic arming of the drone when certain conditions are met during operation.
Node Workflow:
- The node initializes and fetches parameters for the desired position and mode (real drone or simulation).
- Based on the mode (simulation or real), appropriate PID controller gains for altitude, roll, pitch, and yaw are configured.
- The node subscribes to the
quad_pose topic to receive the current position of the quadcopter.
- The
listener_callback function updates the current position of the quadcopter whenever new data is received from the quad_pose topic.
- In the
timer_callback function, control commands (throttle, roll, pitch) are computed based on the difference between the current position and the desired position using the PID controllers.
- When the flag condition is met, the node arms the drone and publishes control commands to the
quad_ctrl topic.
- The node also continuously publishes the actual position path to the
actual_path topic for visualization.
Subscribers:
quad_pose - Subscribes to the current position of the quadcopter (message type: PoseStamped).
Publishers:
quad_ctrl - Publishes control commands (throttle, roll, pitch, yaw) to the quadcopter (message type: QuadCmd).actual_path - Publishes the actual path of the quadcopter for visualization (message type: Path).
Functions:
listener_callback - Updates the current position of the quadcopter and appends the position to the actual path.timer_callback - Runs periodically to compute control commands based on position errors and publishes them to the quad_ctrl topic.set_desired_position - Allows updating the desired position dynamically.
Controller Trajectory Node
Node Overview:
The controller_traj node manages the trajectory tracking of a quadcopter by using a trajectory controller and PID controllers for altitude, roll, and pitch. It tracks the position of the quadcopter, computes errors, and adjusts the control inputs (throttle, roll, pitch, yaw) to move the quadcopter toward a desired position.
Key Features:
- Trajectory tracking using a
TrajectoryController for smooth path execution.
- PID controllers for altitude, roll, and pitch stabilization.
- Ability to control both simulation and real drone with customizable PID gains.
- Publishing the quadcopter’s trajectory and actual position for visualization.
- Automatic arming of the drone when it reaches a safe position and starts the trajectory.
Node Workflow:
- The node initializes parameters for the desired position and simulation mode (real drone or simulation).
- It subscribes to the
quad_pose topic to receive the current position and orientation of the quadcopter.
- The
listener_callback function updates the current position and sets up the trajectory controller if it's the first position.
- In the
timer_callback function, the node computes control commands based on the trajectory tracking and PID control outputs for altitude, roll, and pitch.
- Control commands are published to the
quad_ctrl topic for the quadcopter to follow.
- The trajectory path and actual path are continuously published to the
traj_path and actual_path topics for visualization.
Subscribers:
quad_pose - Subscribes to the current position of the quadcopter (message type: PoseStamped).
Publishers:
quad_ctrl - Publishes control commands (throttle, roll, pitch, yaw) to the quadcopter (message type: QuadCmd).traj_path - Publishes the desired trajectory path (message type: Path).actual_path - Publishes the actual path followed by the quadcopter (message type: Path).
Functions:
listener_callback - Updates the current position and sets up the trajectory controller if it's the first position.timer_callback - Runs periodically to compute and publish control commands based on trajectory tracking and PID control.euler_to_quaternion - Converts Euler angles (roll, pitch, yaw) to quaternion for orientation representation.quaternion_to_euler - Converts quaternion orientation to Euler angles (roll, pitch, yaw).add_trajectory_point - Adds a point to the desired trajectory path.set_desired_position - Allows dynamic updates to the desired target position.
Controllers
PID Controller
The system utilizes three PID loops for controlling the drone:
- Altitude (Z): Maintains the desired height of the quadrotor.
- Roll: Stabilizes the quadrotor along the roll axis and controls its motion along the Y-axis.
- Pitch: Stabilizes the quadrotor along the pitch axis and controls its motion along the X-axis.
PID for Altitude (Z)
The altitude PID loop computes the error between the desired altitude and the current altitude. It incorporates a feedforward term and uses a scaling factor to generate a thrust output value.
PID Equation with Feedforward and Scaling:
\( \text{Output} = \text{Scaling_Factor} \times \left( K_p \cdot \text{Error} + K_i \cdot \int \text{Error} \cdot dt + K_d \cdot \frac{d(\text{Error})}{dt} + \text{Feedforward} \right) \)
Where:
- \( K_p \), \( K_i \), and \( K_d \) are the proportional, integral, and derivative gains, respectively.
- \( \text{Error} \) is the difference between the desired and current values.
- \( \text{Feedforward} \) is a predefined term to account for expected system behavior.
- \( \text{Scaling_Factor} \) adjusts the output to match the system's dynamics.
The final output is mapped to a PWM signal in the range 1000 to 1950.
PID for Roll and Pitch
Separate PID loops for roll and pitch control the drone's stabilization along these axes and enable position control:
- Roll: The roll PID adjusts the drone's angle to achieve the desired Y-axis position.
- Pitch: The pitch PID adjusts the drone's angle to achieve the desired X-axis position.
The roll and pitch PID logic follows similar principles to the altitude PID, with output values converted to PWM signals ranging from 1000 to 2000.
General Behavior
- The controllers integrate the error over time (integral term) to address steady-state errors.
- The derivative term considers the rate of change of error to counteract overshooting and oscillations.
- Thrust output is clamped to ensure stability and mapped to appropriate PWM values.
Control Process
During each control loop:
- The altitude PID adjusts the throttle to maintain or achieve the desired height.
- The roll PID modifies the roll command to control the Y position of the drone.
- The pitch PID modifies the pitch command to control the X position of the drone.
Return Values
The altitude PID generates a PWM signal between 1000 and 1950, which directly drives the motors and affects the drone's vertical motion and stability. Similarly, roll and pitch PID outputs are mapped to motor commands to achieve desired positional corrections.
Minimum-snap
Minimum-Snap Trajectory Generator
This implementation of the trajectory generator is based on the seminal work by Vijay Kumar and colleagues, specifically their paper on Minimum snap trajectory generation and control for quadrotors. It computes smooth, dynamically feasible trajectories for a quadcopter by minimizing snap (the fourth derivative of position) while satisfying boundary conditions for position, velocity, acceleration, and jerk.
Overview
The generator calculates polynomial trajectories for the quadcopter's six degrees of freedom: x, y, z (position), roll, pitch, and yaw. The trajectory is expressed as a seventh-order polynomial, chosen for its ability to satisfy constraints on position, velocity, acceleration, and jerk at both the start and end of the trajectory.
Equations
The polynomial trajectory for a single axis is given by:
\( x(t) = a_7 t^7 + a_6 t^6 + a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 t^2 + a_1 t + a_0 \)
Where \( x(t) \) represents the position at time \( t \), and the coefficients \( a_0, a_1, \ldots, a_7 \) are determined to minimize the snap (fourth derivative of position):
\( \text{Snap} = \frac{d^4x(t)}{dt^4} \).
Boundary Conditions
The trajectory ensures continuity and smoothness by satisfying the following constraints at the start (\( t = 0 \)) and end (\( t = T \)) of the trajectory:
- Position: \( x(0), x(T) \)
- Velocity: \( \dot{x}(0), \dot{x}(T) = 0 \)
- Acceleration: \( \ddot{x}(0), \ddot{x}(T) = 0 \)
- Jerk: \( \dddot{x}(0), \dddot{x}(T) = 0 \)
Matrix Formulation
The coefficients \( a_0 \) to \( a_7 \) are computed by solving the linear system:
\( A \cdot \mathbf{c} = \mathbf{B} \)
Where:
- \( A \): The boundary condition matrix constructed using polynomial terms and their derivatives for the start and end times.
- \( \mathbf{B} \): A vector containing the boundary conditions for position, velocity, acceleration, and jerk.
- \( \mathbf{c} \): The vector of polynomial coefficients to be solved.
Trajectory Evaluation
The trajectory can be evaluated for position, velocity, acceleration, or jerk at any time \( t \) by taking successive derivatives of the polynomial:
- Position: \( x(t) \)
- Velocity: \( \dot{x}(t) = 7a_7 t^6 + 6a_6 t^5 + \ldots + a_1 \)
- Acceleration: \( \ddot{x}(t) = 42a_7 t^5 + 30a_6 t^4 + \ldots \)
- Jerk: \( \dddot{x}(t) = 210a_7 t^4 + 120a_6 t^3 + \ldots \)
Key Features
- Optimal Smoothness: By minimizing snap, the trajectory ensures smooth and efficient motion with minimal actuator effort.
- Flexibility: The generator can handle arbitrary boundary conditions for all six degrees of freedom.
- Real-Time Update: Coefficients can be recomputed dynamically if the starting conditions change mid-flight.
Implementation Notes
The generator constructs individual trajectories for each degree of freedom. The coefficients for each trajectory are precomputed using matrix inversion. At runtime, the polynomial is evaluated at the desired time points to obtain position, velocity, acceleration, or jerk.
Relevance
This approach is crucial for precise quadrotor maneuvers, especially in scenarios requiring high-speed, aggressive, or dynamic flight. The implementation is directly inspired by Vijay Kumar's work, which has been widely adopted for trajectory planning in autonomous drones.
Controller Design
To enable the quadrotor to follow specified trajectories \( \sigma_T(t) = [r_T(t)^T, \psi_T(t)]^T \), the controller uses the following components:
Position and Velocity Errors
The errors on position and velocity are defined as:
\[
e_p = r - r_T, \quad e_v = \dot{r} - \dot{r}_T
\]
Desired Force Vector
The desired force vector for the controller and the desired body frame z-axis are calculated as:
\[
F_{des} = -K_p e_p - K_v e_v + mg \mathbf{x}_W + m \ddot{r}_T
\]
Here, \( K_p \) and \( K_v \) are positive definite gain matrices. We assume \( \|F_{des}\| \neq 0 \).
Total Thrust
The desired force is projected onto the actual body frame z-axis to compute the total thrust:
\[
u_1 = F_{des} \cdot z_B
\]
Desired Orientation
To determine the desired orientation:
- The desired body frame z-axis:
\[
z_{B, des} = \frac{F_{des}}{\|F_{des}\|}
\]
- Using the specified yaw angle \( \psi_T(t) \), the desired body frame x-axis and y-axis are:
\[
x_{C, des} = [\cos\psi_T, \sin\psi_T, 0]^T
\]
\[
y_{B, des} = \frac{z_{B, des} \times x_{C, des}}{\|z_{B, des} \times x_{C, des}\|}, \quad x_{B, des} = y_{B, des} \times z_{B, des}
\]
The desired rotation matrix is then:
\[
R_{des} = [x_{B, des}, y_{B, des}, z_{B, des}]
\]
Orientation and Angular Velocity Errors
The orientation error is defined as:
\[
e_R = \frac{1}{2}(R_{des}^T W R_B - W R_B^T R_{des})^\vee
\]
where \( \vee \) represents the vee map converting elements of \( \text{so}(3) \) to \( \mathbb{R}^3 \).
The angular velocity error is:
\[
e_\omega = \mathbf{B}[\omega_{BW}] - \mathbf{B}[\omega_{BW, T}]
\]
Desired Moments
The desired moments and the remaining three inputs are computed as:
\[
[u_2, u_3, u_4]^T = -K_R e_R - K_\omega e_\omega
\]
where \( K_R \) and \( K_\omega \) are diagonal gain matrices, allowing unique gains for roll, pitch, and yaw tracking.
Hardware setup
The quadrotor hardware includes a Betaflight flight controller, a Raspberry Pi 0 for processing, and 1103 brushless motors with KV ratings of 8000 to 15000 RPM/V. The controller takes inputs such as roll, pitch, yaw (1050-1900 range), and throttle (1000-1900 range).
The drone's position is independently tracked using OptiTrack, providing precise positional data for validation and control loop optimization.
Results
PID Simulation and Hardware Performance
The PID controller was tested in both simulation and hardware environments. The goal was for the drone(s) to move to a desired target location, marked by a green arrow. The red path represents the trajectory taken by the drone(s), and the approximate time taken to reach the target was 40 seconds, indicating slow convergence.
Simulation Video:
Position vs. Time Plot:
The position vs. time plot for X, Y, and Z axes shows the PID controller's performance over time:
Hardware Video:
Multiple Drones in PID Control:
This video demonstrates the performance of multiple drones controlled using the PID controller in a simulation environment:
PID Gains:
- Proportional Gain (𝑲ₚ): 0.3 (altitude), 0.1 (X and Y)
- Integral Gain (𝑲ᵢ): 0.05 (altitude), 0.002 (X and Y)
- Derivative Gain (𝑲ᵈ): 0.09 (altitude), 0.38 (X and Y)
- Feedforward: 0.3 (altitude)
Note: The PID gains used were identical for both simulation and hardware tests.
Minimum-Snap Trajectory Performance
The Minimum-snap trajectory controller was tested in simulation with significant improvements over the PID controller. The controller managed to guide the drone(s) to their target locations in approximately 4 seconds, a notable improvement from the PID's 40 seconds. The green path represents the trajectory predicted by the generator, while the red path shows the actual trajectory taken by the drone(s).
Simulation Video:
Trajectory Plots:
The plots below showcase position, velocity, acceleration, jerk, and snap versus time, highlighting the smooth transitions and efficient trajectory planning achieved by the Minimum-snap controller:
Minimum-Snap Gains:
- Proportional Gain (𝑲ₚ): \( \text{diag}([1.3, 1.3, 14.5]) \)
- Velocity Gain (𝑲ᵥ): \( \text{diag}([1.0, 1.0, 1.7]) \)
- Rotation Gain (𝑲ᵣ): 2.9
- Weight Gain (𝑲𝑤): 0.9