六自由度机器人逆运动学详解及 Matlab 代码

末端执行器的位姿为 $\vec{n}$、$\vec{o}$、$\vec{a}$、$\vec{p}$，问题也就转变为知道这些参量，来求解 $\theta$ 这 6 个关节变量。过程就是用未知的连杆逆变换来左乘上式的等号两边，这样就能把关节变量分离出来，进而就能求解，具体如下：

① $\theta _{1}$ 求解

将一般变换方程两端左乘 ${}^0_1T^{-1}\left (\theta _{1} \right )$ 得到下式，这样左侧就分离出只有 $\theta _{1}$ 的变量：

${}^0_1T^{-1}\left (\theta _{1} \right ){}^0_6T={}^1_2T\left ( \theta _{2} \right ){}^2_3T\left ( \theta _{3} \right ){}^3_4T\left ( \theta _{4} \right ){}^4_5T\left ( \theta _{5} \right ){}^5_6T\left ( \theta _{6} \right )$

补充：如何求逆？

我们已知齐次变换矩阵实际是由旋转矩阵与平移向量的复合。套用公式，其实际上变换矩阵的逆矩阵为：让对应的旋转矩阵处求逆（也就是转置），位移列向量处左乘一个负的旋转矩阵的逆（转置）。

结合上面补充分析，等号左右两式矩阵可得下式：

具体计算过程：

想要逆解出变量结果，就需要真正算出左侧两矩阵相乘后的结果（主要是为了得到 $p_x, p_y, p_z$），这里我用 Matlab 确实具体算了一遍，结果如下：

①首先先算一下 $n_x, n_y, n_z$ 和的值：

（此处省略详细矩阵乘法步骤，直接给出结果）

以上是用齐次变换矩阵定义，利用 matlab 计算出的。

---

②所以计算上面式等号左边两红框相乘的结果：

（此处省略详细展开步骤）

这一步就是按部就班代入、乘开这个式子，然后合并消除后就只剩下了 $ heta_1$，不要被大长式子吓到。

所以可以得到：

---

③同理也可以去计算矩阵两端（1，4）和（3，4）的元素：

（此处省略计算细节）

( $n_x$ 就是 $cos\theta_1$, $n_y$ 就是 $sin\theta_1$, ... 这里不赘述。)

式中：

$^{1}p_{y}=d_{2}$

之后令等号两端的（2，4）元素相等（红框已经圈出）：

**补充：**三角代换知识。如何求 $ heta_1$ 中的 $ heta_1$？

解：利用中间变量进行变换

其中，$p_x = \rho cos\phi$，$p_y = \rho sin\phi$。

所以：

通过三角代换：

$p_{x}=\rho cos\phi$，$p_{y}=\rho sin\phi$。其中 $\rho =\sqrt{p_{x}^{2}+p_{y}^{2}}$，$\phi =Atan2\left ( p_{x}, p_{y}\right )$。

代入后得到：

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

式中，正负号代表了 $\theta _{1}$ 的两个可能解。

② $\theta _{3}$ 求解

选定 $\theta _{1}$ 一个解之后，再令 ${}^1_6T$ 两端元素（1，4）与（3，4）分别相等，可以得到下面方程组：

$\left{ \begin{aligned} c_1 p_x + s_1 p_y &= a_3 c_{23} - d_4 s_{23} + a_2 c_2 \ p_z &= -\left [ a_3s_{23}+d_4c_{23}-a_2s_{2} \right ] \end{aligned} \right.$

之后让 $-s_1p_x+c_1p_y=d_2$ 与此方程组平方求和，可以得到：

$a_3c_3-d_4s_3=k$

式中，$k = \frac{p_x^2 + p_y^2 + p_z^2 - a_2^2 - a_3^2 - d_2^2 - d_4^2}{2 a_2 }$

这样就可以实现式中只有一个变量 $\theta _{3}$，并且这个等式形式和上面求 $\theta _{1}$ 时一致，所以同理：

$\theta_3 = \text{Atan2}(a_3, d_4) - \text{Atan2}\left(k, \pm \sqrt{a_3^2 + d_4^2 - k^2}\right)$

式中，正负号代表了 $\theta _{3}$ 的两个可能解。

③ $\theta _{2}$ 求解

将一般变换方程等式两边左乘一个 ${}^0_3T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} \right )$，如下：

${}^0_3T^{-1}\left (\theta _{1} ,\theta _{2},\theta _{3}\right ){}^0_6T={}^3_4T\left ( \theta _{4} \right ){}^4_5T\left ( \theta _{5} \right ){}^5_6T\left ( \theta _{6} \right )$

这样就可以使得等号左侧只有三个变量，而前面求了 $\theta _{1}$ 与 $\theta _{3}$，因此就只有 $\theta _{2}$ 这一个变量了。等号左右两式矩阵可得下式：

通过计算可以得到：

（此处省略矩阵展开）

然后再根据矩阵求逆的方法去计算，这里不再赘述~

上式中等号左边的矩阵就是计算后的 ${}^0_3T^{-1}$ (这里简化表达)，然后令上式的（1，4）与（2，4）元素相等进行计算，最后合并消除可以得到：

$\theta_{23} = \theta_2 + \theta_3 = \text{atan2}\left[ -(a_3 + a_2 c_3) p_z + (c_1 p_x + s_1 p_y)(a_2 s_3 - d_4), (-d_4 + a_2 s_3) p_x + (c_1 p_x + s_1 p_y)(a_2 c_3 + a_3) \right]$

$\theta _{2}=\theta _{23}-\theta _{3}$

其中 $\theta _{3}$ 的解与 $\theta _{1}$ 和 $\theta _{2}$ 有关，因此得出的 $\theta _{3}$ 有四种可能的解。

④ $\theta _{4}$ 求解

因为上面 ${}^3_6T$ 左式均为已知，令两边的元素（1，3）和（3，3）分别相等，则可以得到：

$\left{ \begin{aligned} a_x c_{1} c_{23} + a_y s_{1} c_{23} - a_z s_{23} &= -c_4 s_5 \ -a_x s_y + a_y c_1 &= s_4 s_5 \end{aligned} \right.$

这里面只要 $s_{5}$ 不为 0，消去之后就可以得到 $\theta _{4}$：

$\theta_4 = \text{Atan2}\left( -a_4 s_1 + a_y c_1, -a_x c_1 c_{23} - a_y s_1 c_{23} + a_z s_{23} \right)$

如果 $s_{5}=0$，此时机械臂处于奇异位置，这时候关节 4 与 6 重合，不能解出来具体值，这个奇异值我们后面讨论。

⑤ $\theta _{5}$ 求解

可以让一般变换方程等式两边左乘一个 ${}^0_4T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} , \theta _{4} \right )$，如下：

${}^0_4T^{-1}\left (\theta _{1} ,\theta _{2},\theta _{3} ,\theta _{4} \right ){}^0_6T= {}^4_5T\left ( \theta _{5} \right ){}^5_6T\left ( \theta _{6} \right )$

这时候前面四个变量都解出来了，所以左侧只有 $\theta _{5}$ 一个变量未知。因此可以求出 ${}^0_4T^{-1}$ 矩阵：

$\left[ \begin{array}{cccc} c_1 c_{23} c_4 + s_1 s_4 & s_1 c_{23} c_4 - c_1 s_4 & -s_{23} c_4 & -a_2 c_3 c_4 + d_2 s_4 - a_3 c_4 \ -c_1 c_{23} s_4 + s_1 c_4 & -s_1 c_{23} s_4 - c_1 c_4 & s_{23} s_4 & a_2 c_3 s_4 + d_2 c_4 + a_3 s_4 \ -c_1 s_{23} & -s_1 s_{23} & -c_{23} & a_2 s_3 - d_4 \ 0 & 0 & 0 & 1 \end{array} \right]$

同理，可以让等式两侧元素（1，3）和（3，3）分别相等，可以得到：

