05.2｜为什么能估计扰动：可观测性、状态反馈与 Lyapunov 逐步证明

先修：状态空间、特征值、导数和微分方程。

零基础桥梁：为什么不直接相信传感器，也不直接相信模型

模型可以在两次测量之间预测运动，却会受未知外力和参数误差影响；测量能纠正漂移，却有噪声且不一定包含速度、扰动。观测器把两者结合：先按动力学预测，再用“实际测到的”和“预测会测到的”之差修正。这个差叫创新 innovation，不是控制目标误差：目标在原点、艇实际在 $1$ m，若模型也预测在 $1$ m，则创新为零，但控制误差仍存在。

例：本课传感器测位置，当前读到 $y=1.00$ m，观测器预测 $\hat p=0.98$ m，创新 $r=y-\hat p=0.02$ m。使用下文增益 $L=(8.6,23.56,27)^T$，三个导数的校正分别为 $0.172$ m/s、$0.4712$ m/s²、$0.54$ m/s³。若以 $0.01$ s 做一次 Euler 更新，仅校正项就分别改变位置估计 $0.00172$ m、速度估计 $0.004712$ m/s、扰动加速度估计 $0.0054$ m/s²；实际更新还要加上模型预测项。同一个位置残差影响三种状态，但通过不同量纲的增益，不是给三个数直接加同一个误差。

极点刻画线性误差的自然时间变化。最简单误差 $\dot e=-\alpha e$ 中，$\alpha>0$ 的单位为 1/s，解 $e(t)=e(0)e^{-\alpha t}$ 表示每过 $1/\alpha$ s 约剩原来的 37%。例如 $\alpha=2/\mathrm s$，经过 $0.5$ s 约剩 37%；正号反过来则越来越大。多状态系统用矩阵特征值表达这种模式，负实部意味着衰减，复数部分可带来振荡。

为什么观测器常选得比控制快一些？控制器需要及时得到状态，但传感器并不会随增益提高而变精确。下文扰动校正增益随观测器速度参数的三次方增长：从 $3/\mathrm s$ 提到 $6/\mathrm s$，该项从 27 变成 216，噪声也更强地进入扰动估计。因此先问采样是否足够快、延迟是否足够小、噪声多大，再选极点；“比控制快”是设计折中，不是固定倍数定理。

Lyapunov 的两个检查为什么一个也不能省

Lyapunov 函数不是必须等于真实机械能，而是一把衡量离目标有多远的尺子。正定表示原点为零，其余任何非零状态都严格为正；若某个非零方向也为零，这把尺子就可能看不见错误。沿轨迹导数负定表示只要不在原点，尺子读数持续下降。前者定义“离开原点”，后者检查“运动是否往回收”；只证明数值为正，完全没有证明它在减少。

下面归一化二状态例中，取位置分量 $p=1$、速度分量 $v=0$，其二次型读数 $V=1.25$、沿闭环导数 $\dot V=-1$；改为 $p=0,v=1$，读数为 $0.25$、导数仍为 $-1$。这些是帮助理解的点检，不代替对所有非零状态的矩阵证明。严格的负导数加上后文特征值界才给出指数收敛；只知道导数不大于零，通常只能说不增加，不能直接断言回到原点。

本章证明范围要随每一步保留：理想线性模型、正确已知参数、无饱和、测量条件满足、恒定扰动及无噪声时得到相应稳定结论。实际 ROV 只在悬停附近近似这些条件，不能把线性全局结论改名为真实机器人全局稳定。有估计误差时还须证明联合系统，不能单拿理想反馈的能量函数作为全部证据。

在线准备： 匀速位置—速度 KF 练预测与创新的分工，标量 KF 练测量权衡；它们不是本课 Luenberger 扰动观测器。扰动状态与 Lyapunov 回归使用本章 C++ 实验。

1. 先把一个轴讲透，再复制矩阵结构

设 $p$ 是相对悬停点的位置误差，$v$ 是该轴速度，$F$ 是控制力，$d_F$ 是未知外力，$m$ 是该轴有效质量。小范围模型为

$$\dot p=v,\qquad m\dot v=-d v+F+d_F.$$

令 $a=d/m$、$u=F/m$、$b=d_F/m$，得到

$$\dot p=v,\qquad\dot v=-av+u+b,\qquad\dot b=0.$$

这里 $a$ 单位 $\mathrm{s^{-1}}$，$u,b$ 单位 $\mathrm{m/s^2}$。本课的 bias 是等效外扰加速度，不是 DVL 的速度偏置，不是 IMU 加计零偏，也不是直接以 N 表示的力。 比较或补偿之前必须先统一单位。$\dot b=0$ 表示短时间内缓慢变化的近似；阶跃到来的一瞬间不满足它，观测器会经历过渡。模型参数误差也会混入估计：若真实阻尼为 $a_p$，观测器使用 $a$，它实际上看到 $b_{total}=b+(a-a_p)v$，不能把该输出解释成纯海流力。

