E80. Experimental Engineering. Lecture 9 Inertial Measurement

Transcription

1 Lecture 9 Inertial Measurement Feb. 19, 2013 Christopher M. Clark

2 Where is the rocket?

3 Outline Sensors People Accelerometers Gyroscopes Representations State Prediction Example Systems Bounding with KF

4 Sensors People

5 Sensors Accelerometers a = k x / m

6 Sensors Accelerometers The accelerometers are typically MEMS based They are small cantilever beams (~100 µm) Khir MH, Qu P, Qu H - Sensors (Basel) (2011)

7 Sensors Gyroscopes (original) Mounted on two nested gimbals, the spinning wheel of the gyroscope is free to take any orientation. High spin rate leads to high angular momentum

8 E80 Sensors

9 Sensors Gyroscopes (original) The gyroscope resists any change of orientation caused by external torques, due to the principle of conservation of angular momentum. The orientation of the gyroscope remains nearly fixed, regardless of any motion of the platform.

10 Sensors Gyroscopes (original) Gyroscope orientation can be obtained by measuring the angles between adjacent gimbals.

11 E80 Sensors MEMS (Microelectromechanical Systems) Gyroscopes q q Two masses oscillate back and forth from the center of rotation with velocity v. A rotation will cause a Coriolis force in this coordinate frame. 11

12 Sensors MEMS Gyroscopes Their deflection y is measured, to establish a force F Coriolis = ky The acceleration is obtained since the mass is known -2m Ω x v = F Coriolis 12

13 Sensors Inertial Measurement Units 3 Accelerometers 3 Gyroscopes 3 Magnetometers(?) 13

14 Sensors Inertial Measurement Units Y 14

15 Outline Sensors Representations Cartesian Coordinate Frames Transformations State Prediction Example Systems Bounding with KF

16 Representations Cartesian Coordinate Frames We can represent the 3D position of a vehicle with the vector r =[ x y z ]

17 Representations Euler Angles We can represent the 3D orientation of a vehicle with the vector ϕ = [ α β γ ] Roll - α Pitch - β Yaw - γ

18 Representations Where do we place the origin? We can fix the origin at a specific location on earth, e.g. a rocket s launch pad. This is called the global or inertial coordinate frame Z r =[x y z] Y X

19 Representations Where do we place the origin? Z Z Y X We can ALSO fix the origin on a vehicle. This is called the local coordinate frame r =[x y z] Y X

20 Representations X L Where do we place the origin? We must differentiate between these two frames. What is the real difference between these two frames? A Transformation consisting of a rotation and translation Z L Z G Y L r =[x y z] Y G X G

21 Representations Transformations The rotation can be about 3 axes (i.e. the roll, pitch, yaw) Z G Z L γ Y L β Y G X L X G α

22 Representations Transformations The rotation can be about 3 axes (i.e. the roll, pitch, yaw) Z L Z G Y L X L Y G X G

23 Representations Transformations The translation can be in three directions Z L Z G Y L X L r =[x y z] Y G X G

24 Representations Rotations In 2D, it is easy to determine the effects of rotation on a vector q 2 Y q 1 α X q 2 = cos(α) -sin(α) q 1 sin(α) cos(α) = R(α) q 1

25 Representations Rotations In 3D, we can use similar rotation matrices Z G q 2 = q 1 0 cos(α) -sin(α) 0 sin(α) cos(α) q 2 α q 1 Y G = R x (α) q 1 X G

26 Representations R x (α) = cos(α) -sin(α) 0 sin(α) cos(α) R y (β) = cos(β) 0 sin(β) sin(β) 0 cos(β) R z (γ) = cos(γ) -sin(γ) 0 sin(γ) cos(γ)

27 Representations Rotations For 3 rotations, we can write a general Rotation Matrix R(α, β, γ) = R x (α) R x (β) R x (γ) Hence, we can rotate any vector with the general Rotation Matrix q 2 = R(α, β, γ) q 1

