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

机器人动力学方程

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

欧拉 - 拉格朗日方程

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

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

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

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

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

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

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

动能与势能

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

动能表示

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

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

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

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

机器人总动能可表示为：
$$K=\frac{1}{2}{\dot{q}}^T\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)]\dot{q}$$
用$D(q)$来表示机器人的惯性矩阵：
$$\begin{gathered} D(q)=\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)] \\ K=\frac{1}{2}{\dot{q}}^{T}D(q)\dot{q} \end{gathered}$$
机器人惯性矩阵$D(q)$有如下特点：

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

势能表示

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

运动方程

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

系统动能是关于广义速度（坐标微分）的二次函数：
$$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}_{ci}$$
欧拉-拉格朗日算子为：
$$L=K-P=\frac{1}{2}\sum _{i,j}^n{d}_{i,j}(q){\dot{q}}_i{\dot{q}}_j-P(q)$$
欧拉-拉格朗日方程为：
$$\frac{d}{dt}\frac{\partial L}{\partial {\dot{q}}_k}-\frac{\partial L}{\partial q_k}={\tau }_k$$
其中：
$$\begin{gathered} \frac{\partial L}{\partial {\dot{q}}_k}=\sum _j{d}_{kj}{\dot{q}}_j \\ \frac{d}{dt}\frac{\partial L}{\partial {\dot{q}}_k}=\sum _j{d}_{kj}{\ddot{q}}_j+\sum _j\frac{d}{dt}{d}_{kj}{\dot{q}}_j \\ =\sum _j{d}_{kj}{\ddot{q}}_j+\sum _{i,j}\frac{\partial d_{kj}}{\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} \end{gathered}$$
因此对于每个$k=1,2,...n$，欧拉-拉格朗日方程可以写成：
∑ j d k j q ¨ j + ∑ i , j { ∂ d k j ∂ q i − 1 2 ∂ d i , j ∂ q k } q ˙ i q ˙ j − ∂ P ∂ q k = τ k \sum_jd_{kj}\ddot q_j+\sum_{i,j}\{ {\partial d_{kj}\over \partial q_i} -{1\over 2}{\partial d_{i,j} \over \partial q_k}\} \dot q_i\dot q_j - {\partial P \over \partial q_k}= \tau_k j∑​dkj​q¨​j​+i,j∑​{∂qi​∂dkj​​−21​∂qk​∂di,j​​}q˙​i​q˙​j​−∂qk​∂P​=τk​
即：
∑ j d k j q ¨ j + ∑ i , j 1 2 { ∂ d k j ∂ q i + ∂ d k j ∂ q i − ∂ d i , j ∂ q k } q ˙ i q ˙ j − ∂ P ∂ q k = τ k \sum_jd_{kj}\ddot q_j+\sum_{i,j} {1\over 2}\{ {\partial d_{kj}\over \partial q_i} + {\partial d_{kj}\over \partial q_i} -{\partial d_{i,j} \over \partial q_k}\} \dot q_i\dot q_j - {\partial P \over \partial q_k}= \tau_k j∑​dkj​q¨​j​+i,j∑​21​{∂qi​∂dkj​​+∂qi​∂dkj​​−∂qk​∂di,j​​}q˙​i​q˙​j​−∂qk​∂P​=τk​
定义Christoffel symbol：
c i j k = c j i k = 1 2 { ∂ d k j ∂ q i + ∂ d k j ∂ q i − ∂ d i , j ∂ q k } c_{ijk} = c_{jik} ={1\over 2}\{ {\partial d_{kj}\over \partial q_i} + {\partial d_{kj}\over \partial q_i} -{\partial d_{i,j} \over \partial q_k} \} cijk​=cjik​=21​{∂qi​∂dkj​​+∂qi​∂dkj​​−∂qk​∂di,j​​}
定义广义重力：
$$g_k=\frac{\partial P}{\partial q_k}$$
最终得到欧拉-拉格朗日方程：
$$\sum _j{d}_{kj}(q){\ddot{q}}_j+\sum _{i,j}{c}_{ijk}(q){\dot{q}}_i{\dot{q}}_j-g_k(q)={\tau }_k$$
方程左侧三项分别为：

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

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

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

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

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

