Advanced Quaternion Forward Kinematics Algorithm including Overview of Different Methods for Robot Kinematics

Formulation of proper and efficient algorithms for robot kinematics is essential for the analysis and design of serial manipulators. Kinematic modeling of manipulators is most often performed in Cartesian space. However, due to disadvantages of most widely used mathematical constructs for description of orientation such as Euler angles and rotational matrices, a need for unambiguous, compact, singularity free, computationally efficient method for representing rotational information is imposed. As a solution, unit quaternions are proposed and kinematic modeling in dual quaternion space arose. In this paper, an overview of spatial descriptions and transformations that can be applied together within these spaces in order to solve kinematic problems is presented. Special emphasis is on a different mathematical formalisms used to represent attitude of a rigid body such as rotation matrix, Euler angles, axis-angle representation, unit quaternions, and their mutual relation. Benefits of kinematic modeling in quaternion space are presented. New direct kinematics algorithm in dual quaternion space pertaining to a particular manipulator is given. These constructs and algorithms are demonstrated on the human centrifuge as 3 DoF robot manipulator.


INTRODUCTION
Robot kinematics pertains to the motion of bodies in a robotic mechanism without regard to the forces/torques that cause the motion.Since robotic mechanisms are by their very essence designed for motion, kinematics is the most fundamental aspect of robot design, analysis, control and simulation [1].Special emphasis in the study of robot kinematics is on geometry of the structure of robotic system which is modelled as multidegree of freedom kinematic chain.A kinematic chain is an assemblage of rigid bodies-links, connected by joints providing pure rotation or translation [2], [3].
In general, a rigid displacement may consist of both translation and rotation of the object [4].Thus, a rigid body in three-dimensional space generally has six degrees of freedom; its location is completely described by its position and orientation-the pose of a rigid body.It is of greatest importance in robotic systems to know the correct location of the end-effector in every moment.The actuated joint variables and their time derivatives can be obtained by reading of sensors installed on the joints.Direct kinematics problem is to find end-effector position and orientation relative to the base when positions of all joints are given [5].During the development of kinematics, a number of mathematic theories, tools and algorithms have been introduced and implemented [4], [6].In this paper, two different spaces, Cartesian space and quaternion space, are used for overview of mathematical constructs representing rigid transformation.
Orientation of the rigid body can be described in several different ways.A rotation is a displacement in which at least one point of the rigid body remains in its initial position and not all lines in the body remain parallel to their initial orientations [1].Psyhically, a rotation may be explained as reorientation of a body without changing the body's size and shape [7].All mathematical formalisms used to describe attitude of the rigid body are derived from Euler's rotation theorem.
According to Euler's rotation theorem, any rotational movement of a rigid body in threedimensional space such that a point of the rigid body remains fixed troughout the movement is equivalent to a single rotation about particular axis passing through the fixed point.The axis of rotation is represented by a unit vector û called Euler axis [8].Hence, any rotation in three dimensional space can be represented via unit vector indicating the direction of a rotational axis and a scalar  (angle of rotation).This rotation representation is called axis-angle representation (Figure 1).From axis-angle representation another representationrotation vector, or Euler vector is derived.Rotation vector is represented by a vector directed towards the rotational axis whose magnitude is value of angle of rotation , Besides the axis-angle and rotational vector representations, several sets of three-parameter representations have been reported in the literature.These three-parameter representations are called minimal representations.Perhaps, the most commonly used minimal representation is set of Euler angles [9].

