Modelling and stabilizing control of a two-axis gimbal for an unmanned aerial vehicle

In this paper, the modelling and stabilizing control of a two-axis airborne gimbal frame is presented. A combination of analytical as well as statistical techniques are used to model the dynamics of the plant and the actuators. State-space control, Field Oriented Control, non-linear feed-forward compensation as well as Kalman filter sensor fusion are employed in synthesis to control the mechatronic system. Practical methods are used to estimate and validate system variables and parameters. Simulation results are compared to implemented results and the robustness of the control system is verified by means of rigorous parameter variations


Introduction
Stabilizing of platforms and in particular surveillance platforms are gaining in popularity as modern techniques simplifies the development process by a great extent. Various stabilizing techniques exist such as digital image stabilization [1] (software based) and mechanical image stabilization (electro-mechanical based) by means of a multiaxis gyro platform [2]. A more recent development in the field is to stabilize a multi-axis platform suspended from an unmanned aerial vehicle by means of direct current (DC) or Brushless direct current (BLDC) motors [3], inertial measurement sensors and other electronic hardware. The goal of the platform stabilization is to keep its payload orientation constant while the unmanned aerial vehicle might be subjected to movements and disturbances from the weather and other elements. The payload of such a platform normally comprises of important surveillance equipment. This type of surveillance might be used for civil, commercial or military purposes [4].
Various commercial as well as industrial or military techniques have been developed to commutate the axis actuators and control the platform orientation. More commercial applications make use of low end open-loop motor control and classical control methods for platform orientation where industrial or military applications normally make use of Direct Torque Control, Field Oriented Control and other vector-based control methods. State or model-based controllers are widely used in combination with these vector-based motor controllers.
This paper bases its designs on a gimbal platform DYS-BLG5D [9] as shown in Fig. 1 with Permanent Magnet Synchronous Motors BGM5208-75 [10]. The gimbal has an un-loaded mass of around 1.7kg, boasts a high frequency damper plate that attaches to the UAVs base and is constructed from carbon fiber tubes and aircraft aluminum joiners and brackets. A custom design box type CCTV camera is used as the surveillance payload.

Figure 1
The mechanical frame (BLG5D) used for the design and control This paper aims to show a systematic approach to analyze, estimate and validate system parameters, design suitable control systems and validate simulations with implemented experimentation for the designed gimbal. The system response to various parameter variations is also addressed. Most parts of the mechatronic system are implemented to support the simulated results. Hardware and software used will be discussed in each applicable section. In the results section it will be shown that the system was able to robustly stabilize the two-axis system to an orientation angle of less than 0.0087 rad (0.5°). This is achieved whilst being subjected to severe parameter variations and a disturbance of 150 °/s induced on the system. The paper is organized as follows. In Section 2 the dynamic frame modelling is presented which is extended by introducing an appropriate friction model. The operational points will be discussed, the actuators will be modelled and finally a holistic model will be developed by means of system identification. In Section 3 a control system overview will be followed by detailed control system discussions for each section within this mechatronic system. Some techniques will be discussed regarding the parameter estimation of the Linear Quadratic Regulator (LQR) as well as the Kalman Filter. Section 4 will discuss the complete integration of the systems pertaining to software, electronics as well as hardware. The penultimate section, Section 4, will present the scientific results and lastly in Section 6 the conclusion of the paper will be outlined.

Materials and methods
An overview of the mechanical system is shown in Fig. 2. The figure depicts the coordinate frame of the two-axis system, where orientation angle α is concerned with the inner gimbal frame and orientation angle β with the outer gimbal frame.
The rotational transformation matrices [11] used to describe the forward kinematics are given by (1) and (2). Where BO R and OI R denote the rotational transformation from the base of the UAV towards the outer gimbal frame and the one from the outer gimbal frame to the inner gimbal frame respectively.

Figure 2
The coordinate frame of the two-axis system

