从二阶倒立摆开始的控制入门¶

系统建模¶

这里借用MIT Underactuated Robotics课程的Acrobot图片。实际上不是倒立摆但是结构完全一样

一个二阶倒立摆系统由3个部分组成：

Cart：底部的水平移动平台，位置由$x$描述；
Link1/Link2：连接在移动平台上的两个串联连杆，几何关系由$\theta_1$和$\theta_2$描述。为了简化计算，这里我们认为连杆无质量，所有质量集中在连杆末端（实际上这舍弃了一部分视角，但是没办法，不这么做的话计算量就太大了容易劝退）。两个连杆的质量分别为$m_1$和$m_2$。

其中我们可以控制施加在Cart上的水平控制力$F$，Link1和Link2都是被动关节（passive joint，无法主动控制位置或施加力矩的关节），这决定了倒立摆是非线性、强耦合和欠驱动（可控自由度<总自由度）的系统。我们的最终目标是通过控制$F$的大小和方向，让两根摆杆稳定在竖直向上的状态。

基于拉格朗日力学的动力学建模¶

倒立摆的非线性模型是经由动力学建模得到的，为此，我们引入拉格朗日力学的概念。

拉格朗日力学的原理需要自行学习，我推荐以下3个教程，如果时间允许，最好都看一遍。推荐的阅读顺序：先看第一个打牢基础，再看第二个明确思路，看第三个补全体系：

怎样在一天内学会分析力学（强烈推荐。毫不夸张的说，初中生都能学会，唯一的瑕疵是篇幅相对较长）
从零学分析力学（拉格朗日力学篇）（思路清晰，叙述简洁有力、主次分明）
梁昆淼《力学》第四版下册第二章、第三章（我自己入门分析力学看的材料。体系严谨，叙述蹙金结绣、高屋建瓴）

以及转动系，想说懂你不容易（跟分析力学关系不大，但是看完这篇会更好理解后面的部分内容）

这里主要展示技术性的操作步骤。这些步骤非常机械，其实没有太多展开讲解的必要。下面的步骤计算量非常大，推导完成之后会介绍利用MATLAB符号计算自动完成非线性模型推导的方法，但在此之前还是建议你至少手推一遍，这对理解原理和增加熟练度都很有帮助。为了提高你推导的积极性（以及节省篇幅），我会列出每一步的中间结果以供验证，但不会展示完整的推导过程。

Step 1：选取广义坐标：这是非常自然的，只需取

\[ \vec q=(x, \theta_1,\theta_2)^T \]

Step 2：我们需要找出系统的运动学表达式（和时间无关）。这基本上是简单的初中几何：

\[ \begin{bmatrix} x_1\\y_1 \end{bmatrix}=\begin{bmatrix} x\\0 \end{bmatrix} \]

\[ \begin{bmatrix} x_2\\y_2 \end{bmatrix}=\begin{bmatrix} x+l_1\sin\theta_1\\l_1\cos\theta_1 \end{bmatrix} \]

\[ \begin{bmatrix} x_3\\y_3 \end{bmatrix}=\begin{bmatrix} x+l_1\sin\theta_1+l_2\sin\theta_2\\ l_1\cos\theta_1+l_2\cos\theta_2 \end{bmatrix} \]

Step 3：然后分别计算广义速度、动能项$\mathcal{K_i}$和势能项$\mathcal{P}_i$。

根据初中物理，广义速度是广义坐标对时间的导数，因为质量集中在连杆末端（相当于是两个质点而不是刚体连杆），只考虑平动动能即可，所以动能项$\mathcal{K}_i=\frac{1}{2}m_i(\dot x_i^2+\dot y_i ^2)$，势能项$\mathcal{P}_i=m_i\mathrm{g}y_i$

\[ \begin{bmatrix} \dot x_1\\ \dot y_1 \end{bmatrix}=\begin{bmatrix} \dot x\\0 \end{bmatrix} \]

\[ \begin{bmatrix} \dot x_2 \\ \dot y_2 \end{bmatrix}= \begin{bmatrix} \dot{x} + l_1 \cos\theta_1 \dot{\theta}_1 \\ -l_1 \sin\theta_1 \dot{\theta}_1 \end{bmatrix} \]

\[ \begin{bmatrix} \dot x_3\\ \dot y_3 \end{bmatrix} = \begin{bmatrix} \dot{x} + l_1 \cos\theta_1 \dot{\theta}_1 + l_2 \cos\theta_2 \dot{\theta}_2 \\ -l_1 \sin\theta_1 \dot{\theta}_1 - l_2 \sin\theta_2 \dot{\theta}_2 \end{bmatrix} \]

\[ \mathcal{K}_1 = \frac{1}{2} M \dot{x}^2, \quad \mathcal{P}_1 = 0 \]

\[ \mathcal{K}_2 = \frac{1}{2} m_1 \dot{x}^2 + \frac{1}{2} m_1 l_1^2 \dot{\theta}_1^2 + m_1 l_1 \cos\theta_1 \dot{x} \dot{\theta}_1, \quad \mathcal{P}_2 = m_1 g l_1 \cos\theta_1 \]

