本页目录

进阶 V · 机器人学与状态估计

层次：研究生方向 + 快速增长的产业｜本页把前面的工具用到一个完整对象上。机器人是控制理论最集中的应用场：它同时涉及非线性动力学（第十五页）、多变量控制（第七页）、状态估计（第九页）、约束优化（第十七页）与实时实现（第十、十三页）。本页给出机器人学的骨架，并说明控制在其中的位置。

学习层：让二连杆末端速度反解在奇异位形附近仍可审计

1. 具体工程谜题：末端想沿某个方向走，关节速度是否真的可行？

对长度为 \(l_1,l_2\) 的平面二连杆，关节角 \(q_1,q_2\) 用 rad，末端位置 \(p=[x,y]^\mathsf T\) 用 m。控制器给出目标速度 \(v=[v_x,v_y]^\mathsf T\)（m/s），需要从

\[ \dot p=J(q)\dot q \]

反解关节速度（rad/s）。工程谜题是：同一条任务速度，在普通姿态可以精确追踪，靠近伸直或折叠姿态时却可能要求很大的关节速度；阻尼最小二乘应当牺牲多少末端残差来换取有界动作？

预测门，必须先作答：

Jacobian 连接的是末端速度和关节速度，还是位置和关节角的静态乘积？
\(q_2=0\) 或 \(\pi\) 时，两连杆共线会对 \(J\) 造成什么影响？
增大 DLS 阻尼 \(\lambda\) 的典型速度—精度交换是什么？

2. 正式模型：正运动学、解析奇异值与两种速度反解

正运动学和 Jacobian 为

\[ p=\begin{bmatrix} l_1\cos q_1+l_2\cos(q_1+q_2)\\ l_1\sin q_1+l_2\sin(q_1+q_2) \end{bmatrix}, \]

\[ J=\begin{bmatrix} -l_1\sin q_1-l_2\sin(q_1+q_2)&-l_2\sin(q_1+q_2)\\ l_1\cos q_1+l_2\cos(q_1+q_2)&l_2\cos(q_1+q_2) \end{bmatrix}. \]

实验由 \(JJ^\mathsf T\) 的 2x2 特征值解析计算奇异值 \(\sigma_1\ge\sigma_2\ge0\)，并报告 manipulability \(|\det J|\)。满秩时 Moore–Penrose 伪逆可写成 \(J^\mathsf T(JJ^\mathsf T)^{-1}\)；奇异或近奇异时用阈值伪逆，DLS 用

\[ \dot q_\lambda=J^\mathsf T(JJ^\mathsf T+\lambda^2I)^{-1}v. \]

这里 \(\lambda\) 的单位与 Jacobian 的数值尺度共同决定，不应跨机构无量纲照搬；它抬高小奇异值，通常以任务空间残差换取更小、更有界的关节速度。

3. 动手实验：同时看机构、速度椭圆和反解账本

先提交三项预测，再调杆长、关节角、目标 \(v_x,v_y\) 和 \(\lambda\)。左图画实际机构与目标速度箭头，右图画由奇异值决定的速度椭圆；ledger 并列伪逆/DLS 的关节速度、达到的末端速度、残差和速度范数。把 \(q_2\) 调到 0 或 \(\pi\)，观察不可达方向与阻尼权衡，而不是把数值放大误读为机构“更灵活”。

JavaScript 失效时的静态 fallback：默认取

\[ l_1=1\ \mathrm m,\quad l_2=0.7\ \mathrm m,\quad q_1=0.7\ \mathrm{rad},\quad q_2=-1.1\ \mathrm{rad}, \quad v=[0.25,0.15]\ \mathrm{m/s},\quad \lambda=0.12. \]

末端位置 \(p=[1.409585,0.371625]\ \mathrm m\)，

\[ J=\begin{bmatrix}-0.371625&0.272593\\1.409585&0.644743\end{bmatrix}, \quad \sigma_1=1.567358,\quad \sigma_2=0.398023,\quad |\det J|=0.623845. \]

