机器人动力学简述 Robot Dynamics

机器人动力学有两个问题需要解决：

1. 动力学正问题——根据关节驱动力矩或力，计算机器人的运动（关节位移、速度和加速度）；
2. 动力学逆问题——已知轨迹运动对应的关节位移、速度和加速度，求出所需要的关节力矩或力。

欧拉-拉格朗日方程 Euler-Lagrange Equation

根据牛顿运动定律，我们来推导欧拉-拉格朗日方程。我们假设质点的运动方程是：
$$m\ddot{y}=f-mg$$
那么，我们可以得到其动能和势能与运动方程的关系。定义动能为 $\mathcal{K}$ 势能为 $\mathcal{P}$。
$$m\ddot{y}=\frac{\mathrm{d}}{\mathrm{d}t}(m\dot{y})=\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial }{\partial \dot{y}}\left(\frac{1}{2}m{\dot{y}}^2\right)=\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{K}}{\partial \dot{y}}$$
$$mg=\frac{\partial }{\partial y}(mgy)=\frac{\partial \mathcal{P}}{\partial y}$$
定义拉格朗日算子：$\mathcal{L}=\mathcal{K}-\mathcal{P}$。
$$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{y}}-\frac{\partial \mathcal{L}}{\partial y}=f$$
下面将要讨论的是上述过程的逆向过程，并用广义坐标 $q$ 和广义力 $\tau$ 来表示。

完整约束 Holonomic Constraint

假设空间中存在一个由 $k$ 个质点组成的系统，各个质点对应的向量为 $r_1,\dots ,r_k$。 如果这些质点在运动的过程中会受到某种形式的约束，那么在分析其动力学时，我们要考虑反约束力(constraint force)，即保持这些约束所需要施加的力。例如刚性无质量连杆两端的两个质点 $r_1,r_2$，他们之间的约束为：
$$(r_1-r_2{)}^T(r_1-r_2)={\ell }^2$$分析这两个质点的运动时，我们希望找到一种方法，它并不需要我们知道具体的约束力。为了进一步分析，我们引入一些必要的术语。
对于有关 $k$ 个坐标的约束，如果具有如下等式约束形式：
$$g_i(r_1,\dots ,r_k)=0,i=1,\dots ,\ell$$那么该约束被称为完整的 (holonomic)。容易看出，对其做微分操作可以得到：
$$\sum _{j=1}^k\frac{\partial g_i}{\partial r_j}\cdot \mathrm{d}{r}_j=0$$
而一个形如
$$\sum _{j=1}^k{\omega }_j\cdot \mathrm{d}{r}_j=0$$的约束是不完整的 (nonholonomic)，如果它不能被积分成上述等式约束形式。如果一个系统受制于 $\ell$ 个非完整约束，那么我们可以认为对于这个约束系统而言，它相比于非约束系统少了 $\ell$ 个自由度。

虚功原理 Principle of Virtual Work

考虑约束 $(r_1-r_2{)}^T(r_1-r_2)={\ell }^2$， 一组与约束相一致的无穷小位移 $\delta r_1,\delta r_2$ 被称为虚位移， 假设 $r_1,r_2$收到扰动分别变为 $r_1+\delta r_1,r_2+\delta r_2$。那么我们有
$$(r_1+\delta r_1-r_2-\delta r_2{)}^T(r_1+\delta r_1-r_2-\delta r_2)={\ell }^2$$忽略掉虚位移的二次项，很容易得到
$$(r_1-r_2{)}^T(\delta r_1-\delta r_2)=0$$ 令 $r_i=r_i(q_1,\dots ,q_n),i=1,\dots ,k$。对上式求微分很容易得到所有的虚位移组合恰是
$$\delta r_i=\sum _{j=1}^n\frac{\partial r_i}{\partial q_i}\delta q_i,i=1,\dots ,k$$其中广义坐标的虚位移 $\delta q_i$ 不受约束， 这也是它们成为广义坐标的原因。

