第二章运动学 - 2.3类ur，二三四轴轴线平行解析解

本文链接：https://blog.csdn.net/hq1215758640/article/details/140299599

这边对另一种符合Piper准则的机器人构型进行运动学分析，与传统的工业机器人相比，此类机械臂关注焦点在于关节二、三、四的轴线平行，并不像传统机械臂种的关节四、五、六轴线交于一点，但是根据Piper准则来说，存在解析解。这也是大多数协作机械臂的基础，下面就对类UR机械臂进行建模分析。

图2.3.1 类ur机械臂DH模型

根据2.1节相关建模方式，可以对类ur类型的机械模型建立如上图所示，建立的时候可以根据一边建坐标系一边写DH模型的方法来写出相应的DH模型表，如下：

（二）、正解

根据上一章节，我们同理可以得到正解：

${{}_{~}^{0}T_{1}} = R_x(\alpha_0)\cdot T_x(a_0)\cdot R_z(\theta_1)\cdot T_z(d_1)$ (2-3-1)

同理可以得到 ${{}_{~}^{1}T_{2}}$ 、 ${{}_{~}^{2}T_{3}}$ 、 ${{}_{~}^{3}T_{4}}$ 、 ${{}_{~}^{4}T_{5}}$ 、 ${{}_{~}^{5}T_{6}}$ 、将这些矩阵相乘即可得到 ${{}_{~}^{0}T_{6}}$ 、

${{}_{~}^{0}T_{6}} = {{}_{~}^{0}T_{1}} \cdot {{}_{~}^{1}T_{2}} \cdot {{}_{~}^{2}T_{3}} \cdot {{}_{~}^{3}T_{4}} \cdot {{}_{~}^{4}T_{5}} \cdot {{}_{~}^{5}T_{6}}$ (2-3-2)

关于奇异，协作机械臂的奇异跟前一章节的奇异类似，公式也差不多，唯一的区别在于肘关节的臂型有所区别，肘关节奇异定义是，关节二轴、三轴、四轴共线，此时关节二的逆解无法解出，其实也就是关节三为0时奇异。根据奇异，可分8种臂型，只不过，一般协作机械臂正逆解，因为是解析解的缘故，一般不会提及8种臂型，但奇异仍然存在。

（三）、逆解（解析解）

我们先将每个关节的 ${{}_{~}^{i-1}T_{i}}$ 矩阵写出来如下：

${{}_{~}^{0}T_{1}} = ~\begin{bmatrix} \begin{matrix} c_1 \\ s_1 \\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} -s_1 \\ c_1 \\ 0 \\ 0\end{matrix} & \begin{matrix} 0 \\ 0 \\ 1\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \\ d_1 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ ${{}_{~}^{1}T_{2}} = ~\begin{bmatrix} \begin{matrix} -c_2 \\ 0 \\ -s_2 \\0 \end{matrix} & \begin{matrix} \begin{matrix} s_2 \\ 0 \\ -c_2 \\ 0\end{matrix} & \begin{matrix} 0 \\ -1 \\ 0\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$

${{}_{~}^{2}T_{3}} = ~\begin{bmatrix} \begin{matrix} c_3 \\ s_3 \\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} -s_3 \\ c_3 \\ 0 \\ 0\end{matrix} & \begin{matrix} 0 \\ 0 \\ 1\\ 0\end{matrix} & \begin{matrix} \begin{matrix} a_2 \\ 0 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ ${{}_{~}^{3}T_{4}} = ~\begin{bmatrix} \begin{matrix} c_4 \\ s_4 \\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} -s_4 \\ c_4 \\ 0 \\ 0\end{matrix} & \begin{matrix} 0 \\ 0 \\ 1\\ 0\end{matrix} & \begin{matrix} \begin{matrix} a_3 \\ 0 \\ d_4 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$

${{}_{~}^{4}T_{5}} = ~\begin{bmatrix} \begin{matrix} 1\\ 0\\ 0 \\0 \end{matrix} & \begin{matrix} \begin{matrix} 0 \\ 0\\ 1 \\ 0\end{matrix} & \begin{matrix} 0 \\ 1 \\ 0\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0\\ d_5 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ ${{}_{~}^{5}T_{6}} = ~\begin{bmatrix} \begin{matrix} -c_6 \\ 0 \\ -s_6 \\0 \end{matrix} & \begin{matrix} \begin{matrix} s_6 \\ 0 \\ -c_6 \\ 0\end{matrix} & \begin{matrix} 0 \\ -1 \\ 0\\ 0\end{matrix} & \begin{matrix} \begin{matrix} 0 \\ -d_6 \\ 0 \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ (2-3-3)

令

${{}_{~}^{0}T_{6}}=~\begin{bmatrix} \begin{matrix} n_x \\ n_y \\ n_z \\0 \end{matrix} & \begin{matrix} \begin{matrix} o_x \\ o_y \\ o_z \\ 0\end{matrix} & \begin{matrix} a_x \\ a_y \\ a_z\\ 0\end{matrix} & \begin{matrix} \begin{matrix} p_x \\ p_y \\ p_z \\ 1\end{matrix} & \\ \end{matrix} \\ \end{matrix} \\ \end{bmatrix}$ (2-3-4)

有等式如下：

${{}_{~}^{0}T_{1}}^{-1}\cdot{{}_{~}^{0}T_{6}} \cdot{{}_{~}^{5}T_{6}}^{-1}= {{}_{~}^{1}T_{5}}={{}_{~}^{1}T_{2}}\cdot{{}_{~}^{2}T_{3}}\cdot{{}_{~}^{3}T_{4}}\cdot{{}_{~}^{4}T_{5}}$ (2-3-5)

（1）、解 $\theta_1$

将式（2-3-3）带入到（2-3-5），通过比较就可以发现，两边矩阵第二行第四列有如下等式：

$-d_4=c_1 p_y-p_x s_1-d_6 (a_y c_1-a_x s_1)$ (2-3-6)

整理得：

$-d_4=c_1 m-p_x n ,$ $m =p_y-d_6 a_y ,n =d_6 a_x-p_x$ (2-3-7)

解出：

$\theta_1= ±atan2(-d_4,sqrt(m^2+n^2-d_4^2 ))-atan2(m,n)$ (2-3-8)

（2）、解 $\theta_5$

同时经过（2-3-5）可以得到第二行第一列，第二行第三列有如下等式：

$c_5= a_x s_1-a_y c_1$ (2-3-9)

那么可得到：

$\theta_5= atan2(s_5,c_5 ),s_5= \pm \sqrt{(1-c_5^2 )}$ (2-3-10)

（3）、解 $\theta_6$

$s_5= s_6 (c_1 o_y-o_x s_1 )-c_6 (c_1 n_y-n_x s_1) -> s_5=s_6 m-c_6 n$ (2-3-11)

$0 = c_6(c_1o_y -o_xs_1)-s_6(c_1 n_y -n_x s_1) ->0 = -c_6 m-s_6n$ (2-3-12)

所以：

$\theta_6=atan2(n,m)+atan2(s_5,0)$ (2-3-13)

（4）、解 $\theta_3$

解完4、5、6轴之后，将上式再次相乘：

${{}_{~}^{0}T_{1}}^{-1}\cdot{{}_{~}^{0}T_{6}} \cdot{{}_{~}^{5}T_{6}}^{-1}\cdot{{}_{~}^{4}T_{5}}^{-1}= {{}_{~}^{1}T_{4}}={{}_{~}^{1}T_{2}}\cdot{{}_{~}^{2}T_{3}}\cdot{{}_{~}^{3}T_{4}}$ (2-3-14)

得到(2-3-14)第一行第四列，和第三行第四列有：

$c_2a_2+a_3 c_{23}= x,$ $s_2 a_2+a_3 s_{23}= y$ (2-3-15)

这里的x和y表示一大长串的关于1、5、6轴的表达式

将（2-3-15）的两式平方可得：

$x^2+y^2=a_2^2+a_3^2+2a_2 a_3 c_3$ $->$ $\theta_3 =arccos((x^2+y^2-a_2^2-a_3^3)/2(a_2a_3))$ (2-3-16)

（5）、解 $\theta_2$

将 $\theta_2$ 带入(2-3-15)，得到二元一次方程组如下：

$\begin{bmatrix} \begin{matrix} c_2\\ s_2\end{matrix} \end{bmatrix} \cdot ~\begin{bmatrix} \begin{matrix} A\\ C\end{matrix} & \begin{matrix} \begin{matrix} B\\ D\end{matrix} \end{matrix} \end{bmatrix}=\begin{bmatrix} \begin{matrix} M\\ N\end{matrix} \end{bmatrix}$ (2-3-17)

其中 $A=a_2+a_3 c_3,B= -a_3 s_3,C= -s_3 a_3,D=a_2+a_3 c_3,M=x,N=y$

得到 $c_2=(DM-NB)/(AD-BC),s_2=(AN-CM)/(AD-BC)$

$\theta_2=atan2(s_2,c_2)$ (2-3-18)

（6）、解 $\theta_4$

式子(2-3-15)中根据矩阵第一行第二列和第三行第二列可以得到：

$-c_6 (c_1 o_x+o_y s_1 )-s_6 (c_1 n_x+n_y s_1 )=s_{234}$

$c_6 o_z+n_z s_6=c_{234}$ (2-3-19)

$\theta_4=atan2(s_{234},c_{234} )-\theta_3-\theta_2$ (2-3-20)

注意，在求 $\theta_6$ 时，(2-3-13)可以看出，当 $\theta_5=0$ 或者 $n\pi$ 时，矩阵某些项为0，在求别的关节时存在正余弦值为0，需要注意，也可以理解为奇异。

另外，根据上面公式，可以看出， $\theta_1$ 有两个解， $\theta_5$ 有四个解， $\theta_6$ 有四个解， $\theta_2,\theta_3,\theta_4$ 有八个解，最后关于解的选取，这里没有一个确定的方法，可根据实际情况，可以由最小角度或者最小能量，或者臂型来选取，甚至还有些是旋转±360度之后，可以产生更多选项的解，所以这块可以根据厂商的要求自己来定义。

完成上述计算之后，程序如下：

公式推导：

syms nx ny nz ox oy oz ax ay az px py pz;
syms d1 a2 a3 d4 d5 d6;
syms q1 q2 q3 q4 q5 q6;
syms c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6
syms T01 T12 T23 T34 T45 T56;
syms T01_1 T12_1 T23_1 T34_1 T45_1 T56_1;
%% q1:rotz
T01 = [c1 -s1 0 0;s1 c1 0 0;0 0 1 d1; 0 0 0 1];
T01_1 = [c1 s1 0 0; -s1 c1 0 0;0 0 1 -d1; 0 0 0 1]; 
%% q2:rotx(90)*rotz(pi+theta);
T12 = [-c2 s2 0 0;0 0 -1 0; -s2 -c2 0 0;0 0 0 1;];
T12_1 = [-c2 0 -s2 0; s2 0 -c2 0;0 -1 0 0;0 0 0 1];
%% q3:transx(a2)*rotz(theta)
T23 = [c3 -s3 0 a2;s3 c3 0 0;0 0 1 0; 0 0 0 1;];
%% q4:transx(a3)*rotz(theta)
T34 = [c4 -s4 0 a3; s4 c4 0 0;0 0 1 d4;0 0 0 1;];
%% q5:rotx(-90)*rotz(theta)*transz(d5);
T45 = [c5 -s5 0 0; 0 0 1 d5;-s5 c5 0 0; 0 0 0 1;];
T45_1 = [c5 0 s5 0;s5 0 c5 0;0 1 0 -d5; 0 0 0 1];
%% q6:rotx(pi/2)*rotz(theta+pi)*transz(d6);
T56 = [-c6 s6 0 0; 0 0 -1 -d6;-s6 -c6 0 0;0 0 0 1];
T56_1 = [-c6 0 -s6 0;s6 0 -c6 0;0 -1 0 -d6;0 0 0 1];
T06 = [nx ox ax px;ny oy ay py;nz oz az pz; 0 0 0 1];
%公式：(2-3-5)
T01_1*T06*T56_1
T12*T23*T34*T45
%公式：(2-3-14)
T01_1*T06*T56_1*T45_1
T12*T23*T34
程序2.3.1协作机械臂公式推导

