四旋翼抗扰控制¶

2025.4.18 周总结报告

本周按照上周计划，详细阅读了徐师兄和胡师兄仿真节点控制算法所参考的论文，笔记如下：

阅读的论文是【Agile Flight Control Under Multiple Disturbances for Quadrotor: Algorithms and Evaluation】。

这篇论文的主要贡献是针对四旋翼运动时的风扰和重心变化（例如四旋翼带着机械臂，机械臂运动引起整体重心位置的变化），提出了无需新增传感器的扰动力/力矩观测器，并基于扰动观测值，用反步法设计了姿态环控制器。

这篇写得很细，公式推导有很多需要注意的细节。

1 参数约定与相关模型¶

定义机体坐标系 $\mathcal{F}_{\mathcal{B}} = \begin{bmatrix}\mathbf{b}_1 & \mathbf{b}_2 & \mathbf{b}_3\end{bmatrix}$ 和世界坐标系 $\mathcal{F}_{\mathcal{W}} = \begin{bmatrix}\mathbf{e}_1 & \mathbf{e}_2 & \mathbf{e}_3\end{bmatrix}$ ，建系方式为 ENU 东北天坐标系。用四元数 $(q_0, \mathbf{q})$ 表示机体坐标系相对于世界坐标系的姿态；用 $\boldsymbol{\omega}$ 表示四旋翼在机体坐标系下的三轴角速度；用 $f$ 表示四旋翼实际推力，沿 $\mathbf{b}_3$ 轴；用 $\mathbf{F}$ 表示控制器在世界坐标系下给出的期望推力。用 $\otimes$ 表示四元数乘法；用下标 d 代表期望值；用 $\{\cdot\}^\wedge$ 代表取三维向量的反对称矩阵。

四旋翼的姿态和角速度的关系可以用四元数定义：

\[\dot{\mathbf{q}} = \frac{1}{2}\left(\mathbf{q}^{\wedge}+q_0 \mathbf{I}_{3\times3}\right)\ \boldsymbol\omega, \ \ \dot{q}_0 = -\frac{1}{2}\mathbf{q}^{\top}\boldsymbol\omega\tag{1-1}\]

根据牛顿-欧拉定理，得到四旋翼的动力学模型：

\[\begin{align} m\mathbf{a} &= f\mathbf{b}_3-mg\mathbf{e}_3+\mathbf{d}_w\tag{1-2a}\\ \mathbf{J}\dot{\boldsymbol\omega} &= -\boldsymbol\omega\times \mathbf{J}\boldsymbol\omega + \boldsymbol\tau + \mathbf{d}_\tau \tag{1-2b} \end{align}\]

论文简单描述了螺旋桨电机电压和升力的关系：

\[u_i = c_m\sqrt{f_i-f_b}-u_b\]

其中， $u_i$ 是螺旋桨电机电压， $f_i$ 是该螺旋桨提供的升力， $f_b, u_b$ 是偏置项， $c_m$ 是比例系数。

2 气动阻力/力矩模型与推力响应模型¶

在高机动跟踪工况下，有必要将风阻纳入控制器加以补偿。我们定义风在机体坐标系下的速度为 $\mathbf{v}_w = \mathbf{v}_a - \mathbf{v} \in \mathbb{R}^3$ ，其中 $\mathbf{v}_a$ 是风在世界坐标系下的速度。参考前人研究，三轴气动阻力具有如下形式：

\[\mathbf{d}_w = \mu_1\mathbf{v}_w + \mu_2\mathbf{v}_w\left\|\mathbf{v}_w\right\| \tag{2-1}\]

其中的 $\mu_1, \mu_2 \in \mathbb{R}^{3\times 3}$ 是相关系数。模型关键是 $\mu_1,\mu_2$ 的设计。前人研究中将 $\mu_1,\mu_2$ 设为固定值，忽视了四旋翼在飞行过程中姿态改变，迎风面积变化的影响。本文作者进了一步，将机体简化为一个垂直于 $\mathbf{b}_3$ 的平面，将平面在世界坐标系三轴的投影面积作为迎风面积纳入考量。

容易想到，可以用 $\mathbf{b}_3$ 与世界坐标系三轴夹角的余弦值，表示投影面积比例。如下定义三个角度值

\[\begin{cases} \gamma_1 = \arccos(\mathbf{b}_3 \cdot \mathbf{e}_1)\\ \gamma_2 = \arccos(\mathbf{b}_3 \cdot \mathbf{e}_2)\\ \gamma_3 = \arccos(\mathbf{b_3}\cdot \mathbf{e}_3) \end{cases}\tag{2-2}\]

相应地，如下设计 $\mu_1,\mu_2$ ：

\[\begin{align}\mu_1 & = k_1\begin{bmatrix}\cos|\gamma_1|\\ &\cos|\gamma_2|\\&&\cos|\gamma_3|\end{bmatrix}:=k_1\Gamma_1\tag{2-3a}\\ \mu_2&= k_2\begin{bmatrix}\cos|\gamma_1|\\&\cos|\gamma_2|\\ & & \cos|\gamma_3|\end{bmatrix}:= k_2\Gamma_2 \tag{2-3b} \end{align}\]

$k_1,k_2$ 与四旋翼 Z 向机体表面积正相关。继而，我们可以给出 $\mathbf{d}_w$ 的表达式：

\[\begin{aligned} \mathbf{d}_w &= \begin{bmatrix}\Gamma_1 & \Gamma_2\end{bmatrix}\begin{bmatrix}k_1\mathbf{v}_w\\k_2\left\|\mathbf{v}_w\right\|\mathbf{v}_w\end{bmatrix}\\ & := S\xi \end{aligned}\tag{2-4}\]