\[ \mathcal{K}_3 = \frac{1}{2} m_2 \dot{x}^2 + \frac{1}{2} m_2 l_1^2 \dot{\theta}_1^2 + \frac{1}{2} m_2 l_2^2 \dot{\theta}_2^2 + m_2 l_1 \cos\theta_1 \dot{x} \dot{\theta}_1 + m_2 l_2 \cos\theta_2 \dot{x} \dot{\theta}_2 + m_2 l_1 l_2 \cos(\theta_1 - \theta_2) \dot{\theta}_1 \dot{\theta}_2 $$ $$ \mathcal{P}_3 = m_2 g (l_1 \cos\theta_1 + l_2 \cos\theta_2) \]

最终得到整个倒立摆系统的拉格朗日函数：

\[ \begin{aligned} \mathcal{L} &= \mathcal{K}-\mathcal{P}\\ &= \frac{1}{2}(M + m_1 + m_2) \dot{x}^2 + \frac{1}{2}(m_1 + m_2) l_1^2 \dot{\theta}_1^2 + \frac{1}{2} m_2 l_2^2 \dot{\theta}_2^2 + (m_1 + m_2) l_1 \cos\theta_1 \dot{x} \dot{\theta}_1 + m_2 l_2 \cos\theta_2 \dot{x} \dot{\theta}_2 + m_2 l_1 l_2 \cos(\theta_1 - \theta_2) \dot{\theta}_1 \dot{\theta}_2 -g l_1 (m_1 + m_2) \cos\theta_1 - m_2 g l_2 \cos\theta_2 \end{aligned} \]

Step 4：代入欧拉-拉格朗日方程，得到整个二阶倒立摆力学系统的动力学方程。

\[ \dfrac{\mathrm{d}}{\mathrm{d}t}\dfrac{\partial \mathcal{L}}{\partial \dot q_i}-\dfrac{\partial \mathcal{L}}{\partial q_i}=\tau_i \]

\[ \tau_1=(M + m_1 + m_2) \ddot{x} + l_1 (m_1 + m_2) \cos\theta_1 \cdot \ddot{\theta}_1 + \\l_2 m_2 \cos\theta_2 \cdot \ddot{\theta}_2 - l_1 (m_1 + m_2) \sin\theta_1 \cdot \dot{\theta}_1^2 - l_2 m_2 \sin\theta_2 \cdot \dot{\theta}_2^2 \]

\[ \tau_2=l_1 (m_1 + m_2) \cos\theta_1 \cdot \ddot{x} + l_1^2 (m_1 + m_2) \ddot{\theta}_1 + \\l_1 l_2 m_2 \cos(\theta_1 - \theta_2) \cdot \ddot{\theta}_2 + l_1 l_2 m_2 \sin(\theta_1 - \theta_2) \cdot \dot{\theta}_2^2\\ - g l_1 (m_1 + m_2) \sin\theta_1 \]

\[ \tau_3=l_2 m_2 \cos\theta_2 \cdot \ddot{x} + l_1 l_2 m_2 \cos(\theta_1 - \theta_2) \cdot \ddot{\theta}_1 + l_2^2 m_2 \cdot \ddot{\theta}_2 \\- l_1 l_2 m_2 \sin(\theta_1 - \theta_2) \cdot \dot{\theta}_1^2 - g l_2 m_2 \sin\theta_2 \]

Step 5：如果你真的手推了上面的整个过程，很容易发现$\tau_i$之间存在相似的结构。为了更清楚的展示这一点，我给上面的3个方程添加了换行。

容易发现，3个方程中都含有3个广义坐标的二阶导数项（广义加速度）。所有这些项按照行列排列起来，就称为力学系统的质量矩阵（稍后会解释这样命名它和其他两个参数的原因），通常记作$M(q)$，因为其中的元素是$\theta$的函数。如下：

\[ \begin{bmatrix} M + m_1 + m_2 & l_1 (m_1 + m_2) \cos\theta_1 & l_2 m_2 \cos\theta_2 \\ l_1 (m_1 + m_2) \cos\theta_1 & l_1^2 (m_1 + m_2) & l_1 l_2 m_2 \cos(\theta_1 - \theta_2) \\ l_2 m_2 \cos\theta_2 & l_1 l_2 m_2 \cos(\theta_1 - \theta_2) & l_2^2 m_2 \end{bmatrix} \]

很容易发现，该矩阵的大小和广义坐标相关，是$n\times n$矩阵。从左到右的3列分别对应3个广义坐标的二阶导数：$\ddot x,\ddot \theta_1,\ddot \theta_2$。质量矩阵必然是半正定的实对称矩阵——它要是不正定动能就得是负的了，这一点会在下面自然揭示.

为什么要把这个矩阵称为“质量矩阵”？当然是因为某种程度上它可以“当成”质量来用。（这种说法很不严谨，感性理解即可）

