机器人动力学——拉格朗日法

引文

机器人动力学所用到的运动学方程是一组具有下列形式的二阶微分方程组

$$\tau = M ( \theta ) \ddot { \theta } + h ( \theta , \dot { \theta } ) \label{1}$$

式中，对于所有关节均为驱动关节的开式运动链，广义坐标 $\theta \in \mathbb{R}^{n}$ 表示关节变量；广义力 $\tau \in \mathbb{R}^{n}$ 表示力或力矩，如果 $\theta_i$ 是旋转关节，则 $\tau_i$ 对应力矩；如果 $\theta_i$ 为平动关节，则 $\tau_i$ 对应力； $M ( \theta )\in \mathbb{R}^{n\times{n}}$ 是一个对称且正定的质量矩阵；$h ( \theta , \dot { \theta })\in \mathbb{R}^{n}$ 是将向心力、科里奥利力、重力和摩擦力等集合在一起的力向量，该向量取决于 $\theta$ 和 $ \dot { \theta}$ 这两个变量。

运动学同样区分正运动学和逆运动学，正向动力学问题是在给定状态变量 $( \theta , \dot { \theta })$ 以及关节力和力矩的前提下确定机器人的加速度 $\ddot { \theta }$ ，即

$$\ddot { \theta } = M ^ { - 1 } ( \theta ) ( \tau - h ( \theta , \dot { \theta } ) )$$

逆动力学问题则是找到对应于机器人状态和期望加速度的关节力和力矩$\tau$，即式 $\eqref{1}$

机器人的动力学方程通常可以通过牛顿-欧拉公式或拉格朗日动力学公式得到。对于简单结构的机器人，如3自由度或自由度更少的情形，拉格朗日形式不仅在概念上十分优雅，并且在实际中非常有效。但对于自由度数目更多的机器人来讲，其计算可能会很快变得烦琐。

下文将讨论拉格朗日动力学公式

拉格朗日函数

力学系统的拉格朗日函数 ${ \cal L } ( q , \dot { q } )$ 定义为整个系统的动能 ${ \cal K } ( q , \dot { q } )$ 减去势能 ${ \cal P } ( q )$ ，式中的—组独立坐标 $q \in \mathbb{R}^{n}$ 用来描述系统的位形（这个坐标 $q$ 被称为广义坐标）

$${ \cal L } ( q , \dot { q } ) = { \cal K } ( q , \dot { q } ) - { \cal P } ( q )$$

运动方程现在可以用拉格朗日函数表示如下（推导过程略，详见动力学教材）

$$f = \frac { d } { d t } \frac { \partial { \cal L } } { \partial \dot { q } } - \frac { \partial { \cal L } } { \partial q }$$

这个方程也称为含外力的欧拉-拉格朗日方程（在标准形式的欧拉-拉格朗日方程中，外力 $f$ 等于零）

$2R$ 开链机器人举例

下面以在重力作用下的—个 $2R$ 开链机器人为例推导动力学方程，如下图所示

将关节坐标 $\theta =(\theta_{1},\theta_{2})$ 选做广义坐标，广义力 $\tau =(\tau_{1},\tau_{2})$ 则对应于关节力矩（因为 $\tau^T\dot{\theta}$ 对应于功率）。本例中的拉格朗日函数 ${ \cal L } ( q , \dot { q } )$ 可以写成如下分量形式:

$$\mathcal{L}(\theta, \dot{\theta})=\sum_{i=1}^{2}\left(\mathcal{K}_{i}-\mathcal{P}_{i}\right)$$

则欧拉-拉格朗日方程可以写成如下分量形式（ $\tau$ 为关节力和力矩向量）:

$$\tau_i = \frac { d } { d t } \frac { \partial { \cal L } } { \partial \dot { \theta_i } } - \frac { \partial { \cal L } } { \partial \theta_i } \qquad i = 1,2 \label{6}$$

下面我们需要计算两杆的动能和势能（认为两杆各自的质量 $\mathrm { m }1$ 和 $\mathrm { m }2$ 均集中于各杆末端）

连杆1质心的位置和速度:

$$\begin{array}{l} {\left[\begin{array}{l} x_{1} \\ y_{1} \end{array}\right]=\left[\begin{array}{c} L_{1} \cos \theta_{1} \\ L_{1} \sin \theta_{1} \end{array}\right]} \\ {\left[\begin{array}{c} \dot{x}_{1} \\ \dot{y}_{1} \end{array}\right]=\left[\begin{array}{r} -L_{1} \sin \theta_{1} \\ L_{1} \cos \theta_{1} \end{array}\right] \dot{\theta}_{1}} \end{array}$$

连杆2质心的位置和速度:

$$\begin{array}{l} {\left[\begin{array}{l} x_{2} \\ y_{2} \end{array}\right]=\left[\begin{array}{c} L_{1} \cos \theta_{1}+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\ L_{1} \sin \theta_{1}+L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \end{array}\right]} \\ {\left[\begin{array}{l} \dot{x}_{2} \\ \dot{y}_{2} \end{array}\right]=\left[\begin{array}{rr} -L_{1} \sin \theta_{1}-L_{2} \sin \left(\theta_{1}+\theta_{2}\right) & -L_{2} \sin \left(\theta_{1}+\theta_{2}\right) \\ L_{1} \cos \theta_{1}+L_{2} \cos \left(\theta_{1}+\theta_{2}\right) & L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \end{array}\right]\left[\begin{array}{l} \dot{\theta}_{1} \\ \dot{\theta}_{2} \end{array}\right] } \end{array}$$