Frame dynamics
The Newton Euler equations for rotational dynamics [12] expressed in the three dimensional Cartesian coordinate system are shown in (3) and (4). Equations (5) and (6) are the torque relationships around each axis of concern, which are obtained by summing the kinematic torque components about the two axes individually.  (1) and (2), the angular rates can further be described by (7) to (10).
Simultaneously solve (3) to (6), one can find the frame dynamics model for the inner and outer gimbals as shown in (11) and (12) respectively. Iz  Ix  Iz  Ix  Iy  Iy   T  T  I  I

Friction model
Another important factor that should be considered is the inclusion of an appropriate friction model. The rotational friction components considered include the static or breakaway torque loss ( b T ), the Stribeck transition region as well as the coulomb torque losses ( c T ) which are both dependent on the actuator shaft velocity and direction of shaft rotation. The general description for a friction model as described in [13] was used and is shown in (13).
is the rotational velocity of the actuator rotor and baseY  is the angular rate caused by the UAVs movements about the y axis.

Relevant operational points
The dynamics model contains control variables, base disturbances and other components. For the base disturbances components (encapsulated within the friction model) to be realistic for implementation, a second IMU will have to be used in addition to the one attached to the stabilizing frame depicted in Fig. 2. In fact, the base disturbances component in the friction model merely represents the torque transfer from the UAV base movements through its mechanical coupling (the rotor and bearing) onto the two respective gimbal frames. In other words, the base disturbances component describes how the UAVs movements will affect the gimbal frames. In this research, the base disturbances are removed from the friction model but added to the system as a type of feed-forward process noise which ensured that the model transfer function response remains unchanged but provided a way of simplifying the model as well as reducing the number of sensors needed. The result yield the base  term in the friction model to become zero as well as Oy  and Oz  .
As both angles α and β will be controlled and stabilized to around 0 rad (  0 ) in respect to the ground, these angles might be considered to be zero in (7)-(10) therefore resulting in Ix  to be zero. If α and β were not to be controlled to zero, the terms Oy  , Oz  and Ix  will remain in (11)-(12) and most certainly have an effect on the resulting torque components acting on each axis. From (7) to (10) and (11) to (13), and taking the operating points into consideration, simplified models for both axes were derived.

Actuator Modelling
The actuators are two permanent magnet synchronous motors (PMSM) with 24 rotor poles and 22 stator poles. Parameter estimation was used to characterize the motors which is discussed in section 4, System Implementation. The motor parameters found were used in a simulation model that was adapted and modified to be controlled by a fieldoriented control (FOC) structure that was shown in Table 1 (Control Systems section). The model is shown in Fig. 3.

System Identification
The statistical modelling approach [14] was employed to derive an appropriate linear model for the motors and plant. By the least-squares method, the fourth-order autoregressive model (ARX) was used to fit the data to a state space structure representing the dynamics of the motor and plant combinations as shown in (16). The order was limited to a fourth order as it proved successful in characterising the training data without adding unnecessary complexity to the overall model order and structure. The structure of the ARX model [15] is given in (16).
Where b n is the number of zeros of the system and a n is the number of poles of the system, both being 4 in this case. k n is the delay. After the statistical modelling approach, the following single-input single-output (SISO) state space representation for the inner gimbal was found. The input command to the SISO system is torque from the motor coupled to the inner gimbal and the output is the orientation angle α itself.
The orientation angle α will be found when the output of the SISO system, 1 y , is solved with each sampling period. A similar approach was used to find the representation for the outer orientation angle β. It should be noted that this identified system does not contain the non-linear friction model. The reasoning to this will be explained in the control system design.

Control Overview
It is important to note that most of the design process will focus on the inner gimbal frame as the inner and outer gimbal frames mathematical model structures are identical. As a mechatronic system consisting of multiple areas of control, control methods are identified for each area separately, and synthesized to co-operate in the final desired response. The considered areas and corresponding methods are listed in Table 1. The control strategy with a holistic overview can be seen in Fig. 4. The Kalman filter applied here has been done in the form of sensor fusion. The Kalman filter would filter and estimate the states by making use of multiple values from the Inertial Measurement Unit (IMU) together with a mathematical model of the IMU.