为了（感性地）说明这一点，我先举一个例子：拉格朗日力学中的系统动能表达式（注意和之前分开算的不同，是整个系统的动能）是

\[ \mathcal K=\dfrac{1}{2}\dot q^TM(q)\dot q \]

凭什么这个表达式就可以表征系统的动能？可以观察一下它的结构。我们熟知的质点动能表达式是$\mathcal K=\frac{1}{2}mv^2$，如果$v$是向量，它又可以写作$T=\frac{1}{2}m\vec v^T\vec v=\frac{1}{2}\vec v^Tm\vec v$，发现了吗？和上面的形式完全一致，只是速度换成了广义速度，质量换成了质量矩阵。事实上动能这个概念的物理实质就是速度的二次型。为了更加确信这一点，我们可以通过上面给出的动能表达式倒推回去来交叉验证：

\[ \begin{aligned} \mathcal K&=\dfrac{1}{2}\dot q^TM(q)\dot q\\ &=\dfrac{1}{2} \begin{bmatrix} \dot x & \dot \theta_1 & \dot \theta_2 \end{bmatrix} \begin{bmatrix} M + m_1 + m_2 & l_1 (m_1 + m_2) \cos\theta_1 & l_2 m_2 \cos\theta_2 \\ l_1 (m_1 + m_2) \cos\theta_1 & l_1^2 (m_1 + m_2) & l_1 l_2 m_2 \cos(\theta_1 - \theta_2) \\ l_2 m_2 \cos\theta_2 & l_1 l_2 m_2 \cos(\theta_1 - \theta_2) & l_2^2 m_2 \end{bmatrix} \begin{bmatrix} \dot x \\ \dot \theta_1 \\ \dot \theta_2 \end{bmatrix}\\ &=\frac{1}{2}(M + m_1 + m_2) \dot{x}^2 + \frac{1}{2}(m_1 + m_2) l_1^2 \dot{\theta}_1^2 + \frac{1}{2} m_2 l_2^2 \dot{\theta}_2^2 + (m_1 + m_2) l_1 \cos\theta_1 \dot{x} \dot{\theta}_1 + m_2 l_2 \cos\theta_2 \dot{x} \dot{\theta}_2 + m_2 l_1 l_2 \cos(\theta_1 - \theta_2) \dot{\theta}_1 \dot{\theta}_2 \end{aligned} \]

可以看到和上面拉格朗日函数的动能项部分一致。为什么会有这种奇妙的数学联系？我们可以从系统动能表达式的定义出发：

\[ \begin{aligned} \mathcal K=\sum^N_k\left(\dfrac{1}{2}m_k\vec v_k^2+\dfrac{1}{2}\vec \omega_k^T I_k\omega_k\right)\\ \end{aligned} \]

非常清晰，就是平动动能+转动动能，其中$I_k$是第$k$根连杆的惯性张量矩阵。但是注意这个表达式中的速度仍然是经典力学中的角速度和线速度，仍然不是我们处理的广义速度，所以，这里需要引入雅各比矩阵的概念。

这里指的是机器人学中的雅各比矩阵，和数学上的并不相同。雅各比矩阵是一个线性映射，从位形空间速度（各个关节的转动速度，在这里其实就是广义速度）映到操作空间速度（线速度和角速度组成的向量，具体见下），也就是

\[ v=Jq \]

其中

\[ \vec v=\begin{bmatrix} \omega_1 \\ \omega_2 \\...\\\omega_d \\ v_1 \\ v_2 \\... \\v_d \end{bmatrix} \]

为操作空间速度（又称为运动旋量）。$v$的维数和操作空间的维数$d$有关，为$2d$，很显然，雅各比矩阵是$2d\times N$矩阵。雅各比矩阵可以分成两个$d\times N$部分：$J_v$和$J_{\omega}$，它们以这样的方式组成$J$：$J=\begin{bmatrix}J_{\omega}\\J_{v}\end{bmatrix}$，分别代表从广义坐标到线速度和角速度的映射。

这里我们先把它当作一个工具来用，先用$J_v \dot q$和$J_{\omega}\dot q$替代线速度和角速度：

\[ \begin{aligned} \mathcal K&=\sum^N_k\left(\dfrac{1}{2}m_k\vec v_k^2+\dfrac{1}{2}\vec \omega_k^T I_k\omega_k\right)\\ &=\sum^N_k\left(\dfrac{1}{2}m_k(J_v\dot q)^2+\dfrac{1}{2}(J_{\omega}\dot q)^T I_k(J_{\omega}\dot q)\right)\\ &=\dfrac{1}{2}\dot q^T\left[\sum^N_k\left(m_kJ_v^TJ_v+\dfrac{1}{2}J_{\omega}^T I_kJ_{\omega}\right)\right]\dot q\\ \end{aligned} \]

可以发现这个形式和上面是相同的。由此，我们导出了质量矩阵的严格定义：

\[ M(q)=\sum^N_k\left(m_kJ_v^TJ_v+\dfrac{1}{2}J_{\omega}^T I_kJ_{\omega}\right) \]

