摘要
如题。
（转自我的CSDN博客）
机器人动力学方程
机器人动力学方程是描述机器人力和运动之间的关系的方程。只描述力和运动的关系，不考虑产生运动的力和扭矩。
欧拉 - 拉格朗日方程
欧拉-拉格朗日方程（OL）描述了处于完整约束下，并且约束力满足虚功原理的机械系统的力和运动随时间的变化。
有两种推导方法，先介绍使用牛顿第二定律的推导方法。
根据牛顿第二定律，某质点的运动方程是： \[ m\ddot y = f-mg \] 先对时间求导，再对\(\dot y\)求偏导，方程左侧可以写为： \[ m \ddot y = {d \over dt}(m \dot y)={d \over dt}{\partial \over \partial \dot y}({1 \over 2}m \dot y^2)={d \over dt}{\partial K \over \partial \dot y} \] 其中\(K = {1 \over 2}m \dot y^2\)，是动能。
接着将重力表示为： \[ mg = {\partial \over \partial y}(mgy) = {\partial P \over \partial y} \] 其中\(P = mgy\)表示重力势能。
定义拉格朗日算子\(L\)，表示系统动能与势能之差： \[ L = K-P ={1 \over 2}m \dot y^2-mgy \] 并且有： \[ {\partial L\over \partial \dot y} = {\partial K \over \partial \dot y},{\partial L \over \partial y} =-{\partial P \over \partial y} \]
则初始的质点运动方程可化为： \[ {d \over dt}{\partial K \over \partial \dot y} = f-{\partial P \over \partial y} \] 即： \[ {d \over dt}{\partial L \over \partial \dot y}-{\partial L \over \partial y} = f \] 此方程被称为欧拉-拉格朗日方程。
推广到n自由度的系统，得到： \[ {d \over dt}{\partial L \over \partial \dot q_k}-{\partial L \over \partial q_k} = \tau_k \] 其中\(\tau_k\)是与广义坐标\(q_k\)相关的力。
动能与势能
欧拉-拉格朗日方程可以直接用来推导动力学方程，前提是我们能够以一组广义坐标来表示该系统的动能和势能。如果要让这能够得到实际应用，那么我们就必须能够针对一个n连杆机器人计算出他的动能和势能。接下来将推到刚性连杆机器人动能和势能的表达式。
动能表示
刚体的动能可表示为平移动能和关于质心的旋转动能之和： \[ K = {1 \over 2}mv^Tv+{1\over 2}\omega^T Z\omega \] \(Z\)表示物体的惯性张量，是一个3*3的对称矩阵。
\(Z = RIR^T\)，R是附体坐标系与惯性坐标系之间的姿态变换。
\(I\)是附体坐标系中的惯性张量，仅取决于物体的形状和质量分布，与物体运动无关。 
速度\(v\)和角速度\(\omega\)需要转置，因为要考虑多个维度的方向。连杆上任意一点的现速度和角速度可通过雅可比矩阵和关节速度（关节变量的导数）来表示： \[ v_i = J_{v_i}q\dot q \\ \omega_i = J_{\omega_i}q\dot q \]
机器人总动能可表示为： \[ K = {1 \over 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)\)来表示机器人的惯性矩阵： \[ 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 = {1 \over 2}\dot q^T D(q)\dot q \] 机器人惯性矩阵\(D(q)\)有如下特点：
- 只与机器人构型有关
- 对称且正定
- 动能总是非负的
势能表示
在刚体动力学情形下，势能总是来源于重力。假设物体质量集中在质心，计算第\(i\)个连杆的势能： \[ P_i = m_ig^Tr_{ci} \] \(g\)是惯性坐标系中的重力向量，\(r_{ci}\)是连杆\(i\)的质心坐标。机器人总势能为： \[ P = \sum_{i=1}^n P_i = \sum_{i=1}^n m_ig^Tr_{ci} \] 在m、g保持不变的情况下，机器人势能只与广义坐标\(r_{ci}\)有关。
运动方程
上面我们得到了如下结果：
系统动能是关于广义速度（坐标微分）的二次函数： \[ K = {1 \over 2}\dot q^T D(q)\dot q = {1 \over 2}\sum_{i,j}^n d_{i,j}(q) \dot q_i \dot q_j \] 系统势能是关于广义坐标的函数，且与广义速度无关： \[ P = \sum_{i=1}^n m_ig^Tr_{ci} \] 欧拉-拉格朗日算子为： \[ L = K-P ={1 \over 2}\sum_{i,j}^n d_{i,j}(q) \dot q_i \dot q_j-P(q) \] 欧拉-拉格朗日方程为： \[ {d \over dt}{\partial L \over \partial \dot q_k}-{\partial L \over \partial q_k} = \tau_k \] 其中： \[ {\partial L \over \partial \dot q_k}= \sum_jd_{kj}\dot q_j\\ {d \over dt}{\partial L \over \partial \dot q_k} = \sum_jd_{kj}\ddot q_j+\sum_{j}{d \over dt}d_{kj}\dot q_j \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \sum_jd_{kj}\ddot q_j+\sum_{i,j}{\partial d_{kj}\over \partial q_i}\dot q_i\dot q_j \\ {\partial L \over \partial q_k} = {1 \over 2}\sum_{i,j} {\partial d_{i,j} \over \partial q_k} \dot q_i \dot q_j - {\partial P \over \partial q_k} \] 因此对于每个\(k=1,2,...n\)，欧拉-拉格朗日方程可以写成： \[ \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 \] 即： \[ \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 \] 定义Christoffel symbol： \[ 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} \} \] 定义广义重力： \[ g_k = {\partial P \over \partial q_k} \] 最终得到欧拉-拉格朗日方程： \[ \sum_jd_{kj}(q)\ddot q_j+\sum_{i,j} c_{ijk}(q) \dot q_i\dot q_j - g_k(q)= \tau_k \] 方程左侧三项分别为：
- 广义坐标的二阶导数：惯性项
- 广义坐标一阶导数的二次型：离心力项+哥氏力项
- 广义位置（0阶导数）重力项
方程可简写为： \[ D(q)\ddot q+C(q,\dot q)\dot q+g(q) = \tau \]
推导平面2关节机器人的动力学模型
现在考虑下图中带有两个转动关节的平面机械臂。
要使用刚刚得到的欧拉-拉格朗日方程，就要与关节位置和关节速度相关的三个量：\(D(q),C(q,\dot q),g(q)\)。
首先使用雅可比矩阵来计算动能，计算平移速度： \[ v_{c1} = J_{v_{c1}}\dot q \\ v_{c2} = J_{v_{c2}}\dot q \\ J_{v_{c1}} = \begin{bmatrix}-l_c sin(q_1) & 0 \\ l_{c1}cos(q_1) & 0\\ 0 & 0 \\\end{bmatrix} \\ J_{v_{c2}} = \begin{bmatrix}-l_1sin(q_1)-l_{c2}sin(q_1+q_2) & -l_{c2}sin(q_1+q_2) \\ l_1cos(q_1)+l_{c2}cos(q_1+q_2) & l_{c2}cos(q_1+q_2)\\ 0 & 0 \\\end{bmatrix} \] 平移部分对应的动能为： \[ {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 \] 接下来考虑角速度项： \[ \omega_1 = \dot q_1k \\ \omega_2 = (\dot q_1+ \dot q_2)k \] 由于\(\omega_i\)与每个关节坐标系的z轴对齐，旋转动能可以简单表示为\({1\over 2}I_i\omega_i^2\)，其中\(I_i\)是转动惯量，它的轴线穿过连杆i的质心且平行于\(z_i\)轴。因此，就广义坐标而言，整个系统的旋转动能为： \[ {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 \] 惯性矩阵： \[ D(q) = m_1J^T_{v_{c1}}J_{v_{c1}} +m_2J^T_{v_{c2}}J_{v_{c2}} + \begin{bmatrix} I_1+I_2&I_2 \\ I_2&I_2 \end{bmatrix} = \begin{bmatrix} d_{11}&d_{12} \\ d_{21}&d_{22} \end{bmatrix} \\ \] 计算得： \[ d_{11} = m_1l_{c1}^2 + m_2(l_1^2+l^2_{c2}+2l_1l_{c2}cos(q_2))+I_1+I_2\\ d_{12} = d_{21} = m_2(l^2_{c2}+l_1l_{c2}cos(q_2))+I_2 \\ d_{22} = m_2l^2_{c2}+I_2 \] 我们已经得到了惯性矩阵，接下来计算Christoffel符号\(c_{ijk}\)： \[ c_{111}= {1 \over 2}{\partial d_{11}\over \partial q_1} = 0 \\ c_{121}= c_{211}= {1 \over 2}{\partial d_{11}\over \partial q_2} = -m_2l_1l_{c2}sin(q_2) = h \\ c_{221}= {\partial d_{12}\over \partial q_2}-{1 \over 2}{\partial d_{11}\over \partial q_1} = h \\ c_{112}= {\partial d_{21}\over \partial q_1}-{1 \over 2}{\partial d_{11}\over \partial q_2} = -h \\ c_{122}= c_{212}= {1 \over 2}{\partial d_{22}\over \partial q_1} = 0\\ c_{222}= {1 \over 2}{\partial d_{22}\over \partial q_2} = 0 \\ \]
接下来计算势能，机械臂的势能等于两个连杆势能之和。 \[ P_1 = m_1gl_{c1}sin(q_1) \\ P_2 = m_2g(l_{2}sin(q_1)+l_{c2}sin(q_1+q_2))\\ P = P_1+P_2 =(m_1l_{c1}+m_2l_1)gsin(q_1)+m_2l_{c2}gsin(q_1+q_2) \] 之前的广义重力\(g_k\)可变为： \[ g_1 = {\partial P \over\partial q_1 } = (m_1l_{c1}+m_2l_1)gcos(q_1)+m_2l_{c2}gcos(q_1+q_2) \\ g_2 = {\partial P \over\partial q_2 } = m_2l_{c2}gcos(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=\tau_1 \\ d_{21} \ddot q_1+d_{22}\ddot q_2+c_{112}\dot q_1^2+g_2 = \tau_2 \] 在这种情况下，原方程矩阵\(C(q, \dot q)\)由下式给出： \[ C = \begin{bmatrix}h\dot q_2 & h \dot q_2+h\dot q_1 \\ -h\dot q_1 & 0\end{bmatrix} \]