Figure 4
Overview of control structure

Actuator Torque Control
PMSM are electronically commutated machines and therefore needs to be controlled with a method that uses the rotor position and controls the currents flowing through the coils at a frequency high enough to characterize the current waveform. For the purpose of this project a sampling rate of 1 kHz was used. The control algorithm used for the electronic commutation was chosen to be field oriented control (FOC). The FOC is a vector-based algorithm which aims to simplify complex 3-phase control to a more suitable two-phase control structure. Furthermore, FOC has been proven to provide superior performance [16] from stall speed all the way to maximum rotation speed with minimum torque ripple and maximum torque per ampere.
The commutator (PMSM) control system structure is depicted in Fig. 5.

Figure 5 FOC control structure
The FOC algorithm can practically be realized by the following steps:

Step 1
Two of the three phase currents need to be sampled and by means of Kirchhoff's junction rule, the third current value is calculated by (17).

Step 2
A two-phase current representation will be derived from the three-phase measurements by making use of the Forward Clark transform after which a stationary to rotating reference frame transform will be made by means of the Forward Park transform.

Step 4
Here the q d V V , are used with the Reverse Park transform in (22) and (23) to form two variables   V V , , which represents the rotating to stationary transformation.
They are then transformed back to the three-phase representation by means on the Reverse Clark transform in (24) to V , and c V are three phase voltage magnitudes, who's phase angles are 120° to each other which are applied to each coil of the PMSM by means of space vector pulse width modulation and power stage hardware which would be discussed in the Hardware and Implementation section.
The values for the two PI controllers in the FOC structure were initialized by making use of the method proposed in [17].
Where 0  is the loop bandwidth and  is the current loop attenuation.

Gimbal frame position control
State space or model-based control can be beneficial when a solid knowledge of the system dynamics to be controlled is known. Assuming that the noise in the Discrete Linear Time-Invariant [18] system is considered as uncorrelated, zero mean Gaussian distributed signals the state space model of the system is presented in (29) to (30).
are determined from the identification of ARX model, which contains the coupled dynamics of the actuator and the inner gimbal frame. v is the measurement noise from the IMU, and w is the process noise. A full state feedback structure is used for control purposes. The control law (31) becomes relevant when using full state feedback.
Where t x is the full state feedback of the system, K is the feedback gains, t r is the reference, and t u is the control signal to modify the dynamics of the system to follow the reference signal. The noise will be addressed in a later section.
The method for determining the K values was chosen to be by means of the Linear-Quadratic Regulator (LQR) algorithm. The cost function [19] is minimized by means of the K gains determined by the LQR. Furthermore, two covariance matrices Q and R are setup for the LQR algorithm.  Numerically larger values in the Q matrix benefits the state by attempting to decrease the convergence time of that state.
R is the measurement noise covariance matrix. Numerically larger values in this matrix changes the K values in such a way that it will decrease the aggressiveness of the overall control effort.

Feed-forward friction compensation
In the previous section with the LQR design, the friction components were removed when the controller was designed. The friction component is of utmost importance as the nature of the system is to stabilize around zero which means low rotational velocities and many directional changes would be observed. The friction model is highly non-linear in the low velocity range and needed an appropriate feed-forward torque compensator based on the shaft velocity supplied by the incremental encoder. A simple piece wise representation of the friction compensation equation (13) is considered.

