The purpose of this research is to develop a control system for spatial stabilization of a gimbal video camera platform with 2 degrees of freedom. The mathematical quaternion theory is applied, which allows to create a convenient and intuitive formalism that uses Rodrigues-Hamilton parameters to describe spatial orientation of a solid body. The method suggested in the research provides a solution for the task of stabilizing a suspended video camera control system. It has been shown as well that from the computational engineering standpoint, this method appears to be the most optimal. The originality of the method is explained by the application of the quaternion algebra transformations in order to determine orientation and to stabilize a video camera on a biaxial suspension. This method allows to utilize mechanical systems with various positions of camera drive units.
Evolution and wide implementation of mobile robots leads to actualization of the development process of stabilization systems for payload (video cameras, thermal imagers, sensors and etc.) spatial positioning.
The development of stabilization systems is associated with the problem of determining orientation of a solid body that arises in various engineering tasks, for example { controlling antenna-based surveillance systems, controlling video equipment which is located on mobile platforms, controlling unmanned aerial vehicles and etc.
Let us consider the objective of mathematical modelling for a platform spatial stabilization control system with a video camera on a suspension with 2 degrees of freedom. This objective has a meaning if a referential coordinate system is established, which is then used to determine solid body orientation.
To solve the existing objective, 7 di erent coordinate systems are used: inertial F I which is tied to Earth; F 1; F 2 { stabilized platform and frame coordinate systems; F B1; F B2 { tied to a stabilized platform and a suspension frame; F I2; F I2{ auxiliary free azimuth coordinate systems.
The purpose of the suggested method is to create a mathematical model for a platform spatial stabilization control system with a video camera on a suspension with 2 degrees of freedom.
In order to achieve the desired goal, the following objectives need to be resolved: 1. Describe a kinematic scheme for the video camera suspension; 2. Provide main de nitions and operations of the quaternion theory that are necessary for mathematical modelling of the video camera suspension stabilization and control system; 3. Derive kinematic equations which bind a solid body angular velocity vector to time derivatives based on kinematic parameters of the model;
2
Spatial positioning stabilization mechanisms are widely used in various modern equipment. The purpose of spatial positioning stabilization equipment is to isolate payload from external perturbations. Such systems are applicable in cinematography[Lew08], unmanned aerial vehicles [Bra12], missile guidance systems[Yu17] and etc. Stabilization systems in general may have three axes of angular stabilization { based on yaw, pitch and roll[Nyb17], but more commonly 2 axes { based on yaw and pitch[Abd13], [Dey14], [Bas17], [Saj13]. The most common regulator that is used to control drive units is PID[Nyb17], [Abd13], [Dey14], while controllers with fuzzy logic[Bas17] and LQR[Saj13] are used less frequently. Practically all of the modern systems utilize brushless direct current motors as their drive units[Bra73]. 3 3.1
The description of the video camera suspension kinematic scheme.
The kinematic scheme of the video camera suspension is provided on gure 1.
While determining spatial orientation of a solid body in relation to a referential coordinate system, the following sensors are used: angular velocity meters { gyroscopes, as well as meters of a sum of linear and gravitational acceleration { accelerometers. The sum of linear and gravitational acceleration is referred to as apparent acceleration. In the general case, to determine spatial orientation, integrated readings of three angular velocity sensors, which are located mutually perpendicular, are used as the main source of information. Spatial position correction is applied based on accelerometer readings, with the assumption that no linear acceleration is present.
Let us provide a description of all 7 coordinate systems used in the research.
Inertial coordinate system F I s tied to Earth, unit vector i1 is directed towards north, j1 is directed towards east, kl is directed down towards the center of Earth. This system is often called NED { North, East, Down. The north direction is designated as inertial x , the east direction { as inertial y, the down direction { as inertial z[Ren15].
Coordinate system F 1 { the origin point of this coordinate system is located at the stabilized camera platform center of gravity, the axes match the NED system.
Coordinate system F 2 { the origin point of this coordinate system is located at the junction point of the suspension frame to the carrier, the coordinate system axes match the NED system.
Coordinate system F B1 hat is tied to the platform { the origin point corresponds to the center of gravity of the stabilized camera platform. x axis is directed forward by the camera optical axis, y axis is directed to the right by drive unit axis M1; z axis supplements the coordinate system up to the right. Unit vectors i, j, k, which correspond to x; y; z axes of this coordinate system, form basis E1 that is tied to the stabilized platform.
Coordinate system F B2 that is tied to the suspension frame { the origin point matches the junction point of the suspension frame to the carrier, x axis is directed forward towards the stabilized platform, z axis is parallel to the frame and is directed to the opposite side of the frame and carrier junction point, y axis supplements the coordinate system up to the right, drive unit axis M2 lies on the plane which is formed by axes x and z. Vectors i; j; k, which correspond to x; y; z axes of system F B2, form basis E2 that is tied to the suspension frame.
Coordinate system F I1 { free in azimuth, the origin point is located in the stabilized platform center of gravity, z axis corresponds to the gravitational acceleration vector direction, the camera optical axis is in the plane formed by x and z axes, the axes supplements coordinate system F I1 up to the right. Basis I1 which is formed by vectors i, j, k, that correspond to x; y; z axes of coordinate system F I1, shall acquire the referential basis role during the process of calculating the stabilized platform resultant rotation quaternion correction and constructing the platform control.
Analogically to system F I1, coordinate system F I2 { free in azimuth, the origin point corresponds to the frame and carrier junction point, z axis corresponds to the gravitational acceleration vector direction, the drive unit motor axis based on roll is located in the plane that is formed by x and z axes, y axis supplements system F I2 up to the right. Basis I2 which is formed by vectors i, j, k, that correspond to x; y; z axes of coordinate system F I2, shall ful ll the referential basis role during the process of determining the suspension frame resultant rotation quaternion correction.
The video camera suspension stabilization system utilizes 2 inertial spatial position sensors. The sensor consists of a combination of 3 gyroscopes and 3 accelerometers with axes, which are positioned mutually perpendicular, as well as a microcontroller that carries out preliminary processing of the obtained data. The inertial sensor measures the projections of the angular velocity vector on to sensitivity axes x; y; z, of a micro electromechanical gyroscope and the projections of apparent acceleration on to the sensitivity axes x; y; z of a micro electromechanical accelerometer. The rst sensor is positioned directly on the camera platform, measurements are carried out in tied coordinate system F B1. The second sensor is mounted on the suspension frame, measurements are carried out in tied coordinate system F B2. Processing of the signals from the spatial position sensors and formation of the controlling actions for the camera drive units is carried out by a specialized controller. Direct control of the camera spatial position is provided by brushless direct current motors based on two axes, one camera drive unit is located on the stabilized camera platform directly, the other one is located on the suspension frame. The application of such motors with direct camera drive ensures absence of mechanical slack and high positioning accuracy of the stabilized platform. There are several di erent kinematic parameters that are used to describe rotary motion of a solid body around a static point: guiding cosines, Euler's angles, Rodrigues-Hamilton parameters. Some of the most convenient parameters are Rodrigues-Hamilton parameters, which represent quaternion components[Bra73]. These parameters do not degenerate (nether the parameters themselves, nor the rates of their change become inde nite) with any position of a solid body; unlike Euler's angles, the number of Rodrigues-Hamilton parameters equals four; there is only one binding equation for these parameters (unlike six equations when guiding cosines are applied). Final rotation of a solid body can be expressed as rotation around a static axis by some degree. According to the Euler's theorem[Bra73], [Lyr61]:\any rotary motion of a solid body is equivalent to at rotation around a certain axis and can be determined by the nal rotation around this axis or by the nal rotation vector, which is directed by the axis of Euler's rotation and has the length that depends on the rotation angle".
The nal rotation vector determines Rodrigues-Hamilton parameters. To describe the solid body spatial position, let us apply the quaternion theory, which shall provide a convenient way of recording the operations that are tied to modelling solid body movements and the operations of projecting vector values from one coordinate system to another. Quaternion { a number composed of real unit 1 and three imaginary units i, j, k, with real elements:
Where y1; y2; y3 { guiding cosines of vector that represents the axis of Euler's rotation of the solid body, # { value of the rotation angle around this axis. It is known that quaternion N , which is de ned as a result of multiplying the quaternions N and M with known parameters:
A quaternion with the norm jj jj = 1 is referred to as normalized quaternion.
= 0 + 1i + 2j + 3k;
M = 0 + 1i + 2j + 3k has the following form and is calculated with the following formula:
N =
M;
where N = 0 + 1i + 2j + 3k while its components with i, j, k have the following form[Bra73]:
If we change the order of the cofactors, considering product M , then the determinant in formula 4 shall change, lines and shall exchange their positions, thus, multiplication of the quaternions is non-commutative.
Quaternion ~ , conjugated to given quaternion , has the following form: 0 = 1 = 2 = 3 = The kinematic equations that bind the angular velocity vector in tied !~E and referential !~I coordinate systems with time derivatives based on the kinematic parameters have the following form [Bra73]:
In our case, the meters that measure angular velocity vector projection are physically located in the tied
coordinate system, hence, we shall consider the equations that have the type of equation (5) or the transformed
equations like equation (10) . Further, it is possible to apply the approximate numerical methods of integrating
the kinematic equations in Rodrigues-Hamilton parameters: the rst-order method (Euler's method), which is
described by formula (
It is also possible to utilize other integration methods of the kinematic equations with norm correction[Bra92].
(5)
(7)
(8)
(9)
(10)
(11)
(
Let us designate the relation of the quaternion de ned in Rodrigues-Hamilton parameters with Euler's angles[Ver09].
In the referential coordinate system, rotation by the angle of yaw ' is executed around axis z in plane XOY , by the angle of pitch # { around axis y, by the angle of roll { around axis x.
The relation formulas for the components of the quaternion, which is de ned in Rodrigues-Hamilton parameters, with Krylov's angles have the following form[Bra73]: 3.4
Sensor signals Output signal of the triaxial gyroscope !~out can be represented as a sum of the actual value of angular velocity vector !~ and generalized error !~[Sye09]: where the generalized gyroscope error can be expressed in a simpli ed form as !~out = !~ +
!~ !~ = b!~ + S!~ + N !~ + (!~) Yaw angle: ' = arctan[ 2( 2 0
1 2( 22 Pitch angle: # = arcsin(2 1 2 where the generalized error can be expressed in the following form[Sye09]:
~a = b~a + S1~a + S2~a + N~a + g + (~a)
Here, b~a { o set, S1 and S2 { matrixes of linear and non-linear scale coe cients, N { matrix that describes non-orthogonality of the sensor measuring axes, g { deviation from the theoretic gravitational acceleration value, (~a) { accelerometer noise. Matrixes S1, N , o set b~a may be found during calibration by a method similar to[Isa16]. 4.1
Instrumental basis construction and correction[Bra92] Let us demonstrate the instrumental basis construction process based on the stabilized platform example.
It is possible to determine a complete spatial position of a body only when two or more reference points, which have their lines of sight located on non-collinear vectors, are available. In our case, it is only possible to utilize one reference point { the gravitational acceleration vector. Therefore, we only have the capability to model the plane formed by vectors i and j. When calculating nal rotation quaternion 1 using formula (10), an integration of all the gyroscope signal constituents takes place, which, supplemented by the error of the integration method, leads to incremental errors in the instrumental basis construction. Thus, instrumental basis I1 emains free in azimuth, that is, it has uncompensated rotation angular velocity around vector k, determined mostly by drift of the gyroscopes located in tied coordinate system F B1 (basis E1).
The spatial position of the sensor in basis I1 is described by nal rotation quaternion 1; it is required to construct instrumental basis I1 which models referential basis I1 in such a way that I1 ! I1 and, correspondingly, 1 ! 1[Bra92].
Let us consider the calculation of the alignment plane construction error in instrumental basis I1 for the sensors located in basis E1[Bra92].
Reference point direction unit vector iI1 (the gravitational acceleration vector direction) shall have in its referential basis I1 components = 0; = 0; = 1g, where g { gravitational acceleration: The same components shall be included in reference point direction unit vector iI2 in basis I2.
Unit vector iI2 in instrumental basis I1 shall have the same components, in[Bra92], this unit vector is referred to as a reference point model.
The reference point direction unit vector and its model are connected by the following relation: Here RI1 { quaternion of the mismatch of instrumental basis I1 relative to referential I1.
The solution of the objective of constructing the alignment plane in basis I1 is carried out in such a way that RI1 ! (1; 0; 0; 0). 4.2
Calculating the error in the instrumental basis plane construction Let us consider the correction construction for the stabilized platform nal rotation quaternion.
The vector of reference point iI1 and its model iI1 mismatch has the following form[Bra92]: The reference point in our case is the following: iI1 = (0; 0; 1g), where g { gravitational acceleration.
The reference point model, as a projection of apparent acceleration measurement vector iB1 from tied basis E1 in nstrumental basis I1 , has the following form:
The correction control of the alignment plane between the instrumental basis and the referential basis is calculated in basis I1 using the following formula:
The alignment construction of the plane formed by vectors i; 0; j of instrumental basis I2 , to the plane formed by i; 0; j of referential basis I2 for the suspension frame is executed analogically.
iI1 = i + j + k iI1 = i + j + k iI2 = i + j + k iI2 = RI1 iI1 ~
RI1 1 = iI1
iI1 iI1 = 1 ~
1 !~I2 = f ( 1 iI1 ) (24) (25) (26) (27) (28) (29) (30)
iI2 iI2 = 2 iB2
~2 !~I2 = f ( 2 !~I2 corr = !~I2 + !~yaw The yaw angle mismatch correction vector for quaternion
2 in referential basis I2 is calculated with the following formula: 4.3
Rotation velocity synchronization for instrument bases I1 and I2 in the azimuth plane In our case, the only reference for the spatial position construction is still the gravitational acceleration vector, therefore, it is only possible to construct a plane perpendicular to the reference point vector. Wherein, it is impossible to determine the azimuth direction of instrumental bases I1 and I2 without including the data about an additional reference point, which should not provide the direction that is collinear to the rst vector. The example of such a vector is the Earth's magnetic eld vector.
Constructively, the yaw angles of the stabilized platform and the frame match, except for the case of exceeding the stabilized platform pitch angle 90 . Nevertheless, due to uncompensated drift of the gyroscopes located in tied bases E1 and E2, azimuth divergence of instrumental bases I1 and I2 occurs, which grows incrementally with time (while the device may be physically static) and renders it impossible to construct a stable stabilization system.
To resolve this problem, it is required to synchronize the rotation speed of bases I1 and I2 in the azimuth plane. Let us deem I1 to be the guiding basis, and I2 to be the corrected basis.
In[Bra92], 2 methods of correcting the instrumental basis based on two reference points are considered. The rst method is superposition of the reference points directions, the second { gradual correction, i.e. the construction of an alignment plane and rotation by the mismatch angle in the constructed plane. In this research, the rst technique of correction is applied.
Let us determine yaw angles of the stabilized platform and the suspension frame 1 and 2 by using the components of quaternions 1 and 2 with formula (16).
The azimuth mismatch of bases I1 and I2 is described by the following angle: !~E1 = f ( ~ 1
( 1 iI1 ) The correction of frame nal rotation quaternion 1) + 1; where _ 1 = f1( ~ 1
( 1 iI1 ) 2 is calculated with the following way: 1) !~E2 = f ( ~ 2 (!~I2 corr + !~yaw) 2) + 1; where _ 1 = f ( ~ 2 (!~I2 corr + !~yaw) 2) Equation (8) with the correction considered is expressed as: The correction for the sensors located in tied bases E1 and E2 can be calculated by the rules of redesigning: ( 1 iI1 ) For the sensor mounted on the suspension frame, we shall have: !~E1 = f ( ~ 1
1) !~E2 = f ( ~ 2 (!~I2 corr + !~yaw) 2)
To increase the alignment accuracy between the instrumental and the referential bases, an integral correction can be applied in the form of[Bra92]: (31) (32) (33) (34) (35) (36) (37) (38) (39) (40)
1(t) Prior to activating the stabilization system, it is necessary to determine the components of the quaternions that describe the spatial orientation of the stabilized platform and the frame. The quaternion components can be calculated using formula (19) , assuming that yaw angle ' s set equal to zero, and the angles of pitch and roll are calculated using the accelerometer data.
It is also possible to engage the system at once with the starting conditions speci ed:
Herewith, the integral component of the correction must not be utilized, the initialization is considered complete when the model of reference point vector iI1 , which is calculated with formula (29) matches the value of reference point iI1 = (0; 0; 1g) with speci ed accuracy. Hereupon, the integral component of the instrumental basis correction is implemented.
The instrumental basis correction algorithm is provided on gure 2.
Constructing the control of the camera platform spatial position The camera platform spatial position control is physically carried out in various coordinate systems. The pitch control drive unit is located on the camera directly, i.e. it is in the basis tied with the camera, the roll control drive unit is located on the mobile base { in the frame basis. Therefore, the necessity of constructing the control in a certain common basis arises.
For our modelled system, when basis I2 ! I1 , the control construction in any of the bases shall be identical. Let us consider the construction of the speci ed camera spatial position quaternion.
Constructively, the stabilization system does not include a yaw control drive unit, based on that, yaw { ' is calculated with the components of quaternion 1 using formula (16).
Roll angle { is set equal to 0, pitch angle # s set within boundaries ( 2 ; 2 ).
The components of de ned spatial position quaternion d are calculated with formula (19).
The quaternion of mismatch between the actual and the de ned stabilized platform spatial orientation can be calculated both in the tied basis using formula (43), and in the referential using formula (44): rE = ~ d Ir =
I
I ~ d 4.6
Constructing the platform spatial orientation control based on the mismatch quaternion components The mismatch quaternion components (43) can be utilized as a controlling signal.
\The application of the quaternion components as controlling signals allows to achieve not only stable control of solid body movement, but, in some cases, a state of control that is close to optimal" [Bra73]. The theoretic ground for the application of the mismatch quaternion components in the construction of the controlling actions is also provided in[Bra73].
For cases of small deviation, tied to the stabilized platform of basis E1, from the position de ned by quaternion d, approximate relations for mismatch quaternion Ir = ( r0; r1; r2; r3), which is calculated in guiding basis I1 , shall be appropriate[Bra73]. (41) (42) (43) (44)
Since the control construction in any of bases I1 is identical for case I2 ! I1 , the orientation control shall be constructed in basis I1 .
For direct control of the motors, an integral back-stepping controller is used[Bou07]. Let us brie y specify the realization of this control method using the example of pitch-based control relative to axis y in referential basis I1 .
e1 = #d # where #d { desired angle, # { actual stabilized platform pitch angle. (46)
In this controller realization, the de ned angle setting error shall be expressed by mismatch quaternion components (45):
e1 = 2 r0 r2 !dy = c1e1 + #_d + 1 1
Z t
0 1 = e1(t)dt (45) (47) (48) (49) The controlling actions on axes x and z are calculated analogically.
The common vector of the controlling actions in referential basis I1 : ~ UI1 = (Ux; Uy; Uz) (50) (51) (52) (53) Since I2 ! I1 , it is possible to express the projections of controlling actions in the tied coordinate systems:
U~ E1 = ~ 1 U~ I1 1; U~ E2 = ~ 2 U~ I1 2 (54)
Considering that constructively the pitch drive unit motor of the stabilized platform is located on axis y in basis E1, and the roll drive unit motor is on axis y of basis E2, in order to control the motors, the corresponding components of vectors U~ E1 and U~ E2 are used.
The algorithm of control formation for the stabilized platform drive units is provided on gure 3. 5
Application of the method described in the article to form control for the drive units allows to implement various kinematic schemes of spatially stabilized platforms. Utilization of the quaternion algebra improves calculation e ciency of the managing controller. The described spatial stabilization system for a video camera on a suspension is realized in the device which was created in the laboratory. [Ren15] Rendal U. Biard, Timoti U. MakLeyn. Malyie bespilotnyie letatelnyie apparatyi: teoriya i praktika. M.
\TEHNOSFERA" 2015{312 s. [Isa16]
A. Isaev. Kalibrovka datchika magnitnogo polya s pomoschyu matritsyi korrektsii. \Aktualnyie nauchnyie issledovaniya v sovremennom mire" ISCIENCE.IN.UA Vyipusk 12(20) Pereslav Hmelnitskiy 2016.