还可以注意到，在动力学方程中质量矩阵乘上的是广义坐标的二阶导数（广义加速度），加上其他项之后在左侧得到广义力，这不禁令人联想到牛顿第二定律：$F=ma$，显然，质量矩阵在动力学方程中扮演的角色也是和质量相同的。

与此同时，在单刚体动力学中，质量的另一个作用是充当平动惯性的描述（转动惯性是由转动惯量描述的）。质量矩阵是否具有类似的功能呢？为了研究这个问题，我们引入一个“椭球”的概念。

椭球是一个封闭的凸曲面，是二次型在空间中对应的图像。举个例子，“二维椭球”就是我们熟悉的椭圆，三维椭球就是椭球面，三维以上的椭球统称“超椭球”。这里我们对不同维数的术语不做区分，统称“椭球”。接下来解释一下为什么椭球会和二次型扯上关系。根据高中数学我们知道椭圆的标准方程如下：

\[ \dfrac{x_1^2}{a_1^2}+\dfrac{x_2^2}{a_2^2}=1 \]

自然引出$n$维空间中的椭球方程

\[ \begin{aligned} \dfrac{x_1^2}{a_1^2}+\dfrac{x_2^2}{a_2^2}+...+\dfrac{x_n^2}{a_n^2}&=1\\ \begin{bmatrix} x_1 & x_2 &...& x_n \end{bmatrix} \begin{bmatrix} \dfrac{1}{a_1^2}& & &\\ &\dfrac{1}{a_2^2} & &\\ & & ...&\\ & & & \dfrac{1}{a_n^2} \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \\...\\ x_n \end{bmatrix}&=1\\ x^TDx=1 \end{aligned} \]

其中$D=\mathrm{diag}(\dfrac{1}{a_1^2},\dfrac{1}{a_2^2},...,\dfrac{1}{a_n^2})$，显然$D$既是对角矩阵，又是实对称矩阵。也就是，椭球本身和二次型是对应的。椭球的几何性质和矩阵$D$的特征值强相关：对角矩阵的特征值就是对角线上的元素，所以我们有椭球的第$i$个半轴长$a_i=\dfrac{1}{\sqrt{\lambda_i}}$。同时，对于实对称矩阵，不同特征值对应的特征向量是单位正交的，也就是所有特征向量$(\vec v_1,\vec v_2,...,\vec v_n)$构成一组单位正交基，且第$i$个特征向量代表第$i$个半轴的方向。

这里借用一张《现代机器人学》的图片。图中展示的是机器人的可操作度椭球$\dot q^T(JJ^T)^{-1}\dot q=1$的图像，由于是逆矩阵，半轴长相应取倒数。

如果从椭球的视角观察质量矩阵，我们会发现它和系统惯性有极其深刻的联系。考虑动能二次型$T=\dfrac{1}{2}\dot q^TM(q)\dot q$对应的椭球（称为惯性椭球），实际上可以证明，质量矩阵$M(q)$的每一个特征向量代表的是一根惯性主轴（不同的惯性主轴之间是惯性解耦的，也就是，沿一根惯性主轴施加加速度，不会在其他的惯性主轴方向上引起惯性效应），并且特征向量对应的特征值代表的是广义惯量。如果要感性地理解，可以认为在特征值越大，在对应特征向量方向上的惯性就越大，对加速度的抵抗就越大；特征值越小，在对应特征向量方向上的惯性就越小，对加速度的抵抗就越小。

为了证明上面的命题，我们考虑将广义速度沿特征向量方向分解，设$\dot q=c\vec v_i$，其中$c$是$\vec v_i$方向上的（标量）广义速度大小，得到：

\[ \begin{aligned} \mathcal K_i&=\dfrac{1}{2}\dot q^TM(q)\dot q\\ &=\dfrac{1}{2}(c\vec v_i)^TM(q)(c\vec v_i)\\ &=\dfrac{1}{2}c^2\vec v_i^TM(q)\vec v_i\\ &=\dfrac{1}{2}c^2\vec v_i^T\lambda_i\vec v_i\\ &=\dfrac{1}{2}\lambda_ic^2 \end{aligned} \]

也就是，在$v_i$方向上的动能仅依赖于该方向上的速度，而没有其他方向上的耦合项（这也是我们一开始的推导中可以将不同连杆的动能直接相加得到总动能的数学基础）。同时，可以注意到这个式子中$\lambda_i$扮演了类似质量的角色，它在此处其实就充当了广义惯量：特征值越大，惯性越大。

结束了对质量矩阵的讨论之后，我们可以注意到，广义主动力$\tau_i$的表达式中还存在很多$\dot\theta_i$和$\theta_i$的耦合项，这部分则称为科里奥利向量，通常记作$c(q,\dot q)$，如下：

\[ \begin{bmatrix} -l_1 (m_1 + m_2) \sin\theta_1 \cdot \dot{\theta}_1^2 -l_2 m_2 \sin\theta_2 \cdot \dot{\theta}_2^2 \\ l_1 l_2 m_2 \sin(\theta_1 - \theta_2) \cdot \dot{\theta}_2^2 \\ -l_1 l_2 m_2 \sin(\theta_1 - \theta_2) \cdot \dot{\theta}_1^2 \end{bmatrix} \]