Figure 2. Proper Euler angles
In linear algebra, the Euler theorem states that in three-dimensional space, any two Cartesian coordinate systems with a common origin can be put to coincidence by a finite rotation about certain axis passing through the origin.This approach is used in mechanics of rigid body.Frame can be attached to the rigid body and moving together with it, it defines orientation of the body.This frame is called local and it is described by three basis vectors.Orientation of these basis vectors can be described by means of their direction cosines with respect to the basis of the fixed frame.These parameters can be composed into 3 × 3 matrix called a rotation matrix [1].A rotation matrix may also be referred to as a direction cosine matrix, because the elements of this matrix are the cosines of the unsigned angles between the body-fixed axes and the axes of the fixed frame.
Combining mathematical constructs for position in the form of position vector in homogeneous coordinates and orientation represented by rotational matrix yields 4x4 homogeneous transformation matrix.This real orthonormal matrix is widely used in robotics community [10].
The most popular mathematical constructs describing attitude of a rigid body possess certain disadvantages.Euler angles suffer from wrist singularities which are unfortunate consequence of using minimal representation [11].Rotational matrix can be numerically unstable [12].These disadvantages are overcome by usage of unit quaternions.The relevant functions of the unit quaternions have no singularities and they are numerically stable end efficent.When used to integrate incremental changes in attitude over time, they are more accurate than Euler angles [9].Counterpart for homogenous transformation matrix in quaternion space is dual quaternion.Dual quaternions inherit advantages of unit quaternions while representing rigid transforms [13].
Although quaternions constitute an elegant representation of rotation, they are not used as much as other constructs in robotics community [14].Main reason for this is lack of physicality, i.e. physical meaning of four quaternion parameters is not apparent which is the case with, for an example, Euler angles.On the other hand, extracting the angle and axis of rotation is simple.
The purpose of this paper is to provide simple explanation of possibilities of usage of unit and dual quaternions for spatial descriptions and transformations in robotics.Their relation with corresponding constructs in Cartesian space is given.Features of unit and dual quaternions are considered as much as necessary to be used in robot kinematics relations and algorithms.New direct kinematics algorithm in dual quaternion space is presented and demonstrated on human centrifuge which is modelled as 3 DoF robot manipulator.
It is important to say that all mathematical formalisms described in this paper apply only to coordinate systems of determined orientation.Here, right handed frames are used.

SPATIAL DESCRIPTIONS AND TRANSFORMATIONS IN CARTESIAN SPACE
To identify the location of a body in Cartesian space, a reference coordinate system should be established.This reference coordinate system is called world coordinate system or fixed coordinate system and here it is denoted by O j x j y j z j .After defining a reference coordinate system, the position and orientation of the rigid body are fully described by the position of the origin of the local frame O i x i y i z i and the orientation of its axes.

Position of the local frame origin
The position of the origin of coordinate frame i relative to coordinate frame j can be denoted by the 3 × 1 vector: The components of this vector are the Cartesian coordinates of the position vector of the frame origin i O with respect to reference j-th frame.R is a matrix whose multiplication with a vector rotates the vector while preserving its length [9].The elements of j i R are dot products of basis vectors of the two coordinate frames: ˆˆˆˆˆĉos( , ) cos( , ) cos( , ) ˆˆˆˆĉos( , ) cos( , ) cos( ,z ) ˆˆˆĉos( , ) cos( , ) cos( , )

R y x y y y z x z y z z
.
The columns of this matrix are unit vectors of local frame with respect to the fixed frame: vectors representing positions of the same point in threedimensional space with respect to the frames i and j and the origins of the two frames coincides, then following relation holds: In this paper, notation is adopted in which if reference frame is fixed frame, superscript j is denoted by 0 or denoting superscript is simply omitted.Rotation matrices are square matrices with real entries.More specifically, they can be characterized as The set of all such matrices of size 3 forms a special orthogonal group SO(3).
The following three rotation matrices rotate vector in space about the x, y, or z axis respecitvely: x, 1 0 0 0 cos sin 0 sin cos y, cos 0 sin z, cos sin 0 Other rotation matrices can be obtained from ( 5), ( 6) and ( 7) using successive matrix multiplication.The result of multiplication is another rotational matrix representing one resultant rotation which produces the same effect as sequential application of the original rotation matrices.
The rotation matrix j i R contains nine elements, while only three parameters are required to define the orientation of a body in space.Therefore, six auxiliary relationships exist between the elements of the matrix [1].This redundancy can introduce numerical problems in calculations and often increase computational cost of an algorithm [12].