程序：正解：

function [T] = ur5_fkine(q,DH,q_option)
    if strcmp(q_option,'deg')
        q = (q+DH.theta)*pi/180.0;
    elseif strcmp(q_option,'rad')
       ;
    else
        q = (q+DH.theta)*pi/180.0;
    end
    if strcmp(q_option,'deg')
        alpha= DH.alpha*pi/180.0;    
    elseif strcmp(q_option,'rad')
        ;
    else
        alpha= DH.alpha*pi/180.0;
    end
    T01 = create_link(q(1),DH.d(1),alpha(1),DH.a(1),'rad');
    T12 = create_link(q(2),DH.d(2),alpha(2),DH.a(2),'rad');
    T23 = create_link(q(3),DH.d(3),alpha(3),DH.a(3),'rad');
    T34 = create_link(q(4),DH.d(4),alpha(4),DH.a(4),'rad');
    T45 = create_link(q(5),DH.d(5),alpha(5),DH.a(5),'rad');
    T56 = create_link(q(6),DH.d(6),alpha(6),DH.a(6),'rad');
    T = T01*T12*T23*T34*T45*T56;
end
程序2.3.2协作机械臂正解

反解：

function [q] = ur5_ikine(DH,T)
    d1 = DH.d(1);
    d4 = DH.d(4); d5 = DH.d(5); d6 = DH.d(6);
    a2 = DH.a(3); a3 = DH.a(4);
    nx = T(1,1); ny = T(2,1); nz = T(3,1);
    ox = T(1,2); oy = T(2,2); oz = T(3,2);
    ax = T(1,3); ay = T(2,3); az = T(3,3);
    px = T(1,4); py = T(2,4); pz = T(3,4);
    m = py-d6*ay;n = d6*ax-px;
    
    %q1:
    q1(1) = atan2(-d4,sqrt(m^2 + n^2-d4^2))-atan2(m,n);
    q1(2) = atan2(-d4,-sqrt(m^2 + n^2-d4^2))-atan2(m,n);
    qa = atan2(-ax,ay)*180/pi
 
    %q5:
    c1 = cos(q1(1));s1 = sin(q1(1));
    c5 = ax*s1 - ay*c1;s5_1 = sqrt(1-c5^2);s5_2 = -sqrt(1-c5^2);
    q5(1) = atan2(s5_1,c5); q5(2) = atan2(s5_2,c5);  
    c1 = cos(q1(2));s1 = sin(q1(2));
    c5 = ax*s1 - ay*c1;s5_1 = sqrt(1-c5^2);s5_2 = -sqrt(1-c5^2);
    q5(3) = atan2(s5_1,c5); q5(4) = atan2(s5_2,c5);
    
    %q6:
    c1 = cos(q1(1));s1 = sin(q1(1));s5 = sin(q5(1));
    m = c1*oy - ox*s1;n = c1*ny - nx*s1;
    q6(1) = atan2(n,m)+atan2(s5,0);
    s5 = sin(q5(2));
    q6(2) = atan2(n,m)+atan2(s5,0);
    c1 = cos(q1(2));s1 = sin(q1(2));s5 = sin(q5(1));
    m = c1*oy - ox*s1;n = c1*ny - nx*s1;
    q6(3) = atan2(n,m)+atan2(s5,0);
    s5 = sin(q5(2));
    q6(4) = atan2(n,m)+atan2(s5,0);
    
    [q3(1),q3(2)] = cal_formula_q3(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)));
    [q3(3),q3(4)] = cal_formula_q3(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)));
    [q3(5),q3(6)] = cal_formula_q3(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)));
    [q3(7),q3(8)] = cal_formula_q3(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)));
    
    [q2(1)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),sin(q3(1)),cos(q3(1)));
    [q2(2)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),sin(q3(2)),cos(q3(2)));
    [q2(3)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),sin(q3(3)),cos(q3(3)));
    [q2(4)] = cal_formula_q2(T,DH,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),sin(q3(4)),cos(q3(4)));
    [q2(5)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),sin(q3(5)),cos(q3(5)));
    [q2(6)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),sin(q3(6)),cos(q3(6)));
    [q2(7)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),sin(q3(7)),cos(q3(7)));
    [q2(8)] = cal_formula_q2(T,DH,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),sin(q3(8)),cos(q3(8)));
    
    [q4(1)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),q2(1),q3(1));
    [q4(2)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(1)),cos(q6(1)),q2(2),q3(2));
    [q4(3)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),q2(3),q3(3));
    [q4(4)] = cal_formula_q4(T,sin(q1(1)),cos(q1(1)),sin(q6(2)),cos(q6(2)),q2(4),q3(4));
    [q4(5)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),q2(5),q3(5));
    [q4(6)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(3)),cos(q6(3)),q2(6),q3(6));
    [q4(7)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),q2(7),q3(7));
    [q4(8)] = cal_formula_q4(T,sin(q1(2)),cos(q1(2)),sin(q6(4)),cos(q6(4)),q2(8),q3(8));
  
    q = zeros(8,6);
    q = [q1(1) q2(1) q3(1) q4(1) q5(1) q6(1);
        q1(1) q2(2) q3(2) q4(2) q5(1) q6(1);
        q1(1) q2(3) q3(3) q4(3) q5(2) q6(2);
        q1(1) q2(4) q3(4) q4(4) q5(2) q6(2);
        q1(2) q2(5) q3(5) q4(5) q5(3) q6(3);
        q1(2) q2(6) q3(6) q4(6) q5(3) q6(3);
        q1(2) q2(7) q3(7) q4(7) q5(4) q6(4);
        q1(2) q2(8) q3(8) q4(8) q5(4) q6(4);];
   
    q = q*180/pi;
    return;