科里奥利向量实际上包括两部分：离心力项（广义速度的二次项$\dot q^2$）和科里奥利力项（不同广义速度的交叉项$\dot q_i\dot q_j,i\neq j$），虽然如此，但是大家都喜欢叫它科里奥利向量，主要是科里奥利力-离心力向量这个名字太绕口了。

如果你真的有手动完成整个推导，就会发现，拉格朗日函数（的动能项）中有广义速度交叉项$\dot\theta_1\dot\theta_2$，但是上面的科里奥利向量中没有交叉项（在推导过程中被抵消了）。为什么呢？实际上这并不是巧合，而是联系到一些数学本质。

上面给出了拉格朗日力学的框架下系统动能的表达式$\mathcal{K}=\dfrac{1}{2}\dot q^TM(q)\dot q$，让我们考虑一种这样的可能性：如果把这个表达式代回拉格朗日函数中，再代入欧拉-拉格朗日方程进行求解，会得到什么？我的意思是：

\[ \begin{aligned} \dfrac{\mathrm{d}}{\mathrm{d}t}\dfrac{\partial \mathcal{L}}{\partial \dot q}-\dfrac{\partial \mathcal{L}}{\partial q}&=\tau\\ \dfrac{\mathrm{d}}{\mathrm{d}t}\dfrac{\partial}{\partial \dot q_i}\left(\dfrac{1}{2}\dot q^TM(q)\dot q-\mathcal{P}\right)-\dfrac{\partial }{\partial q}\left(\dfrac{1}{2}\dot q^TM(q)\dot q-\mathcal{P}\right)&=\tau\\ \dfrac{\mathrm{d}}{\mathrm{d}t}M(q)\dot q-\left(\dfrac{1}{2}\dfrac{\partial}{\partial q}\dot q^TM(q)\dot q-\dfrac{\partial}{\partial q}\mathcal P\right)&=\tau\\ \dot M(q)\dot q+M(q)\ddot q-\dfrac{1}{2}\dfrac{\partial}{\partial q}\left(\dot q^TM(q)\dot q\right)+g(q)&=\tau \end{aligned} \]

可能有必要规范一下符号：上面的推导中$q$是广义坐标向量，$\dot q$是广义速度向量，$\tau$是广义力向量，为了省事没有加箭头。上面的推导中将$\dfrac{\partial}{\partial q}\mathcal P$称为$g(q)$，正式名称是重力矩向量，它是系统势能的梯度向量，后面会详细讲解它，这里我们暂且将它当作一个记号。

同时需要注意，最后等式中的$\dot M(q)\dot q$和$\dfrac{\partial}{\partial q}\dot q^TM(q)\dot q$都是向量。这里我们暂时绕过了矩阵微积分的部分，这些下面会讲，对于上面的推导，可以直接把$M(q)$理解成一个函数

我们仔细观察一下最后得到的等式：

\[ \underbrace{M(q)\ddot q}_{质量矩阵}+\underbrace{\dot M(q)\dot q-\dfrac{1}{2}\dfrac{\partial}{\partial q}\left(\dot q^TM(q)\dot q\right)}_{???}+\underbrace{g(q)}_{重力矩向量}=\tau \]

很明显，排除了质量矩阵和重力矩向量后，中间剩下的就是科里奥利向量的表达式，由此，我们导出了科里奥利向量的严格定义：

\[ c(q,\dot q) =\dot M(q)\dot q-\dfrac{1}{2}\dfrac{\partial}{\partial q}\left(\dot q^TM(q)\dot q\right) \]

相比质量矩阵的严格定义，这个定义看起来非常“丑陋”，看不出什么物理意义，接下来我们通过一些数学技巧来挖掘出它更本质的形式：

如果我们将将任意向量$L$的第$k$个元素记作$[L]_k$，就得到

\[ \begin{aligned} \left[c(q,\dot q)\right]_k&=\left[\dot M(q)\dot q\right]_k-\dfrac{1}{2}\left[\dfrac{\partial}{\partial q}\left(\dot q^TM(q)\dot q\right)\right]_k\\ &=\sum_j\dot M_{kj}\dot q_j - \dfrac{1}{2}\dfrac{\partial}{\partial q_k}\left(\sum_i\sum_j \dot q_i\dot M_{ij}\dot q_j\right)\\ &=\sum_j\left(\sum_i\dfrac{\partial M_{kj}}{\partial q_i}\dot q_i\right)\dot q_j- \dfrac{1}{2}\dfrac{\partial}{\partial q_k}\sum_i\sum_j \dot M_{ij}\dot q_i\dot q_j\\ &=\sum_i\sum_j\dfrac{\partial M_{kj}}{\partial q_i}\dot q_i\dot q_j-\dfrac{1}{2}\sum_i\sum_j\dfrac{\partial M_{ij}}{\partial q_k}\dot q_i\dot q_j\\ &=\sum_i\sum_j\left(\dfrac{\partial M_{kj}}{\partial q_i}-\dfrac{1}{2}\dfrac{\partial M_{ij}}{\partial q_k}\right)\dot q_i\dot q_j \end{aligned} \]

