机械臂笔记（五）机械臂的动力学模型（欧拉-拉格朗日方程）

通过机器人的（逆）运动学可以将机械臂运动到给定的位姿，但是仅适合轻载、低速、无接触的工况，因为无法判断力矩是否合理、是否会激发振动、是否与系统惯量匹配。而通过引入动力学，则可以让机械臂按照需要的加速度运动，实现运动地稳、准、快、省、柔。

一维欧拉-拉格朗日方程推导

假设有以下粒子，受管道约束只能上下移动，并且约束力满足虚功原理。

根据牛顿第二定律，该质点的运动方程为：

$$\begin{array}{}\text{(1)}& m{y}^{″}=f-mg\end{array}$$

将$(1)$式右边先对时间求导、再对速度$y^{\prime }$ 求偏导，可得：

$$\begin{array}{}\text{(2)}& m{y}^{″}=\frac{d}{dt}(m{y}^{\prime })=\frac{d}{dt}\frac{\partial }{\partial y^{\prime }}(\frac{1}{2}m{y}^{\prime 2})=\frac{d}{dt}\frac{\partial K}{\partial y^{\prime }}\end{array}$$

其中$K=\frac{1}{2}m{y}^{\prime 2}$ 表示质点的动能，接下来表示质点的重力势能：

$$\begin{array}{}\text{(3)}& mg=\frac{\partial }{\partial y}(mgy)=\frac{\partial P}{\partial y}\end{array}$$

其中$P=mgy$ 表示质点的重力势能。
之后我们定义拉格朗日算子$L$，表示动能与势能之差：

$$\begin{array}{}\text{(4)}& L=K-P=\frac{1}{2}m{y}^{\prime 2}-mgy\end{array}$$

并且有：

$$\begin{array}{}\text{(5)}& \begin{array}{l}\frac{\partial L}{\partial y^{\prime }}=\frac{\partial K}{\partial y^{\prime }}\\ \frac{\partial L}{\partial y}=-\frac{\partial P}{\partial y}\end{array}\end{array}$$

联立式$(1,2,3)$，初始的质点运动学方程可以化为：

$$\begin{array}{}\text{(6)}& \frac{d}{dt}\frac{\partial K}{\partial y^{\prime }}=f-\frac{\partial P}{\partial y}\end{array}$$

联立式$(5，6)$，进一步得到：

$$\begin{array}{}\text{(7)}& \frac{d}{dt}\frac{\partial L}{\partial y^{\prime }}-\frac{\partial L}{\partial y}=f\end{array}$$

方程$(7)$ 被称为欧拉-拉格朗日方程。推广到$n$ 个自由度的系统，可得：

$$\begin{array}{}\text{(8)}& \frac{d}{dt}\frac{\partial L}{\partial q_k^{\prime }}-\frac{\partial L}{\partial q_k}={\tau }_k\end{array}$$

上面方程中：

1. $q_k$ 表示第$k$ 个广义坐标，在机器人中可以表示转角或直线位移
2. $L$ 表示拉格朗日量，包含动能和势能
3. $\frac{\partial L}{\partial q_k^{\prime }}$ 表示与$q_k$ 对应的广义动量
4. $\frac{d}{dt}\frac{\partial L}{\partial q_k^{\prime }}$ 表示惯性项，对加速度的反抗
5. $\frac{\partial L}{\partial q_k}$ 表示保守力项，构型变化对能量的影响
6. ${\tau }_k$ 表示第$k$ 个广义力（矩）

由虚功原理：

$$\begin{array}{}\text{(9)}& \delta W=\sum _k{\tau }_k\delta q_k\end{array}$$

将其对应机械臂的各个关节旧容易理解一些了：

高维推广

在高维情况下，我们通过一组广义坐标来表示系统的动能和势能。

动能表示

刚体的动能是平移动能和旋转动能之和：

$$\begin{array}{}\text{(10)}& K=\frac{1}{2}m{v}^{T}v+\frac{1}{2}{\omega }^{T}Z\omega \end{array}$$

