第二章运动学-2.2 通用四五六轴轴线交于一点几何解

愈之~

已于 2024-07-09 16:18:27 修改

阅读量991

点赞数 34

分类专栏：机械臂算法仿真笔记文章标签：算法人工智能

于 2024-07-09 16:16:39 首次发布

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

版权

机械臂算法仿真笔记专栏收录该内容

7 篇文章 1 订阅

订阅专栏

在对上一节相关的DH知识了解之后，我们可以尝试对通用工业机械臂进行建模，这里选取的456轴交于一点的机械臂，并且相对于课本来说，DH参数模型相对更复杂一些，笔者在这里的考量是，既然写，那就写相对复杂一点的模型，否则，在实际的运用过程中，一下少了这个参数一下少了那个参数，一下子拿网上的代码跑起来又正逆解对不上，总而言之会出现一些问题，书上最多给个PUMA的例子，它简单，但是实际中的机械手，相对这类更复杂，导致在实际过程中运用的时候又费劲。

（一）、DH建模

有了前面的建模知识之后，我们可以采用对修改DH模型公式(2-1-2)来进行建模，实际中可以参照RoboDK中的样子来进行操作，这里不列出图片了，直接将建立好的模型如下，如图2.2.1所示

图2.2.1 通用机械臂DH模型

根据上面的DH模型，我们可以利用前面一章的对于建立DH模型的理解边建立坐标系边写DH表得到如下DH表，并且在中间过程中不断调整坐标系与参数，在建立这个过程中，其实有一些对坐标系的调整，例如坐标系2，坐标系3，在实际的机械中，坐标系3与坐标系4在坐标系3下均有xyz三个方向的偏移，这里做了将 $d_3$ 放到了坐标系2与坐标系3之间了，或者还有一些别的调整方法（ $d_3$ 其实实际中物理模型是坐标系0123与坐标系456的两个平面之间的距离，现在建立的的模型是坐标系012是一个平面，3456是一个平面），没有说一定需要这么调整，只要符合公式 (2-1-2) ，都是可以的，没有定论，包括坐标系2也可以进行调整，使得 $Y_2$ 的方向朝向坐标系1，等等。实际中灵活运用公式才是关键。那么得到如下DH表格。

$i$	$\alpha_{i-1}$	$a_{i-1}$	$\theta_i$	$d_i$
1	0	0	0	$d_1$
2	-90	$a_1$	90	0
3	0	$a_2$	0	$d_3$
4	-90	$a_3$	0	$d_4$
5	90	0	0	0
6	-90	0	0	$d_6$

（二）、正解

对于已经有了DH参数表之后，我们就很容易得到正解：

