Chapter 2 - Kinematics

Transcription

1 Chapter 2 - Kinematics 2.1 Reference frames 2.2 Transformations between BODY and NED 2.3 Transformations between ECEF and NED 2.4 Transformations between BODY and FLOW BODY The study of dynamics can be divided into two parts: kinematics, which treats only geometrical aspects of motion, and kinetics, which is the analysis of the forces causing the motion 1

2 Overall Goal of Chapters 2 to 8 Represent the 6-DOF dynamics in a compact matrix-vector form according to: J M C D g g 0 wind wave The notation and representation are adopted from: Fossen, T. I. (1991). Nonlinear Modeling and Control of Underwater Vehicles, PhD thesis, Department of Engineering Cybernetics, NTNU, June Fossen, T. I. (1994). Guidance and Control of Ocean Vehicles, John Wiley and Sons Ltd. ISBN:

3 2.1 Reference Frames z e zi, z e N e NED D E BODY e t ECEF/ECI l ECEF/ECI y e ye xi yi x e xe ECI {i}: Earth centered inertial frame; non-accelerating frame (fixed in space) in which Newton s laws of motion apply. ECEF {e}: Earth-Centered Earth-Fixed frame; origin is fixed in the center of the Earth but the axes rotate relative to the inertial frame ECI. NED {n}: North-East-Down frame; defined relative to the Earth s reference ellipsoid (WGS 84). BODY {b}: Body frame; moving coordinate frame fixed to the vessel. x b - longitudinal axis (directed from aft to fore) y b - transversal axis (directed to starboard) z b -normal axis (directed from top to bottom) 3

4 2.1 Reference Frames Body-Fixed Reference Points CG - Center of gravity CB - Center of buoyancy CF - Center of flotation CF is located a distance LCF from CO in the x-direction The center of flotation is the centroid of the water plane area A wp in calm water. The vessel will roll and pitch about this point. Coordinate-free vector u u 1 n n 1 u 2 n n 2 u 3 n n 3 n i i 1,2,3 are the unit vectors that define n Coordinate form of u in n u n u 1 n,u 2 n,u 3 n 4

5 2.1 Reference frames and 6-DOF motions forces and linear and positions and DOF moments angular velocities Euler angles 1 motions in the x-direction (surge) X u x 2 motions in the y-direction (sway) Y v y 3 motions in the z-direction (heave) Z w z 4 rotation about the x-axis (roll, heel) K p 5 rotation about the y-axis (pitch, trim) M q 6 rotation about the z-axis (yaw) N r The notation is adopted from: y b v ( sway) q ( pitch) SNAME (1950). Nomenclature for Treating the Motion of a Submerged Body Through a Fluid. The Society of Naval Architects and Marine Engineers, Technical and Research Bulletin No. 1-5, April 1950, pp p u ( surge) x b ( roll) r ( yaw) w ( heave) 5 z b

6 2.1 Reference Frames - Notation ECEF position: pe b/e x y z 3 Longitude and latitude en l S 2 NED position: pn b/n N E D 3 Attitude (Euler angles) nb S 3 Body-fixed u Body-fixed p linear b v b/n v 3 angular b b/n q 3 velocity w velocity r Body-fixed force: f b b X Y Z 3 Body-fixed moment m b b K M N 3 Generalized position, velocity and force pn b/n or pe b/n ), nb b v b/n b b/n, f b b m b b 6

7 2.2 Transformations between BODY and NED Orthogonal matrices of order 3: O3 R R 33, RR R R I Special orthogonal group of order 3: SO3 R R 33, R is orthogonal and detr 1 Rotation matrix: Example: RR R R I, detr 1 Since R is orthogonal, R 1 R to to R from from 7

8 2.2 Transformations between BODY and NED Cross-product operator as matrix-vector multiplication: a : Sa S S 3 0 1, where S S is a skew-symmetric matrix 8

9 2.2 Transformations between BODY and NED Eulers theorem on rotation: R, I 33 sin S 1 cos S 2 1, 2, 3, 1 n v b/n R n b v b b/n, R n b : R, where R 11 1 cos 2 1 cos R 22 1 cos 2 2 cos R 33 1 cos 2 3 cos R 12 1 cos sin R 21 1 cos sin R 23 1 cos sin R 32 1 cos sin R 31 1 cos sin R 13 1 cos sin 9

