Underactuated Robotics - 欠驱动机器人学（三）- 体操机器人、小车摆杆和四旋翼飞行器

kuan_li_lyg

于 2024-08-30 01:18:03 发布

阅读量396

点赞数 4

文章标签：机器人人工智能

本文链接：https://blog.csdn.net/weixin_46300916/article/details/141691692

版权

系列文章目录

前言

在低维模型系统的背景下，已经开展了大量有关欠驱动系统控制的工作。这些模型系统捕捉到了问题的本质，却没有引入更真实世界的例子中经常涉及的所有复杂性。在本章中，我们将重点讨论两个最著名、研究最深入的模型系统--Acrobot 和 Cart-Pole。在我们开发了一些工具后，我们将看到这些工具可以直接应用于其他模型系统；我们将使用四旋翼机器人举出一些例子。所有这些系统都是微不足道的欠驱动系统，执行器数量少于自由度。

一、杂技机器人

Acrobot 是垂直面上的平面双部件机械臂（逆重力工作），肘部有一个执行器，但肩部没有执行器。它的详细介绍最早见于 [1]。与之配套的系统在肩部有一个致动器，但在肘部没有致动器，被称为 Pendubot[2]。杂技机器人之所以被命名为杂技机器人，是因为它类似于单杠上的体操运动员（或杂技演员），主要通过腰部用力（而不是手腕用力）来控制自己的运动。为杂技机器人研究的最常见控制任务是上摆任务，在该任务中，系统必须使用肘部（或腰部）扭矩将系统移动到垂直配置，然后保持平衡。

杂技机器人是欠驱动机器人面临的主要挑战的代表。为了使整个系统摆动起来并保持平衡，控制器必须推理并利用驱动自由度与非驱动自由度之间的状态耦合。这也是一个重要的系统，因为正如我们将要看到的，它与行走机器人最简单的模型之一非常相似。

1.1 运动方程

图 3.1 展示了我们分析中使用的模型参数。 $\theta _1$ 是肩关节角度， $\theta _2$ 是肘（相对）关节角度，我们将使用 $\mathbf{q}=[\theta _1,\theta _2]^T$ 、 $\mathbf{x}=[\bold{q},\dot{\mathbf{q}}]^T$ 。零配置是两个部件都直接指向下方。惯性矩 $I_1,I_2$ 取自枢轴。任务是稳定不稳定的固定点 $\mathbf{x}=[\pi ,0,0,0]^T$ 。

我们将利用拉格朗日方法推导出杂技机器人的运动方程。每个部件的质心位置 $\mathbf{P}_{c1},\mathbf{P}_{c2}$ 由运动学给出：

$\mathbf{P}_{c1}=\begin{bmatrix} l_{c1}s_1\\ -l_{c1}c_1 \end{bmatrix},\mathbf{P}_{c2}=\begin{bmatrix} l_1s_1+l_{c2}s_{1+2}\\ -l_{1}c_1-l_{c2}c_{1+2} \end{bmatrix},$

其中 $s_1$ 是 $\sin ({\theta _1})$ 的简称， $c_{1+2}$ 是 $\cos (\theta _1+\theta _2)$ 的简称，等等。能量的计算公式为

$T=T_1+T_2, \, T_1=\dfrac{1}{2}I_1\dot{q}_1^2$

${ T}_{2}=\dfrac{1}{2}(m_{2}l_{1}^{2}+I_{2}+2m_{2}l_{1}l_{c2}c_{2})\dot{q}_{1}^{2}+\dfrac{1}{2}I_{2}\dot{q}_{2}^{2}+(I_{2}+m_{2}l_{1}l_{c2}c_{2})\dot{q}_{1}\dot{q}_{2}$

$U=-m_{1}g l_{c1}c_{1}^{}-m_{2}g(l_{1}c_{1}^{}+l_{c2}c_{1+2}^{})$

将这些量输入拉格朗日，就得到了运动方程：

$\begin{array}{c}{{(I_{1}+I_{2}+m_{2}l_{1}^{2}+2m_{2}l_{1}l_{c2}c_{2})\ddot{q}_{1}+(I_{2}+m_{2}l_{1}l_{c2}c_{2})\ddot{q}_{2}-2m_{2}l_{1}l_{c2}s_{2}\dot{q}_{1}\dot{q}_{2}}}\\ {{-m_{2}l_{1}l_{c2}s_{2}\dot{q}_{2}^{2}+m_{1}g l_{c1}s_1+m_{2}g(l_{1}s_{1}+l_{c2}s_{1+2})=0}}\end{array}$

$(I_{2}+m_{2}l_{1}l_{c2}c_{2})\ddot{q}_{1}+I_{2}\ddot{q}_{2}+m_{2}l_{1}l_{c2}s_{2}\dot{q}_{1}^{2}+m_{2}g l_{c2}s_{1+2}=\tau$

以标准的机械手方程形式表示：

$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=\tau _g(\mathbf{q})+\mathbf{B}\mathbf{u}$

利用 $\mathbf{q}=[\theta _1,\theta _2]^T$ , $\mathbf{u}=\tau$ 可以得出

$\mathbf{M}(\mathbf{q})=\begin{bmatrix} I_{1}+I_{2}+m_{2}l_{1}^{2}+2m_{2}l_{1}l_{c2}c_{2} & I_{2}+m_{2}l_{1}l_{c2}c_{2} \\ I_{2}+m_{2}l_{1}l_{c2}c_{2} & I_2 \end{bmatrix},$

$\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\begin{bmatrix} -2m_{2}l_{1}l_{c2}s_{2}\dot{q}_{2} & -m_{2}l_{1}l_{c2}s_{2}\dot{q}_{2} \\ m_{2}l_{1}l_{c2}s_{2}\dot{q}_{1} & 0 \end{bmatrix},$

