Simulation and Resolution

We define the path of the angular acceleration of the Euler’s angles and linear accelerations in the inertial frame as formula(3.2). We assume that sampling rate of DAQ card is 1000HZ, l=50mm and the standard deviation is 25mm/ s².

( )

Fig.3.5.1 shows the state of the algorithm we derived and Fig.3.5.2 shows the angular rate in the inertial frame.

0 1 2 3 4 5

w1 in rotated frame

0 1 2 3 4 5

w2 in rotated frame

0 1 2 3 4 5

w3 in rotated frame measured

Fig.3.5.1 angular rate representation in Body frame

0 1 2 3 4 5

w1 in Inertial frame

0 1 2 3 4 5

w2 in Inertial frame

0 1 2 3 4 5

w3 in Inertial frame measured

Fig.3.5.2 angular rate representation in the Inertial frame

Fig.3.5.3 shows the Euler’s angular rate and Fig.3.5.4 shows the Euler’s angle.

There is pulse at 1.5, 3 and 4 seconds, because the matrix in formula (2.37) is singular.

0 1 2 3 4 5 -8000

-6000 -4000 -2000 0 2000 4000

rad/s

w_fife in euler anlge

0 1 2 3 4 5

-3 -2 -1 0 1 2 3

rad/s

w_theta in euler angle

0 1 2 3 4 5

-8000 -6000 -4000 -2000 0 2000 4000

rad/s

w_phi in euler angle measured real

measured real

measured real

Fig.3.5.3 angular rate of Euler angles

0 1 2 3 4 5

-10 -5 0 5 10 15

rad

fife in euler anlge

0 1 2 3 4 5

0 2 4 6 8 10 12

rad

theta in euler angle

0 1 2 3 4 5

-10 -5 0 5 10 15

rad

phi in euler angle measured real

measured real

measured real

Fig.3.5.4 Euler angles

The iterative time is 40 for initial to 0.1 second and 20 for 0.1 second to 1 second.

Fig.3.5.5 shows the result of iteration.

0 10 20 30 40 50 -2.7375

-2.7374 -2.7373 -2.7372 -2.7371

w1

0 10 20 30 40 50

0.5322 0.5324 0.5326 0.5328 0.533 0.5332 0.5334

w2

0 10 20 30 40 50

1.0432 1.0433 1.0434 1.0435 1.0436 1.0437 1.0438

w3

Iterative convergence Iterative convergence

Iterative convergence

Fig.3.5.5 Iterative convergence of angular rate representation in body frame

Fig.3.5.6 shows the linear accelerations in the body frame and Fig.3.5.7 shows the linear accelerations in the inertial frame.

0 1 2 3 4 5

-3000 -2000 -1000 0 1000 2000 3000

mm/s2

Fr1

0 1 2 3 4 5

-4000 -2000 0 2000 4000

mm/s2

Fr2

0 1 2 3 4 5

-4000 -2000 0 2000 4000

mm/s2

Fr3

measured real

measured real

measured real

Fig.3.5.6 Linear acceleration representation in Body frame

1 1.2 1.4 1.6 1.8 2 -2000

-1000 0 1000 2000

mm/s2

F1 in Inertial frame

1 1.2 1.4 1.6 1.8 2

-2000 -1000 0 1000 2000

mm/s2

F2 in Inertial frame

1 1.2 1.4 1.6 1.8 2

-2000 -1000 0 1000 2000

mm/s2

F3 in Inertial frame measured real

measured real

measured real

Fig.3.5.7 Linear acceleration representation in Inertial frame

Resolution

0 50 100 150

-0.06 -0.04 -0.02 0 0.02 0.04

rad/s

w1 in rotated frame

measured real

0 50 100 150

-0.04 -0.02 0 0.02 0.04 0.06

rad/s

w2 in rotated frame

measured real

0 50 100 150

-0.02 0 0.02 0.04 0.06 0.08

rad/s

w3 in rotated frame

measured real

