【机器人学】机器人开源项目KDL源码学习：（8）KDL的精髓

  首先说一下我的心得：

1. 我认为KDL的精髓是Spatial Vector，结合C++等面向对象的语言可以写出较好的软件。

2. 直接阅读KDL代码不适合初学者学习机械臂动力学。

3. 要学习机械臂动力学的话应首先阅读使用3维向量推导公式的文献，也就是线速度和角速度独立分析。

4. 掌握机械臂动力学的原理之后再考虑如何将其写成代码，顺序不能错，否则会很吃力。

5. 机械臂动力学涉及到刚体运动学、刚体动力学、矩阵分析、李群和螺旋理论等。

一、3维向量和6维向量

  KDL中的算法大多是参考的《Rigid Body Dynamics Algorithms》这本书中的方法，它提出了Spatial Vector—是将3维的线性运动（力）和3维的旋转运动（力）组合起来的6维向量。这种组合可以极大地减小代数运算量，并且可以使代码更短、更清晰、更方便阅读和调试，所以这种程序适合用C++这种面向对象的语言来写。

  Spatial Vector的种类：

Spatial Velocity：v^O=[wxwywzvoxvoyvoz]T=[wvO]



Spatial Acceleration：a^O=ddt[wvO]=[w˙v˙O]



Spatial Force：f^O=[noxnoynozfxfyf]T=[nof]



Spatial Momentum:h^O=[hOh]=[I¯Cw+r×mvCmvC]



Spatial Inertia tensor:IO=[I¯C+Mr×r×Tmc×Tmc×m1]

二、牛顿欧拉法的数学基础

  刚体的运动方程，将牛顿方程和欧拉方程结合：f=ddt(Iv)=Ia+(v×∗I−Iv×)v=Ia+v×∗Iv

  其中定义的一些运算符：

⎡⎣⎢xyz⎤⎦⎥×=⎡⎣⎢0z−y−z0xy−x0⎤⎦⎥，v×∗=[w×0v×w×],v×=[wv]×=[w×v×0w×]，

IOa=[I¯C+mc×c×Tmc×Tmc×m1][w˙Ov˙O]

[wvO]×[mmO]=[w×mw×mO+vO×m]

[wvO]×∗[fOf]=[w×fO+vO×fw×f]

vi=vi−1+siq˙i （以基坐标为参考系，连杆i质心的速度）

ai=ai−1+siq¨i+vi×siq˙i （以基坐标为参考系，连杆i质心的加速度）

vi=iXi−1vi−1+iXi−1siq˙i （以连杆i坐标系为参考系，连杆i质心的速度）

ai=iXi−1ai−1+iXi−1siq¨i+vi×siq˙i（以连杆i坐标系为参考系，连杆i质心的加速度）

fi=Iiai+vi×∗Iivi−fexternal（以连杆i坐标系为参考系，关节i传递给连杆i的力）

τi=sifi （为了产生fi，关节i需要输出的力矩或力）