我们会在下文的观测器中讨论如何观测状态量 $\xi$ 。

【讨论】：有几个问题。首先，如何测定 $k_i$ 及 $b_{n_1n_2}$ ？个人初步判断， $b_{n_1n_2} \approx 0$ ， $k_i$ 是四旋翼 Z 向机体表面积的多项式，具体阶数需要参考空气动力学相关知识。其次，论文忽略了四旋翼 X, Y 向的机体表面积，适用程度不够广泛。

论文将扰动力矩 $\mathbf{d}_{\tau}$ 分成气动阻力矩 $\mathbf{d}_{\tau w}$ 和重心偏移引起的扰动力矩 $\mathbf{d}_{\tau s}$ ： $\mathbf{d}_\tau = \mathbf{d}_{\tau w} + \mathbf{d}_{\tau s}$ 。工作过程中，机体机械结构的改变引起重心位置偏移，导致惯性张量 $\mathbf{J}$ 发生变化： $\mathbf{J} = \mathbf{J}_0 + \Delta\mathbf{J}(t)$ ，进而有扰动力矩：

\[\mathbf{d}_{\tau s} = -\boldsymbol\omega \times \Delta\mathbf{J}\boldsymbol\omega - \Delta \mathbf{J} \dot{\boldsymbol\omega} \tag{2-5}\]

【讨论】：对于机械结构不发生变化的机体， $\Delta\mathbf{J}$ 可以作为惯性张量的建模误差引入，同样具有价值。

论文假设相对空速和扰动力矩不会发生突变，并假设了两个未知上限：

\[\left\|\dot{\xi}\right\| \le \gamma_{w} \ \ \ \ \ \left\|\dot{\mathbf{d}}_{\tau}\right\|\le \gamma_\tau \tag{2-6}\]

结合（2-4）式，假设相对空速导数的上限，相当于假设风扰阻力导数的上限。由于四旋翼具有惯性，面对无序风扰时表现出低通滤波的性质，因此认为风扰阻力/力矩不突变、导数有界是合理的。

高机动工况下，我们希望系统能响应时间尽可能短，因此我们必须考虑系统的响应模型。常见的方式是用一阶惯性环节近似电机转速响应模型，即 $\omega_r = \frac{1}{T_ms+1}\omega_c$ ，其中 $\omega_c$ 是控制器输出期望转速， $\omega_r$ 是电机实际转速， $T_m$ 是响应时间。

更进一步，将控制分配部分、四旋翼动力学部分也纳入响应模型，用二阶惯性模型近似推力/力矩响应：

\[\boldsymbol\tau_r = \left(\frac{1}{T_ms+1}\right)^2\boldsymbol\tau_c \tag{2-7}\]

由于惯性的存在，若要完美补偿扰动阻力/力矩，实际用于补偿的推力/力矩应当大于阻力/力矩。

3 位置环¶

位置环使用经典的 PD + 前馈的控制方案：

\[\mathbf{F} = \mathbf{k}_p\mathbf{e}_p + \mathbf{k}_v\mathbf{e}_v+m\mathbf{a}_{d}+mg\mathbf{e}_3-\hat{\mathbf{d}}_w \tag{3-1}\]

其中， $\mathbf{e}_p = \mathbf{r}_d-\mathbf{r},\ \mathbf{e}_v = \mathbf{v}_d-\mathbf{v}$ ， $\hat{\mathbf{d}}_w$ 是风扰观测器给出的估计值， $\mathbf{k}_p, \mathbf{k}_v$ 都是 $3\times3$ 的对角阵。

已知期望轨迹和期望推力，我们就能求解期望姿态（四元数）、角速度和角加速度：

$$

\[\begin{align} &\begin{bmatrix}q_{d0}\\\mathbf{q}_d\end{bmatrix} = \frac{1}{\sqrt{2\left(1+\bar{\mathbf{F}}_b^\top\bar{\mathbf{F}}\right)}}\begin{bmatrix}1+\bar{\mathbf{F}}_b^\top\bar{\mathbf{F}}\\ \bar{\mathbf{F}}_b^\wedge\bar{\mathbf{F}}\end{bmatrix} \otimes \begin{bmatrix}\cos(\psi_d/2)\\ \begin{pmatrix}0\\0\\\sin(\psi_d/2)\end{pmatrix}\end{bmatrix} \tag{3-2}\\ &^\mathcal{W}\boldsymbol{\omega}_{dxy} = \bar{\mathbf{F}}^{\wedge}\dot{\bar{\mathbf{F}}},\ \ \boldsymbol{\omega}_{dxy} = \left(^{\mathcal{B}}\mathrm{R}_{\mathcal{W}}\right)\left(^\mathcal{W}\boldsymbol\omega_{dxy}\right),\ \ \omega_{dz} = \dot{\psi}_d \tag{3-3} \end{align}\]

$$

其中， $^{\mathcal{B}}\mathrm{R}_{\mathcal{W}}$ 是当前世界坐标系到期望机体坐标系的旋转矩阵， $\bar{\mathbf{F}}$ 是推力 $\mathbf{F}$ 的归一化， $\bar{\mathbf{F}}_b$ 是 $\bar{\mathbf{F}}$ 转到机体坐标系下的表示， $\boldsymbol\omega_{dxy}$ 代表期望角速度在机体坐标系 x-y 平面的投影， $\omega_{dz}$ 代表期望角速度的 z 轴分量。该结论来自 MIT 的【TODO：补充论文链接】。

对（3-3）式求导，有