acceleration and linear acceleration is zero, and rewrite Eq.2.3 as Eq.3.3. We can find out the relationship between resolution of accelerometer and angular rate of our algorithm shown as Eq.3.4.

CHAPTER 4

TEST OF THE IMU DESIGN

To verify the feasibility and reliability of the method for computing angular rate, it is necessary to perform a two-step validation procedure, i.e., the method has to yield consistent and accurate results while acquiring data both from hypothetical and experimental systems.

To check if our method is feasible for any arbitrary motion, some experiments with different configurations should be done. According to the type of Euler＇s angle (ZYZ convention), we design three different experiments and simulations. First, Z axis in the body frame is parallel to the axis of rotation and the other two linear motions are parallel to the inertial frame. Secondly, initial condition is changed in order to verify the observer method is practical when acceleration of gravity effect on the output of accelerometer.

Thirdly, Y axis in body frame is parallel to the axis of rotation and the other two linear motions are parallel to the inertial frame.

Section 4.1 describes the procedures for these experiments with the designed motion, analog amplifier, and Butterworth filter. Section4.2, 4.3 and 4.4 describes the first, second and third experiment and simulation, respectively.

4.1 Experimental procedure

Basic procedure of experiment

Three different function generators output sinusoidal voltage to three different PWM servo amplifiers. These servo amplifiers output sinusoidal current to three different stages. Sinusoidal current excite sinusoidal acceleration on the stage and we put our IMU on the stage to sense the sinusoidal acceleration and read the encoder information by motion card (ADLINK PCI-8133). System noise or disturbance will be generated when three stages create the sinusoidal. And the acceleration output of the ADXL105 is nominally 250mV/g. This scale factor is not appreciated for our applications. An amplifier is need to set an appreciate scale ratio and a low-pass filter is needed to filter accelerometer’s internal or circuit high-frequency noise, so analog 1-pole low-pass filter will be employed to filter those between accelerometer and DAQ card (ADLINK PCI-9114). But other high-frequency noise will be excited when data of acceleration are transferred between the output of 1-pole low-pass filter and DAQ card. The digital filter (4^th order Butterworth filter) must be needed to filter this noise by computer. Let the result of digital filter be as acceleration output (A1~A9) shown as in algorithm, and physical quantities can be calculated by our observer base IMU. To compare these quantities and encoder’s information, and we will know our algorithm be practical or not.

We will discuss every physical quantities which interest us to be practical or not in the following sections. Fig.4.1.1 and Fig.4.1.2 show the process of experiment and the flow chart of algorithm respectively.

Fig.4.1.1 Experimental procedure

DAQ card (ADLINK PCI-9114) Motion card (ADLINK PCI-8133) Function generator

PWM servo amplifiers

Stage(ILS100CC & RV80CC)

Accelerometer

(ADXL105) output signal

Analog amplifier

Butterworth

A1~A9

A1

A2

A3

A4

A5

A6

A7

A8

ω ^v

b

[ ]

^{e b}

e

θ ω

θ

& = ^v

ω ^v

e

System

f

b

v ^{f v}

ⁱ

⁼ [ ] ^w

^e

^{f v}

^b

^{f v}

ⁱ

^{∫∫ p}

system

Route of motion

Because sinusoidal current excited sinusoidal acceleration on the stage, Eq.4.1 describes the relationship about displacement, velocity and acceleration. Fig.3.4.1 shows tendency of Eq.4.1.

( )

( )

( ) ( )

t

ft f f

ft f f

ft

π π π

π π π

π

2 2 1

2 sin nt 1

Displaceme

2 2 1

2 cos Velocity 1

2 sin on Accelarati

2 +

−

=

+

−

=

=

(4.1)

Low-pass analog amplifier (single pole)

Fig.4.1.3 Circuit of low-pass analog amplifier (single pole)

Fig.4.1.3 presents the circuit of low-pass filter and Fig.4.1.4 shows the Bode plot of low-pass filter. Eq.4.2 describes the gain is 5.1282 and Eq.4.3 shows the transfer function when the cutoff frequency is 117.0257 HZ, and.

