The objective of this project is to develop a robust control scheme to enable a quadrotor to track desired trajectories in the presence of external disturbances.
Consider a quintic trajectory of the form ( q_d(t) = a_0 + a_1t + a_2t^2 + a_3t^3 + a_4t^4 + a_5t^5 ).
The coefficients ( a_0, a_1, a_2, a_3, a_4 ) and ( a_5 ) can be found by solving the matrix equation below:
Differentiating ( qd(t) ) with respect to time, we get:
- Velocity: ( q'd(t) = a1 + 2a2t + 3a3t2 + 4a4t3 + 5a5t4 )
- Acceleration: ( q''d(t) = 2a2 + 6a3t + 12a4t2 + 20a5t3 )
Control laws derived in handwritten notes are as follows:
- PD Controller: ( Kp = 110 ), ( Kd = 8 )
- Lambda Parameters: ( λz = 12 ), ( λφ = 13 ), ( λθ = 19 ), ( λψ = 5 )
- Gain Parameters: ( kz = 6 ), ( kφ = 140 ), ( kθ = 111 ), ( kψ = 25 )
Upon receiving a call to odom_callback, the time (self.t) is initialized and relevant information such as the drone's current position, velocity, orientation, and angular velocity along the 3 axes are extracted from the odometry message. These values are then fed into the smc_control function.
smc_control function first invokes the traj_evaluate function which predicts the drone's trajectory and invokes the generate_trajectory to obtain the desired position, velocity, and acceleration for the x, y, z axis at that instant.
The 3D plot reveals that the drone's trajectory slightly deviates from the actual path, which can be attributed to the saturation function implemented to prevent chattering. Therefore, it can be concluded that the controller functions effectively and possesses the ability to handle external disturbances reasonably well, while still being able to closely track the desired trajectory.
To get started with this project, clone the repository:
git clone https://github.com/Shivam7Sharma/RBE502_Final_Project.git