基于欧拉-拉格朗日方程的机器人动力学模型

摘要

如题。

（转自我的CSDN博客）

机器人动力学方程

机器人动力学方程是描述机器人力和运动之间的关系的方程。只描述力和运动的关系，不考虑产生运动的力和扭矩。

欧拉 - 拉格朗日方程

欧拉-拉格朗日方程（OL）描述了处于完整约束下，并且约束力满足虚功原理的机械系统的力和运动随时间的变化。

有两种推导方法，先介绍使用牛顿第二定律的推导方法。

根据牛顿第二定律，某质点的运动方程是： $m \ddot{y} = f - m g$ 先对时间求导，再对 $\dot{y}$ 求偏导，方程左侧可以写为： $m \ddot{y} = \frac{d}{d t} (m \dot{y}) = \frac{d}{d t} \frac{\partial}{\partial \dot{y}} (\frac{1}{2} m {\dot{y}}^{2}) = \frac{d}{d t} \frac{\partial K}{\partial \dot{y}}$ 其中 $K = \frac{1}{2} m {\dot{y}}^{2}$ ，是动能。

接着将重力表示为： $m g = \frac{\partial}{\partial y} (m g y) = \frac{\partial P}{\partial y}$ 其中 $P = m g y$ 表示重力势能。

定义拉格朗日算子 $L$ ，表示系统动能与势能之差： $L = K - P = \frac{1}{2} m {\dot{y}}^{2} - m g y$ 并且有： $\frac{\partial L}{\partial \dot{y}} = \frac{\partial K}{\partial \dot{y}}, \frac{\partial L}{\partial y} = - \frac{\partial P}{\partial y}$

则初始的质点运动方程可化为： $\frac{d}{d t} \frac{\partial K}{\partial \dot{y}} = f - \frac{\partial P}{\partial y}$ 即： $\frac{d}{d t} \frac{\partial L}{\partial \dot{y}} - \frac{\partial L}{\partial y} = f$ 此方程被称为欧拉-拉格朗日方程。

推广到n自由度的系统，得到： $\frac{d}{d t} \frac{\partial L}{\partial {\dot{q}}_{k}} - \frac{\partial L}{\partial q_{k}} = τ_{k}$ 其中 $τ_{k}$ 是与广义坐标 $q_{k}$ 相关的力。

动能与势能

欧拉-拉格朗日方程可以直接用来推导动力学方程，前提是我们能够以一组广义坐标来表示该系统的动能和势能。如果要让这能够得到实际应用，那么我们就必须能够针对一个n连杆机器人计算出他的动能和势能。接下来将推到刚性连杆机器人动能和势能的表达式。

动能表示

刚体的动能可表示为平移动能和关于质心的旋转动能之和： $K = \frac{1}{2} m v^{T} v + \frac{1}{2} ω^{T} Z ω$ $Z$ 表示物体的惯性张量，是一个3*3的对称矩阵。

$Z = R I R^{T}$ ，R是附体坐标系与惯性坐标系之间的姿态变换。

$I$ 是附体坐标系中的惯性张量，仅取决于物体的形状和质量分布，与物体运动无关。

速度 $v$ 和角速度 $ω$ 需要转置，因为要考虑多个维度的方向。连杆上任意一点的现速度和角速度可通过雅可比矩阵和关节速度（关节变量的导数）来表示： $v_{i} = J_{v_{i}} q \dot{q} ω_{i} = J_{ω_{i}} q \dot{q}$

机器人总动能可表示为： $K = \frac{1}{2} {\dot{q}}^{T} \sum_{i = 1}^{n} [m_{i} {(J_{v_{i}} (q))}^{T} J_{v_{i}} (q) + (J_{ω_{i}} (q))^{T} R_{i} (q) I_{i} (R_{i} (q))^{T} J_{ω_{i}} (q)] \dot{q}$ 用 $D (q)$ 来表示机器人的惯性矩阵： $D (q) = \sum_{i = 1}^{n} [m_{i} {(J_{v_{i}} (q))}^{T} J_{v_{i}} (q) + (J_{ω_{i}} (q))^{T} R_{i} (q) I_{i} (R_{i} (q))^{T} J_{ω_{i}} (q)] K = \frac{1}{2} {\dot{q}}^{T} D (q) \dot{q}$ 机器人惯性矩阵 $D (q)$ 有如下特点：

只与机器人构型有关
对称且正定
动能总是非负的

势能表示

在刚体动力学情形下，势能总是来源于重力。假设物体质量集中在质心，计算第 $i$ 个连杆的势能： $P_{i} = m_{i} g^{T} r_{c i}$ $g$ 是惯性坐标系中的重力向量， $r_{c i}$ 是连杆 $i$ 的质心坐标。机器人总势能为： $P = \sum_{i = 1}^{n} P_{i} = \sum_{i = 1}^{n} m_{i} g^{T} r_{c i}$ 在m、g保持不变的情况下，机器人势能只与广义坐标 $r_{c i}$ 有关。

