速度与静力

位置矢量的微分（线速度矢量）

对于位置矢量的速度可以看成是用位置矢量描述的空间上一点的速度，如果想在坐标系{A}表达一个{B}中位置矢量的速度，则有：

$^A(^BV_Q)=\frac{^Ad}{dt} {^BQ} \Leftrightarrow \lim_{\Delta t \to 0} \frac{^A Q(t + \Delta t) - ^A Q(t)}{\Delta t}$

因为旋转矩阵可以表示坐标系的映射，因此有：

$^A(^BV_Q)=^A_BR{^BV_Q}$

由于速度矢量为自由矢量，因此不需要使用平移矩阵即可表达。因此当我们已知一个坐标系{A}原点在{W}中的速度表达时，便可通过旋转矩阵得到其在其他坐标系{B}下的表达：

$^B(^WV_{A_{org}})=^BV_{A_{org}}=^B_WR{^WV_{A_{org}}}=^W_BR^{-1}{^WV_{A_{org}}}$

可以看出，上述计算与{A}无关

角速度矢量

线速度描述的是一个点的速度属性，而角速度则描述了刚体的自转运动（坐标系固连在刚体上）

由于自转肯定是绕着某一轴，因此对于

$^A\Omega_B$

其方向则代表了{B}相对于{A}的瞬时旋转轴的方向，大小则表示旋转速度

对于线速度和角速度，我们规定：

$v_A=^WV_{A_{org}}$ $\omega_A = ^W\Omega_A$

刚体（上一点）的速度表达

上面我们讨论的线速度与角速度都是针对于坐标系的原点，现在讨论在刚体中的情况。

假设参考坐标系为{A}，将{B}固连在一刚体上，刚体上存在一点Q（不在原点上），则有：

$^AV_Q=(^AV_{B_{org}}+^A_BR^BV_Q)+(^A\Omega_B \times^A_BR^BQ)$

前一个括号表示线速度项，后一个表示角速度项

关于线速度、角速度和刚体的速度

线速度描述的是空间上一个点沿某轨迹运动的快慢与方向，在坐标系中则为位置矢量Q发生变化的大小与方向；而角加速度针对的是坐标系的自转情况；将这两个概念引入到刚体中，对于刚体速度的描述，即是刚体上某一点Q在参考坐标系{A}下的坐标变化情况与自转情况，说的更全面一点，即是：在{A}中观测{B}中一位置矢量Q的速度变化，{B}代表这个刚体，Q代表刚体上一点

另外，由于我们通常都是讨论刚体上的固定一点，因此其位置矢量的微分为0，所以其实线速度项就是坐标系{B}原点的移动速度：

$^AV_Q=^AV_{B_{org}}+(^A\Omega_B \times^A_BR^BQ)$

连杆间的速度传递

由于连杆是链式结构，每一个连杆的运动都与其相邻杆有关，因此对于i+1连杆，其速度就是连杆i的速度加上附加到关节i+1上的新速度分量（对于连杆的速度，我们通常讨论的是连杆坐标系原点的线速度和连杆坐标系的角速度）

对于角速度，根据上述规则，则有：

$^i\omega_{i+1}={^i\omega}_{i}+{^i_{i+1}R \dot{\theta}_{i+1}{^{i+1}\hat{Z}_{i+1}}}$

此处乘Z是为了让关节角加速度匹配R矩阵维度，并且该加速度只在Z轴上存在，将上式两边同左乘i相对于i+1的旋转矩阵，则有：

$^{i+1}\omega_{i+1}=^{i+1}_{i}R {^i\omega}_{i}+{ \dot{\theta}_{i+1}{^{i+1}\hat{Z}_{i+1}}}$

对于线速度，其等于{i}原点线速度加上由于i角速度引起的新分量：

${^iv_{i+1}}={^iv_{i}}+{^i\omega}_{i}\times{^iP_{i+1}}$

其中P为两坐标系原点形成的矢量，同样的，通过左乘旋转矩阵可得到{i+1}下的速度表达：

${^{i+1}v_{i+1}}={^{i+1}_iR}({^iv_{i}}+{^i\omega}_{i}\times{^iP_{i+1}})$

对于移动关节，则额外有：

${^{i+1}v_{i+1}}={^{i+1}_iR}({^iv_{i}}+{^i\omega}_{i}\times{^iP_{i+1}})+\dot{d}_{i+1}\hat{Z}_{i+1}$

雅可比矩阵

对于一个6自由度机器人，我们可以设其正运动学方程为：

$x=f(q)$