上面基本上是简单的矩阵微积分和求和的技巧，接下来的内容会更有技巧性，需要集中注意力。通过交换求和指标$i$和$j$，我们注意到

\[ \begin{aligned} \sum_i\sum_j\dfrac{\partial M_{kj}}{\partial q_i}\dot q_i\dot q_j =\sum_j\sum_i\dfrac{\partial M_{ki}}{\partial q_j}\dot q_j\dot q_i =\sum_i\sum_j\dfrac{\partial M_{ki}}{\partial q_j}\dot q_i\dot q_j \end{aligned} \]

也就是说：

\[ \left[c(q,\dot q)\right]_k=\dfrac{1}{2}\sum_i\sum_j\left(\dfrac{\partial M_{kj}}{\partial q_i}+\dfrac{\partial M_{ki}}{\partial q_j}-\dfrac{\partial M_{ij}}{\partial q_k}\right)\dot q_i\dot q_j \]

这样，我们就把科里奥利向量从上面的原始定义化简成了极其简洁、优美的轮换对称形式。但是，这个形式还能进一步简化吗？为了进一步简化，这里引入第一类Christoffel记号：

\[ \Gamma_{ijk}=\dfrac{1}{2}\left(\dfrac{\partial M_{jk}}{\partial q_i}+\dfrac{\partial M_{ik}}{\partial q_j}-\dfrac{\partial M_{ij}}{\partial q_k}\right) \]

最终就得到一个非常干净的二次型表达

\[ \begin{aligned} \left[c(q,\dot q)\right]_k&=\sum_i\sum_j\Gamma_{ijk}\dot q_i\dot q_j\\ c(q,\dot q)&=\dot q^T\Gamma(q)\dot q \end{aligned} \]

其中$\Gamma(q)$是$n\times n\times n$矩阵，满足

\[ \Gamma(q)= \begin{bmatrix} \Gamma_1 \\ \Gamma_2 \\ ... \\ \Gamma_n \\ \end{bmatrix},\, \Gamma_i(q)= \begin{bmatrix} \Gamma_{i11} & \Gamma_{i12} & \cdots & \Gamma_{i1n}\\ \Gamma_{i21} & \Gamma_{i22} & \cdots & \Gamma_{i2n}\\ \vdots & \vdots & & \vdots\\ \Gamma_{in1} & \Gamma_{in2} & \cdots & \Gamma_{inn}\\ \end{bmatrix} \]

可能你会好奇为什么数学上“刚好”有个记号和科里奥利向量的数学表示对应，这是因为第一类Christoffel符号和黎曼流形上的Levi-Civita联络直接相关，更具体的解释需要微分几何基础，这里不打算展开（这要再展开就有点拔的太高了，调个车不用懂微分几何的）。如果你感兴趣，可以自行搜索“几何控制”或Levi-Civita 联络。

接下来我们回到正题，为什么我们处理的二阶倒立摆系统中没有出现广义速度交叉项？我们观察一下$\left[c(q,\dot q)\right]$的表达式，可以发现，如果要出现交叉项，这一项的$\Gamma_{ijk}$必须不为零。如果我们再回头观察一下质量矩阵，可以发现，它呈现出非常清晰的反对称结构，也就是$M_{ij}=M_{ji}$，两个指标相同时，质量矩阵对角线上的元素（和广义坐标无关）对广义坐标求导得到0，两个指标不同时，质量矩阵非对角线上的元素和它的对称元素相减消去，最终导致任何交叉项的系数都是0，因此这个系统的科里奥利向量中没有科里奥利力项。

另外，补充一个常识：一些文献（包括本文的一些部分）会采用如下的记号$C(q)$（称为科里奥利矩阵）

\[ C(q)\dot q=c(q,\dot q) \]

这实际上是把含有$\dot q$的项单独剥离出来，剩下的部分作为一个矩阵处理。这和科里奥利向量的本质是一样的，只是换了个形式。

最后，我们将剩下的部分——也就是仅含有广义坐标自身（零阶导数）的项称为重力矩向量，通常记作$g(q)$，如下

\[ \begin{bmatrix} 0 \\ -g l_1 (m_1 + m_2) \sin\theta_1 \\ -g l_2 m_2 \sin\theta_2 \end{bmatrix} \]

实际上这里的重力矩指的是重力在各个关节上产生的力矩作用，机械臂的重力补偿就是基于重力矩向量实现的。不像前两个参数，重力矩向量的物理含义非常清晰，就是重力在广义坐标空间的投影，它本质上是重力势能函数$\mathcal P$的负梯度：

\[ g(q)=-\nabla_q \mathcal P(q)=-\dfrac{\partial \mathcal P}{\partial q} \]