首先使用雅可比矩阵来计算动能，计算平移速度：
$$\begin{gathered} v_{c1}=J_{v_{c1}}\dot{q} \\ {v}_{c2}=J_{v_{c2}}\dot{q} \\ {J}_{v_{c1}}=\left[\begin{array}{cc}-l_{c}sin(q_1)& 0\\ l_{c1}cos(q_1)& 0\\ 0& 0\end{array}\right] \\ {J}_{v_{c2}}=\left[\begin{array}{cc}-l_{1}sin(q_1)-l_{c2}sin(q_1+q_2)& -l_{c2}sin(q_1+q_2)\\ l_{1}cos(q_1)+l_{c2}cos(q_1+q_2)& l_{c2}cos(q_1+q_2)\\ 0& 0\end{array}\right] \end{gathered}$$
平移部分对应的动能为：
1 2 m 1 v c 1 T v c 1 + 1 2 m 2 v c 2 T v c 2 = 1 2 q ˙ { m 1 J v c 1 T J v c 1 + m 2 J v c 2 T J v c 2 } q ˙ {1\over 2}m_1 v^T_{c1}v_{c1}+{1\over2}m_2v^T_{c2}v_{c2} = {1\over 2}\dot q\{ m_1J^T_{v_{c1}}J_{v_{c1}} +m_2J^T_{v_{c2}}J_{v_{c2}} \}\dot q 21​m1​vc1T​vc1​+21​m2​vc2T​vc2​=21​q˙​{m1​Jvc1​T​Jvc1​​+m2​Jvc2​T​Jvc2​​}q˙​
接下来考虑角速度项：
$$\begin{gathered} {\omega }_1={\dot{q}}_{1}k \\ {\omega }_2=({\dot{q}}_1+{\dot{q}}_2)k \end{gathered}$$
由于${\omega }_i$与每个关节坐标系的z轴对齐，旋转动能可以简单表示为$\frac{1}{2}{I}_i{\omega }_i^2$，其中$I_i$是转动惯量，它的轴线穿过连杆i的质心且平行于$z_i$轴。因此，就广义坐标而言，整个系统的旋转动能为：
1 2 q ˙ T { I 1 [ 1 0 0 0 ] + I 2 [ 1 1 1 1 ] } q ˙ {1\over 2}\dot q^T \{ I_1 \begin{bmatrix}1 &0 \\ 0&0 \end{bmatrix} + I_2 \begin{bmatrix} 1&1\\1&1 \end{bmatrix}\}\dot q 21​q˙​T{I1​[10​00​]+I2​[11​11​]}q˙​
惯性矩阵：
$$D(q)=m_1{J}_{v_{c1}}^T{J}_{v_{c1}}+m_2{J}_{v_{c2}}^T{J}_{v_{c2}}+\left[\begin{array}{cc}{I}_1+I_2& I_2\\ I_2& I_2\end{array}\right]=\left[\begin{array}{cc}{d}_{11}& d_{12}\\ d_{21}& d_{22}\end{array}\right]$$
计算得：
$$\begin{gathered} d_{11}=m_1{l}_{c1}^2+m_2(l_1^2+l_{c2}^2+2l_1{l}_{c2}cos(q_2))+I_1+I_2 \\ {d}_{12}=d_{21}=m_2(l_{c2}^2+l_1{l}_{c2}cos(q_2))+I_2 \\ {d}_{22}=m_2{l}_{c2}^2+I_2 \end{gathered}$$
我们已经得到了惯性矩阵，接下来计算Christoffel符号$c_{ijk}$：
$$\begin{gathered} 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}_{c2}sin(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 \end{gathered}$$

接下来计算势能，机械臂的势能等于两个连杆势能之和。
$$\begin{gathered} P_1=m_{1}g{l}_{c1}sin(q_1) \\ {P}_2=m_{2}g(l_{2}sin(q_1)+l_{c2}sin(q_1+q_2)) \\ P=P_1+P_2=(m_1{l}_{c1}+m_2{l}_1)gsin(q_1)+m_2{l}_{c2}gsin(q_1+q_2) \end{gathered}$$
之前的广义重力$g_k$可变为：
$$\begin{gathered} g_1=\frac{\partial P}{\partial q_1}=(m_1{l}_{c1}+m_2{l}_1)gcos(q_1)+m_2{l}_{c2}gcos(q_1+q_2) \\ {g}_2=\frac{\partial P}{\partial q_2}=m_2{l}_{c2}gcos(q_1+q_2) \end{gathered}$$
最后可以写出系统的动力学方程：
$$\begin{gathered} 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={\tau }_1 \\ {d}_{21}{\ddot{q}}_1+d_{22}{\ddot{q}}_2+c_{112}{\dot{q}}_1^2+g_2={\tau }_2 \end{gathered}$$
在这种情况下，原方程矩阵$C(q,\dot{q})$由下式给出：
$$C=\left[\begin{array}{cc}h{\dot{q}}_2& h{\dot{q}}_2+h{\dot{q}}_1\\ -h{\dot{q}}_1& 0\end{array}\right]$$