选扩张状态 $z=[p,v,b]^T$：

$$\dot z=A_ez+B_eu,\quad A_e=\begin{bmatrix}0&1&0\\0&-a&1\\0&0&0\end{bmatrix},\quad B_e=\begin{bmatrix}0\\1\\0\end{bmatrix}.$$

为什么加入 $b$？只用 $[p,v]$ 时，模型会把持续加速残差当成每次都新出现的误差；加入常值状态以后，模型能够“记住有一股持续推力”。这与积分控制具有相似的内部模型思想，但估计器与控制器的职责分开了。

2. 不是想估计就能估计：先检查测量

先采用教学传感器 $y=p+n$：位置加测量噪声 $n$。这可代表可用的外部定位、声学定位或已校准的相对视觉位置；它不是 DVL＋IMU＋深度计自动提供的水平绝对位置。测量矩阵 $C_p=[1,0,0]$。

$$\mathcal O_p=\begin{bmatrix}C_p\\C_pA_e\\C_pA_e^2\end{bmatrix} =\begin{bmatrix}1&0&0\\0&1&0\\0&-a&1\end{bmatrix},\qquad\det\mathcal O_p=1.$$

$\mathcal O_p$ 是可观测矩阵；秩为 3 表示在理想已知输入、已知参数、无噪声条件下，一段位置输出包含区分三个初始状态的信息。直觉上，位置给出 $p$，位置变化包含 $v$，速度变化减去已知控制和阻尼包含 $b$。实际实现不直接对位置做两次差分，而用动态观测器过滤。

如果只有 DVL 速度 $y=v+n$，则 $C_v=[0,1,0]$：

$$\mathcal O_v=\begin{bmatrix}0&1&0\\0&-a&1\\0&a^2&-a\end{bmatrix},\qquad\operatorname{rank}\mathcal O_v=2.$$

第一列全零意味着初始位置平移任意常数都不会改变速度输出。因此可估计速度与在模型假设下的总扰动，不能恢复绝对水平位置。积分速度可以维护选定初始原点的航位推算，但误差会积累；要做绝对定点需要额外位置约束。深度计可以给竖直轴位置，不能消除水平两轴的自由平移。

更进一步，若把未知 DVL 常偏 $b_v$ 也加入 $y=v+b_v$，并允许未知恒定外力，两者可能互相解释同一段测量；必须重新检查扩张系统可观测性、增加独立信息或约束，不能仅把观测器维度加大。

3. 观测器每一项为什么这样写

采用 Luenberger 型扩张观测器：

$$\dot{\hat z}=A_e\hat z+B_eu+L(y-C_p\hat z),\qquad L=[l_1,l_2,l_3]^T.$$

帽号表示估计；$r=y-\hat p$ 是创新，即测量与模型预测之差。展开后：

$$\dot{\hat p}=\hat v+l_1r,\quad \dot{\hat v}=-a\hat v+u+\hat b+l_2r,\quad \dot{\hat b}=l_3r.$$

第一式用速度预测位置，第二式用力学预测速度，第三式把持续的位置预测偏差积累为扰动估计。不是“误差大就随意改三个状态”，而是用 $L$ 让完整误差系统稳定。位置误差单位 m，因此 $l_1,l_2,l_3$ 分别具有 $\mathrm{s^{-1}},\mathrm{s^{-2}},\mathrm{s^{-3}}$ 单位。

令 $\tilde z=z-\hat z$。在无噪声且 $\dot b=0$ 时，真实模型与观测器相减，已知输入 $u$ 抵消：

$$\dot{\tilde z}=(A_e-LC_p)\tilde z.$$

于是设计目标就是把 $A_e-LC_p$ 的特征值放在左半平面。它的特征多项式为

$$\det(sI-A_e+LC_p)=s^3+(a+l_1)s^2+(al_1+l_2)s+l_3.$$

选期望多项式 $(s+\omega_o)^3=s^3+3\omega_os^2+3\omega_o^2s+\omega_o^3$，逐项比较，得到

$$l_1=3\omega_o-a,\qquad l_2=3\omega_o^2-al_1,\qquad l_3=\omega_o^3.$$

本课 $a=0.4$、$\omega_o=3\,\mathrm{s^{-1}}$，因此 $L=(8.6,23.56,27)^T$。重复极点并不意味着每个状态都恰按单个指数衰减，还可能出现指数乘多项式；但所有误差仍渐近收敛。增大 $\omega_o$ 会增加噪声注入，尤其 $l_3$ 按三次方增大。不能只以“越快越好”调参。