$\tau _g(\mathbf{q})=\begin{bmatrix} -m_{1}g l_{c1}s_1-m_{2}g(l_{1}s_{1}+l_{c2}s_{1+2}) \\ -m_{2}g l_{c2}s_{1+2} \end{bmatrix}$

$\mathbf{B}=\begin{bmatrix} 0 \\ 1 \end{bmatrix}$

二、车-杆系统

我们在这里要研究的另一个模型系统是小车-杆系统，其中的任务是仅利用小车上的水平力，围绕其不稳定平衡平衡一个简单的摆。小车-杆系统的平衡在许多控制入门课程中都有使用，包括麻省理工学院的 6.003，因为它可以通过简单的线性控制（如杆放置）技术来实现。在本章中，我们将考虑完全摆动和平衡控制问题，这需要完全的非线性控制处理。

$x$ 是小车的水平位置， $\theta$ 是摆锤的逆时针角度（0 为竖直向下）。我们将使用 $\mathbf{q}=[x,\theta ]^T$ 和 $\mathbf{x}=[\bold{q},\dot{\mathbf{q}}]^T$ 。任务是稳定 $\mathbf{x}=[0,\pi ,0,0]^T$ 处的不稳定定点。

2.1 运动方程

系统的运动学方程为

$\mathbf{x}_1=\begin{bmatrix} x\\ 0 \end{bmatrix}, \mathbf{x}_2=\begin{bmatrix} x+l\sin \theta \\ -l\cos \theta \end{bmatrix},$

能量的计算公式为

$T=\dfrac{1}{2}(m_c+m_p)\dot{x}^2+m_p\dot{x}\dot{\theta }l\cos \theta +\dfrac{1}{2}m_pl^2\dot{\theta }^2$

$U=-m_pgl\cos \theta$

拉格朗日得出运动方程：

$(m_c+m_p)\ddot{x}+m_pl\ddot{\theta}\cos\theta-m_pl\dot{\theta}^2\sin\theta=f_x$

$m_pl\ddot{x}\cos\theta+m_pl^2\ddot{\theta}+m_pgl\sin\theta=0$

以标准的机械手方程形式表示：

$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=\tau _g(\mathbf{q})+\mathbf{B}\mathbf{u}$

利用 $\mathbf{q}=[x,\theta ]^T$ , $\mathbf{u}=f_x$ 可以得出

$\mathbf{M}(\mathbf{q})=\begin{bmatrix} m_c+m_p & m_pl\cos\theta\\ m_pl\cos\theta & m_pl^2 \end{bmatrix},\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\begin{bmatrix} 0 & -m_pl\dot\theta\sin\theta\\ 0 & 0 \end{bmatrix}$

$\tau _g(\mathbf{q})=\begin{bmatrix} 0\\ -m_pgl\sin\theta \end{bmatrix},\mathbf{B}=\begin{bmatrix} 1\\ 0 \end{bmatrix}$

在这种情况下，直接求解加速度特别容易：

$\begin{matrix} \ddot{x} =& \dfrac{1}{m_c + m_p \sin^2\theta}\left[ f_x + m_p \sin\theta (l \dot\theta^2 + g\cos\theta)\right] \label{eq:ddot_x}\\ \ddot{\theta} =& \dfrac{1}{l(m_c + m_p \sin^2\theta)} \left[ -f_x \cos\theta - m_p l \dot\theta^2 \cos\theta \sin\theta - (m_c + m_p) g \sin\theta \right] \label{eq:ddot_theta} \end{matrix}$

在接下来的一些分析中，我们将研究运动方程的形式，忽略细节，任意设置所有常数为 1：

$\begin{matrix} 2\ddot{x} + \ddot\theta \cos\theta - \dot\theta^2 \sin\theta = f_x \label{eq:simple}\\ \ddot{x}\cos\theta + \ddot\theta + \sin\theta = 0. \label{eq:simple2} \end{matrix}$

三、四旋翼飞行器

在过去的几年里，四旋翼飞行器大受欢迎--业余爱好者在外转子电机方面的进步使其功能强大、重量轻且价格低廉！它们足够强大，可以携带有趣的有效载荷（例如用于测绘/拍摄环境的传感器），但又足够动态，可以相对快速地移动。最有趣的动态效果在更高的速度下开始显现，这时推进器开始从水平运动产生的气流中获得升力，或者当它们离地面足够近时，会产生明显的地面效应，但我们不会在这里尝试捕捉这些效应（暂时）。

当四旋翼飞行器革命开始时，我曾预言，人们很快就会意识到固定翼飞行器更适合大多数应用。推进器产生推力的效率几乎是最佳的，这使得四旋翼飞行器在悬停时非常高效，但要在向前飞行时保持高效，你可能需要一个翼面。机翼确实是个好主意！但我错了，四旋翼无人机已经完全取代了固定翼商用无人机。也许只是因为它们更容易控制？也许还有希望...

3.1 平面四旋翼飞行器

我们可以从一个极其简单的四旋翼飞行器模型开始，它只能在飞机上飞行。在这种情况下，它实际上只需要两个推进器，但称之为 "生物旋翼机 "就不太合适了。由于它只有一个刚体，因此运动方程几乎微不足道，当然也符合我们的标准操纵器方程：

$\begin{matrix} m \ddot{x} = -(u_1 + u_2)\sin\theta, \label{eq:quad_x}\\ m \ddot{y} = (u_1 + u_2)\cos\theta - mg, \label{eq:quad_y}\\ I \ddot\theta = r (u_1 - u_2) \label{eq:quad_theta} \end{matrix}$

