Kinematics of Serial Manipulators

This book chapter deals with kinematic modeling of serial robot manipulators (open-chain multibody systems) with focus on forward as well as inverse kinematic model. At first, the chapter describes basic important definitions in the area of manipulators kinematics. Subsequently, the rigid body motion is presented and basic mathematical apparatus is introduced. Based on rigid body conventions, the forward kinematic model is established including one of the most used approaches in robot kinematics, namely the Denavit-Hartenberg convention. The last section of the chapter analyzes inverse kinematic modeling including analytical, geometrical, and numerical solutions. The chapter offers several examples of serial manipulators with its mathematical solution.


Introduction and basic definitions
In the following sections, this chapter will deal with direct and inverse kinematics of open-chain multibody systems consisting of rigid bodies. The whole problematics is analyzed from the view of robotics. Each manipulator or mechanism investigated in this chapter will be of serial kinematic structure (open chain).
Open-chain multibody systems are mechanically constructed by connecting a set of bodies, called links, by means of various types of joints. In general, the joints can be passive or active. The joints, which are moved by actuators, are active joints.
In general, from the view of robotics, there are two tasks in kinematics: • Forward kinematics-the forward kinematics problem represents relationship between individual joints of investigated robot and end-effector.
• Inverse kinematics-the problem of inverse kinematics is as follows: given a desired configuration of end-effector of robot, find the joint angles that achieve that configuration.
Before these terms are explained and demonstrated by some study cases, we have to mention the basic definitions, necessary for the further analyses.
Degrees of freedom (DOF): is the smallest number of coordinates needed to represent the robot configuration. Thus, the number of DOF equals to the dimension of configuration space.

Rigid body motion
Rigid motion of an object is a motion that preserves distance between points [4]. Rigid body is a set of particles such that the distance between any two particles remains constant in time, regardless of any motions of the body or forces exerted on the body. If we consider p and q as two points on rigid body, while rigid body moves, p and q must satisfy p t ð Þ À q t ð Þ k k¼ p 0 ð Þ À q 0 ð Þ k k¼ constant, see Figure 1. Let us consider an object, described as a subset O of  3 . Then a motion of object (rigid body) is represented by mapping f t ð Þ : O !  3 . This mapping describes how the points of this object move as a function of time, relative to some fixed coordinate system.
Let the inertial reference frame be O ¼ x r , y r , z r È É and i r , j r , k r represent unit vectors of the reference frame. The vector p can be expressed with respect to inertial reference frame O ¼ x r , y r , z r È É by the following equation p ¼ p xr i r þ p yr j r þ p zr k r (1) where p ¼ p x , p y , p z h i T ∈  3 . Coordinates of vector p can be also expressed as its projections in directions of individual unit vectors as scalar product. In order to find the relation, the vector p needs to be expressed in coordinates O 1 ¼ x 1 , y 1 , z 1 È É p xr ¼ i r p ¼ i r p x1 i 1 þ i r p y1 j 1 þ i r p z1 k 1 p yr ¼ j r p ¼ j r p x1 i 1 þ j r p y1 j 1 þ j r p z1 k 1 p zr ¼ k r p ¼ k r p x1 i 1 þ k r p y1 j 1 þ k r p z1 k 1 (2) which can be rewritten in matrix form p xr p yr p zr 2 6 4 The meaning of this term is as follows. Coordinates of the vector p expressed in so that they are left multiplied by transformation matrix R r1 .
As can be seen in Figure 2, coordinate system x 0 , y 0 , z 0 is rotated with respect to coordinate system x r , y r , z r by angle q around the axis z r . By consideration of previous equations; and by consideration of the facts that scalar product of two perpendicular vectors equals zero, scalar product of two parallel unit vectors is one, and scalar product of concurrent unit vectors is cos α; and by assuming that one can obtain the following rotation matrix where R x is a rotation matrix for rotation around the x-axis by angle α. Subsequently, rotation matrices can be also be expressed for rotation around y-axis and z-axis such as socket joint or ball joint [5]. Within this chapter will be considered only revolute and prismatic joints, which have only a single degree-of-freedom motion. Let us consider a mechanism with N links connected together by N À 1 joints. The ith joint connects link i À 1 to link i. The number of the joints starts with 1 and ends with N À 1. The next consideration for the following mathematical model is that the first link is connected to the base fixed to inertial reference frame, while the last link is free and able to move.
The i-th joint is associated with joint variable q i , while q i may contain θ i and d i for revolute and prismatic joints, respectively. The local coordinate frame is attached to each link, so to i-th link is attached to I i frame, . When a mechanism performs any motion in its workspace, the coordinates of each point on i-th link are constant with respect to their coordinate frame Let A i be a homogeneous transformation matrix, which holds position and orientation of frame It should be noticed that values of matrix A i are not constant, but they change with changing configuration of the mechanism. In general, a homogeneous transformation matrix expressing the position and orientation of We can also define the following matrix where 0 R n is a 3 Â 3 rotation matrix with and 0 o n is a 3 Â 1 vector expressing position and orientation of end-effector (the last point of mechanism) with respect to inertial reference frame (base of mechanism). Eq. (10) can then be written as while A i equals

