Serial Manipulator Arm Analysis

# Serial Arm Kinematics

In order to better understand the motion of a serial manipulator arm, an example is given below. A 6 DOF arm is shown, and contains three prismatic and three revolute joints. For this arm, the three revolute joints compose a spherical wrist. The forward and inverse kinematics of the arm will be determined and a demonstration of the serial arm's motion will be presented.

## Forward Kinematics

The general procedure for deriving the forward kinematics of a serial robot manipulator is given in the following steps:
1. Label all joints i = 1 to n.

Figure 1: Step 1

2. Assign z-axes for joints 0 to n-1 (zo along joint 1, etc.).

Figure 2: Step 2

3. Assign xo normal to zo.
4. Assign x1 through xn-1, which lie at the common normals between zo to zn-1.

Figure 3: Steps 3,4

5. Establish y1 to yn-1 to complete each frame.
6. Assign zn freely (but carefully) and define xn.

Figure 4: Steps 5,6

7. Create the table of DH link parameters, defining $\alpha$i, ai, di, and $\theta$i for each joint.

Link i $\alpha$i ai di $\Theta$i Tii-1
1 90 0 d1* 180 T10
2 90 0 d2* -90 T21
3 90 0 d3* 90 T32
4 90 0 d4 $\Theta$4* T43
5 90 0 0 $\Theta$5* T54
6 0 0 d6 $\Theta$6* T65

(* denotes variable joint parameters)

8. Create Tii-1 for i = 1 to n.
9. Solve Tno = T1o * T21 * … * Tnn-1.

(1)
\begin{align} T _{\rm 3}^{0} = \left[ { \begin{array}{cccc} 0 & 1 & 0 & d_{\rm 3}\\ 1 & 0 & 0 & d_{\rm 2}\\ 0 & 0 & -1 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(2)
\begin{align} T _{\rm 6}^{3} = \left[ { \begin{array}{cccc} c_{\rm 4}c_{\rm 5}c_{\rm 6} + s_{\rm 4}s_{\rm 6} & -c_{\rm 4}c_{\rm 5}s_{\rm 6} + s_{\rm 4}c_{\rm 6} & c_{\rm 4}s_{\rm 5} & c_{\rm 4}s_{\rm 5}d_{\rm 6}\\ s_{\rm 4}c_{\rm 5}c_{\rm 6} - c_{\rm 4}s_{\rm 6} & -s_{\rm 4}c_{\rm 5}s_{\rm 6} - c_{\rm 4}c_{\rm 6} & s_{\rm 4}s_{\rm 5} & s_{\rm 4}s_{\rm 5}d_{\rm 6}\\ s_{\rm 5}c_{\rm 6} & -s_{\rm 5}s_{\rm 6} & -c_{\rm 5} & d _{\rm 4} - c_{\rm 5}d _{\rm 6}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(3)
\begin{align} T _{\rm 6}^{0} = \left[ { \begin{array}{cc} R _{\rm 6}^{0} & o _{\rm 6}^{0}\\ 0 & 1\\ \end{array} } \right] = \left[ { \begin{array}{cccc} s_{\rm 4}c_{\rm 5}c_{\rm 6} - c_{\rm 4}s_{\rm 6} & -s_{\rm 4}c_{\rm 5}s_{\rm 6} - c_{\rm 4}c_{\rm 6} & s_{\rm 4}s_{\rm 5} & d_{\rm 3} + s_{\rm 4}s_{\rm 5}d_{\rm 6}\\ c_{\rm 4}c_{\rm 5}c_{\rm 6} + s_{\rm 4}s_{\rm 6} & -c_{\rm 4}c_{\rm 5}s_{\rm 6} + s_{\rm 4}c_{\rm 6} & c_{\rm 4}s_{\rm 5} & d_{\rm 2} + c_{\rm 4}s_{\rm 5}d_{\rm 6}\\ -s_{\rm 5}c_{\rm 6} & s_{\rm 5}s_{\rm 6} & c_{\rm 5} & d _{\rm 1} - d _{\rm 4} + c_{\rm 5}d _{\rm 6}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}

10. Show the position as the last column of T6o and the orientation as the first three columns of T6o, excluding the fourth row.

(4)
\begin{align} o _{\rm 6}^{0} = \left[ { \begin{array}{ccc} d_{\rm 3} + s_{\rm 4}s_{\rm 5}d_{\rm 6}\\ d_{\rm 2} + c_{\rm 4}s_{\rm 5}d_{\rm 6}\\ d _{\rm 1} - d _{\rm 4} + c_{\rm 5}d _{\rm 6}\\ \end{array} } \right] \end{align}
(5)
\begin{align} R _{\rm 6}^{0} = \left[ { \begin{array}{cccc} s_{\rm 4}c_{\rm 5}c_{\rm 6} - c_{\rm 4}s_{\rm 6} & -s_{\rm 4}c_{\rm 5}s_{\rm 6} - c_{\rm 4}c_{\rm 6} & s_{\rm 4}s_{\rm 5}\\ c_{\rm 4}c_{\rm 5}c_{\rm 6} + s_{\rm 4}s_{\rm 6} & -c_{\rm 4}c_{\rm 5}s_{\rm 6} + s_{\rm 4}c_{\rm 6} & c_{\rm 4}s_{\rm 5}\\ -s_{\rm 5}c_{\rm 6} & s_{\rm 5}s_{\rm 6} & c_{\rm 5}\\ \end{array} } \right] \end{align}

## Inverse Kinematics

1) Start with the given tool pose T6o. The top left 3x3 is the orientation matrix and the 4th column is the position matrix.

(6)
\begin{align} \left [ \begin{array}{cccc} r11 & r12 & r13 & xp \\ r21 & r22 & r23 & yp \\ r31 & r32 & r33 & zp \\ 0 & 0 & 0 & 1 \end{array} \right ] \end{align}

2) Solve the forward kinematics for T3o and R63. Normally R63 is a spherical wrist, so it controls the orientation of the end effector. T3o will work on satisfying the position requirement.

3) Find the location of the wrist center, pc. The vector (x,y,z) would be the end effector location. We need to back it up to the wrist center, (xp , yp , zp).

(7)
\begin{align} p_{\rm c} = \left [ \begin{array}{c} x_{\rm p} \\ y_{\rm p} \\ z_{\rm p} \end{array} \right ] = \left [ \begin{array}{c} x \\ y \\ z \end{array} \right ] - d6* \left [ \begin{array}{c} r13 \\ r23 \\ r33 \end{array} \right ]_{\rm fromgiven} \end{align}

4) Set pc equal to the last column of T3o from the forward kinematics. Solve these equations for the 1st 3 joint parameters.

(8)
\begin{align} p_{\rm c} = \left [ \begin{array}{c} x_{\rm p} \\ y_{\rm p} \\ z_{\rm p} \end{array} \right ] = \left [ \begin{array}{c} d_{\rm 3} \\ d_{\rm 2} \\ d_{\rm 1} \end{array} \right ]_{\rm forwardkin} \end{align}
(9)
\begin{align} x_{\rm p} = d_{\rm 3} \end{align}
(10)
\begin{align} y_{\rm p} = d_{\rm 2} \end{align}
(11)
\begin{align} z_{\rm p} = d_{\rm 1} \end{align}

5) From the forward kinematics, use R3o and R6o. Determine R63 (given) = (R3o)T * R6o.
Remember that R6o = R3o * R63. For this case, it is a spherical wrist.