Kalman filter sensor fusion
A common MEMS gyroscope will be used to measure some of the output states of the gimbal frame whilst in operation.
As not all of the states can directly be measured, some sensor fusion technique will be used to derive the desired states. The sensor can measure tri-axial angular rate ( s rad / ) as well as tri-axial linear acceleration ( 2 s m ). The angular rate is useful as it corresponds to the state variable of the gimbal platform system. The second state to be derived is the angular orientation. Some translation and rotation algebra can be used to represent orientation angles by making use of linear accelerations. The process started by normalizing [21] the tri-axial angular rates. The angular orientations would not be used as is, instead they will be combined with the angular rates to estimate a more accurate and precise angular orientation. Towards this purpose, a Kalman filter is set up, similarly to the LQR, R and Q matrices needed to be defined. R is the measurement noise and is found by means of experimentation and Q is the process noise matrix.
A method proposed in [22] uses the fusion algorithm with the model of the gyroscope itself rather than the plant model. Both sensor values are derived by the electro-mechanisms within the MEMS gyroscope and relate to each other in the following ordinary differential equation (ODE): where Iy  is the angular rate of the Y axis of the inner gimbal frame, t  is the present orientation angle of the inner gimbal, The ODE can be represented in the state space form: x is the previous full state feedback of the system, t u in this case is the angular rate of the Y axis,  and n is the amount of terms to be used. Here n was set to 3.
The process noise covariance matrix was initialized by (43).
k Q is used as the static matrix, then it can be multiplied by the spectral density factor  to tune the system towards the desired output as shown in (45).
The Kalman filter estimates the angular orientation by means of a sensor fusion algorithm and a simple way of tuning the process noise covariance matrix.

Actuators
The actuators used in the research are two Permanent Magnet Synchronous Motors (PMSM) of model BGM5208-75. From the manufacturer it was supplied that they have 24 rotor poles, 22 stator poles and a stator resistance of 15Ω. Some parameters essential to the characterization of the motor need to be determined. These include the coil inductance, the voltage and the torque constants.
The inductance is calculated by some method of injecting a low frequency AC waveform onto two of the coils of the motor. Various readings and equations would be solved to get to a theoretical inductance value for the motor. Some of the common equations used in the process are shown in (46) to (49). Where Z is impedance, L X is reactance, L is the inductance and Hz is the frequency of the injected AC waveform. The measurement inductance was verified by means of an inductance meter.
The voltage constant, e K , was found by means of observing the amplitude of the Back-EMF when the rotor was being turned at different set speeds. From the voltage constant the torque constant was found by means of (51). In this paper, another method of verifying the motor parameters is used by making use of an on-line software estimation provided by INSTASPIN_FOC from Texas Instruments. The hardware used was the DRV8301 power-stage board as well as the F28069 Launchpad development board.

System identification
Once the Ordinary Differential Equation, (14) to (15) of the gimbal frames has been set up, the actuator model was combined with this model where the input command would be directed to the actuator model and in turn the actuator's corresponding torque output would stimulate the input T _  and input T _  terms of the gimbal frame models in (14) to (15).

Figure 7 System Identification ARX model validation results
This model is stimulated by a series of realistic and conditioned input data at a sample rate of 0.01s while the output states were observed. Fig. 6 shows a portion of the input (u1, bottom) and output (y1, top) training data used for the system identification of the combined plant. The input represents a torque command in Nm and the output data the system response (angular orientation) in radians.
Verification of the identified model is crucial to the validity of results following with the use of this combined model. The cross correlation between the training set data and the identified model as well as the autocorrelation of residuals has been evaluated. From Fig. 7, one finds that both the autocorrelation as well as the cross-correlation tests proved successful as the function lies well in-between the 99% confidence intervals shown as black dotted lines. 264

Friction and Inertia Estimation
To find the inertia, an accurate scale model was produced in Solid Works with all the geometric parameters of each part in the overall assembly of the gimbal frame. In-software inertia estimation was then done on the assembly. The Solid Works model is shown in Fig. 8.

Figure 8 CAD model of gimbal frames
The friction components (Static friction losses, Stribeck region and Coulomb friction losses) were estimated by a series of repetitive on-line load tests with the motor by varying commanding torque to the FOC actuator controller and by varying the load of the motor under certain conditions while evaluating the motor response for every scenario.

Figure 9
Testing rig A second validation test is done by means of an on-line software method with INSTASPIN_MOTION by Texas Instruments. The same electronic hardware was used as in the previous section. For this test a temporary suspension rig was created for the gimbal frame to hang from. The inertia and other tests are implemented more realistically by means of the suspension rig as is Fig. 9.

