坐标系

D-H参数表

axis i	\alpha_{i-1}	a_{i-1}	d_i	\theta_i
1	0	0	0	\theta_1 = 0
2	-\frac{\pi}{2}	0	d_2=116.5	\theta_2 = -\frac{\pi}{2}
3	0	a_2=203.5	0	\theta_3 = 0
4	0	a_3=173	0	\theta_4 = \frac{\pi}{2}
5	\frac{\pi}{2}	0	d_5 = 79.2	\theta_5 = 0
6	-\frac{\pi}{2}	0	0	\theta_6 = 0

正运动学

_1^0T = \left[\begin{matrix}\cos{\left(\theta_{1} \right)} & - \sin{\left(\theta_{1} \right)} & 0 & 0\\\sin{\left(\theta_{1} \right)} & \cos{\left(\theta_{1} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]

_2^1T = \left[\begin{matrix}\cos{\left(\theta_{2} \right)} & - \sin{\left(\theta_{2} \right)} & 0 & 0\\0 & 0 & 1 & d_{2}\\- \sin{\left(\theta_{2} \right)} & - \cos{\left(\theta_{2} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]

_3^2T = \left[\begin{matrix}\cos{\left(\theta_{3} \right)} & - \sin{\left(\theta_{3} \right)} & 0 & a_{2}\\\sin{\left(\theta_{3} \right)} & \cos{\left(\theta_{3} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]

_4^3T = \left[\begin{matrix}\cos{\left(\theta_{4} \right)} & - \sin{\left(\theta_{4} \right)} & 0 & a_{3}\\\sin{\left(\theta_{4} \right)} & \cos{\left(\theta_{4} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]

_5^4T = \left[\begin{matrix}\cos{\left(\theta_{5} \right)} & - \sin{\left(\theta_{5} \right)} & 0 & 0\\0 & 0 & -1 & - d_{5}\\\sin{\left(\theta_{5} \right)} & \cos{\left(\theta_{5} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]

_6^5T = \left[\begin{matrix}\cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{6} \right)} & 0 & 0\\0 & 0 & 1 & 0\\- \sin{\left(\theta_{6} \right)} & - \cos{\left(\theta_{6} \right)} & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]

逆运动学

求\theta_1

已知目标齐次矩阵^0_6T

 \left[\begin{matrix}r_{11} & r_{12} & r_{13} & p_{x}\\r_{21} & r_{22} & r_{23} & p_{y}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] = ^0_6T 

两边各左乘_1^0T^{-1}，即

_1^0T^{-1} \times \left[\begin{matrix}r_{11} & r_{12} & r_{13} & p_{x}\\r_{21} & r_{22} & r_{23} & p_{y}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] = _6^1T

计算得

\left[\begin{matrix}r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)} & r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)} & r_{13} \cos{\left(\theta_{1} \right)} + r_{23} \sin{\left(\theta_{1} \right)} & p_{x} \cos{\left(\theta_{1} \right)} + p_{y} \sin{\left(\theta_{1} \right)}\\- r_{11} \sin{\left(\theta_{1} \right)} + r_{21} \cos{\left(\theta_{1} \right)} & - r_{12} \sin{\left(\theta_{1} \right)} + r_{22} \cos{\left(\theta_{1} \right)} & - r_{13} \sin{\left(\theta_{1} \right)} + r_{23} \cos{\left(\theta_{1} \right)} & - p_{x} \sin{\left(\theta_{1} \right)} + p_{y} \cos{\left(\theta_{1} \right)}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] =

\left[\begin{matrix}- \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} + \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - \sin{\left(\theta_{6} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} + \theta_{3} \right)} + d_{5} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\\\sin{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & - \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{6} \right)} & \cos{\left(\theta_{5} \right)} & d_{2}\\- \sin{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} - \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} & \sin{\left(\theta_{6} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} \cos{\left(\theta_{5} \right)} - \cos{\left(\theta_{6} \right)} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - a_{2} \sin{\left(\theta_{2} \right)} - a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)} + d_{5} \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]

注意到两边矩阵(2, 4)处元素，即

- p_{x} \sin{\left(\theta_{1} \right)} + p_{y} \cos{\left(\theta_{1} \right)} = d2

该等式中只有\theta_1未知，故可解出\theta_1

对等式左边进行三角变换，即

 - p_{x} \sin{\left(\theta_{1} \right)} + p_{y} \cos{\left(\theta_{1} \right)} = \rho \sin{(\phi - \theta_1)} 

其中，\rho = \sqrt{p_x^2 + p_y^2}，\phi = Atan2(p_y, p_x)

于是，有

 \begin{aligned} \sin{(\phi - \theta_1)} &= \frac{d_2}{\rho} \\
cos(\phi - \theta_1) &= \pm \sqrt{1 - \dfrac{d_2^2}{\rho^2}} \end{aligned}

