参考

哈密顿原理和拉格朗日的推导: 第六章拉格朗日动力学.

书籍：
机械工业出版社的《机器人学导论》145页开始
《Rigid Body Dynamics Algorithms》
《MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL》

文献: A New Lagrangian Formulation of Dynamics for Robot Manipulators.

思路

其实在ppt中和书中，会推导出一个叫拉格朗日函数的标量，由系统的动能和势能的差组成：
并得到动力学方程：

进一步展开得到

但是式子中第一项直接求系统动能再对时间求导计算量巨大，所以使用了文献: A New Lagrangian Formulation of Dynamics for Robot Manipulators.的方法：

对动力学方程中的D、C、G分别计算。

实现

文献中式子 31、32、38、39、42、43

        delta{i} = T{i-1}(1:3, 3);
        lambda{i} = cross(T{i-1}(1:3, 4),  delta{i});
        w{i} = w{i-1} + delta{i} * qd(i);
        u{i} = u{i-1} + lambda{i} * qd(i);
        wd{i} = wd{i-1} + cross(w{i-1}, delta{i} * qd(i));
        ud{i} = ud{i-1} + cross(w{i-1}, lambda{i} * qd(i)) + cross(u{i-1}, delta{i} * qd(i));
复制

文献中式子47、48、50、66、64

        K{i} = K{i+1} + I{i} + m(i) * skew(r{i}) * skew(r{i})';
        H{i} = H{i+1} + m(i) * r{i};
        M(i) = M(i+1) + m(i);
        N{i} = N{i+1} + I{i} * wd{i} + cross(w{i}, I{i} * w{i}) + cross(m(i) * r{i}, ud{i} + cross(wd{i}, r{i}) + cross(w{i}, u{i} + cross(w{i}, r{i})));
        D{i} = D{i+1}  + m(i) * (ud{i} + cross(wd{i}, r{i}) + cross(w{i}, u{i} + cross(w{i}, r{i})));
复制

文献中式子58、59、60、65

    X{i} = K{i} * delta{i} + cross(H{i} ,  lambda{i});
    Y{i} = cross(delta{i}, H{i}) + M(i) * lambda{i};
    DD(i, j) = delta{i}' * X{j} +  lambda{i}' * Y{j};
复制

4.带入动力学方程求解：