Denavit-Hartenberg convention
For the computation of forward kinematics for open-chain robot according to Eq. (11), a general approach was derived in order to determine the relative position and orientation of two consecutive links. This approach determines two frames attached to two links (rigid bodies) and computes the coordinate transformations between them [6].
For utilization of the Denavit-Hartenberg convention, some rules need to be observed. Let us consider Figure 3. Let axis i represent the axis connecting link i À 1 and link i þ 1. In order to define link frame i, the procedure is as follows. First of all, the axis z i and axis z iÀ1 are chosen. Next, origin O i is located at the intersection of axis z i with the common normal to axes z i and z iÀ1 . By this step be get points O i and O 0 i . The common normal of these two axes is a minimum distance between them. Subsequently, the axis x i is chosen along the common normal to axes z iÀ1 and z i in the direction from joint i to the joint i þ 1. In the last step, axis y i is chosen so as to complete a right-handed frame.
After these steps, the link frames have been established and now the position and orientation of frame i with respect to frame i À 1 can be determined by following DH parameters [7]: It should be also noted that parameters a i and α i are always constant, because they depend on the geometric aspect of mechanism. Considering the two other parameters d i and ϑ i , depending on the joint type, one is constant and other one may change as follows: • Revolute joint: ϑ i is the joint variable and d i is constant • Prismatic joint: d i is the joint variable and ϑ i is constant In general, six parameters are necessary in order to describe the position and orientation of a rigid body in the 3D space. Based on previously mentioned facts, we can say about DH convention that only four parameters are required by assuming that the axis x i intersects z iÀ1 , and that axis x i is perpendicular to z iÀ1 .

Example of forward kinematics using the Denavit-Hartenberg convention
Let us consider some kind of industrial robot, namely SCARA (Selective Compliance Assembly Robot Arm) robot, which has RRP structure. Its kinematic structure is shown in Figure 4. Considering the basic principles of the Denavit-Hartenberg convention introduced in the previous section, we are able to introduce D-H parameters, see Table 1.
Based on DH parameters, which are obvious from Figure 4, particular homogeneous transformation matrices can be established.   Link Table 1.
So, the final transformation matrix is manipulator with respect to its base inertial reference frame.

Inverse kinematics
The solution exists only if the given end-effector position and orientation are in dexterous workspace of the solved mechanism. While the forward kinematic model is expressed as where f is a function defined between joint space  n and workspace  m , which maps the joint position variables q ∈  n to the position/orientation of the endeffector of mechanism, the inverse kinematic model is based on where q ∈  n and x ∈  m . In the case of the forward kinematic model, endeffector position and orientation are computed for various kinds of mechanisms like manipulators, in a unique manner, for example, by above-mentioned transformation matrices. The inverse kinematic problem is more complex and finding the solution could be in many cases very complicated. While forward kinematics has a closed-form solution, an inverse kinematics in most cases does not have a closedform solution. A forward kinematic model has a unique solution, while an inverse kinematic model may have multiple solutions or infinite number of solutions, especially for kinematically redundant mechanisms. In order to obtain a closedform solution, there are two main approaches, namely algebraic approach and geometric approach.

Closed-form solution of inverse kinematics
Let us consider a two-link mechanism moving in the 2D plane, see Figure 5.
Considering the forward kinematic model, while the angles of joints ϑ 1 and ϑ 2 are given, the aim is to find the position of end-effector The forward kinematic model can be easily determined by the following equations Now, the inverse kinematic problem is to find angles ϑ 1 and ϑ 2 , while the endeffector position x and y are given by vector As can be seen in Figure 5, the presented configuration of the mechanism has two solutions in inverse kinematics. These two solutions are based on signum in Eqs. (25) and (27).