Figure 10
Hardware Setup

Control Stage (FOC, LQR and Kalman filter)
The FOC mathematical structure, the coil current feedback (embedded shunt resistors on the DRV8301 power stage board) and the position feedback (Incremental encoder, HEDM-5500-J06 with 2095 counts per revolution) from the incremental encoder was programmed on the DRV8301 and F28069 Launchpad combination. This set of electronic hardware was used to program and act as a type of standalone torque controller and driver for the actuators.
The main control stage board was the STM32F4 Discovery from ST Microelectronics. This controller manages the IMU (MPU-6050 6DOF sensor) state feedback as well as the control structure itself. The control stage board multiplies the sensor feedback values (for both axis) with the LQR gains and accordingly send a current (torque) and direction command via a DAC channel to the F28069 Launchpad combination. The control stage platform will also handle the Kalman filtering. In Fig. 10 the hardware setup can be seen which controls one of the axis actuators (PMSM).

Actuators
After making use of the experimental method of estimating the motor parameters, the on-line software method was used to verify the values. In most cases the motor manufacturer would supply a datasheet with the required parameters in which case the parameter estimation section in this text would not be needed to characterize the motor. No significant value discrepancies can be observed between the two methods used, which means the estimated parameters are reliable.

Friction and Inertia Estimation
The principle inertia estimated by means of the CAD model yielded a value of 0.00146 2 kgm for the inner gimbal frame and 0.009 2 kgm for the outer gimbal frame whereas the on-line software method produced the values of 0.00161 2 kgm and 0.0099 2 kgm for the inner and outer gimbal frames respectively. Both values are within 10% deviation from each other and will therefore be considered to validate each method used. The inertia tensors (negating cross-coupled components) that were found and used are given in (52)  Friction estimation with the software method only yielded results for viscous friction. As viscous friction was not part of the modelled structure, only the experimental friction components were considered.
The breakaway frictional losses were estimated to be 0.0182 Nm, the Stribeck region losses estimated from 0.0156 Nm to 0.006 Nm and the Coulomb frictional losses were estimated around 0.0052 Nm. The variable k in the friction model is 0.4 to ensure that the Stribeck region decays to zero leaving only Coulomb friction at about 95 RPM of rotor speed and above.
The inertial and frictional values were used in the different models created for simulation as well as implementation.

Figure 11 Simulated FOC Id and Iq results
With the motor parameters found, and the PI controller values tuned within the FOC structure, the motor controller can be simulated as well as implemented.

Figure 12 Measured FOC Id and Iq results
Two of the most important variables to consider in the FOC structure are the current values q I and d I . The FOC structure was setup to run at a sampling rate of 1 kHz. Fig. 11 and Fig. 12 depict q I and d I under simulation and experimentation respectively. The system was subject to a reference signal of 0.55sin( t  2 ) Ampere. Comparing both figures one finds that the controller is keeping the d I value close to zero. The maximum deviation from zero is around 1.2 mA for the simulated and 1.7 mA for the actual system. For the reference tracking in q I the simulation is accurate and precise where the q I measured from the real system can be seen to be accurate as well. The errors are a result of sensing noise and tolerances on shunt resistors, ADC sampling noise and other un-modelled system responses.
The FOC controller was then subject to a step input of about half the expected current of the actuators which is 0.55 A to evaluate for critical characteristics such as rise time.   Fig. 13 the rise time for the simulated step response was seen to be around 0.0022s (10%-90%) and the rise time measured from the implemented system around 0.003s (10%-90%), which means the simulation and measured values from implementation validate each other. Fig. 14 shows region around the step rise time that had to be reconstructed as the rise time is about three times the sampling period of the system, which makes the step-like curvature difficult to read a definite value from. The FOC algorithm rate is then increased to 10 kHz for better signal reconstruction by the current sensors and the incremental position encoder.