其中$Z$ 表示物体的惯性张量，是一个$3\times 3$ 的矩阵。$Z=RI{R}^T$，$R$ 是附体坐标系（随刚体运动）与惯性坐标系（世界坐标系）之间的姿态变换。$I$ 是附体坐标系中的惯性张量，仅取决于物体的形状和质量分布，与物体的运动无关。

展开$I$ 的计算

$$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]$$

其中，各元素的计算如下：

$$\begin{array}{l}{I}_{xx}=\int \int \int (y^2+z^2)\rho (x,y,z)dxdydz\\ I_{yy}=\int \int \int (x^2+z^2)\rho (x,y,z)dxdydz\\ I_{zz}=\int \int \int (x^2+y^2)\rho (x,y,z)dxdydz\\ I_{xy}=I_{yx}=-\int \int \int (xy)\rho (x,y,z)dxdydz\\ I_{xz}=I_{zx}=-\int \int \int (xz)\rho (x,y,z)dxdydz\\ I_{yz}=I_{zy}=-\int \int \int (yz)\rho (x,y,z)dxdydz\end{array}$$

连杆上任意一点的线速度和角速度可以通过雅可比矩阵和关节速度表示：

$$\begin{array}{}\text{(11)}& \begin{array}{l}{v}_i=J_{v_i}(q)q^{\prime }\\ {\omega }_i=J_{{\omega }_i}(q)q^{\prime }\end{array}\end{array}$$

$J_{{\ast }_i}(q)$ 表示第$i$ 个连杆的线（角）速度雅可比矩阵，依赖于构型$q$。映射是线性的。

则某一关机的动能可以表示为如下（$m_i$）是常数：

$$\begin{array}{}\text{(12)}& \begin{array}{lll}{K}_i& =& \frac{1}{2}{m}_i{v}_i^T{v}_i+\frac{1}{2}{\omega }_i^T{R}_i(q)I_i{R}_i^T(q){\omega }_i\\ & =& \frac{1}{2}[m_i{q}^{\prime T}{J}_{v_i}^T(q)J_{v_i}(q)q^{\prime }+q^{\prime T}{J}_{{\omega }_i}^T(q)R_i(q)I_i{R}_i^T(q)J_{{\omega }_i}(q)q^{\prime }]\\ & =& \frac{1}{2}{q}^{\prime T}[m_i{J}_{v_i}^T(q)J_{v_i}(q)+J_{{\omega }_i}^T(q)R_i(q)I_i{R}_i^T(q)J_{{\omega }_i}(q)]q^{\prime }\end{array}\end{array}$$

于是，机械臂的总动能可以表示为：

$$\begin{array}{}\text{(13)}& \begin{array}{lll}{K}_{total}& =& \sum _{i=1}^n{K}_i\\ & =& \frac{1}{2}{q}^{\prime T}\sum _{i=1}^n[m_i{J}_{v_i}^T(q)J_{v_i}(q)+J_{{\omega }_i}^T(q)R_i(q)I_i{R}_i^T(q)J_{{\omega }_i}(q)]q^{\prime }\end{array}\end{array}$$

可以用$D(q)$ 表示机器人的惯性矩阵：

$$\begin{array}{}\text{(14)}& D(q)=\sum _{i=1}^n[m_i{J}_{v_i}^T(q)J_{v_i}(q)+J_{{\omega }_i}^T(q)R_i(q)I_i{R}_i^T(q)J_{{\omega }_i}(q)]\end{array}$$

则式$(13)$可以化简为如下形式：

$$\begin{array}{}\text{(15)}& K_{total}=\frac{1}{2}{q}^{\prime T}D(q)q^{\prime }\end{array}$$

机器人惯性矩阵$D(q)$ 具有以下特点：

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

势能表示

假设物体质量集中在质心，计算第$i$个连杆的势能：

$$\begin{array}{}\text{(16)}& P_i=m_i{\overrightarrow{g}}^T{\overrightarrow{r}}_{c_i}\end{array}$$

其中：

• $\overrightarrow{g}$ 表示重力加速度向量，$\overrightarrow{g}=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$
• ${\overrightarrow{r}}_{c_i}$ 表示第$i$ 个连杆质心在惯性（世界）坐标系中的位置向量。${\overrightarrow{r}}_{c_i}=\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]$