\[\begin{align} \dot{\omega}_{dz} &=\ddot{\psi}_d \tag{3-4}\\ \frac{\mathrm{d}}{\mathrm{d}t}\left(\left(^\mathcal{W}R_{\mathcal{B}}\right)\boldsymbol\omega_{dxy}\right) &= \frac{\mathrm{d}}{\mathrm{d}t}(\bar{\mathbf{F}}\times\dot{\bar{\mathbf{F}}})\\ \left(^\mathcal{W}\boldsymbol\omega_d\right)\times\left(^\mathcal{W}\boldsymbol\omega_{dxy}\right)+\left(^\mathcal{W}\dot{\boldsymbol\omega}_{dxy}\right) &= \dot{\bar{\mathbf{F}}}\times\dot{\bar{\mathbf{F}}} + \bar{\mathbf{F}} \times \ddot{\bar{\mathbf{F}}}\\ ^\mathcal{W}\mathrm{R}_{\mathcal{B}}\dot{\boldsymbol\omega}_{dxy} &= \dot{\bar{\mathbf{F}}}\times\dot{\bar{\mathbf{F}}} + \bar{\mathbf{F}} \times \ddot{\bar{\mathbf{F}}} - \left(^\mathcal{W}\boldsymbol\omega_d\right)\times\left(^\mathcal{W}\boldsymbol\omega_{dxy}\right) \tag{3-5} \end{align}\]

这里的 $^\mathcal{W}R_{\mathcal{B}}$ 是期望机体坐标系到世界坐标系的旋转矩阵。

（3-2）式很有意思，它通过推力向量在世界坐标系和机体坐标系下的不同表示，来确定相对姿态。该结论可以追溯到【TODO：补充论文链接】，实际被提出的时间应该更早。详细讨论一下（3-2）式。

【例子（旋转的相对性）】：若世界坐标系下的 $x_w = \begin{bmatrix}1 & 0 & 0\end{bmatrix}^\top$ 在机体坐标系下表示为 $x_b = \begin{bmatrix}0& 1 & 0\end{bmatrix}^\top$ ，则说明世界坐标系绕 yaw 轴旋转 -90° 得到机体坐标系。换一个思路，不是坐标系转，而是向量转：将 $x_b$ 和 $x_w$ 视作世界坐标系下的两个不同向量，则 $x_b$ 绕 yaw 轴旋转 -90° 得到 $x_w$ 。

同样地，将 $\bar{\mathbf{F}}_b$ 和 $\bar{\mathbf{F}}$ （这两个向量本质上是同一向量在机体坐标系和世界坐标系中的不同表示）视为世界坐标系下的两个向量，那么 $\bar{\mathbf{F}}_b$ 到 $\bar{\mathbf{F}}$ 的旋转，就等效于世界坐标系到机体坐标系的旋转。待求的四元数 $q_{d0}+\mathbf{q}_d$ 作用在 $\bar{\mathbf{F}}_b$ 上，会得到 $\bar{\mathbf{F}}$ 。

【讨论（旋转的拆解）】：向量 a 旋转到 b 的过程，可以拆解为 a 先绕自身旋转任意角度，再绕向量 a×b 旋转角度到 b ；或者 a 先绕向量 a×b 旋转角度到 b ，再绕 b 旋转任意角度。可以这样拆解的理由是：在 a 到 b 的无数旋转中，绕向量 a×b 旋转角度是唯一一个不使 a 本身自旋的，它与自旋不耦合。

熟知四元数 $\cos(\theta/2)+\mathbf{k}\sin(\theta/2)$ 表示绕轴 $\mathbf{k}$ 旋转 $\theta$ 角度。（3-2）式等号右边的第一个四元数表示绕 $\bar{\mathbf{F}}_b \times \bar{\mathbf{F}}$ 旋转两向量夹角 $\phi$ 。验证如下：

\[\begin{align}\frac{1+\bar{\mathbf{F}}_b^\top\bar{\mathbf{F}}}{\left\|\bar{\mathbf{F}}_b^{\wedge}\bar{\mathbf{F}}\right\|} &= \frac{1+\cos\phi}{\sin\phi}= \frac{2\cos^2(\phi/2)}{2\sin(\phi/2)\cos(\phi/2)} = \frac{\cos(\phi/2)}{\sin(\phi/2)}\\ \frac{(1+\bar{\mathbf{F}}_b^\top\bar{\mathbf{F}})^2 + \left\|\bar{\mathbf{F}}_b^{\wedge}\bar{\mathbf{F}}\right\|^2}{2(1+\bar{\mathbf{F}}_b^\top\bar{\mathbf{F}})} &= \frac{(1+\cos\phi)^2+\sin\phi^2}{2(1+\cos\phi)} = 1 \end{align}\]

（3-2）式等号右边的第二个四元数，表示绕 $\bar{\mathbf{F}}_b$ 旋转 $\psi_d$ 角度，即期望轨迹中的偏航信息。

原文在（3-3）式犯了个错，将 $^{\mathcal{W}}\boldsymbol\omega_{dxy}$ 当成了机体坐标系下的 $\boldsymbol\omega_{dxy}$ 。其参考的文献没有特别强调这一点，写得模棱两可。我们来推导一下。

对世界坐标系下归一化推力求时间导数：

\[\begin{aligned}\dot{\bar{\mathbf{F}}} &= \frac{\mathrm{d}}{\mathrm{d}t}\left(^\mathcal{W}R_{\mathcal{B}}\bar{\mathbf{F}}_b\right)\\ &= \frac{\mathrm{d}}{\mathrm{d}t}(^\mathcal{W}R_{\mathcal{B}})\bar{\mathbf{F}}_b + (^\mathcal{W}R_{\mathcal{B}})\dot{\bar{\mathbf{F}}}_b\\ &= \left(^{\mathcal{W}}\boldsymbol\omega_{\mathcal{B}d}\right)\times\left(^\mathcal{W}R_{\mathcal{B}}\bar{\mathbf{F}}_b\right)\\ &= \left(^{\mathcal{W}}\boldsymbol\omega_{\mathcal{B}d}\right)\times\bar{\mathbf{F}} \end{aligned}\]

其中，由于 $\bar{\mathbf{F}}_b = \begin{bmatrix}0 & 0 & 1\end{bmatrix}^\top$ ，所以其导数为 0 。对旋转矩阵求导的结论详见【TODO：补充链接】。注意这里的 $^\mathcal{W}R_{\mathcal{B}}$ 是期望机体坐标系到世界坐标系的旋转矩阵。

【定理】：考虑三维空间中的一个单位向量 a 和一个任意向量 b ，a×b×a 代表向量 b 在 a 的垂直平面的投影。

根据上述定理， $\bar{\mathbf{F}} \times \dot{\bar{\mathbf{F}}} = \bar{\mathbf{F}} \times \left(^{\mathcal{W}}\boldsymbol\omega_{\mathcal{B}d}\right)\times\bar{\mathbf{F}}$ 代表 $^\mathcal{W}\boldsymbol\omega_{\mathcal{B}d}$ 在 $\bar{\mathbf{F}}$ 垂直平面的投影。

论文给出的位置环能够有效规避欧拉角、微分平坦引入的奇异点——该位置环的奇异点出现在（3-2）式分母为零，即 $\bar{\mathbf{F}}_b^\top\bar{\mathbf{F}} = -1$ ，即机体完全倒转向下的情况。在实际飞行过程中，遇到这一奇异点的概率很低。

4 气动干扰力观测器¶

4.1 能观性说明¶

构造一个新系统，将 $\begin{bmatrix}\mathbf{e}_v^\top & \mathbf{d}_w^\top\end{bmatrix}^\top$ 视为系统的状态，将 $\mathbf{e}_v$ 视为系统的输出。论文指出可以验证该系统的可观性。论文在这里一笔带过，是因为想要把这件事严谨地说清楚，太占篇幅。我们简单地加以说明。

\[\begin{aligned} \begin{bmatrix}\dot{\mathbf{e}}_v\\ \dot{\mathbf{d}}_w\end{bmatrix} &= \overbrace{\begin{bmatrix}\cdots & -I_{3\times3}\\ \cdots & \cdots\end{bmatrix}_{6\times6}}^{A}\begin{bmatrix}\mathbf{e}_v\\ \mathbf{d}_w\end{bmatrix} + \cdots\\ \mathbf{e}_v &= \underbrace{\begin{bmatrix}I_{3\times3} & 0\end{bmatrix}_{3\times6}}_{C}\begin{bmatrix}\mathbf{e}_v\\ \mathbf{d}_w\end{bmatrix} \end{aligned}\]

所以能观性矩阵 $\begin{bmatrix}C^\top & (CA)^\top & \cdots & \left(CA^5\right)^\top\end{bmatrix}^\top$ 列满秩，系统状态能观。

4.2 观测器设计¶

【注】原文这一部分的观测器结构、辅助变量设计是错误的；原文（17）式的推导过程不成立。

设计观测器结构为

\[\begin{cases} \dot{\hat{\xi}} = L_1(m\mathbf{a}-\mathbf{F}+mg\mathbf{e}_3 - S\hat{\xi})\\ \hat{\mathbf{d}}_w = S\hat{\xi} \end{cases} \tag{4-1}\]

这里的 $L_1 \in \mathbb{R}^{6\times 3}$ 是一个增益矩阵，表达式为

\[L_1 = \begin{bmatrix}l_{11} & 0 &0\\0 & l_{12} & 0\\ 0 & 0 & l_{13}\\ l_{14} & 0 & 0\\ 0 & l_{15} & 0\\ 0 & 0 & l_{16}\end{bmatrix}\tag{4-2}\]

（4-1）式结构中包含加速度 $\mathbf{a}$ 一项，但在工程实际中，应尽量避免使用加速度传感反馈信息，因为其噪声很大。鉴于此，我们考虑引入一个辅助变量 $\mu = \hat{\xi}-mL_1\mathbf{v}$ ，以擦除观测器结构中的 $\mathbf{a}$ 。

\[\begin{cases} \begin{aligned}\dot{\mu} &= \dot{\hat{\xi}}-mL_1\dot{\mathbf{v}}\\ &= L_1(m\mathbf{a}-\mathbf{F}+mg\mathbf{e}_3-S\hat{\xi}-m\mathbf{a})\\ &= L_1(mg\mathbf{e}_3-S\hat{\xi}-\mathbf{F})\end{aligned}\\ \hat{\xi} = \mu+mL_1\mathbf{v}\\ \hat{\mathbf{d}} = S\hat{\xi} \end{cases}\tag{4-3}\]

定义推力误差 $\tilde{\mathbf{F}} = \mathbf{F}-f\mathbf{b}_3$ 以及观测误差 $\tilde{\xi}=\hat{\xi}-\xi$ ，则其导数可以如下计算

\[\begin{aligned}\dot{\tilde{\xi}} &= \dot{\hat{\xi}}-\dot{\xi}\\ &= \dot{\mu}+mL_1\dot{\mathbf{v}}-\dot{\xi}\\ &= L_1(mg\mathbf{e}_3-S\hat{\xi}-\mathbf{F}+m\mathbf{a})-\dot{\xi}\\ &= L_1(\mathbf{d}_w-S\hat{\xi} + f\mathbf{b}_3-\mathbf{F})-\dot{\xi}\\ &= -L_1S\tilde{\xi}-L_1\tilde{\mathbf{F}}-\dot{\xi}\end{aligned} \tag{4-4}\]

【讨论】：原文中没有 $\tilde{\mathbf{F}}$ 一项，认为 $f\mathbf{b}_3-\mathbf{F} = 0$ ，这是合理的。因为 $\tilde{\mathbf{F}}$ 是一个较小的有界值，对 $\tilde{\xi}$ 的影响可以忽略。

5 姿态环¶

定义姿态误差 $q_e = q_{e0}+\mathbf{q}_e = qq_d^*$ （ $q_e$ 作用在 $q_dvq_d^*$ 上得到 $q$ ，可以将其理解为期望姿态到当前姿态的误差，广义上等效于“ $q - q_d$ ”）。

\[\begin{align} q_e &= (q_0, \mathbf{q})\otimes(q_{d0}, -\mathbf{q}_d) \tag{5-6}\\ q_{e0} &= q_0q_{d0}+\mathbf{q}^\top \mathbf{q}_d \tag{5-7}\\ \mathbf{q_e} &= -q_0\mathbf{q}_d + q_{d0}\mathbf{q} - \mathbf{q}\times\mathbf{q}_d\tag{5-8} \end{align}\]

【注】（5-8）式对应于原文的（18b）式，原文弄错了 $\mathbf{q}\times\mathbf{q}_d$ 项的正负号。

四元数 $q_e$ 对应的旋转矩阵 $\mathbf{R}(q_e)$ 代表了期望坐标系 $\mathcal{F}_{\mathcal{D}}$ 相对于当前机体坐标系 $\mathcal{F}_{\mathcal{B}}$ 的姿态， $\mathcal{F}_{\mathcal{D}}$ 固定而 $\mathcal{F}_{\mathcal{B}}$ 移动。根据旋转矩阵求导的知识， $\dot{\mathbf{R}} = -^{\mathcal{B}}\boldsymbol\omega_e\times \mathbf{R}$ 。角速度误差可以如下计算：

\[\boldsymbol{\omega}_e = \boldsymbol\omega - \mathbf{R}(q_e)\boldsymbol\omega_d\tag{5-9}\]

根据四元数求导的知识和（1-2b）式，有姿态环系统的状态模型

\[\begin{align} \dot{\mathbf{q}}_e &= \frac{1}{2}(\mathbf{q}_e^\wedge+q_{e0}\mathbf{I}_3)\boldsymbol\omega_e, \ \ \dot{q}_{e0}=-\frac{1}{2}\mathbf{q}_e^\top\boldsymbol\omega_e\tag{5-10a}\\ \mathbf{J}\dot{\boldsymbol\omega}_e &= -\boldsymbol\omega\times\mathbf{J}\boldsymbol\omega+\boldsymbol\tau+\mathbf{d}_\tau - \mathbf{J}(\dot{\mathbf{R}}\boldsymbol\omega_d + \mathbf{R}\dot{\boldsymbol\omega}_d)\\ &= -\boldsymbol\omega\times\mathbf{J}\boldsymbol\omega+\boldsymbol\tau+\mathbf{d}_\tau + \mathbf{J}(\boldsymbol\omega_e\times\mathbf{R}\boldsymbol\omega_d - \mathbf{R}\dot{\boldsymbol\omega}_d) \tag{5-10b} \end{align}\]

接下来，使用 反步法（backstepping） 设计控制输入 $\boldsymbol\tau$ 。根据姿态环系统特性，分角速度、角加速度两步走：

选取 $\boldsymbol\omega_e = \boldsymbol\varpi(\mathbf{q}_e), \boldsymbol\varpi(\mathbf{0})=\mathbf{0}$ 镇定 $\mathbf{q}_e$ 在原点；
选取角度环输出——期望力矩 $\boldsymbol\tau_c(\mathbf{q}_e, \boldsymbol\omega_e), \boldsymbol\tau(\mathbf{0},\mathbf{0}) = \mathbf{0}$ 镇定 $z_2 = \boldsymbol\omega_e - \boldsymbol\varpi$ 在原点。

【讨论】：反步法设计控制器，从结构上期望 $\boldsymbol\omega_e$ 不为 0 ，这一点还挺有意思的。

Step 1： 选取 Lyapunov 函数为 $V_1 = \frac{1}{2}\mathbf{q}_e^\top\mathbf{q}_e$ ，其导数为 $\dot{V}_1 = \frac{1}{2}\dot{\mathbf{q}}_e^\top\mathbf{q}_e + \frac{1}{2}\mathbf{q}_e^\top\dot{\mathbf{q}}_e$ ，注意到 $\mathbf{q}_e^\top\mathbf{q}_e \in \mathbb{R}$ ，其转置不变，所以

\[\begin{aligned} \dot{V}_1 &= \mathbf{q}_e^\top\dot{\mathbf{q}}_e =\frac{1}{2}\mathbf{q}_e^\top(\mathbf{q}_e^\wedge+q_{e0}\mathbf{I}_3)\boldsymbol\omega_e\\ &= \mathbf{q}_e^\top Q\boldsymbol\omega_e \end{aligned}\]

其中 $Q = \frac{1}{2}(\mathbf{q}_e^{\wedge}+q_{e0}\mathbf{I}_3)$ 是一个可逆矩阵。我们可以选取 $\boldsymbol\omega_e = \boldsymbol\varpi = -Q^{-1}C_1\mathbf{q}_e$ 使得 $\dot{V}_1 = -\mathbf{q}_e^\top C_1 \mathbf{q}_e$ 负定，从而镇定 $\mathbf{q}_e$ 在原点。这里 $C_1$ 是一个对角正增益矩阵。

Step 2： 要镇定的误差量为 $z_2 = \boldsymbol\omega_e - \boldsymbol\varpi$ 。选取 Lyapunov 函数为 $V_2 = V_1 + \frac{1}{2}z_2^\top \mathbf{J} z_2$ ，其导数为

\[\begin{aligned} \dot{V}_2 &= \dot{V}_1 + \frac{1}{2}\dot{z}_2^\top\mathbf{J}z_2 + \frac{1}{2}z_2^\top\mathbf{J}\dot{z}_2\\ &= \mathbf{q}_e^\top Q(z_2+\boldsymbol\varpi) + z_2^\top\mathbf{J}\dot{z}_2\\ &= -\mathbf{q}_e^\top C_1\mathbf{q}_e + \mathbf{q}_e^\top Q z_2 + z_2^\top\mathbf{J}(\dot{\boldsymbol\omega}_e - \dot{\boldsymbol\varpi})\\ &= -\mathbf{q}_e^\top C_1\mathbf{q}_e + \mathbf{q}_e^\top Q z_2 - z_2^\top\mathbf{J}\dot{\boldsymbol\varpi} \\ &\ \ \ \ \ + z_2^\top(-\boldsymbol\omega\times\mathbf{J}\boldsymbol\omega+\boldsymbol\tau+\mathbf{d}_\tau + \mathbf{J}(\boldsymbol\omega_e\times\mathbf{R}\boldsymbol\omega_d - \mathbf{R}\dot{\boldsymbol\omega}_d))\\ \end{aligned}\]

考虑到 $\mathbf{q}_e^\top Q z_2 = z_2^\top Q^\top \mathbf{q}_e$ （标量），设计控制输出

\[\begin{aligned} \boldsymbol\tau_c &= -Q^\top\mathbf{q}_e+\mathbf{J}\dot{\boldsymbol\varpi}+\boldsymbol\omega\times\mathbf{J}\boldsymbol\omega - \hat{\mathbf{d}}_\tau - C_2\mathbf{J}z_2\\ &\ \ \ \ \ \ - \mathbf{J}(\boldsymbol\omega_e\times\mathbf{R}\boldsymbol\omega_d-\mathbf{R}\dot{\boldsymbol\omega}_d) \end{aligned}\tag{5-11}\]

其中，$C_2$ 是一个对角正增益矩阵； $\hat{\mathbf{d}}_\tau$ 是扰动力矩的观测值—— $\dot{V}_2$ 中包含扰动力矩 $\mathbf{d}_\tau$ 的部分不是负定的，需要用控制输出将其抵消，我们无法获得扰动力矩的真值，故使用观测值代替。定义扰动力矩的观测误差为 $\tilde{\mathbf{d}}_\tau = \hat{\mathbf{d}}_\tau -\mathbf{d}_\tau$ 。

\[\dot{V}_2 = -\mathbf{q}_e^\top C_1\mathbf{q}_e-z_2^\top \tilde{\mathbf{d}}_\tau - z_2^\top C_2\mathbf{J}z_2 \tag{5-12}\]

（5-12）的负定性依赖于观测误差的具体表达式。我们将在下文介绍扰动力矩观测器设计方法，证明系统的稳定性。

6 扰动力矩观测器¶

6.1 能观性说明¶

和第四节的方法相同，构造一个新系统，状态变量为 $\begin{bmatrix}\boldsymbol\omega_e^\top & \mathbf{d}_\tau^\top\end{bmatrix}^\top$ ，输出为 $\boldsymbol\omega_e$ ，有

\[\begin{aligned} \begin{bmatrix}\dot{\boldsymbol\omega_e}\\ \dot{\mathbf{d}}_\tau\end{bmatrix} &= \overbrace{\begin{bmatrix}\cdots & I_{3\times3}\\ \cdots & \cdots\end{bmatrix}}^A\begin{bmatrix}\boldsymbol\omega_e\\ \mathbf{d}_\tau\end{bmatrix}\\ \boldsymbol\omega_e &= \underbrace{\begin{bmatrix}I_{3\times3} & 0\end{bmatrix}_{3\times6}}_C\begin{bmatrix}\boldsymbol\omega_e\\ \mathbf{d}_\tau\end{bmatrix} \end{aligned}\]

显然能观性矩阵列满秩，系统状态能观。

6.2 观测器设计¶

结合（2-7）式，如下设计观测器结构：

\[\begin{cases} T_m^2\ddot{\hat{\boldsymbol\tau}}_r+2T_m\dot{\hat{\boldsymbol\tau}}_r + \hat{\boldsymbol\tau}_r = \boldsymbol\tau_c\\ \dot{z} = L_2(-\hat{\boldsymbol\tau}_r+\boldsymbol\omega \times \mathbf{J}\boldsymbol\omega)-L_2(z+L_2\mathbf{J}\boldsymbol\omega)\\ \hat{\mathbf{d}}_\tau = z + L_2\mathbf{J}\boldsymbol\omega \end{cases}\tag{6-1}\]

这里的 $\hat{(\cdot)}$ 代表观测值， $z$ 是辅助变量， $L_2 = diag\{l_x, l_y, l_z\} \in \mathbb{R}^{3\times 3}$ 是观测器增益。定义扰动力矩误差 $\tilde{\mathbf{d}}_\tau = \hat{\mathbf{d}}_\tau - \mathbf{d}_\tau$ ，因而有

\[\begin{aligned} \dot{\tilde{\mathbf{d}}}_\tau &= \dot{\hat{\mathbf{d}}}_\tau - \dot{\mathbf{d}}_\tau\\ &= \dot{z} +L_2\mathbf{J} \dot{\boldsymbol\omega}-\dot{\mathbf{d}}_\tau\\ &= L_2(-\hat{\boldsymbol\tau}_r+\boldsymbol\omega\times\mathbf{J}\boldsymbol\omega-z-L_2\mathbf{J}\boldsymbol\omega-\boldsymbol\omega\times\mathbf{J}\boldsymbol\omega+\boldsymbol\tau_r+\mathbf{d}_\tau)-\dot{\mathbf{d}}_\tau\\ &= L_2(\boldsymbol\tau_r - \hat{\boldsymbol\tau}_r -\tilde{\mathbf{d}}_\tau)-\dot{\mathbf{d}}_\tau\\ &= -L_2\tilde{\mathbf{d}}_\tau-\dot{\mathbf{d}}_\tau - L_2 \tilde{\boldsymbol\tau}_r \end{aligned}\]

论文指出当 $\tilde{\mathbf{d}}_\tau(0) = 0$ 时， $\tilde{\mathbf{d}}_\tau \equiv 0$ 。这是建立在（2-7）式的二阶惯性模型完美建模现实响应模型的基础上的，实际上这不可能。但是我们可以认为 $\tilde{\mathbf{d}}_\tau$ 是一个可以忽略的小量。

7 稳定性分析¶

7.1 位置环¶

假设 $\tilde{\mathbf{F}} = f\mathbf{b}_3 -\mathbf{F} = 0$ ，考虑（1-2a）和（3-a）式，有

\[\begin{aligned} m\mathbf{a} &= \mathbf{k}_p\mathbf{e}_p+\mathbf{k}_v\mathbf{e}_v+m\mathbf{a}_d+mg\mathbf{e}_3-\hat{\mathbf{d}}_w-mg\mathbf{e}_3+\mathbf{d}_w\\ &= \mathbf{k}_p\mathbf{e}_p+\mathbf{k}_v\mathbf{e}_v+m\mathbf{a}_d-\tilde{\mathbf{d}}_w\\ -m\mathbf{e}_a &= \mathbf{k}_p\mathbf{e}_p + \mathbf{k}_v\mathbf{e}_v - \tilde{\mathbf{d}}_w\\ \mathbf{e}_a &= \dot{\mathbf{e}}_v = -\frac{1}{m}\mathbf{k}_p\mathbf{e}_p-\frac{1}{m}\mathbf{k}_v\mathbf{e}_v+\frac{1}{m}\tilde{\mathbf{d}}_w \end{aligned}\]

其中， $\mathbf{d}_w =0$ 。定义误差变量 $\mathbf{e} = \begin{bmatrix}\mathbf{e}_p^\top & \mathbf{e}_v^\top\end{bmatrix}^\top$ 。

\[\begin{aligned} \dot{\mathbf{e}} &= \begin{bmatrix}\dot{\mathbf{e}_p}\\ \dot{\mathbf{e}_v}\end{bmatrix}= \underbrace{\begin{bmatrix}0_{3\times3} & I_{3\times 3}\\-\frac{1}{m}\mathbf{k}_p & -\frac{1}{m}\mathbf{k}_v\end{bmatrix}}_A \mathbf{e}+ \underbrace{\begin{bmatrix}0_{3\times 3} & 0_{3\times3}\\ 0_{3\times3} & \frac{1}{m}I_{3\times3}\end{bmatrix}}_B\begin{bmatrix}0_{3}\\\tilde{\mathbf{d}}_w\end{bmatrix} \end{aligned} \tag{7-1}\]

当 $\mathbf{k}_p$ 和 $\mathbf{k}_v$ 的对角元素都为正数时，矩阵 $A$ 的特征方程为 $\lambda^3(\lambda+k_{v1})(\lambda+k_{v2})(\lambda+k_{v3})+k_{p1}k_{p2}k_{p3} = 0$ ，显然，矩阵 $A$ 的特征值实部均小于 0 ，即 $A$ 是一个 Hurwitz 矩阵。

有定理：对于任意的 Hurwitz 矩阵 $A$ 和正定对称矩阵（此处取单位阵）$I$ ，必定能找到一个正定对称矩阵 $W$ ，满足 $A^\top W + WA = -I$ 。这是 Lyapunov 方程的形式，但我们不能凭此认定（7-1）式代表的系统稳定，因为 $\tilde{\mathbf{d}}_w$ 中也包含了状态变量 $\mathbf{e}_v$ 的信息，但未被加以考量。

我们构造 Lyapunov 函数 $V_3$ 为

\[\begin{aligned} V_3 &= \mathbf{e}^\top W \mathbf{e}+\frac{1}{2}\tilde{\xi}^\top\tilde{\xi}\\ \dot{V}_3 &= \mathbf{e}^\top W\dot{\mathbf{e}} + \dot{\mathbf{e}}^\top W \mathbf{e}+\tilde{\xi}^\top\dot{\tilde{\xi}}\\ &= \mathbf{e}^\top W\left(A\mathbf{e}+B\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}\right)+\left(\mathbf{e}^\top A^\top+\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}^\top B^\top\right)W\mathbf{e}+\tilde{\xi}^\top\dot{\tilde{\xi}}\\ &= \mathbf{e}^\top(WA+A^\top W)\mathbf{e}+2\mathbf{e}^\top WB\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}+\tilde{\xi}^\top\dot{\tilde{\xi}}\\ &= -\mathbf{e}^\top \mathbf{e}+2\mathbf{e}^\top WB\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}+\tilde{\xi}^\top\dot{\tilde{\xi}} \end{aligned}\]

上述推导中，需要注意到 $\tilde\xi^\top\tilde\xi$ 和 $\mathbf{e}^\top WB\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}$ 是标量。由于 $W$ 正定， $V_3$ 肯定是正定的。我们接下来说明 $\dot{V}_3$ 负定。

\[\begin{aligned} \mathbf{e}^\top WB\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix} &= \mathbf{e} \cdot WB\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}\ \ \ \ (向量点乘)\\ & \le \left\|\mathbf{e}\right\|\left\|WB\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}\right\|\\ &\le \left\|\mathbf{e}\right\|\left\|WB\right\|\left\|\begin{bmatrix}0_3\\\tilde{\mathbf{d}}_w\end{bmatrix}\right\|\\ &= \sqrt{\mathbf{e}^\top\mathbf{e}}\sqrt{\lambda_{\max}(WBB^\top W^\top)}\sqrt{\tilde{\mathbf{d}}_w^\top\tilde{\mathbf{d}}_w}\ \ \ \ (WB是对称矩阵)\\ &= \lambda_{\max}(WB)\sqrt{\mathbf{e}^\top\mathbf{e}}\sqrt{\tilde{\mathbf{d}}_w^\top\tilde{\mathbf{d}}_w}\\ &\le \frac{1}{2\varepsilon}\lambda_{\max}^2(WB)\mathbf{e}^\top\mathbf{e} + \frac{\varepsilon}{2}\tilde{\mathbf{d}}_w^\top\tilde{\mathbf{d}}_w \end{aligned}\]

其中， $\left\|\cdot\right\|$ 表示取向量的 2-范数或者矩阵的 2-诱导范数， $\lambda_{\max}(\cdot)$ 表示取最大特征值。推导过程用到了一个定理 $\forall a, b,\varepsilon \in \mathbb{R}^+, \frac{1}{2\varepsilon}a+\frac{\varepsilon}{2}b \ge \sqrt{ab}$ 。从而有

\[\dot{V}_3 \le \left(\frac{1}{\varepsilon}\lambda^2_{\max}(WB)-1\right)\mathbf{e}^\top \mathbf{e} + \varepsilon\tilde{\mathbf{d}}_w^\top\tilde{\mathbf{d}}_w+\tilde{\xi}^\top\dot{\tilde{\xi}}\]

因为 $W$ 是对称矩阵，所以有 $\mathbf{e}^\top W\mathbf{e} \le \left\|\mathbf{e}\right\|^2\left\|W\right\| = \lambda_{\max}(W)\mathbf{e}^\top\mathbf{e}$ ，即 $-\mathbf{e}^\top\mathbf{e} \le -\frac{\mathbf{e}^\top W \mathbf{e}}{\lambda_{\max}(W)}$ 。同理有 $\tilde{\mathbf{d}}^\top_w\tilde{\mathbf{d}}_w = \tilde{\xi}^\top S^\top S \tilde{\xi} \le \lambda_{\max}\left(S^\top S\right)\tilde{\xi}^\top\tilde{\xi}$ 。结合（4-4）式，有

\[\begin{aligned} \dot{V}_3 &\le -\frac{\varepsilon-\lambda_{\max}(WB)}{\varepsilon\lambda_{\max}(W)}\mathbf{e}^\top W\mathbf{e}+\varepsilon\lambda_{max}(S^\top S)\tilde{\xi}^\top \tilde{\xi} + \tilde{\xi}^\top(-L_1S\tilde{\xi}-L_1\tilde{\mathbf{F}}-\dot{\xi})\\ &= -\frac{\varepsilon-\lambda_{\max}(WB)}{\varepsilon\lambda_{\max}(W)}\mathbf{e}^\top W\mathbf{e}-\left(-\varepsilon\lambda_{max}(S^\top S)+\sqrt{\lambda_{\max}(S^\top L_1^\top L_1 S)}\right)\tilde{\xi}^\top\tilde{\xi}\\ &\ \ \ \ \ -L_1\tilde{\mathbf{F}}-\tilde{\xi}^\top\dot{\xi} \end{aligned}\]

其中， $\tilde{\mathbf{F}}$ 是一个可忽略的小量。对最后一项做如下放缩：

$$-\tilde{\xi}^\top\dot{\xi} \le \left\|\tilde{\xi}\right\|\left\|\dot{\xi}\right\| \le \left\|\tilde{\xi}\right\|\gamma_w^2 \le \frac{1/2}{2} \left\|\tilde{\xi}\right\|^2 + \frac{1}{2\times (1/2)}\gamma_w^2=\frac{1}{4}\tilde{\xi}^\top\tilde{\xi}+\gamma_w^2$$ 从而有

\[\dot{V}_3 = -\frac{\varepsilon-\lambda_{\max}(WB)}{\varepsilon\lambda_{\max}(W)}\mathbf{e}^\top W\mathbf{e}-\left(\sqrt{\lambda_{\max}(S^\top L_1^\top L_1 S)}-\varepsilon\lambda_{\max}(S^\top S)-\frac{1}{4}\right)\tilde{\xi}^\top\tilde{\xi}+\gamma_w^2\]

由于 $\varepsilon$ 可以任意取正数，因此可以说明 Lyapunov 函数的导数负定。