机械臂雅可比矩阵的矢量积求法

雅可比矩阵的第Ji列：

对于任何一个移动关节，Ji可以这样计算：

而对于任何一个旋转关节，Ji这样计算：

网上很多地方写的没有注意移动关节和旋转关节的区分，导致出现一些莫名其妙的问题。

附matlab代码：

function [J]=Jacobian2(Q)
    for i=1:6
        T0(:,:,i)=eye(4);
        T6(:,:,i)=eye(4);
    end
    Z6=zeros(3,6);
    P6=zeros(3,6);
    T=zeros(4,4,6);
    J=zeros(6,6);
    global alpha;
    global a;
    global d;
    global theta;
    global d0;
    global theta0;
    for i=1:5
        theta(i)=Q(i)+theta0(i);
    end
    d(6)=Q(6)+d0(6);
    for i=1:6
        temp=zeros(4,4);
        temp(1,:)=[cos(theta(i))                    -sin(theta(i))                  0               a(i)                ];
        temp(2,:)=[sin(theta(i))*cos(alpha(i))      cos(theta(i))*cos(alpha(i))     -sin(alpha(i))  -d(i)*sin(alpha(i)) ];
        temp(3,:)=[sin(theta(i))*sin(alpha(i))      cos(theta(i))*sin(alpha(i))     cos(alpha(i))   d(i)*cos(alpha(i))  ];
        temp(4,:)=[0 0 0 1];
        T(:,:,i)=temp;%Ti是i-1到i的变换矩阵
        if i==1
            T0(:,:,i)=temp;
        else
            T0(:,:,i)=T0(:,:,i-1)*temp;
        end
    end
    for i=5:-1:1
        T6(:,:,i)=T(:,:,i+1)*T6(:,:,i+1);
    end
    for i=1:6
        Z6(:,i)=T0(1:3,3,i);
        P6(:,i)=T6(1:3,4,i);
    end
    for i=1:5
        J(:,i)=[cross(Z6(:,i),T0(1:3,1:3,i)*P6(:,i));Z6(:,i)];
    end
    J(:,6)=[Z6(:,6);0;0;0];
end

有时候要求按照末端执行器坐标系{n}来执行一些位移旋转之类的操作，在末端执行器坐标系下的雅可比矩阵可以这样计算：

参考：战强版《机器人学：机构、运动学、动力学及运动规划》