两式相除，则有

 \phi - \theta_1 = Atan2 \left( \dfrac{d_2}{\rho}, \sqrt{1 - \dfrac{d_2^2}{\rho^2}} \right) 

即可解出\theta_1

\theta_1 = Atan2(p_y, p_x) - Atan2(d_2, \pm \sqrt{p_x^2 + p_y^2 - d_2^2})

求\theta_5

上面已经求出\theta_1了，注意到(1)处矩阵两边的元素(2, 3)，即

 - r_{13} \sin{\left(\theta_{1} \right)} + r_{23} \cos{\left(\theta_{1} \right)} = \cos{\left(\theta_{5} \right)}

可以解出\theta_5，此时\theta_1已知

\theta_5 = \pm \arccos \left( - r_{13} \sin{\left(\theta_{1} \right)} + r_{23} \cos{\left(\theta_{1} \right)} \right) 

求\theta_6

上面已经求出\theta_1, \theta_5了，注意到(1)处矩阵两边的元素(2, 1)和(2, 2)，即

\begin{aligned}
-r_{11} \sin{\left(\theta_{1} \right)} + r_{21} \cos{\left(\theta_{1} \right)} &= \sin{\left(\theta_{5} \right)} \cos{\left(\theta_{6} \right)} \\
-r_{12} \sin{\left(\theta_{1} \right)} + r_{22} \cos{\left(\theta_{1} \right)} &= - \sin{\left(\theta_{5} \right)} \sin{\left(\theta_{6} \right)}
\end{aligned}

两式相除即可求得\theta_6

\theta_6 = Atan2 \left( \dfrac{-r_{12} \sin{\left(\theta_{1} \right)} + r_{22} \cos{\left(\theta_{1} \right)}}{-\sin(\theta_5)} , \dfrac{-r_{11} \sin{\left(\theta_{1} \right)} + r_{21} \cos{\left(\theta_{1} \right)}}{\sin(\theta_5)} \right)

求\theta_3

上面已经求出关节角\theta_1, \theta_5, \theta_6了，剩下的2, 3, 4关节可以看成简单的平面三连杆机械臂

已知目标齐次矩阵^0_6T

 \left[\begin{matrix}r_{11} & r_{12} & r_{13} & p_{x}\\r_{21} & r_{22} & r_{23} & p_{y}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] = ^0_6T 

两边各左乘_1^0T^{-1}，右乘_6^5T^{-1} {}_5^4T^{-1}，即

 _1^0T^{-1} \times \left[\begin{matrix}r_{11} & r_{12} & r_{13} & p_{x}\\r_{21} & r_{22} & r_{23} & p_{y}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] \times _6^5T^{-1} \times {}_5^4T^{-1} = _1^0T^{-1} \times _6^0T \times _6^5T^{-1} \times {}_5^4T^{-1} = _4^1T 

计算后得

\left[\begin{matrix}- \left(r_{13} \cos{\left(\theta_{1} \right)} + r_{23} \sin{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{5} \right)} + \left(\left(r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - \left(r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)}\right) \cos{\left(\theta_{5} \right)} & \left(r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} + \left(r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} & \left(r_{13} \cos{\left(\theta_{1} \right)} + r_{23} \sin{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{5} \right)} + \left(\left(r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - \left(r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)}\right) \sin{\left(\theta_{5} \right)} & d_{5} \left(\left(r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} + \left(r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)}\right) + p_{x} \cos{\left(\theta_{1} \right)} + p_{y} \sin{\left(\theta_{1} \right)}\\\left(r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{5} \right)} - \left(\left(r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - \left(r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)}\right) \cos{\left(\theta_{5} \right)} & - \left(r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} - \left(r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} & - \left(r_{13} \sin{\left(\theta_{1} \right)} - r_{23} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{5} \right)} - \left(\left(r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)} - \left(r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)}\right) \sin{\left(\theta_{5} \right)} & - d_{5} \left(\left(r_{11} \sin{\left(\theta_{1} \right)} - r_{21} \cos{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} + \left(r_{12} \sin{\left(\theta_{1} \right)} - r_{22} \cos{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)}\right) - p_{x} \sin{\left(\theta_{1} \right)} + p_{y} \cos{\left(\theta_{1} \right)}\\- r_{33} \sin{\left(\theta_{5} \right)} + \left(r_{31} \cos{\left(\theta_{6} \right)} - r_{32} \sin{\left(\theta_{6} \right)}\right) \cos{\left(\theta_{5} \right)} & r_{31} \sin{\left(\theta_{6} \right)} + r_{32} \cos{\left(\theta_{6} \right)} & r_{33} \cos{\left(\theta_{5} \right)} + \left(r_{31} \cos{\left(\theta_{6} \right)} - r_{32} \sin{\left(\theta_{6} \right)}\right) \sin{\left(\theta_{5} \right)} & d_{5} \left(r_{31} \sin{\left(\theta_{6} \right)} + r_{32} \cos{\left(\theta_{6} \right)}\right) + p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] =