平面四旋翼系统（我们在代码中也将其称为 "Quadrotor2D"，以便在目录列表中将其放在 "Quadrotor "旁边）。模型参数为质量 m、惯性矩 I 和螺旋桨中心到底部的距离 r。

3.2 全 3D 四旋翼飞行器

三维四旋翼飞行器的动力学原理与此类似。唯一的复杂之处在于处理 3D 旋转。在 Drake 中实现 QuadrotorPlant 示例动力学的代码几乎与 LaTeX 推导一样可读。该模型最有趣的特点是包含了螺旋桨旋转产生的力矩。令人惊讶的是，如果没有这些力矩，围绕悬停配置线性化的系统实际上是不可控制的。

QuadrotorPlant 示例实现了简单模型。但通常情况下，您可能希望为四旋翼飞行器模型添加额外的功能。例如，您可能希望添加碰撞动力学以着陆或从障碍物上弹起，或者您可能希望在四旋翼飞行器上添加摆锤以保持平衡，或者在底部悬挂有效载荷。在这些情况下，使用 Drake 的主物理引擎（在 MultibodyPlant 中）要比手动执行方程式方便得多。我们只需手动将螺旋桨力导入图中即可（因为螺旋桨还不是 URDF 或 SDF 支持的概念）。我在笔记本中并列实现了这两个版本。

请注意，默认情况下，MultibodyPlant 中每个浮体的方向都由单位四元数表示。如果尝试使用四元数表示状态对模型进行线性化，而不考虑单位规范约束，那么该模型的线性化也是不可控的。为了暂时避免这种情况，我在模型中手动添加了 "滚动-俯仰-偏航 "浮动基数。这样可以使线性化保持简单，但在 "万向节锁定 "时会引入一个奇点。在这些说明中，我尽量避免涉及三维旋转，但我在这里还有一些相关说明。

四、平衡

对于杂技机器人系统和小车摆杆系统，我们首先要设计一个线性控制器，当系统开始接近不稳定的固定点时，该控制器可以平衡系统。为此，我们将对有关固定点的非线性方程进行线性化处理，检查该线性系统的可控性，然后利用线性二次调节器 (LQR) 理论设计我们的反馈控制器。

4.1 机械手方程线性化

虽然这两个模型系统的运动方程都比较简单，但前向动力学仍涉及大量非线性项，必须在任何线性化过程中加以考虑。让我们来考虑操纵器方程描述的系统线性化的一般问题。

我们可以使用泰勒展开法围绕定点 $(\mathbf{x}^*, \mathbf{u}^*)$ 进行线性化：

${\dot{\mathbf{x}}}=\mathbf{f}(\mathbf{x},\mathbf{u})\approx\mathbf{f}(\mathbf{x}^{*},\mathbf{u}^{*})+\left[{\dfrac{\partial\mathbf{f}}{\partial\mathbf{x}}}\right]_{\mathbf{x}=\mathbf{x}^{*},\mathbf{u}=\mathbf{u}^{*}}(\mathbf{x}-\mathbf{x}^{*})+\left[{\dfrac{\partial\mathbf{f}}{\partial\mathbf{u}}}\right]_{\mathbf{x}=\mathbf{x}^{*},\mathbf{u}=\mathbf{u}^{*}}(\mathbf{u}-\mathbf{u}^{*})$

让我们考虑一下围绕一个（稳定或不稳定的）固定点对操纵器方程进行线性化的具体问题。在这种情况下， $\mathbf{f}(\mathbf{x}^{*},\mathbf{u}^{*})$ 为零，我们只剩下标准的线性状态空间形式：

$\dot{\mathbf{x}}=\begin{bmatrix} \dot{\mathbf{q}}\\ \mathbf{M}^{-1}(\mathbf{q})[\tau_g(\mathbf{q})\mathbf{u}-\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}] \end{bmatrix},$

$\approx \mathbf{A}_{lin}(\mathbf{x}-\mathbf{x}^*)+\mathbf{B}_{lin}(\mathbf{u}-\mathbf{u}^*)$

其中 $\mathbf{A}_{lin}$ 和 $\mathbf{B}_{lin}$ 是常数矩阵。让我们定义 $\bar{\mathbf{x}} = \mathbf{x} - \mathbf{x}^*, \bar{\mathbf{u}} = \mathbf{u} - \mathbf{u}^*$ ，并写出

$\dot{\bar{\mathbf{x}}} = {\mathbf{A}}_{lin}\bar{\mathbf{x}} + \mathbf{B}_{lin}\bar{\mathbf{u}}.$

对定点周围的泰勒展开式进行求值，可以得到以下非常简单的方程，其分块形式为：

$\begin{matrix} {\mathbf{A}}_{lin} =& \begin{bmatrix} {\mathbf{0}} & {\mathbf{I}} \\ \mathbf{M}^{-1} \dfrac{\partial \tau_g}{\partial \mathbf{q}} + \sum_{j} \mathbf{M}^{-1}\dfrac{\partial \mathbf{B}_j}{\partial \mathbf{q}} u_j & {\mathbf{0}} \end{bmatrix}_{\mathbf{x}=\mathbf{x}^*,\mathbf{u}=\mathbf{u}^*} \\ \mathbf{B}_{lin} =& \begin{bmatrix} {\mathbf{0}} \\ \mathbf{M}^{-1} \mathbf{B} \end{bmatrix}_{\mathbf{x}=\mathbf{x}^*, \mathbf{u}=\mathbf{u}^*} \end{matrix}$

其中 $\mathbf{B}_j$ 是 $\mathbf{B}$ 的第 j 列。注意，涉及 $\dfrac{\partial\mathbf{M}^{-1}}{\partial q_i}$ 的项消失了，因为 $\tau_g+\mathbf{Bu}-\mathbf{C\dot{q}}$ 在定点处必须为零。所有的 $C\dot{q}$ 导数也会因为 $\dot{\mathbf{q}}^*=0,$ 而消失，任何涉及 $\mathbf{C}$ 的项也会消失，因为当速度为零时，向心力和离心力都为零。在许多情况下，包括杂技机器人和车极系统（但不包括四旋翼机器人），矩阵 $\mathbf{B}(\mathbf{q})$ 是一个常数，因此 $\dfrac{\partial\mathbf{B}}{\partial\mathbf{q}}$ 项也会消失。

例 3.3 杂技机器人的线性化

围绕（不稳定的）直立点进行线性化，我们可以得到

$\begin{bmatrix}\dfrac{\partial\tau_{\mathbf{g}}}{\partial\mathbf{q}}\end{bmatrix}_{\mathbf{x}=\mathbf{x}^*}=\begin{bmatrix}g(m_1l_{c1}+m_2l_1+m_2l_{c2})&m_2gl_{c2}\\m_2gl_{c2}&m_2gl_{c2}\end{bmatrix}$

线性动力学可直接从这些方程和机械手形式的杂技机器人方程中得出。

例 3.4 小车摆杆系统的线性化
围绕该系统的不稳定定点进行线性化，我们可以得到

$\left[\dfrac{\partial\tau_g}{\partial\mathbf{q}}\right]_{\mathbf{x}=\mathbf{x}^*}=\begin{bmatrix}0&0\\0&m_pgl\end{bmatrix}$

同样，线性动力学也很简单。

研究线性化系统的特性可以让我们了解非线性系统的（局部）特性。例如，严格稳定的线性化意味着非线性系统的局部指数稳定性[3]（定理 4.15）。值得注意的是，线性化不稳定也意味着系统局部不稳定，但如果线性化略微稳定，则无法断定非线性系统是渐近稳定、i.s.L.稳定还是不稳定（只能断定它不是指数稳定）[4]。

4.2 线性系统的可控性

定义 3.1 可控性形式如下的控制系统

$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$

如果可以构建一个无约束输入信号 $\mathbf{u}(t)$ , $t\in[0,t_f]$ ，使系统在有限的时间间隔 $0<t<t_f$ 内从任意初始状态移动到任意最终状态，则称为可控 [5]。

对于线性系统

$\dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u},$

我们可以用封闭形式对这个线性系统进行积分，因此可以得出可控性的确切条件。特别是，对于线性系统来说，只需证明存在一个能将任何初始条件驱动到原点的控制输入即可。

4.2.1 非重复特征值的特殊情况

让我们先研究一个特例，它虽然不能作为通用工具，但对于理解可控性的直觉可能更有用。让我们对系统矩阵 $\mathbf{A}$ 进行特征值分析，即

$\mathbf{A}\mathbf{v}_i=\lambda_i\mathbf{v}_i,$

其中 $\lambda _i$ 是第 i 个特征值， $\mathbf{v}_{i}$ 是相应的（右）特征向量。 $n\times n$ 矩阵 $\mathbf{A}$ 将有 n 个特征值。将（列）特征向量集合到矩阵 $\mathbf{V}$ 中，并将特征值集合到对角矩阵 $\mathbf{\Lambda}$ 中，我们可以得到

$\mathbf{A}\mathbf{V}=\mathbf{V}\boldsymbol{\Lambda}.$

这就是我们的主要假设：假设每个特征值都是一个不同的值（没有重复）。有了这个假设，就可以证明特征向量 $\mathbf{v}_{i}$ 构成了一个线性独立的基集，因此 $\mathbf{V}^{-1}$ 是定义明确的。

通过定义模态坐标 $\mathbf{r}$ ，我们可以继续对线性系统进行特征模态分析：

$\mathbf{x}=\mathbf{V}\mathbf{r},\quad\mathrm{or}\quad\mathbf{r}=\mathbf{V}^{-1}\mathbf{x}.$

在模态坐标中，线性系统的动力学方程为

$\dot{\mathbf{r}}=\mathbf{V}^{-1}\mathbf{A}\mathbf{V}\mathbf{r}+\mathbf{V}^{-1}\mathbf{B}\mathbf{u}=\mathbf{\Lambda}\mathbf{r}+\mathbf{V}^{-1}\mathbf{B}\mathbf{u}.$

这说明了模态分析的威力；在模态坐标中，动力学对角线产生了独立的线性方程：

$\dot{r}_i=\lambda_ir_i+\sum_j\beta_{ij}u_j,\quad\beta=\mathbf{V}^{-1}\mathbf{B}.$

现在，可控性的概念变得清晰了。输入 j 可以影响模态坐标 i 中的动力学，当且仅当 $\beta_{ij}\neq0.$ . 在特征值不重复的特殊情况下，控制每个单独的特征模态足以（在有限的时间内）调节所有的特征模态[5]。因此，我们说系统是可控的，当且仅当

$\forall i,\exists j\text{ such that }\beta_{ij}\neq0.$

4.2.2 可控性与欠驱动

对杂技机器人和车柱系统的可控性分析表明，围绕直立方向的线性化动力学实际上是可控的。这意味着线性化系统如果从零状态开始，可以在有限时间内回到零状态。这有可能令人吃惊--毕竟系统是欠驱动的。例如，杂技机器人在没有肩部电机的情况下也能在直立状态下保持平衡，这既有趣又令人惊讶。

这些模型系统的可控性说明了一个极其重要的问题：欠驱动系统并不一定是不可控系统。欠驱动系统无法遵循任意轨迹，但这并不意味着它们无法到达状态空间中的任意点。然而，将系统置于特定状态所需的轨迹可能是任意复杂的。

这里介绍的可控性分析是针对线性时不变（LTI）系统的。对于线性时变（LTV）系统也有类似的分析。我们甚至还将看到对非线性系统的扩展；不过，我们通常会用 "可达性 "分析这个同义词来称呼它。

4.2.3 LQR 反馈

可控性告诉我们存在通往定点的轨迹，但并没有告诉我们应该选择哪条轨迹，也没有告诉我们什么样的控制输入会导致轨迹出现。为什么不能呢？可能有无限多的解决方案。我们必须选择一个。

线性系统的控制器设计工具非常先进。特别是，正如我们在线性最优控制一章中所描述的，只要我们愿意围绕工作点对系统进行线性化，并用二次成本函数来定义最优性，就可以很容易地为平衡等调节任务设计出最优反馈控制器：

$J(\mathbf{x}_0)=\int_0^\infty\left[\mathbf{x}^T(t)\mathbf{Q}\mathbf{x}(t)+\mathbf{u}^T(t)\mathbf{R}\mathbf{u}(t)\right]dt,\quad\mathbf{x}(0)=\mathbf{x}_0,\mathbf{Q}=\mathbf{Q}^T>0,\mathbf{R}=\mathbf{R}^T>0.$

线性反馈矩阵 K 用作

$\mathbf{u}(t)=-\mathbf{K}\mathbf{x}(t),$

就是所谓的最优线性二次调节器（LQR）。即使不了解详细的推导过程，我们也能很快成为 LQR 的实践者。Drake 提供了一个函数

K = LinearQuadraticRegulator(A, B, Q, R)

线性系统。它还提供了一个版本

controller = LinearQuadraticRegulator(system, context, Q, R)

将围绕平衡点对系统进行线性化，并返回控制器（原始坐标）。因此，要使用 LQR，只需定义对称的正有限代价矩阵 $\mathbf{Q}$ 和 $\mathbf{R}$ 即可。在最常见的形式中， $\mathbf{Q}$ 和 $\mathbf{R}$ 是正对角矩阵，其中项 $Q_{ii}$ 惩罚状态变量 $x_i$ 与其他状态变量相比的相对误差，项 $R_{ii}$ 惩罚 $u_i$ 中的动作。

例 3.5 用于 Acrobot 和 Cart-Pole 的 LQR

用一分钟时间玩一下 Acrobot 和 Cart-Pole 的 LQR 控制器

请务必花一分钟时间查看这些示例中运行的代码。能否以不同的方式设置 $\mathbf{Q}$ 和 $\mathbf{R}$ 矩阵，以提高性能？

利用 LQR 反馈对闭环响应进行的仿真显示，任务确实完成了，而且完成得令人印象深刻。为了最终到达原点，系统的状态经常要剧烈偏离原点。进一步检查发现，（线性化的）闭环动力学实际上是非最小相位的（杂技机器人有 3 个右半零点，车杆有 1 个零点）。

例 3.6 用于四旋翼飞行器的 LQR

如果围绕一个标称的固定点（螺旋桨的非零推力与重力平衡的位置）进行线性化，那么 LQR 对于四旋翼飞行器来说基本上是开箱即用。

请注意，LQR 虽然是线性化系统的最优解，但并不一定是最大化定点吸引盆的最佳线性控制解。鲁棒控制理论（如 [8]）明确考虑了线性化模型和非线性模型之间的差异，在这方面会产生优于 LQR 解决方案的控制器。

五、部分反馈线性化

虽然我们不能总是简化系统的全部动力学，但仍有可能将系统动力学的一部分线性化。这种技术称为部分反馈线性化。

请看小车-杆的例子。小车的动力学受摆的运动影响。如果我们知道这个模型，那么我们就可以创建一个反馈控制器，它将以必要的方式推动小车，以抵消摆的动力学贡献，从而实现小车动态的线性化。我们将看到的是，我们也可以使用小车的反馈控制器来反馈线性化被动摆关节的动力学，这可能会更令人惊讶。

我们将使用 "协同部分反馈线性化 "这一术语来描述一种控制器，它能使被驱动关节的动态线性化。更令人惊讶的是，非共轭部分反馈线性化通常是可以实现的，即对未驱动关节的动力学进行线性化的控制器。这里介绍的处理方法源自 [9]。

5.1 车杆系统的部分反馈线性化

同轴
从方程 18 和 19 出发，我们可以得到

$\begin{gathered}\ddot{\theta}=-\ddot{x}c-s\\\ddot{x}(2-c^2)-sc-\dot{\theta}^2s=f_x\end{gathered}$

因此，应用反馈控制

$f_x=(2-c^2)\ddot{x}^d-sc-\dot{\theta}^2s$

结果是

$\begin{aligned}\ddot{\boldsymbol{x}}=&\ddot{\boldsymbol{x}}^d\\\ddot{\boldsymbol{\theta}}=&-\ddot{\boldsymbol{x}}^dc-s,\end{aligned}$

是全局有效的。

请仔细观察得出的方程。当然，我们可以在小车上施加任何我们喜欢的加速度。但是，即使是由此得出的极点方程，在这里也恰好采用了一种很好的形式：它们被简化为单摆（没有小车）的方程，其中的扭矩输入现在改由 $\ddot{x}c$ 给出。这就好比我们有一个带扭矩控制的单摆，只不过我们的指令是由 $\cos \theta$ 项调制的，而这个 $\cos \theta$ 项是基本的--当杆处于水平状态时，我们的控制权确实为零，因为在这种配置下，小车上的任何力都不能像杆上的扭矩一样发挥作用。

非集中式
从方程 18 和 19 出发，我们得出

$\begin{gathered}\ddot{\boldsymbol{x}}=-\frac{\ddot{\boldsymbol{\theta}}+s}c\\\ddot{\theta}(c-\frac2c)-2\tan\theta-\dot{\theta}^2s=f_x\end{gathered}$

