# Two-Axis Brushless Gimbal Controller

Eric Dai (emd88)

Nikita Ermoshkin (ne75)

## Introduction

The group desgined and built a two-axis brushless AC gimbal controller for the purpose of stabilizing a camera on an aerial platform.

A camera gimbal is a device used to stabilize a camera for use on an aerial system, such as a multirotor for aerial photography or a fixed wing UAV for search and rescue missions. A two axis gimbal is targeted specifically at fixed-wing aircraft, whose primary degrees of freedom are roll and pitch, so a two axis gimbal is necessary to account for those two degrees of freedom.

This project was developed with the intention to integrate into Cornell University Unmanned Air Systems' (CUAir) aerial vehicle.

## System Architecture

There are three primary components to this system are actuation, measurement, and feedback.

Actuation is achieved using brushless AC motors. These were chosen over standard servos or DC motors because they provide a uniform torque at all speeds and have no vibration or noise that could be introduced into the images taken by the attached camera.

The brushless motors used to control the gimbal are open-loop, so two Inertial Measurement Units (IMUs) were added to close the loop—one in the camera's frame of reference and one in the moving body's (such as an aircraft) frame of reference. These IMUs report two sets of data: rate of rotation around each axis (from the gyroscope) and the linear acceleration (from the accelerometer). Individually, a gyroscope or an accelerometer does not provide an accurate estimate of orientation in space, but the data from each can be combined to create an accurate estimate. The process used to combine this data is called a complementary filter.

Orientation can be represented in many ways, including Euler angles, axis-angle, and quaternions. Quaternions are four-dimensional complex numbers that are used to represent rotations. While they are quite cumbersome and unintuitive, they allow for rotations to be performed much more efficiently than by multiplying rotation matrices. So, the complementary filter was implemented using quaternions.

These quaternions were used to determine the error in the two motors' positions, which were fed into a PID loop that maintains the position of the motors.

This gimbal controller was inspired by the work of BaseCam Electronics and their SimpleBGC.

## Hardware Design

The primary hardware necessary for this controller are motor drivers. We chose to use the TI DRV8313, which is effectively a triple half-H bridge, one for every motor phase. There is some support circuitry necessary, but is well documented in the part's datasheet.

The secondary piece of hardware necessary is the IMU itself. The InvenSense MPU6050 is a small QFN package, so we used off-the-shelf breakout boards to easily connect to and use with our system prototype. Specifically, the DFRobot MPU6050 Breakout.

Everything was soldered onto a PCB to create a neat and clean hardware system, shown in Figure 1:

## Firmware Design

The code was designed as a set of modules. Each module had an associated struct data type that would be used as a mailbox to pass information to and from the module's routines. Each of the modules is described below.

### Brushless Motor Driver

First, a little theory on brushless motors. Each motor is a set of three phases, which get a sinusoidal voltage applied to them, each shifted in phase by 120 degrees. The result is a rotating magnetic field vector, which the motor's magnets will follow, spinning the motor's rotor.

Figure 2 shows the three phases applied to the motor in an ideal case:

This 3-phase AC voltage can be simulated on a digital system using PWM. Each motor would require three independent PWM signals, so our system would require six total PWM signals. The PIC32, however, only has five PWM modules, so a more creative solution was necessary. This solution was to approximate the sinusoid as a trapezoid, as shown in Figure 3:

At any given time, one phase is high, one phase is low, and one is using PWM to be somewhere in between. Now, each motor only requires one PWM module, which is switched between phases as necessary using the PIC32's PPS Output selection system.

The primary drawback of this method is that torque is now not uniform, since the magnitude of the voltage is not constant for all electrical angles. This, however, is not an issue as we close the loop and use a PID controller to linearize the system.

The PWM frequency was chosen to be 18 KHz, the slowest frequency that would not produce an audible signal. A timer was used to set this frequency for each Output Compare module, and an interrupt attached to the timer would calculate which pin should be high, low, and PWM controlled based on the desired electrical angle set by the rest of the software.

The module has two primary functions for other modules: stepping by a certain amount and setting a velocity. Generally, these would be used exclusively. In our system, we set the velocity to 0 and the PID controller calculates by how much to step the motor.

### MPU6050 Driver

The IMU module serves two purposes: communicate with the IMUs using I2C and run a data filter to combine the data to create an orientation estimate.

The PIC32 Peripheral Library has a simple interface to the I2C hardware, and the MPU6050 complies with a typical I2C protocol that involves reading and writing byte-sized registers on the MPU6050. The PIC32 acts as the master and the IMU acts as the slave. The data is read at a constant rate by the filter and then processed to create an accurate orientation estimate.

