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.
- inverse kinematics
1. 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.
2. Rigid body motion
Rigid motion of an object is a motion that preserves distance between points . 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 and as two points on rigid body, while rigid body moves, and must satisfy , see Figure 1.
Let us consider an object, described as a subset
Let the inertial reference frame be and represent unit vectors of the reference frame. The vector can be expressed with respect to inertial reference frame by the following equation
where . Coordinates of vector can be also expressed as its projections in directions of individual unit vectors as scalar product. In order to find the relation, the vector needs to be expressed in coordinates
which can be rewritten in matrix form
that is . The meaning of this term is as follows. Coordinates of the vector expressed in are computed to so that they are left multiplied by transformation matrix .
As can be seen in Figure 2, coordinate system is rotated with respect to coordinate system by angle
one can obtain the following rotation matrix
where is a rotation matrix for rotation around the
Since rotation matrix
where is a identity matrix. Considering the case when there is displacement of local coordinate system and at the same time also its rotation, it would be expressed as
Eq. (8) represents a system of three equations, which will be extended by fourth equation 1 = 0 + 0 + 0 + 1, which is
3. Forward kinematics
The forward kinematic model determines the position and orientation of the end-effector relating to base frame of the mechanism or to global coordinate system (GCS).
3.1 Open kinematic chain
We will focus on robots, which contain a set of links connected together by joints. The joints are usually revolute or prismatic or they can be more complex, such as socket joint or ball joint . 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 links connected together by joints. The -th joint connects link to link
Let be a homogeneous transformation matrix, which holds position and orientation of frame with respect to . It should be noticed that values of matrix are not constant, but they change with changing configuration of the mechanism. In general, a homogeneous transformation matrix expressing the position and orientation of with respect to is called a transformation matrix . We can also define the following matrix
where is a rotation matrix with and is a 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
3.2 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 .
For utilization of the Denavit-Hartenberg convention, some rules need to be observed. Let us consider Figure 3. Let axis
After these steps, the link frames have been established and now the position and orientation of frame with respect to frame can be determined by following DH parameters :
: distance between the points and
: distance between and along the axis
: angle between the axes and about axis (positive direction—counter-clockwise rotation)
: angle between axes and (positive direction—counter-clockwise rotation)
It should be also noted that parameters and are always constant, because they depend on the geometric aspect of mechanism. Considering the two other parameters and , depending on the joint type, one is constant and other one may change as follows:
Revolute joint: is the joint variable and is constant
Prismatic joint: is the joint variable and 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 intersects , and that axis is perpendicular to .
3.2.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.
|0||π||d2 + q3||0|
Based on DH parameters, which are obvious from Figure 4, particular homogeneous transformation matrices can be established.
So, the final transformation matrix is
By vector is defined position of end-effector of SCARA manipulator with respect to its base inertial reference frame.
4. 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 is a function defined between joint space and workspace , which maps the joint position variables to the position/orientation of the end-effector of mechanism, the inverse kinematic model is based on
where and . In the case of the forward kinematic model, end-effector 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 closed-form 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 closed-form solution, there are two main approaches, namely algebraic approach and geometric approach.
4.1 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 and 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 and , while the end-effector position and y are given by vector .
5. Differential kinematics
5.1 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 , their velocity is . Then, we can apply Eq. (19). Then, one obtains
where 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
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 .
where the elements of the Jacobian matrix are
5.2 Geometric Jacobian
Besides analytically expressing the Jacobian, we can express it by a geometric approach. To establish function 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 on linear velocity 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
For prismatic joint, the
where is the end-effector position defined in transformation matrix defined in the previous section. Next, is the position of frame , defined in transformation matrix . Finally, is the third column of rotation matrix , while .
The following example will demonstrate the derivation of geometric Jacobian for a two-link mechanism (Figure 7).
where , , and rotational axes are . By solving Eq. (37), we get
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.
5.3 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 does not exist or is difficult to find. In this section, we will focus on kinematically redundant mechanisms. Considering the dimension of joint space and dimension of task space , for kinematically redundant mechanisms . The level of redundancy can be expressed by . 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 . 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  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 . 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, 13]. 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 with minimum norm. The term represents the Moore-Penrose pseudoinverse given by for kinematically redundant mechanisms where or by for
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 defined for a real variable , derivative of function , and initial guess for a root of function . 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 is an arbitrary joint space velocity vector, chosen according to desired behavior; so it is chosen for optimization purposes. Next, is the identity matrix. The term represents the orthogonal projection matrix in the null space of , is orthogonal to . For this reason, . 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 is symmetric and idempotent (). Also, it ensures . The inverse kinematic solution expressed by Eq. (42) is equivalent to solving a quadratic programming (QP) problem based on subjected to .
Now the question is, how the vector can be chosen. One of the basic ways to choose it is the so-called projected gradient method . Supposing that , that is, the mechanism performs only self-motion, it can be written ; so, 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
where and .
Obstacle avoidance—maximize the minimum distance to obstacle
where represents points on investigated mechanism and 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 . The purpose of the simulation is path tracking (circle form) described by matrix , where is the number of geometric points of the desired path, while , , assuming the step of increase to be 0.2. That is, and . From the matrix will be in each path point determined the desired point . Since, there is consideration of planar task with focus on end-effector position, the task space is . 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 where is a non-zero scalar constant. By minimizing this term, one obtains
where is the identity matrix. The simulation will work according to the following algorithm.
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 .
During the simulation, no restriction such as joint limit was considered. The simulation was done with a tolerance 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 . 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 .
After mathematical adjustment, we get the final formula for kinematic control
where , , , and 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 would consist of values 100 while 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 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 where are parts of the final weight matrix of the joint limit avoidance task . The final weight matrix is the diagonal matrix :
The weight matrix is used with the corresponding Jacobian matrix . The Jacobian matrix for the joint limit avoidance task is . If a particular joint does not consider the joint limit avoidance task, the value of is set to be zero; otherwise it is set to be one. The limit of all joints in motion of the manipulator investigated in this study is set to be ±100°. Different way of joint limit control is according to above mentioned Eq. (44).
During the obstacle avoidance task, the control system investigates the relation between manipulator links and obstacles in their environment. In general, this task can be solved from two views. At first, one group of obstacles can represent static obstacles or other robots in an investigated environment. The second group of obstacles can be represented by dynamic obstacles, which means that these obstacles change their position relating to global reference system in the time. Of course, the second group of obstacles is more difficult from the view of control in comparison with static obstacles. It is more difficult especially in the cases of requirements for real-time control .
The aim of obstacle avoidance is to prevent the collision between any part of the manipulator and potential obstacles, other robots, or collision with itself. Again, there are many methods on how to control robot motion at a safe distance from other objects, regardless of whether the obstacles have regular or irregular shape. For simplification, the irregular shapes are usually replaced by appropriate regular shape. This simplification can also significantly simplify the mathematical model and obtaining the numerical solution can be faster and the solution more stable. One of the methods on how to simplify irregular shapes is to replace all irregular shapes by a cylinder, with the obstacle being situated in the center of the cylinder, see Figure 10. The diameter of the cylinder determines the distance of influence of this obstacle.
At first, we set the coordinate of an obstacle in the task space as . The projection of the line from the
The coordinate of the link point with potential to get into collision is:
The distance between the potential point of collision and the center of the cylinder is:
Subsequently, the unit vector of the potential point of collision can be given by:
Now, the Jacobian matrix for the obstacle avoidance task can be given. The
where matrix is:
The Jacobian matrix consists of submatrices . The dimension of the Jacobian matrix is , where
For numerical simulation, we will use following algorithm: Inverse kinematic model for 20-link manipulator .
1: CYCLE WHILE 1
2: Determination of new required vector from the matrix of planned path
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 with actual generalized variables
6: Computation of general equation
9: IF THEN
END CYCLE WHILE 2
CYCLE WHILE 2 continues
10: END CYCLE WHILE 2
11: END CYCLE WHILE 1
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 “self-motion” 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.
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.
The authors would like to thank to Slovak Grant Agency VEGA 1/0389/18 “Research on kinematically redundant mechanisms” and to KEGA 018TUKE-4/2018 “Implementation of new technologies and education methods in area of controlling systems for improvement of educational level and practical experiences of graduate students of Mechatronics study program” and also to KEGA 030TUKE-4/2020 “Transfer of knowledge from the field of industrial automation and robotics to the education process in the teaching program mechatronics.”