由 $\mathcal{K} =\frac{1}{2} \mathfrak{m} v^2 = \frac{1}{2} \mathfrak{m}(\dot{x}^2 + \dot{y}^2)$ ，连杆的动能项 ${ \cal K }_1$ 和 ${ \cal K }_2$ 分别为

$$\begin{aligned} \mathcal{K}_{1} &=\frac{1}{2} \mathfrak{m}_{1}\left(\dot{x}_{1}^{2}+\dot{y}_{1}^{2}\right) \\ &=\frac{1}{2} \mathfrak{m}_{1} L_{1}^{2} \dot{\theta}_{1}^{2} \\ \mathcal{K}_{2} &=\frac{1}{2} \mathfrak{m}_{2}\left(\dot{x}_{2}^{2}+\dot{y}_{2}^{2}\right) \\ &=\frac{1}{2} \mathfrak{m}_{2}\left(\left(L_{1}^{2}+2 L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) \dot{\theta}_{1}^{2}+2\left(L_{2}^{2}+L_{1} L_{2} \cos \theta_{2}\right) \dot{\theta}_{1} \dot{\theta}_{2}+L_{2}^{2} \dot{\theta}_{2}^{2}\right) \end{aligned}$$

势能只取决于高度，即 $y$ 坐标。由 $\mathcal{P} = \mathfrak{m} g y$ ，连杆的势能项 ${ \cal P }_1$ 和 ${ \cal P }_2$ 分别为

$$\begin{array}{l} \mathcal{P}_{1}=\mathfrak{m}_{1} g y_{1}=\mathfrak{m}_{1} g L_{1} \sin \theta_{1} \\ \mathcal{P}_{2}=\mathfrak{m}_{2} g y_{2}=\mathfrak{m}_{2} g\left(L_{1} \sin \theta_{1}+L_{2} \sin \left(\theta_{1}+\theta_{2}\right)\right) \end{array}$$

将动能项与势能项带入拉格朗日函数 $\eqref{6}$ 即可得到平面 $2R$ 运动链的动力学方程:

$$\left\{ \begin{aligned} \tau_{1}=&\left(\mathfrak{m}_{1} L_{1}^{2}+\mathfrak{m}_{2}\left(L_{1}^{2}+2 L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right)\right) \ddot{\theta}_{1} \\ &+\mathfrak{m}_{2}\left(L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) \ddot{\theta}_{2}-\mathfrak{m}_{2} L_{1} L_{2} \sin \theta_{2}\left(2 \dot{\theta}_{1} \dot{\theta}_{2}+\dot{\theta}_{2}^{2}\right) \\ &+\left(\mathfrak{m}_{1}+\mathfrak{m}_{2}\right) L_{1} g \cos \theta_{1}+\mathfrak{m}_{2} g L_{2} \cos \left(\theta_{1}+\theta_{2}\right), \\ \tau_{2}=& \mathfrak{m}_{2}\left(L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) \ddot{\theta}_{1}+\mathfrak{m}_{2} L_{2}^{2} \ddot{\theta}_{2}+\mathfrak{m}_{2} L_{1} L_{2} \dot{\theta}_{1}^{2} \sin \theta_{2} \\ &+\mathfrak{m}_{2} g L_{2} \cos \left(\theta_{1}+\theta_{2}\right) . \end{aligned} \right.$$

可以看到，即使是一个简单的 $2R$ 机器人，其逆运动学方程也十分复杂。对上述各项进行整理，得到如下形式的方程:

$$\tau=M(\theta) \ddot{\theta}+\underbrace{c(\theta, \dot{\theta})+g(\theta)}_{h(\theta, \dot{\theta})}$$

式中

$$\begin{aligned} M(\theta) &=\left[\begin{array}{cc} \mathfrak{m}_{1} L_{1}^{2}+\mathfrak{m}_{2}\left(L_{1}^{2}+2 L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) & \mathfrak{m}_{2}\left(L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) \\ \mathfrak{m}_{2}\left(L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) & \mathfrak{m}_{2}L_{2}^{2} \\ \end{array}\right] \\ c(\theta, \dot{\theta}) &=\left[\begin{array}{c} -\mathfrak{m}_{2} L_{1} L_{2} \sin \theta_{2}\left(2 \dot{\theta}_{1} \dot{\theta}_{2}+\dot{\theta}_{2}^{2}\right) \\ \mathfrak{m}_{2} L_{1} L_{2} \dot{\theta}_{1}^{2} \sin \theta_{2} \end{array}\right] \\ g(\theta) &=\left[\begin{array}{c} \left(\mathfrak{m}_{1}+\mathfrak{m}_{2}\right) L_{1} g \cos \theta_{1}+\mathfrak{m}_{2} g L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\ \mathfrak{m}_{2} g L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \end{array}\right] \end{aligned}$$

式中 $M(\theta)$ 为对称正定的质量矩阵， $c(\theta, \dot{\theta})$ 为速度乘积项，包含科里奥利和向心力矩的向量，其中包含 $\dot{\theta}^2$ 的二次项称为向心项，包含 $\dot{\theta}_{i} \dot{\theta}_{j}(i \ne j)$ 的二次项则称为科里奥利项。 $g(\theta)$ 为重力项 ，因为此处势能仅来自重力，若末端有弹簧，弹簧也会贡献势能。这些揭示了运动方程是 $\ddot{\theta}$ 的线性函数、 $\dot{\theta}$ 的二次函数、 $\theta$ 的三角函数。上述结论不仅适用于 $2R$ 型机器人，对于包含转动关节的串联运动链也同样适用。