应用反馈控制

$f_x=(c-\frac2c)\ddot{\theta}^d-2\tan\theta-\dot{\theta}^2s$

结果是

$\begin{aligned}&\ddot{\theta}=\ddot{\theta}^d\\&\ddot{\boldsymbol{x}}=-\frac1c\ddot{\boldsymbol{\theta}}^d-\tan\theta.\end{aligned}$

请注意，该表达式仅在 $\cos\theta\neq0.$ 时有效。我们再次知道，当磁极完全水平时，力无法产生扭矩。事实上，我们编写的控制器会 "爆炸"--要求在 $\cos\theta=0$ 处产生无穷大的力；因此，在硬件上（甚至在模拟中）执行命令之前，请确保其饱和。尽管这可能只是一个小小的安慰，但至少我们知道 $(c-\frac2c)$ 不会归零；事实上，你可以自己检查 $|c-\frac2c|\geq1.$ 是否归零。

5.2 一般形式

对于微不足道的欠驱动系统（某些关节上有扭矩，其他关节上没有扭矩），我们可以在不失一般性的前提下，将操纵器方程所描述的任何欠驱动系统的关节坐标重组为以下形式：

$\begin{aligned}&\mathbf{M}_{11}\ddot{\mathbf{q}}_1+\mathbf{M}_{12}\ddot{\mathbf{q}}_2=\tau_1,\\&\mathbf{M}_{21}\ddot{\mathbf{q}}_1+\mathbf{M}_{22}\ddot{\mathbf{q}}_2=\tau_2+\mathbf{u},\end{aligned}$

其中， $\mathbf{q}\in\mathbb{R}^n,\mathbf{q}_1\in\mathbb{R}^l,\mathbf{q}_2\in\mathbb{R}^m,l=n-m.$ $\mathbf{q}_1$ 代表所有被动关节， $\mathbf{q}_2$ 代表所有主动关节， $\tau=\tau_g-\mathbf{C\dot{q}}$ 项包含所有科里奥利和重力项，以及

$\mathbf{M}(\mathbf{q})=\begin{bmatrix}\mathbf{M}_{11}&\mathbf{M}_{12}\\\mathbf{M}_{21}&\mathbf{M}_{22}\end{bmatrix}.$

幸运的是，由于 $\mathbf{M}$ 是均匀正定的（例如对于所有 $\mathbf{q}$ ），因此根据正定性的舒尔补码条件， $\mathbf{M}_{11}$ 和 $\mathbf{M}_{22}$ 也是正定的。

5.2.1 协同线性化

用同样的方法代入完整的机械手方程，我们可以得到

$\ddot{\mathbf{q}}_1=\mathbf{M}_{11}^{-1}\left[\tau_1-\mathbf{M}_{12}\ddot{\mathbf{q}}_2\right]$

$(\mathbf{M}_{22}-\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\mathbf{M}_{12})\ddot{\mathbf{q}}_2-\tau_2+\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\tau_1=\mathbf{u}$

可以很容易地证明矩阵 $(\mathbf{M}_{22}-\mathbf{M}_{21}\mathbf{M}_{11}^{-1}\mathbf{M}_{12})$ 是可逆的[9]；通过观察我们可以发现它是对称的。PFL 自然随之而来，并且在全局范围内有效。

5.2.2 非共线线性化

$\ddot{\mathbf{q}}_2=\mathbf{M}_{12}^+\left[\tau_1-\mathbf{M}_{11}\ddot{\mathbf{q}}_1\right]$

$(\mathbf{M}_{21}-\mathbf{M}_{22}\mathbf{M}_{12}^+\mathbf{M}_{11})\ddot{\mathbf{q}}_1-\tau_2+\mathbf{M}_{22}\mathbf{M}_{12}^+\tau_1=\mathbf{u}$

其中 $\mathbf{M}_{12}^+$ 是摩尔-彭罗斯伪逆。当 $\mathbf{M}_{12}$ 的秩等于 $l$ ，即系统中被动自由度的数量（不可能更多，因为矩阵只有 $l$ 行）时，该逆变换提供了唯一解。它与状态有关；在上面的小车摆杆示例中，当 $\cos\theta=0$ 时， $\mathbf{M}_{12}=\cos\theta$ 的秩正好下降。在这种情况下，秩条件有时被称为 "强惯性耦合 "特性。如果一个系统在每个状态下都表现出强惯性耦合，那么它就具有全局强惯性耦合。

5.2.3 任务空间部分反馈线性化

一般来说，我们可以定义想要控制的主动和被动关节的某种组合。这种组合有时被称为 "任务空间"。考虑以下形式的输出函数

$\mathbf{y}=\mathbf{h}(\mathbf{q}),$

$\mathbf{y}\in\mathbb{R}^p$ 定义了任务空间。定义 $\mathbf{H}_1=\frac{\partial\mathbf{h}}{\partial\mathbf{q}_1}\mathbf{~H}_2=\frac{\partial\mathbf{h}}{\partial\mathbf{q}_2},\mathbf{H}=[\mathbf{H}_1,\mathbf{H}_2].$

定理 3.1 任务空间 PFL（部分反馈线性化）

如果对驱动关节发出指令，使其

$\ddot{\mathbf{q}}_2=\bar{\mathbf{H}}^+\left[\ddot{\mathbf{y}}^d-\dot{\mathbf{H}}\dot{\mathbf{q}}-\mathbf{H}_1\mathbf{M}_{11}^{-1}\tau_1\right],$

其中 $\bar{\mathbf{H}}=\mathbf{H}_2-\mathbf{H}_1\mathbf{M}_{11}^{-1}\mathbf{M}_{12}$ ， $\bar{\mathbf{H}}^+$ 是右摩尔-彭罗斯伪逆，