下面我们讨论处于平衡态的受约束系统，平衡态意味着合力为零，同时也意味着虚位移做功为零，因此：
$$\sum _{i=1}^k{F}_i^T\delta r_i=0$$其中合力 $F_i$是两个量之和，即外加作用力 $f_i$ 和约束力 $f_i^a$ 。如果任何一组虚位移做功为零，即
$$\sum _{i=1}^k{f}_i^{aT}\delta r_i=0$$每当一对质点间的约束力方向与质点间径向向量同向上式成立。进一步可得到：
$$\sum _{i=1}^k{f}_i^T\delta r_i=0$$该公式仅与外力有关，不涉及约束力。该公式表示了虚功原理：外力经过任何满足约束条件的虚位移所做的功为零。

虚功原理不具备普适性，它要求 ${\sum }_{i=1}^k{f}_i^{aT}\delta r_i=0$ 成立，也就是约束力不做功。虚功原理适用于刚性是对运动的唯一约束这一情形。

达朗贝尔原理 D’ Alembert’s Principle

公式 ${\sum }_{i=1}^k{f}_i^T\delta r_i=0$ 中虚位移不是互相独立的，因此我们不能从该公式中推出每个系数 $F_i$ 都为零这样的结论。考虑一个不一定处于平衡态的系统，达朗贝尔原理指出：如果在每个质点上引入一个虚构的付加力 $-{\dot{p}}_i$ ，其中 $p_i$ 为质点 $i$ 的动量，那么每个质点将会处于平衡状态。简言之就是：对于任何物体，外加力和运动阻力（惯性力）在任何方向上的代数和均为零。因此，我们可以用 $F_i-{\dot{p}}_i$ 来代替 $F_i$，所得的方程对任意系统适用。如果惯性坐标系的原点为刚体的质心，则达朗贝尔原理归结为：线动量和角动量的导数分别等于外力和外力矩。

去掉约束力，得到如下方程：
$$\sum _{i=1}^k{f}_i^T\delta r_i-\sum _{i=1}^k{\dot{p}}_i^T\delta r_i=0$$
下面将其转化为关于独立广义坐标的表达式，具体推导步骤省略。公式的第一部分：
$$\sum _{i=1}^k{f}_i^T\delta r_i=\sum _{i=1}^k\sum _{j=1}^n{f}_i^T\frac{\partial r_i}{\partial q_j}\delta q_j=\sum _{j=1}^n{\psi }_j\delta q_j$$其中 ${\psi }_i$ 被称为第 $j$ 个广义力。注意它不一定具备力的量纲。公式的第二部分：
$$\sum _{i=1}^k{\dot{p}}_i^T\delta r_i=\sum _{j=1}^n\left\{\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial K}{\partial {\dot{q}}_j}-\frac{\partial K}{\partial q_j}\right\}\delta q_j$$其中 $K={\sum }_{i=1}^k\frac{1}{2}{m}_i{v}_i^T{v}_i$是动能，$v_i={\dot{r}}_i={\sum }_{j=1}^n\frac{\partial r_i}{\partial q_j}{\dot{q}}_j$。最后我们得到：
$$\sum _{j=1}^n\left\{\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial K}{\partial {\dot{q}}_j}-\frac{\partial K}{\partial q_j}-{\psi }_j\right\}\delta q_j=0$$由于虚位移 $q_j$ 相互独立，我们可以断定上述公式各项系数均为零，即
$$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial K}{\partial {\dot{q}}_j}-\frac{\partial K}{\partial q_j}={\psi }_j,j=1,\dots ,n$$如果广义力 ${\psi }_j$ 是外界施加的广义力与势场引入的广义力之和，那么，我们可以进一步修改。假设存在函数 ${\tau }_j$ 以及一个势能函数 $P(q)$，使得：
$${\psi }_j=-\frac{\partial P}{\partial q_j}+{\tau }_j$$那么公式可以被写为如下形式：
$$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial {\dot{q}}_j}-\frac{\partial \mathcal{L}}{\partial q_j}={\tau }_j,j=1,\dots ,n$$
我们得到了欧拉-拉格朗日运动方程。