方法	\(\dot q_1\) (rad/s)	\(\dot q_2\) (rad/s)	达到 \(v_x\)	达到 \(v_y\)	残差范数	关节速度范数
伪逆	-0.192831	0.654233	0.250000	0.150000	\(1.0\times10^{-16}\)	0.682059
DLS (\(\lambda=0.12\))	-0.171737	0.601785	0.227864	0.145918	0.022509	0.625810

该账本的单位是 m、m/s、rad 和 rad/s；若接近奇异构型，必须同时报告奇异值和残差，不能只报一组关节速度。

4. 误区与模型边界

正运动学唯一不等于逆运动学唯一：这里的速度反解只讨论局部 Jacobian，未解决全局多解、工作空间边界和碰撞。
奇异位形是方向性退化：\(\sigma_2\to0\) 表示某些末端速度方向不可达；伪逆给的是可达投影，不会创造物理自由度。
DLS 不是无代价修复：增大 \(\lambda\) 可抑制关节速度峰值，但会增加任务空间残差；真实系统还要检查关节限位、速度/加速度/力矩和采样延迟。
单位与尺度要保留：\(J\) 的列是 m/rad，\(\dot q\) 是 rad/s，末端速度是 m/s；不同长度尺度下同一个数值 \(\lambda\) 不代表同一正则化强度。
模型边界：本实验是无障碍、无动力学、无摩擦的平面二连杆速度级 toy；接触、柔性、负载和执行器约束需要动力学层另行验证。

迁移问题：一台七轴协作臂要避开人并保持末端速度。你会如何把 Jacobian 的零空间、阻尼、关节限位和碰撞约束组合进速度 QP？请说明哪些量由奇异值证书支持，哪些必须由约束求解和实时监控补上。

一、运动学：从关节角到末端位姿

正运动学：给定关节角 \(q\)，求末端位姿 \(T(q)\)。用 DH 参数或齐次变换矩阵逐关节连乘，总是有唯一解，计算直接。

逆运动学：给定期望位姿，求关节角。困难得多：

可能无解（超出工作空间）；
可能多解（如肘上/肘下两种姿态）；
可能无穷解（冗余机器人，自由度多于任务维数——如 7 轴机械臂做 6 维任务，多出的自由度可用来避障或优化姿态）。

雅可比矩阵 \(J(q)\) 连接关节速度与末端速度：

\[\dot{p} = J(q)\dot{q}\]

奇异位形是关键概念：当 \(J\) 降秩时，机器人在某些方向失去运动能力，而反解 \(\dot q = J^{-1}\dot p\) 会要求无穷大的关节速度。

工程后果：轨迹必须避开奇异点附近，或使用阻尼最小二乘（DLS）伪逆：

\[\dot{q} = J^T(JJ^T+\lambda^2 I)^{-1}\dot{p}\]

这本质上是正则化（🔗 与岭回归同一数学）——用一点跟踪误差换取有界的关节速度。又一次"用精度换可行性"的权衡。

二、动力学

\[M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) + F(\dot q) = \tau\]

\(M(q)\)：惯量矩阵（随姿态变化——这是机器人本质非线性的主要来源）；
\(C\)：科氏与离心项；\(G\)：重力项；\(F\)：摩擦（第十五页的老对手）。

几个有用的结构性质（它们使许多控制方法成为可能）：

\(M\) 对称正定；
\(\dot{M}-2C\) 反对称——这个性质直接用于 Lyapunov 稳定性证明；
对参数线性：\(\tau = Y(q,\dot q,\ddot q)\Theta\)，\(\Theta\) 是惯性参数向量。这使参数辨识成为线性最小二乘问题（第十八页），是工业机器人标定的基础。

主要控制方法：

方法	思想	评价
独立关节 PD/PID	每关节当作独立系统，把耦合当扰动	工业机器人的主流做法（高减速比使耦合减弱）
PD + 重力补偿	加上 \(G(q)\) 前馈	简单有效，可用无源性证明稳定
计算力矩法	用模型抵消非线性（反馈线性化，第十五页）	性能好，依赖模型精度
阻抗/导纳控制	不控位置或力，而控二者的动态关系	人机协作与接触作业的关键

