前言
本文整理了一些关于《现代机器人：力学，规划，控制》（Modern Robotics: Mechanics, Planning, and Control）的动力学部分内容，为机器人学的部分的第三篇笔记，之前的现代机器人学运动学部分在这里
动力学方法
动力学方法有：
- 拉格朗日公式 Lagrangian formulation
- 牛顿-欧拉公式 Newton-Euler formulation
拉格朗日方法
力学系统的拉格朗日函数等于动能减势能。这里势能只与关节角（位置）有关，动能与关节角和速度（位置和速度）有关。
\[L(\theta, \dot\theta) = K(\theta, \dot\theta)-P(\theta)\]下面给出拉格朗日运动方程（无推导）：
\[\tau = \frac{d}{dt}\frac{\partial L}{\partial \dot\theta} - \frac{\partial L}{\partial \theta}\]求得拉格朗日方程关于各个关节的分量，并进行整理如下：
\[\tau = M(\theta)\dot\theta + c(\theta,\dot\theta) + g(\theta)\]其中，
- M：$n\times n$的质量矩阵
- c：$n\times 1$的速度乘积项
- 单个关节速度平方的：向心项
- 两个不同关节速度乘积的：科里奥利项
- g：$n\times 1$的重力项
若考虑外力的作用,则要在最后加上一项,即为:
\[\tau = M(\theta)\dot\theta + c(\theta,\dot\theta) + g(\theta) + J^T(\theta)F_{tip}\]速度乘积项
速度乘积项c可以写成$\dot\theta^T\Gamma(\theta)\dot\theta$，$\Gamma(\theta)$是$n\times n\times n$的三维矩阵，可以用质量矩阵对关节变量求导计算：
\[\Gamma_{ijk}(\theta) = \frac{1}{2}(\frac{\partial m_{ij}}{\partial\theta_k} + \frac{\partial m_{ik}}{\partial\theta_j} - \frac{\partial m_{jk}}{\partial\theta_i})\]速度乘积项也可以写成$C(\theta,\dot\theta)\dot\theta$，这里$C(\theta,\dot\theta)$表示为科里奥利矩阵和关节速度向量的乘积
\[C_{ij}(\theta,\dot\theta) = \underset{k=1}{\overset{n}{\Sigma}}\Gamma_{ijk}(\theta)\dot\theta_k\]质量矩阵
质量矩阵永远是正的，因为对于如下动能不能为负
\[K = \frac{1}{2}\dot\theta^TM(\theta)\dot\theta\]并且质量矩阵是对称的。且质量矩阵与$\theta$相关，因为关节处的惯量取决于手臂的伸展状态。
通过质量矩阵，可以将关节加速度单位圆映射到关节力矩的椭圆。如果质量矩阵可逆，则可以反过来映射。
下面定义有效质量，用来描述在该位形下质量对力的影响。
\[\frac{1}{2}V^T\Lambda(\theta)V = \frac{1}{2}\dot\theta^TM(\theta)\dot\theta\]如果雅可比矩阵可逆，就可以表示为：
\[\Lambda = J^{-T}MJ^{-1}\]
这个矩阵完成的映射是从末端加速度空间到关节受力。
牛顿-欧拉方法
单刚体动力学
考虑一个刚体中存在质量的单元：$m_1,m_2,……$坐标系[b]在刚体的质心上。后面的所有讨论在以b为参考系的基础上。
由于b是之心，因此：
\[\underset{i}{\Sigma}m_ip_i = 0\]然后可以求得刚体上每一个质量点的速度、加速度：
\[\dot p_i = v_b + \omega_b\times p_i\] \[\ddot p_i = \dot v_b + \dot\omega_b\times p_i + \omega_b\times(v_b + \omega_b\times p_i)\]通过加速度，可以根据牛二求得质量点的力、力矩（这里方括号运算代表叉乘）
\[f_b = m_i\ddot p_i = m(\dot v_b + [\omega_b]v_b) 这里m是质量\] \[m_i = [p_i]f_i = I_b\dot\omega_b + [\omega_b]I_b\omega_b这里m_i是力矩\]其中$I_b$为惯性矩阵，$I_b = -\underset{i}{\Sigma}m_i[p_i]^2\in R^{3\times3}$当刚体质量连续分布时，将求和符号换成积分符号并将$m_i$换成$\rho(x,y,z)$
惯性矩阵和质量矩阵一样，也是对称且正定的旋转刚体的动能为$K = \frac{1}{2}\omega_b^TI_b\omega_b$。惯性矩阵对角元素称为惯性矩，非对角元素称为惯性积，存在一组坐标轴，使得惯量矩阵正好为对角矩阵，此时这个坐标轴称为惯性主轴，对角线上的元素为惯性矩阵的特征值，且主轴分别为三个特征向量。
如果[b]与惯性主轴重合，方程可以简化为：
\[m_b = \left( \begin{matrix} I_{xx}\dot\omega_x + (I_{zz}-I_{yy})\omega_y\omega_z \\ I_{yy}\dot\omega_x + (I_{xx}-I_{zz})\omega_y\omega_z \\ I_{zz}\dot\omega_z + (I_{yy}-I_{xx})\omega_x\omega_y \end{matrix} \right)\]综上，通过刚体质量、惯性矩阵，可以将单刚体的运动旋量转换为力旋量。
\[\left( \begin{matrix}m_b\\f_b\end{matrix}\right) = \left(\begin{matrix}I_b & 0\\0 & mI\end{matrix}\right)\left(\begin{matrix}\dot\omega_b \\\dot v_b\end{matrix}\right) + \left(\begin{matrix} [\omega_b] & 0\\ 0 & [\omega_b] \end{matrix} \right)\left( \begin{matrix} I_b & 0\\ 0 & mI \end{matrix} \right)\left( \begin{matrix} \omega_b\\ v_b \end{matrix} \right)\]这里，由惯性矩阵和质量组成的矩阵为$G_b\in R^{6\times6}$，也是对称且正定的这样，刚体的动能可以写成：
\[K = \frac{1}{2}V_b^TG_bV_b\]下面介绍一个用于六维向量的运算方法：
\[[ad_V] = \left( \begin{matrix} [\omega] & 0\\ [v] & [\omega] \end{matrix} \right)\in R^{6\times 6}\]通过这个形式可以将上上上式进行处理，从而化简。因为$v\times v = 0$且$[v]^T = -[v]$，所以这里就可以凭空多出来一个$[v_b]$：
\[\left(\begin{matrix} [\omega_b] & 0\\ 0 & [\omega_b] \end{matrix} \right)\left( \begin{matrix} I_b & 0\\ 0 & mI \end{matrix} \right)\left( \begin{matrix} \omega_b\\ v_b \end{matrix} \right) = \left(\begin{matrix} [\omega_b] & [v_b]\\ 0 & [\omega_b] \end{matrix} \right)\left( \begin{matrix} I_b & 0\\ 0 & mI \end{matrix} \right)\left( \begin{matrix} \omega_b\\ v_b \end{matrix} \right) = -\left(\begin{matrix} [\omega_b] & 0\\ [v_b] & [\omega_b] \end{matrix} \right)\left( \begin{matrix} I_b & 0\\ 0 & mI \end{matrix} \right)\left( \begin{matrix} \omega_b\\ v_b \end{matrix} \right)\]然后利用上述的伴随变换$[ad_V]$化简。
上面这些概念给我有点搞晕了，这里澄清一下。 伴随矩阵是$[ad_V]$，伴随变换是$[ad_{V_1}]V_2 = ad_{V_1}(V_2)$，李括号是$[V_1,V_2] = [V_1][V_2] - [V_2][V_1],它等于
\[\begin{matrix}[\omega'] & v'\\0 & 0\end{matrix}\]然后将这个（四维）矩阵里的$\omega ‘$和$v’$提出来作为一个新的旋量，则它等于$[ad_{V_1}]V_2$
原文讲的更清楚：
反正最后原方程可以化为如下的六维向量方程：
\[F_b = G_b\dot V_b - [ad_{V_b}]^TG_bV_b\]将刚体的坐标系从[b]转换到[a]，通过动能不变，用到转换矩阵的伴随表示：
\[\frac{1}{2}V_a^TG_aV_a = \frac{1}{2}V_b^TG_bV_b = \frac{1}{2}V_a^T[Ad_{T_{ba}}]^TG_b[Ad_{T_{ba}}]V_a\]发现在哪个坐标系中形式都相同：
\[F_a = G_a\dot V_a - [ad_{V_a}]^TG_aV_a\]通过上述方程，可以得到正动力学方程，即输入当前的运动和力旋量，计算加速度
\[\dot V_b = G_b^{-1}(F_b + [ad_{C_b}]^TG_bV_b)\]牛顿-欧拉解逆动力学 Newton-Euler inverse dynamics
对于一个含n个连杆的机器人，每个连杆的质量都在重心，世界坐标系为[0]，第i个连杆上坐标系为[i]，末端执行器坐标系为[n+1]。
逆动力学的任务是通过给定的关节位置$\theta$、速度$\dot\theta$、加速度$\ddot\theta$计算关节力矩$\tau$
首先正向迭代，计算每个连杆的位形、运动旋量和加速度。给定关节位置$\theta$、速度$\dot\theta$、加速度$\ddot\theta$，从连杆1开始。首先，计算坐标系$[i-1]$相对于$[i]$的位形
\[T_{i,i-1} = e^{-[A_i]\theta_i}M_{i,i-1}\]连杆i的运动旋量$V_i$为$V_{i-1}$加上关节速度$\dot\theta_{i-1}$增加的速度。（参考徐均为{i}）
\[V_i = A_i\dot\theta_i + [Ad_{T_{i,i-1}}]V_{i-1}\]然后计算加速度$\dot V_i$，等于连杆i-1的加速度加上由关节加速度$\ddot\theta_{i-1}$引起的加速度，再加由$\dot\theta_i$和$V_i$引起的速度乘积项（参考徐均为{i}）
\[\dot V_i = A_i\ddot\theta_i + [Ad_{T_{i,i-1}}]\dot V_{i-1} + [ad_{V_i}]A_i\dot\theta_i\]到此，正向迭代完成，开始执行逆向迭代，从关节n开始返回到关节1。
首先计算连杆i所需的力旋量$F_i$，为连杆i+1所需的力旋量$F_{i+1}$加上连杆i的加速度所需的力旋量，参考系为{i}。用到的公式为：
\[G_i\dot V_i - ad_{V_i}^T(G_iV_i) = F_i-Ad^T_{T_{i+1,i}}(F_{i+1})\]然后计算$\tau_i$，即力旋量$F_i$沿关节螺旋轴的分量（由关节电机提供）。
\[\tau_i = F_i^TA_i\]牛顿-欧拉逆动力学部分结束，下面是对原文一些内容解释
将关节i为零位的时候坐标系[i-1]相对于[i]的变换矩阵定义为：$M_{i,i-1}$。$a_I$为关节i的螺旋轴（参考系为[i]）。$F_{n+1}$为末端执行器的力旋量。$\dot V_0$为基座加速度，与重力加速度相反。
牛顿-欧拉法的优点为：1. 不用计算微分。 2. 使用递归特性，效率很高。
牛顿-欧拉解正动力学 Newton-Euler forward dynamics
正动力学任务是给定关节扭矩$\tau$、关节位置$\theta$、以及末端执行器力旋量$F_{tip}$的情况下，求解$\ddot\theta$
首先，使用逆动力学来计算关节加速度$\ddot\theta = 0$时的关节扭矩。
\[\tau = ~~M(\theta)\ddot\theta~~ + c(\theta,\dot\theta) + g(\theta) + J^T(\theta)F_{tip}\]然后，利用逆动力学方法求解质量矩阵$M(\theta)$，将逆动力学算法调用n次（每个关节一次），每次将重力、末端力旋量、关节速度和其余所有关节加速度设为0，关节i的加速度$\ddot\theta = 1$。这样算出来的关节力矩向量$\tau$就和质量矩阵M的第i列相同。这样调用n次之后就可以得到质量矩阵。
\[\tau = M(\theta)~~\ddot\theta~~ + ~~c(\theta,\dot\theta) + g(\theta) + J^T(\theta)F_{tip}~~\]至此，调用了$n+1$次逆动力学，得到了$M(\theta)$、$c(\theta,\dot\theta)$、$g(\theta)$、$J^T(\theta)F_{tip}$，又因为$\tau$已知，就可以求出$\ddot\theta$了
下面通过关节空间的动力学方程给出在任务空间的动力学方程：
\[\text{关节空间}\tau = M(\theta)\ddot\theta + h(\theta,\dot\theta)\] \[\text{任务空间}F = \Lambda(\theta)\dot V + \eta(\theta,V)\]其中，
\[\Lambda(\theta) = J^{-T}M(\theta)J^{-1}\] \[\eta(\theta,V) = J^{-T}h(\theta,J^{-1}V)-\Lambda(\theta)\dot JJ^{-1}V\]这样给定关节角、末端执行器运动旋量和加速度、质量矩阵就可以求得末端执行器的力旋量了。
约束动力学 constrained dynamics
在有约束的情况下，关节力、力矩动力学方程为：
\[\tau = M(\theta)\ddot\theta + h(\theta,\dot\theta) + \tau_{con}\]其中，因为约束力不做功，因此$\tau^T_{con}\dot\theta = 0$
约束函数用$b(\theta)$表示，为关节位置满足的约束方程组成的矩阵。将它求导得到速度约束$\frac{\partial b}{\partial \theta}\dot\theta = 0$，令它的雅可比矩阵为$A=\frac{\partial b}{\partial \theta}$
约束力矩必然是矩阵A各行的线性组合，因此$\tau_{con} = A^T(\theta)\lambda,\lambda\in R^k$，这里约束数为k，$\lambda$是k维的拉格朗日乘子。
由于速度约束必须始终满足，因此加速度约束也始终满足：
\[A(\theta)\ddot\theta + \dot A(\theta)\dot\theta = 0\]且
\[\tau = M(\theta)\ddot\theta + h(\theta,\dot\theta) + A^T(\theta)\lambda\]解如上两个方程（存在n+k个等式和n+k个未知量），可以解得$\lambda$和 $\ddot\theta$或$\tau$（取决于是正动力学还是逆动力学，正动力学是已知$\tau$求$\ddot\theta$，逆动力学是已知$\ddot\theta$求$\tau$）
下面引入一个投影矩阵P可以消去k维的拉格朗日乘数（直接跳过推导）：
\[P(\theta) = I-A^T(AM^{-1}A^T)^{-1}AM^{-1}\in R^{n\times n}, \ \ rank\ n-k\]则动力学方程变为：
\[P\tau = P(M\ddot\theta + h)\]由于P不可逆，因此不可以两边左乘$P^{-1}$
实际动力学
机器人的驱动关节由编码器、电机、减速箱构成。减速比为$G>1$由于磨擦存在，扭矩放大系数略小于G
电机定子连接到连杆i-1，减速器输出连接到杆i，但有时减速器输出并不与电机同轴，且转子的速度与关节不同，因此计算连杆i的质量和惯量时，惯量为转子惯量的$G^2$倍（表观惯性），但质量由于较小可忽略。
