Keywords

1 Introduction

The inverse kinematics (IK) of general serial 6-R robots was in the 1980s of the 20th century considered to be one of the most challenging problems in robot kinematics. The first solution to this problem was given by Lee and Liang [4]. Many papers followed, the most popular being [5], a solution that is mostly cited in textbooks when the inverse problem is discussed. An overview of the existing literature can be found in the thesis [2, 3], where a new approach to the inverse problem was presented. This solution algorithm uses the Study parametrization of the Euclidean displacement group SE(3) and needs much less equations than Raghavan’s algorithm (eight compared to fourteen). Furthermore, the starting equations are so simple that they can be formulated completely general, i.e. without specifying the Denavit-Hartenberg parameters, making the algorithm applicable to any thinkable robot architecture without reformulation. Study parametrization is an algebraic parametrization of SE(3), using eight parameters, that can be interpreted as homogeneous coordinates in a seven dimensional projective space P 7. To meet the dimensions of the Euclidean displacement group the coordinates have to fulfill a quadratic equation, corresponding to a quadric in P 7, the so called Study quadric. Detailed information on this parametrization and its use in kinematics can be found e.g. in [6].

Within a software developing project called Kinsoft this algorithm was implemented in C# and this package allows to compute all inverse kinematics solutions fast along a given motion trajectory at as many instances of the trajectory as specified. With motion trajectory (or equivalently motion curve) a curve on the Study quadric is meant. Note that a curve on the Study quadric corresponds to a one parameter motion in the Euclidean space. Mathematically the curve is represented by an eight dimensional vector function encoding position end orientation of the end effector (EE). At any instant the IK algorithm returns all solutions of the inverse kinematics. Having all solutions of the inverse kinematics at many instances the following problems arise:

  1. 1.

    How can the solutions for different joint angles be separated such that a continuous path for each joint angle and each of its different solutions can be generated?

  2. 2.

    How can the discrete solution set be transformed into curves that have some desired properties, like stable behaviour at the end points for at least three orders of differentiability?

  3. 3.

    How can the data be used to distinguish the different solutions in order to find out which joint trajectory path is optimal according to some optimizing criteria.

The first two items are discussed in [1]. The results of this paper are recalled briefly in Sect. 2. In Sect. 3 the optimization strategies are presented. Section 4 shows some examples where one can clearly see that the presented optimization algorithm allows to choose between different solutions of the inverse kinematics to optimize the desired behaviour of the manipulator.

2 Separating the Joint Paths

In this section results from [1] are recalled to make the optimization strategies of Sect. 3 understandable.

When the Denavit-Hartenberg parameters of a 6-R robot are specified and a desired trajectory of the EE in position and orientation is given then the Kinsoft program [7] will return a text file that graphically processed yields an output as shown in Fig. 1. On the axis of abscissae of this plot one can see that 1,000 points on the EE trajectory have been used to compute the inverse kinematics. On the axis of ordinates the corresponding joint angles are displayed in different colours in the range of − 180°…180°. Although it seems that there are continuous curves, the program can only return discrete sets of joint angles at every instant.

Fig. 1
figure 1

Joint angle values resulting form inverse kinematics along an EE trajectory

Full size image

Remark The trajectory in the example of Fig. 1 had been chosen such that the manipulator hits its boundary which can be seen clearly because of the gap in all joint trajectories. The path generation algorithm follows the following steps:

  1. 1.

    Data import from the Kinsoft output and data storing in special arrays.

  2. 2.

    Separating the paths using the distance function.

Definition 1

Let a, b ∊ [− 180∘, 180]6 be two vectors. Then the difference between the two sets of angles is defined as

$$ {\parallel }a - b{\parallel }_{p}^{W} : = \left\| {\left( {\begin{array}{*{20}c} {min(|a_{1} - b_{1} |,360 - (|a_{1} - b_{1} |))} \\ {min(|a_{2} - b_{2} |,360 - (|a_{2} - b_{2} |))} \\ {min(|a_{3} - b_{3} |,360 - (|a_{3} - b_{3} |))} \\ {min(|a_{4} - b_{4} |,360 - (|a_{4} - b_{4} |))} \\ {min(|a_{5} - b_{5} |,360 - (|a_{5} - b_{5} |))} \\ {min(|a_{6} - b_{6} |,360 - (|a_{6} - b_{6} |))} \\ \end{array} } \right)} \right\|_{p} $$

where on the right side any p-Norm from \( {\mathbb{R}}^{6} \) can be used.

An upper bound of solutions is given by the number of solutions n 1 in the first point of the motion curve. In order to find a continuation of the first solution of the first point θ 11 , the differences \( {\parallel }\theta_{1}^{1} - \theta_{i}^{2} {\parallel }_{2}^{W} ,i = 1, \ldots ,n_{2} \) are computed. If the minimum of these differences is smaller than an error bound ϵ, then the solution is added to the path.

The error bound must depend on the number of points on the motion trajectory m, because with the number of points in which the inverse kinematics is computed the distance between the data points diminishes. Furthermore the error bound must be adapted to the used norm. In the algorithm different values of the error bound are tested to find out in which range the error bound has to be chosen such that all paths are found.

  1. 3.

    After path separation the joint paths are interpolated using quintic splines. Boundary conditions are given by the first two derivatives on the interval boundaries. The system of equations used is classical and can be found in any textbook on spline interpolation (see. e.g. [8] and in the thesis [9]). The derivatives at the boundary of the interval are not given explicitly, they have to be estimated. Discrete estimation using the first two points of the data did not yield satisfying results, the resulting splines showed unwanted oscillations in the derivatives of the curves near the boundaries. To obtain better results smoothness of the curves was used. The data which have to be interpolated result from the solution of a polynomial system of equations. If the solutions are not complex then they have to describe a smooth \( {\mathcal{C}}^{\infty } \) curve in \( {\mathbb{R}}^{6} \). Using this fact the derivatives in the boundary point were approximated. To do this the first six and the last six points of the data were used to construct a quintic polynomial. The derivatives of those two polynomials were used as boundary conditions for the interpolation of the path splines.

Figure 2 shows the result of the three steps explained above for a classical example from the literature (example nr.7 from Wampler-Morgan [10], abbreviated CW7 in the following). The Denavit-Hartenberg parameters of the manipulator are given in Table 1.

Fig. 2
figure 2

Joint angles of the four continuous paths in joint space of 6-R robot performing a continuous EE motion

Full size image
Table 1 DH-parameter of example CW7
Full size table

The motion which has been performed is given by the Euclidean motion matrix \( {\mathbf{B}}_{C} \):

$$ {\mathbf{B}}_{C} = \left( {\begin{array}{*{20}c} 1 & 0 & 0 & 0 \\ { - 1.140175 - t} & { - 0.760117} & { - 0.641689} & {0.102262} \\ {0.133333 - t} & {0.133333} & {0.991071} & 0 \\ \frac{t}{2} & { - 0.635959} & {0.766965} & {0.085558} \\ \end{array} } \right). $$
(1)

Note that European notation is used in \( {\mathbf{B}}_{C} \), writing the translation part of the motion in the first column and the homogenizing coordinate at first place. In Fig. 2 the upper boundary and the lower boundary have to be identified, and some angles (e.g. θ 5) seem to have four paths and others only two (e.g. θ 1). This is due to the fact that in these angles two solutions coincide. Figure 3 shows a typical result of this procedure where one of the possible continuous solutions of the joint trajectories of a robot motion has been plotted after performing the quintic spline interpolation.

Fig. 3
figure 3

Interpolated joint angle paths of one solution of a 6-R robot performing a continuous EE motion

Full size image

3 Joint Trajectory Optimization

After the first three steps of the algorithm quintic splines of the joint trajectories are given. Because of the fact that the inverse kinematics yields more than one solution more than one continuous solution in the joint trajectories will exist. Therefore one has the possibility to choose among the solutions according to some optimization criteria. As joint angles are computed in the inverse kinematics, the chosen criteria should be linked to the joint angles along a given motion trajectory. In a first choice (type 1 optimization) we will minimize the overall change in the joint angles and in a second choice (type 2 optimization) we will ask for the solution which has a minimum change in joint angular velocities for all joints.

To perform this task a function \( f:[t_{1} ,t_{2} ] \mapsto {\mathbb{R}} \) is considered, which determines the joint angles and which is at least twice continuously differentiable. Then \( f^{\prime}(t) = \frac{df}{dt} \) determines the change of joint angles or the joint velocity. The change of the velocity \( f^{\prime\prime}(t) = \frac{{d^{2} f}}{{dt^{2} }} \) yields the joint acceleration. The overall joint angle changes are given by the integral \( \int_{{t_{1} }}^{{t_{2} }} f^{\prime}(t)dt \). This integral has the disadvantage that positive and negative velocities cancel therefore we have taken the absolute values of the functions \( \int_{{t_{1} }}^{{t_{2} }} |f^{\prime}(t)|dt \) and \( \int_{{t_{1} }}^{{t_{2} }} |f^{\prime\prime}(t)|dt \). Integration of the absolute value of the velocity function yields information about the total length of the joint trajectory. The shorter the trajectory the less movement in the joint will be. Analogous consideration gives information that a smaller value for the integral \( \int_{{t_{1} }}^{{t_{2} }} |f^{\prime\prime}(t)|dt \) yields a smaller change of joint velocities in the interval [t 1, t 2].

The interpolation curves are polynomials of degree five. Therefore it is possible to determine the integral exactly with help of quadrature rules. Because the curves are continuous and are piecewise polynomial, integration can be done separately for each piece of the curve. For computation of the integrals Gauss quadrature rule was used

$$ {\mathcal{I}}_{n} : = \sum\limits_{k = 1}^{n} \sigma_{k} f(\lambda_{k} ){\text{for}}\;f \in [t_{1} ,t_{2} ], $$
(2)

with the nodes \( \lambda_{1} , \ldots ,\lambda_{n} \in {\mathbb{R}} \) and the weights σ 1, …, σ n .

4 Examples

4.1 Example 1

In a first example the Denavit-Hartenberg parameters of the CW7 example in Table 1 were slightly modified as in (Table 2). The motion performed is the motion of Eq. (1) in the interval \( [ - \frac{1}{2},\frac{1}{2}] \).

Table 2 DH-parameter of Example 1
Full size table

This example has three continuous paths (Fig. 4). The results of the optimization are shown in Table 3. The average of the six joint trajectory lengths is the smallest in the green path 3. Therefore this path is optimal when smallest overall change of joint parameters is desired. It can also be seen that the same joint trajectories are optimum when smallest overall change of joint velocities is desired. Once this result is obtained it is of course enough to put the manipulator in the starting values of path 3 and then it will stay on this path, because there is no singularity on this path. A boundary singularity would be seen in such a plot when a path ends, i.e. when two solutions meet in a point having a vertical tangent. This behaviour can be seen in Fig. 1 where the motion trajectory was chosen such that the manipulator hits its boundary and therefore no continuous path is possible. There is a gap in all joint trajectories.

Fig. 4
figure 4

Interpolated joint trajectories of Example 1

Full size image
Table 3 Result of the optimization
Full size table

4.2 Example 2

In this example the DH-parameters of CW7 are changed according to Table 4. The motion performed is the same as in Eq. (1) in the interval \( t \in [ - \frac{1}{4},\frac{1}{4}] \). This example is especially interesting, because 14 continuous joint paths exist. The given task can be performed with 14 different configurations of the manipulator. It should be obvious that these 14 paths are not equivalent The 14 continuous joint trajectories are shown in Figs. 5 and 6.

Table 4 DH-parameter of Example 2
Full size table
Fig. 5
figure 5

Interpolated joint trajectories 1–9 of Example 2

Full size image
Fig. 6
figure 6

Interpolated joint trajectories 10–14 of Example 2

Full size image
Table 5 Results of the optimization Example 2
Full size table

In the result of the optimization one can see big differences in the different trajectories (Table 5). Concerning the average joint motion change trajectory 13 is optimal whereas the path 12 is the worst path. One can see this also in the plot of Fig. 6 on the right side. Trajectories of path 13 (black) have almost horizontal curves, whereas the trajectories of path 12 have much more vertical deviation. Concerning the overall velocity change averaged over all joints (type 2) one can see that path 8 does slightly better than path 13.

Remark In a real robot joint limits would have to be taken into account. This has not been done in this example because the joint limits of the Wampler example are not specified. But it would be no additional effort to plot these limits and make a decision if the given motion can be performed without hitting a joint limit.

Summarizing the results of both optimization algorithms one can state clearly that it matters which starting configuration is chosen from the different possibilities offered by the different solutions of the inverse kinematics. The better choice of a starting can reduce the cycle time of a desired task considerably.

5 Conclusion

The inverse kinematics of a general 6-R manipulator yields up to 16 solutions for the joint angles when the end effector pose is given. Using the fast algorithm developed in [2, 3] the inverse kinematics can be computed along a given end effector motion efficiently. The algorithm presented in this paper detects in the set of joint angles continuous paths and the joint trajectories of these paths are interpolated with quintic splines. Having polynomial curves for joint trajectories, velocities and acceleration a optimization procedures were discussed to decide which of the possible solutions is optimal according to a given optimization criterion like e.g. minimum overall change of joint motion or minimum average overall change of joint velocities. This work can help to decide in which starting pose the manipulator should be brought to perform a given task in a shorter time or with overall smaller change of joint velocities.

6 Acknowledgments

The research presented in this paper was supported by the Translational research project KineControl of the Standortagentur Tirol, Austria. M. Husty acknowledges the support of FWF project P 23831-N13, Algebraic Methods in Collision Detection and Path Planning.