阻抗控制值得强调：与环境接触时，纯位置控制是危险的（微小位置误差可产生巨大接触力）。阻抗控制让机器人表现得像一个可调的弹簧-阻尼系统：

\[M_d\ddot{e} + B_d\dot{e} + K_d e = F_{ext}\]

设计者直接指定期望的"柔顺程度"——这是协作机器人（cobot）能安全地与人共处的技术基础。从"控制位置"转向"控制动态关系"，是一个观念上的跃迁。

三、轨迹规划

分层结构（与第十七页的 MPC 分层同构）：

\[\text{任务规划} \to \text{路径规划（几何）} \to \text{轨迹生成（含时间）} \to \text{跟踪控制}\]

路径规划：RRT/RRT、PRM（采样法，适合高维）、A/Dijkstra（栅格）、势场法（易陷局部极小）；
轨迹生成：在路径上分配时间，满足速度/加速度/加加速度（jerk）限制。限制 jerk 对减少机械振动与磨损很重要；
优化式方法：直接把避障与动力学约束写进优化问题——这正是 MPC 的思路（第十七页），当前主流方向。

四、状态估计与 SLAM

机器人必须知道自己在哪。这是第九页卡尔曼滤波的主战场。

SLAM（同步定位与建图）：在未知环境中，同时估计自身位姿与环境地图。

它的困难在于"鸡与蛋"：定位需要地图，建图需要定位。这在数学上是一个联合估计问题。

主要方法：

滤波式：EKF-SLAM（状态含机器人位姿 + 所有地标，协方差矩阵随地标数平方增长，难以扩展）；粒子滤波（FastSLAM，适合多峰分布）；
优化式（现代主流）：图优化 / 因子图——把位姿与观测建成图，求解非线性最小二乘。回环检测（认出"我来过这里"）提供强约束，可显著校正累积漂移；
传感器：激光雷达、视觉（VIO：视觉惯性里程计）、IMU、轮式里程计、GNSS。多传感器融合是常态，因为各自的误差特性互补（承第九页）。

关键概念——可观性退化：某些运动模式下部分状态不可观（如纯旋转时无法估计尺度、匀速直线运动时某些偏差无法辨识）。好的实现必须能表达"这个方向我不确定"，而非给出一个自信的错误估计。这是第九页那句"滤波器不能过度自信"在机器人上的具体化。

五、当前的活跃方向

足式机器人：欠驱动、接触切换、实时性要求高。实时非线性 MPC + 全身控制是当前主流框架，落脚点规划与平衡恢复是核心问题；
柔顺与接触作业：装配、打磨、人机协作——阻抗控制 + 力感知；
移动操作：底盘 + 机械臂协同，冗余度更高；
学习与控制结合（第二十页）：从演示中学习、sim2real、视觉-动作策略。这是当前投入最大、争议也最多的方向。

六、机器人为什么是好的综合训练

它把本课的几乎所有内容串在一起：

用到	页
级联控制、PID 整定	第六、十二页
状态空间与 MIMO	第七页
状态估计与传感器融合	第九页
采样、实时性	第十、十三页
电机与驱动	第十二页
非线性与 Lyapunov	第十五页
约束优化与 MPC	第十七页
参数辨识	第十八页
功能安全（协作机器人的力/速度限制）	第十四页

且它有一个教学上的巨大优势：可以看见。 控制律的好坏直接表现为机器人动作的流畅或抖动——这种即时的、具身的反馈，是理解控制最有效的途径之一。

七、要点

逆运动学可能无解/多解/无穷解；奇异位形处雅可比降秩，需阻尼伪逆（正则化）；
动力学的结构性质（对称正定、反对称、参数线性）是控制与辨识方法的基础；
工业机器人主流仍是独立关节 PID + 前馈——高减速比削弱了耦合；
阻抗控制把目标从"位置"改为"动态关系"，是接触作业与人机协作的关键；
SLAM 的现代主流是图优化 + 回环检测；
可观性退化时必须诚实表达不确定性。

线四结束。下一线走向前沿：当模型难以获得、任务难以形式化时，能否让系统从数据中学会控制？以及——怎样在学习的同时保证安全。