Keywords

1 Introduction

In a few decades, collaborative robots, commonly called as cobots, are improving rapidly in the robotics industry for their flexibility. This cobot is figured out to have the ability to physically interact with humans in a shared workspace by using sensors, intelligent controls, and other design features such as lightweight materials, rounded edges.

To manipulate dexterously like a human arm, the 7-degree-of-freedom (DOF) cobot is designed with a structure of anthropomorphic arm. There are many approach of solving this 7-DOF arm inverse kinematics problem. Authors in [1, 2] consider the problem by analytical method in case of non-offset arm. In this approach, the wrist and shoulder are supposed to be spherical. Then the analytical solution can be found, by choosing the free internal motion of elbow joint around axis through shoulder and wrist. The disadvantage of this method is the problem of singularity and joint limit avoidance. Another approach in solving the inverse kinematic is based on Jacobian matrix. With this anthropomorphic arm, the number of variables is more than the number of equations derived from the position and orientation of the robot hand. Hence, pseudo-inverse matrix method is used and the solution can be achieved by integration joint velocities, acceleration or jerks. The benefit of using this method is problems of obstacle avoidance, joint limitations, singularities are solved clearly by the study of null space [7].

In previous research [1, 2], the inverse kinematics based on matrix calculation is carried out with some link offsets, the general case has not been investigated carefully as 7DOF anthropomorphic arm is an “enormous” case. Additionally, by using Jacobian matrix, the accumulated error may occur due to the integration calculation.

In this paper, the method based on Jacobian matrix and projection method are combined for finding solution of inverse kinematics of a general 7-DOF anthropomorphic manipulator. The combination of these methods can improve the accuracy of the solutions, the null space of pseudo-Jacobian is also considered to avoid joint limitation. Some simulations are given to verify the effectiveness of the proposed approach.

2 Kinematic Analysis

2.1 Direct Kinematics

Let’s consider a 7-DOF manipulator as shown in. The direct kinematics can be solved systematic by using Denavit-Hartenberg (DH) method [6]. The link coordinate systems established with the DH convention and the corresponding DH parameters are shown in Fig. 1 and Table 1, respectively. In which \(q_{i} ,i = 1,2,...,7\) represents the joint variables.

Fig. 1.
figure 1

7-DOF manipulator and link-frames based on DH convention

Full size image

The relative homogeneous transformation matrices \({\mathbf{A}}_{i}^{i-1}({q}_{i})\) are calculated by substituting the DH parameters in Table 1 into the matrix equation for each joint:

$$ {\mathbf{A}}_{i}^{i - 1} \left( {\theta_{i} } \right) = \left[ {\begin{array}{*{20}c} {\cos \;\theta_{i} } & { - \sin \;\theta_{i} \;\cos \;\alpha_{i} } & {\sin \;\theta_{i} \;\sin \;\alpha_{i} } & {a_{i} \;\cos \;\theta_{i} } \\ {\sin \;\theta_{i} } & {\cos \;\theta_{i} \;\cos \;\alpha_{i} } & { - \cos \;\theta_{i} \;\sin \;\alpha_{i} } & {a_{i} \;\sin \;\theta_{i} } \\ 0 & {\sin \;\alpha_{i} } & {\cos \;\alpha_{i} } & {d_{i} } \\ 0 & 0 & 0 & 1 \\ \end{array} } \right] $$
(1)

The position and orientation of the kth link are given by:

$${\mathbf{T}}_{k}^{0}\left(\mathbf{q}\right)={\mathbf{A}}_{1}^{0}\left({q}_{1}\right){\mathbf{A}}_{2}^{1}\left({q}_{2}\right)\dots {\mathbf{A}}_{k}^{k-1}\left({q}_{k}\right)$$
(2)
$$=\left[\begin{array}{cc}{\mathbf{R}}_{k}^{0}(\mathbf{q})& {\mathbf{r}}_{Ok}^{0}(\mathbf{q})\\ 0& 1\end{array}\right], k=1,\dots ,7$$

Results of direct kinematics are given as followings:

$${\mathbf{r}}_{{O}_{1}}^{\left(0\right)}=\left[\begin{array}{c}0\\ 0\\ {d}_{1}\end{array}\right], {\mathbf{r}}_{{O}_{2}}^{\left(0\right)}=\left[\begin{array}{c}{d}_{2}s{q}_{1}\\ {d}_{2}c{q}_{1}\\ {d}_{1}\end{array}\right],$$
$${\mathbf{r}}_{{O}_{3}}^{(0)}=\left[\begin{array}{c}{d}_{2}s{q}_{1}+{d}_{3}c{q}_{1}s{q}_{2}\\ {-d}_{2}c{q}_{1}+{d}_{3}s{q}_{1}s{q}_{2}\\ {d}_{1}-{d}_{3}c{q}_{2}\end{array}\right]$$
$$ {\mathbf{r}}_{{O_{4} }}^{\left( 0 \right)} = \left[ {\begin{array}{*{20}c} {d_{2} sq_{1} + d_{3} cq_{1} sq_{2} + d_{4} \left( {sq_{1} cq3 - cq_{1} cq_{2} sq_{3} } \right)} \\ { - d_{2} cq_{1} + d_{3} sq_{1} sq_{2} + d_{4} \left( { - cq_{1} cq3 - sq_{1} cq_{2} sq_{3} } \right)} \\ {d_{1} - d_{3} cq_{2} - d_{4} sq_{2} sq_{3} } \\ \end{array} } \right] $$
$$ {\mathbf{r}}_{{O_{5} }}^{\left( 0 \right)} = {\mathbf{r}}_{{O_{4} }}^{\left( 0 \right)} + \left[ {\begin{array}{*{20}c} {d_{5} \left( {\left( {cq_{1} cq_{2} cq_{3} + sq_{1} sq_{3} } \right)sq_{4} + cq_{1} sq_{2} cq_{4} } \right)} \\ {d_{5} \left( {\left( {sq_{1} cq_{2} cq_{3} - cq_{1} sq_{3} } \right)sq_{4} + sq_{1} sq_{2} cq_{4} } \right)} \\ {d_{5} \left( {sq_{2} cq_{3} - cq_{2} cq_{4} } \right)} \\ \end{array} } \right] $$
$${\mathbf{r}}_{{O}_{6}}^{(0)}={\mathbf{r}}_{{O}_{5}}^{(0)}+\left[\begin{array}{c}-{d}_{6}((\left(c{q}_{2}c{q}_{3}c{q}_{4}-s{q}_{2}s{q}_{4}\right)s{q}_{5}\\ +c{q}_{2}s{q}_{3}c{q}_{5})c{q}_{1}+(c{q}_{4}s{q}_{5}s{q}_{3}\\ -c{q}_{5}c{q}_{3})s{q}_{1}, \\ -{d}_{6}((\left(c{q}_{2}c{q}_{3}c{q}_{4}-s{q}_{2}s{q}_{4}\right)s{q}_{5}\\ +c{q}_{2}s{q}_{3}c{q}_{5})s{q}_{1}-(c{q}_{4}s{q}_{5}s{q}_{3}\\ -c{q}_{5}c{q}_{3})c{q}_{1},\\ -{d}_{6}(\left(s{q}_{2}c{q}_{3}c{q}_{4}+c{q}_{2}s{q}_{4}\right)s{q}_{5}\\ +s{q}_{2}s{q}_{3}c{q}_{5})\end{array}\right]$$
$${\mathbf{r}}_{{O}_{7}}^{\left(0\right)}={\mathbf{r}}_{{O}_{6}}^{\left(0\right)}+\left[\begin{array}{c}{d}_{7}((\left(c{q}_{5}c{q}_{3}c{q}_{4}-s{q}_{3}s{q}_{5}\right)c{q}_{2}\\ -c{q}_{5}s{q}_{2}s{q}_{4})s{q}_{6}+c{q}_{6}(c{q}_{2}s{q}_{4}c{q}_{3}\\ +s{q}_{2}c{q}_{4}))c{q}_{1}+s{q}_{1}((c{q}_{4}c{q}_{5}s{q}_{3}+ \\ s{q}_{5}c{q}_{3})s{q}_{6}+c{q}_{6}s{q}_{3}s{q}_{4})),\\ {d}_{7}((\left(c{q}_{5}c{q}_{3}c{q}_{4}-s{q}_{3}s{q}_{5}\right)c{q}_{2}\\ -c{q}_{5}s{q}_{2}s{q}_{4})s{q}_{6}+c{q}_{6}(c{q}_{2}s{q}_{4}c{q}_{3}\\ +s{q}_{2}c{q}_{4}))s{q}_{1}-c{q}_{1}((c{q}_{4}c{q}_{5}s{q}_{3}+\\ s{q}_{5}c{q}_{3})s{q}_{6}+c{q}_{6}s{q}_{3}s{q}_{4})),\\ {d}_{7}((\left(c{q}_{5}c{q}_{3}c{q}_{4}-s{q}_{3}s{q}_{5}\right)s{q}_{6}+\\ +c{q}_{6}s{q}_{4}c{q}_{3})s{q}_{2}+c{q}_{2}(c{q}_{5}s{q}_{6}s{q}_{4}-c{q}_{6}c{q}_{4})\end{array}\right]$$
Table 1. DH Parameters
Full size table

2.2 Inverse Kinematics

In robotics, inverse kinematics is the mathematical process of calculating the variable joint parameters needed to place the end of a kinematic chain. The inverse kinematics are solved based on the constraints at the levels of position, velocity, or acceleration [7]:

$$ {\mathbf{x}} - {\mathbf{f}}\left( {\mathbf{q}} \right) = 0 $$
(3)
$$ {\dot{\mathbf{x}}} - {\mathbf{J}}_{q} {\dot{\mathbf{q}}} = 0 $$
(4)
$$ {\ddot{\mathbf{x}}} - {\mathbf{J}}_{q} {\ddot{\mathbf{q}}} - {\dot{\mathbf{J}}}_{q} {\dot{\mathbf{q}}} = 0 $$
(5)

2.2.1 Jacobian-Based Method

As 7DOF cobot is a redundant arm, the solutions of (6) and (5) are given by applying pseudo-inverse:

$$ {\dot{\mathbf{q}}} = {\mathbf{J}}^{\dag } \left( {\mathbf{q}} \right){\dot{\mathbf{x}}} $$
(6)
$$ {\ddot{\mathbf{q}}} = {\mathbf{J}}^{\dag } \left( {\mathbf{q}} \right)\left[ {{\ddot{\mathbf{x}}} - {\dot{\mathbf{J}}}\left( {\mathbf{q}} \right)} \right] $$
(7)

The solution of the inverse kinematics under null space consideration can be found in acceleration level [5] as following:

$$ {\ddot{\mathbf{q}}} = {\mathbf{J}}_{W}^{\dag } \left( {\mathbf{q}} \right)\left[ {{\ddot{\mathbf{x}}} - {\dot{\mathbf{J}}}\left( {\mathbf{q}} \right){\dot{\mathbf{q}}}} \right] + \left( {{\mathbf{I}} - {\mathbf{J}}_{W}^{\dag } {\mathbf{J}}} \right){\mathbf{z}}_{0} $$
(8)

in which, \({\mathbf{z}}_{0} \in {\mathbb{R}}^{n}\) is an arbitrary vector that guarantees the robot able to avoid obstacles, singularity, and joint collision. The vector \({\mathbf{z}}_{0}\) is computed as following:

$$ {\mathbf{z}}_{0} = \alpha \frac{{\partial \phi \left( {\mathbf{q}} \right)}}{{\partial {\mathbf{q}}}} $$
(9)

in which, \(\phi \left( {\mathbf{q}} \right)\) is the objective function that depends on the set of requirements. For example, to avoid singularity, the objective function is chosen as the function of manipulation measurement.

$$ \phi \left( {\mathbf{q}} \right) = \sqrt {\det [{\mathbf{J}}\left( {\mathbf{q}} \right){\mathbf{J}}^{T} \left( {\mathbf{q}} \right)} $$
(10)

The manipulation function is cancelled at singularities. Therefore, maximizing this function value will help the robot avoid singularities during operation.

To neglect joint limitation, the objective function is selected by measuring distance to joint limitation.

$$ \phi \left( {\mathbf{q}} \right) = - \frac{1}{2}\sum\nolimits_{i = 1}^{n} {c_{i} \left( {\frac{{q_{i} - \overline{q}_{i} )}}{{q_{iM} - q_{im} }}} \right)^{2} } $$
(11)

In which, \(q_{iM} \left( {q_{im} } \right)\) is the maximum (minimum) value of joint limitation and \(\overline{q}_{i}\) is the average value of joint limitation, \(c_{i}\) is weight parameters.

To avoid obstacles, we use the function that measure the distance to obstacles.

$$ \phi \left( {\mathbf{q}} \right) = \min {\mathbf{p}}\left( {\mathbf{q}} \right) - {\mathbf{o}} $$
(12)

In which, \({\mathbf{o}}\) is position vector of a point on the obstacle and \({\mathbf{p}}\left( {\mathbf{q}} \right)\) is generalized vector of robot manipulator.

\({\mathbf{J}}_{W}^{\dag }\) is generalized pseudo inverse matrix of Jacobian matrix and is calculated as following:

$$ {\mathbf{J}}_{W}^{\dag } = {\mathbf{W}}^{ - 1} {\mathbf{J}}^{{\mathbf{T}}} \left( {\mathbf{q}} \right)\left[ {{\mathbf{J}}\left( {\mathbf{q}} \right){\mathbf{W}}^{ - 1} {\mathbf{J}}^{T} \left( {\mathbf{q}} \right)} \right]^{ - 1} $$
(13)

\({\mathbf{W}}\) is called the weight matrix. There are several choices of matrix \({\mathbf{W}}\), if \({\mathbf{W}} = {\mathbf{I}}\), the solution in (3) will have the minimum norm. If \({\mathbf{W}} = {\mathbf{M}}\left( {\mathbf{q}} \right)\), the solution is found with the optimization of kinetic energy.

2.2.2 Projection Method

The problem of inverse kinematics is accumulation of error when finding the joint variables by taking integration. By using the coordinate and velocity projection, the joint variables found by the integration are adjusted and revised so that they are forced onto manifolds. Then the accuracy of the solutions is improved significantly.

Fig. 2.
figure 2

Block diagram for inverse kinematics

Full size image

Coordinate Projection

Due to the integration, the found joint variables \({\mathbf{q}}^{*}\) may not satisfy the Eq. (3) and belong outside the manifold. Therefore, the coordinate projection is proposed to figure out the point \({\mathbf{q}}\) that belongs to the curve of the Eq. (3) and has the shortest distance to the point \({\mathbf{q}}^{*}\). Distance function \(V\) is given as following:

$$ V = \frac{1}{2}\left( {{\mathbf{q}} - {\mathbf{q}}^{*} } \right)^{T} {\mathbf{P}}\left( {{\mathbf{q}} - {\mathbf{q}}^{*} } \right) \to min, {\mathbf{P}} > 0 $$
(14)

The problem of this method is finding the point point \({\mathbf{q}}\) that satisfies the Eq. (3) and minimize function \(V\).

By using the calculation algorithm in [5], the joint variables are adjusted and forced onto the manifolds.

Velocity Projection

The method of velocity projection is similar to the coordinate projection, it is necessary to find the \({\dot{\mathbf{q}}}\) satisfy the Eq. (4) by adjusting \({\dot{\mathbf{q}}}^{*}\) to the manifolds. The distance function in this case is given as following:

$$ V = \frac{1}{2}\left( {{\dot{\mathbf{q}}} - {\dot{\mathbf{q}}}^{*} } \right)^{T} {\mathbf{Q}}\left( {{\dot{\mathbf{q}}} - {\dot{\mathbf{q}}}^{*} } \right) \to min, {\mathbf{Q}} > 0 $$
(15)

It is essential to find the point \({\dot{\mathbf{q}}}\) that satisfies the Eq. (6) and minimize function \(V\). Hence, the solution of velocity level is given as following:

$$ {\dot{\mathbf{q}}} = {\mathbf{J}}_{Q}^{\dag } {\mathbf{J}}_{x} {\dot{\mathbf{x}}} + \left( {{\mathbf{I}} - {\mathbf{J}}_{Q}^{\dag } } \right){\mathbf{J}}_{q} {\dot{\mathbf{q}}}^{*} $$
(16)

The block diagram for inverse kinematics based on the combination of Jacobian matrix and projection method is shown in Fig. 2.

3 Numerical Simulation

To verify the accuracy of the proposed method, some numerical simulations are done. Two trajectories have been implemented by MATLAB. The motion law along the trajectory is defined as following:

$$ s\left( t \right) = s_{i} + \frac{{s_{f} - s_{i} }}{\pi }\left( {\frac{\pi t}{{t_{f} }} - \frac{1}{2}\sin \frac{2\pi t}{{t_{f} }}} \right), 0 \le t \le t_{f} $$
(17)

In this paper, \({\mathbf{z}}_{0}\) is chosen to avoid singularities, and \(\phi \left( {\mathbf{q}} \right)\) is calculated as in (10).

The first simulation is carried out by designing the linear trajectory between two picking points: \(A\left( {0.52, - 0.26, 0.28} \right)\) and \(B\left( {0.52, 0.26, 0.478} \right)\) in 5 s in 1 time. The results are illustrated in Fig. 3. The second simulation results are given in Fig. 4 with curvilinear trajectory.

Fig. 3.
figure 3

Tracking of end-effector along linear trajectory between 2 picking points

Full size image
Fig. 4.
figure 4

Tracking of end-effector along curve trajectory

Full size image

From the results, it can be seen that the robot configuration changes uniformly and smoothly, without any singularities and joint limitation. The position errors in \(x,y,z\) coordinates of end-effector also called as \({\mathbf{e}} = \left[ {e_{1} ,e_{2} ,e_{3} } \right]\) are all within \(10^{ - 6}\) and \(10^{ - 16}\) and decreases sharply after 2 s, so the accuracy of this approach is significantly improved.

4 Conclusion

This paper proposed a new approach for solving the inverse kinematics of the 7DOF collaborative robot. The problem of singularity, joint limitation and obstacle avoidance has been neglected by using null space matrix. The accuracy of the solution is also improved significantly by projection method. The numerical simulation are illustrated to clarify the validity of the proposed method.