四自由度机械臂逆解中的问题

Question

最近在做四自由度机械臂逆解的时候遇到一个问题，我用的是标准dh建立的机械臂，机械臂建立如下

% theta   d   a   alpha
a1=0.101;a2=0.605;a3=0.2844;a4=0.4844;
L(1) = Link([0  0   a1  pi/2]);
L(2) = Link([0  0   a2  0]);
L(3) = Link([0  0   a3  0]);
L(4) = Link([0  0   a4  0]);

robot = SerialLink(L,'name','robot');

在建立过程中没有考虑到机械臂各个关节存在角度限制，建立模型如图所示

但是实际机械臂由于角度限制，应该是下图这样

我是按没有角度限制的时候求的逆解，带入正逆解验证的时候也正确

1.想请问在不加角度限制时建立的dh求解出来的逆解是否正确。
另一个问题是，在四个角度都存在限制时，我的逆解应该如何处理才能做后只得到一组解下面是四个角度限制和逆解程序

function [q] = ikine1(T)
    a1=0.101;a2=0.605;a3=0.2844;a4=0.4844;
    px=T(1,4);
    py=T(2,4);
    pz=T(3,4);
    nx=T(1,1);
    ny=T(2,1);
    nz=T(3,1);
    ax=T(1,3);
    ay=T(2,3);
    az=T(3,3);
    ox=T(1,2);
    oy=T(2,2);
    oz=T(3,2);
    
    t11=-atan2(py,-px)+atan2(0,+sqrt(px^2+py^2));
    t12=-atan2(py,-px)+atan2(0,-sqrt(px^2+py^2));
    
    t14_1=px*cos(t11)-a1+py*sin(t11)-ny*a4*sin(t11)-nx*a4*cos(t11);
    t14_2=px*cos(t12)-a1+py*sin(t12)-ny*a4*sin(t12)-nx*a4*cos(t12);

    t24_1=pz-a4*nz;
    t24_2=pz-a4*nz;
    
    flag1=4*a2^2*a3^2-(t14_1^2+t24_1^2-a2*a2-a3*a3)^2;
    if flag1<0
        flag1=-flag1;
    end
    flag2=4*a2^2*a3^2-(t14_2^2+t24_2^2-a2*a2-a3*a3)^2;
    if flag2<0
        flag2=-flag2;
    end

    t31=atan2(sqrt(flag1),t14_1^2+t24_1^2-a2*a2-a3*a3);
    t32=atan2(-sqrt(flag1),t14_1^2+t24_1^2-a2*a2-a3*a3);
    t33=atan2(sqrt(flag2),t14_2^2+t24_2^2-a2*a2-a3*a3);
    t34=atan2(-sqrt(flag2),t14_2^2+t24_2^2-a2*a2-a3*a3);

    t21=atan2(-a3*t14_1*sin(t31)+t24_1*(a3*cos(t31)+a2),t14_1*(a3*cos(t31)+a2)+a3*t24_1*sin(t31));
    t22=atan2(-a3*t14_1*sin(t32)+t24_1*(a3*cos(t32)+a2),t14_1*(a3*cos(t32)+a2)+a3*t24_1*sin(t32));
    t23=atan2(-a3*t14_2*sin(t33)+t24_2*(a3*cos(t33)+a2),t14_2*(a3*cos(t33)+a2)+a3*t24_2*sin(t33));
    t24=atan2(-a3*t14_2*sin(t34)+t24_2*(a3*cos(t34)+a2),t14_2*(a3*cos(t34)+a2)+a3*t24_2*sin(t34));
    
    t11_1=nx*cos(t11)+ny*sin(t11);
    t11_2=nx*cos(t12)+ny*sin(t12);
    t21_1=nz;
    t21_2=nz;

    t41=atan2(t21_1,t11_1)-t21-t31;
    t42=atan2(t21_1,t11_1)-t22-t32;
    t43=atan2(t21_2,t11_2)-t23-t33;
    t44=atan2(t21_2,t11_2)-t24-t34;

    q=[t11,t21,t31,t41;
        t11,t22,t32,t42;
        t12,t23,t33,t43;
        t12,t24,t34,t44]*180/pi;
    for i=1:4
        for j=1:4
            if q(i,j)<-180
                q(i,j)=q(i,j)+360;
            end
            if q(i,j)>180
                q(i,j)=q(i,j)-360;
            end
        end
    end
end
  

Answer 1

该回答通过自己思路及引用到GPTᴼᴾᴱᴺᴬᴵ搜索,得到内容具体如下：

1. 当没有考虑到机械臂各个关节存在角度限制时，使用标准DH法求解逆解是正确的。但是，实际机械臂由于角度限制，可能存在无法到达的位置或者姿态，这时需要对逆解进行限制。

2. 当四个关节都存在角度限制时，可以考虑在求解逆解时加入角度限制的约束条件。具体而言，可以在逆解程序中加入如下的约束条件：

% t_min和t_max分别是每个关节的最小角度和最大角度
if t(j) < t_min(j)
    t(j) = t_min(j);
elseif t(j) > t_max(j)
    t(j) = t_max(j);
end

其中，j表示当前处理的关节编号，t_min和t_max分别是每个关节的最小角度和最大角度。如果计算得到的逆解超出了角度限制范围，就将其限制在范围内。

需要注意的是，加入角度限制的约束条件可能会导致求解失败或者得到不稳定的逆解。在实际应用中，需要根据具体情况进行调整和优化。

另外，可以使用数值优化方法（如基于梯度的优化方法）来求解符合角度限制的逆解。这种方法可以通过定义逆解的目标函数和约束条件，使用数学优化算法来求解最优解。

---

如果以上回答对您有所帮助，点击一下采纳该答案～谢谢