# 3D Inverted Pendulum Swing-up and Catch with a Crazyflie Quadcopter

The specific task presented in this project is the swing-up of a ball hanging from the bottom of the quadrotor and then catching the ball in a basket on the top of the quadrotor (i.e. the drone must successfully complete the “Bilbo Catcher” game). This task is important because it is a highly visual demonstration of the dynamic capabilities of modern quadrotors, and gives insight into advanced control strategies that can be adapted to other complex tasks. This project is decomposed into three major control challenges: (1) a trajectory tracking Controller, (2) a swing-up trajectory generator, and (3) a ball-catching controller.

The control problem is decomposed into several parts depending on the state of the hybrid dynamical system. The first major step is the development and implementation of an effective trajectory tracking controller. The main challenge and novelty in this work is the formulation of the trajectory generation problem as a sparse nonlinear optimization problem. Using this framework, an optimal swing-up trajectory is generated. Then, when the string loses tension and the hybrid dynamics of the ball switch to a ballistic model, the controller switches into a ball catching mode, detailed below.

### Position Control

This control system is a series of cascading PD controllers. At the highest level, a global position setpoint is given (or generated by a trajectory generator) to a position controller, which uses PD position control to produce a desired orientation and thrust of the quadrotor. This desired orientation is then passed to a PD controller that regulates the quadrotor attitude through three torques around the quadrotor body axes. These three desired torques and the desired thrust is then passed to a motor controller that translates them into rotor velocities using the transformation detailed in the system model section.

### Swing-Up Trajectory Generation

Optimal trajectory generation methods are utilized to generate realistic a swing-up trajectory for the quadrotor system. Direct collocation is used to generate full state trajectories of both the quadrotor and the ball, as well as nominal inputs to generate these state trajectories during swing-up. This trajectory optimization framework is able to quickly and effectively find swing-up trajectories for the quadrotor. The nonlinear optimization problem was solved using Matlab’s *fmincon*, and generally solved in under ten seconds.

### String Length Optimization

One freely tunable physical parameter of our system was the length of the string used to suspend the ball. In order to find the optimal string length, a brute-force optimization was solved. String lengths were varied from 0.1 meter to 0.4 meters in 0.01 meter increments, and for each string length a nominal swing-up trajectory was generated and simulated. These trials were then evaluated according to the following objection function:

Where u is the actuator effort, x is the full state, and t_a is the time the ball spends above the quadrotor during swing-up. The t_a parameter was included because it was observed during testing that the swing-up catch success rate was directly correlated to the amount of time the quadrotor had to catch the ball. In other words, the longer the ball is above the quadrotor, the longer the quadrotor has to respond and reach the catch position. According to this cost function, the optimal string length was found to be 30 cm, which was used in hardware testing.

### Swing-Up Dimensionality Reduction

For effective hardware implementation, the full 16 dimensional state trajectory generated by the direct collocation algorithm were projected onto a one-dimensional control parameterization. This is believed to be valid because the direct collocation algorithm produced trajectories that were inert in most quadrotor states. The pendulum excitation was produced by oscillations in the sagittal plane. To reduce the dimensionality of the trajectory tracking problem, these optimally generated oscillations were fit with a one-dimensional sinusoidal function. A sine wave with the fitted amplitude and frequency is used as the reference trajectory for the physical system.

This low-dimensional trajectory was tested in simulation with the following results:

### Ball Catching

The second major design challenge is the development and implementation of a controller capable of catching the swinging load. To do this, we forward-predict the ball trajectory at every time step, giving it the ability to effectively deal with unmodeled disturbances. In order to forward-predict the location of the ball, it is treated as a ballistic mass with negligible air resistance.

Current velocities of the ball are approximated using a first-order backward difference derivative approximation, and the filtered using an exponential moving average filter with an alpha value of 0.1:

The forward predicted ball location is then commanded as a position setpoint to the quadrotor. This behavior can be seen in the following image, which was generated with actual test data. The current ball location is shown in white, and the predicted ball location is shown in magenta.

### Switching Manifold

A central component of successful ball-in-cup behavior is effective switching between control phases. This is accomplished through a succession of switching conditions. At every time step in the swing-up phase, the maximum height location the ball will achieve during that swing is calculated through simple energy balance formulas:

Once the maximum predicted z height reaches a user-defined threshold, which in our case was defined as 80% of the maximum pendulum length, the system prepares to switch into the ball-catching control mode. This prediction can be seen in the following image, which was generated using real test data. The predicted ball height is shown in magenta during swing-up. When the controller switches to ball-catching mode, the predicted ball ballistic trajectory location is then shown.

### Implementation

Special thanks to Lawrence Papincak, who lead the team in getting the simulated results implemented on the actual crazyflie hardware. An overview of the system architecture he put together is shown here:

### Results

The above image demonstrates the harmony achieved between our constrained trajectory optimization (top right in 3D, lower right in 2D) and our implementation (top left real video, bottom left RVIZ view), despite idealized dynamical assumptions and dimensionality reduction of the generated trajectories. This system was capable of achieving ~40% success rate, a great achievement considering we went from no quadcopter experience to this in a single semester.

Another fun by-product of the controls and system work detailed above was intuitive quadcopter control through ball position tracking: