机械臂动力学建模（2）- Kane凯恩算法

参考

主要的参考是：

1. 链接: 百度文库的ppt.
   这个ppt的思路能讲明白，但是公式到了最后一步求τ的时候有问题：
   
   计算τ的第一项中缺少一个加速度项，应该是：
   
2. 文献: 基于凯恩方程的机器人动力学递推算法.
   里面开始就给了公式

思路

可以看参考中ppt，主要就是通过kane提出的广义速度的偏速度：
来表征动力学方程中的微分位移

之后就可以把普通的动力学方程

改写成广义主动力和广义惯性力的形式。
之后求广义偏速度，并带入
最后使τ为主动力，通过广义惯性力计算（式子可能有错，参考前面修改意见）：

实现

1. 迭代法计算线速度，角速度，重心速度以及导数
   
   
   matlab代码：

    for i = 1:6
        if i == 1
          w{i} =  R{i}*w0 + qd(i)*e;
          wd{i} = R{i}*wd0 + qdd(i)*e + cross(R{i}*w0,qd(i)*e);
          v{i} =  R{i} * (v0 + cross(w{i},p{i}));
          vd{i} = R{i} * (vd0 + cross(w0, cross(w0, p{i})) + cross(wd0, p{i}));
        else
          w{i} =  R{i}*w{i-1} + qd(i)*e;
          wd{i} = R{i}*wd{i-1} + qdd(i)*e + cross(R{i}*w{i-1},qd(i)*e);
          v{i} =  R{i} * (v{i-1} + cross(w{i},p{i}));
          vd{i} = R{i} * (vd{i-1} + cross(w{i-1}, cross(w{i-1}, p{i})) + cross(wd{i-1}, p{i}));
        end
        vc{i} = vd{i} + cross(wd{i},c{i}) + cross(w{i}, cross(w{i}, c{i}));
    end

2. 计算偏速度
   
   matlab代码
   i=1时候自己设置一下

    for i = 2:6
        for j = 1:6
            if j < i
               wp{i,j} = R{i} * wp{i-1,j};
               vp{i,j} = R{i} * (cross(wp{i-1,j},p{i}) + vp{i-1,j});
               vcp{i,j} = vp{i,j} + cross(wp{i,j},c{i});
            elseif j == i
               wp{i,j} = e;
               vp{i,j} = [0 0 0]';
               vcp{i,j} = cross(e, c{i});
            else
               wp{i,j} = [0 0 0]';
               vp{i,j} = [0 0 0]';
               vcp{i,j} = [0 0 0]';
            end
        end
    end

3. 计算tau
   
   matlab代码

    for i = 1:6
        N{i} =  I{i}*wd{i} + cross(w{i},I{i}*w{i});
    end

    tau = zeros(6,1);
    for j = 1:6
        for i = j:6
            tau(j) = tau(j) + M(i)*dot(vc{i},vcp{i,j}) + dot(N{i}, wp{i,j});
        end
    end

至此计算完成