$\bar{\mathbf{H}}^+=\bar{\mathbf{H}}^T(\bar{\mathbf{H}}\bar{\mathbf{H}}^T)^{-1},$

则

$\ddot{\mathbf{y}}=\ddot{\mathbf{y}}^d.$

须符合

$\operatorname{rank}\left(\bar{\mathbf{H}}\right)=p,$

为了执行任务空间轨迹，可以命令

$\ddot{\mathbf{y}}^d=\ddot{\mathbf{y}}^d+\mathbf{K}_d(\dot{\mathbf{y}}^d-\dot{\mathbf{y}})+\mathbf{K}_p(\mathbf{y}^d-\mathbf{y}).$

假设内部动力学是稳定的，则当 $\mathbf{K}_p,\mathbf{K}_d>0$ [4] 时会产生收敛误差动力学，这意味着 $y(t)\to y^d(t)$ . 对于位置控制机器人，(38) 中的加速度指令就足够了。或者，将 (38) 和 (41) 代入 (33) 即可得到扭矩指令。

例 3.7 用车-杆系统跟踪端点轨迹

考虑在车杆系统中尝试跟踪摆杆端点的理想运动轨迹。在只有一个驱动器和运动学约束的情况下，我们可能很难在水平坐标和垂直坐标上跟踪轨迹。但我们至少可以尝试追踪末端执行器垂直位置的轨迹。

利用任务空间 PFL 推导，我们可以得到

$\begin{gathered}y=h(\mathbf{q})=-l\cos\theta\\\dot{y}=l\dot{\theta}\sin\theta\end{gathered}$

如果我们定义一条理想的轨迹：

$y^d(t)=\dfrac l2+\dfrac l4\mathrm{sin}(t),$

那么任务空间控制器就可以通过上述推导轻松实现。

上述任务空间推导为部分反馈线性化（PFL）[9] 提供了一个方便的概括，它包含了同位和非同位结果。如果我们选择 $\mathbf{y}=\mathbf{q}_2$ （同位），则有

$\mathbf{H}_1=0,\mathbf{H}_2=\mathbf{I},\dot{\mathbf{H}}=0,\bar{\mathbf{H}}=\mathbf{I},\bar{\mathbf{H}}^+=\mathbf{I}.$

由此，（38）中的指令变为 $\ddot{\mathbf{q}}_2=\ddot{\mathbf{q}}_2^d$ 。
因此，执行器指令为

$\mathbf{u}=\mathbf{M}_{21}\mathbf{M}_{11}^{-1}(\tau_1-\mathbf{M}_{12}\ddot{\mathbf{q}}_2^d)+\mathbf{M}_{22}\ddot{\mathbf{q}}_2^d-\tau_2,$

且始终满足秩条件 (40)。

如果我们选择 $\mathbf{y}=\mathbf{q}_1$ （非共轭），则有

$\mathbf{H}_1=\mathbf{I},\mathbf{H}_2=0,\dot{\mathbf{H}}=0,\bar{\mathbf{H}}=-\mathbf{M}_{11}^{-1}\mathbf{M}_{12}.$

秩条件（40）要求 $(\mathbf{M}_{12})=l$ ，在这种情况下，我们可以写成 $\mathbf{\bar{H}}^+=-\mathbf{M}_{12}^+\mathbf{M}_{11}$ ，将秩条件精确地简化为[9]中描述的 "强惯性耦合 "条件。现在，（38）中的指令简化为

$\ddot{\mathbf{q}}_2=\mathbf{M}_{12}^+\left[\tau_1-\mathbf{M}_{11}\ddot{\mathbf{q}}_1^d\right]$

将 (45) 代入 (33)，即可求出执行器指令，结果与 [9] 相同。

六、上摆控制

6.1 能量整形

回顾上一章，我们使用能量整形来摆动单摆。事实证明，这个想法比单摆更普遍。我们将看到，我们可以使用类似的 "能量整形 "概念，为杂技演员和车杆系统制作摆动控制器。值得注意的是，只需一个执行器就能改变系统的总能量。

尽管针对这些模型系统已经提出了多种摆动控制器（参见 [10]、[11]、[12]、[13]、[14]、[15]、[1]、[16]），但能量整形控制器往往是最自然的推导方法，也许也是最著名的方法。

6.2 小车摆杆

让我们尝试将能量整形思想应用于车杆系统。基本思路来自文献 [17]，即使用同位 PFL 来简化动力学，使用能量整形来调节摆的同位轨道，然后添加几个项来确保小车保持在原点附近。这有点出人意料......如果我们想控制摆，难道不应该使用非定位版本吗？实际上，我们最终既要控制小车，又要控制极点，我们将以同轴版本为基础，因为它避免了可能归零的 x 项的倒置，而且（当所有参数都设为 1 时）我们还剩下一个特别简单的方程形式：

$\begin{gathered}\ddot{x}=u\\\ddot{\theta}=-uc-s.-\frac1l(uc+gs)\end{gathered}$

第一个等式显然很简单，但即使是第二个等式，也完全是去耦摆的等式，只是控制输入稍微有点奇怪，是由 $\cos\theta$ 调制的。

让我们用能量整形来调节这个去耦摆的能量。摆（单位质量、单位长度、单位重力下的单摆）的能量由以下公式给出：

$E(\mathbf{x})=\dfrac12\dot{\theta}^2-\cos\theta.$

所需的能量相当于所需的定点能量，即

$E^d=1.$

再次定义 $\tilde{E}(\mathbf{x})=E(\mathbf{x})-E^d$ ，我们现在观察到