Analytical Jacobian
In order to describe the relation between joint angles and end-effector configuration, often the relation between the joint and end-effector velocities is used. Let us consider a set of coordinates x ∈  m , their velocity is _ x ¼ dx=dt ∈  m . Then, we can apply Eq. (19). Then, one obtains where J q ð Þ ∈  mÂn is the analytical Jacobian matrix, which is very often used in kinematics and dynamics of robotic systems. Jacobian reflects differences between joint and configurations space of the investigated mechanism. In robotics, Jacobian is often used for several purposes such as for the definition of the relation between joint and configuration space, definition of the relation between forces/torques between spaces, the study of kinematic singularities, the definition of numerical solution for inverse kinematic problem, and the study of manipulability properties. We can look at Jacobian from a different perspective. Particular Jacobian columns represent the influence of i-th joint on the end-effector velocity.
The following example will demonstrate the derivation of analytical Jacobian for a three-link mechanism (Figure 6). In order to utilize Eq. (29), we need to define position of point Assuming Eqs. (30) and (31), Jacobian according to Eq. (29) will be matrix where the elements of the Jacobian matrix are

Geometric Jacobian
Besides analytically expressing the Jacobian, we can express it by a geometric approach. To establish function f q ð Þ in closed-form, a symbolic formalism is necessary, which could be difficult from the view of implementation. For this reason, a different way of Jacobian expression, the so-called geometric Jacobian, was developed. The geometric Jacobian can be obtained by consideration of rotational velocity vector ω. Let us consider link according to Figure 6. The Jacobian can be expressed as The first term expresses the effect of _ q 1 on linear velocity v and the second term expresses the effect on the rotational velocity ω. Thus, That is, the analytical Jacobian differs from the geometrical Jacobian for the rotational part. Considering the revolute joint, the i-th column of Jacobian can be computed as For prismatic joint, the i-th column of Jacobian can be computed as where 0 p n is the end-effector position defined in transformation matrix 0 T n defined in the previous section. Next, 0 p iÀ1 is the position of frame The following example will demonstrate the derivation of geometric Jacobian for a two-link mechanism (Figure 7).
where p 0 ¼ Now, the Jacobian is a 6 Â 2 matrix and its maximum rank is 2. That is, at most two components of angular/linear end-effector velocity can be independently assigned. In this case, when orientation is not required, only first two rows are considered.