+ -

R1 R2

C

IN OUT

VMID

F 10 8 . 6 C

K 39 R2 K 200 R1

R2 GAIN R1

2ππCR f 1

9 3

−

−

×

=

Ω

= Ω

=

−

=

dB =

(4.2)

1 00136 . 0

128 . T 5

= +

s (4.3)

-30 -20 -10 0 10 20

Magnitude (dB)

10¹ 10² 10³ 10⁴ 10⁵

-90 -45 0

Phase (deg)

analog low pass filter

Frequency (rad/sec)

Fig.4.1.4 Low-pass analog amplifier (single pole)

Butterworth filter

needed, an elliptic or Chebyshev filter can generally provide steeper roll-off characteristics with a lower filter order. Eq.4.4 shows the Z-transform and Fig.4.1.5 shows frequency response when data sampled at 2000 Hz and design a 4th-order low-pass Butterworth filter with cutoff frequency of 50 Hz.

( )

⁵ ₁^{4 1} ₂ ^{4 2} ₃ ^{4 3} _{1 4} ^{5 4}

Frequency (Hz)

Phase (degrees)

0 100 200 300 400 500 600 700 800 900 1000

Frequency (Hz)

Magnitude (dB)

Butterworth

Fig.4.1.5 Butterworth low-pass filter (fourth order)

There is invariably a time delay between a demodulated signal and the original received signal. The Butterworth filter parameters directly affect the length of this delay.

4.2 Z-axis rotation with biaxial linear acceleration

In this experiment, Z axis in body frame is parallel to the axis of rotation and the other two linear motions are parallel to X axis and Y axis in the inertial frame respectively.

Fig.4.2.1 shows this experiment setup. Eq.4.5 presents Euler＇s transform in these condition. Generally speaking, whether the quantity of Z-axis in the rotation frame or in the inertial frame must be equal to the Euler＇s angle

(

φ+ϕ

)

, and the quantities of X-axis, Y-axis and Euler＇s angleθ must be zero. But that is not exact correct in practical experiment. We shall now look more carefully into Eq.4.5, the quantity of Z-axis must be equal to the Euler＇s angle

(

φ+ϕ

)

when the disturbance or noise in the Euler＇s angleθ is close to zero, and the quantity of Z-axis is equal to ϕ when the disturbance or noise in the Euler＇s angleθ is larger then a value which is over than 0.06 radian after 0.2 seconds in this experiment. Compared Fig.4.2.7 with Fig.4.2.8, and we can clearly understand this question. Fig.4.2.2 shows accelerations which are through analog low-pass filter and Fig.4.2.3 shows accelerations which are through analog low-pass filter and then are through digital Butterworth low-pass filter. The curve in Fig.4.2.3 is smoother then in Fig.4.2.2 and in Fig.4.2.3; there is invariably a time delay which is introduced in the above section. In Fig.4.2.4 and Fig.4.2.6~4.2.13, the information of encoder shown by solid line, result of estimated shown by dotted line and subtracting time delay shown by dash-dot line. The rule holds in the flowing two sections.

Fig.4.2.1 Experimental set up

0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2 -1

-0.5 0 0.5 1

x 10⁴ nine accelerometer output with only_rotation motion

mm/s2

sec

NO.1 NO.2 NO.3 NO.4 NO.5 NO.6 NO.7 NO.8 NO.9

Fig.4.2.2 Nine accelerometers output

-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1

x 10⁴ nine accelerometer output with only_rotation motion after filtering

mm/s2

NO.1 NO.2 NO.3 NO.4 NO.5 NO.6 NO.7 NO.8 NO.9

