Navigation Mathematics: Kinematics (Coordinate Frame Transformation) EE 570: Location and Navigation

Transcription

1 Lecture Navigation Mathematics: Kinematics (Coordinate Frame Transformation) EE 570: Location and Navigation Lecture Notes Update on Feruary 16, 2016 Aly El-Osery and Kevin Wedeward, Electrical Engineering Dept., New Mexico Tech In collaoration with Stephen Bruder, Electrical & Computer Engineering, Emry-Riddle Aeronautical University.1 Coordinate Frame Transformation Determine the detailed kinematic relationships etween the 4 major frames of interest The Earth-Centered Inertial (ECI) coordinate frame (i-frame) The Earth-Centered Earth-Fixed (ECEF) coordinate frame (e-frame) The Local Navigation (Nav) coordinate frame (n-frame) The Body coordinate frame (-frame).2 ECI/ECEF Relationship etween the ECI and ECEF frames ECI & ECEF have co-located orgins r ie = r ie = r ie = 0 The x, y, and z axis of the ECI & ECEF frames are coincident at time t 0 The ECEF frame rotates aout the common z-axis at a fixed rate (ω ie ) Ignoring minor speed variatins (precession & nutation) ω ie = µrad/sec (WGS84) which is 15 /hr.3 ECI/ECEF The angular velocity and acceleration are The angle of rotation is ω i ie = 0 0 i ω ie = ω ie θ ie = ω ie (t t 0 ) = ω ie t + θ GMST where GMST is the Greenwich mean sidereal time

2 The orientation of frame {e} wrt frame {i} ecomes Ce i = R ( z,θie) = cos θ ie sin θ ie 0 sin θ ie cos θ ie Note: ω i ie = ω e ie.4 Description of the navigation frame Orientation of the n-frame wrt the e- frame z e z i Cn e = R ( z,λ )R ( y, L 90 ) cos λ sin λ 0 sin L 0 cos L = sin λ cos λ cos L 0 sin L sin L cos λ sin λ cos L cos λ = sin L sin λ cos λ cos L sin λ cos L 0 sin L where geodetic Lat = L and Geodetic Lon = λ Vernal Equinox x i x n NMT θ lat y n z n N ω e θlon Greenwichhh y e y i x e.5 Angular velocity of the n-frame wrt the e-frame resolved in the e-frame as a skewsymmetric matrix Ċn e = CnΩ e n en = Ω e encn e Ω e en = Ċe n [Cn] e T s L s λ λ c L c λ L c λ λ c λ s L L + c L s λ λ = c L s λ L c λ s L λ s λ λ s L s λ L c L c λ λ [Ce n] T s L L 0 c L L 0 λ L cos(λ ) = λ 0 L sin(λ ) L cos(λ ) L sin(λ ) 0.6 The angular velocity vector sin(λ) L ω e en = cos(λ) L λ cos(λ) λ ω n en = [Cn] e T ωen e = L sin(l ) λ.7 2

3 ECI/ Hence the orientation of the n-frame wrt the i-frame ecomes c θie s θie 0 s L c λ s λ c L c λ Cn i = CeC i n e = s θie c θie 0 s L s λ c λ c L s λ c L 0 s L sin(l ) cos(θ ie + λ ) sin(θ ie + λ ) cos(l ) cos(θ ie + λ ) = sin(l ) sin(θ ie + λ ) cos(θ ie + λ ) cos(l ) sin(θ ie + λ ) cos(l ) 0 sin(l ).8 ECI/ The angular velocity of the n-frame wrt the i-frame resolved in the i-frame is ω i in = ω i ie + Ce ω i e en 0 cos(θ ie ) sin(θ ie ) 0 sin(λ ) L = 0 + sin(θ ie ) cos(θ ie ) 0 cos(λ ) L ω ie λ sin(θ ie + λ ) L = cos(θ ie + λ ) L ω ie + λ.9 The vector from the origin of the e-frame to the n-frame origin resolved in the e-frame (from the last lecture) (R E + h ) cos(l ) cos(λ ) Origins of the n-frame r e e = (R E + h ) cos(l ) sin(λ ) = r e en and the -frame (R E (1 e 2 ) + h ) sin(l ) are the same The velocity of the n-frame wrt the e-frame resolved in the e-frame v e en = d dt r e en = r e en L + r e en λ + r e en ḣ L λ h sin(l ) cos(λ ) sin(λ ) cos(l ) cos(λ ) (R N + h ) L = sin(l ) sin(λ ) cos(λ ) cos(l ) sin(λ ) cos(l )(R E + h ) λ cos(l ) 0 sin(l ) ḣ.10 Recalling the form of Cn e suggests that (R N + h ) L v e en = Cn e cos(l )(R E + h ) λ = Ce n v n en ḣ 3

4 and hence, Restating v n en as and recalling that suggests that (R N + h ) L v n en = cos(l )(R E + h ) λ ḣ (R N + h ) L v n en = cos(l )(R E + h ) λ = ḣ v n en,n v n en,e v n en,d cos(l ) λ ω n en = L sin(l ) λ v n en,e /(R E + h ) ω n en = v n en,n /(R N + h ) tan(l ) v n en,e /(R E + h ) Description wrt the ody frame Orientation of the -frame wrt the n-frame in terms of relative yaw (ψ), pitch (θ), then roll (φ) angles c ψ s ψ 0 c θ 0 s θ C n = R ( z,ψ) R ( y,θ) R ( x,φ) = s ψ c ψ c φ s φ s θ 0 c θ 0 s φ c φ c θ c ψ c ψ s θ s φ c φ s ψ c φ c ψ s θ + s φ s ψ = c θ s ψ c φ c ψ + s θ s φ s ψ c φ s θ s ψ c ψ s φ y s θ c θ s φ c θ c pitch φ Center of Gravity roll x yaw z.13 4

5 The angular velocity of the -frame wrt the i-frame resolved/coordinatized in the i- frame ω i i = ω i in + C i n ω n n = ω i ie + C i e ω e en + C i n ω n n.14 Position vectors to the origin of the ody frame The origins of the ody and Nav frames are co-incident r n = 0 The origins of the ECI and ECEF frames are co-incident r e = r i = r en = r in Velocity of the -frame wrt the i-frame resolved in the i-frame A moving point in a rotation frame v i i = d dt r i i = d dt Ci e r e e = C i eω e ie r e e + C i e v e e = C i e (Ω e ie r e e + v e e).15 Acceleration of the -frame wrt the i-frame resolved in the i-frame A moving point in a rotation frame a i i = d dt v i i = d dt ( C i e (Ω e ie r e e + v e e) ) = Ċi e (Ω e ie r e e + v e e) + Ce i ω = 0 Ω e ie r e e + Ω e r ie e e + v e e = C i eω e ie (Ω e ie r e e + v e e) + C i e (Ω e ie v e e + a e e) = C i e (Ω e ieω e ie r e e + 2Ω e ie v e e + a e e).16 5