则系统的总势能为：

$$\begin{array}{}\text{(17)}& P_{total}=\sum _{i=1}^n{P}_i=\sum _{i=1}^n{m}_i{\overrightarrow{g}}^T{\overrightarrow{r}}_{c_i}\end{array}$$

在$m,g$ 是常数的情况下，机器人势能只与${\overrightarrow{r}}_{c_i}$ 有关。

运动方程

由式$(15)$，系统的动能是关于广义速度（坐标微分）的二次函数：

$$\begin{array}{}\text{(18)}& K_{total}=\frac{1}{2}{q}^{\prime T}D(q)q^{\prime }=\frac{1}{2}\sum _{ij}^n{d}_{ij}(q)q_i^{\prime }{q}_j^{\prime }\end{array}$$

$d_{ij}(q)$ 是矩阵$D(q)$的第$(i,j)$个元素。
结合势能方程$(17)$ 可以得到欧拉-拉格朗日算子：

$$\begin{array}{}\text{(19)}& L=K_{total}-P_{total}=\frac{1}{2}\sum _{ij}^n{d}_{ij}(q)q_i^{\prime }{q}_j^{\prime }-P(q)\end{array}$$

第$k$ 个关节的欧拉-拉格朗日方程为：

$$\begin{array}{}\text{(20)}& \frac{d}{dt}\frac{\partial L}{\partial q_k^{\prime }}-\frac{\partial L}{\partial q_k}={\tau }_k\end{array}$$

其中：

$$\begin{array}{}\text{(21)}& \begin{array}{lll}\frac{\partial L}{\partial q_k^{\prime }}& =& \sum _j^n{d}_{kj}{q}_j^{\prime }\\ \frac{d}{dt}\frac{\partial L}{\partial q_k^{\prime }}& =& \sum _j^n{d}_{kj}{q}_j^{″}+\sum _j^n\frac{d}{dt}{d}_{kj}{q}_j^{\prime }\\ & =& \sum _j^n{d}_{kj}{q}_j^{″}+\sum _{ij}^n\frac{\partial d_{kj}}{\partial q_i}{q}_i^{\prime }{q}_j^{\prime }\\ \frac{\partial L}{\partial q_k}& =& \frac{1}{2}\sum _{ij}^n\frac{\partial d_{ij}}{\partial q_k}{q}_i^{\prime }{q}_j^{\prime }-\frac{\partial P}{\partial q_k}\end{array}\end{array}$$

因此，对于每一个$k=1,2,...,n$，欧拉-拉格朗日方程可以写为：

$$\begin{array}{}\text{(22)}& \begin{array}{l}\sum _j^n{d}_{kj}{q}_j^{″}+\sum _{ij}^n\frac{\partial d_{kj}}{\partial q_i}{q}_i^{\prime }{q}_j^{\prime }-\frac{1}{2}\sum _{ij}^n\frac{\partial d_{ij}}{\partial q_k}{q}_i^{\prime }{q}_j^{\prime }+\frac{\partial P}{\partial q_k}\\ =\sum _j^n{d}_{kj}{q}_j^{″}+\sum _{ij}^n[(\frac{\partial d_{kj}}{\partial q_i}-\frac{1}{2}\frac{\partial d_{ij}}{\partial q_k})q_i^{\prime }{q}_j^{\prime }]+\frac{\partial P}{\partial q_k}\\ =\sum _j^n{d}_{kj}{q}_j^{″}+\sum _{ij}^n[\frac{1}{2}(\frac{\partial d_{kj}}{\partial q_i}+\frac{\partial d_{kj}}{\partial q_i}-\frac{\partial d_{ij}}{\partial q_k})q_i^{\prime }{q}_j^{\prime }]+\frac{\partial P}{\partial q_k}\\ ={\tau }_k\end{array}\end{array}$$

定义Christoffel symbol：

$$\begin{array}{}\text{(23)}& c_{ijk}=c_{jik}=\frac{1}{2}(\frac{\partial d_{kj}}{\partial q_i}+\frac{\partial d_{kj}}{\partial q_i}-\frac{\partial d_{ij}}{\partial q_k})\end{array}$$