28 Outline Sensors Representations State Prediction Updating R(t) Updating r(t) Example Systems Bounding Errors with KF

29 State Prediction X L Strapdown Inertial Navigation Our IMU is fixed to the local frame Z L Z G Y L IMU We care about the state of the vehicle in the global frame r =[x y z] Y G X G

30 Updating R(t) Given: ω L (t) = [ ω x,l (t) ω y,l (t) ω z,l (t) ] Find: R(t)

31 Updating R(t) Lets define the rotational velocity matrix based on our gyroscope measurements ω L = [ω x,l (t) ω y,l (t) ω z,l (t)] Ω(t) = 0 -ω z (t) ω y (t) ω z (t) 0 -ω x (t) -ω y (t) ω x (t) 0

32 Updating R(t) It can be shown that the vehicle rotating with velocity Ω(t) for δt seconds will (approximately) yield the resulting rotation Matrix R(t+δt): R(t+δt) = R(t) [ I + Ω(t)δt ]

33 Updating r(t) Given: a L = [ a x,l a y,l a z,l ] R(t) Find: r G = [ a x,l a y,l a z,l ]

34 Updating r(t)

35 Updating r(t) First, convert to global reference frame a G (t) = R(t) a L (t) Second, remove gravity term a G (t) = [ a x,g (t) a y,g (t) a z,g (t)-g ]

36 Updating r(t) Third, integrate to obtain velocity v G (t) = v G (0) + t a G (τ) dτ Fourth, integrate to obtain position 0 r G (t) = r G (0) + t v G (τ) dτ 0

37 Updating r(t) Third, integrate to obtain (approximate) velocity v G (t+δt) = v G (t) + a G (t+δt) δt Fourth, integrate to obtain (approximate) position r G (t+δt) = r G (t) + v G (t+δt) δt

38 Outline Sensors Representations State Prediction Example Systems LN-3 The Jaguar Bounding Errors with KF

39 E80 Example Systems LN-3 Inertial Navigation System q q q q Developed in 1960 s Used gyros to help steady the platform Accelerometers on the platform were used to obtain accelerations in global coordinate frame Accelerations (double) integrated to obtain position 39

40 Example Systems

41 Example Systems The Jaguar Lite Equipped with an IMU, camera, laser scanner, encoders, GPS

42 Example Systems The Jaguar Lite GUI provides acceleration measurements

43 Example Systems Question: Can we use the accelerometers alone to measure orientation? α

44 Outline Sensors Representations State Prediction Example Systems Bounding Errors with KF Exteroceptive Sensing Fusing measurements

45 Bounding our Errors Exteroceptive sensors Drift in inertial navigation is a problem We often use exteroceptive sensors which measure outside the robot to bound our errors Examples include vision systems, GPS, range finders

46 Bounding our Errors Exteroceptive sensors We can fuse measurements, e.g. integrated accelerometer measurements and range measurements, by averaging. For example, consider the 1D position estimate of the jaguar. x = 0.5 ( x IMU + (x wall - x laser ) ) x IMU is the double integrated IMU measurement x wall is the distance from the origin to the wall x laser is the central range measurement

47 Bounding our Errors Exteroceptive sensors Lets weight the average, where weights reflect measurement confidence x = w IMU x IMU + w laser (x wall - x laser ) w IMU + w laser

48 Bounding our Errors Exteroceptive sensors This leads us to a 1D Kalman Filter x t = x IMU,t + K t [(x wall x laser,t ) - x IMU,t ] K t = σ 2 IMU σ 2 IMU + σ2 laser σ 2 x = (1 K t ) σ2 IMU

49 Bounding our Errors IMU GPS Higher sampling rate Small errors between time steps (maybe centimeters) Uncertainty increases Large build up over time (unbounded) Lower sampling rate Larger errors(maybe meters) Uncertainty decreases No build up (bounded)

50 E80 Bounding our Errors