算法求解过程

 
  参数说明：  

• a i a_iai​：{0}-{i}之间的矢量距离
• e i e_iei​：在base系下描述的关节轴向量
• $p$：{0}-{p}之间的矢量距离
• r i r_iri​：{i}-{p}之间的矢量距离

代码及解析（参考Robotics Toolbox）

 
  以下内容转载请联系！  

% SerialLink.jacob_dot Derivative of Jacobian
%
% JDQ = R.jacob_dot(Q, QD) is the product (6x1) of the derivative of the Jacobian (in the world frame) and the joint rates.
% Notes::
% - This term appears in the formulation for operational space control XDD = J(Q)QDD + JDOT(Q)QD
%
function Jdot = jacob_dot(robot, q, qd)
	n = robot.n;
    links = robot.links;
    % 使用Angeles的表示法：
    %   [Q,a] ~ [R,t] the per link transformation                                       相邻连杆间的变换
    %   P ~ R   the cumulative rotation t2r(Tj) in world frame                          世界坐标系的累积旋转变换t2r(Tj)
    %   e       the last column of P, the local frame z axis in world coordinates       P的最后一列，世界坐标中的局部坐标系z轴
    %   w       angular velocity in base frame                                          base坐标系中的角速度 
    %   ed      deriv of e                                                              e的导数,其中e为关节轴向量在base下的表示
    %   r       is distance from final frame                                            {i}距最后坐标系的距离
    %   rd      deriv of r                                                              r的导数
    %   ud      temporary variable                                                      ud = e × rd

    %            e1        e2      ...      en        
    %   J = [----------------------------------------]           
    %          e1 x r1   e2 x r2   ...    en x rn     
    %
    %          w 
    %   t = [-----]
    %          v
    %
    %   t = J * dot_q
    %   robert.h.x.s
    % 提取数据，为算法计算做准备
    for i=1:n
        T = links(i).A(q(i));
        Q{i} = t2r(T);          % 旋转矩阵
        a{i} = transl(T)';      % 平移矩阵
    end
    % P为累积变换，e{i}为{i}系的旋转轴z轴变换到base系下的表示,P195
    P{1} = Q{1};
    e{1} = [0 0 1]';
    for i=2:n
        P{i} = P{i-1}*Q{i};
        e{i} = P{i}(:,3);
    end

    % step 1 求{i}在base系下的角速度w{i}，有速度叠加（注意点：旋转矩阵求逆==求转置）,P207-P209
    w{1} = qd(1)*e{1};
    for i=1:(n-1)
        w{i+1} = qd(i+1)*[0 0 1]' + Q{i}'*w{i};
    end

    % step 2 求dot_e,P207-P209
    ed{1} = [0 0 0]';
    for i=2:n
        ed{i} = cross(w{i}, e{i});
    end

    % step 3 求dot_r,方法同step2（注意点：有矢量叠加）,P207-P209
    rd{n} = cross(w{n}, a{n});
    for i=(n-1):-1:1
        rd{i} = cross(w{i}, a{i}) + Q{i}*rd{i+1};
    end

    % 求{i}-{p}的距离,P198
    r{n} = a{n};
    for i=(n-1):-1:1
        r{i} = a{i} + Q{i}*r{i+1};
    end

    % 求d(e x rd)/dt,其中d(e x rd)/dt = dot_e x r + e x dot_r ,P207-P209
    ud{1} = cross(e{1}, rd{1});
    for i=2:n
        ud{i} = cross(ed{i}, r{i}) + cross(e{i}, rd{i});
    end

    % step 4 swap ud and ed，目的：为了统一工具箱的描述形式
    v{n} = qd(n)*[ud{n}; ed{n}];
    for i=(n-1):-1:1
        Ui = blkdiag(Q{i}, Q{i});                               % blkdiag 分块对角矩阵
        v{i} = qd(i)*[ud{i}; ed{i}] + Ui*v{i+1};
    end

    Jdot = v{1};    % 结果得到的是 dot_J * dot_q,即公式的后半部分

  详细参考《Fundamentals of Robotic Mechanical Systems Theory, Methods, and Algorithms》，着重参考5.1-5.5部分，具体公式推导有空再补。