EE 570: Location and Navigation: Theory & Practice

Transcription

1 EE 570: Locaton and Navgaton: Theory & Practce Navgaton Sensors and INS Mechanzaton Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 1 of 14

2 Navgaton Sensors and INS Mechanzaton Navgaton Equatons The Fundamental Prolem The Fundamental Inertal Navgaton Prolem: Usng nertal sensors (accels & gyros) and an ntal poston and orentaton, determne the vehcle s (.e. ody frame) current poston, velocty, and atttude (PVA) Assumptons: 1. Know where we started (ntal PVA: r??, v??, & C? ) 2. Inertal sensors (ω and f ) are error free (relax later) 3. Have a gravty (g? ) and/or gravtatonal (γ? ) model Where am I? Current PVA? o Wth respect to whch frame? Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 2 of 14

3 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Inertal Navgaton Inertal Navgaton The process of ntegratng angular velocty & acceleraton to determne one s poston, velocty, and atttude (PVA) o Effectvely dead reckonng To measure the acceleraton and angular velocty vectors we need at least 3-gyros and 3-accels o Typcally confgured n an orthogonal trad The mechanzaton can e performed wrt: o The ECI frame, o The ECEF frame, or o The Nav frame. Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 3 of 14

4 Navgaton Sensors and INS Mechanzaton Navgaton Equatons ISA, IMU, & INS An Inertal Navgaton System (INS) ISA Inertal Sensor Assemly o Typcally, 3-gyros + 3-accels + asc electroncs (power, ) IMU Inertal Measurement Unt o ISA + Compensaton algorthms (.e. asc processng) INS Inertal Navgaton System o IMU + gravty model + mechanzaton algorthms INS Intalzaton ISA IMU Raw sensor outputs Comp Algs - Basc Processng f Mechanzaton Equatons Poston, Velocty, and Atttude (PVA) Gravty Model Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 4 of 14

5 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Mechanzaton Process Pror PVA IMU Measurements f Pror Atttude Pror Velocty Mechanzaton Equatons Gravty / Gravtatonal Model Pror Poston Updated Atttude Updated Velocty Updated PVA Updated Poston Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 5 of 14

6 Navgaton Sensors and INS Mechanzaton Navgaton Equatons A Four Step Mechanzaton Can e genercally performed n four steps: 1. Atttude Update o Update the pror atttude (rotaton matrx) usng the current angular velocty measurement (C1 0 0 = C 1 Ω 1 01 = Ω C 1 ) 2. Transform the specfc force measurement (f? = C? f ) o Typcally, usng the atttude computed n step Update the velocty o Essentally ntegrate the result from step 2. wth the use of a gravty/gravtaton model (f = a γ ) 4. Update the Poston o Essentally ntegrate the result from step 3. Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 6 of 14

7 Navgaton Sensors and INS Mechanzaton Navgaton Equatons A Four Step Mechanzaton Pror PVA IMU Measurements f Pror Atttude 1. Atttude Update 2. SF Transform Pror Velocty 3. Velocty Update Grav Model Pror Poston 3. Poston Update Updated Atttude Updated Velocty Updated PVA Updated Poston Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 7 of 14

8 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton CASE 1: ECI Frame Mechanzaton Determne the Poston, Velocty, and Atttude of the Body frame wth respect to the Inertal Frame Determne our PVA wrt the ECI frame Poston: Vector from the orgn of the nertal frame to the orgn of the ody frame resolved n the nertal frame: r Velocty: Velocty of the ody frame wrt the nertal frame resolved n the nertal frame: v Atttude: Orentaton of the ody frame wrt the nertal frame C Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 8 of 14

9 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton 1. Atttude Update: Method A Body orentaton frame at tme k wrt tme k-1 C C C o t = Tme k Tme k-1 lm C ( k) C ( k 1) C ( k1) t 0 t C ( ) C ( ) C ( ) t C C I t ( ) ( ) z k ( 1) y k ( 1) Body Frame at tme k-1 k ( ) z x k ( 1) k ( ) y k ( ) x Body Frame at tme k Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 9 of 14

10 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton 1. Atttude Update: Method B Body orentaton frame at tme k wrt tme k-1 k ( 1) k ( ) o t = Tme k Tme k-1 C C C ( k 1) ( k ) ( k1) ( k ) C e C ( ) C ( ) e t e K K K I K 2! 3! C I t t ( ) ˆ t k 2 K I sn( ) K 1 cos( ) R e K e z ˆ ( k, ) k ( 1) y k ( 1) Body Frame at tme k-1 k ( ) z x k ( 1) 0 kˆ 3 kˆ 2 kˆ ˆ 3 0 k1 kˆ ˆ 2 k1 0 k ( ) y k ( ) x Body Frame at tme k Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 10 of 14

11 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton 1. Atttude Update: Hgh Fdelty 2 K C( ) C( ) I sn( ) K 1 cos( ) ˆ t k ˆ K Sk k Lower Fdelty C C I t ( ) ( ) Sk Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 11 of 14

12 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton 2. Specfc Force Transformaton Smply coordnatze the specfc force 3. Velocty Update Assumng that we are n space (.e. no centrfugal component) f f a a Thus, y smple numercal ntegraton 4. Poston Update f C ( ) f By smple numercal ntegraton v ( ) v ( ) a t 2 t ( ) ( ) ( ) a r r v t Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 12 of 14 2

13 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton f C C I t ( ) ( ) C ( ) 1. Atttude Update 2. SF Transform f C ( ) f f a f v ( ) 3. Velocty Update Grav Model v ( ) v ( ) a t 2 t ( ) ( ) ( ) a r r v t 2 r ( ) 3. Poston Update C ( ) v ( ) r ( ) Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 13 of 14

14 Navgaton Sensors and INS Mechanzaton Navgaton Equatons Case 1: ECI Mechanzaton In contnuous tme notaton: Atttude: C = C Ω Velocty: v Poston: r = C f + γ = v a Comnng nto a state-space equaton: f f C f v r v v C f C C Tuesday 26 Fe 2013 NMT EE 570: Locaton and Navgaton: Theory & Practce Slde 14 of 14