end
function [q,q_] = cal_formula_q3(T,DH,s1,c1,s6,c6)
    d1 = DH.d(1);
    d4 = DH.d(4); d5 = DH.d(5); d6 = DH.d(6);
    a2 = DH.a(3); a3 = DH.a(4);
    nx = T(1,1); ny = T(2,1); nz = T(3,1);ox = T(1,2); oy = T(2,2); oz = T(3,2);ax = T(1,3); ay = T(2,3); az = T(3,3);px = T(1,4); py = T(2,4); pz = T(3,4);
    x = c1*px + py*s1 + d5*(c6*(c1*ox + oy*s1) + s6*(c1*nx + ny*s1)) - d6*(ax*c1 + ay*s1);
    y = pz - d1 - az*d6 + d5*(c6*oz + nz*s6);
    q = acos((x^2+y^2-a2^2-a3^2)/(2*a2*a3));q_ = -acos((x^2+y^2-a2^2-a3^2)/(2*a2*a3));
end
 
function [q] = cal_formula_q2(T,DH,s1,c1,s6,c6,s3,c3)
    d1 = DH.d(1);
    d4 = DH.d(4); d5 = DH.d(5); d6 = DH.d(6);
    a2 = DH.a(3); a3 = DH.a(4);
    
    nx = T(1,1); ny = T(2,1); nz = T(3,1);ox = T(1,2); oy = T(2,2); oz = T(3,2);ax = T(1,3); ay = T(2,3); az = T(3,3);px = T(1,4); py = T(2,4); pz = T(3,4);
    x = c1*px + py*s1 + d5*(c6*(c1*ox + oy*s1) + s6*(c1*nx + ny*s1)) - d6*(ax*c1 + ay*s1);
    y = pz - d1 - az*d6 + d5*(c6*oz + nz*s6);
    A = a2+a3*c3;B = -a3*s3;
    C = s3*a3;  D = a2+a3*c3;      M = -x;N = -y;   
    c2 = (D*M-N*B)/(A*D-B*C); s2 = (A*N-C*M)/(A*D-B*C);
    q = atan2(s2,c2);
end
 
function [q] = cal_formula_q4(T,s1,c1,s6,c6,q2,q3)
    nx = T(1,1); ny = T(2,1); nz = T(3,1);ox = T(1,2); oy = T(2,2); oz = T(3,2);   
   s234 = -c6*(c1*ox + oy*s1) - s6*(c1*nx + ny*s1) ;
   c234 = c6*oz + nz*s6 ;
   q = atan2(s234,c234) - q2 - q3;
end
程序2.3.3协作机械臂逆解