假定\(z_0,z_1,z_2\)垂直纸面向里。 从\((x_0,y_0,z_0)\)到\((x_1,y_1,z_1),(x_1,y_1,z_1)\)到\((x_2,y_2,z_2)\)的齐次旋转变换矩阵分别为\({}^0_1T,{}^1_2T\): \[\begin{aligned} {}^0_2T={}^0_1T\cdot {}^1_2T & = \begin{bmatrix} \cos(q_1) & -\sin(q_1) & 0 & 0 \\ \sin(q_1) & \cos(q_1) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\cdot \begin{bmatrix} \cos(q_2) & -\sin(q_2) & 0 & a_1 \\ \sin(q_2) & \cos(q_2) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ & =\begin{bmatrix} \cos(q_1+q_2) & -\sin(q_1+q_2) & 0 & a_1\cos(q_1) \\ \sin(q_1+q_2) & \cos(q_1+q_2) & 0 & a_1\sin(q_1) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \end{aligned}\] 那么，连杆2末端在基础坐标系中的位置矢量\({}^0P\)为: \[\begin{aligned} {}^0P={}^0_2T\cdot{}^2P & =\begin{bmatrix} \cos(q_1+q_2) & -\sin(q_1+q_2) & 0 & a_1\cos(q_1) \\ \sin(q_1+q_2) & \cos(q_1+q_2) & 0 & a_1\sin(q_1) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\cdot\begin{bmatrix} a_2 \\0\\0\\1 \end{bmatrix} \\ & =\begin{bmatrix} a_1\cos(q_1)+a_2\cos(q_1+q_2) \\ a_1\sin(q_1)+a_2\sin(q_1+q_2) \\ 0 \\ 1 \end{bmatrix}=\begin{bmatrix} x_p \\ y_p \\ z_p \\ 1 \end{bmatrix} \end{aligned}\] 那么我们得出结果: \[\left\{ \begin{matrix} x_p=a_1\cos(q_1)+a_2\cos(q_1+q_2) \\ y_p=a_1\sin(q_1)+a_2\sin(q_1+q_2) \end{matrix} \right.\]
假定\(z_0,z_1,z_2\)垂直纸面向里。 从\((x_0,y_0,z_0)\)到\((x_1,y_1,z_1),(x_1,y_1,z_1)\)到\((x_2,y_2,z_2)\),\((x_2,y_2,z_2)\)到\((x_3,y_3,z_3)\)的齐次旋转变换矩阵分别为\({}^0_1T,{}^1_2T,{}^2_3T\): \[\begin{aligned} {}^0_3T={}^0_1T\cdots {}^2_3T & = \begin{bmatrix} \cos(q_1) & -\sin(q_1) & 0 & 0 \\ \sin(q_1) & \cos(q_1) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \cos(q_2) & -\sin(q_2) & 0 & a_1 \\ \sin(q_2) & \cos(q_2) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \cos(q_3) & -\sin(q_3) & 0 & a_2 \\ \sin(q_3) & \cos(q_3) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ & =\begin{bmatrix} \cos(q_1+q_2+q_3) & -\sin(q_1+q_2+q_3) & 0 & a_1\cos(q_1)+a_2\cos(q_1+q_2) \\ \sin(q_1+q_2+q_3) & \cos(q_1+q_2+q_3) & 0 & a_1\sin(q_1)+a_2\sin(q_1+q_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \end{aligned}\] 那么，连杆3末端在基础坐标系中的位置矢量\({}^0P\)为: \[\begin{aligned} {}^0P={}^0_3T\cdot{}^3P & =\begin{bmatrix} \cos(q_1+q_2+q_3) & -\sin(q_1+q_2+q_3) & 0 & a_1\cos(q_1)+a_2\cos(q_1+q_2) \\ \sin(q_1+q_2+q_3) & \cos(q_1+q_2+q_3) & 0 & a_1\sin(q_1)+a_2\sin(q_1+q_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix}\cdot \begin{bmatrix} a_3 \\0\\0\\1 \end{bmatrix} \\ & =\begin{bmatrix} a_1\cos(q_1)+a_2\cos(q_1+q_2)+a_3\cos(q_1+q_2+q_3) \\ a_1\sin(q_1)+a_2\sin(q_1+q_2)+a_3\sin(q_1+q_2+q_3) \\ 0 \\ 1 \end{bmatrix}=\begin{bmatrix} x_p \\ y_p \\ z_p \\ 1 \end{bmatrix} \end{aligned}\] 那么我们得出结果: \[\left\{ \begin{matrix} x_p=a_1\cos(q_1)+a_2\cos(q_1+q_2)+a_3\cos(q_1+q_2+q_3) \\ y_p=a_1\sin(q_1)+a_2\sin(q_1+q_2)+a_3\sin(q_1+q_2+q_3) \end{matrix} \right.\]