(12)
\begin{align} \left [ \begin{array}{c} r13 \\ r23 \\ r33 \end{array} \right ] _{\rm fromgiven} = \left [ \begin{array}{c} c_{\rm 4} s_{\rm 5} \\ s_{\rm 4} c_{\rm 5} \\ -c_{\rm 5} \end{array} \right ]_{\rm forwardkin} \end{align}
(13)
\begin{align} \left [ \begin{array}{ccc} r31 & r32 & r33 \end{array} \right ] _{\rm fromgiven} = \left [ \begin{array}{ccc} s_{\rm 5}c_{\rm 6} & c_{\rm 5}s_{\rm 6} & -c_{\rm 5} \end{array} \right ] _{\rm forwardkin} \end{align}

6) Solve for the final joint parameters by setting the orientation matrix equal to R63.
Remember to keep track of the number of solutions.

(14)
\begin{align} $\Theta$_{\rm 5} = acos(r33) \end{align}

(Remember that two solutions exist.)

(15)
\begin{align} $\Theta$_{\rm 4} = atan2(-r23/s_{\rm 5}, -r13/s_{\rm 5}) \end{align}
(16)
\begin{align} $\Theta$_{\rm 6} = atan2(-r32/s_{\rm 5}, r31/s_{\rm 5}) \end{align}

## Video Example

This video shows how the robot moves through some simple lines in the x, y, and z direction. Next, it shows the changing orientation of the spherical wrist. Last, it moves through a more complex shape by solving the inverse kinematics as shown above.

## References

Bibliography
1. Spong, et.al. Robot Modeling and Control.
page revision: 25, last edited: 24 Feb 2010 15:50