Fig.4.2.4 shows the angular rate which is estimated from observer and obtained from differentiating the data of encoder in the rotation frame, Fig.4.2.10 shows that those are in the inertial frame and Fig.4.2.7 shows the Euler＇s angular rate. These quantities in X-axis and Y-axis should converge to zero, and in Z-axis should be the sinusoidal wave which is according Eq.4.1. But zero is smaller then resolution in our algorithm, these two curves will not converge to zero. We simulate the same condition as shown in Fig.4.2.5, and angular rate don’t converge to zero in X-axis and Y-axis. Because quantity in Z-axis in the rotation frame is equal to it in the inertial frame and

(

φ+ϕ

)

, angular displacement can be obtained by integrating from angular rate directly whether it is in the rotation or inertial frame as shown in Fig.4.2.6 and Fig.4.2.11 respectively. Euler＇s angular displacement is obtained from solving differential equation (Eq.2.37) directly and shown in Fig.4.2.9. The integral method will accumulate the error and make serious mistake when using Euler＇s transform as shown in Fig.4.2.13.

0 0.1 0.2 0.3

0 0.5 1 1.5

rad/s

w1 in rotated frame

0 0.1 0.2 0.3

-1 -0.5 0

rad/s

w2 in rotated frame

0 0.05 0.1 0.15 0.2 0.25 0.3

-1 -0.5 0 0.5 1 1.5

rad/s

w3 in rotated frame

rotation frame differentiate encoder subtract delay time

Fig.4.2.4 Angular rate in the rotation frame

0 0.05 0.1 0.15 0.2 0.25 -0.05

0 0.05 0.1 0.15

rad/s

w1 in rotated frame

0 0.05 0.1 0.15 0.2 0.25

0.1 0.2 0.3 0.4

rad/s

w2 in rotated frame

0 0.05 0.1 0.15 0.2 0.25

0 0.2 0.4 0.6 0.8 1

rad/s

w3 in rotated frame

measured real

Fig.4.2.5 Angular rate in the rotation frame (simulation)

0.05 0.1 0.15 0.2 0.25 0.3 0

0.05 0.1 0.15 0.2 0.25

rad

angle1 in rotation frame

0 0.1 0.2 0.3

-0.06 -0.04 -0.02 0

rad

angle2 in rotation frame

0 0.05 0.1 0.15 0.2 0.25 0.3

-0.04 -0.02 0 0.02

rad

angle3 in rotation frame

rotation frame encodeer subtract delay time

0.05 0.1 0.15 0.2 0.25 -20

-15 -10 -5 0

rad/s

w_fife in euler anlge

0.05 0.1 0.15 0.2 0.25 -0.5

0 0.5 1

rad/s

w_theta in euler angle

0 0.1 0.2 0.3

0 5 10 15 20

rad/s

w_phi in euler angle

eular frame differentiate encoder subtract delay time

Fig.4.2.7 Euler’s angular rate

0 0.1 0.2 0.3

-20 -15 -10 -5 0

rad/s

wfife in euler anlge

0.05 0.1 0.15 0.2 0.25 -0.5

0 0.5 1

rad/s

wtheta in euler angle

0.05 0.1 0.15 0.2 0.25 0.3 -1

-0.5 0 0.5 1 1.5

rad/s

wfife+w

theta in euler angle substrate offset eular frame rotation frame differentiate encoder

Fig.4.2.8 Euler’s angular rate (

(

ωφ +ωϕ

)

in the third sub-figure)

0.05 0.1 0.15 0.2 0.25 0.3 -1.5

-1 -0.5 0

fife in euler anlge

0 0.1 0.2 0.3

0 0.05 0.1 0.15 0.2 0.25

theta in euler angle

0 0.05 0.1 0.15 0.2 0.25 0.3 -0.06

-0.04 -0.02 0 0.02 0.04

fife+phi in euler angle

eular frame rotation frame differentiate encoder

Fig.4.2.9 Euler’s angular displacement (

(

φ+ϕ

)

in the third sub-figure)

0 0.1 0.2 0.3

0.5 1 1.5 2

rad/s

w1 in Inertial frame

0 0.05 0.1 0.15 0.2 0.25

-1 -0.5 0

rad/s

w2 in Inertial frame

-1 -0.5 0 0.5 1 1.5

