Inverse kinematics of 6-links manipulator

As promised in the previous article.

Let’s look at direct kinematics.
H = T_z(L_0) \cdot R_z(q_1) \cdot T_z(L_{11}) \cdot T_x(L_{12}) \cdot R_y(q_2) \cdot T_z(L_2) \cdot R_y(-q_3) \cdot T_z(L_{31}) \cdot R_x(-q_4) \cdot T_x(L_4) \cdot R_y(-q_5) \cdot T_x(L_5) \cdot R_x(-q_6)

Where T_a – translation matrix with respect to a axis, and R_a – rotational matrix with respect to a axis.
H – homogeneous transformation matrix, which contains the position and rotation of the endpoint in global coordinates. (L_{32} is already included in L_4 because there is no difference where J_4 is located on this link)

Minus signs near angles q_i show that in reality, the joint moves in the another direction.

6-links manipulator
6-links manipulator

For inverse kinematics, there were not invent such a simple thing as a simple multiplication of the rotation and translate matrices. Therefore, we take steps.

At the entrance we have the matrix T – the position of the endpoint, its format is the same (4×4) as H = \begin{bmatrix}   R & T   \\ 0.. & 1   \end{bmatrix}

0. Since J_6 rotates along the X axis, we can easily move the point of rotation to the center of J_5. In addition, remove the initial translation L_0 :
T_{all} = T_z(-L_0) \cdot T \cdot T_x(-L_5)

2. Since rotation around Z is only in the first joint, we can find q_1:
x = T_{all}[0][3]
y = T_{all}[1][3]
z = T_{all}[2][3]
T_{all} is also a homogeneous transformation matrix, so we get coordinates from the desired positions. And by them, if x! = 0, then q_1 = atan2(y, x), otherwise any.

3. Now find q_2 and q_3 according to standard formulas for a planar two-link. Only need to do a little preparation. Now the axes are located at the point P – and we know the coordinates of center of J5. We need to transfer the axes in J_2 and then in the plane of rotation J_2 and J_3 only q_2 and q_3 will change the coordinates of the center J_5.

Finding q2 and q3
Finding q2 and q3

For that :
x = \frac{x}{\cos{q_1}} - L_{12}
z = z - L_{11}
Also, two links (right angle) L_{31} and L_4 are replaced by one (with an additional angle \delta q_3):
L_{33} = \sqrt{L_{31}^2 + L_4^2}
\delta q_3 = atan2(L_4, L_{31})
Then for a planar two-links manipulator we get:
q_3 = \arccos{ \frac{x^2+z^2 - L_2^2-L_{33}^2}{2\cdot L_2\cdot L_{33}}} - \delta q_3
q_{33} = q_3 + \delta q_3
q_2 = \frac{\pi}{2} - atan2(z,x) - atan2(L_{33}\cdot \sin{q_{33}}, L_2 + L_{33}\cdot\cos{q_{33}})
q_3 = -q_3

4. Calculate T_{456} in symbolic and numerical form. Joints 4,5 and 6 we moved to the center J_5. Therefore, in symbolic form, direct kinematics looks like this:
T_{456s} = R_x(-q_4)\cdot R_y(-q_5) \cdot R_x(-q_6) =
=  \begin{bmatrix}   \cos{q_5} & \sin{q_5}\cdot \sin{q6} & -\sin{q_5}\cdot \cos{q_6} & 0   \\ \sin{q_4}\cdot \sin{q_5} & ..  & .. & 0 \\ \sin{q_5}\cdot \cos{q_4} & ..  & .. & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

To do the same for the numerical form, we find:
T_{123} = R_z(q_1)\cdot T_z(L_{11})\cdot T_x(L_{12}) \cdot R_y(q_2) \cdot T_z(L_2)\cdot R_y(-q_3) \cdot T_z(L_{31})\cdot T_x(L_4)

Knowing, that T_{all} = T_{123} \cdot T_{456}, find:
T_{456} = T_{123}^{-1} \cdot T_{all}

5. Comparing the symbolic and numerical form from the last step. If T_{456}[0][0] == 1, then we get a degenerate case, q_5 = \arccos{T_{456}[0][0]}, q_4 and q_6 – any . Otherwise (one of the solutions):
q_4 = atan2(T_{456}[1][0],T_{456}[2][0])
q_6 = atan2(T_{456}[0][1],T_{456}[0][2])
q_5 = atan2(T_{456}[0][1], T_{456}[0][0] \cdot \sin{q_6}), if T_{456}[0][1] is not zero and q_5 = atan2(T_{456}[0][2], T_{456}[0][0] \cdot \cos{q_6}) in another case.

Thus, we found all the positions of the joints according to the position of the endpoint.

0 0 vote
Рейтинг статьи
0 комментариев
Inline Feedbacks
View all comments