The data filter was inspired by Phillip Schmidt's work but was modified to improve performance and stability. The key to this filter is that unlike typical IMU complementary filters, this one performs all it's calculations using quaternions, greatly reducing the CPU load compared to using complex trigonometric functions to determine orientation based on gravity. Below are the general steps:

1. Read accelerometer and gyroscope data
2. Rotate data by IMU orientation offset
3. Rotate the vector (0, 0, 1) by the current rotation estimate ($q(n)$). This describes the expected gravity vector from the accelerometer.
4. Take the cross product of the this estimated gravity vector and the accelerometer reading. If the IMU is not accelerating, it will simply point in the direction of gravity. This cross product will point in the opposite direction of the gyroscope drift and will describe the correction needed to be applied to $q(n)$ to correct for gyro drift.
5. Weight this correction by some scalar $\alpha$, which has units of 1/s.
6. Add this correction to the current gyro data. We save on computation time by applying the correction first and integrating once, instead of integrating twice and then applying the correction afterwards.
7. Create a quaternion ($q_d$) representing the rotation rate based on this corrected gyro data.
8. Rotate $q(n)$ by $q_d$ to get $q(n+1)$
9. Re-normalize the estimate to avoid error from building up and creating a non-unit quaternion.

Given a filter frequency $f$ and a gyroscope reading vector $\vec{g}$, quaternion integration can be performed using the following equations:

$$\vec{\omega} = \vec{g}/(2f)$$ $$q_d = (1 - (\omega_x^2 + \omega_y^2 + \omega_z^2)/2,\ \omega_x,\ \omega_y,\ \omega_z)$$ $$q(n+1) = q_d q(n)$$

The derivation of this formula is shown in Phillip Schmidt's work and makes use of a small angle approximation. This holds true if the amount of rotation between samples of data is small, which is true for our system.

Phillip Schmidt's system would perform this correction system differently than our implementation. His system would rotate the acceleration by the current estimate, take the cross product with $(0,\ 0,\ 1)$ to create a correction in the world frame, rotate it by the inverse of the estimate to get the correction back into the IMU's frame, and then apply the correction as we did here. This method was not only more computationally intensive, but seemed to behave unpredictably whenever approaching gimbal lock scenarios. So, using his system as a starting point, we developed our own efficient filtering algorithm. The filter runs at 500Hz. This resulted in approximately a 25% CPU load, leaving the rest of the CPU to handle PID control and the UART interface.

### PID

The IMUs are used to close the loop for the motors to create a complete control system. A modified PID algorithm is used as the controller. The modification is that the derivative term is the derivative of the measurement, not the derivative of the error. Additionally, a low pass filter was included to smooth out any spikes due to discontinuities in the setpoint. Figure 4 shows a block diagram of the entire PID control system.

Both the error and the derivative terms are simply the difference of two variables, but these variables are both quaternions, so simple subtraction cannot be performed. However, the difference of two rotations can be calculated simply, given two rotations $q_1$ and $q_2$, as follows:

$$\Delta q = q_1^* q_2$$