Kinematically redundant manipulators
The next approach to inverse kinematic solution we want to focus on is numerical solutions. Nevertheless, many times, it is hard to find a closed-form solution for inverse kinematics; the basic kinematics of industrial robots have developed an approach to solve it. The problems arise with nonconventional kinematic structures, especially with kinematically redundant manipulators. A kinematically redundant manipulator has more number of DOFs than is absolutely necessary to perform the desired task. For example, conventional industrial robot has usually six DOFs, by which it is able to reach any point in its workspace. By adding an additional DOF, this robot becomes kinematically redundant due to this additional DOF.
A numerical solution is usually used when a closed-form solution for q does not exist or is difficult to find. In this section, we will focus on kinematically redundant mechanisms. Considering the dimension of joint space n and dimension of task space m, for kinematically redundant mechanisms n > m. The level of redundancy can be expressed by r ¼ n À m. Kinematic redundancy is used for many tasks such as kinematic singularities avoidance, obstacle avoidance, joint limits avoidance, increasing the manipulability in specified directions, minimizing the energy consumption, minimum of motion torques, optimizing execution time, etc. As can be seen, kinematic redundancy allows many optimization tasks to be solved. On the other hand, kinematic redundancy brings some disadvantages as well; for example, a greater structural complexity of construction caused by many of DOFs (mechanical, actuators, sensors), which have an influence on final cost of this kind of mechanism. Next field of potential disadvantages is the field of control, due to complicated algorithms for inverse kinematic computation or motion control. From this reason redundant manipulators could be difficult in real-time control.
There are many approaches within numerical solution of inverse kinematics, which are still in focus of research. Most approaches deal with Jacobian matrix in order to find a linear approximation to the inverse kinematic problem. Among the most used of them are damped least squares (DLSs), Jacobian transpose, and damped least squares with singular value decomposition (SVD) [8,9].
Another kind of approach is the approach based on Newton methods [6]. The aim of these algorithms is to find the final configuration of joints with focus on minimization problem. For this reason, the final motion of robot is smooth. This family of methods includes methods such as Powell's method [10] or Broyden, Fletcher, Goldfarb and Shanno (BFGS).
A very well-known and used method for inverse kinematics of kinematically redundant mechanisms is the so-called cyclic coordinate descent (CCD) algorithm [11]. The CCD method is a very simple and at the same time a very strong method. It is a heuristic iterative method with low computational cost per iteration. Next very know heuristic iterative method is FABRIK (Forward And Backward Reaching Inverse Kinematics) [12]. The FABRIK method minimizes the system error by adjusting each joint angle one at a time.
Most of inverse kinematics numerical methods could be divided into two classes: linearization algorithms and minimization algorithms. Concerning the linearization algorithms, the idea is piecewise linearization of nonlinear inverse kinematic problem, which is based on the Jacobian matrix. An example of this kind of method is the Jacobian transpose method. In minimization algorithms, the idea is to formulate some cost function, which will be minimized, for example cyclic coordinate descent algorithm. Besides the mentioned methods, there are many other such as pseudoinverse methods, such as the Levenberg-Marquardt damped least squares methods, quasi-Newton and conjugate gradient methods, neural network and artificial intelligence methods.
The basic technique is based on Eq. (39) The above relation can be inverted to so-called Jacobian control method which leads to joint velocity vector q with minimum norm. The term J † represents the Moore-Penrose pseudoinverse given by . A very common method on which the solution is based is the Newton-Rhaphson method. The Newton-Rhaphson method is a root-finding algorithm that produces approximations of the roots of a real-valued function. The method starts with differentiable function f defined for a real variable x, derivative of function f 0 , and initial guess x 0 for a root of function f . If these assumptions are satisfied and initial guess is close, then Talking about numerical motion optimization of kinematically redundant mechanisms, there are two approaches. The first approach deals with local methods, which are solved typically online, and the second one with global methods, which require quantity of computation. For this reason, the global methods are computed usually offline.
One of the commonly used local methods is the family of null-space methods. This method uses the extension of Eq. (40) and gives where _ q 0 ∈  n is an arbitrary joint space velocity vector, chosen according to desired behavior; so it is chosen for optimization purposes. Next, I ∈  nÂn is the identity matrix. The term I À J † J À Á represents the orthogonal projection matrix in the null space of J, J † q ð Þ_ x is orthogonal to I À J † J À Á _ q 0 . For this reason, q ¼ 0. Physically, this term corresponds to self-motion, where the combined joints motion generates no motion in the task space (no motion of end-effector). So, the term I À J † J À Á is symmetric and idempotent ( I À J † J À Á 2 ¼ I À J † J À Á ). Also, it ensures The inverse kinematic solution expressed by Eq. (42) is equivalent to solving a quadratic programming (QP) problem based on Now the question is, how the vector _ q 0 can be chosen. One of the basic ways to choose it is the so-called projected gradient method _ q 0 ¼ ∇ q H q ð Þ. Supposing that _ x ¼ 0, that is, the mechanism performs only self-motion, it can be written _ q ¼ I À J † J À Á ∇ q H; so, I À J † J À Á ∇ q H ¼ 0 is a necessary condition of constrained optimality. Based on these facts, an objective function can be chosen for some optimization of motion: • Manipulability-maximize the distance from kinematic singularities • Joint limit avoidance-minimize the distance from the middle of joints range • Obstacle avoidance-maximize the minimum distance to obstacle where a q ð Þ represents points on investigated mechanism and b represents points on the obstacle.
Example: Let us consider a planar six-link robot connected by six revolute joints. All links have the same length L ¼ 100 mm. The purpose of the simulation is path where p is the number of geometric points of the desired path, while f gassuming the step of φ increase to be 0.2. That is, From the matrix X path will be in each path point determined the desired point e p ¼ x p y p h i T ∈  m . Since, there is consideration of planar task with focus on end-effector position, the task space is m ¼ 2. The expected solution assumes only primary solution without any secondary tasks. For inverse kinematic solution, damped least squares method, which avoids many of pseudoinverse method's problems, will be used. This method is also known as the Levenberg-Marquardt method, which arises from cost function JΔq À Δx k k 2 þ λ 2 Δq k k 2 where λ ∈  is a non-zero scalar constant. By minimizing this term, one obtains where I ∈  mÂm is the identity matrix. The simulation will work according to the following algorithm.
1: Set desired path for end-effector by matrix X path and parameters such as L, λ. 2: FOR step = 1 ! p:

3:
Set the desired position of end-effector from X path ¼)e p ¼ x p y p h i T ∈  m 4: WHILE x actual 6 ¼ e p 5: Compute Jacobian 6: Compute

END WHILE 10: END FOR
The results of the simulation can be seen in Figures 8 and 9. Figure 8 presents the motion of planar robot. The aim is to tracking of path with circle shape (green color) by end-effector of the robot. The robot has a fixed frame in point [0,0]. The initial position of all joints is q ¼ 0 0 0 0 0 0 ½ T . During the simulation, no restriction such as joint limit was considered. The simulation was done with a tolerance AE5 mm. Figure 9 presents the variation of individual robot joints during the path tracking.
Example: Let us consider a planar 20-link robot connected with revolute joints. The links have the same length L ¼ 16:75 mm. The aim is to move the end-effector from its initial position to the end position by tracking the desired path. We will consider two cases. The first one considers free robot environment, the second one considers obstacles in the robot environment. The second solution will also consider kinematic singularities avoidance task and joint limit avoidance task. Now, let us consider the cost function dealing with all mentioned secondary tasks [10].
After mathematical adjustment, we get the final formula for kinematic control where W, W c , W L , and W s are in our case 20 Â 20 weight matrices of primary task, obstacle avoidance task, joint limit avoidance task, and kinematic singularities avoidance task, respectively. The setting of weight matrices is subjective. For example, if obstacle avoidance task should have higher priority above primary task by 100 times, the matrix W c would consist of values 100 while W consist of values 1.
Let us consider individual secondary tasks. The joint limit avoidance task deals with the range of motion of individual manipulator links. How many joints will have their limit range is up to us. Of course, in real robots, usually all joints are limited in their motion. There are several ways on how to model the range of joint motion. In this case, an approach with changing of value of weight variable W li based on joint position will be used. If the joint is in admissible range, the value of the weight variable is set to be zero. When the joint reaches the boundary of its range motion, the value of the weight variable increases. When the joint reaches a value outside its admissible range, the value of the weight variable increases to its maximum. This approach can be expressed by Eq. (49) The value of the weight variable has to be set for every joint of the manipulator that needs to be limited in the range of motion. Individual weight variables W li where i ∈ 0, … , N f gare parts of the final weight matrix of the joint limit avoidance task W l ∈  nÂn . The final weight matrix W l is the diagonal matrix [14]: where matrix J s ci is: The Jacobian matrix J c consists of submatrices J ci . The dimension of the Jacobian matrix is J c ∈ R cÂc , where c represents the number of manipulator links that could collide with the obstacles.
For numerical simulation, we will use following algorithm: Inverse kinematic model for 20-link manipulator [14].
Algorithm: Inverse kinematic model for 20-link manipulator 1: CYCLE WHILE 1 2: Determination of new required vector x d ∈ R m from the matrix of planned path P ∈ R rÂ2 3: CYCLE WHILE 2 4: Computation of Jacobian matrix J (damped least squares method) 5: Determination of actual end-effector position in the task space x ∈ R m with actual generalized variables q ∈ R n 6: Computation of general equation    In Figure 11 can be seen the simulation of motion of a 20-link manipulator, which moves in free environment without any obstacles. This case also does not consider any joint limit avoidance task. The aim is to move the end-effector of manipulator by predefined path (green color). Figure 12 depicts a different situation. A 20-link manipulator moves according to predefined path between four obstacles. The solver also assumes joint limit avoidance task for all 20 joints. From this figure can be seen the difference in "selfmotion" in joint space, while it does not affect the motion of end-effector in task space. The end-effector always tracks the same path (by neglecting end-effector orientation).
The second case is significantly difficult from the view of computational complexity in comparison with the case without any constraints in motion.

Conclusion
This chapter was focused on forward and inverse kinematics of open-chain mechanisms, namely manipulators with serial kinematic structures. We have introduced rigid motion and subsequently we have focused on forward kinematics. The chapter presents a kinematic model of SCARA by the well-known Denavit-Hartenberg convention. Within inverse kinematics was introduces several methods, including analytical, geometrical, and numerical. The last section dealt with modeling of kinematically redundant planar robots. At first, we introduced a six-link planar robot with focus on numerical solution using DLS. The last example presented a 20-link redundant manipulator moving between the obstacles. The solution includes, besides the primary solution, secondary tasks such as singularity avoidance, joint limit avoidance, and obstacle avoidance.