Method determinate angle of rotation of an IMU application for UAV

Trinh Dinh Quan

Southern TaiWan University  

1. Introduction about camera auto balancing system of UAV

2.Proposal auto balancing platform

3.Step by step solving rotation (attitude) of an IMU

OOutline

• 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

OIntroduction

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 1 : Initialization (alignment): To determine the initial attitude including roll, pitch, and heading .

• Step 2 : Parameter transformation.

• Step3 : Update attitude .

OStep by step solving rotation (attitude) of an IMU

Figure 1: local level frame Figure 2: Attitude presentation

Step 1: Initialization (alignment): To determine the initial attitude including roll, pitch, and heading of the IMU at start time k

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 = arcsin(fx/g)=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:

 

 Where , , and 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:

• The Earth’ magnetic heading is then compensated for the declination angle to get the l-frame heading (geographical heading):

 

Step 2: Parameter transformation

32 23 11 22 331

13 31 11 22 332

3 21 12 11 22 33

411 22 33

0.25( ) / 0.5 1( )

0.25( ) / 0.5 1( )

( ) 0.25( ) / 0.5 1( ) 0.5 1

C C C C Cq k

C C C C Cq k

q k C C C C Cq k C C C

11 12 13

21 22 23

31 32 33

cos( )cos( ) sin( )sin( ) cos( ) cos( )sin( ) sin( ) sin( ) cos( )sin( ) cos( )

cos( )sin( ) cos( ) cos( ) sin( )sin( )sin( ) cos( ) sin( )sin( ) sin( ) cos( )

sin( ) sin

lb

C C C

C C C C

C C C

( ) cos( ) cos( ) cos( )

•Determine DCM matrix from Euler angles (attitude)

•For optimal in computation the attitude is usually expressed in quaternion form:

• 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:

  (2.11) 

Step 3: Update attitude

• The body increment with respect to the l-frame is determined as:

• Where:

• (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. )

• If only attitude of the IMU is considered, the

•Updated quaternion:

1 1 1

2 2 2

3 3 3

4 4 4

( 1) ( ) ( )

( 1) ( ) ( )0.5

( 1) ( ) ( )

( 1) ( ) ( )

z y x

z x z

y x z

b

x y z

c s s sq k q k q k

s c s sq k q k q k

s s c sq k q k q k

s s s cq k q k q k

2sin

2s

2(cos 1)

2c

Where:

•Calculate updated DCM matrix

2 2 2 21 2 3 4 1 3 2 41 2 3 4

2 2 2 21 2 3 4 2 1 3 4 2 3 1 4

2 2 2 22 3 1 41 3 2 4 3 1 2 4

( ) 2( )2( )

( 1) 2( ) ( ) 2( )

2( )2( ) ( )

lb

q q q q q q q qq q q q

C k q q q q q q q q q q q q

q q q qq q q q q q q q

Calculate updated attitude:

1 31

2

31

tan1

C

C

32 33tan 2( , )a C C

21 11tan 2( , )a C C