有噪声和变化扰动时，误差方程变成

$$\dot{\tilde z}=(A_e-LC_p)\tilde z+\begin{bmatrix}0\\0\\\dot b\end{bmatrix}-Ln.$$

这直接说明两种代价：观测器越慢，越跟不上变化扰动；观测器越快，越容易把测量噪声解释成扰动。实际先根据传感器采样、延迟、噪声与控制带宽选范围，再实验，而不是照抄数字。

4. 状态反馈不是换一个名字的调参

先假设估计完全准确、输入不饱和，选择

$$u=a\hat v-k_p\hat p-k_v\hat v-\hat b.$$

$a\hat v$ 补偿模型阻尼，$-k_p\hat p$ 把艇拉回目标，$-k_v\hat v$ 耗散速度，$-\hat b$ 抵消持续外扰。实际符号为 $u$（控制加速度），不要与上一课六维速度向量 $\nu$ 混淆。

代入真实模型且令估计误差为零，得到

$$\ddot p+k_v\dot p+k_pp=0.$$

选两个期望极点为 $-1,-2\,\mathrm{s^{-1}}$，于是 $(s+1)(s+2)=s^2+3s+2$，得到 $k_p=2\,\mathrm{s^{-2}}$、$k_v=3\,\mathrm{s^{-1}}$。这比“调一个 P、调一个 D”更清楚：先指定希望的闭环时间尺度，再由模型解增益。

推广到多轴可以使用 $u=-Kx$；也可通过 LQR 选择 $K$。LQR 解的是给定模型下状态误差与控制代价的权衡，不自动解决不可观测性、饱和或未知恒定外扰。扩张扰动估计与输入分配仍需单独设计。

5. 手算一个 Lyapunov 证明，而不只写“系统稳定”

先把位置与速度按固定参考单位无量纲化；以下数值矩阵作用在该归一化坐标上，避免把 m 与 m/s 的平方直接无权相加。令 $x=[p,v]^T$，理想闭环为

$$\dot x=A_cx,\qquad A_c=\begin{bmatrix}0&1\\-2&-3\end{bmatrix}.$$

想找一个“广义能量” $V=x^TPx$，其中 $P$ 对称正定，使沿轨迹的能量导数严格为负：

$$\dot V=\dot x^TPx+x^TP\dot x=x^T(A_c^TP+PA_c)x.$$

选 $Q=I_2$ 并求 $A_c^TP+PA_c=-Q$。写 $P=\begin{bmatrix}p_{11}&p_{12}\\p_{12}&p_{22}\end{bmatrix}$，逐个元素展开：

$$-4p_{12}=-1,\qquad p_{11}-3p_{12}-2p_{22}=0,\qquad 2p_{12}-6p_{22}=-1.$$

第一式给 $p_{12}=1/4$，第三式给 $p_{22}=1/4$，第二式给 $p_{11}=5/4$，所以

$$P=\begin{bmatrix}1.25&0.25\\0.25&0.25\end{bmatrix},\quad \det P=0.25>0,\quad p_{11}>0,\quad \dot V=-p^2-v^2<0\quad(x\ne0).$$

两个顺序主子式都为正，证明 $P$ 正定；导数负定，证明原点渐近稳定。进一步，由 $\lambda_{min}(P)\|x\|^2\le V\le\lambda_{max}(P)\|x\|^2$，得到 $\dot V\le -V/\lambda_{max}(P)$，因此有指数收敛界。$\lambda_{min},\lambda_{max}$ 是最小、最大特征值，不是新调参。

估计不准确时，证明在哪里接上？

把 $\hat p=p-\tilde p$、$\hat v=v-\tilde v$、$\hat b=b-\tilde b$ 代入控制器：

$$\dot x=A_cx+H\tilde z,\qquad H=\begin{bmatrix}0&0&0\\k_p&k_v-a&1\end{bmatrix}.$$

因此完整闭环为

$$\frac{d}{dt}\begin{bmatrix}x\\\tilde z\end{bmatrix} =\begin{bmatrix}A_c&H\\0&A_e-LC_p\end{bmatrix} \begin{bmatrix}x\\\tilde z\end{bmatrix}.$$

这是块上三角矩阵，特征值是两个对角块特征值的并集：$-1,-2,-3,-3,-3$。所以在前述理想线性条件下，观测器和控制器可分别设计并形成稳定闭环。不能用刚才的 $V(x)$ 单独宣称估计误差存在时始终单调下降，因为多出了 $2x^TPH\tilde z$ 交叉项。

若要完整 Lyapunov 函数，可对这个五维 Hurwitz 矩阵 $A_{all}$ 求

