Transcript 下載/瀏覽

Method determinate angle of rotation
of an IMU application for UAV
Trinh Dinh Quan
Southern TaiWan University
Outline
O
1. Introduction about camera auto balancing
system of UAV
2. Proposal auto balancing platform
3. Step by step solving rotation (attitude) of
an IMU
O
Introduction
• UAV is without pilot, it has some problems when
taking the picture from camera because of vibration of
UVA when plying due to disturbance such as wind …
To overcome this problem; an auto-balancing
photogrammetric platform was designed in this
project.
• A proposed auto-balancing photogrammetric platform
includes an Inertial Measurement Unit (IMU) for
measuring three angles of Roll, Pitch, and Yaw.
• With this proposed platform, we can easily take a
photo from camera with small disturbance by auto
adjusting three angles of Roll, Pitch, and Yaw to get
desired direction
Proposal auto balancing platform
• A proposed platform includes a 4-link mechanism for motions
around three directions corresponding to three angles of Roll,
Pitch, and Roll which are measured by an Inertial Measurement
Unit (IMU).
• Three servo motors are used as actuator for rotating around
three directions. This proposed platform will be mounted at
UAV.
• Direction of platform can be set and controlled by a
microcontroller via PID algorithm. Schematic of a proposed
platform was shown in Figure .
Step by step solving rotation (attitude) of an IMU
O
• Step 1 : Initialization (alignment): To determine the
initial attitude including roll, pitch, and heading .
• Step 2 : Parameter transformation.
• Step3 : Update attitude .
Figure 1: local level frame
Figure 2: Attitude presentation
Step 1: Initialization (alignment): To determine the initial attitude
𝜙0
including roll, pitch, and heading 𝜃0 of the IMU at start time k
𝜓0
Determining roll and pitch (Leveling):
Assumption: For error-free accelerometers, the measurements fx and fy
represent the tilt in the x and y directions of the vertical accelerometer,
i.e.
fx = g sin (𝜙 ) ;
fy = g sin (𝜃 )
where 𝜙 and 𝜃 are the tilts in the x and y directions respectively
(usually called roll (𝜙 ) and pitch (𝜃 )). Therefore
𝜙0 = arcsin(fx/g)
𝜃0 =arcsin(fy/g)
Determining heading (Gyro compassing)
The heading is commonly determined based on a magnetometer.
The Earth’ magnetic field He can be projected into the x, y, z exes in
the body frame of the magnetometer. To get the magnetic heading,
first, the magnetic field along x axis Hx and y axis Hy are
compensated for the un-leveling of the system by the below equation:
Hy = Mx cos θ + Mz sin(θ)
Hx = Mx sin ϕ + My sin(ϕ)cos θ + Mz sin ϕ cos(θ)
Where Mx , My , and Mz are the output of the magnetometer, θ and ϕ
are roll and pitch of the system respectively.
Determining heading (Gyro compassing)
• The magnetic heading is then determined by the below
equation:
𝐻𝑦
𝐻𝑒 = arctan( )
𝐻𝑥
• The Earth’ magnetic heading is then compensated for the
declination angle to get the l-frame heading (geographical
heading):
𝜓0 = 𝐻𝑒 + 𝐴𝑑𝑒𝑐
Step 2: Parameter transformation
•Determine DCM matrix from Euler angles (attitude)
 C11 C12
Cbl  C21 C22
C31 C32
C13  cos( ) cos( ) sin( )sin( ) cos( )  cos( )sin( ) sin( ) sin( )  cos( )sin( ) cos( ) 
C23    cos( )sin( ) cos( ) cos( )  sin( )sin( )sin( ) cos( ) sin( )sin( )  sin( ) cos( ) 

C33    sin( )
sin( ) cos( )
cos( ) cos( )
•For optimal in computation the attitude is usually expressed in quaternion form:


 q1 ( k )   0.25(C32  C23 ) / 0.5 1  C11  C22  C33 
 q ( k )   0.25(C  C ) / 0.5 1  C  C  C 
13
31
11
22
33
 2


 q3 ( k )   0.25(C  C ) / 0.5 1  C  C  C 
21
12
11
22
33

 

 q4 ( k ) 
 0.5 1  C11  C22  C33

𝑏
• Convert output of the IMU into Δ𝜃𝑖𝑏
and Δ𝑣 𝑏
𝑏
• Commonly, the output of a MEMS IMU are angular rate vector 𝜔𝑖𝑏
(the
angular changes of the body frame with respect to the inertial frame
expressed in the body frame) and specific force vector 𝑓 𝑏 . The
𝑏
corresponding incremental angles Δ𝜃𝑖𝑏
and incremental velocities Δ𝑣 𝑏
can be calculated as:
𝑏
Δ𝜃𝑖𝑏
=
𝑡+1 𝑏
𝜔𝑖𝑏
𝑡
Δ𝑣 𝑏 =
𝑡+1 𝑏
𝑓
𝑡
𝑏
𝑑𝑡 = 𝜔𝑖𝑏
Δt
𝑑𝑡 = 𝑓 𝑏 Δt
(2.11)
Step 3: Update attitude
• The body increment with respect to the l-frame is
determined as:
𝑏
Δ𝜃𝑙𝑏
= [Δ𝜃𝑥
Δ𝜃𝑦
𝑏
𝑙
𝑙
Δ𝜃𝑧 ]𝑇 = Δ𝜃𝑖𝑏
− 𝐶𝑙𝑏 (𝜔𝑖𝑒
+ 𝜔𝑒𝑙
)Δt
• Where:
cos(𝜑)
0
𝑙
0
𝜔𝑖𝑒
= 𝐶𝑒𝑙 0 = 𝜔𝑖𝑒
𝜔𝑖𝑒
−sin(𝜑)
𝑟𝑎𝑑
• 𝜔𝑖𝑒 = 7.292115147𝑒 − 5 ( ) (The rotational speed of
𝑠
the earth)
• 𝜑 is latitude of the current point, if only attitude of the IMU
is considered, 𝜑 is given an arbitrary value (eg. 𝜑 = 230 )
𝑣𝑦 /(𝑁 + ℎ)
𝑙
−𝑣𝑥 /(𝑀 + ℎ)
𝜔𝑒𝑙
=
−𝑣𝑦 tan(𝜑)/(𝑁 + ℎ)
• If only attitude of the IMU is considered, the 𝑣𝑥 = 𝑣𝑦 = 0
•Updated quaternion:
 c
 q1 (k  1)   q1 (k ) 
  s
 q (k  1)   q (k ) 
z
2
2


  0.5 
 s y
 q3 (k  1)   q3 (k ) 


 

  s x
 q4 (k  1)   q4 (k ) 
Where:
s
s z
 s y
c
s x
 s x
 s y
c
 s zb
2

sin

2
s x   q1 (k ) 
s z   q2 (k ) 


s z   q3 (k ) 


c   q4 (k ) 
c  2(cos

2
 1)
•Calculate updated DCM matrix
(q12  q22  q32  q42 )

2(q1q3  q2 q4 )
2(q1q2  q3q4 )


Cbl (k  1)   2(q1q2  q3q4 )
(q22  q12  q32  q42 ) 2(q2 q3  q1q4 )

2
2
2
2 
 2(q1q3  q2 q4 )
2(
q
q

q
q
)
(
q

q

q

q
2
3
1
4
3
1
2
4 )

Calculate updated attitude:

   tan 1 


C31
1 C
2
31




  a tan 2(C32 , C33 )
  a tan 2(C21 , C11 )