定义广义重力：

$$\begin{array}{}\text{(24)}& g_k=\frac{\partial P}{\partial q_k}\end{array}$$

最终得到欧拉-拉格朗日方程：

$$\begin{array}{}\text{(25)}& \sum _j{d}_{kj}(q)q_j^{″}+\sum _{ij}{c}_{ijk}(q)q_i^{\prime }{q}_j^{\prime }+g_k(q)={\tau }_k\end{array}$$

方程可简写为：

$$\begin{array}{}\text{(26)}& M(q)q^{″}+C(q,q^{\prime })q^{\prime }+G(q)=\tau \end{array}$$

其中，方程中各项的意义分别是：

• $M(q)$：n×n惯性矩阵
• $C(q,q^{\prime })$：科氏力和离心力矩阵
• $G(q)$：重力项
• $\tau$：关节力矩向量

实际应用

单摆

对于长度为$l$、质量为$m$的单摆，角度为$\theta$：

• 动能：$K=\frac{1}{2}m{l}^2{\theta }^{\prime 2}$
• 势能：$P=-mgl\cos (\theta )$
• 欧拉-拉格朗日算子：$L=K-P=\frac{1}{2}m{l}^2{\theta }^{\prime 2}+mgl\cos (\theta )$
  应用欧拉-拉格朗日方程得到：

$$\begin{array}{}\text{(27)}& \frac{d}{dt}\frac{\partial L}{\partial {\theta }^{\prime }}-\frac{\partial L}{\partial \theta }=\frac{d}{dt}m{l}^2{\theta }^{\prime }-(-mgl\sin (\theta ))=m{l}^2{\theta }^{″}+mgl\sin (\theta )=\tau \end{array}$$

如果所受外力为0 的情况下有：

$$\begin{array}{}\text{(28)}& m{l}^2{\theta }^{″}+mgl\sin (\theta )=0\end{array}$$

两边同时除以$m{l}^2$，可以得到单摆的非线性运动方程：

$$\begin{array}{}\text{(29)}& {\theta }^{″}+\frac{g}{l}\sin (\theta )=0\end{array}$$

如果需要$\theta (t)$ 按照某条轨迹走，则根据式$(27)$，需要施加外力：

$$\begin{array}{}\text{(30)}& \tau (t)=m{l}^2{\theta }^{″}(t)+mgl\sin (\theta (t))\end{array}$$

力$\tau$ 应该以逆时针为正方向。

推导平面2关节机械臂动力学模型

现有2 关节机械臂如下图所示：

参数	长度	质量	角度
连杆1	$l_1$	$m_1$	$q_1$
连杆2	$l_2$	$m_2$	$q_2$

首先求连杆质心的位置：

$$\begin{array}{}\text{(31)}& \begin{array}{l}{r}_{c_1}=\left[\begin{array}{c}\frac{l_1}{2}\cos (q_1)\\ \frac{l_1}{2}\sin (q_1)\\ 0\end{array}\right]\\ r_{c_2}=\left[\begin{array}{c}{l}_1\cos (q_1)+\frac{l_2}{2}\cos (q_1+q_2)\\ l_1\sin (q_1)+\frac{l_2}{2}\sin (q_1+q_2)\\ 0\end{array}\right]\end{array}\end{array}$$

线速度的雅可比矩阵定义：

$$\begin{array}{}\text{(32)}& v_{c_i}=\frac{d}{dt}{r}_{c_i}=J_{v_i}(q)q^{\prime }\end{array}$$

其中，$r_{c_i}=r_{c_i}(q)$，是一个关于$q$ 的函数，而$q=\left[\begin{array}{c}{q}_1\\ 1_2\end{array}\right]$，是一组关于时间$t$ 的函数。
根据链式法则：

$$\begin{array}{}\text{(33)}& v_{c_i}=\frac{d}{dt}{r}_{c_i}=\frac{\partial r_{c_i}}{\partial q}\frac{dq}{dt}=\frac{\partial r_{c_i}}{\partial q}{q}^{\prime }\end{array}$$

可得线速度的雅可比矩阵为：