Attitude presentations: Euler angles
The most common way to represent the attitude of a rigid body in minimal representation is a set of three Euler angles φ, θ, and ψ, known respectively as spin, nutation, and precession.These are popular because they are easy to understand and easy to use [9].
In the Euler angle representation, three rotations in sequence about the coordinate axis of either a fixed or a moving coordinate frame can describe any rotation provided that two successive rotations are not performed about parallel axes.If these elemental rotations are performed about the axes of the fixed frame, they are called extrinsic rotations.Otherwise, if the frame about which axes rotations are performed is local frame, these elemental rotations are called intrinsic.These two sets can be classified in two groups, called proper Euler angles and Tait-Bryan angles also known as roll, pitch and yaw (RPY angles).In the case of proper Euler angles (Figure 2), the first and third rotational axis are the same (e.g.x-y-x, or z-y'-z″).RPY angles represent rotations about three different axes (e.g.x-y-z, or x-y'-z″).Here, superscripts ′ and ′′ are used in the case of intrinsic rotations to denote axes of moving frame after first and second rotation respectively.
Traditionally, rotational matrices have been used to represent Euler angles because the basic rotation matrices for rotation about the x, y, and z axes are simple and well-known [15].Let us consider RPY convention in which elemental rotations are performed about z, y' and x″ axis, with angles ψ , θ , and φ.The function that maps this transformation to its corresponding rotation matrix Here , are given by ( 7), ( 6), ( 5) and following rotational matrix is obtained: Here and throughout the paper, abbreviations c and s are used to denote cosine and sine of an angle.If orientation of end-effector is given in the form of rotation matrix: RPY angles are obtained from following equations: a tan 2 r ,cos r sin r   a tan 2 sin r cos r , sin r cos r The conversion from a general rotation to Euler angles is ambiguous since the same rotation can be obtained with different sets of Euler angles [15].Another disadvantage of this method is singularities found in the various Euler angle representations which occur in problem of gimbal lock situation [9].A gimbal is a physical device consisting of spherical concentric hoops with pivots connecting adjacent hoops, allowing them to rotate within each other [16].A gimbal is constructed by aligning three rings and attaching them orthogonally.Gimbals are often seen in gyroscopes used by the aeronautical industry.Gimbal lock is loss of one degree of freedom which occurs when two rotational axis align (Figure 4).This phenomenon can be considered mathematically through rotation matrix representation of Euler angles (9).If angle  is /2, angles  and  cannot be determined, only their difference can be determined.It is very important to avoid gimbal lock situation because the unpredictability could potentially cause fatal consequences [17].
In robotics, gimbal lock is commonly referred to as "wrist flip" or wrist singularity and it happens when two axes align, for example first and third or fourth and sixth axis.As joints rotate and approach to wrist flip, the singularity will cause numerical ill-conditioning.In order to maintain constant orientation, second (or fifth) axis makes a spin of 360° or 180°.This high velocity motion could cause damages and should be avoided.
Another problem using Euler angles is that they are less accurate than unit quaternions when used to integrate incremental changes in attitude over time [9].This creates a problem in relating the angular velocity vector of a body to the time derivatives of Euler angles, which in some way limits their usefulness in modelling robotic systems.

Homogeneous transformations matrix using D-H convention
Given the above considerations, a general transformation of any vector i p given with respect to a moving frame i into its representation in reference frame j denoted by j p can be described by following vector equation: where j i p is position vector of the origin of the moving frame with respect to reference frame (1).The same applies to position vector of the arbitrary point M of the robotic segment.Using homogeneous coordinates, by which position vector of a point is given as [5]: equation ( 14) can be written if the following compact matrix form: where: This 4x4 matrix is homogenous transformation matrix which maps homogenous position vector from one coordinate system to another.If several transformations are performed successively, transformation matrix describing total transformation is obtained by multiplication of transformation matrices describing those successive transformations.Homogenous transformation matrix describing rotation of angle  i provided by revolute joint whose axis is z i is denoted by: ( , ) while homogenous transformation matrix describing translation for i d provided by prismatic joint whose axis is i z is denoted by : ( , ) Denavit and Hartenberg showed that general transformation between two coordinate systems requires knowledge of the 4 parameters [18].These parameters called the Denavit-Hartenberg parameters (DH parameters) became standard for describing the robot kinematics.Figure 6 shows the DH parameters for the case of two joints.Within this convention, z axis is in the direction of the joint axis, x axis is parallel to the normal vector z n-1 ×z n , and y axis follows from the x and z axis to establish right-handed coordinate system. s