$$P_{all}=\int_0^\infty e^{A_{all}^Tt}Q_{all}e^{A_{all}t}\,dt,\qquad Q_{all}>0.$$

稳定矩阵使积分收敛，任意非零向量代入积分均得到正值，因此 $P_{all}>0$；对积分求导可验证 $A_{all}^TP_{all}+P_{all}A_{all}=-Q_{all}$。这给出完整证明，而不是把二状态的证据偷换成五状态结论。有界噪声和 $\dot b$ 则通常导向有界误差邻域，而非严格归零。

6. 把推导变成可以自己运行的 C++

配套文件在下载项目的 sim-lab/hover_lab.cpp，只依赖 C++17 标准库。程序不是 Gazebo 替代品，而是把符号推导变成可回归测试的最小实验。

cd sim-lab
c++ -std=c++17 -O2 -Wall -Wextra -Wpedantic hover_lab.cpp -o hover_lab
mkdir -p results
./hover_lab results/hover

程序使用前向 Euler，步长 $h=0.002\,\mathrm s$，持续 40 s。对标称线性极点，离散特征值为 $1+h\lambda$；均在单位圆内只是该线性离散化的必要检查之一，不意味着任意增益和非线性饱和都安全。将步长减半是数值收敛检查的下一项实验。

核心更新对应上面的三条方程：

const double e = y - z.x;
const State dz{z.v+l1*e,
               -a*z.v+applied_u+z.b+l2*e,
               l3*e};
z.x += dt*dz.x;
z.v += dt*dz.v;
z.b += dt*dz.b;

applied_u 必须是饱和后的真实执行输入，不是请求值；否则观测器会把执行器没能提供的力误判为外扰。三个导数先基于同一旧状态计算，再一起更新，避免无意混用时间层。

如何避免把真值偷偷送进控制器

plant 内部保存 $p,v,b$，仅用于积分和 CSV 评分。控制器只收到 $y=p+0.002\sin(17t)$ 与自己实际施加的输入；正弦项是确定性测量扰动，方便复现，不是随机白噪声模型。所有控制器共享同一状态观测器，区别只在控制律：

• PD：使用模型阻尼补偿和估计位置、速度，不前馈 $\hat b$。
• PID：在 PD 上加入 $-0.6\int\hat p\,dt$，使用条件积分抗饱和。
• DOB：在 PD 上加入 $-\hat b$；这里 DOB 是本课扩张状态扰动估计＋补偿的简称。

这样比较不会把“一个控制器能看到真速度、另一个不能”的信息差误当成算法优势。PID 本来也能消除可补偿常值扰动，实验不会故意把它设置成永远有静差。

已运行结果与应该怎样解释

$t=5\,\mathrm s$ 注入 $b=0.6\,\mathrm{m/s^2}$，标称输入限制为 $|u|\le2\,\mathrm{m/s^2}$。晚段指标统计 30–40 s。

场景	PD 位置 RMS，m	PID 位置 RMS，m	DOB 位置 RMS，m
标称 $a_p=0.4$	0.300000	0.0000227	0.0000291
阻尼失配 $a_p=0.7$	0.300000	0.0000381	0.0000291
推力不足 $	u	\le0.3$	21.0011

标称 PD 静差可以手算：平衡时 $v=0$，因此 $0=-2p+0.6$，即 $p=0.3$ m。PID 与 DOB 都能近似消除偏差；这组试验不支持“DOB 必然优于 PID”的结论。它支持的是：模型驱动的估计、反馈与证明能形成一致的可检验设计。

最后一行是故意设计的反例。扰动 $0.6$ 大于最大反向控制 $0.3$，不存在原点静止平衡。即便把 $b$ 估得完全正确，也无法凭空获得推力。观测器稳定不等于受约束闭环可实现目标。

程序还检查了理想八推几何分配的秩与右逆、二状态 Lyapunov 矩阵等式、标称 DOB 精度、PD 理论静差与不可行饱和场景。CSV 内含真值、测量、估计、真实外扰、模型失配后的总扰动、实际输入、请求输入与饱和标记；不要只画一根漂亮的位置曲线。

7. 下一轮实验怎么做

按顺序只改一个因素：把观测器带宽减半／加倍；增加测量噪声；给测量加入延迟；引入慢变正弦外扰；减少推进器余量；加入姿态恢复力和轴间耦合。每次同时记录位置 RMS、最大偏差、恢复时间、输入平方积分、饱和比例和扰动估计误差。

扩展到六轴时不要简单把 6 个位置观测器复制后宣称系统可观测。先明确每个轴的传感器、坐标与测量模型，再构造完整 $C$；水平没有位置约束时，改做速度抗扰或相对定位，而不是在估计器里创造不存在的定位信息。

下一课：把控制器接到 Gazebo、SITL 与 ROS 2。