其中x是一个包含位姿与姿态的6x1向量，q为机器人的六个关节变量组成的6x1向量，因此f为一个6x6矩阵，代表了x每一个元素与q的非线性函数关系，将上式两边同时对时间求导，则有：

$\text{v}=J(q)\dot{q}$

此时，该式表示了连杆速度v（笛卡尔表示）（取决于选择的连杆和参考坐标系）与关节角速度之间的映射关系

对于常见的求末端速度的雅各比矩阵，有：

$^0\text{v}_6=\begin{bmatrix}^0v_6\\^0\omega_6 \end{bmatrix}={^0J(q)}\dot{q}$

此时，该矩阵代表了在{0}坐标下，各关节角速度对末端速度（线+角）的共享

奇异性

因此，当已知机器人结构（等同于已知雅各比矩阵表达式），各关节转角（可求出J具体值）和笛卡尔速度时，便可以求出各关节速度：

$\dot{q}=J^{-1}(q)\text{v}$

但此时要求J是可逆的，当机器人出现奇异位姿时，会导致J的行列式为0，此时，J为奇异的

参考系的变换

对于在{A}中表达的某雅各比矩阵，若要变换到{B}中表达，则有：

$^BJ(q)=\begin{bmatrix}{^B_AR} &0 \\ 0 &{^B_AR} \end{bmatrix} {^AJ(q)}$

力域中的雅可比

当一个F作用与机器人时，我们想要求出各关节需输出多少力矩以保持机器人的静力平衡。

当一个笛卡尔广义力F（形式类似于笛卡尔速度）作用于机器人末端时，根据功率守恒，有：

$F^T\text{v}=\tau\dot{q}$

将上一节中计算的速度雅可比方程带入v中则最终有：

$\tau={^0J^T}{^0F}$

于速度雅可比方程一样，F与J需在同一坐标系下表达

力-力矩变换

对于笛卡尔广义力的在不同坐标系下的表示，有：

$\begin{bmatrix} {}^A F_A \\ {}^A N_A \end{bmatrix} = \begin{bmatrix} {}^A_B R & 0 \\ {}^A P_{BORG} \times {}^A_B R & {}^A_B R \end{bmatrix} \begin{bmatrix} {}^B F_B \\ {}^B N_B \end{bmatrix}$

机器人动力学

假设已知关节的转角、速度和加速度，求驱动关节所需要的力矩（逆动力学）

刚体的加速度

对上一章“刚体的速度表达”中两边同时对时间进行求导最终可得到在{A}中表达一个{B}中的常量矢量Q的加速度公式：（常量Q代表着其在{B}中的速度、加速度表达均为0）

${^A\dot{V}_Q}={^A\dot{V}_{Borg}}+{^A\Omega_B}\times({^A\Omega_B}\times{}^A_B RQ)+{^A\dot{\Omega}_B}\times{}^A_B RQ$

刚体的质量分布

在一个刚体绕某个轴做旋转运动时，需要一种能够表征其质量分布的方法，因此我们引入惯性张量：

$I = \begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{yx} & I_{yy} & I_{yz} \\ I_{zx} & I_{zy} & I_{zz} \end{bmatrix}$

其中，xx，yy，zz为惯量矩，其余交叉项为惯量积；这六个参量取决于其所在坐标系（即刚体中的{B}）的位姿；

平移移轴定理

该定理描述了一个以刚体质心为原点的坐标系平移到另一个坐标系时的惯性张量变换关系：

${^AI}={^CI}+m(P^T_cP_cI_3-P_cP^T_c)$

其中I代表要变换的惯量矩/惯量积，Pc为质心坐标系{A}原点位置（3x1），I3为3x3单位矩阵

牛顿-欧拉法

牛顿方程与欧拉方程

牛顿方程描述了作用与质心的力F与质心加速度之间的关系：

$F=m\dot{v}$

对于加速度的计算已在“刚体的加速度”小节中给出（没错，就是包含了角加速度的那个加速度）

欧拉方程描述了惯量、力矩与角加速度的关系：

$N=I\dot{\omega}+\omega \times I\omega$

外向迭代法（计算速度、加速度以得到连杆上的力、力矩）

由连杆间速度传递，对角速度求导可得：

${^{i+1}\dot{\omega}_{i+1}}=\frac{d(^{i+1}_iR^i\omega_i)}{dt}+{ \ddot{\theta}_{i+1}{^{i+1}\hat{Z}_{i+1}}}={}^{i+1}_iR ^i\dot{\omega}_i+{}^{i+1}_iR^i\omega_i\times{ \dot{\theta}_{i+1}{^{i+1}\hat{Z}_{i+1}}}+{ \ddot{\theta}_{i+1}{^{i+1}\hat{Z}_{i+1}}}$