10 2.2.1 Euler Angle Transformation Three principal rotations: 0, 0, 1 0, 1, 0 1, 0, 0 x 3 x 2 u 2 u 3 U v 3 y 3 (1) Rotation over yaw angle about z3. Note that w =w. 3 2 v 2 y 2 R z, c s 0 s c (2) Rotation over pitch angle about y2. Note that v =v. 2 1 x 1 x 2 u 2 U u 1 w 2 c 0 s w 1 R y, s 0 c z 1 z 2 10 R x, c s 0 s c w=w 0 z 0=zb z 1 w 1 v 1 v=v 2 U y 1 y =y 0 b (3) Rotation over roll angle about x1. Note that u 1=u2.

11 2.2.1 Euler Angle Transformation Linear velocity transformation (zyx-convention): n p b/n R n b b nb v b/n where R b n nb : R z, R y, R x, R n b nb 1 R b n nb R x, R y, R z, R b n nb cc sc css ss ccs sc cc sss cs ssc s cs cc Small angle approximation: R b n nb I 33 S nb

12 2.2.1 Euler Angle Transformation NED positions (continuous time and discrete time): n p b/n R n b b nb v b/n p n b/n k 1 p n b/n k hr n b nb kv b b/n k Euler integration Component form: N u coscos vcossinsin sincos wsinsin coscossin Ė u sincos vcoscos sinsinsin wsinsincos cossin D u sin vcossin wcoscos 12

13 2.2.1 Euler Angle Transformation Angular velocity transformation (zyx-convention): b nb T nb b/n b b/n 0 R x, 0 R x, R y, 0 0 : T 1 nb nb 0 0 where 1 0 s 1 st ct T 1 nb 0 c cs T nb 0 c s 0 s cc 0 s/c c/c Small angle approximation: Notice that: T nb Singular point at 90 o T 1 nb T nb 13

14 2.2.1 Euler Angle Transformation Euler angle attitude representations: ODE for Euler angles: ODE for rotation matrix b nb T nb b/n R n b R n b S b b/n Component form: p q sin tan rcos tan q cos rsin q sin cos r cos cos, 90o where S b b/n 0 r q r 0 p q p 0 + algorithm for computation of Euler angles from the rotation matrix nb,, R b n nb 14

15 2.2.1 Euler Angle Transformation Summary: 6-DOF kinematic equations: J 3-parameter representation n p b/n nb R n b nb T nb b v b/n b b/n nb,, with singularity at 90 o Component form: N u coscos vcossinsin sincos wsinsin coscossin Ė u sincos vcoscos sinsinsin wsinsincos cossin p q sintan rcostan q cos rsin q sin cos r cos cos, 90o D u sin vcossin wcoscos 15

16 2.2.2 Unit Quaternions 4-parameter representation: -avoids the representation singularity of the Euler angles -numerical effective (no trigonometric functions) Q q q q 1, q,, 3 and 1, 2, 3 Unit quaternion (Euler parameter) rotation matrix (Chou 1992): R, I 33 sin S 1 cos S 2 cos 2 q 1 2 cos 2 sin 2 Q 1, 2, 3 sin 2 3 R b n q : R, I 33 2S 2S 2 16

17 2.2.2 Unit Quaternions Linear velocity transformation n p b/n R n b b qv b/n NB! must be integrated under the constraint q q 1 or where R b n q R b n q 1 R b n q Component form (NED positions): N u v w Ė 2u v w D 2u v w

18 2.2.2 Unit Quaternions Angular velocity transformation Alternative representation (Kane 1983) b q T q q b/n q 1 2 I 33 S b b/n where T q q , T q qt q q 1 4 I 33 NB! nonsingular to the price of one more parameter Component form: 1 2 1p 2 q 3 r p 3q 2 r p q 1 r p 1 q r The equations are derived using R n b R n b S b b/n

19 2.2.2 Unit Quaternions Summary: 6-DOF kinematic equations (7 ODEs): J 4-parameter representation n p b/n q R n b q T q q b v b/n b b/n q, 1, 2, 3 Nonsingular but one more ODE is needed Component form: N u v w p 2 q 3 r Ė 2u v w D 2u v w p 3q 2 r p q 1 r p 1 q r 19

20 2.2.2 Unit Quaternions Discrete-time algorithm for unit quaternion normalization q q Algorithm ( Discrete - Time Normalization of the Unit Quaternions ) 1. k 0. Compute initial values of q k For simplicity, Euler Integration implies that 3. Normalization where h is the sampling time. q k 1 b q k 1 q k h T q q k b / n k q k 1 q q k 1 q 4. Let k k 1 and return to Step 2. A continuous time algorithm for unit quaternion normalization can be implemented by noting that d dt q q 2 q b T q q b / n 0 q k 1 q k 1 q k 1 20