$$\begin{array}{}\text{(34)}& J_{v_{c_i}}(q)=\frac{\partial r_{c_i}}{\partial q}\end{array}$$

其中雅可比矩阵：

$$\begin{array}{}\text{(35)}& \begin{array}{l}{J}_{v_{c_1}}=\left[\begin{array}{cc}\frac{\partial r_{c_1}}{\partial q_1}& \frac{\partial r_{c_1}}{\partial q_2}\end{array}\right]=\left[\begin{array}{cc}-l_{c1}\sin (q_1)& 0\\ l_{c1}\cos (q_1)& 0\\ 0& 0\end{array}\right]\\ J_{v_{c_2}}=\left[\begin{array}{cc}\frac{\partial r_{c_2}}{\partial q_2}& \frac{\partial r_{c_2}}{\partial q_2}\end{array}\right]=\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{array}\end{array}$$

其线速度对应的动能为：

$$\begin{array}{}\text{(36)}& \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}{q}^{\prime T}(m_1{J}_{v_{c_1}}^T{J}_{v_{c_1}}+m_2{J}_{v_{c_2}}^T{J}_{v_{c_2}})q^{\prime }\end{array}$$

下面考虑转动部分动能：

$$\begin{gathered} \begin{array}{}\text{(37)}& \overrightarrow{{\omega }_1}=q_1^{\prime }\overrightarrow{k} \\ \overrightarrow{{\omega }_2}=(q_1^{\prime }+q_2^{\prime })\overrightarrow{k}\end{array} \end{gathered}$$

其中$\overrightarrow{k}=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$，表示方向向量。
因为${\omega }_i$与每个关节坐标系的$z$ 轴对齐，所以旋转动能可以简单的表示为：$\frac{1}{2}{I}_i{\omega }_i^2$，其中$I_i$ 是转动惯量，它的轴线穿过连杆的质心且平行于$z_i$轴。因此，对于广义坐标系，整个系统的旋转功能为：

$$\begin{gathered} \begin{array}{}\text{(38)}& \begin{array}{lll}K& =& \frac{1}{2}{I}_1(\overrightarrow{{\omega }_1^{\prime }}{)}^2+\frac{1}{2}{I}_2(\overrightarrow{{\omega }_2^{\prime }}{)}^2\\ & =& \frac{1}{2}\{{\overrightarrow{{\omega }_1^{\prime }}}^T{I}_1\overrightarrow{{\omega }_1^{\prime }}+{\overrightarrow{{\omega }_2^{\prime }}}^T{I}_2\overrightarrow{{\omega }_2^{\prime }}\}\\ & =& \frac{1}{2}\{(\left[\begin{array}{cc}1& 0\end{array}\right]q^{\prime }{)}^T{I}_1(\left[\begin{array}{cc}1& 0\end{array}\right]q^{\prime })+(\left[\begin{array}{cc}1& 1\end{array}\right]q^{\prime }{)}^T{I}_2(\left[\begin{array}{cc}1& 1\end{array}\right]q^{\prime }) \\ \}\\ & =& \frac{1}{2}\{q^{\prime T}\left[\begin{array}{c}1\\ 0\end{array}\right]I_1\left[\begin{array}{cc}1& 0\end{array}\right]q^{\prime }+q^{\prime T}\left[\begin{array}{c}1\\ 1\end{array}\right]I_2\left[\begin{array}{cc}1& 1\end{array}\right]q^{\prime } \\ \}\\ & =& \frac{1}{2}{q}^{\prime T}\{\left[\begin{array}{c}1\\ 0\end{array}\right]I_1\left[\begin{array}{cc}1& 0\end{array}\right]+\left[\begin{array}{c}1\\ 1\end{array}\right]I_2\left[\begin{array}{cc}1& 1\end{array}\right] \\ \}{q}^{\prime }\\ & =& \frac{1}{2}{q}^{\prime T}\{I_1\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]+I_2\left[\begin{array}{cc}1& 1\\ 1& 1\end{array}\right]\}{q}^{\prime }\\ & =& \frac{1}{2}{q}^{\prime T}\left[\begin{array}{cc}{I}_1+I_2& I_2\\ I_2& I_2\end{array}\right]q^{\prime }\end{array}\end{array} \end{gathered}$$

