第 9 章 里程计与观测器¶
状态估计的两个来源：自己怎么动（里程计 / 运动模型推算）与外面看到什么（观测器 / 传感器修正）。本章从原理级把轮式、视觉、激光三类里程计的可观性与退化场景对比清楚，把龙伯格观测器与分离原理走通、并给出它在失配下失效的仿真反例。
实现侧不在覆盖范围：robot_localization 的 EKF/UKF 融合树归 ROS2-note 第 10 章「SLAM 与自主导航」，视觉里程计的直接法/特征法归 camera-note 第 6 章「重建与场景表达」——本章讲"为什么这样融合"。运行环境 python 3.11 + control 0.10.2。
估计的两个来源与分离原理的使用边界：
flowchart LR
odo["里程计：自己怎么动<br/>轮式 / 视觉 / 激光"] --> est["状态估计"]
obs["观测器：外面看到什么<br/>Luenberger 增益 L"] --> est
est --> sep["分离原理：K 与 L 各配各的"]
sep -.->|"模型失配时失效"| fail["仿真反例"]1. 轮式里程计：相对位姿与无界漂移¶
差速驱动的轮式里程计从左右轮增量 \((\Delta s_L,\Delta s_R)\) 解算车身 \((v,\omega)\)、再积分位姿 \(\dot x=v\cos\theta,\ \dot y=v\sin\theta,\ \dot\theta=\omega\)（运动学约束方程由 robotics-note 第 10 章「移动与足式机器人运动学」给出）。它是纯相对推算，误差随时间无界累积：
速度标定偏 5%（轮径/打滑）行驶 20s：末端位置漂移 = 0.282 m
航向恒定偏 1°：10 m 行程横向偏移 = 0.175 m
航向误差被行程"乘以杠杆臂"放大——1° 虽小，10 m 外就是 0.17 m 横漂，且机器人自身无从知晓。结论一句：轮式里程计给的是短时间可信、长时间必漂的相对位姿，必须有绝对或相对观测周期性拉回。
两个标定参数可以解耦：轮半径误差只改纵向尺度（走一段已知长度即可校），轮距误差只改转向曲率（原地转一整圈比对航向）。反过来说，只跑直线看不出轮距偏差，只做原地转看不出轮径偏差，单一路径的测试总会漏掉另一半。
2. 三类里程计的可观性与退化¶
三种里程计的差异在"哪些自由度可观测、何时退化"：
| 里程计 | 直接观测量 | 可观自由度 | 典型退化场景 |
|---|---|---|---|
| 轮式 | 轮子转角 | 前进 + 转向（2-DoF 约束） | 打滑/空转使"转了多少"≠"走了多少"；全向轮约束失效 |
| 视觉 | 像素特征位移 | 6-DoF 位姿增量（含尺度歧义） | 低纹理/纯色墙、纯旋转（无视差→深度不可恢复）、光照剧变 |
| 激光（LiDAR） | 点到点距离 | 2D/3D 位姿增量 | 长直走廊（沿墙方向退化）、玻璃/镜面透射、开阔地特征稀疏 |
退化本质都是观测雅可比在某些方向秩亏：走廊里激光只能测到垂直于墙的方向、沿墙平移几乎不改变点云，该自由度不可观；纯旋转时所有特征只转不移、三角化深度爆炸（camera-note 第 2 章「射影几何与多视图」的退化几何同源）。融合策略就是把互补的可观性拼起来——轮式给连续低频航位推算、视觉/激光给绝对校正，用第 8 章「滤波与融合」的 EKF 融合。何时该切哪路、退化时如何降级（如不可观方向冻结增益、回退纯 IMU），是 robot_localization 侧的运行决策，归 ROS2-note 第 10 章「SLAM 与自主导航」。
3. 状态观测器：龙伯格最小实现¶
控制器要全状态（第 6 章「状态反馈与现代控制」），但常只测得到部分。龙伯格观测器（Luenberger observer）用模型 + 输出校正重建状态：
误差 \(e=x-\hat x\) 满足 \(\dot e=(A-LC)e\)——观测误差动态由 \(A-LC\) 特征值定，与 \(B\)、控制无关。可任意配这些极点 ⇔ \((A,C)\) 可观测（可控性的对偶）。双积分仅测位置（\(C=[1,0]\)）、配观测极点 \(-4,-6\)：
观测器增益 L=[10, 24]ᵀ (A-LC) 特征值=[-6,-4]（误差指数收敛）
仿真 t=3s: 真 x=[2.212,1.500] 估计=[2.287,1.525] 误差=[0.075,0.025]
\(L\) 的两个分量把"位置新息"分别灌回位置与速度：速度本身没测，靠位置误差的积分重建（\(l_2=24\)）。观测器极点选得比控制极点快（此处 −4,−6 vs「分离原理」一节的 −1,−2），估计误差才能"先收敛、再当真实状态用"。降阶观测器（只对不可测状态建 \(n-m\) 维观测器）是它的省维变体，原理一致，本章不单列。
极点不能一味求快：\(L\) 越大，测量噪声通往估计的通路越宽，估计值会带着噪声抖，又被控制器放大成执行器抖振。观测器带宽因此受量测噪声水平约束，取"比控制快一档但不贴近噪声主频"的折中；若想自动权衡模型不确定与量测噪声，就把固定极点换成卡尔曼滤波的时变增益（第 8 章「滤波与融合」）。
4. 分离原理：K 与 L 可各配各的¶
把 \(u=-K\hat x\) 代回、与误差方程联立，坐标变换 \([x;\hat x]\to[x;e]\) 后闭环系统变块三角：
块三角矩阵的特征值就是对角块之并。控制器极点 \(-1,-2\)、观测器极点 \(-4,-6\)：
控制器极点=[-1,-2] 观测器极点=[-6,-4]
联立系统特征值=[-1,-2,-6,-4] （恰为两组之并=分离原理成立）
输出反馈 u=-K xhat 闭环位置终值=0.0000（应→0）
这就是分离原理：标称线性定常下，先按假设全状态可得设计 \(K\)，再单独设计 \(L\) 让估计快收敛，两者互不干扰——第 6 章「状态反馈与现代控制」"测不全就用观测器补"那笔账在此结清。
5. 一个仿真反例：分离原理何时失效¶
分离原理的"互不干扰"依赖模型精确、无延迟。未建模的回路延迟会把这块三角打破。给控制器配快极点 \(-3,-4\)（带宽越高延迟裕度越小）、在对象侧注入整数拍纯延迟（观测器与控制器仍按无延迟模型设计）：
回路延迟=0.00s(0拍): 位置峰倍=1.000e+00 稳定=True
回路延迟=0.20s(4拍): 位置峰倍=1.235e+00 稳定=True
回路延迟=0.40s(8拍): 位置峰倍=1.041e+28 稳定=False ← 发散
0.2 s 内尚稳、0.4 s（8 拍）直接爆到 \(10^{28}\)。机制：延迟让"施加的命令"与"观测器内建的命令"错开 \(nd\) 拍，误差方程不再是 \((A-LC)e\) 的干净块三角，控制器与观测器的极点耦合并向不可预测方向迁移。
工程含义：控制器带宽越高、可用延迟裕度越小，分离原理越脆——这与第 4 章「频率域与鲁棒性」的"带宽换鲁棒性"是同一条约束在观测回路的再现，也和第 1 章「线性系统与时域分析」的 \(\omega dt/2\) 相位侵蚀同源。系统侧的周期抖动治理归 linux-note 第 7 章「内核、启动与实时调优」，算法侧的补救是把延迟显式建模进观测器（延迟观测器/增广状态），本章仅以反例立界。
6. 滑模观测器：一句名目¶
扰动强、还想反推扰动本身时，滑模观测器（sliding-mode observer）用不连续切换项把估计驱动到"滑动面"上，对匹配有界扰动完全鲁棒，并可从等效切换量反演扰动当估计（常用于无传感电流环、故障诊断）。代价是抖振（chattering），需低通滤波或高阶滑模抑制。它属"强扰动/需残余估计"才值得换的一档，此处不展开。
7. 要点¶
- 轮式里程计是相对推算，误差无界累积（速度偏 5%→20 s 漂 0.28 m；航向偏 1°→10 m 横漂 0.175 m），必须被绝对观测周期拉回。
- 三类里程计的差别在可观自由度与退化方向（打滑 / 纯旋转无视差 / 走廊沿墙退化），本质是观测雅可比某些方向秩亏；融合靠互补可观性，实现归 ROS2-note/camera-note。
- 龙伯格观测器误差动态 \((A-LC)e\)，可观测⇔观测极点可任意配；极点选得比控制极点快，估计才当真实状态用。
- 分离原理：标称无延迟下 \(K\)、\(L\) 各配各的、闭环特征值为两组并集；块三角结构是其根据。
- 失效反例：未建模延迟 0.4 s（8 拍）就把快控制器 \(-3,-4\) 的观测闭环推到发散（峰 \(10^{28}\)）——带宽越高延迟裕度越小，与第 4 章「频率域与鲁棒性」的带宽-鲁棒权衡同源。
- 滑模观测器是强扰动/需残余估计档的一招，代价抖振，本章仅点名。
参考文献与延伸阅读¶
- Luenberger, D. G. An Introduction to Observers. IEEE Transactions on Automatic Control, 1971. DOI
- Thrun, S., Burgard, W., & Fox, D. Probabilistic Robotics. MIT Press, 2005.
- Särkkä, S. Bayesian Filtering and Smoothing. Cambridge University Press, 2013.