This difference can be converted into the two axes of rotation using the following formulae: $$\theta = \arcsin{(2(q_w q_y - q_x q_z)}$$ $$\psi = \arctan{(2(q_w q_x - q_y q_z),\ (q_w^2 - q_x^2 - q_y^2 + q_z^2))}$$ These values are integrated to form the integral term and the same difference process is applied to the measured value to create the derivative term.

The last piece of the PID puzzle is making sure each rotation measurement is in the correct frame of reference. The IMUs measure their data in the world frame. The actuators, however, are in a moving frame, let's say an aircraft. So, one IMU is used to measure the aircraft's orientation in the world frame and the other is used to measure the camera in the world frame. Then, the camera's setpoint and measured orientation (in the world frame) are rotated by the aircraft's orientation to get the setpoint and measurement in the aircraft's frame. Now, the calculated roll and pitch angles for the motors will be in the proper frame.

### Parameter Storage

Any gimbal controller requires some form of parameter storage. This is to store the configurations for the motors, the IMU's orientation offset, and PID values. These values are stored in the PIC32's flash storage. A data struct that contains all of the parameters to be stored creates a parameter file. This file is 512 bytes long (the size of a row in the PIC32's flash storage) and whenever a parameter in the file is updated, it gets written into NVM at a specific address. On boot, this address location is read into the parameter file struct. The first entry in the file is a magic number constant that is used to determine if the data is valid. That number is 0x0ECE4760.

### UART and Command Interface

The last major module in the gimbal controller system is the UART and command interface. The command set allows a computer to control the desired orientation, PID tuning values, and IMU configuration values. Each command has a standard format:

Start (0x7E) Message Length (l) Message ID Data
1 byte 1 byte 1 byte (l-1) bytes

Each message starts with a delimiting character, ~ (0x7E in hex). Then, a single byte is sent with the number of bytes that need to be read in this command. This string of binary data starts with an ID byte. This is used to determine how the data is parsed. Table 1 shows the supported commands.

Each command has an associated handler function that gets called whenever a valid command ID is received. The data is analyzed for validity and a response is sent, acknowledging that the data was correct or that there was an error.

## Results

The resulting system was able to successfully stabilize a camera and account for changes in the frame's orientation. When the system was tuned for setpoint tracking (instead of disturbance rejection) the camera's orientation would be compensated for with little lag and little overshoot. All together, the modules used about 75% of the PIC32's CPU. Figure 5 shows the error between the desired roll and pitch and the actual roll and pitch when rotating back and forth by 90 degrees about both axes. From the graph, it can be seen that even though the frame is moving by 90 degrees, the most the camera is pointed away from down is 10-15 degrees. This error can be greatly reduced with better PID tuning and system modeling.

This data was acquired by writing the values over the serial port at a constant rate and saving them to a file.

## Conclusion

The designed system met all of the expectations set out at the beginning of the project. We successfully implemented a quaternion-based complementary filter for a 6-DOF IMU, a brushless motor driver, and a PID controller to close the loop, with a UART-based command set to configure and control the gimbal. Moving forward, the system requires an auto-tuning algorithm to learn the dynamics of the attached gimbal and calculate the appropriate PID values. Additionally, a sinusoidal AC driver would help improve the linearity of the motors and also help reduce unnecessary power consumption.

There were no standards applicable to this project.

### Intellectual Property Considerations

All code and hardware design in this project were developed by the group. Bruce Land's code examples and the PLIB code examples were referenced to develop the UART and I2C interfaces. The motor driver hardware was developed from the reference design in the driver datasheet. There is not much literature on the use of IMUs as a feedback system for a brushless motor gimbal system or on efficient quaternion integration, so there is the potential for publication on the topic.

### Ethical Considerations

In designing this project, one of the major applications we considered was camera stabilization on an unmanned aircraft. While our main focus was for use in search and rescue missions, we recognize there also exist many military applications as well. With consideration for the IEEE Code of Ethics "to hold paramount the safety, health, and welfare of the public", we chose not to publically release our source code and instead can provide it on request to future students so that we can "assist colleagues and co-workers in their professional development and to support them in following this code of ethics".

Additionally, this project will be expanded upon and integrated into a larger system for CUAir whose primary mission is to educate on and develop and grow the field of unmanned aerial systems. We believe that this will help "improve the understanding by individuals and society of the capabilities and societal implications of conventional and emerging technologies" and "maintain and improve our technical competence and to undertake technological tasks".

Also we didn't take any bribes.

### Legal Considerations

There are no legal considerations for this project.

## Appendix

### A. Release Consent

The group approves this report for inclusion on the course website. The group approves the video for inclusion on the course YouTube channel.

### C. Schematics

#### D. Bill of Materials

Description MPN Quantity Unit Price Total
PIC32 Microcontroller PIC32MX250F128B 1 $5.00$5.00
MicroStick II DM330013-2 1 $10.00$10.00
Power Supply N/A 1 $5.00$5.00
IMU DFRobot SEN0142 2 $9.90$19.80
Motor Driver TI DRV8313 2 $3.73$7.44
TSSOP Breakout, 3 pack Adafruit 1208 1 $4.95$4.95
Headers N/A 96 $0.05$4.80
Gimbal Mechanism and Camera N/A 1 $0.00$0.00
\$56.99

The primary tasks involved were the brushless motor driver, the IMU interface and filter, the PID controller, and the UART interface. Nikita worked on the brushless motor driver and the IMU driver. Eric worked on the UART interface. Both members worked together on developing and tuning the PID controller.

### F. References

DRV8313 Datasheet

MPU6050 Datasheet

MPU6050 Register Map

Quaternion-based IMU Filter

Brushless Motor Driver Theory

Quaternions and Spatial Rotations

### Contact

Nikita Ermoshkin: ne75

Eric Dai: emd88