\left[\begin{matrix}\cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & 0 & a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} + \theta_{3} \right)}\\0 & 0 & 1 & d_{2}\\- \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & - \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} & 0 & - a_{2} \sin{\left(\theta_{2} \right)} - a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]

注意到上述矩阵两边元素(1, 4)和(3, 4)，其中左边矩阵中所有元素均已知，为了方便，记左边矩阵(1, 4)和(3, 4)处元素分别为m, n， 即

\begin{aligned}
m &= d_{5} \left(\left(r_{11} \cos{\left(\theta_{1} \right)} + r_{21} \sin{\left(\theta_{1} \right)}\right) \sin{\left(\theta_{6} \right)} + \left(r_{12} \cos{\left(\theta_{1} \right)} + r_{22} \sin{\left(\theta_{1} \right)}\right) \cos{\left(\theta_{6} \right)}\right) + p_{x} \cos{\left(\theta_{1} \right)} + p_{y} \sin{\left(\theta_{1} \right)} \\
n &= d_{5} \left(r_{31} \sin{\left(\theta_{6} \right)} + r_{32} \cos{\left(\theta_{6} \right)}\right) + p_{z}
\end{aligned}

令矩阵左右两边元素分别相等，有

\begin{aligned}
a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} + \theta_{3} \right)} &= m \\
-a_{2} \sin{\left(\theta_{2} \right)} - a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)} &= n
\end{aligned}

将上述两式取平方和，即可消去\theta_2，得

a_{2}^{2} + 2 a_{2} a_{3} \cos{\left(\theta_{3} \right)} + a_{3}^{2} = m^2 + n^2

这一步消去\theta_2用到了下面这个式子

 \sin(\theta_2) \sin(\theta_2 + \theta_3) + \cos(\theta_2) \cos(\theta_2 + \theta_3) = \cos(\theta_3) 

由此，未知量只剩\theta_3，可解出来

 \theta_3 = \pm \arccos \left( \dfrac{m^2 + n^2 - a_2^2 - a_3^2}{2 a_2 a_3} \right) 

求\theta_2

\begin{aligned}
a_{2} \cos{\left(\theta_{2} \right)} + a_{3} \cos{\left(\theta_{2} + \theta_{3} \right)} &= m \\
-a_{2} \sin{\left(\theta_{2} \right)} - a_{3} \sin{\left(\theta_{2} + \theta_{3} \right)} &= n
\end{aligned}

在上述方程中，已经求出了\theta_3，故未知变量只有\theta_2，将上面两式展开，得

\begin{aligned}
\left( a_{2} + a_{3} \cos{\left(\theta_{3} \right)} \right) \cos{\left(\theta_{2} \right)} &- a_{3} \sin{\left(\theta_{3} \right)} \sin{\left(\theta_{2} \right)} = m \\
-a_{3} \sin{\left(\theta_{3} \right)} \cos{\left(\theta_{2} \right)} &- \left( a_{2} + a_{3} \cos{\left(\theta_{3} \right)} \right) \sin{\left(\theta_{2} \right)}  = n
\end{aligned}

为了方便，作如下代换

\begin{aligned}
    a_{2} + a_{3} \cos{\left(\theta_{3} \right)} &= k_1 \\
    -a_{3} \sin{\left(\theta_{3} \right)} &= k_2
\end{aligned}

则上述方程可写为

\begin{aligned}
    k_1 \cos{\theta_2} + k_2 \sin{\theta_2} &= m \\
    k_2 \cos{\theta_2} - k_1 \sin{\theta_2} &= n
\end{aligned}

看作关于\sin \theta_2 和 \cos \theta_2的方程组，可以解出

\begin{aligned}
    \sin \theta_2 &= \dfrac{mk_2 - nk_1}{k_1^2 + k_2^2} \\
    \cos \theta_2 &= \dfrac{mk_1 + nk_2}{k_1^2 + k_2^2}
\end{aligned}

两式相除，即可解出\theta_2

\theta_2 = Atan2\left( {mk_2 - nk_1}, {mk_1 + nk_2} \right)

求\theta_4

注意(2)处矩阵两边的元素(1, 2)和(3, 2)，其中左边矩阵元素均已知，为了方便，分别记左边矩阵(1, 2)和(3, 2)处元素为p, q，即

\begin{aligned}
\left(r_{11} \cos{\theta_{1} } + r_{21} \sin{\theta_{1}}\right) \sin{\theta_{6} } + \left(r_{12} \cos{\theta_{1} } + r_{22} \sin{\theta_{1}}\right) \cos{\theta_{6}} &= p \\
r_{31} \sin{\theta_{6}} + r_{32} \cos{\theta_{6}} &= q
\end{aligned}