Kalman Filter Sensor Fusion
The sensory feedback needs to be as precise and accurate as possible to ensure realistic signal re-construction for use by the control system. All tests in this paper regarding the Kalman Filter were done on a hardware level, no simulations were attempted. Fig. 15, shows the angular orientation response of the IMU in a position of rest. The black line in the figure serves as control data and shows the desired position as seen from the incremental encoder.

Figure 15 IMU static response
The Kalman filter was able to improve the standard deviation of the noise by more than three times from ±0.01 rad (±0.6°) down to ±0.0029 rad (±0.17°). Fig. 16, emphasizes the ability of the Kalman filter to accurately estimate the angular orientation while the sensor is rotating about an axis. The yellow line on the graph is the position as seen from the incremental encoder attached and rotating about the same shaft as the IMU.

Figure 16 IMU dynamic response
In Fig. 16 from about 3s to 6s sharp impulse disturbances are introduced to the frame and observed. The Kalman filter rejects the impulse disturbances. The Kalman filter was also seen to be able to handle ringing in the areas of transitions and directional changes very well.

LQR Position Controller
Simulations were done with a perturbation of 150 °/s and all simulation results shown are based on the inner gimbal frame unless otherwise stated. Initially the friction model and friction compensation has been removed from the system. Fig. 17 shows the control being applied after around 2s. Although the state of concern is the angular orientation, full state feedback requires all states and therefore both angular orientation and angular velocity will be shown.

Figure 17 Measured FOC step response
As it is expected the angle keep increasing until the control is applied. Once the control has been applied a sharp spike in the angular velocity is seen in an attempt to drive the states to zero. However, the transition rate here is not important as the system will not be used in this manner. This test is merely done to show the control action of the full state feedback system and its natural characteristic to drive to asymptotic stability at a value of zero. In Fig. 18, the steady-state potion of Fig. 17 was zoomed in.
From the steady-state region shown in Fig. 18, the controller successfully rejected the perturbation and controlled to within ±0.5° of the desired zero value.

Figure 18
Steady state area of system response In Fig. 19 the control effort was applied from 0s and the friction model introduced without the friction compensation.   Fig. 20 it is clear that the feed-forward compensation is vital as the un-compensated system only managed to control to within ±0.035 rad (±2.05°) and the fully compensated system down to ±0.0045 rad (±0.26°). Fig. 21 shows the system response for the outer gimbal frame under similar perturbation and with the friction model as well as the friction compensation included. The outer gimbal frame was able to be stabilized to within ±0.0059 rad (±0.34°) from its desired zero reference.

Figure 21
System response for outer gimbal with friction model as well as friction compensation

Parameter Variation
A robustness check was performed by varying the parameters found for the system. It is desired that the system should be insensitive to slight and medium variations of parameters. The table below shows the original parameter value as well as the varied value to use for the robustness test.

Henry
The inertia of the system has been doubled for both the inner and outer gimbal frames while a whole combination of other less severe variations has been introduced simultaneously as can be seen in Table 3. The system response for both the inner and the outer gimbal frames are shown in Fig. 22 and Fig. 23 respectively.

Figure 22
Parameter variation on Inner gimbal frame

Figure 23
Parameter variation on Outer gimbal frame Both axes were able to be stabilized to less than 0.0087 rad (0.5°) despite the various parameter modifications.

Conclusion
In this paper, the electromechanical control system was designed and implemented for a two-axis gimbal system with inner and outer movable frames. The parameter estimation, system modelling and identification, and friction compensation were considered in the framework of FOC and LQR controllers.
The proposed system was verified for its effectiveness and robustness to parameter variation. The feed-forward friction compensation rejected the non-linear friction components, which improved the control performance significantly. The Kalman filter was applied to provide high quality state feedback via sensing fusion.
It was demonstrated that the proposed system is parameter-insensitive to a great degree. Validation and cross validation of estimated model variables, friction model variables and motor parameters by means of implementation ensured that the system design is realistic and robust.

Disclosure of conflict of interest
The authors declare that there is no conflict of interest. The authors alone are responsible for the content of the paper.