运动方程

上面我们得到了如下结果：

系统动能是关于广义速度（坐标微分）的二次函数： $K = \frac{1}{2} {\dot{q}}^{T} D (q) \dot{q} = \frac{1}{2} \sum_{i, j}^{n} d_{i, j} (q) {\dot{q}}_{i} {\dot{q}}_{j}$ 系统势能是关于广义坐标的函数，且与广义速度无关： $P = \sum_{i = 1}^{n} m_{i} g^{T} r_{c i}$ 欧拉-拉格朗日算子为： $L = K - P = \frac{1}{2} \sum_{i, j}^{n} d_{i, j} (q) {\dot{q}}_{i} {\dot{q}}_{j} - P (q)$ 欧拉-拉格朗日方程为： $\frac{d}{d t} \frac{\partial L}{\partial {\dot{q}}_{k}} - \frac{\partial L}{\partial q_{k}} = τ_{k}$ 其中： $\frac{\partial L}{\partial {\dot{q}}_{k}} = \sum_{j} d_{k j} {\dot{q}}_{j} \frac{d}{d t} \frac{\partial L}{\partial {\dot{q}}_{k}} = \sum_{j} d_{k j} {\ddot{q}}_{j} + \sum_{j} \frac{d}{d t} d_{k j} {\dot{q}}_{j} = \sum_{j} d_{k j} {\ddot{q}}_{j} + \sum_{i, j} \frac{\partial d_{k j}}{\partial q_{i}} {\dot{q}}_{i} {\dot{q}}_{j} \frac{\partial L}{\partial q_{k}} = \frac{1}{2} \sum_{i, j} \frac{\partial d_{i, j}}{\partial q_{k}} {\dot{q}}_{i} {\dot{q}}_{j} - \frac{\partial P}{\partial q_{k}}$ 因此对于每个 $k = 1, 2, . . . n$ ，欧拉-拉格朗日方程可以写成： $\sum_{j} d_{k j} {\ddot{q}}_{j} + \sum_{i, j} {\frac{\partial d_{k j}}{\partial q_{i}} - \frac{1}{2} \frac{\partial d_{i, j}}{\partial q_{k}}} {\dot{q}}_{i} {\dot{q}}_{j} - \frac{\partial P}{\partial q_{k}} = τ_{k}$ 即： $\sum_{j} d_{k j} {\ddot{q}}_{j} + \sum_{i, j} \frac{1}{2} {\frac{\partial d_{k j}}{\partial q_{i}} + \frac{\partial d_{k j}}{\partial q_{i}} - \frac{\partial d_{i, j}}{\partial q_{k}}} {\dot{q}}_{i} {\dot{q}}_{j} - \frac{\partial P}{\partial q_{k}} = τ_{k}$ 定义Christoffel symbol： $c_{i j k} = c_{j i k} = \frac{1}{2} {\frac{\partial d_{k j}}{\partial q_{i}} + \frac{\partial d_{k j}}{\partial q_{i}} - \frac{\partial d_{i, j}}{\partial q_{k}}}$ 定义广义重力： $g_{k} = \frac{\partial P}{\partial q_{k}}$ 最终得到欧拉-拉格朗日方程： $\sum_{j} d_{k j} (q) {\ddot{q}}_{j} + \sum_{i, j} c_{i j k} (q) {\dot{q}}_{i} {\dot{q}}_{j} - g_{k} (q) = τ_{k}$ 方程左侧三项分别为：

广义坐标的二阶导数：惯性项
广义坐标一阶导数的二次型：离心力项+哥氏力项
广义位置（0阶导数）重力项

方程可简写为： $D (q) \ddot{q} + C (q, \dot{q}) \dot{q} + g (q) = τ$

推导平面2关节机器人的动力学模型

现在考虑下图中带有两个转动关节的平面机械臂。

要使用刚刚得到的欧拉-拉格朗日方程，就要与关节位置和关节速度相关的三个量： $D (q), C (q, \dot{q}), g (q)$ 。