rad/s

w3 in Inertial frame

inertial frame differentiate encoder subtract delay time

0 0.2 0.4 0.6 0.8 0

0.1 0.2 0.3 0.4 0.5

rad

angle1 in Inertial frame

0 0.2 0.4 0.6 0.8

-0.08 -0.06 -0.04 -0.02 0 0.02

rad

angle2 in Inertial frame

0 0.1 0.2 0.3 0.4

-0.1 -0.05 0 0.05

rad

angle3 in Inertial frame

inertial frame encodeer subtract delay time

Fig.4.2.11 Angular displacement in the inertial frame

We rewrite Eq.2.6 as Eq.4.6 to obtain linear acceleration in the rotation frame and use Euler＇s transform (Eq.2.36) to transfer it in the rotation frame into it in the inertial frame Fig.4.2.12 and Fig.4.2.13 indicate the result. With accumulation of the integration error of Euler＇s angle, acceleration diverges in the Z-axis in the inertial frame. Here, data of encoder is regard as Euler’s angle and put it into Eq.2.37 ; and we can get convergent acceleration in the inertial frame by using Euler’s angle. Fig.4.2.14 shows the convergent acceleration in the inertial frame.

0.05 0.1 0.15 0.2 0.25

subtract delay time

Fig.4.2.12 Linear acceleration in the rotation frame

[ ]

0 0.05 0.1 0.15 0.2 0.25 0.3 -2000

-1000 0 1000 2000

mm/s2

F1 in Inertial frame

0 0.1 0.2 0.3

-3000 -2000 -1000 0 1000 2000 3000

mm/s2

F2 in Inertial frame

0.05 0.1 0.15 0.2 0.25 0.3

-1000 -500 0 500 1000

mm/s2

F3 in Inertial frame

estimate real

subtract delay time

Fig.4.2.13 Linear acceleration in the inertial frame

0.05 0.1 0.15 0.2 0.25 -2000

-1000 0 1000 2000

F1 in Inertial frame

mm/s2

0.05 0.1 0.15 0.2 0.25 -3000

-2000 -1000 0 1000 2000 3000

F2 in Inertial frame

mm/s2

0 0.05 0.1 0.15 0.2 0.25 -40

-20 0 20

F3 in Inertial frame

mm/s2

estimate real

Fig.4.2.14 Linear acceleration in the inertial frame (with encoder information)

4.3 Z-axis rotation with biaxial linear acceleration and non-zero initial condition

Fig4.3.1 Experimental set up

The difference between this experiment and the above experiment is initial condition of angle of Y-axis and Z-axis as shown in Fig.4.3.1, angle of Y axis is 90 degree and Z axis is -90 degree as shown in Fig.4.3.1. Fig.4.3.1 presents the set up of this experiment.

Because initial condition in Y axis is not zero, equ.2.35 can not be rewritten as equ.4.5.

Quantities of Y axis and Z axis in the rotation frame are similar to -X axis and -Y axis in the inertial frame respectively when ϕ is still changed small enough. Quantities of X axis in the rotation frame are equal to Z axis in the inertial frame. The influence of

0 0.1 0.2 0.3 0.4 0

0.5 1 1.5 2

rad/s

w1 in rotated frame

0 0.1 0.2 0.3 0.4

-1.2 -1 -0.8 -0.6 -0.4 -0.2 0

rad/s

w2 in rotated frame

0 0.1 0.2 0.3 0.4

-1 0 1 2

rad/s

w3 in rotated frame

rotation frame differentiate encoder subtract delay time

Fig4.3.2 Angular rate in the rotation frame

0 0.1 0.2 0.3 0.4

0 0.2 0.4 0.6 0.8 1 1.2

wfife in euler anlge

rad/s

0 0.1 0.2 0.3

-2 -1.5 -1 -0.5 0

wtheta in euler angle

rad/s

0 0.1 0.2 0.3 0.4

-1 0 1 2

wphi in euler angle

rad/s

