MODARES JOURNAL OF ELECTRICAL ENGINEERING, VOL. 13, NO.4, WINTER 2014
is used which the sensor is mounted in the inner gimbal. Two rate gyroscopes are placed beside the sensor which measures the angular velocity of the sensor in the inertial space. The output signal of the gyroscopes is used as feedback to the gimbal torquers through a control system to make the LOS of the sensor follow certain rate commands [7-9]. In many applications, LOS stabilization is enough for reaching the requirements [10, 12]. However, in some applications such as camera stabilization, the LOS direction could be stabilized with the help of mentioned gimbal configuration, but the image rotation cannot be compensated and cause performance degradation. For doing this, an extra gimbal should be provided to achieve image rotation compensation and better isolation from the base movements and disturbances. Different kind of disturbances is affecting the sensor including platform movements, cross coupling between gimbals, and some imperfections in the gimbals mechanism. To keep the sensor direction constant in the inertial space, these disturbances should be rejected or attenuated. Therefore, dynamic modeling of the gimbaled system is necessary for identifying these disturbances and designing a control system structure. The dynamic modeling and designing the control system for two-axes gimbal system have been studied extensively [10, 13-17], but there is a few works on threeaxes gimbal system. In this paper, the equation of motion of a three axes gimbal system is derived by the moment equation. The effect of angular velocities of the base into the gimbaled dynamic system and cross-coupling between gimbals are discussed. In addition, some critical notes are presented for constructing the gimbal assembly. Moreover, a control strategy is presented for controlling the gimbal dynamic. The organization of this paper is as follows. In Section II, the kinematics of the gimbal system is presented. In Section III, the equation of motion for the system is derived by the moment equation. In Section IV, a control strategy is proposed and the inverse kinematics is presented. Finally, Section V concludes the paper.
Equations of Motion Extraction for a Three Axes Gimbal System Ali Kazemy1,*, Mahdi Siahi 2 Received: 05/04/2016
Accepted: 30/06/2016
Abstract Inertial stabilization of a sensor mounted on nonstationary platforms is an important task in many applications such as image processing, astronomical telescopes, and tracking systems. For this purpose, some type of gimbaling arrangements is typically used. For implementing the LOS stabilization, usually a two-axes gimbal system is used which the sensor is mounted in the inner gimbal. The dynamic modeling and designing the control system for two axes gimbal system have been studied extensively, but there is a few works on three axes gimbal system. In this paper, the equation of motion for a three axes gimbal system is derived by the moment equation. The effect of angular velocities of the base into the gimbaled dynamic system and cross-coupling between gimbals are presented. In addition, some critical notes are presented for constructing the gimbal assembly. Moreover, a model based control strategy is proposed for controlling the gimbal dynamics.
Keywords: Gimbal system, Inertial Stabilization Platform, Equation of Motion, Dynamic System.
1. Introduction nertial stabilization of a sensor (such as Television, Telescope, IR, Radar, and Laser) mounted on non-stationary platforms is an important task in many applications such as image processing, guided missiles, astronomical telescopes, and tracking systems [1-5]. In such systems, the Line-of-Sight (LOS) of sensor must be pointed to a constant or a moving target while the base of the sensor is a moving platform. For this purpose, some type of gimbaling arrangements equipped with inertial sensors is typically used [2, 6]. For implementing the LOS stabilization, usually a two-axes gimbal system
1.Department
of Electrical Engineering, Tafresh University, Tafresh.
[email protected]. 2. Department of Electrical Engineering, Islamic Azad University, Garmsar Branch, Iran. 37
MODARES JOURNAL OF ELECTRICAL ENGINEERING, VOL. 13, NO.4, WINTER 2014
ω BIB = [ p q
2. Gimbal System Kinematics Consider a three axes gimbal system and related reference frames as depicted in Figure 1. As it shown in this figure, five reference frames are introduced: the inertial coordinate frame I, a body-fixed frame B, and three frames Y, P, and R fixed to the yaw, pitch, and roll gimbals, respectively. The sensor is placed and fixed on the roll gimbal (frame R) which x R -axis coincides with the sensor optical axis or the sensor LOS. Axes directions in frame I and B are as usual as flight dynamics which z-axis pointing down and y-axis is along the right wing [18]. All frames origins are coincident and the gimbals are considered as rigid bodies. The body-fixed frame B is carried into frame Y by a positive angle of rotation θY about the z B -axis. Frame Y is carried into frame P by a positive angle of rotation θ P about the yY -axis. Finally, frame P is carried into frame R by a positive angle of rotation θ R about the x P -axis. Hence, we have the following transformations with respect to these rotations: cY CYB = −sY 0 c P C = 0 s P P Y
sY cY 0
0 0 , 1
T
(4)
is measured, where ω BIB is the angular velocity of frame B respect to frame I, and introduced in frame B. Note that most of moving platforms which the sensor mounted on them (such as helicopters, missiles, ships, and etc.) uses Inertial Measurement Unit (IMU) for navigation system. Hence, ω BIB is measured and available. Angular velocities of frame Y, P, and R respect to frame I, and introduced in its own frames are: ω
Y IY
0 = ω + ω = C ω + 0 θ&Y pcY + qsY pY = qcY − psY = qY , r + θ&Y rY Y IB
Y BY
Y B
B IB
(5)
0 pY c P − rY s P ω PIP = ω PIY + ωYPP = CYP ωYIY + θ&P = qY + θ&P 0 pY s P + rY c P
(
)
(
)
c P ( pcY + qsY ) − s P r + θ&Y = qcY − psY + θ&P s pc + qsY ) + c P r + θ&Y P ( Y
(1)
0 −s P 1 0 , 0 c P
r] ,
ω RIR = ω RIP + ω RPR
(2)
(6)
pP = q P , r P
θ&R p P + θ&R p R R P = CP ω IP + 0 = q P c R + rP s R = q R 0 −q P s R + rP c R rR
(
)
θ&R − s P r + θ&Y + c P ( pcY + qsY ) = s R s P ( pcY + qsY ) + c P r + θ&Y + c R θ&P + qcY − psY c s pc + qsY ) + c P r + θ&Y − s R θ&P + qcY − psY R P ( Y
( (
( (
)) ))
( (
) )
,
(7) where CYB , CYP , and CRP are introduced in (1)-(3). The inertia matrices of the gimbals are Frame I (Inertial) Frame B (Base)
Frame Y (Yaw)
Frame P (Pitch)
Frame R (Roll)
Fig 1: Reference frames of a three-axes gimbal
0 s R , c R
(8)
J P = diag {J Px , J Py , J Pz } ,
(9)
J R = diag {J Rx , J Ry , J Rz } ,
(10)
where diag {J kx , J ky , J kz } refers to a real matrix with diagonal elements J kx , J ky , J kz ( k =Y , P , R ). For simplicity the off-diagonal elements of inertia matrices, the products of inertia, are assumed to be neglected and just the moments of inertia are considered. In addition, it is assumed that the centers of mass of gimbals are located in the centers of their common rotation. In other words, it is assumed that there is no mass
system 1 0 CRP = 0 c R 0 −s R
JY = diag {JY x , JY y , JY z } ,
(3)
where Cij is the transformation from frame i to j , and c i = cos θi , s i = sin θi . Assumption 1: It is assumed that 38
KAZEMY AND SIAHI :EQUATIONS OF MOTION EXTRACTION FOR A THREE AXES GIMBAL SYSTEM
unbalance in the gimbals. Furthermore, external lumped torques τY , τ P , and τ R about z B , yY ,and x P axes, respectively, are applied to gimbals from motor and other external disturbance torques.
With considering Assumption 2, (14) can be written as J Rx θ&&R = τ R + τ RI + τ RB
(16)
where
(
3. Equations of Motion Consider a rigid body and a reference frame x, y, and z fixed to this body. Also, assume that this body has inertia matrix J and rotate in an inertial frame with angular velocity ω , then the angular momentum of the body will be H = Jω . Hence, The moment equation for the body is τ = dH / dt + ω × H , where τ is the total external torques [19]. In the following, the equation of motion for the gimbals will be derived separately with the help of the moment equation.
τ RI = J Rx θ&Y θ&P c P + θ&&Y s p
(
& p + θ&P s P ( pcY + qsY τ RB = J Rx θ&P rc P + rs
dH R + ω RIR × H R dt
2.2 Pitch Channel The angular momentum of the pitch gimbal is
(12)
P R H P = J P ω IP + ( CPR ) J R ω IR T
Since the roll gimbal can only rotate about its x-axis, the moment equation (12) about this axis with respect to (11) and (7) is (13) τ R = J Rx p& R + ( J Rz − J Ry ) q R rR .
(
(
)
(
)
(
& Y + qs & Y − θ&Y psY + θ&Y qcY +s p r& + θ&&Y − c P pc
(s (s ( pc (c (s ( pc
( )) + c (θ& ( r + θ& ) ) − s (θ&
R
P
Y
+ qsY ) + c P r + θ&Y
R
P
Y
+ qsY ) + c P
R
Y
R
Rz
P
+ qcY − psY
P
+ qcY − psY
where the second part of the equation is the angular momentum of the roll gimbal transformed to pitch frame. The moment equation for the roll gimbal is
− J Ry ) ×
)) × )).
τP =
(14) .
dH P + ω PIP × H P dt
.
(20)
Since the pitch gimbal can only rotate about its y-axis, the moment equation (20) about this axis with respect to (19) and (6) is
Assumption 2: It is assumed that J Rz = J Ry
)
(19)
)
)) − ( J
(
J Px p P + J Rx p P + θ&R = J Py q P + J Ry c R (q P c R + rP s R ) − J Rz s R ( −q P s R + rP c R ) . J Pz rP + J Ry s R (q P c R + rP s R ) + J Rz c R ( −q P s R + rP c R )
With substituting (7) in (13) and derivation p R , (13) can be written as J Rx θ&&R = τ R + J Rx θ&P c P r + θ&Y + θ&P s P ( pcY + qsY
(18)
))
is the effect of base movements to roll channel. From (17), it is obvious that in the case θ P = ±90° the rotation axis of yaw and roll channel will be coincident and have same effect on the sensor. Hence, how much the pitch rotation deviate from zero, the interaction between the yaw and roll channel will be increased. In addition, changing the angular velocity of the base around z-axis ( r& ) has the same effect on the roll channel. Moreover, in (17), J Rx θ&Y θ&P c P is the coriolis effect from yaw and pitch channels to the roll channel.
(11)
.
(
)
& Y + qs & Y − θ&Y psY + θ&Y qcY −c P pc
where the parameters are introduced in (7) and (10). So, the moment equation for the roll gimbal can be written as τR =
(17)
is the interaction between yaw and pitch channels to roll and
2.1. Roll Channel The angular momentum of the roll gimbal is J Rx p R R H R = J R ω IR = J Ry q R , J Rz rR
)
(15)
τ P = q&P ( J Py + J Ry c R2 + J Rz s R2 ) + r&P s R c R ( J Ry − J Rz
(
+ q p 2θ&R s R c R + p P s R c R
Remark 1: The assumption 2 is a practical and critical point and should be considered in the mechanism design procedure. Considering this assumption in implementation phase will be reduced interaction, increase the performance, and simplify the controller design.
+ rP θ&R
((s
2 R
−c
2 R
) (J
Rz
)(J
Rz
− J Ry
− J Ry ) + J Rx
)
)
+ p P rP ( J Px + J Rx − J Pz − J Ry s R2 − J Rz c R2 ) .
39
) (21)
MODARES JOURNAL OF ELECTRICAL ENGINEERING, VOL. 13, NO.4, WINTER 2014
HY = JY ωYIY + ( CYP )
With substituting (6) in (21) and derivation q P and rP , (21) can be written as
(
)
& Y − qc & Y + θ&Y qsY + θ&Y pcY J PT θ&&P = τ P + J PT ps
(
(
& Y + qs & Y − θ&Y psY + θ&Y qcY + θ&P c P ( pcY + qsY ) + s P pc
T
)
(
+ qcY − psY + θ&P P
Y
R
Y
P
P
Ry
Px
Y
Rx
)
P
Rz
R
2 R
2 R
Y
Rz
Y
Ry
Y
2 R
(22) where J PT = J Py + J Ry c + J Rz s is the total moment of inertia of the pitch gimbal bout y-axis. With considering Assumption 2, (22) can be written as 2 R
(J
Py
2 R
+ J Ry ) θ&&P = τ P + τ PI + τ PB ,
τY =
)
(24)
is the interaction between yaw and roll channels to pitch and τ PB = ( J Py + J Ry +θ&Y
((
(
(s − c 2 P
2 P
) ( ps&
Y
& Y + θ&Y qsY + θ&Y pcY − qc
)
)
) ( pcY + qsY ) + 2s P c P r ( J Px + J Rx − J Pz − J Ry
− 2c P ( pcY + qsY ) − 2s P r − s P θ&Y
)
) ( s ( pc P
Y
−J Rx θ&R ( s P ( pcY + qsY ) + rc P ) ,
)
)
. )
dHY + ωYIY × HY dt
.
(27)
(
− J Px ) − J Rx θ&R θ&P c P + p&Y c P − θ&P pY s P − r&Y s P − θ&P rY c P ×
(J (J (J
+ J Ry s + J Rz c ) − θ&P s P ( pY s P + rY c P ) ×
Pz Pz Ry
(
)
− J Px ) s P + p&Y s P + r&Y pY c P − θ&P rY s P × 2 R
2 R
+ J Ry s R2 + J Rz c R2 ) + 2θ&R s R c R c P ( pY s P + rY c P ) × − J Rz ) − qY c P ( pY c P − rY s P )( J Px + J Rx
(
(
q&Y + θ&&P
+ qY + θ&P
(
)( −θ& s P
) (J
Ry
)
− J Rz ) − JY x pY qY − θ&R J Rx c P qY
c s R − θ&R c P s R2 + θ&R c P c R2
P R
) (J
Ry
− J Rz
+ −qY s P ( J Pz + J Ry s R2 + J Rz c R2 ) + pY c R s R ( J Ry − J Rz
)
( ) (J
( pY s P + rY c P ) + pY
(25) is the effect of base movements to pitch channel. Remark 2: If in the mechanism design the relation between moments of inertial be how ( J Px + J Rx − J Pz − J Ry ) ≈ 0 , the gimbal interactions and effect of base movements on the pitch channel would be reduced.
)
( J Rx ( J Rx
+c P c R s R
+ qsY ) + c P r )
+ θ&Y c P (c P ( pcY + qsY ) − s P r ) ( J Px + J Rx − J Pz − J Ry
)
τY = JY x r&Y − J Rx θ&&R s R + JY y qY pY + θ&P c P ( pY c P − rY s P ) ×
where τ PI = − J R x θ&Y θ&R c P + 2θ& s P c P ( J Px + J R x − J Pz − J R y
T
Since the yaw gimbal can only rotate about its z-axis, the moment equation (27) about this axis is
(23)
2 Y
+ ( CRP ) J R ω RIR
(26) where the parameters are introduced in (2)-(10). In this equation, the angular momentum of the pitch and roll gimbals transformed to yaw frame. The moment equation for the yaw gimbal is
Rx
P
P IP
rP s P ( J Pz + J Ry s R2 + J Rz c R2 ) + q P s P c R s R ( J Ry − J Rz + rP c R s R ( J Ry − J Rz ) rP c P ( J Pz + J Ry s R2 + J Rz c R2 ) + q P c P c R s R ( J Ry − J Rz
Ry
R
Y
2 R
Ry
R
Y
P
Pz
R
P
Rz
R
Y
Y
Y
R
Rz
P
Y
R
P
JY x pY + θ&R J Rx c P + p P ( J Px c P + J Rx c P ) = JY y qY + q P ( J Py + J Ry c R2 + J Rz s R2 ) & JY z rY − θ R J Rx s P + p P ( −J Px s P + J Rx s P )
)) s c (J − J ) ( ) ( J − J ) ( 2θ& s c + (c ( pc + qs ) − s ( r + θ& )) s c ) −θ& ( s ( pc + qs ) + c ( r + θ& ) ) ( ( s − c )( J − J ) + J ) − (c ( pc + qs ) − s ( r + θ& ) ) ( s ( pc + qs ) + c ( r + θ& ) ) × (J + J − J − J s − J c ) (
− c P r& + θ&&Y − θ&P s P r + θ&Y
(J ω
−qY s c
2 P R
(q
Y
+ θ&P
qY + θ&P Ry
)(J ).
Py
+ J Ry c + J Rz s 2 R
2 R
)
)
)) ×
− J Rz
(28) With substituting (5) in (28) and derivation pY , qY , and rY , (28) can be written as & Y TT − θ&&P c P c R s R ( J Ry − J Rz JY TT θ&&Y = τY − rJ
(
& Y + qs & Y − θ&Y psY + θ&Y qcY − pc −c P c R s R
2.3. Yaw Channel The angular momentum of the yaw gimbal is
(
) (c
P
s P ( J Rx − J Px ) + s P J PTZ
& Y − ps & Y − θ&Y qsY − θ&Y pcY qc
− ( pcY + qsY
) ( (θ&P c P − θ&P s P2 ) ( J Rx
)
) (J
Ry
− J Rz
)
− J Px ) + θ&P s P2 J PTZ +
+2θ&R s R c R c P s P ( J Ry − J Rz ) + θ&P ( J Py + J Ry c R2 + J Rz s R2 )
40
)
)
KAZEMY AND SIAHI :EQUATIONS OF MOTION EXTRACTION FOR A THREE AXES GIMBAL SYSTEM
− (qcY − psY
(
) ( −θ&R J Rx c P +
)(J
+ −θ&P s P c R s R − θ&R c P s R2 + θ&R c P c R2 − θ&P s P c R2
− J Rz
Ry
))
−2θ&P c P s P ( J Rx − J Px ) − θ&P s P (1 + c P ) J PTZ − r + θ&Y +2θ&R s R c R c P2 ( J R y − J Rz ) JY y − c P2 ( J Px + J Rx ) + J PT − (qcY − psY )( pcY + qsY ) 2 −s P ( J Pz + J Ry s R2 + J Rz c R2 ) − JY x − ( r + θ& ) (qc − ps ) (c s ( J + J ) − s c J )
(
(
Ry
4. Control Strategy For stabilizing the sensor in the inertial space, it is necessary to measure the angular velocity of the sensor (frame R) with respect to inertial frame. Therefore, three rate gyroscopes are placed beside the sensor and measure the angular velocity of the sensor in the inertial space ( ω RIR ). In addition, gimbal rotation angles ( θ R , θ P , and θY ) are measured with resolvers. Remark 4: Due to mechanical restrictions, the yaw and pitch angles have the following limitations:
)
Y
Y
− ( pcY + qsY
(J
then, affect the other channels from the interaction between them.
Y
)(
P
P
Px
Rx
P
)
r + θ&Y c P c R s R − (qcY − psY
− J Rz ) − c R s R s P ( pcY + qsY
)
(
(J
2
P
)
2
PTZ
)
s P c R2 ×
− J Rz ) + J R x θ&&R s R
Ry
+J Rx θ&R θ&P c P − θ&P −θ&P s P c R s R − θ&R c P s R2 + θ&R c P c R2
) (J
− J Rz
Ry
)
(29)
θY