最后的最后，利用质量矩阵、科里奥利向量和重力矩向量这三个参数，我们就可以将通过拉格朗日力学得到的动力学方程描述化简成标准动力学方程，形式如下：

\[ \vec\tau=M(q)\ddot{\vec{q}}+C(q)\dot q+g(q) \]

本节最后，需要解释一下怎样使用MATLAB符号计算进行非线性模型推导，得到$M$、$c$和$g$。可以直接看下面的代码，注释非常详尽。如果你没碰过Symbolic Math Toolbox，可以直接看文档学习：Symbolic Math Toolbox Documentation

实际上如果你准备接轮腿或者工程的电控，CAS（计算机代数系统，简单来讲就是自动推公式）工具应该是必备技能，无论是MATLAB、Mathematica还是Sympy，否则很容易推了n张A4纸之后发现是错的，那样真的会欲哭无泪，我自己是碰见过相当多次这种情况。

clc;
clear;

syms t g real
syms L real

L = 0; % 拉格朗日函数
n = 3; % 广义坐标数量
x = sym('x', [1 n]); % 水平方向表达式
y = sym('y', [1 n]); % 竖直方向表达式
m = sym('m', [1 n]); % 连杆质量
l = sym("l", [1 n]); % 连杆长度
% 批量创建符号函数
for i = 1:n
    q(i) = symfun(sprintf('q%d(t)', i), t);
end
tau = sym('tau', [1 n]);

% 运动学表达式
x(1) = q(1);
y(1) = 0;

x(2) = q(1) + l(1)*sin(q(2));
y(2) = l(1)*cos(q(2));

x(3) = q(1) + l(1)*sin(q(2)) + l(2)*sin(q(3));
y(3) = l(1)*cos(q(2)) + l(2)*cos(q(3));

for i = 1:n
    % 求x和y两个方向的广义速度
    dxdt = diff(x(i), t);
    dydt = diff(y(i), t);
    % 计算动能和势能项
    % 这里动能项加了展开、化简和合并同类项的步骤，主要是方便打印查看
    k = 0.5*m(i)*collect(simplify(expand(dxdt^2+dydt^2), "Steps", 50), diff(q, t));
    p = m(i)*g*y(i);
    % 累加到拉格朗日函数
    L = L + k - p;
end

for i = 1:n
    % 通过欧拉-拉格朗日方程计算广义力表达式
    % 这里化简之后把二阶导数项全部提出来，方便打印查看
    tau(i) = collect(simplify(expand(diff(diff(L, diff(q(i), t)), t) - diff(L, q(i))), "Steps", 50), diff(q,t,2));
end

syms M H C G real
qdd_s = sym("d2q%ddt2", [1 n]);
qd_s = sym("dq%ddt", [1 n]);
% 这里需要说明一下，MATLAB中equationsToMatrix函数的vars参数不支持符号函数，只能是独立符号变量。我的解决方法是直接定义对应的符号变量然后用subs代入替换
tau_s = subs(tau, [diff(q, t, 2) diff(q, t)], [qdd_s qd_s]);
% equationsToMatrix函数会提取出tau_s中所有带广义坐标二阶导数的项系数放到M矩阵，剩下的项挪到方程右侧（变号），系数放进H矩阵
[M, H] = equationsToMatrix(tau_s, qdd_s);
H = -H; % 上面变号之后再变回来
% 所有二阶导数项被提走之后，还剩下一阶和零阶项，此时把一阶项全部置零，就得到重力矩项
G = subs(H, qd_s, zeros(1, n));
C = simplify(H - G);
disp(simplify(M));
disp(simplify(C));
disp(simplify(G));

系统的数学模型¶

在上一节中我们拿到了倒立摆的的非线性动力学模型，接下来我们考虑对它进行状态空间建模。

首先解释一下系统建模的最终目的：要拿到这个系统的状态空间方程。所谓状态空间方程，就是用于描述系统行为和性质的两个方程：

第一个是关于时间的常微分方程，描述系统随时间的演化性质；

第二个是代数方程，用于描述输出和状态/输入的直接关系。

参与状态空间方程的有3个关键的向量：状态向量$\vec x$、控制向量$\vec u$和输出向量$\vec y$。

状态向量是用于描述系统动态所需的所有变量（或曰“我们关心的最少独立变量集合”）。现实世界中的系统非常复杂，对于真实系统，严格地说，实际上变量的数目是无穷的，将它们全部纳入数学模型并不现实。为了解决实际的工程问题，我们必须省去无关变量、利用约束关系化简冗余变量，并忽略对系统整体动态无显著影响的边缘变量，最终得到一个能够满足实际工程需要的、无法继续简化的最少独立变量集合。例如，对于我们现在研究的倒立摆模型，采用的状态向量如下：

\[ \vec{x}=\begin{bmatrix} x\\ \theta_1\\ \theta_2 \\ \dot{x}\\ \dot \theta_1\\ \dot \theta_2 \end{bmatrix} \]

注意区分Cart的水平位置$x$和状态向量$\vec{x}$