${{ ^0}T}_1=R_x\left(\alpha_0\right)\bullet\ T_x\left(a_0\right)\bullet\ R_z\left(\vartheta_1\right)\bullet\ T_z\left(d_1 \right)$ (2-2-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\bullet{{ ^1}T}_2\bullet{{^2}T}_3\bullet{{ ^3}T}_4\bullet{{ ^4}T}_5\bullet{{ ^5}T}_6$ (2-2-2)

奇异及臂型粗略图如下：

图2.2.2 通用机械臂各类奇异点及臂型

上图2.2.2种，第一种奇异在于456轴线的交点在在底座的投影在坐标系1的原点附近时，第二种奇异，当456轴线的交点在于大臂的延长线上时，第三种奇异在于5轴的关节角度为0时。

机械臂有奇异的本质在于机械臂的运动学结构所导致的，一般有一下几个方面相关，

1.当机械臂的关节轴关节共线时，会导致机械臂失去一些自由度，导致无法达到所需的位置与姿态，此种称为轴空间奇异。例如上图2.2.2中的大臂奇异和腕关节奇异；

2.当机械臂运行超出了机械臂的可行性范围时，此中情况下，机械臂无法达到所需要的位置和姿态，此种也可称为工作空间奇异，例如上图2.2.2中的肘关节奇异；

3.还有一种范围奇异，属于当关节角度超过关节范围时，属于范围奇异。

因为有了奇异点，所以机械臂会有相应的臂型分类，每个奇异会产生两种臂型，所以机械臂的整体有8种构型。

那么如何区分臂型呢，有如下公式：

大臂：在图中可以看出，456轴交点在1号坐标系下的X轴方向区分， $P_x>0$ 为前， $P_x<0$ 为后那么有如下公式：

$P={{ ^1}T}_2\bullet{{ ^2}T}_3\bullet{{ ^3}T}_4\bullet \begin{bmatrix} 0\\ 0\\ 0\\ 1 \end{bmatrix}$ (2-2-3)

$shoulder\left\{\begin{matrix}front\ \ \ P_x\geq0\\back\ \ \ P_x<0\\\end{matrix}\right.$ (2-2-4)

肘关节：可以得出当关节三的角度是否在三角的范围内

$elbow\left\{\begin{matrix}down\ \ \ tan\theta_3\le d_4/a_3\\up\ \ \ tan\theta_3>d_4/a_3\\\end{matrix}\right.$ (2-2-5)

腕关节：关节5的角度跟0的关系（不考虑翻转多圈的情况下）：

$wrist\left\{\begin{matrix}up\ \ \ 0<\theta_5<\pi\\down\ \ -\pi<\theta_5<0\\\end{matrix}\right.$ (2-2-6)

（三）、逆解

前三关节采用几何投影法，后三个关节采用解析法，设末端位姿矩阵为

${{T}}=\left[\begin{matrix}n_x\\n_y\\\begin{matrix}n_z\\0\\\end{matrix}\\\end{matrix}\begin{matrix}o_x\\o_y\\\begin{matrix}o_z\\0\\\end{matrix}\\\end{matrix}\begin{matrix}a_x\\a_y\\\begin{matrix}a_z\\0\\\end{matrix}\\\end{matrix}\begin{matrix}p_x\\p_y\\\begin{matrix}p_z\\1\\\end{matrix}\\\end{matrix}\right]$ (2-2-7)

设第4坐标系在基坐标系中的位置为 $\left(x_p,y_p,z_p\right)$ ，在第6坐标系中的位置为 $\left(0,0,{-d}_6\right)$ ，则可得关系式：

$\left[\begin{matrix}x_p\\y_p\\\begin{matrix}z_p\\1\\\end{matrix}\\\end{matrix}\right]={T}\left[\begin{matrix}0\\0\\\begin{matrix}{-d}_6\\1\\\end{matrix}\\\end{matrix}\right]=\left[\begin{matrix}p_x-a_xd_6\\p_y-a_yd_6\\\begin{matrix}p_z-a_zd_6\\1\\\end{matrix}\\\end{matrix}\right]$ (2-2-8)

因此 $x_p=p_x-a_xd_6$ ， $y_p=p_y-a_yd_6$ ， $z_p=p_z-a_zd_6$ 。关节1(坐标系0以z轴的逆向投影，得到以下大致的粗略图)

图2.2.3 逆解关节1简略图

1.(关节一)、

根据上图可以列出如下公式：

$\left\{\begin{matrix}x_p{'} = x_p+d_3*sin\theta_1\ \ \ \\y_p{'} = y_p-d_3*cos\theta_1\ \ \\ tan\theta_1 = y_p{'}/x_p{'}\end{matrix}\right.$ (2-2-9)

三个未知数三条等式得到公式 $x_psin\theta_1-y_pcos\theta_1+d_3=0$ (2-2-10)

那么解： $\theta_1=atan2{\left(y_p,x_p\right)}-atan2{\left(d_3,\sqrt{{x_p}^2+{y_p}^2-{d_3}^2}\right)}$ （2-2-11）

或者： $\theta_1=\theta_1-\frac{\pi}{\left|\theta_1\right|}\theta_1$ (2-2-12)

同时可以求得：

$x_p^\prime=x_p+d_3\ast\ sin\theta_1$ , $y_p^\prime=y_p-d_3\ast cos\theta_1$ （2-2-13）

用新坐标去求解新的反解。

2.(关节二)、

如下图所示：

图2.2.4 逆解关节2简略图

图2.2.4 左为前臂型情况，相应的角度计算表达式为

$\left\{\begin{matrix}\theta_a=arccos{\left(\left(O_1O_2^2+O_1{P^\prime}^2-O_2{P^\prime}^2\right)/\left(2O_1O_2\bullet O_1P^\prime\right)\right)}\\\theta_b=atan2\left(y_{p1},x_{p1}\right)\\\theta_2=\theta_a+\theta_b\\\end{matrix}\right.$ (2-2-14)

图2.2.4 右为后臂型情况，相应的角度计算表达式为

其中 $O_1O_2=a_2$ , $O_2P^\prime=\sqrt{a_3^2+d_4^2}$ , $O_1P^\prime=\sqrt{x_{p1}^2+y_{p1}^2}$ , $\left[\begin{matrix}x_{p1}\\ y_{p1}\\\begin{matrix}z_{p1}\\0\\ \end{matrix}\\\end{matrix}\right]={{^{0}}{A}}_{1}^{{-1}}{ }\left[\begin{matrix}x_p^ \prime\\y_p^\prime\\\begin{matrix} z_p^\prime\\0\\\end{matrix}\\\end{matrix}\right]$

$x_{p1}=x_p^\prime\cos{\theta_1-a_1+y_p^\prime\sin{\theta_1}}$ ， $y_{p1}=z_p^\prime-d_1$ ， $z_{p1}=x_p^\prime\sin{\theta_1-y_p^\prime\cos{\theta_1}}$

3.(关节三)、

$\left[\begin{matrix}x_{p2}\\y_{p2}\\\begin{matrix}z_{p2}\\0\\\end{matrix}\\\end{matrix}\right]={{^{1}{A}}_{2}^{-1}}\left[\begin{matrix}x_{p1}\\y_{p1}\\\begin{matrix}z_{p1}\\0\\\end{matrix}\\\end{matrix}\right]$ (2-2-17)

$y_{p2}=x_{p1}\cos{\theta_2}-a_2+y_{p1}\sin{\theta_2}$

$x_{p2}=y_{p1}\cos{\theta_2}-\ x_{p1}\sin{\theta_2}$ (2-2-18)

${z_{p2}=z}_{p1}$

图2.2.5 逆解关节3简略图

图2.2.5左为上臂型情况，此时 $y_{p2}>0$ ，相应的角度计算表达式为

$\left\{\begin{matrix}\theta_c=\pi-arctan{\left(\frac{d_4}{a_3}\right)}\\\theta_d=arccos{\left(\left(O_1O_2^2+O_2{P^\prime}^2-O_1{P^\prime}^2\right)/\left(2O_1O_2\bullet O_2P^\prime\right)\right)}\\\theta_3=\theta_d-\theta_c\\\end{matrix}\right.$ (2-2-19)

图2.2.5 右为下臂型情况，此时 $y_{p2}\le0$ ，相应的角度计算表达式为

（4）(关节四五六)、

将式(2-2-2)等式两边左乘 ${^0}T_3^{-1}\left(\theta_1,\theta_2,\theta_3\right)$ ，则有

${^0}T_3^{-1}\left(\theta_1,\theta_2,\theta_3\right){^0}T_6={{^4}T}_6$ (2-2-21)

有：

${{^4}T}_6=\left[\begin{matrix}c_4c_5c_6-s_4s_6\\c_6s_5\\\begin{matrix}c_4s_6+c_5c_6s_4\\0\\\end{matrix}\\\end{matrix}\begin{matrix}{\ \ \ \ -c}_6s_4-c_4c_5s_6\\-s_5s_6\\\begin{matrix}c_6c_4-s_4c_5s_6\\0\\\end{matrix}\\\end{matrix}\begin{matrix}{\ \ \ \ c}_4s_5\ \ \ \ \ \\-c_5\\\begin{matrix}s_4s_5\\0\\\end{matrix}\\\end{matrix}\begin{matrix}a_3+{d_6c}_4s_5\\-d_4-c_5d_6\\\begin{matrix}d_6s_5s_4\\1\\\end{matrix}\\\end{matrix}\right]$ (2-2-22)

1） $\theta_5 \neq 0$

根据上式(2-2-20) 令 $T={{^4}T}_6=\left[\begin{matrix}\begin{matrix}r_{11}&r_{12}\\r_{21}&r_{22}\\\end{matrix}&\begin{matrix}r_{13}&p_{14}\\r_{23}&p_{24}\\\end{matrix}\\\begin{matrix}r_{31}&r_{32}\\0&0\\\end{matrix}&\begin{matrix}r_{33}&p_{34}\\0&1\\\end{matrix}\\\end{matrix}\right]$ (2-2-23)

$s_5=\pm\sqrt{{r_{13}}^2+{r_{33}}^2}$ , $c_5=-r_{23}$ , $\theta_5=atan2\left(s_5,c_5\right)$ (2-2-24)

是正还是负取决于腕关节臂型。

$\theta_4=atan2\left(\frac{r_{33}}{s_5},r_{13}/s_5\right)$ , $\theta_6=atan2\left(-\frac{r_{22}}{s_5},r_{21}/s_5\right)$ (2-2-25)

2） $\theta_5=0$

此时只能计算出 $\theta_4$ 、 $\theta_6$ 的和

$\theta_4+\theta_6=atan2\left(r_{31},r_{11}\right)$ (2-2-26)

Maltab仿真代码如下：

function [T,shoulder,elbow,wrist] = arm_fkine_dh(q,DH,q_option,alpha_theta_option)
    a1= DH.a(2); a2= DH.a(3); a3= DH.a(4);
    d1 = DH.d(1);d3 = DH.d(3); d4 = DH.d(4);d6 = DH.d(6);
    a0 = 0;a4 =0;a5 = 0;
    d2 = 0;d5 =  DH.d(5);
    if strcmp(q_option,'deg')
        q = q*pi/180.0;
    elseif strcmp(q_option,'rad')
       ;
    else
        q = q*pi/180.0;
    end
 
    if strcmp(alpha_theta_option,'deg')
        alpha= DH.alpha*pi/180.0;
        theta = DH.theta*pi/180.0;
    elseif strcmp(alpha_theta_option,'rad')
        ;
    else
        alpha= DH.alpha*pi/180.0;
        theta = DH.theta*pi/180.0;
    end
    
    q = q+theta; 
    alpha0 = alpha(1); alpha1 = alpha(2); alpha2 = alpha(3);alpha3 = alpha(4);  alpha4 = alpha(5); alpha5 = alpha(6);
    T01 = create_link(q(1),d1,alpha0,a0,'rad');
    T12 = create_link(q(2),d2,alpha1,a1,'rad');
    T23 = create_link(q(3),d3,alpha2,a2,'rad');
    T34 = create_link(q(4),d4,alpha3,a3,'rad');
    T45 = create_link(q(5),d5,alpha4,a4,'rad');
    T56 = create_link(q(6),d6,alpha5,a5,'rad');
    T = T01*T12*T23*T34*T45*T56;
    T_temp = T12*T23*T34;
    if abs(T_temp(1,4))<eps
        shoulder = 0;     
    elseif (T_temp(1,4))
        shoulder = 1;
    else 
        shoulder = -1;
    end
   
    if abs(tan(q(3))-d4/a3)<eps
        elbow = 0;
    elseif(tan(q(3))>d4/a3)||(q(3)>pi/2)
        elbow = 1;
    else
        elbow = -1;       
    end    
    if abs(q(5))<eps
        wrist = 0;
    elseif(q(5)>0)
        wrist = 1;
    else
        wrist = -1;
    end
end

程序2.2.1 通用机械臂正解

反解：

function q = arm_ikine_dh(T,DH,shoulder,elbow,wrist,alpha_theta_option,option)
    a1= DH.a(2); a2= DH.a(3); a3= DH.a(4);
    d1 = DH.d(1);d3 = DH.d(3); d4 = DH.d(4);d6 = DH.d(6);
    if strcmp(alpha_theta_option,'deg')
        alpha = DH.alpha*pi/180.0; 
        theta = DH.theta*pi/180;
    elseif strcmp(alpha_theta_option,'rad')
        ;
    else
        alpha = DH.alpha*pi/180.0; 
        theta = DH.theta*pi/180;
    end
    alpha0 = alpha(1); alpha1 = alpha(2); alpha2 = alpha(3);alpha3 = alpha(4);  alpha4 = alpha(5); alpha5 = alpha(6);
    a0 = 0;a4 =0;a5 = 0;
    d2 = 0;d5 =  0;
    V65 = [0 0 -d6 1]';
    V05 = T*V65;
    xp = V05(1);yp = V05(2);zp = V05(3);
    if shoulder == -1
        q(1) = atan2(yp,xp)+atan2(-d3,sqrt(xp^2+yp^2-d3^2));
        if q(1)>0 
            q(1) = q(1)-pi;
        elseif q(1)<0
            q(1) = q(1)+pi;
        end
    elseif shoulder == 1
        q(1) = atan2(yp,xp)-atan2(-d3,sqrt(xp^2+yp^2-d3^2));
    elseif shoulder == 0
          return;
    end
    xp_1 = xp + d3*sin(q(1));
    yp_1 = yp - d3*cos(q(1));  
    xp1=xp_1*cos(q(1))-a1+yp_1*sin(q(1));
    yp1=zp-d1; 
    o1p=sqrt(xp1^2+yp1^2);
    o1o2=a2;
    o2p=sqrt(a3^2+d4^2);
    if(elbow == -1)
        q(2) = acos((o1o2^2+o1p^2-o2p^2)/(2*o1o2*o1p))+atan2(yp1,xp1);
    elseif elbow == 1
        q(2) = atan2(yp1,xp1)-acos((o1o2^2+o1p^2-o2p^2)/(2*o1o2*o1p));
    else 
        return;
    end
    if q(2)>pi
        q(2)= q(2)-2*pi;
    elseif q(2)<-pi
        q(2) = q(2)+2*pi;
    end    
    thetac=pi-atan(d4/a3);
    if(elbow == -1)
        q(3) = acos((o1o2^2+o2p^2-o1p^2)/(2*o1o2*o2p))-thetac;
    elseif elbow == 1
        q(3) = 2*pi-acos((o1o2^2+o2p^2-o1p^2)/(2*o1o2*o2p))-thetac;
    elseif elbow == 0
        q(3) = atan2(d4,a3);
    end
    T01 = create_link(q(1),d1,alpha0,a0,'rad');
    T12 = create_link(q(2),d2,alpha1,a1,'rad');
    T23 = create_link(q(3),d3,alpha2,a2,'rad'); 
    T_46 = inv(T01*T12*T23)*T;
    if wrist == 1
        s5 = (T_46(1,3)^2+T_46(3,3)^2)^0.5;
        c5 = -T_46(2,3);    
        q(5) = atan2(s5,c5);
        q(4) = atan2(T_46(3,3)/sin(q(5)),T_46(1,3)/sin(q(5)));
        q(6) = atan2(T_46(2,2)/(-sin(q(5))),T_46(2,1)/sin(q(5)));
    elseif wrist == -1
        s5 = -(T_46(1,3)^2+T_46(3,3)^2)^0.5;
        c5 = -T_46(2,3);
        q(5) = atan2(s5,c5);
        q(4) = atan2(T_46(3,3)/sin(q(5)),T_46(1,3)/sin(q(5)));
        q(6) = atan2(T_46(2,2)/(-sin(q(5))),T_46(2,1)/sin(q(5)));
    else
        q(5) = 0;
        theta46 = atan2(T_46(3,1),T_46(1,1));
        q(4) = 0;
        q(6) = theta46;
    end   
     if strcmp(option,'deg')
         q = q*180/pi-theta*180/pi;
     else
         q = q-theta;
     end
end

程序2.2.2 通用机械臂反解

我们采用对关节值产生随机数来验证正逆解的正确性：

clc;
clear;
DH.a(1) = 0.0;DH.a(2) = 0.163655;DH.a(3) = 0.550739;
DH.a(4) = 0.208847;DH.a(5) = 0.0;DH.a(6) = 0.0;
DH.alpha(1) = 0.0;DH.alpha(2) = 90;DH.alpha(3) = 0.0;
DH.alpha(4) = 90;DH.alpha(5) = -90;DH.alpha(6) = 90;
DH.d(1) = 0.43;DH.d(2) = 0.0;DH.d(3) = -0.001279;
DH.d(4) = 0.700029;DH.d(5) = 0.0;DH.d(6) = 0.115;
 
DH.theta(1) = 0.0;DH.theta(2) = 0.0;DH.theta(3) = 0.0;
DH.theta(4) = 0.0;DH.theta(5) = 0.0;DH.theta(6) = 0.0;
q = linspace(0,0,6);
 for i = 1:4000
    q(1) = (rand()-0.5)*2*165;      %1-axis  -165~165;
    q(2) = rand()*160;                %2-axis   0~160, zero 90degree
    q(3) = rand()*200-70;            %3-axis  -70~130;
    q(4) = (rand()-0.5)*2*170;      %4-axis  -170~170;
    q(5) = (rand()-0.5)*2*120;      %5-axis  -120~120;
    q(6) = (rand()-0.5)*2*180;      %6-axis  -180~180;
 
%dh
   [T,shoulder,elbow,wrist] = arm_fkine_dh(q,DH,'deg','deg');
   q_i = arm_ikine_dh(T,DH,shoulder,elbow,wrist,'deg','deg');
    if (abs(q(1)-q_i(1))>0.01)||(abs(q(2)-q_i(2))>0.01)||(abs(q(3)-q_i(3))>0.01)||(abs(q(4)-q_i(4))>0.01)||(abs(q(5)-q_i(5))>0.01)||(abs(q(6)-q_i(6))>0.01)
        if(abs(q(1)-q_i(1))-360 <= 0.1)
            continue;
        end
        display(i,'failure!');
        break;
    else
        display(i,'nice boy!');
    end
end

程序2.2.3 通用机械臂正逆解验证

至此，通用六自由度机械臂正逆解算法完成。其实这章节已经把市面上的通用456轴线交于一点的机械臂的正逆解算法都包含其中，很多机械臂在设计的时候，DH中的部分参数被置为了0，例如 $d_3$ , $a_3$ $a_3$ 。

愈之~

关注

34
点赞
踩
24

收藏

觉得还不错? 一键收藏
0
评论
第二章运动学-2.2 通用四五六轴轴线交于一点几何解

在对上一节相关的DH知识了解之后，我们可以尝试对通用工业机械臂进行建模，这里选取的456轴交于一点的机械臂，并且相对于课本来说，DH参数模型相对更复杂一些，笔者在这里的考量是，既然写，那就写相对复杂一点的模型，否则，在实际的运用过程中，一下少了这个参数一下少了那个参数，一下子拿网上的代码跑起来又正逆解对不上，总而言之会出现一些问题，书上最多给个PUMA的例子，它简单，但是实际中的机械手，相对这类更复杂，导致在实际过程中运用的时候又费劲。大臂：在图中可以看出，456轴交点在1号坐标系下的X轴方向区分，
复制链接

扫一扫