21 2.2.2 Unit Quaternions Continuous-time algorithm for unit quaternion normalization: d dt q q 2q b T q q b/n 0 If q is initialized as a unit vector, then it will remain a unit vector. However, integration of the quaternion vector q from the differential equation will introduce numerical errors that will cause the length of q to deviate from unity. In Simulink this is avoided by introducing feedback: q T q q b nb 2 1 q qq 0 (typically 100 d dt q q 2q T q q nb 0 if q is initialized as a unit vector b 1 q qq q 1 q qq q Change of coordinates x 1 q q (x=0 gives q q 1) 21 x x1 x linearization about x=0 gives x x

22 2.2.3 Quaternions from Euler Angles Ref. Shepperd (1978) Algorithm ( Quaternions From Euler Angles ) n 1. Given the Euler angles,, and. Let the transformation matrix R b according to be written : n R b : R 1 1 R 1 2 R 1 3 R 2 1 R 2 2 R 2 3 R 3 1 R 3 2 R 3 3 e 2. The trace of R b n is computed as : R 44 tr R b R 11 R 22 R Let 1 i 4 be the index corresponding to : R ii max R 1 1, R 2 2, R 3 3, R Compute p i corresponding to Index i of Step 3 according to : p i 1 2 R ii R 4 4 where the sign ascribed to p i can be chosen to be either positive or negative. 5. Compute the other three p i - values from : p 4 p 1 R 3 2 R 2 3 p 2 p 3 R 3 2 R 2 3 p 4 p 2 R 1 3 R 3 1 p 3 p 1 R 1 3 R 3 1 p 4 p 3 R 2 1 R 1 2 p 1 p 2 R 2 1 R 1 2 by dividing the 3 equations containing the component p i with the known value of p i ( from Step 4 ) on both sides Compute the Euler parameters q, 1, 2, 3 according to : p 4 / 2 and j p j / 2 j 1, 2, 3

23 2.2.4 Euler Angles from Quaternions Require that the rotation matrices of the two kinematic representations are equal: R b n nb : R b n q nb,, q, 1, 2, 3 cc sc css ss ccs sc cc sss cs ssc s cs cc R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 Algorithm: One solution is: atan2r 32, R 33 sin 1 R 31 tan 1 R R 31 atan2r 21, R 11 ; 90 o where atan2(y,x) is the 4-quadrant arctangent of the real parts of the elements of x and y, satisfying: atan2y, x 23

24 2.3 Transformation between ECEF and NED A point on or above the Earth s surface is uniquely determined by: Longitude: l (deg) Latitude: (deg) Ellipsoidal height: h (m) NED {n}-frame h ECEF {e}-frame NED axes definitions: N - North axis is pointing North E - East axis is pointing East D Down axis is pointing down in the normal direction to the Earth s surface 24

25 2.3.1 Longitude and Latitude transformations The transformation between the ECEF and NED velocity vectors is: n p b/n R n b b nb v b/n en l, S 2 Two principal rotations: e p b/e 1. a rotation l about the z-axis 2. a rotation ( /2) about the y-axis. R e n n en p b/e R e n en R n b b nb v b/e cos l sinl 0 cos 2 0 sin 2 R n e en R z,l R y, 2 sinl cos l sin 2 0 cos 2 R n e en coslsin sinl coslcos sinl sin cosl sinlcos cos 0 sin 25

26 2.3.1 Flat Earth Navigation For flat Earth navigation it can be assumed that the NED tangent plane is fixed on the surface of the Earth-that is, l and are constants, by assuming that the operating radius of the vessel is limited: e p b/e R e n n en p b/e R e n en R n b b nb v b/e en l, S 2 en constant R n e en R o constant R n e en coslsin sinl coslcos sinl sin cosl sinlcos cos 0 sin 26

27 2.3.2 Longitude/Latitude from ECEF Coordinates Satellite navigation system measurements are given in the ECEF frame: Not to useful for the operator. pe b/e x,y,z Presentation of terrestrial position data pe b/e x,y,z is therefore made in terms of the ellipsoidal parameters longitude l, latitude and height h. Transformation: pe b/e x,y,z en l, and height h 27

28 2.3.2 Longitude/Latitude from ECEF Coordinates Parameters Comments r e m Equatorial radius of ellipsoid (semimajor axis) r p m Polar axis radius of ellipsoid (semiminor axis) e rad/s Angular velocity of the Earth e Eccentricity of ellipsoid pe b/e x,y,z N r e 2 r e 2 cos 2 r p 2 sin 2 e 1 r p r e 2 l atan y e x e while latitude and height h are implicitly computed by: tan z e p 1 e 2 N N h p h cos N 1 28

29 2.3.2 Longitude/Latitude from ECEF Coordinates Algorithm ( Transformation of x e, y e, z e to l,, h ) Compute p x e y e. 2. Compute the approximate value 0 from : tan 0 z e p 1 e Compute an approximate value N from : N 2 r e r 2 e cos 2 0 r 2 p sin Compute the ellipsoidal height by : h p cos 0 N Compute an improved value for the latitude by : tan z e p 1 e 2 N 0 N 0 h 1 6. Check for another iteration step : if 0 < tol where tol is a small number, then the iteration is complete. Otherwise set 0 and continue with Step 3. 29

30 2.3.3 ECEF Coordinates from Longitude/Latitude Ref. Heiskanen (1967) The transformation from en l, for given heights h to pe b/e x,y,z is given by x y z N hcoscosl N hcossinl r p 2 r e 2 N h sin 30

31 2.4 Transformation between BODY and Flow FLOW axes are often used to express hydrodynamic data. The FLOW axes are found by rotating the BODY axis system such that resulting x-axis is parallel to the freestream flow. In FLOW axes, the x-axis directly points into the relative flow while the z-axis remains in the reference plane, but rotates so that it remains perpendicular to the x-axis. The y-axis completes the right-handed system. y b x b U x flow - x stab z b 31

32 2.4.1 Definitions of Course, Heading and Sideslip Angles The relationship between the angular variables course, heading, and sideslip is important for maneuvering of a vehicle in the horizontal plane (3 DOF). The terms course and heading are used interchangeably in much of the literature on guidance, navigation and control of marine vessels, and this leads to confusion. Definition (Course angle χ): The angle from the x-axis of the NED frame to the velocity vector of the vehicle, positive rotation about the z-axis of the NED frame by the right-hand screw convention Definition: Heading (yaw) angle : The angle from the NED x-axis to the BODY x-axis, positive rotation about the z-axis of the NED frame by the right-hand screw convention. Definition: Sideslip (drift) angle β: The angle from the BODY x-axis to the velocity vector of the vehicle, positive rotation about the BODY z-axis frame by the right-hand screw convention 32

33 2.4.1 Definitions of Course, Heading and Sideslip Angles Course angle: Sideslip (drift) angle: arcsin v U small v U U u 2 v 2 Remark: In SNAME (1950) and Lewis (1989) the sideslip angle for marine craft is defined according to: β SNAME = -β The sideslip definition follows the sign convention used by the aircraft community, for instance as in Nelson (1998) and Stevens (1992). This definition is more intuitive from a guidance point-of-view than SNAME (1950). 33 Note that the heading angle equals the course angle ( = χ) when the sway velocity v = 0, that is when there is no sideslip.

34 2.4.2 Sideslip and Angle of Attack y b v stab R y, v b v flow R z, v stab x b U x flow - x stab z b u v w R y, R z, U 0 0 or u U cos cos v U sin w U sin cos U u 2 v 2 cos 0 sin cos sin 0 R y, 0 1 0, R z, R z, sin cos 0 sin 0 cos

35 2.4.2 Sideslip and Angle of Attack Extension to Ocean Currents For a marine craft exposed to ocean currents, the concept of relative velocities is introduced. The relative velocities are: u r u u c v r v v c w r w w c Hence, U r u r2 v r2 w r 2 u r U r cos r cos r v r U r sin r w r U r sin r cos r r tan 1 r sin 1 w r u r v r U r 35

36 2.4.2 Sideslip and Angle of Attack Lift and drag forces can then be computed as a function of forward speed and transformed back to BODY coordinates. y b u U cos cos v U sin w U sin cos x b U x flow - Linear approximation: u U, v U, w U x stab z b BODY to FLOW axes: u, v, w, p, q, r flow TU TU diag1,1/u,1/u,1,1,1 flow U,,,p,q,r TUMTU 1 flow TUNTU 1 flow TU 36