$\begin{aligned} \dot{\tilde{E}}(\mathbf{x})=& \dot{E}(\mathbf{x})=\dot{\theta}\ddot{\theta}+\dot{\theta}s \\ =& \dot{\theta}[-uc-s]+\dot{\theta}s \\ \text{=}& -u\dot{\theta}\cos\theta. \end{aligned}$

因此，如果我们设计一个形式如下的控制器

$u=k\dot{\theta}\cos\theta\tilde{E},\quad k>0$

结果是

$\dot{\tilde{E}}=-k\dot{\theta}^2\cos^2\theta\tilde{E}.$

这保证了 $|\tilde{E}|$ 不会递增，但还不足以保证 $|\tilde{E}|$ 会归零。例如，如果 $\theta=\dot{\theta}=0$ ，系统将永远不会移动。但是，如果我们有

$\int_0^t\dot{\theta}^2(t^{\prime})\cos^2\theta(t^{\prime})dt^{\prime}\to\infty,\quad\mathrm{~as~}t\to\infty,$

则有 $\tilde{E}(t)\to0$ 。
这个条件是拉萨尔定理的一个版本，我们将在关于李雅普诺夫方法的注释中加以阐述。除了定点处的琐碎恒定轨迹外，其他轨迹都满足这个条件。

现在我们回到整个系统动力学（包括小车）。文献[17]和[18]使用了简单的摆能控制器，并增加了一个 PD 控制器来调节小车：

$\ddot{x}^d=k_E\dot{\theta}\cos\theta\tilde{E}-k_px-k_d\dot{x}.$

文献[17]提供了该控制器在某些名义参数下的收敛性证明。[10] 则提供了一种略有不同的控制器，直接从 Lyapunov 论证中推导出来。

小车-摆杆上升：使用能量整形控制的摆锤子系统相位图示例。控制器驱动系统进入同轴轨道，然后在接近顶端时切换到 LQR 平衡控制器。

6.3 杂技演员

杂技演员的摇摆控制可以通过大致相同的方式实现。[13] - 泵能。简洁明了。没有证明。略有改动的版本（用 arctan 代替 sat）见 [19]。最清晰的表述见 [18]。

使用同位 PFL。 $(\ddot{q}_2=\ddot{q}_2^d).$

$\begin{gathered} E(\mathbf{x})=\frac12\dot{\mathbf{q}}^T\mathbf{M}\dot{\mathbf{q}}+U(\mathbf{x}). \\ E_d=U(\mathbf{x}^*). \\ \bar{\boldsymbol{u}}=\dot{q}_1\tilde{E}. \\ \ddot{q}_2^d=-k_1q_2-k_2\dot{q}_2+k_3\bar{u}, \end{gathered}$

额外的 PD 项阻碍了对渐近收敛至同次轨道的证明。另一个基于能量的控制器的证明见 [12]。

6.4 讨论

本文介绍的上摆动能量整形控制器是非线性欠动控制领域的忠实代表。通常，这些控制推导需要一些巧妙的技巧来简化或抵消非线性方程中的项，然后用一些巧妙的 Lyapunov 函数来证明稳定性。在这些情况下，PFL 被用来简化方程，从而简化控制器的设计。

当我们研究轨迹优化的 "微分平坦性 "时，将看到另一个改变非线性方程坐标以简化问题的好例子。如今，这种方法在四旋翼飞行器的动态轨迹规划中最为著名。

这些控制器非常重要，具有代表性和相关性。但是，利用非线性方程的巧妙技巧似乎从根本上受到了限制。本书其余大部分内容都将强调制定和解决这些及其他控制问题的通用计算方法。

七、其他模型系统

杂技机器人和小车-摆杆系统只是欠动控制研究中大量使用的两个模型系统。其他例子包括

惯性轮摆
惯性轮摆
古田摆（水平旋转和垂直摆）
气垫船

八、练习

练习 3.1 (小车-摆杆：线性化和平衡）

在本练习中，您将只在本笔记本上进行练习。您需要完成以下步骤。

推导小车-摆杆系统的状态空间动力学 $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$ 。
从 (a) 点开始，围绕不稳定平衡点进行线性化。
分析不同状态值 $\mathbf{x}$ 下的线性化误差和控制 $\mathbf{u}$ .
在 (c) 点的状态中，确定哪些状态可通过 LQR 控制器稳定到原点。
练习 3.2 (小车-摆杆：编写 URDF 并用 LQR 平衡）
本练习将在本笔记本中进行。您将完成以下步骤。

构建单摆小车-摆杆 URDF 结构。
通过修改机器人的 URDF 模型并测试 LQR 的控制能力，将单摆小车-摆杆扩展为双摆小车-摆杆。
练习 3.3 (离散和连续 LTI 系统的可控性）
(带有离散双积分器的二维网格）考虑一个离散网格世界，其中

$\mathbf{x}=\begin{bmatrix}x_1\\x_2\end{bmatrix},\mathrm{~and~}x_1[n],x_2[n]\in\mathbb{Z}.$

x 的动力学是离散时间版的双积分器、

kuan_li_lyg

关注

4
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
Underactuated Robotics - 欠驱动机器人学（三）- 体操机器人、小车摆杆和四旋翼飞行器

在低维模型系统的背景下，已经开展了大量有关欠驱动系统控制的工作。这些模型系统捕捉到了问题的本质，却没有引入更真实世界的例子中经常涉及的所有复杂性。在本章中，我们将重点讨论两个最著名、研究最深入的模型系统--Acrobot 和 Cart-Pole。在我们开发了一些工具后，我们将看到这些工具可以直接应用于其他模型系统；我们将使用四旋翼机器人举出一些例子。所有这些系统都是微不足道的欠驱动系统，执行器数量少于自由度。
复制链接

扫一扫