eular frame differentiate encoder subtract delay time

Fig4.3.3 Euler’s angular rate

Algorithm flow (Fig.4.2.2) indicated that the influence of integral error of Euler’s angle on linear acceleration in the inertial frame is the most serious. Because linear acceleration in the rotation frame is obtained by Eq.4.6 and it carry about a lot of white noise, it is inaccuracy when we transfer it into linear acceleration in the inertial frame with Euler’s angle. There is the same circumstance when we transfer the angular rate in the rotation frame into it in the inertial frame. The angular rate in the rotation frame is obtained by Iterated Extended Kalman filter without white noise, so it is more accuracy than linear acceleration. In this experiment, because the angular rate in the rotation frame converges after 0.3 second, the angular rate in the inertial frame is divergent shown as Fig4.3.4 and Fig.4.3.6. If accuracy Euler’s angle as encoder information is being substituted Euler’s angle calculated by Eq.2.37 shown as Fig.4.3.5 and Fig.4.3.7, the angular rate will not be divergence in X-axis in the inertial frame. Fig.4.3.4 and Fig.4.3.5 show the angular rate between 0 and 0.4 second, Fig.4.3.6 and Fig.4.3.7 show the angular rate between 0 and 4 second.

Linear acceleration in the inertial frame is also divergence as the above experiment.

Encoder information is being substituted Euler’s angle shown to obtained linear acceleration in the inertial frame as Fig.4.3.10. Fig.4.3.8 presents linear acceleration in the rotation frame and Fig.4.3.9 shows linear acceleration transferred by Euler’s angle which is calculated by Eq.2.37 in the rotation frame and it will be divergence.

0 0.1 0.2 0.3 0.4 -1

0 1 2

rad/s

w1 in Inertial frame

0 0.1 0.2 0.3 0.4

-2 -1.5 -1 -0.5 0 0.5

rad/s

w2 in Inertial frame

0 0.1 0.2 0.3 0.4

0 0.5 1

rad/s

w3 in Inertial frame

inertial frame differentiate encoder subtract delay time

Fig4.3.4 Angular rate in the inertial frame

0 0.1 0.2 0.3 0.4

-1 -0.5 0 0.5 1 1.5 2

rad/s

w1 in Inertial frame

0 0.1 0.2 0.3 0.4

0 0.5 1 1.5

rad/s

w2 in Inertial frame

0 0.1 0.2 0.3 0.4

-1 -0.5 0

rad/s

w3 in Inertial frame

estimate real

subtract delay time

Fig4.3.5 Angular rate in the inertial frame (with encoder information)

-2 0 2 4 6 -2

-1 0 1 2 3

rad/s

w1 in Inertial frame

inertial frame differentiate encoder subtract delay time

0 1 2 3 4 5

-2 -1.5 -1 -0.5 0 0.5

rad/s

w2 in Inertial frame

0 1 2 3 4 5

-0.5 0 0.5 1 1.5

rad/s

w3 in Inertial frame

Fig4.3.6 Angular rate in the inertial frame (with encoder information)

-2 0 2 4 6

-2 -1 0 1 2 3

rad/s

w1 in Inertial frame

-2 0 2 4 6

0 0.5 1 1.5 2

rad/s

w2 in Inertial frame

-2 -1.5 -1 -0.5 0

rad/s

w3 in Inertial frame estimate real

subtract delay time

inertial frame differentiate encoder

0 0.1 0.2 0.3 0.4 -4000

-2000 0 2000 4000

Fr1

mm/s2

0 0.1 0.2 0.3 0.4

-10200 -10100 -10000 -9900 -9800 -9700 -9600

Fr2

mm/s2

0 0.1 0.2 0.3 0.4

-3000 -2000 -1000 0 1000 2000 3000

Fr3

mm/s2

estimate real

subtract delay time

Fig4.3.8 Linear acceleration in the rotation frame

0 0.1 0.2 0.3 0.4

-6000 -4000 -2000 0 2000

F1 in Inertial frame

mm/s2