可以发现，对于真实世界中的倒立摆，需要考虑的变量远比这6个要多，例如环境温度会影响金属的质量分布，进而影响连杆的转动惯量，但是这种影响极其轻微，对整体的系统动态和几何关系没有显著影响，因此不会被纳入建模考量。

控制向量是我们可以对系统施加的控制量集合。我们无法直接控制系统的每一个部分的每一个参数——甚至也无法直接控制任意一个部分的每一个参数。真正能做到的仅仅是控制系统其中一些部分的其中一些参数。在这个倒立摆的例子中，我们唯一能直接控制的是施加在Cart上的控制力$F$这一个参数，至于其他的参数（如摆角、Cart位置等）将会由控制量和系统当前状态通过系统的自然演化和动力学约束来决定。也就是控制向量如下：

\[ \vec{u}= \begin{bmatrix} F \end{bmatrix} \]

可能看起来有点滑稽，但它确实是个只有一个元素的向量

输出向量是我们关心的/可以实际测量的系统输出量集合。为什么需要“额外”定义一个向量？因为在真实系统中，很多时候并不是状态向量的每一个变量都能实际观测，或者不是每一个变量我们都关心。实际上输出向量就是系统对外的输出通道，是经过C矩阵（后面会讲）“过滤”后的系统内部动态。对于我们现在研究的倒立摆，实际上输出向量可以完全和状态向量等同。

根据状态空间方程的数学特征，我们可以简单地将所有系统划分为线性系统和非线性系统。

如果$\dot{\vec{x}}$能表示为$\vec x$和$\vec u$的线性组合，则称该系统为线性系统，否则为非线性系统。

为什么系统的“线性性”如此重要，以至于我们要采用“是否线性”这一标准将所有系统一分为二？这就涉及到线性系统一些至关重要的数学性质。

线性系统的状态空间方程具有如下的结构：

\[ \begin{aligned} \dot{\vec{x}} &= A\vec{x}+B\vec{u}\\ \vec y&=C\vec{x}+D\vec{u} \end{aligned} \]

也就是，对于线性系统而言，我们最终实际关心的是$A$、$B$、$C$、$D$四个矩阵。这里我们先建立对它们的感性认知，稍后会从矩阵的语言分析它们的数学内涵

系统矩阵$A$定义不同状态变量的相互耦合关系，描述系统在无输入状态下的自然演化。

控制矩阵$B$定义输入量对系统状态的影响方式，表征外部输入对内部状态的直接作用。

输出矩阵$C$定义状态向量的哪些部分组成输出向量。在我们这个例子中它是单位阵。

前馈矩阵$D$定义输入向量怎样直接影响输出向量（与系统行为无关），用的比较少，通常都是零矩阵。

对于线性系统而言，$A$、$B$、$C$和$D$四个矩阵只可能是常数矩阵或与时间$t$相关的矩阵。

对于前一种情况（四个都是常数矩阵），这样的系统称为线性时不变（LTI）系统，

对于后一种情况（四个矩阵中存在和时间相关的项）则称为线性时变（LTV）系统。

从线性系统的数学定义中我们可以看到以下几个要点：

1.整个系统的内部动态和演化性质完全是由四个矩阵确定的，也就是，我们可以利用矩阵的语言来定义系统性质。后面会提到系统能控性、能观性等一系列概念，都是用矩阵的语言表述的。

2.线性系统的状态空间方程是一个线性方程组，这意味着我们可以在线性代数的框架下研究系统行为，使用比较简单的数学工具就可以达到目的，降低了难度和复杂性。非线性系统则需要在微分几何和Lie理论的框架下研究，难度和复杂性都更高。

基于以上两点，在实际工程应用中我们经常会选择线性化（linearization）的方法来将非线性系统模型转化成线性系统模型，线性化的策略又分为两种：基于泰勒展开的平衡点线性化，和反馈线性化，这两种方法后面都会讲。

为了研究它的性质，我们先尝试把之前拿到的动力学方程（定义在广义坐标空间）转换到状态空间（也就是用状态向量表示）

\[ \vec\tau=M(q)\ddot{\vec{q}}+C(q)\dot q+g(q) \]

我们可以注意到状态向量$\vec x=\begin{bmatrix}q\\\dot q\end{bmatrix}$，从而$\dot{ \vec{x}}=\begin{bmatrix}\dot q\\\ddot q\end{bmatrix}$，于是自然想到有

\[ \ddot q=M^{-1}(q)\left[\tau-C(q)\dot q-g(q)\right] \]

\[ \begin{aligned} \dot x&= \begin{bmatrix} \dot q \\ M^{-1}(q)\left[\tau-C(q)\dot q-g(q)\right] \end{bmatrix}\\ &= \begin{bmatrix} \dot q \\ M^{-1}(q)\left[-C(q)\dot q-g(q)\right] \end{bmatrix}+ \begin{bmatrix} 0 \\ M^{-1}(q) \end{bmatrix}\tau \end{aligned} \]

可能这么操作看起来有点简单粗暴，我刚学的时候也是这么想的，后面我们会看到这样做的合理性。