因为R本身也与时间相关，因此其导数也有特殊形式，上式记住就行

由“刚体加速度”中的公式可得每个连杆坐标系原点和其质心的加速度：

${^{i+1}\dot v_{i+1}}=^{i+1}_iR({^i\dot v_i} + ^i\omega_i\times {}^iP_{i+1}+ ^i\omega_i \times(^i\omega_i \times {}^iP_{i+1}))$ ${^i\dot v_{C_i}}= {^i\dot v_{i}} + ^i\dot{\omega}_i \times {^iP_{C_i}}+^i\omega_i \times (^i\omega_i+{^iP_{C_i}})$

假设质心坐标系坐标轴方位与连杆坐标系{i}方向相同

得出角加速度和加速度后，即可根据牛顿、欧拉方程计算出每个作用在每个连杆上的惯性力和力矩

内向迭代法

在得到作用在连杆上的力和力矩后，即可根据力、力矩平衡计算出连杆i-1对连杆i施加的力f和力矩n，最后得到关节i的驱动力矩：

${^if_i}={^i_{i+1}R}f_{i+1}+{}^iF_i$ ${}^in_i={}^iN_i+{^i_{i+1}R} \ n_{i+1}+{^iP_{C_i}}\times {}^iF_i+{}^iP_{i+1}\times{^i_{i+1}R}{^{i+1}f_{i+1}}$

最终，对于旋转关节：

$\tau_i={}^in_i^T\hat{Z}_i$

对于移动关节：（此时tau代表线性驱动力）

$\tau_i={}^if_i^T\hat{Z}_i$

对于末端未与环境接触的机器人，其f_i+1与n_i+1为0

总流程

结合外向和内向迭代，便可在已知关节转角、及其一阶、二阶导的情况下，计算出每个关节的驱动力矩：

外向迭代，依次求连杆上惯性力和力矩。从0连杆推到n连杆（0连杆（基座）的角速度、角加速度为0）
- 角速度计算(不需计算线速度)
- 角加速度计算
- 连杆坐标系原点加速度计算
- 连杆质心加速度计算
- 惯性力、力矩计算
内向迭代，依次求各关节驱动力矩。从n连杆推到0连杆
- i-1连杆对i连杆的作用力计算
- i-1连杆对i连杆的作用力矩计算
- 根据关节属性计算驱动力矩/力

考虑重力存在的情况

为基座加速度假想一个与重力矢量G大小相等，方向相反的加速度，即：

${^0\dot{v}_0}=G$

动力学方程

状态空间方程

对于用牛顿-欧拉法分析得到的关节驱动力矩矩阵，其动力学方程可以表示为：

$\tau=M(\theta)\ddot{\theta}+V(\theta,\dot{\theta})+G(\theta)$

其中M为nxn质量矩阵，V为nx1的离心力与哥氏力矢量，G为nx1的重力矢量

假设有如下关节力矩表示：

则可得到：

笛卡尔状态空间方程

当我们项将笛卡尔变量的一般形式应用于动力学方程中时，有：

拉格朗日法

牛顿-欧拉法是通过分析作用于连杆的之间的约束力和力矩而进行动力学求解，可以被认为是一种力平衡方法。而拉格朗日公式则是一种基于能量的动力学方法。

拉格朗日函数描述了一个系统的动能和势能的差值：

$L(\theta,\dot{\theta})=k(\theta,\dot{\theta})-u(\theta)$

对于连杆i的动能，可表示为：

$k_i=\frac1 2 m_iv^2_{Ci}+\frac1 2 I_i\omega^2_i$

对于连杆i的势能，可表示为：

$u_i=m_igh+C$

其中C为势能参考0点所带来的常数，可取任意值，通常参考点为基座，则C为0（由于动力学方程中仅出现势能对θ的偏导，因此C无关紧要）

由能量守恒，我们可以得知外力做功=系统机械能变化量，对等式两边同时对位移（θ）求导，即有拉格朗日动力学方程：

$\tau=\frac d {dt} \frac{\partial L}{\partial \dot{\theta}}-\frac{\partial L}{\partial \theta}=\frac d {dt} \frac{\partial k}{\partial \dot{\theta}}-(\frac{\partial k}{\partial \theta}-\frac{\partial u}{\partial \theta})$