令矩阵左右两边元素对应相等，即有

\begin{aligned}
    - \sin{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} &= p \\
    - \cos{\left(\theta_{2} + \theta_{3} + \theta_{4} \right)} &= q
\end{aligned}

可解出\theta_4

 \theta_4 = Atan2(-p, -q) - \theta_2 - \theta_3 

总结

已知目标齐次矩阵^0_6T

 \left[\begin{matrix}r_{11} & r_{12} & r_{13} & p_{x}\\r_{21} & r_{22} & r_{23} & p_{y}\\r_{31} & r_{32} & r_{33} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right] = ^0_6T 

已知D-H参数表如下：

axis i	\alpha_{i-1}	a_{i-1}	d_i	\theta_i
1	0	0	0	\theta_1 = 0
2	-\frac{\pi}{2}	0	d_2=116.5	\theta_2 = -\frac{\pi}{2}
3	0	a_2=203.5	0	\theta_3 = 0
4	0	a_3=173	0	\theta_4 = \frac{\pi}{2}
5	\frac{\pi}{2}	0	d_5 = 79.2	\theta_5 = 0
6	-\frac{\pi}{2}	0	0	\theta_6 = 0

可解出其6个关节角为：

\begin{aligned}
    \theta_1 &= Atan2(p_y, p_x) - Atan2(d_2, \pm \sqrt{p_x^2 + p_y^2 - d_2^2}) \\
    \theta_5 &= \pm \arccos \left( - r_{13} \sin{\theta_{1}} + r_{23} \cos{\theta_{1}} \right) \\
    \theta_6 &= Atan2 \left( \dfrac{-r_{12} \sin{\left(\theta_{1} \right)} + r_{22} \cos{\left(\theta_{1} \right)}}{-\sin(\theta_5)} , \dfrac{-r_{11} \sin{\left(\theta_{1} \right)} + r_{21} \cos{\left(\theta_{1} \right)}}{\sin(\theta_5)} \right) \\
    \theta_3 &= \pm \arccos \left( \dfrac{m^2 + n^2 - a_2^2 - a_3^2}{2 a_2 a_3} \right) \\
    \theta_2 &= Atan2\left( {mk_2 - nk_1}, {mk_1 + nk_2} \right) \\
    \theta_4 &= Atan2(-p, -q) - \theta_2 - \theta_3
\end{aligned}

其中代换变量为：

\begin{aligned}
    m &= d_{5} \left(\left(r_{11} \cos{\theta_{1}} + r_{21} \sin{\theta_{1}}\right) \sin{\theta_{6}} + \left(r_{12} \cos{\theta_{1}} + r_{22} \sin{\theta_{1}}\right) \cos{\theta_{6}}\right) + p_{x} \cos{\theta_{1}} + p_{y} \sin{\theta_{1}} \\
    n &= d_{5} \left(r_{31} \sin{\left(\theta_{6} \right)} + r_{32} \cos{\left(\theta_{6} \right)}\right) + p_{z} \\
    k_1 &= a_{2} + a_{3} \cos{\left(\theta_{3} \right)} \\
    k_2 &= -a_{3} \sin{\left(\theta_{3} \right)} \\
    p &= \left(r_{11} \cos{\theta_{1} } + r_{21} \sin{\theta_{1}}\right) \sin{\theta_{6} } + \left(r_{12} \cos{\theta_{1} } + r_{22} \sin{\theta_{1}}\right) \cos{\theta_{6}} \\
    q &= r_{31} \sin{\theta_{6}} + r_{32} \cos{\theta_{6}}
\end{aligned}

由于\theta_1, \theta_5, \theta_3中存在正负号，因此共有8组解

第7轴

假设有一个末端执行器被安装在机械臂末端，这将形成一个虚假的第7轴，这个轴一般无法造成旋转，只会在第6轴的轴线上形成一段连杆偏距d_7。因此，可以得到末端执行器相对第6个坐标系的齐次变换矩阵_7^6T

_7^6T = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & d_{7}\\0 & 0 & 0 & 1\end{matrix}\right]

当安装了末端执行机构后，我们给定的是末端执行机构的空间位姿，也就是说给定的目标齐次变换矩阵是_7^0T，我们需要将目标齐次变换矩阵转换成_6^0T，来应用上述的逆运动学解，转换方式非常简单，右乘_7^6T^{-1}即可

_6^0T = _7^0T \times _7^6T^{-1} = _7^0T \times \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & - d_{7}\\0 & 0 & 0 & 1\end{matrix}\right]

我们现在假设末端执行机构被安装在第6个电机的法兰盘中心，即d_7 = 42