$\left{ \begin{aligned} a_x \left( c_1 c_{23} c_4 + s_1 s_4 \right) + a_y \left( s_1 c_{23} c_4 - c_1 s_4 \right) - a_z \left( s_{23} c_4 \right) &= -s_5 \ a_x \left( -c_1 s_{23} \right) + a_y \left( -s_1 s_{23} \right) + a_z \left( -c_{23} \right) &= c_5 \end{aligned} \right.$

由此得到 $\theta _{5}$：

$\theta _5=Atan2\left ( s_5, c_5 \right )$

⑥ $\theta _{6}$ 求解

同理让一般变换方程等式两边左乘一个 ${}^0_5T^{-1}\left (\theta _{1}, \theta _{2} ,\theta _{3} , \theta _{4} , \theta _{5} \right )$，如下：

${}^0_5T^{-1}\left (\theta _{1} ,\theta _{2},\theta _{3} ,\theta _{4} , \theta _{5} \right ){}^0_6T= {}^5_6T\left ( \theta _{6} \right )$

通过上面等式（3，1）与（1，1）的元素相等，可以得到 $\theta _{6}$：

$\begin{cases} -n_x \left( c_1 c_{23} s_4 - s_1 c_4 \right) - n_y \left( s_1 c_{23} s_4 + c_1 c_4 \right) + n_z \left( s_{23} s_4 \right) = s_6 \ n_x \left[ \left( c_1 c_{23} c_4 + s_1 s_4 \right) c_5 - c s_{23} s_5 \right] + n_y \left[ \left( s_1 c_{23} c_4 - c_1 s_4 \right) c_5 - s_1 s_{23} s_5 \right] - n_z \left( s_{23} c_4 c_5 + c_{23} s_5 \right) = c_6 \end{cases}$

$\theta _6=Atan2\left ( s_6, c_6 \right )$

至此六个变量求解完成，以上存在多解的关节还需要考虑关节结构，是否能够到达这一位置，如果因结构限制则可能这对应的解不存在。

二、Matlab 求解过程

1. 整体代码

先把具体代码展示如下：

clc; clear; %输入已知的位姿矩阵（正解可得）---------------------- a=[ 0.0000 0.0000 1.0000 800.0000; -0.0000 -1.0000 0.0000 120.0000; 1.0000 -0.0000 -0.0000 10.0000; 0 0 0 1.0000 ]; %------------------------------------------------- %定义连杆的 D-H 参数 %连杆偏移 d1 = 0; d2 = 120; d3 = 0; d4 = 400; d5 = 0; d6 = 0; %连杆长度 a2 = 400; a3 = 10; %连杆扭角 alpha1 = 0; alpha2 = -pi/2; alpha3 = 0; alpha4 = -pi/2; alpha5 = pi/2; alpha6 = -pi/2; %建立机器人模型 % theta d a alpha L1=Link([0 d1 0 alpha1 ],'modified'); L2=Link([0 d2 0 alpha2 ],'modified'); L3=Link([0 d3 a2 alpha3 ],'modified'); L4=Link([0 d4 a3 alpha4 ],'modified'); L5=Link([0 d5 0 alpha5 ],'modified'); L6=Link([0 d6 0 alpha6 ],'modified'); %限制关节空间 L1.qlim = [(-165/180)*pi,(165/180)*pi]; L2.qlim = [(-150/180)*pi, (60/180)*pi]; L3.qlim = [(-150/180)*pi, (90/180)*pi]; L4.qlim = [(-180/180)*pi,(180/180)*pi]; L5.qlim = [(-115/180)*pi,(115/180)*pi]; L6.qlim = [(-360/180)*pi,(360/180)*pi]; %连接连杆，机器人取名为 robot robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','6Rrobot'); robot.display(); robot.teach(); nx=a(1,1);ox=a(1,2);ax=a(1,3);px=a(1,4); ny=a(2,1);oy=a(2,2);ay=a(2,3);py=a(2,4); nz=a(3,1);oz=a(3,2);az=a(3,3);pz=a(3,4); %解关节 1 theta1_1 = atan2(py,px)-atan2(d2,abs(sqrt(px^2+py^2-d2^2))); theta1_2 = atan2(py,px)-atan2(d2,-abs(sqrt(px^2+py^2-d2^2))); %解关节 3 kk 为中间值 kk = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2); theta3_1 = atan2(a3,d4)-atan2(kk,abs(sqrt(a3^2+d4^2-kk^2))); theta3_2 = atan2(a3,d4)-atan2(kk,-abs(sqrt(a3^2+d4^2-kk^2))); %解关节 2 ko2 为 theta23 计算式中第一部分，kt2 为第二部分 % theta1_1 theta3_1 对应解 ko2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_1)-d4); kt2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_1)+a3); theta23_1 = atan2(ko2_1,kt2_1); theta2_1 = theta23_1 - theta3_1; % theta1_2 theta3_1 对应解 ko2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_1)-d4); kt2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_1)+a3); theta23_2 = atan2(ko2_2,kt2_2); theta2_2 = theta23_2 - theta3_1; % theta1_1 theta3_2 对应解 ko2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_2)-d4); kt2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_2)+a3); theta23_3 = atan2(ko2_3,kt2_3); theta2_3 = theta23_3 - theta3_2; % theta1_2 theta3_2 对应解 ko2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_2)-d4); kt2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_2)+a3); theta23_4 = atan2(ko2_4,kt2_4); theta2_4 = theta23_4 - theta3_2; % 求解关节 4 % theta1_1 theta2_1 theta3_1 对应解 ko4_1=-ax*sin(theta1_1)+ay*cos(theta1_1); kt4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*cos(theta23_1)+az*sin(theta23_1); theta4_1=atan2(ko4_1,kt4_1); % theta1_2 theta2_2 theta3_1 对应解 ko4_2=-ax*sin(theta1_2)+ay*cos(theta1_2); kt4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*cos(theta23_2)+az*sin(theta23_2); theta4_2=atan2(ko4_2,kt4_2); % theta1_1 theta2_3 theta3_2 对应解 ko4_3=-ax*sin(theta1_1)+ay*cos(theta1_1); kt4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*cos(theta23_3)+az*sin(theta23_3); theta4_3=atan2(ko4_3,kt4_3); % theta1_2 theta2_4 theta3_2 对应解 ko4_4=-ax*sin(theta1_2)+ay*cos(theta1_2); kt4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*cos(theta23_4)+az*sin(theta23_4); theta4_4=atan2(ko4_4,kt4_4); % 求解关节 5 % theta1_1 theta2_1 theta3_1 theta4_1 对应解 ko5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))-ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))+az*(sin(theta23_1)*cos(theta4_1)); kt5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))+az*(-cos(theta23_1)); theta5_1=atan2(ko5_1,kt5_1); % theta1_2 theta2_2 theta3_1 theta4_2 对应解 ko5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))-ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))+az*(sin(theta23_2)*cos(theta4_2)); kt5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))+az*(-cos(theta23_2)); theta5_2=atan2(ko5_2,kt5_2); % theta1_1 theta2_3 theta3_2 theta4_3 对应解 ko5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))-ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))+az*(sin(theta23_3)*cos(theta4_3)); kt5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))+az*(-cos(theta23_3)); theta5_3=atan2(ko5_3,kt5_3); % theta1_2 theta2_4 theta3_2 theta4_4 对应解 ko5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))-ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))+az*(sin(theta23_4)*cos(theta4_4)); kt5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))+az*(-cos(theta23_4)); theta5_4=atan2(ko5_4,kt5_4); % 求解关节角 6 % theta1_1 theta2_1 theta3_1 theta4_1 theta5_1 对应解 ko6_1=-nx*(cos(theta1_1)*cos(theta23_1)*sin(theta4_1)-sin(theta1_1)*cos(theta4_1))-ny*(sin(theta1_1)*cos(theta23_1)*sin(theta4_1)+cos(theta1_1)*cos(theta4_1))+nz*(sin(theta23_1)*sin(theta4_1)); kt6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1)); theta6_1=atan2(ko6_1,kt6_1); % theta1_2 theta2_2 theta3_1 theta4_2 theta5_2 对应解 ko6_2=-nx*(cos(theta1_2)*cos(theta23_2)*sin(theta4_2)-sin(theta1_2)*cos(theta4_2))-ny*(sin(theta1_2)*cos(theta23_2)*sin(theta4_2)+cos(theta1_2)*cos(theta4_2))+nz*(sin(theta23_2)*sin(theta4_2)); kt6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)+cos(theta23_2)*sin(theta5_2)); theta6_2=atan2(ko6_2,kt6_2); % theta1_1 theta2_3 theta3_2 theta4_3 theta5_3 对应解 ko6_3=-nx*(cos(theta1_1)*cos(theta23_3)*sin(theta4_3)-sin(theta1_1)*cos(theta4_3))-ny*(sin(theta1_1)*cos(theta23_3)*sin(theta4_3)+cos(theta1_1)*cos(theta4_3))+nz*(sin(theta23_3)*sin(theta4_3)); kt6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)+cos(theta23_3)*sin(theta5_3)); theta6_3=atan2(ko6_3,kt6_3); % theta1_2 theta2_4 theta3_2 theta4_4 theta5_4 对应解 ko6_4=-nx*(cos(theta1_2)*cos(theta23_4)*sin(theta4_4)-sin(theta1_2)*cos(theta4_4))-ny*(sin(theta1_1)*cos(theta23_4)*sin(theta4_4)+cos(theta1_2)*cos(theta4_4))+nz*(sin(theta23_4)*sin(theta4_4)); kt6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)+cos(theta23_4)*sin(theta5_4)); theta6_4=atan2(ko6_4,kt6_4); % 整理得到 4 组运动学非奇异逆解 theta_MOD1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1; theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2; theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3; theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4; ]*(180/pi); % 将操作关节'翻转'可得到另外 4 组解 theta_MOD2 = ... [ theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi; theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi; theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi; theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi; ]*(180/pi); %输出关节变量值 J = [theta_MOD1;theta_MOD2]

上面需要我们输入改变的是 a 矩阵的具体内容，这个 a 实际上就是齐次变换矩阵 ${}^0_6T$。如果要求逆解，那么这个矩阵是知道的。

2. 举例分析正确性

先看上面代码我的 a 是什么条件下得来的，这是我的转角变量所赋值的内容：

这是对应变量下的齐次变换矩阵 ${}^0_6T$：

现在假设我们有了这个齐次变换矩阵 ${}^0_6T$ 的信息，我们把这个数据替换掉上面代码的 a 矩阵，然后运行程序：

至此完成了逆运动学解算~