首先使用雅可比矩阵来计算动能，计算平移速度： $v_{c 1} = J_{v_{c 1}} \dot{q} v_{c 2} = J_{v_{c 2}} \dot{q} J_{v_{c 1}} = [\begin{matrix} - l_{c} s i n (q_{1}) & 0 \\ l_{c 1} c o s (q_{1}) & 0 \\ 0 & 0 \end{matrix}] J_{v_{c 2}} = [\begin{matrix} - l_{1} s i n (q_{1}) - l_{c 2} s i n (q_{1} + q_{2}) & - l_{c 2} s i n (q_{1} + q_{2}) \\ l_{1} c o s (q_{1}) + l_{c 2} c o s (q_{1} + q_{2}) & l_{c 2} c o s (q_{1} + q_{2}) \\ 0 & 0 \end{matrix}]$ 平移部分对应的动能为： $\frac{1}{2} m_{1} v_{c 1}^{T} v_{c 1} + \frac{1}{2} m_{2} v_{c 2}^{T} v_{c 2} = \frac{1}{2} \dot{q} {m_{1} J_{v_{c 1}}^{T} J_{v_{c 1}} + m_{2} J_{v_{c 2}}^{T} J_{v_{c 2}}} \dot{q}$ 接下来考虑角速度项： $ω_{1} = {\dot{q}}_{1} k ω_{2} = ({\dot{q}}_{1} + {\dot{q}}_{2}) k$ 由于 $ω_{i}$ 与每个关节坐标系的z轴对齐，旋转动能可以简单表示为 $\frac{1}{2} I_{i} ω_{i}^{2}$ ，其中 $I_{i}$ 是转动惯量，它的轴线穿过连杆i的质心且平行于 $z_{i}$ 轴。因此，就广义坐标而言，整个系统的旋转动能为： $\frac{1}{2} {\dot{q}}^{T} {I_{1} [\begin{matrix} 1 & 0 \\ 0 & 0 \end{matrix}] + I_{2} [\begin{matrix} 1 & 1 \\ 1 & 1 \end{matrix}]} \dot{q}$ 惯性矩阵： $D (q) = m_{1} J_{v_{c 1}}^{T} J_{v_{c 1}} + m_{2} J_{v_{c 2}}^{T} J_{v_{c 2}} + [\begin{matrix} I_{1} + I_{2} & I_{2} \\ I_{2} & I_{2} \end{matrix}] = [\begin{matrix} d_{11} & d_{12} \\ d_{21} & d_{22} \end{matrix}]$ 计算得： $d_{11} = m_{1} l_{c 1}^{2} + m_{2} (l_{1}^{2} + l_{c 2}^{2} + 2 l_{1} l_{c 2} c o s (q_{2})) + I_{1} + I_{2} d_{12} = d_{21} = m_{2} (l_{c 2}^{2} + l_{1} l_{c 2} c o s (q_{2})) + I_{2} d_{22} = m_{2} l_{c 2}^{2} + I_{2}$ 我们已经得到了惯性矩阵，接下来计算Christoffel符号 $c_{i j k}$ ： $c_{111} = \frac{1}{2} \frac{\partial d_{11}}{\partial q_{1}} = 0 c_{121} = c_{211} = \frac{1}{2} \frac{\partial d_{11}}{\partial q_{2}} = - m_{2} l_{1} l_{c 2} s i n (q_{2}) = h c_{221} = \frac{\partial d_{12}}{\partial q_{2}} - \frac{1}{2} \frac{\partial d_{11}}{\partial q_{1}} = h c_{112} = \frac{\partial d_{21}}{\partial q_{1}} - \frac{1}{2} \frac{\partial d_{11}}{\partial q_{2}} = - h c_{122} = c_{212} = \frac{1}{2} \frac{\partial d_{22}}{\partial q_{1}} = 0 c_{222} = \frac{1}{2} \frac{\partial d_{22}}{\partial q_{2}} = 0$

接下来计算势能，机械臂的势能等于两个连杆势能之和。 $P_{1} = m_{1} g l_{c 1} s i n (q_{1}) P_{2} = m_{2} g (l_{2} s i n (q_{1}) + l_{c 2} s i n (q_{1} + q_{2})) P = P_{1} + P_{2} = (m_{1} l_{c 1} + m_{2} l_{1}) g s i n (q_{1}) + m_{2} l_{c 2} g s i n (q_{1} + q_{2})$ 之前的广义重力 $g_{k}$ 可变为： $g_{1} = \frac{\partial P}{\partial q_{1}} = (m_{1} l_{c 1} + m_{2} l_{1}) g c o s (q_{1}) + m_{2} l_{c 2} g c o s (q_{1} + q_{2}) g_{2} = \frac{\partial P}{\partial q_{2}} = m_{2} l_{c 2} g c o s (q_{1} + q_{2})$ 最后可以写出系统的动力学方程： $d_{11} {\ddot{q}}_{1} + d_{12} {\ddot{q}}_{2} + c_{121} {\dot{q}}_{1} {\dot{q}}_{2} + c_{211} {\dot{q}}_{2} {\dot{q}}_{1} + c_{221} {\dot{q}}_{2}^{2} + g_{1} = τ_{1} d_{21} {\ddot{q}}_{1} + d_{22} {\ddot{q}}_{2} + c_{112} {\dot{q}}_{1}^{2} + g_{2} = τ_{2}$ 在这种情况下，原方程矩阵 $C (q, \dot{q})$ 由下式给出： $C = [\begin{matrix} h {\dot{q}}_{2} & h {\dot{q}}_{2} + h {\dot{q}}_{1} \\ - h {\dot{q}}_{1} & 0 \end{matrix}]$