动力学方程 Dynamic Equation

为了使用欧拉-拉格朗日方程，我们下面推导刚性连杆机器人的动能和势能表达式，使用Denavit-Hartenberg关节变量作为广义坐标。

动能 Kinetic Energy

一个刚性物体的动能是两项之和：整个物体收缩到质心而得到的平移动能以及关于质心的旋转动能。我们将一个附体坐标系附在质心处，那么，这个刚体的动能如下所示：
$$\mathcal{K}=\frac{1}{2}m{v}^{T}v+\frac{1}{2}{\omega }^T\mathcal{I}\omega$$其中 $\mathcal{I}$ 被称为物体的惯性张量 (inertial tensor)，是一个 $3\times 3$ 的对称矩阵。上述线速度 $v$ 和角速度 $\omega$ 均被表示在惯性坐标系里。此时 $S(\omega )=\dot{R}{R}^T$。$R$ 是附体坐标系和惯性坐标系之间的姿态变换。惯性张量取决于物体的位形。用 $I$ 来表示附体坐标系中的惯性张量，那么
$$\mathcal{I}=RI{R}^T$$附体坐标系中的惯性张量 $I$ 是常数矩阵，与物体的运动无关。令物体的质量密度表示为 $\rho (x,y,z)$，那么
$$I=\left(\begin{array}{ccc}{I}_{xx}& I_{xy}& I_{xz}\\ I_{yx}& I_{yy}& I_{yz}\\ I_{zx}& I_{zy}& I_{zz}\end{array}\right)$$其中主惯性矩：
$$I_{xx}=\iiint (y^2+z^2)\rho (x,y,z)\mathrm{d}x\mathrm{d}y\mathrm{d}z$$$$I_{yy}=\iiint (x^2+z^2)\rho (x,y,z)\mathrm{d}x\mathrm{d}y\mathrm{d}z$$$$I_{zz}=\iiint (x^2+y^2)\rho (x,y,z)\mathrm{d}x\mathrm{d}y\mathrm{d}z$$惯性叉积：
$$I_{xy}=I_{yx}=-\iiint xy\rho (x,y,z)\mathrm{d}x\mathrm{d}y\mathrm{d}z$$$$I_{xz}=I_{zx}=-\iiint xz\rho (x,y,z)\mathrm{d}x\mathrm{d}y\mathrm{d}z$$$$I_{yz}=I_{zy}=-\iiint yz\rho (x,y,z)\mathrm{d}x\mathrm{d}y\mathrm{d}z$$如果物体的质量相对于附体坐标系对称，那么惯性叉积均为零。此时，对于这一坐标系，惯性张量是对角型的。此坐标系的各轴被称为惯性主轴，相应的质量矩被称为主惯性矩。
综上n-连杆机器人的动能可表示为：
$$\mathcal{K}=\frac{1}{2}{\dot{q}}^T\left[\sum _{i=1}^n(m_i{J}_{v_i}(q{)}^T{J}_{v_i}(q)+J_{{\omega }_i}(q{)}^T{R}_i(q)I_i{R}_i(q{)}^T{J}_{{\omega }_i}(q))\right]\dot{q}$$即$$\mathcal{K}=\frac{1}{2}{\dot{q}}^{T}D(q)\dot{q}$$
其中我们选择合适的雅克比矩阵 $J$ 使得：
$$v_i=J_{v_i}(q)\dot{q},{\omega }_i=J_{{\omega }_i}(q)\dot{q}$$
$D$ 被称为惯性矩阵，对于任何机械臂，惯性矩阵都是对称且正定的。

势能 Potential Energy

现在考虑势能项。在刚体动力学的情形下，势能的唯一来源是重力：
$${\mathcal{P}}_i=m_i{g}^T{r}_{ci}$$
$$\mathcal{P}=\sum _{i=1}^n{\mathcal{P}}_i=\sum _{i=1}^n{m}_i{g}^T{r}_{ci}$$ $r_{ci}$ 是连杆 $i$ 的质心坐标。

动力学方程 Dynamic Equation

我们专门研究下述两种条件下的欧拉-拉格朗日方程：

1. 动能是向量 $\dot{q}$ 的二次函数，并且形如$$\mathcal{K}=\frac{1}{2}{\dot{q}}^{T}D(q)\dot{q}=\frac{1}{2}\sum _{i,j}{d}_{ij}(q){\dot{q}}_i{\dot{q}}_j$$
2. 势能 $\mathcal{P}=\mathcal{P}(q)$ 与 $\dot{q}$ 无关。

简单起见我们省略详细推导步骤，仅给出结论。欧拉-拉格朗日方程可写为：
$$\sum _{j=1}^n{d}_{kj}(q){\ddot{q}}_j+\sum _{i=1}^n\sum _{j=1}^n{c}_{ijk}(q){\dot{q}}_i{\dot{q}}_j+g_k(q)={\tau }_k,k=1,\dots ,n$$
其中
$$c_{ijk}=\frac{1}{2}\left\{\frac{\partial d_{kj}}{\partial q_i}+\frac{\partial d_{ki}}{\partial q_j}-\frac{\partial d_{ij}}{\partial q_k}\right\}$$被称为第一类Christoffel符号。对于固定的 $k$ 我们有 $c_{ijk}=c_{jik}$。$$g_k=\frac{\partial \mathcal{P}}{\partial q_k}$$上式中广义坐标一阶导数的二次型的系数可分为：形如 ${\dot{q}}_i^2的乘积类型$ 和 $ \dot{q}{i}\dot{q}{j} (i\neq j)$ 的乘积类型。前者被称为离心的 (centrifugal)，后者被称为科里奥利(Coriolis)项。通常欧拉-拉格朗日方程还可写为
$$D(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau$$其中矩阵 $C(q,\dot{q})$中的第$(k,j)$项元素被定义为：
$$c_{kj}=\sum _{i=1}^n{c}_{ijk}(q){\dot{q}}_i=\sum _{i=1}^n\frac{1}{2}\left\{\frac{\partial d_{kj}}{\partial q_i}+\frac{\partial d_{ki}}{\partial q_j}-\frac{\partial d_{ij}}{\partial q_k}\right\}{\dot{q}}_i$$并且重力向量由下式给出：
$$g(q)=[g_1(q),\dots ,g_n(q){]}^T$$

运动方程的性质 Properties

反对称性 Skew Symmetry

矩阵 $N(q,\dot{q})=\dot{D}(q)-2C(q,\dot{q})$是反对称的，即 $n_{jk}=-n_{kj}$ 。

无源性 Passivity

存在一个常数 $\beta \ge 0$ 使得 ${\int }_0^T{\dot{q}}^T(\xi )\tau (\xi )\mathrm{d}\xi \ge -\beta ,\forall T>0$。

参数线性化 Linearity-in-the-parameter

存在 $n\times \ell$ 的函数 $Y(q,\dot{q},\ddot{q})$，以及 $\ell$维向量 $\Theta$，使得欧拉-拉格朗日方程可被写为
$$D(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=Y(q,\dot{q},\ddot{q})\Theta$$函数 $Y(\cdot )$ 被称为回归方程，而 $\Theta \in {\mathbb{R}}^{\ell }$为参数向量。

---

• Thanks Mark W. Spong for his great of Robot Modeling and Control.
• 感谢熊有伦等——《机器人学：建模、控制与视觉》.