0.1 0.2 0.3 0.4

-4000 -2000 0 2000 4000

F2 in Inertial frame

mm/s2

0 0.1 0.2 0.3 0.4

8500 9000 9500 10000

F3 in Inertial frame

mm/s2

estimate real

subtract delay time

Fig4.3.9 Linear acceleration in the inertial frame

0 0.1 0.2 0.3 -3000

-2000 -1000 0 1000 2000 3000

F1 in Inertial frame

mm/s2

0 0.1 0.2 0.3 0.4

-4000 -2000 0 2000 4000

F2 in Inertial frame

mm/s2

0 0.1 0.2 0.3 0.4

9600 9700 9800 9900 10000 10100

F3 in Inertial frame

mm/s2

estimate real

subtract delay time

Fig4.3.10 Linear acceleration in the inertial frame (with encoder information)

4.4 Y-axis rotation with biaxial linear acceleration

The difference between this experiment and the above experiments is initial condition and the rotational axis. Initial condition of angle is zero whether it is in any axes and the axis of rotation is parallel to Y axis in the rotation frame shown in Fig.4.4.1. We rewrite Eq.2.36 as Eq.4.7 with these initial conditions. According to Eq.4.7, quantities in the Euler’s angle θ are the same as quantities of Y axis in the rotation or inertial frame.

Fig.4.4.2, Fig.4.4.6 and Fig.4.4.4 show the angular rate in the rotation frame, inertial frame and Euler’s angular rate respectively, there are only sine wave in the Y axis whether the quantities are shown in any Figure.

b

Fig4.4.1 Experimental set up

Because accumulation of integral error is more serious in this experiment, Euler’s angular rate which is calculated by Eq.2.37 is divergent. We transfer quantities (angular rate or linear acceleration) in the rotation frame into inertial frame and those are divergent more easily in the inertial frame. Accuracy Euler’s angle as encoder information is

the angular rate in the inertial frame, Euler’s angular rate and linear acceleration in the inertial frame will not be divergence in Y-axis.

0 0.1 0.2 0.3 0.4

-0.5 0 0.5 1 1.5

rad/s

w1 in rotated frame

0 0.1 0.2 0.3 0.4

-1 0 1 2

rad/s

w2 in rotated frame

0 0.1 0.2 0.3 0.4

-2 -1 0 1 2

rad/s

w3 in rotated frame

rotation frame differentiate emcodeer subtract delay time

Fig.4.4.2 Angular rate in the rotation frame

0 0.1 0.2 0.3 0.4 -60

-40 -20 0 20 40 60

w_fife in euler anlge

rad/s

0.1 0.2 0.3 0.4

-1 0 1 2

w_theta in euler angle

rad/s

0.1 0.2 0.3 0.4

-30 -20 -10 0 10 20 30

w_phi in euler angle

rad/s

eular frame differentiate encoder subtract delay time

Fig.4.4.3 Euler’s angular rate

0 0.1 0.2 0.3 0.4

-150 -100 -50 0 50 100 150

wfife in euler anlge

rad/s

0.1 0.2 0.3 0.4

-1.5 -1 -0.5 0 0.5 1 1.5

wtheta in euler angle

rad/s

0 0.1 0.2 0.3 0.4

0 20 40 60 80 100

wphi in euler angle

rad/s

inertial frame differentiate encoder subtract delay time

Fig.4.4.4 Euler’s angular rate (with encoder information)

0.1 0.2 0.3 0

0.5 1 1.5

rad/s

w1 in Inertial frame

0.1 0.2 0.3 0.4

-2 -1 0 1 2

rad/s

w2 in Inertial frame

0 0.1 0.2 0.3 0.4

-1 0 1 2

rad/s

w3 in Inertial frame

inertial frame differentiate encoder subtract delay time

Fig.4.4.5 Angular rate in the inertial frame

0 0.1 0.2 0.3 0.4

0 0.2 0.4 0.6 0.8 1 1.2

rad/s

w1 in Inertial frame