结合式$(14),(36),(38)$ 可得，惯性矩阵：

$$\begin{array}{}\text{(39)}& \begin{array}{lll}D(q)& =& m_1{J}_{v_{c_1}}^T{J}_{v_{c_1}}+m_2{J}_{v_{c_2}}^T{J}_{v_{c_2}}+\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]\\ & =& \left[\begin{array}{cc}\boxed{\text{与}{q}_1\text{相关的惯性}}& \boxed{q_1{q}_2\text{耦合}}\\ \boxed{q_1{q}_2\text{耦合}}& \boxed{\text{与}{q}_2\text{相关的惯性}}\end{array}\right]\end{array}\end{array}$$

结合式$(35),(39)$ 可得：

$$\begin{array}{}\text{(40)}& \begin{array}{lll}{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}& =& m2(l_{c2}^2+l_1{l}_{c2}\cos (q_2))+I_2\\ d_{22}& =& m_2{l}_{c2}^2+I_2\end{array}\end{array}$$

结合式$(23),(40)$ 可得$c_{ijk}$：

$$\begin{array}{}\text{(41)}& \begin{array}{lllll}{c}_{111}& =& \frac{1}{2}\frac{\partial d_{11}}{\partial q_1}& =& 0\\ c_{121}& =& \frac{1}{2}\frac{\partial d_{11}}{\partial q_2}& =& -m_2{l}_1{l}_{c2}\sin (q_2)& =& h\\ c_{211}& =& c_{121}& =& 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}& =& \frac{1}{2}\frac{\partial d_{22}}{\partial q_1}& =& 0\\ c_{212}& =& c_{122}& =& 0\\ c_{222}& =& \frac{1}{2}\frac{\partial d_{22}}{\partial q_2}& =& 0\end{array}\end{array}$$

接下来计算势能，机械臂的势能等于两个连杆的势能之和：

$$\begin{array}{}\text{(42)}& \begin{array}{lll}{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)g\sin (q_1)+m_2{l}_{c2}g\sin (q_1+q_2)\end{array}\end{array}$$

之前的广义重力$g_k$ 可变为：

$$\begin{array}{}\text{(42)}& \begin{array}{lll}{g}_1& =& \frac{\partial P}{\partial q_1}\\ & =& (m_1{l}_{c1}+m_2{l}_1)g\cos (q_1)+m_2{l}_{c2}g\cos (q_1+q_2)\\ g_2& =& \frac{\partial P}{\partial q_2}\\ & =& m_2{l}_{c2}g\cos (q_1+q_2)\end{array}\end{array}$$

结合式$(25),(40),(41),(42)$，最后可以写出系统的动力学方程：

$$\begin{array}{}\text{(43)}& \begin{array}{rcl}{d}_{11}{q}_1^{″}+d_{12}{q}_2^{″}+c_{121}{q}_1^{\prime }{q}_2^{\prime }+c_{211}{q}_2^{\prime }{q}_1^{\prime }+c_{221}{q}_2^{\prime 2}+g_1& =& {\tau }_1\\ d_{21}{q}_1^{″}+d_{22}{q}_2^{″}+c_{112}{q}_1^{\prime 2}+g_2& =& {\tau }_2\end{array}\end{array}$$

结合式$(26)$在这种情况下，原方程矩阵$C(q,q^{\prime })$ 由下式给出：

$$\begin{array}{}\text{(44)}& C=\left[\begin{array}{cc}h{q}_2^{\prime }& h{q}_2^{\prime }+h{q}_1^{\prime }\\ -h{q}_1^{\prime }& 0\end{array}\right]\end{array}$$

虚功原理的几何意义不过是说约束力$f=m{X}^{″}-F_a$，与运动轨迹垂直。

参考资料

1. 从理论力学到机器人动力学（二）：虚功原理与雅可比矩阵
2. 基于欧拉-拉格朗日方程的机器人动力学模型
3. 虛功原理及歐拉-拉格朗日方程式 | 数学播客 2021年12月 45卷4期