Rodrigues' approach
Conversion from angle and vector representation of orientation to rotation matrix may be achived using Rodrigues' rotational formula [3]: where q is angle of rotation about axis determined by unit vector ê , d e is skew symmetric matrix of vector ê and I is 3×3 identity matrix.In order to obtain general transformation matrix in the form of ( 22) that would apply for revolute as well for prismatic joint j, additional parameter j  is introduced so that it has value 1 for rotation and 0 for translation, and following equation is obtained: (1 cos ) sin Position of the centre of mass i C of the i-th segment w.r.t. to fixed frame is given by equation: where position vectors (Figure 7).Parameter k  has value 0 for rotation and 1 for translation; q is translational or angular displacement. where . It is important to say that all vectors are given with respect to local frame

Unit quaternions
The quaternion representation of orientation due to Hamilton, while largely superseded by the simpler vector representations of Gibbs and Grassmann is extremely useful for problems in robotics that result in representational singularities in the vector/matrix notation [1].Unit quaternion is a convenient and compact mathematical notation for describing the attitude of rigid bodies in three dimensions.Quaternions are hyper complex numbers that can be written as the linear combination a+bi+cj+dk with q ,q ,q ,q  q or via vector and scalar representation   s,  q v .Late formulation is very suitable for rotation description.
Expressing rotations in 3D via unit quaternions instead of matrices has following advantages [20]:  Concatenating rotations is computationally faster and numerically more stable. Since gimbal lock is innate to the matrix representation of Euler angles, this problem does not appear in the quaternion representation. Unambiguousness  Extracting the angle and axis of rotation is simple  Interpolation is more straightforward.As indicated before, from Euler rotational theorem it can be concluded that any rotation in three dimensional space can be described by unit vector indicating the direction of rotational axis and a rotational angle θ.Using extension of Euler formula, this transformation in unit quaternion space becomes: x y z x y z e cos / 2 ,sin / 2 u ,u ,u s, cos ,sin u ,u ,u To define pure rotation, quaternions must satisfy |q| =1, i.e. pure rotation is defined by unit quaternions.
There is an extensive review of quaternion mathematic features in the literature [10].In this paper, only basic quaternion operations required for deriving kinematic algorithms in robotics are considered.
For given unit quaternions s ,  q v , quaternion addition is defined as: and quaternion product is defined as: Quaternion multiplication is associative and distributive across addition, but not commutative.Two successive pure rotations represented by unit quaternion representatives 1 q and 2 q can be achieved by a single rotation about an appropriately chosen axis.Unit quaternion corresponding to this transformation is obtained from quaternion product 1 2  q q , analogously to the resultant rotational matrix representing total rotation in Cartesian space is obtained by successive rotational matrix multiplication.
For a given unit quaternion   q ,q ,q ,q  q , corresponding rotational matrix   R q is obtained by following formula: q q q q 2 q q q q 2 q q q q 2 q q q q q q q q 2 q q q q 2 q q q q 2 q q q q q q q q (30)

