FlightGoggles : Acro/Rate Mode Controller

In order to ease the implementation of high-level guidance and control algorithms, a quadcopter rate mode controller can be enabled. This controller allows direct control of the vehicle thrust and angular rates, while maintaining accurate low-level dynamics in simulation.

Low-Pass Filter (LPF)

The rate mode controller employs gyroscope measurements for closed-loop angular rate control. A low-pass Butterworth filter is used to reduce the influence of IMU noise. The filter dynamics are as follows:

where the positive gains and represent the filter damping and stiffness, respectively.

Proportional-Integral-Derivative (PID) Control

A standard PID control design is used to compute the commanded angular acceleration as a function of the angular rate command, as follows:

where , , and are diagonal gain matrices.

Control Allocation

Given the angular acceleration command and the collective thrust command , the controller computes the motor speed commands that result in the appropriate control moment and thrust force.

First, the required control moment is found by inversion of the expressions for the angular acceleration given in Multicopter Dynamics, as follows:

Note that the aerodynamic moment and the rotor and propeller inertia are neglected in the inversion. The stochastic moment is assumed zero.

Next, the expressions for and given in Multicopter Dynamics are inverted to obtain the commanded motor speeds. For a quadcopter in standard configuration, this amounts to inversion of a constant full-rank 4-by-4 matrix, as follows:

where is the moment arm from motor to vehicle center of gravity, i.e., for . (Again, the rotor and propeller inertia is neglected.)

Finally, the signed commanded motor speeds are computed using a sign-aware square-root operation. Note that the motor speed commanded may be clipped to within admissible range in the dynamics simulation. The minimum motor speed is set to zero, while the maximum is set as a ROS Parameter.