【机器人学】机器人开源项目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]

<script type="math/tex; mode=display" id="MathJax-Element-380">{{\hat{v}}_{O}}={{\left[ \begin{matrix} {{w}_{x}} & {{w}_{y}} & {{w}_{z}} & {{v}_{ox}} & {{v}_{oy}} & {{v}_{oz}} \\ \end{matrix} \right]}^{T}}=\left[ \begin{matrix} w \\ {{v}_{O}} \\ \end{matrix} \right]</script>

Spatial Acceleration：

a^O=ddt[wvO]=[w˙v˙O]

<script type="math/tex; mode=display" id="MathJax-Element-381">{{\hat{a}}_{O}}=\frac{d}{dt}\left[ \begin{matrix} w \\ {{v}_{O}} \\ \end{matrix} \right]=\left[ \begin{matrix} {\dot{w}} \\ {{{\dot{v}}}_{O}} \\ \end{matrix} \right]</script>

Spatial Force：

f^O=[noxnoynozfxfyf]T=[nof]

<script type="math/tex; mode=display" id="MathJax-Element-382">{{\hat{f}}_{O}}={{\left[ \begin{matrix} {{n}_{ox}} & {{n}_{oy}} & {{n}_{oz}} & {{f}_{x}} & {{f}_{y}} & f \\ \end{matrix} \right]}^{T}}=\left[ \begin{matrix} {{n}_{o}} \\ f \\ \end{matrix} \right]</script>

Spatial Momentum:

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

<script type="math/tex; mode=display" id="MathJax-Element-383">{{\hat{h}}_{O}}=\left[ \begin{matrix} {{h}_{O}} \\ h \\ \end{matrix} \right]=\left[ \begin{matrix} {{{\bar{I}}}_{C}}w+r\times m{{v}_{C}} \\ m{{v}_{C}} \\ \end{matrix} \right]</script>

Spatial Inertia tensor:

IO=[I¯C+Mr×r×Tmc×Tmc×m1]

<script type="math/tex; mode=display" id="MathJax-Element-384">{{I}_{O}}=\left[ \begin{matrix} {{{\bar{I}}}_{C}}+Mr\times r{{\times }^{T}} & mc\times \\ mc{{\times }^{T}} & m1 \\ \end{matrix} \right]</script>

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

  刚体的运动方程，将牛顿方程和欧拉方程结合：

f=ddt(Iv)=Ia+(v×∗I−Iv×)v=Ia+v×∗Iv

<script type="math/tex; mode=display" id="MathJax-Element-931">f=\frac{d}{dt}\left( Iv \right)=Ia+\left( v\times *I-Iv\times \right)v=Ia+v\times *Iv</script>
  其中定义的一些运算符：

⎡⎣⎢xyz⎤⎦⎥×=⎡⎣⎢0z−y−z0xy−x0⎤⎦⎥

<script type="math/tex; mode=display" id="MathJax-Element-932">\left[ \begin{matrix} x \\ y \\ z \\ \end{matrix} \right]\times =\left[ \begin{matrix} 0 & -z & y \\ z & 0 & -x \\ -y & x & 0 \\ \end{matrix} \right]</script>，

v×∗=[w×0v×w×]

<script type="math/tex; mode=display" id="MathJax-Element-933">v\times *=\left[ \begin{matrix} w\times & v\times \\ 0 & w\times \\ \end{matrix} \right]</script>,

v×=[wv]×=[w×v×0w×]

<script type="math/tex; mode=display" id="MathJax-Element-934">v\times =\left[ \begin{matrix} w \\ v \\ \end{matrix} \right]\times =\left[ \begin{matrix} w\times & 0 \\ v\times & w\times \\ \end{matrix} \right]</script>，

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

<script type="math/tex; mode=display" id="MathJax-Element-935">{{I}_{O}}a=\left[ \begin{matrix} {{{\bar{I}}}_{C}}+mc\times c{{\times }^{T}} & mc\times \\ mc{{\times }^{T}} & m1 \\ \end{matrix} \right]\left[ \begin{matrix} {{{\dot{w}}}_{O}} \\ {{{\dot{v}}}_{O}} \\ \end{matrix} \right]</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-936">\left[ \begin{matrix} w \\ {{v}_{O}} \\ \end{matrix} \right]\times \left[ \begin{matrix} m \\ {{m}_{O}} \\ \end{matrix} \right]=\left[ \begin{matrix} w\times m \\ w\times {{m}_{O}}+{{v}_{O}}\times m \\ \end{matrix} \right]</script>

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

<script type="math/tex; mode=display" id="MathJax-Element-937">\left[ \begin{matrix} w \\ {{v}_{O}} \\ \end{matrix} \right]\times *\left[ \begin{matrix} {{f}_{O}} \\ f \\ \end{matrix} \right]=\left[ \begin{matrix} w\times {{f}_{O}}+{{v}_{O}}\times f \\ w\times f \\ \end{matrix} \right]</script>

vi=vi−1+siq˙i <script type="math/tex" id="MathJax-Element-938">{{v}_{i}}={{v}_{i-1}}+{{s}_{i}}{{\dot{q}}_{i}}</script> （以基坐标为参考系，连杆i质心的速度）
ai=ai−1+siq¨i+vi×siq˙i <script type="math/tex" id="MathJax-Element-939">{{a}_{i}}={{a}_{i-1}}+{{s}_{i}}{{\ddot{q}}_{i}}+{{v}_{i}}\times {{s}_{i}}{{\dot{q}}_{i}}</script> （以基坐标为参考系，连杆i质心的加速度）
vi=iXi−1vi−1+iXi−1siq˙i <script type="math/tex" id="MathJax-Element-940">{{v}_{i}}={}^{i}{{X}_{i-1}}{{v}_{i-1}}+{}^{i}{{X}_{i-1}}{{s}_{i}}{{\dot{q}}_{i}}</script> （以连杆i坐标系为参考系，连杆i质心的速度）
ai=iXi−1ai−1+iXi−1siq¨i+vi×siq˙i <script type="math/tex" id="MathJax-Element-941">{{a}_{i}}={}^{i}{{X}_{i-1}}{{a}_{i-1}}+{}^{i}{{X}_{i-1}}{{s}_{i}}{{\ddot{q}}_{i}}+{{v}_{i}}\times {{s}_{i}}{{\dot{q}}_{i}}</script>（以连杆i坐标系为参考系，连杆i质心的加速度）
fi=Iiai+vi×∗Iivi−fexternal <script type="math/tex" id="MathJax-Element-942">{{f}_{i}}={{I}_{i}}{{a}_{i}}+{{v}_{i}}\times *{{I}_{i}}{{v}_{i}}-{{f}_{external}}</script>（以连杆i坐标系为参考系，关节i传递给连杆i的力）
τi=sifi <script type="math/tex" id="MathJax-Element-943">{{\tau }_{i}}={{s}_{i}}{{f}_{i}}</script> （为了产生 fi <script type="math/tex" id="MathJax-Element-944">{{f}_{i}}</script>，关节i需要输出的力矩或力）