Dual quaternions
Dual Quaternions (DQ) were proposed by William Kingdom Clifford in 1873.and they can be considered as extension of quaternions since they represent both rotations and translations of rigid body-it's spatial displacement [12].The dual-quaternion model is an accurate, computationally efficient, robust, and flexible method of representing rigid transforms and should not be overlooked [16].They can be defined as quaternions whose parameters are dual numbers and thus are represented by eight-dimensional vector: x, y,z,s ,x , y ,z  s,x, y,z where  is dual factor,  2 =0, 0.Since spatial displacements of a rigid body in three-dimensional space have six degrees of freedom, dual quaternions are subject to two constraints: q q q q q q q q 0 Some important operations with dual quaternions (from the robotics point of view) are presented bellow.Dual quaternion multiplication is defined as: q q q q q q q q q q q q .(34) Dual conjugate of q is given by: ˆ    q q q .(35) A second conjugation operator of dual quaternion is defined as [13]: If vector p is defined in Cartesian space as , representation of this vector in unit quaternion space is: and in in dual quaternion space: or: Representing pure rotation Rotation of the vector p about axis a for angle  is represented in dual quaternion space by following equation: where dual quaternion of transformed vector p in the form: is obtained from: ˆcos ,sin a , a , a , 0,0,0,0 Representing pure translation Dual quaternion vector corresponding to translation described by translation vector Dual quaternion representing vector p after translation defined by T is: from which dual quaternion vector of transformed vector p is obtained in the form: A pure translation of a vector can be represented as vector addition of a given vector p and translational vector T, thus translation of the vector in dual quaternion space can be represented with another identity: Representing rigid transformations Let us assume that vector p was rotated about axis a for an angle  and after that translated by translation vector T.This transformation is described in dual quaternion space in a following manner: where R q is given by (42).Special attention should be paid that ' T q is dual quaternion describing pure translation defined by translational vector T'-which is rotated vector T: If we assume transformation in which translation given by T is performed first, and rotation given by R q second, transformated vector ' p will be obtained from following:

Direct kinematics algorithm in quaternion space
In this paragraph, it will be considered how vector transformations in dual quaternion space described before are related to direct kinematics algorithm.
To define an efficient kinematic algorithm, a suitable reference frame should be established, as well as local frames moving together with robot segments.Rotations are described by following dual quaternion: where  is value of joint angle for rotational axis represented by vector a in three dimensional space.These vectors are determined with respect to the fixed frame for a robot in a initial position.Rotations for angle  about axis parallel to x,y,z axis of fixed coordinate system will be described respectively: As stated before, we consider clockwise rotations.That mentioned if the axis of rotation is in opposite direction of unit axis of fixed coordinate system, we adopt that the rotation is performed for angle - about axis in the same direction as referent axis of coordinate frame and substitute Translations for value a along axis parallel to x,y,z axis of fixed frame will be respectively: Consider a manipulator with n degrees of freedom, and let n R be a number of rotational joints.Dual quaternion which defines orientation of the end-effector for given joint inputs is given by following equation: where R i q is dual quaternion representing transformation provided by i-th rotational joint.This transformation can be represented in unit quaternion space as following: To verify these results, we can transform dual quaternion 0,n q into rotation matrix using (30).
To determine position of the end-effector, we consider radius vector of the end-effector p as the sum of vectors along respective segments moving together with segments.Dual quaternion vector transformations given in subparagraph 3.2 are used.Equations ( 47) and (49) are applied successifully to define correspondent transformations provided by rotational and prismatic joints.Special attention should be paid to the order of applied successive vector position transformations and especcially to the case where rotation is followed by translation.