0 0.1 0.2 0.3 0.4

-1.5 -1 -0.5 0 0.5 1 1.5

rad/s

w2 in Inertial frame

-2 -1 0 1

rad/s

w3 in Inertial frame

inertial frame differentiate emcode subtract delay time

0.1 0.2 0.3 -6000

-4000 -2000 0 2000 4000

Fr1

mm/s2

0 0.1 0.2 0.3 0.4

-3000 -2000 -1000 0 1000 2000 3000

Fr2

mm/s2

0 0.1 0.2 0.3 0.4

9000 9500 10000 10500

Fr3

mm/s2

estimate real

subtract delay time

Fig.4.4.7 Linear acceleration in the rotation frame

0 0.1 0.2 0.3 0.4

-6000 -4000 -2000 0 2000 4000

F1 in Inertial frame

mm/s2

0 0.1 0.2 0.3 0.4

-2000 -1000 0 1000 2000 3000

F2 in Inertial frame

mm/s2

0 0.1 0.2 0.3 0.4

-10500 -10000 -9500 -9000

F3 in Inertial frame

mm/s2

estimate real

subtract delay time

Fig.4.4.8 Linear acceleration in the inertial frame (with encoder information)

4.5 Discussion

The uncertainties of experiment scheme may cause the difference between information from encoder and states of algorithm. The discussion of experimental error below will be shown to emphasize the importance of three features; linear vibration of the optical table, rotational vibration of the optical table and DC-drift of accelerometer output or other noise.

Linear vibrations of the optical table

The first question to be discussed is linear vibration of the optical table. Fig4.5.1 illustrates that the frequency of linear vibration is the same as the motion in the experiment.

Amplitude of this vibration is almost 200mm/s² in the X-axis and Y-axis, and 50mm/s² in the Z-axis as shown in Fig.4.5.2. According these conditions, we simulate how this error influences physical quantities in our algorithm.

0.44 0.46 0.48 0.5 0.52 0.54 0.56 0.58 0.6 -150

-100 -50 0 50 100 150

accelerometers output

mm/s2

x-axis y-axis z-axis

Fig.4.5.2 Vibration of tri-axes

Fig.4.5.3 shows that the vibration makes converging rate of the angular rate to be slow, but it does not change the accuracy when it converges. The amplitude will increase by 200mm/s² in the X-axis and Y-axis, as shown in the Fig.4.5.4. It follows from what has been said that the error of linear vibration affects only the accuracy in the linear acceleration and converging rate of the angular rate in the rotation frame.

0 1 2 3 -0.8

-0.6 -0.4 -0.2 0

rad/s

w1 in rotated frame

0 1 2 3

-0.2 0 0.2 0.4 0.6

rad/s

w2 in rotated frame

0 0.05 0.1 0.15 0.2 0.25

0 0.2 0.4 0.6 0.8 1

rad/s

w3 in rotated frame measured real

measured real

measured real

Fig.4.5.3 Angular rate in the rotation frame (with linear vibration)

0.05 0.1 0.15

-2000 -1000 0 1000 2000

mm/s2

Fr1

0 0.05 0.1

-2000 -1000 0 1000 2000

mm/s2

Fr2

-150 -100 -50 0 50 100 150

mm/s2

Fr3

measured real

measured real

measured real

Rotational vibration of the optical table

Because the angular rate can not be sensed by our experiments, we just discuss this question without experimental verification. We assumed that frequency of rotational vibration is equal to actual motion and the amplitude is about 0.1rad/s². Fig.4.5.5 and Fig.4.5.6 show that the result of this simulation is contrary to the above simulation.

Rotational vibration affects the accuracy of angular rate in the rotation frame and it doesn’t affect linear acceleration. The influence is the same to rotational vibration we

Rotational vibration affects the accuracy of angular rate in the rotation frame and it doesn’t affect linear acceleration. The influence is the same to rotational vibration we

在文檔中 觀測器基礎純加速規平面式慣性量測單元 (頁 41-0)