SIMULATION RESULTS PERFORMED ON HUMAN CENTRIFUGE (3 DOF MANIPULATOR)
To demonstrate presented direct kinematics algorithm in dual quaternion space, human centrifuge (Figure 9) will be considered.Human centrifuge is dynamic flight simulatior which purpose is to simulate as closely as possible real flight conditions and to provide an effective pilot training [21].This device can be modeled as 3 DoF manipulator with rotational joints [22].Main motion is rotation of the centrifuge arm about vertical, planetary axis.Arm carries gondola which is able to rotate about two axes, pitch and roll.These two axes intersect in the center of the gondola.Pilot seat is considered as end-effector and it is assumed to be placed in the center of the gondola.First, a reference fixed frame is determined and rotational axis are defined (Figure 10).By usage of Rodrigues' approach [3], following rotational matrices and position vector of the endeffector are obtained: This rotational matrices and position vector are given for the purpose of comparison with method given here.Planetary, roll and pitch rotations in dual quaternion space are given by: Here, rotational axis z 0 is parallel to z axis of fixed frame, z 1 is parallel to y axis, and z 2 is parallel to x axis axis of fixed frame.Dual quaternion representing orientation of the end-efector is: Using (30), equality of results obtained by ( 63) and (69) can be obtained.
Since end-effector is placed in the center of the gondola where roll and pitch axis intersect, rotation of the second and the third axis will not affects its position.Vector position in initial position of the robot in Cartesian space is given by vector


 T 1 a 0 0  p and in dual quaternion space by: After rotation of the centrifuge arm, by the usage of (40), a new position of the end-effector in dual quaternion space is: or in Cartesian space with respect to fixed frame: This example shows the simplicity and compactness of presented direct kinematics algorithm.Comparing this approach with Rodrigues' approach applied to this particular manipulator [3], [23], forward kinematics algorithm presented here proved to be simpler and computational cost is smaller.This approach is particularly advantageous in overcoming the problem of singularity.Namely, in previous work [23] it is determined that this manipulator has a singular position in 2 / 2 q    . In this position first and third axes align and from (63), ( 11), ( 12) and ( 13) it can be seen that it is impossible to determine or control orientation by the means of three Euler angles.If we notice that axes of this manipulator are directed in the same manner as zy'x'' sequence of Euler angles in (8), from directions of rotational axes it can be concluded that 2 q    .In this singular position, one degree of freedom is lost and only the difference      can be controlled.This results in numerical ill-conditioning and unpredictable behaviour causing potentially damage situation.On the other hand, from (69) it can be seen that all terms of quaternion representing this orientation are determined.

CONCLUSION
In this paper, compact overview of mathematical constructs used most often for representation of the position and attitude in robotics is given.Advantages of robot kinematic modelling in quaternion and dual quaternion space stemmed from singularity free, numerically stable and suitable for interpolation representation of attitude.Vector transformations using dual quaternions are presented.These transformations are used as foundation for new, simple and compact direct kinematic algorithm in dual quaternion space.Developed algorithm is demonstrated on human centrifuge which is modelled as 3 DoF manipulator with rotational joints.According to the results from the section 4, it can be concluded that the advantage of presented quaternion based algorithm, with respect to Rodrigues' approach, is smaller computational cost.Also, it can be seen that in application of presented quaternion algorithm the problem of singularity, innate to the matrix representation of Euler angles, and particularly important to control of actuators in robotics does not appear.

Figure 3 .
Figure 3.Fixed and local coordinate system

Figure 4 .
Figure 4. Gimbal with three degrees of freedom and gimbal lock situation in which one degree of freedom is lost

Figure 5 .
Figure 5. Transformation of vector from one coordinate system to another

Figure 6 .
Figure 6.Frames and parameters in D-H convention D-H parameters are: • a n -the distance between the axis z n-1 and z n , • α n -the angle between joint axes z n-1 and z n , • d n -the offset along z n-1 to the x n • θ n -the angle between the joint axes x n-1 and x n .Since joints used in manipulators provide usually rotation or translation, only one parameter is variable, while other three are constant.In general, homogeneous transformation matrix based on D-H convention describing motion of link n provided by actuator in joint n-1 with respect to frame attached to joint n-1, n 1 n  T , is

Figure 7 .
Figure 7. Rodrigues' approach: representative vectors in open chain robot mechanism Position vector of the end-effector tip w.r.t. to fixed frame is: Here a represents real part, while b, c and d are imaginary parts or the componests of pure quaternion[19].They can be also interpreted as quadruple

Figure 8 .
Figure 8. Visualisation of unit quaternion parametres through relation with axis-angle presentation hereinafter, 2    .Thus, if rotation is given in the axis-angle form where rotational axis is defined by unit vector x y z ˆˆû u u    u i j k and by angle of rotation  , quaternion representing this rotation is: