第76章：轮式运动学与 Pfaffian 约束——非完整几何的完整推导

元信息	值
难度	⭐⭐⭐（非完整约束完整推导 + 轮接触建模 + 可控性分析）
预计时间	1.5 周（30-35 小时）
前置依赖	复合/10_复合机器人全景、复合/20_浮动基座臂统一动力学、足式/70_腿足简化模型理论
下游连接	复合/70_轮足混合MPC、复合/80_Wheel_Legged_Gym_RL、复合/90_Swiss_Mile商业化

本章定位：轮足机器人比纯四足多出的第一个数学对象不是电机，而是速度层面的非完整滚动约束。本章从最基本的"为什么轮子不能横移"出发，完整推导 Pfaffian 约束矩阵，经过 Frobenius 可积性判据和 Chow 定理的可控性分析，延伸到实际的滑移建模和轮足混合运动学，最终落地到 ROS2 导航栈的运动学插件接口。全部推导不省略任何步骤，每个公式都给出物理动机和工程意义。

---

前置自测

📋 前置自测（答不出 2 题以上请先回顾对应章节）

#	问题	来源
1	浮动基座系统中 model.nq 与 model.nv 为什么不同？两者的差异来自哪个数学结构？	复合/20
2	足端静止接触约束如何写成 \(J_c(q)\dot{q}=0\) 的形式？\(J_c\) 的物理含义是什么？	足式/50
3	SE(2) 群上的运动有几个自由度？用齐次坐标写出 SE(2) 变换矩阵。	足式/50
4	什么是"约束"在数学上的含义？位置约束和速度约束有什么本质区别？	基础力学
5	差速底盘为什么不能瞬时侧向平移？这个限制是永久性的还是瞬时性的？	本科机器人学

自测标准：5/5 直接阅读；3-4/5 边读边查；0-2/5 先回顾 复合/20 + 足式/50。

---

本章目标

学完本章后，你应当能够：

1. 从物理约束出发，独立推导差速、阿克曼、全向和麦克纳姆四种底盘的 Pfaffian 约束矩阵 \(A(q)\dot{q}=0\)。
2. 用 Frobenius 定理判断一个 Pfaffian 约束是否可积（完整），用 Lie bracket 给出判据。
3. 用 Chow 定理解释"差速车不能横移但能到达任意位置"的数学本质。
4. 定义滑移比和滑移角，解释 Pacejka 模型的简化形式及其工程用途。
5. 写出轮足混合系统中足+轮共存的统一 Pfaffian 约束矩阵。
6. 理解 ROS2 nav2 中运动学插件的接口设计及其与 Pfaffian 矩阵的对应关系。

---

76.1 动机：为什么轮式机器人不能随意横移？⭐

这一节解决什么问题

每个学过机器人学的人都知道"差速车不能横移"，但很少有人能精确回答：这个限制的数学本质是什么？它是位置限制还是速度限制？它能被克服吗？本节通过一个日常体验引入非完整约束的核心概念，为后续严格推导铺路。

从停车场说起

想象你正在驾驶一辆普通轿车（后轮驱动，前轮转向）。你需要从当前车道横向平移到相邻车道。直觉告诉你：不能直接横移——车子只能前进或后退，同时可以转弯。但经过一系列"前进-打方向-后退-回正"的操作，你确实可以到达相邻车道的目标位置。

这个日常经验揭示了一个深刻的数学性质：

特征	描述
瞬时限制	在任意时刻，车辆的速度方向受到约束——不能垂直于车轮平面运动
全局自由	给定足够时间和空间，车辆可以到达平面上的任意位置和朝向
矛盾的表象	速度受限，但位置不受限——这怎么可能？

这种"速度受限但位置不受限"的约束，就是非完整约束（nonholonomic constraint）。它与"位置受限"的完整约束（holonomic constraint）有本质不同。

完整约束 vs 非完整约束：第一次区分

在经典力学中，约束分为两大类：

完整约束（holonomic constraint）：可以写成位置（和时间）的函数：

\[ \phi(q, t) = 0 \]

例如，一个质点被约束在球面上：\(x^2 + y^2 + z^2 - R^2 = 0\)。这个约束限制了质点的可达位置——质点永远只能在球面上。

非完整约束（nonholonomic constraint）：只能写成速度的函数，不能积分为位置约束：

\[ A(q)\dot{q} = 0 \]

例如，轮子的纯滚动约束。这个约束限制了瞬时速度方向，但不限制可达位置。

本质洞察：完整约束减少了系统的配置空间维度（质点从三维空间降到二维球面），而非完整约束减少了系统的瞬时速度自由度但不减少配置空间维度。这个区别正是轮式机器人运动规划复杂性的根源。

如果不区分这两种约束会怎样？

假设你在设计轮式机器人的路径规划器。如果你错误地把非完整约束当作完整约束处理：

1. 规划器会尝试生成横向平移轨迹——因为在完整约束视角下，轮子的约束只限制了某些位置，而不限制速度方向
2. 底层控制器无法执行——因为生成的轨迹在物理上不可行（要求车辆横移）
3. 实际表现：机器人原地打转或完全不动，轮胎发出刺耳的摩擦声

反过来，如果你错误地把非完整约束当作"不可克服的限制"——认为差速车只能走直线：

1. 规划器过于保守——很多可达位置被错误地标记为不可达
2. 机器人只能在狭窄走廊中直行——丧失了通过多次机动到达任意位置的能力

正确的理解方式是：非完整约束限制的是瞬时运动方向（微分约束），而非最终可达空间（积分约束）。

历史脉络

非完整约束的研究始于 19 世纪末的经典力学：

年代	人物	贡献
1894	Hertz	首次系统讨论非完整约束在力学中的角色
1897	Appell	提出处理非完整系统的 Appell 方程
1900s	Chaplygin	研究滚动圆盘等非完整系统的精确解
1939	Chow	证明了 Chow 定理——非完整系统的可控性判据
1970s	Brockett	将非完整约束与控制论联系，开创非线性控制理论
1994	Murray, Li, Sastry	《A Mathematical Introduction to Robotic Manipulation》系统化了非完整运动规划
2000s	LaValle	将采样方法（RRT 等）推广到非完整系统

类比：非完整约束之于运动规划，就像不可压缩条件之于流体力学——都是对"速度场"的微分约束，而非对"位置"的代数约束。在流体力学中，\(\nabla \cdot \mathbf{v} = 0\) 限制了速度场的散度为零，但流体仍可到达任意位置。在轮式机器人中，\(A(q)\dot{q} = 0\) 限制了速度方向，但机器人仍可到达任意配置。

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：直接用 A* 或 Dijkstra 规划轮式机器人路径 - 错误做法：在栅格地图上用标准 A* 搜索最短路径，然后直接让轮式机器人跟踪 - 典型现象：规划出的路径包含急转弯甚至 90 度直角转弯，底层控制器无法跟踪，机器人频繁偏离路径 - 根本原因：A/Dijkstra 是对完整系统的路径搜索——它假设机器人可以沿任意方向移动。非完整系统需要使用 state-lattice planner 或 Hybrid A 等考虑运动学约束的规划器 - 正确做法：使用 Hybrid A*（在 \((x, y, \theta)\) 空间搜索）或 state-lattice planner（预计算满足运动学约束的运动原语）

💡 概念误区：认为"非完整 = 不可控" - 新手想法："轮子不能横移，所以差速车的横向位置不可控" - 实际上：非完整约束限制的是瞬时速度方向，不是可达位置集合。差速车通过"前进-转弯-前进"的序列可以到达平面上任意 \((x, y, \theta)\) 配置。非完整系统的可控性由 Chow 定理保证（本章 76.4 节详述） - 关键区别：不可控是指存在永远无法到达的状态；非完整是指到达某些状态需要"绕路"

🧠 思维陷阱：混淆"不能横移"和"不能到达侧面位置" - 新手想法："差速车不能横移，所以它不能到达正侧方 1 米处的位置" - 实际上：差速车完全可以到达侧方任意位置——只需要先前进、转 90 度、再前进。"不能横移"只是说不能瞬时沿侧向运动，不是说侧向位置不可达 - 正确思维：区分"瞬时运动方向"（微分层面）和"可达配置集合"（积分层面）

练习 76.1

[A] ⭐ 列举三个日常生活中的非完整约束和三个完整约束的例子。对每个例子，说明它限制的是位置还是速度。

[B] ⭐⭐ 一个冰球在冰面上自由滑动。它受到的约束（如果有）是完整的还是非完整的？如果你给冰球加上一个"只能沿当前朝向滑动"的约束，情况如何变化？

[C] ⭐⭐ 考虑一个在平面上运动的刚体，其配置空间是 \(SE(2)\)，维度为 3。如果施加一个非完整约束减少了一个瞬时速度自由度，剩余几个瞬时速度自由度？配置空间维度是否改变？

---

76.2 SE(2) 上的运动学与速度约束 ⭐⭐

这一节解决什么问题

轮式机器人在平面上运动，其配置空间是 \(SE(2)\)——平面上的刚体位姿空间。本节建立 \(SE(2)\) 上的运动学框架，作为后续推导 Pfaffian 约束的数学基础。

从复合/20 桥接：浮动基座到平面约束

连接前序知识：复合/20 中我们建立了浮动基座的全身动力学框架——\(SE(3)\) 配置空间中的广义坐标和速度。本节将这个三维框架投影到二维平面，得到轮式底盘的 \(SE(2)\) 运动学。这种投影不是"简化"——而是因为轮子与地面的接触约束把运动限制在了平面上。理解这个投影关系，是后续将 Pfaffian 约束嵌入全身动力学的基础。

回顾复合/20_浮动基座臂统一动力学：浮动基座机器人的配置空间是 \(SE(3)\)（六维），广义坐标 \(q_b\) 用四元数表示方位（\(n_q = 7\)），广义速度 \(v_b\) 用 twist 表示（\(n_v = 6\)），因此 \(n_q \neq n_v\)。这是 Lie 群的基本特征。

对于平面运动，情况简化到 \(SE(2)\)——三维 Lie 群。一个在平面上运动的刚体，其配置完全由 位置 \((x, y)\) 和 朝向 \(\theta\) 确定：

\[ q = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} \in SE(2) \cong \mathbb{R}^2 \times S^1 \]

\(SE(2)\) 的齐次坐标表示为：

\[ T = \begin{pmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{pmatrix} \in SE(2) \]

与 \(SE(3)\) 的区别：\(SE(2)\) 上朝向只有一个角度 \(\theta\)，不需要四元数，因此 \(n_q = n_v = 3\)。这是 \(S^1\)（圆）相比 \(SO(3)\)（球面上的旋转）的简化。

body frame 速度与 world frame 速度

刚体的速度可以在两个坐标系中表示：

World frame 速度（空间速度）：

\[ \dot{q} = \begin{pmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{pmatrix} \]

Body frame 速度（体速度）：

\[ v^b = \begin{pmatrix} v_x^b \\ v_y^b \\ \omega \end{pmatrix} \]

其中 \(v_x^b\) 是沿车体前向的速度，\(v_y^b\) 是沿车体侧向的速度，\(\omega = \dot{\theta}\) 是偏航角速度。

两者通过旋转矩阵联系：

\[ \begin{pmatrix} \dot{x} \\ \dot{y} \end{pmatrix} = R(\theta) \begin{pmatrix} v_x^b \\ v_y^b \end{pmatrix} = \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix} \begin{pmatrix} v_x^b \\ v_y^b \end{pmatrix} \]

展开后：

\[ \dot{x} = v_x^b \cos\theta - v_y^b \sin\theta \]

\[ \dot{y} = v_x^b \sin\theta + v_y^b \cos\theta \]

\[ \dot{\theta} = \omega \]

反事实推理：如果我们不区分 body frame 和 world frame 速度会怎样？考虑一个朝向 \(\theta = 90°\) 的车辆以 body frame 前向速度 \(v_x^b = 1\) m/s 行驶。在 world frame 中，它实际上是沿 \(y\) 方向运动（\(\dot{x}=0, \dot{y}=1\)）。如果我们错误地把 body frame 速度直接当作 world frame 速度，就会计算出车辆沿 \(x\) 方向运动——方向完全错误。这种错误在仿真中表现为机器人"螃蟹走路"。

约束的 Pfaffian 形式

现在我们可以精确定义：Pfaffian 约束是对广义速度的线性约束，其一般形式为：

\[ A(q)\dot{q} = 0 \]

其中 \(A(q) \in \mathbb{R}^{k \times n}\)，\(k\) 是约束个数，\(n\) 是广义坐标维度。

Pfaffian 约束矩阵 \(A(q)\) 的每一行定义了一个禁止的速度方向——系统的速度不能有沿该方向的分量。所有满足约束的速度构成一个允许速度分布（distribution）：

\[ \mathcal{D}(q) = \{v \in \mathbb{R}^n \mid A(q)v = 0\} = \ker A(q) \]

\(\mathcal{D}(q)\) 的维度为 \(n - \text{rank}(A(q))\)，即系统在任意时刻的瞬时运动自由度。

概念	数学表示	物理含义
配置空间维度	\(n = \dim(q)\)	系统需要几个坐标描述位置
约束个数	\(k = \text{rank}(A)\)	几个独立的速度方向被禁止
瞬时自由度	\(n - k\)	系统此刻能沿几个方向运动
允许速度分布	\(\mathcal{D}(q) = \ker A(q)\)	此刻允许的所有速度的集合

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：在 world frame 中写约束矩阵时忘记旋转 - 错误做法：直接写 \(A = [0, 1, 0]\)（禁止 \(y\) 方向速度）作为"不能横移"的约束 - 典型现象：约束只在 \(\theta = 0\) 时正确，其他朝向下约束方向错误 - 根本原因：\(A(q)\) 必须是配置 \(q\) 的函数——特别是 \(\theta\) 的函数。"禁止横向速度"在 body frame 是常数约束 \(v_y^b = 0\)，但转换到 world frame 后变成 \(-\dot{x}\sin\theta + \dot{y}\cos\theta = 0\)，矩阵 \(A\) 依赖 \(\theta\) - 正确做法：先在 body frame 写约束，再通过旋转矩阵变换到 world frame

💡 概念误区：认为 Pfaffian 约束 \(A(q)\dot{q}=0\) 的形式与足端接触 \(J_c(q)v=0\) 不同 - 新手想法："轮式约束和足端约束是两个完全不同的东西" - 实际上：两者在数学形式上完全相同！足端静止接触 \(J_c(q)v = 0\) 就是一个 Pfaffian 约束，其中 \(A(q) = J_c(q)\)。区别仅在于物理含义：足端约束要求接触点速度为零（完整约束，可积分为固定位置），轮式约束要求接触点不横滑（非完整约束，不可积分为位置关系） - 延伸：这种统一视角正是轮足混合系统（76.7 节）的数学基础

练习 76.2

[A] ⭐⭐ 写出 \(SE(2)\) 上从 body frame 速度到 world frame 速度的完整变换矩阵。验证其行列式等于 1（即变换保体积）。

[B] ⭐⭐ 如果一个平面机器人受到约束 \(v_y^b = 0\)（body frame 侧向速度为零），将其转换为 Pfaffian 形式 \(A(q)\dot{q} = 0\)。写出 \(A(q)\) 并验证它在 \(\theta = 0, \pi/4, \pi/2\) 时的具体形式。

[C] ⭐⭐⭐ 考虑一个受到约束 \(v_x^b = v_y^b = 0\)（body frame 线速度为零，只能原地旋转）的机器人。这是一个什么类型的约束（完整/非完整）？它的允许速度分布维度是多少？

---

76.3 Pfaffian 约束矩阵的完整推导：四种底盘 ⭐⭐⭐

这一节解决什么问题

本节是全章的核心。我们将从物理约束出发，逐一推导差速、阿克曼、全向（Swedish wheel）和麦克纳姆四种主流底盘的 Pfaffian 约束矩阵。每种推导都遵循同一个三步框架：（1）明确物理约束（纯滚动 + 不横滑），（2）将 body frame 约束转换为 world frame，（3）组装 Pfaffian 矩阵 \(A(q)\)。

76.3.1 单轮圆盘：最小非完整系统 ⭐⭐

动机

单轮圆盘（upright rolling disk）是非完整约束的最小模型——只有两个约束和四个自由度。理解它是理解所有轮式系统的基础。

配置空间

一个竖直圆盘在平面上滚动，其配置由四个变量描述：

\[ q = \begin{pmatrix} x \\ y \\ \varphi \\ \theta \end{pmatrix} \in \mathbb{R}^2 \times S^1 \times S^1 \]

变量	含义	范围
\((x, y)\)	轮心在世界系中的位置	\(\mathbb{R}^2\)
\(\varphi\)	轮平面的朝向角（heading）	\([0, 2\pi)\)
\(\theta\)	轮绕自身轴的转角（rolling）	\(\mathbb{R}\)（无界）

配置空间维度 \(n = 4\)。

推导：从物理到数学

Step 1：建立局部坐标系

在轮的 body frame 中定义三个单位向量：

• 前向单位向量：\(e_\parallel = (\cos\varphi, \sin\varphi)^T\)（轮平面朝向）
• 横向单位向量：\(e_\perp = (-\sin\varphi, \cos\varphi)^T\)（垂直于轮平面）
• 法向单位向量：\(e_n = (0, 0, 1)^T\)（垂直于地面向上）

Step 2：写出物理约束

纯滚动条件包含两个独立约束：

约束 1：纵向纯滚动——轮心沿前向的速度等于轮缘线速度 \(r\dot{\theta}\)

这个约束的物理含义是：轮子向前滚动时，轮心走过的距离等于轮缘展开的弧长。如果不满足这个约束，要么轮子在地面上打滑（轮缘速度大于轮心速度），要么轮子被拖着走（轮心速度大于轮缘速度）。

数学表达：

\[ e_\parallel^T \dot{p} - r\dot{\theta} = 0 \]

展开：

\[ \dot{x}\cos\varphi + \dot{y}\sin\varphi - r\dot{\theta} = 0 \]

约束 2：横向不滑——轮心沿横向的速度为零

这个约束的物理含义是：轮子不会沿着轮轴方向滑动。如果违反这个约束，轮子就是在"侧滑"——就像汽车在冰面上打横一样。

数学表达：

\[ e_\perp^T \dot{p} = 0 \]

展开：

\[ -\dot{x}\sin\varphi + \dot{y}\cos\varphi = 0 \]

Step 3：组装 Pfaffian 矩阵

将两个约束写成矩阵形式 \(A(q)\dot{q} = 0\)：

\[ \underbrace{\begin{pmatrix} \cos\varphi & \sin\varphi & 0 & -r \\ -\sin\varphi & \cos\varphi & 0 & 0 \end{pmatrix}}_{A(q)} \begin{pmatrix} \dot{x} \\ \dot{y} \\ \dot{\varphi} \\ \dot{\theta} \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \]

验证维度：\(A \in \mathbb{R}^{2 \times 4}\)，\(\text{rank}(A) = 2\)（两行线性无关，因为 \(\cos^2\varphi + \sin^2\varphi = 1 \neq 0\)），瞬时自由度 = \(4 - 2 = 2\)。

瞬时自由度的物理含义：圆盘在任意时刻只能做两种运动——向前/后滚动（改变 \(\theta\) 和 \((x,y)\)）和改变朝向（改变 \(\varphi\)）。不能横移，也不能在不转轮的情况下平移。

允许速度的参数化

从 \(A(q)\dot{q} = 0\) 的零空间求出允许速度：

\[ \dot{q} = u_1 \underbrace{\begin{pmatrix} r\cos\varphi \\ r\sin\varphi \\ 0 \\ 1 \end{pmatrix}}_{g_1(q)} + u_2 \underbrace{\begin{pmatrix} 0 \\ 0 \\ 1 \\ 0 \end{pmatrix}}_{g_2(q)} \]

其中 \(u_1 = \dot{\theta}\)（轮转速）和 \(u_2 = \dot{\varphi}\)（转向速率）是两个输入。\(g_1\) 和 \(g_2\) 是两个控制向量场，它们生成了允许速度分布 \(\mathcal{D}(q) = \text{span}\{g_1(q), g_2(q)\}\)。

本质洞察：Pfaffian 约束 \(A(q)\dot{q} = 0\) 的零空间定义了一组控制向量场 \(\{g_1, g_2, \ldots\}\)。系统的运动方程变成 \(\dot{q} = \sum_i u_i g_i(q)\)——这是漂移-free 仿射控制系统的标准形式。非完整运动规划的所有理论工具（Chow 定理、RRT、steering function）都建立在这个形式之上。

76.3.2 差速底盘 ⭐⭐

动机

差速底盘（differential drive）是最简单的实用移动机器人底盘，由两个独立驱动的轮子和一个或多个被动轮组成。TurtleBot、iRobot Create 和大量教学机器人都使用这种构型。

配置与约束推导

差速底盘的配置只需三个变量（轮转角通过运动学约束隐含）：

\[ q = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} \in SE(2) \]

其中 \((x, y)\) 是两轮中点（或底盘质心）在世界系中的位置，\(\theta\) 是底盘朝向。

两个轮子的转速 \(\omega_L, \omega_R\) 通过以下运动学关系映射到底盘速度：

\[ v_x^b = \frac{r(\omega_R + \omega_L)}{2}, \quad \omega = \dot{\theta} = \frac{r(\omega_R - \omega_L)}{L} \]

其中 \(r\) 是轮半径，\(L\) 是两轮间距。

差速底盘的唯一约束是横向不滑（body frame 侧向速度为零）：

\[ v_y^b = 0 \]

转换到 world frame：

\[ -\dot{x}\sin\theta + \dot{y}\cos\theta = 0 \]

Pfaffian 矩阵：

\[ A(q) = \begin{pmatrix} -\sin\theta & \cos\theta & 0 \end{pmatrix} \]

维度分析：\(n = 3\)，\(k = \text{rank}(A) = 1\)，瞬时自由度 = \(3 - 1 = 2\)（前进/后退 + 原地旋转）。

注意差速底盘和单轮圆盘的关键区别：差速底盘有两个独立电机输入（\(\omega_L, \omega_R\)），恰好等于两个瞬时自由度——系统是完全驱动的（在允许速度子空间内）。

运动方程

\[ \dot{q} = \begin{pmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{pmatrix} = \underbrace{\begin{pmatrix} \cos\theta \\ \sin\theta \\ 0 \end{pmatrix}}_{g_1} v + \underbrace{\begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix}}_{g_2} \omega \]

其中 \(v = v_x^b\) 是前向线速度，\(\omega = \dot{\theta}\) 是角速度。

76.3.3 阿克曼转向 ⭐⭐⭐

动机

阿克曼转向（Ackermann steering）是汽车和类车机器人的标准转向方式。与差速底盘不同，阿克曼系统的转弯是通过前轮偏转实现的，而非两轮差速。

配置与约束推导

阿克曼系统的配置需要四个变量：

\[ q = \begin{pmatrix} x \\ y \\ \theta \\ \delta \end{pmatrix} \]

其中 \((x, y)\) 是后轴中点的位置，\(\theta\) 是车体朝向，\(\delta\) 是前轮转向角。

约束同样是后轮不横滑：

\[ v_y^b = 0 \implies -\dot{x}\sin\theta + \dot{y}\cos\theta = 0 \]

但阿克曼运动学引入了额外关系——前轮转向角 \(\delta\) 与转弯半径 \(R\) 的关系：

\[ \tan\delta = \frac{L_{\text{wb}}}{R} \]

其中 \(L_{\text{wb}}\) 是轴距。这给出了偏航角速度与前向速度的关系：

\[ \dot{\theta} = \frac{v}{R} = \frac{v \tan\delta}{L_{\text{wb}}} \]

Pfaffian 矩阵：

\[ A(q) = \begin{pmatrix} -\sin\theta & \cos\theta & 0 & 0 \end{pmatrix} \]

维度分析与差速底盘相同：\(n = 4\)，\(k = 1\)，瞬时自由度 = 3。但实际输入只有 2 个（前向速度 \(v\) 和转向角速率 \(\dot{\delta}\)），因此系统是欠驱动的——\(\delta\) 的变化不受 Pfaffian 约束限制，但受到机械转向角限制 \(|\delta| \leq \delta_{\max}\)。

阿克曼运动方程：

\[ \dot{q} = \begin{pmatrix} \cos\theta \\ \sin\theta \\ \frac{\tan\delta}{L_{\text{wb}}} \\ 0 \end{pmatrix} v + \begin{pmatrix} 0 \\ 0 \\ 0 \\ 1 \end{pmatrix} \dot{\delta} \]

反事实推理：如果阿克曼系统没有最大转向角限制（\(\delta_{\max} = 90°\)），会发生什么？当 \(\delta \to 90°\) 时，\(\tan\delta \to \infty\)，意味着即使前向速度 \(v\) 很小，偏航角速度 \(\dot{\theta}\) 也趋向无穷——车辆绕后轴做极小半径旋转。物理上这等价于差速底盘的原地旋转。但实际汽车的 \(\delta_{\max}\) 通常在 30-40 度，这进一步限制了最小转弯半径，使得停车入库等操作需要多次前进后退。

76.3.4 全向轮（Swedish/Omni Wheel） ⭐⭐

动机

全向底盘（如 Kuka youBot 的底座、RoboCup 小型组机器人）使用特殊设计的轮子消除了横向不滑约束，使机器人能够沿任意方向瞬时运动。

约束分析

全向轮（Swedish wheel / omni wheel）的关键设计：轮缘上安装了自由滚动的小辊子，使得轮子可以沿轴向自由滑动。因此，横向不滑约束被消除。

三轮全向底盘（120 度对称布置）的运动方程：

\[ \begin{pmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{pmatrix} = J^{-1}(\theta) \begin{pmatrix} r\omega_1 \\ r\omega_2 \\ r\omega_3 \end{pmatrix} \]

Pfaffian 约束矩阵：\(A(q) = \emptyset\)——无约束。全向底盘是完整系统。

这意味着：全向底盘可以瞬时沿任意方向运动（前后、左右、旋转的任意组合）。配置空间维度 = 瞬时自由度 = 3。

对比项	差速底盘	阿克曼	全向
Pfaffian 约束数	1	1	0
瞬时自由度	2	2（实际输入）	3
能否瞬时横移	否	否	能
运动规划复杂度	中等	高（最小转弯半径）	低
机械复杂度	低	中	高

76.3.5 麦克纳姆轮 ⭐⭐⭐

动机

麦克纳姆轮（Mecanum wheel）是全向运动的另一种实现方案，常用于仓库搬运机器人和 RoboCup 中型组。与 Swedish wheel 的 90 度辊子不同，麦克纳姆轮的辊子以 45 度角安装。

运动学推导

四轮麦克纳姆底盘（矩形布置）的逆运动学：

\[ \begin{pmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \\ \omega_4 \end{pmatrix} = \frac{1}{r} \begin{pmatrix} 1 & -1 & -(L_x + L_y) \\ 1 & 1 & (L_x + L_y) \\ 1 & 1 & -(L_x + L_y) \\ 1 & -1 & (L_x + L_y) \end{pmatrix} \begin{pmatrix} v_x^b \\ v_y^b \\ \omega \end{pmatrix} \]

其中 \(L_x, L_y\) 是轮子到底盘中心的前后和左右距离。

Pfaffian 约束：与全向轮类似，麦克纳姆底盘没有非完整约束——是完整系统。但实际工程中，辊子与地面的接触力学使得麦克纳姆轮的横向力传递效率低，因此侧向运动的最大加速度远小于前向。这不是运动学约束，而是动力学限制。

类比：麦克纳姆轮之于 Swedish 轮，类似于锥齿轮之于正齿轮——都能实现同样的运动学功能，但通过不同的机械几何。45 度辊子分解力为纵向和横向分量，使得四个轮子的合力可以产生任意方向的平面运动。

代码验证：四种底盘运动学的统一仿真

以下代码将四种底盘的运动学方程封装为统一接口，便于对比仿真：

import numpy as np
import matplotlib.pyplot as plt

class DifferentialDrive:
    """差速底盘运动学。"""
    def __init__(self, wheel_radius=0.05, track_width=0.3):
        self.r = wheel_radius
        self.L = track_width

    def forward_kinematics(self, q, u, dt):
        """
        q = (x, y, theta), u = (v, omega)
        返回: q_next
        """
        x, y, theta = q
        v, omega = u
        # Euler 积分
        x_next = x + v * np.cos(theta) * dt
        y_next = y + v * np.sin(theta) * dt
        theta_next = theta + omega * dt
        return np.array([x_next, y_next, theta_next])

    def pfaffian_residual(self, q, q_dot):
        """计算 Pfaffian 约束残差 A(q) @ q_dot"""
        theta = q[2]
        A = np.array([[-np.sin(theta), np.cos(theta), 0]])
        return A @ q_dot


class AckermannDrive:
    """阿克曼底盘运动学。"""
    def __init__(self, wheel_radius=0.05, wheelbase=0.5):
        self.r = wheel_radius
        self.L_wb = wheelbase

    def forward_kinematics(self, q, u, dt):
        """
        q = (x, y, theta, delta), u = (v, delta_dot)
        返回: q_next
        """
        x, y, theta, delta = q
        v, delta_dot = u
        x_next = x + v * np.cos(theta) * dt
        y_next = y + v * np.sin(theta) * dt
        theta_next = theta + v * np.tan(delta) / self.L_wb * dt
        delta_next = delta + delta_dot * dt
        return np.array([x_next, y_next, theta_next, delta_next])


# 仿真对比：差速车平行泊车 vs 全向底盘直接横移
def simulate_parking():
    diff = DifferentialDrive()

    # 差速车平行泊车轨迹：前进-左转-后退-右转
    q = np.array([0.0, 0.0, 0.0])
    trajectory_diff = [q.copy()]

    maneuvers = [
        (0.3, 0.5, 2.0),    # 前进 + 左转
        (-0.3, -0.5, 2.0),  # 后退 + 右转
        (0.3, 0.5, 2.0),    # 前进 + 左转
        (-0.3, -0.5, 2.0),  # 后退 + 右转
    ]

    dt = 0.01
    for v, omega, duration in maneuvers:
        for _ in range(int(duration / dt)):
            q = diff.forward_kinematics(q, [v, omega], dt)
            trajectory_diff.append(q.copy())

    trajectory_diff = np.array(trajectory_diff)

    fig, ax = plt.subplots(1, 1, figsize=(8, 6))
    ax.plot(trajectory_diff[:, 0], trajectory_diff[:, 1], 'b-', label='差速车')
    ax.plot([0, 0], [0, 1], 'r--', label='全向底盘（直线）')
    ax.set_xlabel('x (m)')
    ax.set_ylabel('y (m)')
    ax.set_title('平行泊车：差速车 vs 全向底盘')
    ax.legend()
    ax.set_aspect('equal')
    ax.grid(True)
    plt.show()

# simulate_parking()  # 取消注释运行

四种底盘的 Pfaffian 矩阵总结

底盘类型	\(A(q)\)	\(\text{rank}(A)\)	瞬时自由度	非完整？
单轮圆盘	\(\begin{pmatrix} \cos\varphi & \sin\varphi & 0 & -r \\ -\sin\varphi & \cos\varphi & 0 & 0 \end{pmatrix}\)	2	2	是
差速	\(\begin{pmatrix} -\sin\theta & \cos\theta & 0 \end{pmatrix}\)	1	2	是
阿克曼	\(\begin{pmatrix} -\sin\theta & \cos\theta & 0 & 0 \end{pmatrix}\)	1	3	是
全向/麦轮	\(\emptyset\)	0	3	否

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：阿克曼模型中混用前轴和后轴参考点 - 错误做法：阿克曼运动学公式中 \((x, y)\) 取前轴中点，但 \(\dot{\theta} = v\tan\delta/L\) 公式是以后轴为参考推导的 - 典型现象：仿真中车辆走的弧线半径与预期不符，低速时误差小但高速时误差显著 - 根本原因：\(\dot{\theta} = v\tan\delta/L\) 中的 \(v\) 是后轴中点的速度。如果 \((x,y)\) 取前轴，需要额外的运动学变换 - 正确做法：统一选择后轴中点作为参考点，或者使用完整的双轮模型

💡 概念误区：认为"全向底盘没有约束，所以运动规划更简单" - 新手想法："全向底盘可以随意横移，路径规划就是直线连接起点和终点" - 实际上：全向底盘虽然没有运动学约束，但仍有动力学约束（最大加速度、轮子打滑限制）和环境约束（避障）。而且全向底盘的横向力传递效率低，实际最大横向加速度通常只有前向的 50-70% - 延伸：运动规划的复杂度不仅来自运动学约束，还来自动力学约束和环境约束

🧠 思维陷阱：从单轮推导直接推广到多轮时忽略轮间耦合 - 新手想法："四个轮子分别满足各自的纯滚动约束就行了" - 实际上：多轮系统的约束是冗余的——四个轮子各有两个约束共 8 个，但 \(SE(2)\) 只有 3 个自由度。因此 8 个约束中只有 1-3 个是独立的，其余是线性相关的。轮间几何关系（轮距、轴距）决定了哪些约束是独立的 - 正确思维：先写出所有轮的约束，然后分析 \(A(q)\) 的秩，只保留独立约束

练习 76.3

[A] ⭐⭐ 为差速底盘推导正运动学：给定 \(\omega_L, \omega_R\)，计算 \((\dot{x}, \dot{y}, \dot{\theta})\)。验证当 \(\omega_L = \omega_R\) 时机器人直行，当 \(\omega_L = -\omega_R\) 时原地旋转。

[B] ⭐⭐⭐ 推导自行车模型（阿克曼的简化）的 Pfaffian 约束矩阵。自行车模型用 \(q = (x, y, \theta, \delta)\) 描述，其中 \((x, y)\) 是后轮接地点。写出完整的 \(A(q)\) 并与汽车的阿克曼矩阵对比。

[C] ⭐⭐⭐ 用 Python + SymPy 符号计算四轮麦克纳姆底盘的正运动学矩阵。验证当四轮同速同向转时机器人前进，当对角轮反向时机器人原地旋转。

---

76.4 可积性与 Frobenius 定理 ⭐⭐⭐

这一节解决什么问题

上一节推导了四种底盘的 Pfaffian 约束。但一个关键问题悬而未决：如何判断一个 Pfaffian 约束是否是非完整的？换句话说，\(A(q)\dot{q} = 0\) 能否积分为 \(\phi(q) = 0\)？Frobenius 定理给出了精确判据。

这个问题不只是数学好奇心——它直接决定了运动规划的策略。如果约束是完整的，机器人的可达空间被降维了，规划在低维流形上进行。如果约束是非完整的，可达空间不变，但每一步的运动方向受约束，规划需要"绕路"。搞错了约束类型会导致规划器给出不可执行的路径。

可积性判定的实际工程流程

1. 写出 Pfaffian 约束：\(A(q)\dot{q} = 0\)
2. 求零空间基向量场：\(g_1, \ldots, g_m\) 使得 \(A g_i = 0\)
3. 计算所有 Lie bracket：\([g_i, g_j]\) 对所有 \(i, j\)
4. 检查 Frobenius 条件：\([g_i, g_j] \in \text{span}(g_1, \ldots, g_m)\)？
5. 如果不可积，进一步计算 LARC 验证可控性
6. 如果可积，找到积分约束 \(\phi(q) = 0\)

这个流程可以完全用 SymPy 自动化，下面的代码框架可以处理任意 Pfaffian 约束。

可积性的含义

如果 Pfaffian 约束 \(A(q)\dot{q} = 0\) 可以积分为位置约束 \(\phi(q) = 0\)，则称该约束是可积的（integrable）或完整的。否则称为不可积的（non-integrable）或非完整的。

跨领域类比：一个向量场是否是某个标量函数的梯度？在微积分中，\(\mathbf{F} = \nabla \phi\) 当且仅当 \(\nabla \times \mathbf{F} = 0\)（无旋条件）。Frobenius 定理是这个条件的高维推广——将"无旋"推广为"对合性"（involutivity）。相似之处：两者都判断一个微分关系是否可以积分为一个有限关系。不同之处：旋度只适用于三维空间中的单个约束，Frobenius 定理适用于任意维度的分布。

可积性的物理意义

从工程角度，可积性决定了机器人的可达性：

约束类型	可积性	对可达位形的影响	运动规划复杂度
完整约束	可积	位形空间被降维——某些位形永远不可达	在降维空间中规划
非完整约束	不可积	位形空间不变——所有位形仍然可达	在完整空间中规划，但路径受约束

本质洞察：非完整约束的"非直觉性"正是它有趣的地方。它限制了你在任何时刻可以移动的方向，但不限制你最终能到达的位置——你只需要"绕路"。差速底盘不能横移，但通过"前进-转弯-前进"的组合可以到达任何位姿。这正是平行泊车的数学本质：汽车（阿克曼转向，非完整）不能侧移，但通过多次前进-倒退可以"蹭"进停车位。如果汽车可以侧移（全向轮，完整），平行泊车就不需要"多点转"了。

Frobenius 定理的陈述

定理（Frobenius）：设分布 \(\mathcal{D}(q) = \text{span}\{g_1(q), \ldots, g_m(q)\}\) 是一个光滑的 \(m\) 维分布。则 \(\mathcal{D}\) 是可积的（即存在 \(n - m\) 个独立函数 \(\phi_1, \ldots, \phi_{n-m}\) 使得 \(\nabla\phi_i \perp \mathcal{D}\)），当且仅当 \(\mathcal{D}\) 是对合的（involutive）：

\[ \forall\, i, j: \quad [g_i, g_j](q) \in \mathcal{D}(q) \]

其中 \([g_i, g_j]\) 是 Lie bracket（李括号）。

Lie bracket 的定义与计算

两个向量场 \(g_i, g_j\) 的 Lie bracket 定义为：

\[ [g_i, g_j](q) = \frac{\partial g_j}{\partial q} g_i(q) - \frac{\partial g_i}{\partial q} g_j(q) \]

直觉理解：Lie bracket \([g_i, g_j]\) 衡量了"先沿 \(g_i\) 走一小步、再沿 \(g_j\) 走一小步"与"先沿 \(g_j\) 走一小步、再沿 \(g_i\) 走一小步"之间的差异。如果两条路径的终点一致（bracket 为零或在 \(\mathcal{D}\) 内），则约束是可积的；如果终点不同（bracket 不在 \(\mathcal{D}\) 内），则说明系统可以通过交替使用两个输入来生成新的运动方向——这正是非完整系统"绕路到达"的数学本质。

差速底盘的可积性验证

让我们用 Frobenius 定理验证差速底盘的约束确实是非完整的。

差速底盘的两个控制向量场（76.3.2 节）：

\[ g_1 = \begin{pmatrix} \cos\theta \\ \sin\theta \\ 0 \end{pmatrix}, \quad g_2 = \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \]

计算 Lie bracket \([g_1, g_2]\)：

首先计算两个雅可比矩阵：

\[ \frac{\partial g_1}{\partial q} = \begin{pmatrix} 0 & 0 & -\sin\theta \\ 0 & 0 & \cos\theta \\ 0 & 0 & 0 \end{pmatrix}, \quad \frac{\partial g_2}{\partial q} = \begin{pmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{pmatrix} \]

\[ [g_1, g_2] = \frac{\partial g_2}{\partial q} g_1 - \frac{\partial g_1}{\partial q} g_2 = \begin{pmatrix} 0 \\ 0 \\ 0 \end{pmatrix} - \begin{pmatrix} -\sin\theta \\ \cos\theta \\ 0 \end{pmatrix} = \begin{pmatrix} \sin\theta \\ -\cos\theta \\ 0 \end{pmatrix} \]

关键检验：\([g_1, g_2] = (\sin\theta, -\cos\theta, 0)^T\)。这个向量是否在 \(\mathcal{D} = \text{span}\{g_1, g_2\}\) 内？

如果是，则需要存在 \(\alpha, \beta\) 使得：

\[ \begin{pmatrix} \sin\theta \\ -\cos\theta \\ 0 \end{pmatrix} = \alpha \begin{pmatrix} \cos\theta \\ \sin\theta \\ 0 \end{pmatrix} + \beta \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \]

从第三行：\(\beta = 0\)。从前两行：\(\alpha\cos\theta = \sin\theta\) 且 \(\alpha\sin\theta = -\cos\theta\)。两式相除得 \(\tan\theta = -1/\tan\theta\)，即 \(\tan^2\theta = -1\)——无实数解。

结论：\([g_1, g_2] \notin \mathcal{D}\)，分布不是对合的，约束不可积，系统是非完整的。

本质洞察：\([g_1, g_2] = (\sin\theta, -\cos\theta, 0)^T\) 恰好是 body frame 的侧向方向。这意味着：通过交替使用前进（\(g_1\)）和转向（\(g_2\)），差速车可以生成侧向运动——这正是平行泊车的数学本质！Lie bracket 产生的新方向，就是系统通过"绕路"可以到达的方向。

几何直觉：Lie bracket 生成新方向的过程

为了建立对 Lie bracket 的几何直觉，考虑差速底盘的以下操作序列：

1. 沿 \(g_1\) 前进 \(\epsilon\)：机器人向前移动 \(\epsilon\)
2. 沿 \(g_2\) 转动 \(\epsilon\)：机器人原地旋转 \(\epsilon\)
3. 沿 \(-g_1\) 后退 \(\epsilon\)：机器人向后移动 \(\epsilon\)
4. 沿 \(-g_2\) 转动 \(-\epsilon\)：机器人原地反转 \(\epsilon\)

如果这四步后机器人回到了起始点（\([g_1, g_2] = 0\)），约束就是完整的。但差速底盘做完这四步后不会回到起点——它会有一个侧向位移 \(O(\epsilon^2)\)。这个侧向位移正是 \([g_1, g_2]\) 的方向——"前进-转弯-后退-反转"的组合产生了原来不存在的侧向运动。

\[ \Delta q \approx \epsilon^2 [g_1, g_2] + O(\epsilon^3) \]

这就是平行泊车的数学本质——通过反复"前进-打方向-后退-回正方向"，汽车逐步侧移进入停车位。每次操作产生 \(O(\epsilon^2)\) 的侧向位移，需要多次重复才能完成。

本质洞察：Lie bracket 不是一个抽象的数学操作——它精确描述了"通过已有运动方向的交替使用，系统能产生什么新的运动方向"。差速底盘的 \([g_1, g_2] = g_3\)（侧向）告诉我们：前进和旋转的交替可以产生侧移。阿克曼转向的 LARC 也成立，但需要更多步操作——这反映了阿克曼转向在狭小空间中停车比差速底盘更困难的物理现实。

Frobenius 定理的实用判据

对于 \(k\) 个约束（\(A \in \mathbb{R}^{k \times n}\)）、\(m = n - k\) 个控制向量场的情况，Frobenius 定理的等价条件可以用约束矩阵 \(A(q)\) 直接判断：

约束形式的 Frobenius 条件：设 \(\omega^i = A^i(q)\) 是 \(A\) 的第 \(i\) 行（约束一形式）。约束可积当且仅当：

\[ d\omega^i \wedge \omega^1 \wedge \cdots \wedge \omega^k = 0, \quad \forall\, i \]

其中 \(d\omega^i\) 是 \(\omega^i\) 的外微分，\(\wedge\) 是楔积。

对于单个约束（\(k = 1\)），条件简化为：

\[ d\omega \wedge \omega = 0 \]

对于差速底盘（\(\omega = -\sin\theta\, dx + \cos\theta\, dy\)）：

\[ d\omega = -\cos\theta\, d\theta \wedge dx - \sin\theta\, d\theta \wedge dy \]

\[ d\omega \wedge \omega = (-\cos\theta\, d\theta \wedge dx - \sin\theta\, d\theta \wedge dy) \wedge (-\sin\theta\, dx + \cos\theta\, dy) \]

\[ = \cos^2\theta\, d\theta \wedge dx \wedge dy + \sin^2\theta\, d\theta \wedge dy \wedge dx \]

\[ = (\cos^2\theta - \sin^2\theta + 2\sin\theta\cos\theta)? \text{...} \]

实际上注意到 \(d\theta \wedge dy \wedge dx = -d\theta \wedge dx \wedge dy\)，所以：

\[ d\omega \wedge \omega = (\cos^2\theta + \sin^2\theta)\, d\theta \wedge dx \wedge dy = d\theta \wedge dx \wedge dy \neq 0 \]

结论一致：\(d\omega \wedge \omega \neq 0\)，约束不可积。

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：用数值方法计算 Lie bracket 时步长选取不当 - 错误做法：用有限差分近似 \(\partial g / \partial q\)，步长取 \(h = 0.1\)（过大） - 典型现象：数值 Lie bracket 与解析结果差异大，特别是在 \(\theta\) 接近 \(0\) 或 \(\pi\) 时 - 根本原因：\(g_1 = (\cos\theta, \sin\theta, 0)^T\) 对 \(\theta\) 的导数是三角函数，有限差分需要小步长才能精确近似 - 正确做法：使用 SymPy 进行符号计算，或使用自动微分（如 JAX 的 jacfwd）

💡 概念误区：认为"Frobenius 定理只是一个理论工具，工程中用不到" - 新手想法："我知道差速车不能横移，不需要 Frobenius 定理来验证" - 实际上：Frobenius 定理在工程中有直接应用——当你设计新型轮系（如球形轮、万向节连接的轮组）时，需要判断新约束是否是非完整的，才能选择正确的运动规划算法。完整系统可以用简单的路径搜索 + 逆运动学，非完整系统必须使用 steering function 或采样规划 - 工程应用：ROS2 nav2 的运动规划器选择（DWA vs TEB vs Lattice planner）正是基于底盘是否满足非完整约束

练习 76.4

[A] ⭐⭐⭐ 用 SymPy 计算单轮圆盘的 Lie bracket \([g_1, g_2]\)（76.3.1 的两个控制向量场），验证它不在分布 \(\mathcal{D} = \text{span}\{g_1, g_2\}\) 内。

[B] ⭐⭐⭐ 考虑一个配置为 \(q = (x, y, \theta)\) 的系统，受约束 \(\dot{x}\sin\theta - \dot{y}\cos\theta + \ell\dot{\theta} = 0\)（\(\ell\) 为常数）。用 Frobenius 定理判断这个约束是否可积。（提示：这是拖车约束）

[C] ⭐⭐⭐⭐ 证明全向底盘的约束是可积的（trivially，因为没有约束）。然后考虑一个全向底盘加上一个拖车：全向底盘的 \((x, y, \theta)\) 无约束，但拖车的角度 \(\phi\) 受到约束。写出完整系统的 Pfaffian 矩阵并判断可积性。

---

76.5 非完整运动规划：Chow 定理与可控性 ⭐⭐⭐

这一节解决什么问题

Frobenius 定理告诉我们差速车的约束是非完整的——瞬时不能横移。但"不能瞬时横移"不等于"不能到达侧方位置"。Chow 定理回答了最关键的问题：非完整系统到底能到达哪些配置？

Lie Algebra Rank Condition (LARC)

回忆差速底盘的两个控制向量场 \(g_1, g_2\) 和它们的 Lie bracket \(g_3 = [g_1, g_2]\)：

\[ g_1 = \begin{pmatrix} \cos\theta \\ \sin\theta \\ 0 \end{pmatrix}, \quad g_2 = \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix}, \quad g_3 = [g_1, g_2] = \begin{pmatrix} \sin\theta \\ -\cos\theta \\ 0 \end{pmatrix} \]

检查 \(\{g_1, g_2, g_3\}\) 的秩：

\[ \text{rank}\begin{pmatrix} \cos\theta & 0 & \sin\theta \\ \sin\theta & 0 & -\cos\theta \\ 0 & 1 & 0 \end{pmatrix} = 3 \quad (\text{处处满秩}) \]

验证：行列式 = \(0 \cdot (\cdots) - 1 \cdot (\cos\theta \cdot (-\cos\theta) - \sin\theta \cdot \sin\theta) + 0 = \cos^2\theta + \sin^2\theta = 1 \neq 0\)。

三个向量场（原始 2 个 + Lie bracket 生成的 1 个）张成了完整的 \(\mathbb{R}^3\) 空间。这就是 Lie Algebra Rank Condition (LARC) 满足的标志。

从 Frobenius 到 Chow：理论路径

Frobenius 定理回答了"约束是否非完整"。紧接着的问题是：如果约束是非完整的，系统是否可控？Chow 定理给出了肯定的条件。

定理	回答的问题	条件	结论
Frobenius	约束是否可积？	Lie bracket 在分布内	约束是完整的（可积）
Chow	系统是否可控？	Lie bracket 生成全空间	系统完全可控

反事实推理：如果一个系统既有非完整约束又不满足 Chow 条件会怎样？这意味着某些位形不可达——系统被困在位形空间的低维子流形上。实际工程中，纯轮式机器人通常满足 Chow 条件，但加入拖车的铰接约束后可能出现可控性退化。因此在设计轮足系统的约束时，验证 LARC 是必要的工程步骤。

Chow 定理

定理（Chow, 1939; Rashevsky, 1938）：设控制系统 \(\dot{q} = \sum_{i=1}^m u_i g_i(q)\)，其中 \(g_1, \ldots, g_m\) 是光滑向量场。如果在每一点 \(q\) 处，\(g_1, \ldots, g_m\) 及其所有阶 Lie bracket 生成的 Lie algebra 的维度等于配置空间维度 \(n\)（即 LARC 成立），则系统是小时间局部可控的（small-time locally controllable, STLC）——从任意配置出发，可以在任意短时间内到达其邻域内的任意配置。

对差速底盘的意义：

• \(g_1, g_2\) 只生成 2 维分布（不含横向）
• \(g_1, g_2, [g_1, g_2]\) 生成 3 维分布（包含横向）——填满整个 \(SE(2)\)
• LARC 满足 \(\Rightarrow\) 差速车可以从任意配置到达任意配置

本质洞察：Chow 定理是"绕路到达"的数学保证。\([g_1, g_2]\) 代表的横向运动方向虽然不能通过单个输入直接产生，但可以通过交替使用 \(g_1\) 和 \(g_2\) 来逼近。这就像停车时的前进-转弯-后退-回正序列——每一步都在允许速度空间内，但序列的净效果是横向位移。

可控性秩条件的层次

对于更复杂的系统，LARC 可能需要多阶 Lie bracket：

阶数	向量场	生成的新方向
0 阶	\(g_1, g_2\)	前进/后退, 旋转
1 阶	\([g_1, g_2]\)	横向平移
2 阶	\([g_1, [g_1, g_2]], [g_2, [g_1, g_2]]\)	更高阶运动
\(k\) 阶	\([\cdots]\)	更精细的运动方向

需要 1 阶 Lie bracket 的系统（差速车）比需要多阶 Lie bracket 的系统（如带拖车的卡车）在运动规划上更容易——因为"绕路"的路径更短。

反事实推理：如果 Lie bracket 不产生新方向（即分布是对合的/完整的），会怎样？那就意味着系统被约束在一个低维子流形上——类似于质点被约束在球面上。约束减少了可达配置集合的维度。这正是完整约束和非完整约束的根本区别：完整约束让配置空间"塌缩"，非完整约束保持配置空间维度不变，只限制到达某些配置的路径。

可控性等级的物理意义

LARC 中 Lie bracket 的阶数有直接的运动学含义：

Lie bracket 阶数	运动学含义	示例
0 阶（原始向量场）	直接可控的运动方向	差速车的前进和原地转
1 阶（\([g_i, g_j]\)）	通过交替使用两个输入产生的新方向	差速车的侧移（通过前进+转弯组合）
2 阶（\([g_i, [g_j, g_k]]\)）	需要更复杂的输入序列才能产生的方向	拖车的某些自由度
\(k\) 阶	需要 \(k+1\) 步交替操作	高阶非完整系统

可控性等级直接决定了运动规划的"机动次数"——等级为 1 的系统（如差速底盘）只需要 2 步操作（前进+转弯）就能产生侧移效果；等级为 2 的系统可能需要 3 步以上。

跨领域类比：可控性等级类似于魔方的"解法步数"。一步可以直接完成的操作（如旋转一个面）对应 0 阶可控；需要"先做 A 再做 B 才能实现 C"的操作对应 1 阶；需要更复杂的序列才能实现的操作对应更高阶。差速底盘的可控性等级为 1，意味着侧移"只需要前进+转弯两步组合"——像解魔方中的简单组合。阿克曼转向的可控性等级也是 1，但最小转弯半径的约束让"组合"需要更大的空间。

运动规划的实际影响

Chow 定理的理论结果直接影响运动规划器的设计：

系统类型	LARC	可控性	规划方法
完整系统（全向底盘）	自然满足	沿任意方向瞬时可控	A*, RRT, 任意路径搜索
非完整 + LARC 满足	1-2 阶	可控但需要绕路	Hybrid A*, RRT + steering, lattice
非完整 + LARC 不满足	不满足	不完全可控	需要分析可达集

⚠️ 常见陷阱与误区

💡 概念误区：认为 Chow 定理保证了"快速到达" - 新手想法："差速车满足 LARC，所以可以快速到达任意位置" - 实际上：Chow 定理只保证存在到达的路径，不保证路径的长度或时间。通过 Lie bracket 生成的运动方向（横移）需要多次来回机动才能实现，效率远低于直接运动的方向。差速车横移 1 米需要的路径长度远大于前进 1 米 - 延伸：路径效率与 Lie bracket 的阶数有关——阶数越高，"绕路"越长，运动规划越困难

🧠 思维陷阱：把 LARC 当作设计指标 - 新手想法："只要 LARC 满足，底盘设计就没问题" - 实际上：LARC 是可控性的必要条件的充分条件。实际底盘设计还需要考虑到达效率、能耗、机械复杂度等因素。例如，全向底盘不需要 Lie bracket 就能横移，但机械更复杂、负载能力更低 - 正确思维：LARC 是理论保证，工程选择需要综合权衡运动学灵活性、动力学性能和机械可靠性

SymPy 符号验证

以下代码用 SymPy 自动化地验证差速底盘的 LARC。这不仅是一个验证工具，也是读者将来分析新型底盘可控性时的模板：

import sympy as sp

# 定义符号
x, y, theta = sp.symbols('x y theta')
q = sp.Matrix([x, y, theta])

# 定义控制向量场
g1 = sp.Matrix([sp.cos(theta), sp.sin(theta), 0])  # 前进
g2 = sp.Matrix([0, 0, 1])                            # 旋转

def lie_bracket(f, g, q_vars):
    """
    计算两个向量场的 Lie bracket [f, g]。
    [f, g] = dg/dq * f - df/dq * g
    """
    n = len(q_vars)
    df_dq = sp.Matrix([[sp.diff(f[i], q_vars[j]) for j in range(n)] for i in range(n)])
    dg_dq = sp.Matrix([[sp.diff(g[i], q_vars[j]) for j in range(n)] for i in range(n)])
    return sp.simplify(dg_dq * f - df_dq * g)

# 计算 [g1, g2]
g3 = lie_bracket(g1, g2, q)
print("g1 =", g1.T)
print("g2 =", g2.T)
print("[g1, g2] =", g3.T)
# 输出: [g1, g2] = [sin(theta), -cos(theta), 0]

# 检查 LARC：{g1, g2, [g1,g2]} 的秩
rank_matrix = sp.Matrix([g1.T, g2.T, g3.T]).T
rank = rank_matrix.rank()
print(f"LARC 矩阵的秩 = {rank}, 配置空间维度 = 3")
print(f"LARC 满足: {rank == 3}")
# 输出: LARC 满足: True

# 验证行列式
det = rank_matrix.det()
print(f"行列式 = {sp.simplify(det)}")
# 输出: 行列式 = 1（处处非零！）

这段代码的输出确认了我们手工推导的结论：\([g_1, g_2] = (\sin\theta, -\cos\theta, 0)^T\) 与 \(g_1, g_2\) 一起生成完整的 \(\mathbb{R}^3\)。

读者练习：将上述代码修改为分析单轮圆盘（4 维配置空间，2 个控制向量场），验证 LARC 满足需要几阶 Lie bracket。

可控性等级与运动规划效率

LARC 满足只保证可控性存在，但不同系统"绕路"的效率差异巨大。Sub-Riemannian 几何中的 Ball-Box 定理 给出了量化关系：

如果系统需要 \(k\) 阶 Lie bracket 才能生成某个方向 \(d\)，那么在该方向上移动距离 \(\epsilon\) 所需的最短路径长度约为 \(O(\epsilon^{1/k})\)。

系统	LARC 阶数	横移 \(\epsilon\) 米所需路径长度	规划难度
全向底盘	0（无需 Lie bracket）	\(\epsilon\)（直线）	低
差速底盘	1	\(O(\sqrt{\epsilon})\)	中
阿克曼底盘	1（有最小转弯半径）	\(O(\sqrt{\epsilon})\) 但下界更大	高
拖车系统	2	\(O(\epsilon^{1/3})\)	很高

这解释了一个直觉事实：差速车倒车入库比全向底盘难得多（需要多次前进后退），而拖着拖车倒车入库更是极其困难（需要更多次的精细机动）。

练习 76.5

[A] ⭐⭐⭐ 对阿克曼底盘计算所有需要的 Lie bracket，验证 LARC 是否满足。如果满足，需要几阶 Lie bracket？

[B] ⭐⭐⭐ 考虑一辆差速车拖着一个被动拖车（tractor-trailer system）。配置 \(q = (x, y, \theta_1, \theta_2)\)，其中 \(\theta_1\) 是拖车朝向。写出控制向量场并验证 LARC 是否满足。需要几阶 Lie bracket？

[C] ⭐⭐⭐⭐ （开放题）如果一个系统的 LARC 需要 \(k\) 阶 Lie bracket 才能满足，直觉上"绕路"到达侧向位置需要多少倍于直线距离的路径长度？（提示：研究 sub-Riemannian geometry 中的 Ball-Box 定理）

---

76.6 滑移建模：当纯滚动假设失效 ⭐⭐⭐

这一节解决什么问题

前面五节的所有推导都基于一个关键假设：纯滚动 + 不横滑。但在低摩擦地面（如湿滑地砖、松软沙土）、高加速度转弯或紧急制动时，这个假设会失效。本节介绍如何量化"滑移程度"，并简要介绍 Pacejka 轮胎力模型。

为什么纯滚动假设会失效

前面五节的所有推导都建立在"纯滚动"假设上——轮子与地面的接触点没有滑动。这个假设在以下条件下近似成立：

• 低速运动（< 1 m/s）
• 干燥硬地面（混凝土、瓷砖）
• 轻载（接触力远小于摩擦极限）
• 直线运动（无急转弯）

但在实际轮足机器人运行中，以下场景会让纯滚动假设严重失效：

场景	失效原因	纵向滑移率	侧向滑移角
湿滑地面	摩擦系数下降至 0.2-0.4	5-20%	5-15°
急加速/急刹车	轮胎力矩超过摩擦极限	10-30%	< 5°
急转弯	侧向力超过摩擦极限	< 5%	10-30°
松软地面（草地/泥土）	轮胎沉入地面	5-15%	5-10°
高速运动（> 2 m/s）	气动效应和离心力	3-10%	3-8°

本质洞察：Pfaffian 约束 \(A(q)\dot{q} = 0\) 描述的是"理想运动学"——它假设轮子和地面是刚性接触且无滑动。真实世界中轮胎是弹性体，接触区域有有限面积，轮胎力通过变形产生而非理想约束。滑移建模就是在 Pfaffian 框架中引入"约束的偏差"——从 \(A(q)\dot{q} = 0\) 到 \(A(q)\dot{q} = b(q, \sigma)\)，其中 \(b\) 依赖于滑移参数 \(\sigma\)。

两个滑移参数

滑移用两个无量纲参数描述：

纵向滑移比（longitudinal slip ratio）\(\kappa\)：

\[ \kappa = \frac{r\omega - v_x}{\max(|v_x|, v_{\text{low}})} \]

\(\kappa\) 值	物理含义
\(\kappa = 0\)	纯滚动，\(r\omega = v_x\)
\(\kappa > 0\)	驱动打滑（轮缘速度 > 地面速度），如起步时轮胎空转
\(\kappa < 0\)	制动打滑（地面速度 > 轮缘速度），如急刹车
\(\kappa = 1\)	完全打滑，轮子空转（\(v_x = 0\) 但 \(\omega \neq 0\)）
\(\kappa = -1\)	完全抱死，轮子锁死（\(\omega = 0\) 但 \(v_x \neq 0\)）

注意：分母中的 \(v_{\text{low}}\) 是一个小正数（典型值 0.05-0.1 m/s），用于避免低速时除以零。这个低速阈值在工程中非常重要——低速时滑移比会因为分母接近零而剧烈波动，失去物理意义。

侧向滑移角（lateral slip angle）\(\alpha\)：

\[ \alpha = \arctan\left(\frac{v_y}{|v_x| + v_{\text{low}}}\right) \]

\(\alpha\) 值	物理含义
\(\alpha = 0\)	无侧滑，轮子沿前向行驶
\(\alpha > 0\)	轮子向左侧滑
\(\alpha < 0\)	轮子向右侧滑

Pacejka 魔术公式简介

Hans Pacejka（1934-2017）提出的"Magic Formula"是汽车工业中最广泛使用的轮胎力模型。它被称为"魔术"公式是因为没有直接的物理推导——它是一个纯经验拟合公式，但惊人地适用于各种轮胎和工况。

简化 Pacejka 公式（纯纵向或纯侧向）：

\[ F = F_z \cdot D \cdot \sin\left(C \cdot \arctan\left(B \cdot s - E \cdot (B \cdot s - \arctan(B \cdot s))\right)\right) \]

其中 \(s\) 是滑移参数（纵向为 \(\kappa\)，侧向为 \(\alpha\)），\(B, C, D, E\) 是四个经验参数：

参数	名称	物理含义
\(B\)	刚度因子	小滑移时的力-滑移斜率
\(C\)	形状因子	曲线形状（通常 \(C \approx 1.3\) 纵向，\(C \approx 1.5\) 侧向）
\(D\)	峰值因子	最大摩擦系数 \(\mu_{\max}\)
\(E\)	曲率因子	峰值后的下降速率

简化摩擦锥约束：

无论使用多复杂的轮胎模型，接触力始终受到摩擦锥限制：

\[ \sqrt{F_x^2 + F_y^2} \leq \mu F_z \]

这是所有轮式接触约束的最终边界。

滑移参数与约束违反的关系

纵向滑移率 \(\kappa\) 和侧向滑移角 \(\alpha\) 直接量化了 Pfaffian 约束的违反程度。回忆纯滚动 Pfaffian 约束 \(A(q)\dot{q} = 0\)。当存在滑移时：

\[ A(q)\dot{q} = r(\kappa, \alpha) \neq 0 \]

约束残差 \(r\) 与滑移参数的关系是：

残差分量	物理含义	与滑移参数的关系
纵向残差 \(r_{lon}\)	轮子旋转速度与地面速度的差	\(r_{lon} = R\dot{\phi} \cdot \kappa\)（线性近似）
侧向残差 \(r_{lat}\)	轮子侧向滑动速度	\(r_{lat} = v \cdot \sin\alpha\)（小角近似）

当 \(\kappa = 0, \alpha = 0\) 时，\(r = 0\)——回到理想的纯滚动。当滑移增大时，约束残差增大，纯滚动运动学模型的预测越来越不准确。

滑移对导航定位的影响

基于纯滚动假设的轮式里程计（wheel odometry）在有滑移时会积累定位误差。误差增长率近似为：

\[ \dot{e}_{pos} \approx v \cdot \sqrt{\kappa^2 + \alpha^2} \]

对于 \(v = 1\) m/s, \(\kappa = 0.05, \alpha = 3°\)，\(\dot{e}_{pos} \approx 0.07\) m/s——即每秒 7 cm 的定位漂移。在 10 秒内就会积累 70 cm 的误差——对于配送机器人在门口精确放置包裹来说完全不可接受。

这就是为什么实际导航系统不能只依赖轮式里程计——必须融合 IMU、视觉和/或 LiDAR 来补偿滑移导致的定位误差。本章 76.6 节的滑移建模为里程计误差模型提供了数学基础，导航系统可以根据估计的滑移参数动态调整里程计的信任权重。

反事实推理：如果在光滑大理石地面上不考虑滑移使用纯轮式里程计会怎样？大理石地面的摩擦系数可能低至 0.3-0.5，急转弯时侧向滑移角可达 10-20 度。10 秒的行驶可能积累 1-2 m 的定位误差。在室内配送场景中，这可能导致机器人走错走廊或撞到墙壁。融合 LiDAR SLAM 后，即使轮式里程计完全不准，定位精度仍能保持在 5-10 cm——但 SLAM 的计算开销和传感器成本都显著增加。

Pacejka 曲线的物理理解

Pacejka 曲线的典型形状是一条"S 形"曲线，具有以下特征区域：

区域	滑移范围	物理行为	控制意义
线性区	$	s	< 0.03$
峰值区	$	s	\approx 0.05\(-\)0.1$
滑动区	$	s	> 0.15$

对于轮足机器人，通常不需要精确拟合 Pacejka 参数——因为轮子很小（半径 \(\sim 0.05\) m），接触面积小，轮胎力学与汽车差异大。但力-滑移曲线的定性形状仍然适用：小滑移时力线性增长，大滑移时力饱和甚至下降。

线性轮胎模型的适用范围：

对于轮足机器人的控制器设计，一个简化的线性模型通常足够：

\[ F_x = C_\kappa \cdot \kappa, \quad F_y = C_\alpha \cdot \alpha \]

其中 \(C_\kappa\) 是纵向刚度（典型值 \(\sim 500\)-\(2000\) N/rad），\(C_\alpha\) 是侧偏刚度（典型值 \(\sim 300\)-\(1500\) N/rad）。这个模型在 \(|\kappa| < 0.05\) 和 \(|\alpha| < 3°\) 的范围内精度足够。

滑移参数的在线估计方法

实时估计滑移参数对轮足导航和控制至关重要。两种主要方法：

方法一：基于 IMU-轮速差异的直接估计

纵向滑移可以通过对比 IMU 速度和轮速推算速度来估计：

\[ \hat{\kappa} = 1 - \frac{v_{IMU}}{R\omega_{wheel}} \]

侧向滑移可以通过 IMU 的侧向加速度估计：

\[ \hat{\alpha} \approx \arctan\left(\frac{a_{lateral}}{a_{longitudinal}}\right) \]

这些估计在稳态运动中准确，但在急加速/减速时因为 IMU 的惯性效应而有偏差。

方法二：基于卡尔曼滤波的融合估计

将滑移参数作为扩展状态纳入 EKF，通过融合 IMU、编码器和视觉里程计来联合估计位姿和滑移。这种方法在动态条件下更鲁棒，但计算复杂度更高。

滑移在轮足控制中的角色

对于轮足机器人，滑移不是简单的"异常状态"，而是需要在控制回路中主动管理的物理量。具体来说：

模块	滑移的用途
状态估计	滑移大时增大轮速观测的协方差——减小对轮里程计的信任
MPC	滑移大时软化纯滚动约束——从等式约束变为惩罚项
WBC	滑移大时减小分配给该轮的法向力——防止进一步打滑
安全监控	滑移超阈值时限制最大速度——触发降级策略

反事实推理：如果控制器完全忽略滑移，假设轮子永远满足纯滚动，会怎样？在高摩擦干燥地面上也许没问题。但一旦进入低摩擦表面（湿地、草地、碎石），纯滚动假设给出的轮速观测与实际地面速度不符，状态估计会产生漂移。估计漂移传给 MPC，MPC 产生错误的力/速度参考，WBC 执行错误的力矩——形成正反馈环路，最终可能导致失控。这正是为什么滑移检测和闭环使用如此重要。

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：低速时直接使用滑移比公式 - 错误做法：\(\kappa = (r\omega - v_x) / v_x\)，不加低速保护 - 典型现象：机器人低速启动时滑移比突然飙到 \(\pm 100\)，触发安全保护或导致控制器震荡 - 根本原因：当 \(v_x \to 0\) 时分母趋零，数值爆炸 - 正确做法：使用 \(\max(|v_x|, v_{\text{low}})\) 作为分母，并在 \(|v_x| < v_{\text{low}}\) 时切换为直接使用速度残差 \(r\omega - v_x\) 而非比值

💡 概念误区：认为 Pacejka 模型是轮足机器人必须使用的轮胎模型 - 新手想法："Pacejka 是最好的轮胎模型，所以轮足机器人应该用它" - 实际上：Pacejka 模型有 6-20 个经验参数，需要大量轮胎测试数据来标定。轮足机器人的"轮胎"通常是橡胶包覆的小轮，与汽车轮胎差异巨大。实际工程中，轮足机器人更常用线性力-滑移模型（\(F = k \cdot s\)，\(s\) 为滑移参数）或简单摩擦锥（\(|F_t| \leq \mu F_n\)），因为参数少、可标定、计算快 - 正确选择：根据应用精度需求选择模型复杂度。户外高速场景可考虑简化 Pacejka，室内低速场景用线性模型足够

练习 76.6

[A] ⭐⭐ 实现一个 Python 函数 slip_ratio(v_wheel, v_ground, v_low=0.05)，计算带低速保护的纵向滑移比。测试 \(v_{\text{ground}} = 0.01, 0.1, 1.0\) m/s 和 \(v_{\text{wheel}} = 0, 0.5, 1.0, 2.0\) m/s 的各种组合，画出滑移比热力图。

[B] ⭐⭐⭐ 用 Pacejka 简化公式（\(B=10, C=1.5, D=0.8, E=-0.5\)）绘制侧向力 vs 滑移角曲线。标出峰值摩擦系数对应的滑移角。讨论：为什么峰值不在最大滑移角处？

[C] ⭐⭐⭐ 设计一个"滑移感知权重调度器"：输入为四轮的滑移比，输出为四个权重 \(w_i \in [0, 1]\)，用于调节状态估计器对每个轮速观测的信任度。要求 \(w_i\) 在 \(|\kappa_i| < 0.05\) 时为 1，在 \(|\kappa_i| > 0.3\) 时为 0，中间平滑过渡。

---

76.7 轮足混合运动学：足+轮统一 Pfaffian ⭐⭐⭐

这一节解决什么问题

前面六节分别讨论了纯轮式底盘的 Pfaffian 约束和轮胎滑移。本节将它们统一到轮足混合系统中——同一台机器人可以用轮子滚动也可以用腿走路，甚至可以同时使用。统一 Pfaffian 框架让我们能在一个数学对象中描述所有运动模式。

为什么需要统一框架

考虑一台轮足机器人从平路滚动到台阶行走的过程。在模式切换瞬间，运动学约束发生变化——滚动模式有 4 个轮子的纯滚动约束，行走模式有 0-4 个脚底的接触约束（取决于有几条腿在支撑）。如果没有统一框架，每种模式需要单独的运动学模型，切换时需要重新初始化——这会导致瞬时的不连续和控制抖动。

统一 Pfaffian 框架的核心思想是：把所有模式的约束写成一个参数化矩阵 \(A(q, m)\)，其中 \(m\) 是模式参数。模式切换只是 \(m\) 的变化，约束矩阵平滑变化（或通过插值平滑），运动学模型始终保持一致。

前面所有推导都是针对纯轮式系统的。但轮足机器人（如 ANYmal-on-wheels、Swiss-Mile 的产品）的每个末端可以在足模式（foot）、轮模式（wheel）和摆动模式（swing）之间切换。本节把这三种接触模式统一到同一个 Pfaffian 约束框架中，为下一章（复合/70_轮足混合MPC）打下基础。

从复合/20 桥接

回顾复合/20_浮动基座臂统一动力学：浮动基座系统的动力学方程为 \(M(q)\dot{v} + h(q,v) = S^T\tau + J_c^T\lambda\)。其中接触雅可比 \(J_c\) 描述了接触点速度与广义速度的关系：\(v_c = J_c(q)v\)。

在纯足式系统中，接触约束是 \(J_c(q)v = 0\)——接触点世界系速度为零。这是一个完整约束（可积分为"接触点位置固定"）。

在轮足系统中，轮模式的接触约束不再是速度为零，而是速度方向受限：

\[ \text{足模式}: \quad v_c = J_c(q)v = 0 \quad (\text{三维速度为零，完整约束}) \]

\[ \text{轮模式}: \quad \begin{cases} e_\perp^T J_c(q) v = 0 & (\text{横向不滑}) \\ e_\parallel^T J_c(q) v - r\dot{\theta} = 0 & (\text{纵向纯滚动}) \\ n^T J_c(q) v = 0 & (\text{法向不穿透}) \end{cases} \]

\[ \text{摆动模式}: \quad \text{无接触约束} \]

模式相关的统一约束矩阵

设第 \(i\) 个末端的接触点速度为 \(v_{c,i} = J_{c,i}(q)v\)，模式为 \(m_i \in \{\text{swing}, \text{foot}, \text{wheel}\}\)。定义模式相关的约束矩阵 \(A_i^{m_i}\)：

\[ A_i^{\text{swing}} = \emptyset \quad (\text{无约束行}) \]

\[ A_i^{\text{foot}} = J_{c,i}(q) \quad (\text{3 行约束}) \]

\[ A_i^{\text{wheel}} = \begin{pmatrix} e_{\perp,i}^T J_{c,i}(q) \\ e_{\parallel,i}^T J_{c,i}(q) - r_i [0 \cdots 1_{\theta_i} \cdots 0] \\ n_i^T J_{c,i}(q) \end{pmatrix} \quad (\text{3 行约束，但结构不同}) \]

其中 \([0 \cdots 1_{\theta_i} \cdots 0]\) 是一个行向量，在轮转角对应的速度索引位置为 1，其余为 0。

统一 Pfaffian 矩阵：将所有末端的约束垂直堆叠：

\[ A_{\text{unified}}(q, \sigma) = \begin{pmatrix} A_1^{m_1} \\ A_2^{m_2} \\ A_3^{m_3} \\ A_4^{m_4} \end{pmatrix} \]

其中 \(\sigma = (m_1, m_2, m_3, m_4)\) 是当前模式向量。

关键观察：统一 Pfaffian 矩阵的结构随模式切换而变化——行数、非零模式都不同。这种结构变化正是轮足 MPC（复合/70）面临的核心数学困难。

模式组合	约束行数	瞬时自由度（\(n_v = 24\) 的 Go2+wheels）
4 foot	12	12
4 wheel	12	12（但约束结构不同）
2 foot + 2 wheel	12	12
2 foot + 2 swing	6	18
4 swing（空中）	0	24

局部坐标系的构造

轮模式约束需要在每个接触点定义局部坐标系 \((e_\parallel, e_\perp, n)\)。这三个方向的来源是：

• 法向 \(n\)：由地形表面法向决定。平地上 \(n = (0, 0, 1)^T\)，坡面上由地形估计给出
• 前向 \(e_\parallel\)：轮平面朝向在地面切平面上的投影。必须先投影到切平面再归一化
• 横向 \(e_\perp\)：\(e_\perp = n \times e_\parallel\)

注意：在非平坦地面上，\(e_\parallel\) 不能直接取车体前向——必须先投影到地形切平面。否则约束方程中会出现法向分量的泄漏，导致残差偏置。

import numpy as np

def build_contact_frame(body_forward, terrain_normal):
    """
    从车体前向和地形法向构造接触点局部坐标系。

    参数:
        body_forward: 车体前向方向 (3,)，世界系
        terrain_normal: 地形表面法向 (3,)，世界系，朝上
    返回:
        e_par: 滚动方向（前向投影到切平面）
        e_perp: 横向方向
        n: 法向方向
    """
    n = terrain_normal / np.linalg.norm(terrain_normal)

    # 将车体前向投影到地形切平面
    e_par = body_forward - np.dot(body_forward, n) * n
    norm_par = np.linalg.norm(e_par)
    if norm_par < 1e-6:
        raise ValueError("车体前向与地形法向平行，无法构造坐标系")
    e_par = e_par / norm_par

    # 横向 = 法向 x 前向
    e_perp = np.cross(n, e_par)
    e_perp = e_perp / np.linalg.norm(e_perp)

    return e_par, e_perp, n

完整的残差计算代码

以下代码给出可复用的滚动残差模块——它是本章理论落地到代码的核心产物，也是后续 MPC（复合/70）和 RL（复合/80）都会复用的基础组件：

import numpy as np

def rolling_residual(jac_contact, v_generalized, wheel_speed, radius, 
                     forward, lateral, normal):
    """
    计算轮接触点的三维滚动残差。

    参数:
        jac_contact: 接触点雅可比 (3, nv) 或 (6, nv)，取前 3 行
        v_generalized: 广义速度 (nv,)
        wheel_speed: 轮角速度 (rad/s)，标量
        radius: 轮半径 (m)，标量
        forward: 滚动方向（前向），已投影到切平面 (3,)
        lateral: 横向方向 (3,)
        normal: 地面法向 (3,)
    返回:
        residual: (3,) = [r_roll, r_lat, r_normal]
        - r_roll: 纵向纯滚动残差（应为 0）
        - r_lat:  横向侧滑残差（应为 0）
        - r_normal: 法向穿透残差（应为 0）
    """
    # 取雅可比的线性部分（如果是 6xnv，取前 3 行）
    if jac_contact.shape[0] == 6:
        jac_contact = jac_contact[:3, :]

    # 接触点线速度 (3,)
    v_contact = jac_contact @ v_generalized

    # 归一化方向向量
    def safe_normalize(v):
        n = np.linalg.norm(v)
        if n < 1e-9:
            raise ValueError(f"方向向量长度过小: {n}")
        return v / n

    e_fwd = safe_normalize(forward)
    e_lat = safe_normalize(lateral)
    e_nrm = safe_normalize(normal)

    # 三维残差
    r_roll = e_fwd @ v_contact - radius * wheel_speed  # 纵向匹配
    r_lat = e_lat @ v_contact                           # 横向不滑
    r_normal = e_nrm @ v_contact                        # 法向不穿透

    return np.array([r_roll, r_lat, r_normal])


def mode_aware_constraint(jac_contact, v_generalized, wheel_speed, radius,
                          forward, lateral, normal, mode):
    """
    根据模式返回对应的约束残差。

    参数:
        mode: 'swing' | 'foot' | 'wheel'
    返回:
        residual: 约束残差向量（维度取决于模式）
        - swing: 空向量 (0,)
        - foot: (3,) = J_c @ v（三维速度为零）
        - wheel: (3,) = [r_roll, r_lat, r_normal]
    """
    if mode == 'swing':
        return np.array([])

    if jac_contact.shape[0] == 6:
        jac_contact = jac_contact[:3, :]

    if mode == 'foot':
        # 足模式：接触点三维速度为零
        return jac_contact @ v_generalized

    elif mode == 'wheel':
        # 轮模式：Pfaffian 约束
        return rolling_residual(
            jac_contact, v_generalized, wheel_speed, radius,
            forward, lateral, normal
        )
    else:
        raise ValueError(f"未知模式: {mode}")

单元测试思路：

1. 构造一个满足纯滚动约束的广义速度（即 \(v_x^b = r\omega\)，\(v_y^b = 0\)），验证残差为零
2. 故意修改轮半径（减小 5%），验证 \(r_{\text{roll}}\) 出现系统偏置
3. 故意反转轮轴方向，验证 \(r_{\text{roll}}\) 符号反转
4. 在坡面上不做切平面投影，验证 \(r_n\) 出现偏置

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：轮模式仍使用足模式的 \(J_c v = 0\) 约束 - 错误做法：模式切换为 wheel 后，约束矩阵不更新 - 典型现象：机器人在轮模式下无法滚动——优化器试图让轮接触点速度为零，与电机驱动的轮转速矛盾，产生力矩尖峰 - 根本原因：足模式要求 \(v_c = 0\)（三维速度为零），轮模式允许纵向速度 \(e_\parallel^T v_c = r\dot{\theta}\) - 正确做法：模式切换时同步更新约束矩阵的行数和结构

💡 概念误区：认为轮足切换只是"约束矩阵换一行" - 新手想法："足到轮的切换只需要把 \(J_c v = 0\) 改成 \(e_\perp^T J_c v = 0\)" - 实际上：切换涉及多个层面的变化——约束矩阵结构、MPC 代价函数权重、WBC 任务优先级、状态估计器的观测模型。任何一层不同步都会导致切换瞬间的力矩尖峰或状态跳变 - 正确做法：模式切换消息必须带时间戳，所有模块（MPC、WBC、估计器）必须在同一控制周期切换

🧠 思维陷阱：在坡面上直接用世界系 forward 作为滚动方向 - 新手想法："滚动方向就是车头方向，取车体前向就行" - 实际上：在 8 度坡面上，如果不做切平面投影，法向残差 \(r_n = n^T J_c v\) 会出现系统性偏置（因为"前向"有法向分量）。这个偏置被估计器当作法向穿透，触发虚假的接触力 - 正确做法：所有方向向量必须先投影到地形切平面，再归一化

练习 76.7

[A] ⭐⭐ 写出一个四轮轮足机器人在"前两轮 wheel + 后两腿 foot"模式下的统一 Pfaffian 矩阵的维度和结构（用分块矩阵表示）。

[B] ⭐⭐⭐ 实现 rolling_residual 函数：输入 Pinocchio 的接触雅可比、广义速度、轮速、半径和局部坐标系，输出三维残差向量 \((r_{\text{roll}}, r_{\text{lat}}, r_n)\)。用单位测试验证：当广义速度满足纯滚动约束时，残差为零。

[C] ⭐⭐⭐ 讨论：当模式从 foot 切换到 wheel 时，约束矩阵的秩是否改变？如果秩不变，约束空间的几何结构有何变化？用零空间的基底来解释。

---

76.8 工程实现：ROS2 nav2 运动学插件接口 ⭐⭐

这一节解决什么问题

前七节建立了完整的理论框架。本节将理论落地到最常用的移动机器人软件栈——ROS2 Navigation2（nav2），展示 Pfaffian 约束如何映射到 nav2 的运动学插件接口。

为什么需要标准化的运动学插件接口

每种底盘类型的运动学不同——差速底盘接受 \((v, \omega)\)，阿克曼接受 \((v, \phi)\)，全向底盘接受 \((v_x, v_y, \omega)\)。如果导航规划器为每种底盘硬编码运动学，换一种底盘就要重写规划器——这违反了软件工程的"开闭原则"（对扩展开放，对修改关闭）。

nav2 的解决方案是定义一个标准化的运动学插件接口：

\[ \text{Plugin}: (q, u) \to \dot{q} \]

规划器只需要调用 getForwardKinematics(q, u) 得到状态变化率，不需要知道底盘的具体类型。添加新底盘只需实现这个接口——规划器代码不变。

这与本章 Pfaffian 约束的理论完美对应：

\[ \dot{q} = G(q) u \quad \Leftrightarrow \quad \text{getForwardKinematics}(q, u) = G(q) \cdot u \]

其中 \(G(q)\) 是 Pfaffian 约束零空间的基矩阵，\(u\) 是降维后的控制输入。每种底盘类型的 \(G(q)\) 不同，但接口形式统一。

底盘类型	\(u\) 维度	\(G(q)\) 矩阵	输入含义
差速	2	\([\cos\theta, \sin\theta, 0; 0, 0, 1]^T\)	\((v, \omega)\)
阿克曼	2	\([\cos\theta, \sin\theta, \tan\phi/L; 0, 0, 0]^T\)	\((v, \phi)\)
全向	3	\(I_{3\times3}\)	\((v_x, v_y, \omega)\)
麦克纳姆	3	\(I_{3\times3}\)（近似）	\((v_x, v_y, \omega)\)

nav2 的运动学抽象

ROS2 nav2 在 Galactic/Humble+ 版本中引入了运动学插件（kinematics plugin）的概念。核心接口包括：

接口方法	功能	与 Pfaffian 的关系
isPositionReachable(from, to)	判断目标是否可达	Chow 定理的工程实现
getMinTurningRadius()	最小转弯半径	阿克曼约束的参数化
isHolonomic()	底盘是否全向	\(A(q) = \emptyset\) 的判断
projectVelocity(cmd_vel)	将命令速度投影到可行空间	\(\mathcal{D}(q) = \ker A(q)\) 上的投影

projectVelocity 是最核心的方法。它将一个可能违反运动学约束的速度命令投影到允许速度分布上：

\[ v_{\text{projected}} = (I - A^{\dagger}A) \, v_{\text{cmd}} \]

这恰好是 \(A(q)\) 零空间的正交投影。

运动学参数与 Pfaffian 矩阵的对应

nav2 参数	Pfaffian 对应	差速底盘值	阿克曼值
holonomic	\(\text{rank}(A) = 0?\)	false	false
min_turning_radius	\(L/\tan(\delta_{\max})\)	0（可原地转）	1-5 m
max_velocity_x	不在 \(A\) 中，是动力学限制	1.0 m/s	3.0 m/s
max_velocity_y	\(A\) 决定是否为零	0（非完整）	0（非完整）
max_angular_velocity	不在 \(A\) 中，是动力学限制	2.0 rad/s	0.5 rad/s

运动学参数的在线标定

部署到新机器人时，运动学参数（轮距 \(2L\)、轮半径 \(R\)、阿克曼轴距等）可能与 CAD 标称值不同。在线标定方法：

1. 直线测试：机器人直行 5 m，比较编码器里程和外部测量距离，校正轮半径 \(R\)
2. 旋转测试：机器人原地旋转 360 度，比较编码器角度和外部测量角度，校正轮距 \(2L\)
3. 持续自校正：运行中融合 IMU 角速度和编码器差速角速度，在线估计 \(L/R\) 比值

import numpy as np


def calibrate_wheel_radius(encoder_distance: float,
                           measured_distance: float,
                           nominal_radius: float) -> float:
    """中文注释：通过直线测试校正轮半径。"""
    scale = measured_distance / encoder_distance
    return nominal_radius * scale


def calibrate_wheel_base(encoder_angle: float,
                         measured_angle: float,
                         nominal_base: float) -> float:
    """中文注释：通过旋转测试校正轮距。"""
    scale = measured_angle / encoder_angle
    return nominal_base / scale

轮足机器人的 nav2 适配

对于轮足机器人，nav2 的标准运动学插件需要扩展——因为底盘类型在运行时可以切换：

1. wheel 模式：等效于差速底盘（非完整），isHolonomic() = false
2. foot 模式：等效于全向（在平面投影上），isHolonomic() = true
3. 混合模式：需要自定义插件

# 轮足运动学插件的概念代码
class WheelLeggedKinematicsPlugin:
    def __init__(self, robot_config):
        self.wheel_radius = robot_config['wheel_radius']
        self.track_width = robot_config['track_width']
        self.current_mode = 'wheel'  # 初始模式

    def is_holonomic(self):
        """根据当前模式返回是否全向"""
        if self.current_mode == 'wheel':
            return False  # 差速/阿克曼约束
        elif self.current_mode == 'foot':
            return True   # 足式步态可横移
        else:
            return False  # 混合模式保守处理

    def project_velocity(self, cmd_vel, current_state):
        """将速度命令投影到当前模式的可行空间"""
        if self.current_mode == 'wheel':
            # 差速约束：v_y = 0
            return [cmd_vel[0], 0.0, cmd_vel[2]]
        elif self.current_mode == 'foot':
            # 无约束
            return cmd_vel
        else:
            # 混合模式：根据接触状态决定
            return self._mixed_projection(cmd_vel, current_state)

⚠️ 常见陷阱与误区

⚠️ 编程陷阱：nav2 插件中硬编码底盘类型 - 错误做法：在 nav2 配置中固定 robot_model_type: differential，不支持运行时切换 - 典型现象：轮足机器人在足模式下使用差速规划器，生成弧线路径去到达旁边 1 米处的目标（实际可以直接横移） - 修正思路：编写自定义运动学插件，根据模式消息动态切换 isHolonomic() 返回值

💡 概念误区：认为 nav2 的 cmd_vel 就是机器人的实际速度 - 新手想法："nav2 发布的 cmd_vel 直接发给底盘就行" - 实际上：cmd_vel 是期望速度，底盘控制器需要将其转换为轮速命令（通过逆运动学），再由电机控制器执行。中间有延迟、饱和和执行误差。nav2 的 velocity smoother 插件部分处理了这些问题，但轮足机器人的模式切换需要额外的平滑逻辑

练习 76.8

[A] ⭐⭐ 为差速底盘实现完整的 projectVelocity 函数：输入 \((v_x, v_y, \omega)\)，输出投影后的速度。验证投影后 \(v_y = 0\)。

[B] ⭐⭐⭐ 设计一个模式切换状态机：定义 wheel/foot/swing 三种模式的切换条件和最短驻留时间。画出状态转移图并解释每个转移条件的物理含义。

[C] ⭐⭐⭐ 阅读 ROS2 nav2 的 nav2_costmap_2d 文档，理解 costmap 如何处理机器人的足印（footprint）。讨论：轮足机器人在 wheel 模式和 foot 模式下的等效足印是否相同？对避障有什么影响？

---

本章小结

模块	核心问题	应掌握的能力
76.1 动机	为什么轮子不能横移？	区分完整/非完整约束
76.2 SE(2) 运动学	速度约束的数学形式？	写出 \(A(q)\dot{q}=0\) 形式
76.3 Pfaffian 推导	四种底盘的约束矩阵？	独立推导差速/阿克曼/全向/麦轮的 \(A(q)\)
76.4 Frobenius 定理	约束是否可积？	用 Lie bracket 判断可积性
76.5 Chow 定理	非完整系统能到达哪里？	验证 LARC 并解释可控性
76.6 滑移建模	纯滚动失效时怎么办？	计算滑移比/角，理解 Pacejka 模型
76.7 轮足混合	足+轮约束如何统一？	写出模式相关的统一 Pfaffian
76.8 工程实现	如何落地到 ROS2？	理解 nav2 运动学插件接口

---

本章常见误解汇总

误解	正确理解
非完整约束限制了可达位置	只限制瞬时速度方向，可达位形空间不变（Chow 定理）
全向轮完全无约束	全向轮无非完整约束，但有关节速度限制和轮间协调约束
Pfaffian 矩阵是固定的	对于某些系统（如阿克曼转向），\(A(q)\) 随位形变化
差速底盘最简单	运动学简单但控制上是非完整的，路径规划比全向底盘复杂
滑移总是有害的	轮足控制中有意的滑移可以提高敏捷性（如快速转弯）
Lie bracket 只是数学工具	Lie bracket 直接决定了通过"交替操控"能产生哪些新运动方向
Frobenius 定理难以工程应用	SymPy 可以自动完成 Lie bracket 计算和可积性判断
所有非完整系统都一样难控制	可控性等级不同，等级越高运动规划越复杂

---

Pfaffian 约束的数值灵敏度分析

约束违反的来源与量化

在实际系统中，Pfaffian 约束 \(A(q)\dot{q} = 0\) 从不被精确满足。违反来源和量级如下：

违反来源	典型量级	影响	处理方法
轮子打滑	\(0.01-0.1\) m/s	累积定位误差	里程计融合 + 滑移检测
编码器量化	\(10^{-4}\) rad	速度计算噪声	低通滤波
控制器离散化	\(O(\Delta t)\)	步间约束不精确满足	减小控制周期
模型参数误差	1-5%	\(A(q)\) 矩阵不准确	在线参数估计
地面变形	变化大	纯滚动假设失效	滑移建模 + 自适应

约束残差的监控

定义约束残差为：

\[ r(t) = A(q(t))\dot{q}(t) \]

在理想情况下 \(r(t) = 0\)。在实际系统中，\(\|r(t)\|\) 的大小反映了约束违反的严重程度。

\(\|r(t)\|\) 范围	含义	处理
\(< 0.01\)	正常运行	无需干预
\(0.01 - 0.1\)	轻微滑移	融合 IMU/视觉修正
\(0.1 - 0.5\)	显著滑移	切换到滑移模型，降低速度
\(> 0.5\)	严重失控	紧急减速，重新定位

import numpy as np


def monitor_constraint_residual(A: np.ndarray, q_dot: np.ndarray,
                                 thresholds: dict) -> str:
    """中文注释：监控 Pfaffian 约束残差，返回状态等级。"""
    residual = A @ q_dot
    norm = np.linalg.norm(residual)

    if norm < thresholds.get('normal', 0.01):
        return 'NORMAL'
    elif norm < thresholds.get('mild_slip', 0.1):
        return 'MILD_SLIP'
    elif norm < thresholds.get('severe_slip', 0.5):
        return 'SEVERE_SLIP'
    else:
        return 'EMERGENCY'

跨领域类比：约束残差监控类似于工厂中的统计过程控制（SPC）。SPC 通过监控产品质量的统计偏差来检测生产过程是否失控。同样地，监控 \(\|r(t)\|\) 的均值和方差可以检测轮式系统是否在正常工况下运行。如果 \(\|r(t)\|\) 的均值突然增大，可能意味着轮胎磨损、地面条件变化或载荷超限。

轮足系统中 Pfaffian 约束的在线估计

当轮足机器人在未知地面上运动时，Pfaffian 约束矩阵可能因滑移参数变化而需要实时更新。一种实用方法是用扩展卡尔曼滤波器（EKF）在线估计滑移参数：

状态向量：\(x = [q^T, \dot{q}^T, \sigma_{lon}, \sigma_{lat}]^T\)，其中 \(\sigma_{lon}\) 和 \(\sigma_{lat}\) 是纵向和侧向滑移参数。

过程模型：

\[ \begin{aligned} \dot{q} &= G(q, \sigma_{lon}, \sigma_{lat}) u + w_q \\ \dot{\sigma}_{lon} &= w_{\sigma,lon} \\ \dot{\sigma}_{lat} &= w_{\sigma,lat} \end{aligned} \]

其中 \(w\) 是过程噪声，\(G\) 是考虑了滑移的修正输入矩阵。

观测模型：IMU 提供角速度和加速度，编码器提供轮速。通过对比 IMU 测量和轮速推算的速度差异，EKF 可以在线估计滑移参数。

这种在线估计让导航系统能够自适应地补偿滑移——在干燥路面上 \(\sigma \approx 0\)，在湿滑路面上 \(\sigma\) 增大，系统自动调整运动学模型以保持定位精度。

---

累积项目：本章新增模块——Rolling Constraint Checker

本章为轮足复合机器人的累积项目新增以下模块：

阶段	任务	交付物
1. 几何检查	读取 URDF 中的轮关节配置（轴向、半径、速度索引）	轮配置报告脚本
2. 约束构造	实现 rolling_residual() 函数，计算三维残差	可复用的 Python 模块
3. 可积性验证	用 SymPy 计算 Lie bracket，验证非完整性	符号计算脚本
4. 滑移检测	实现滑移比/角计算和低速保护	滑移时间线绘图脚本
5. 异常注入	故意修改半径/轴向/索引，验证残差是否正确定位异常	故障定位报告

---

Pfaffian 约束在轮足混合平台上的扩展讨论

从固定底盘到浮动基座：约束结构的本质变化

前文推导的 Pfaffian 约束 \(A(q)\dot{q}=0\) 隐含了一个关键假设——底盘是刚体，轮子与底盘的几何关系固定。当我们从纯轮式平台过渡到轮足混合平台（如 ANYmal-on-wheels、Unitree B2-W），这个假设不再成立：轮子安装在腿末端，而腿的构型会改变轮子相对于基座的位置和姿态。

这意味着 Pfaffian 约束矩阵 \(A(q)\) 不再只是底盘构型 \(q_b \in SE(2)\) 的函数，而是整个系统广义坐标 \(q = (q_b, q_\ell)\) 的函数，其中 \(q_\ell\) 包含所有腿关节角度。具体来说，每个轮子的接地点位置和滚动方向都要通过正运动学从基座经由腿链计算得到：

\[ p_{\text{contact},i} = \text{FK}(q_b, q_{\ell,i}), \quad e_{\parallel,i} = R_{\text{wheel},i}(q_b, q_{\ell,i}) \cdot e_x \]

因此，统一 Pfaffian 约束变为：

\[ A(q_b, q_\ell) \begin{pmatrix} \dot{q}_b \\ \dot{q}_\ell \end{pmatrix} = 0 \]

这个扩展后的约束矩阵有两个重要特性：（1）它的维度随着系统模式（哪些腿处于轮模式、哪些处于足模式）动态变化；（2）它对腿关节角度的依赖使得约束的 Lie bracket 结构更加复杂——这直接影响 Chow 定理的可控性分析。

Chow 定理的直觉解释

Chow 定理（也称 Chow-Rashevskii 定理）回答了一个根本性问题：受非完整约束限制的系统，能否到达构型空间中的任意点？ 其精确陈述为：如果约束分布 \(\Delta\) 及其所有 Lie bracket 生成的分布 \(\text{Lie}(\Delta)\) 在每一点都张成整个切空间，则系统是完全可控的——即任意两点之间存在满足约束的路径。

直觉理解：想象一辆汽车（阿克曼转向）。它在任何瞬间只能沿前进方向移动或转弯——不能横移。这是非完整约束的效果。但是通过"前进-转弯-前进-转弯"的组合（类似平行泊车），汽车可以到达平面上的任意位置和朝向。Chow 定理告诉我们：反复执行允许的运动并取它们的"组合效应"（Lie bracket），就能产生原本被禁止的运动方向。

用数学语言说，如果 \(f_1\) 和 \(f_2\) 是两个允许的速度方向，它们的 Lie bracket \([f_1, f_2]\) 表示"先沿 \(f_1\) 走一小段，再沿 \(f_2\) 走一小段，再沿 \(-f_1\) 走一小段，再沿 \(-f_2\) 走一小段"的净效应——这个净效应可能是一个全新的运动方向（如汽车的侧移）。

对轮足混合的意义：纯轮式底盘的可控性取决于底盘类型——差速可控、阿克曼可控、全向可控。但轮足混合系统有一个纯轮式没有的优势：腿的运动提供了额外的速度方向。即使轮子受非完整约束限制，腿的摆动可以直接在约束被限制的方向上产生位移。这意味着轮足混合系统的可控性通常优于同等轮式配置的纯轮系统——Chow 条件更容易满足。

LARC 条件的实际验证

Lie Algebra Rank Condition (LARC) 是 Chow 定理的可操作形式：在某个构型点 \(q_0\) 处，计算约束分布的所有阶 Lie bracket，检查它们是否张成整个切空间。对于轮足混合系统，这个验证可以用 SymPy 符号计算自动化：

1. 写出约束向量场 \(f_1, f_2, \ldots, f_k\)（约束零空间的基底）
2. 计算一阶 Lie bracket \([f_i, f_j]\)
3. 检查 \(\{f_1, \ldots, f_k, [f_1,f_2], [f_1,f_3], \ldots\}\) 的秩
4. 如果秩等于构型空间维度，则系统可控

工程价值：LARC 验证不仅是理论工具，它直接指导运动规划器的设计。如果某个模式组合下 LARC 条件不满足（比如所有腿都锁定在某个特殊构型），规划器应当避免这个构型——或者切换到其他模式。

学习建议：初学者可以先在最简单的差速底盘上手动验证 LARC（只需 2 个向量场，1 个 Lie bracket），建立对"Lie bracket 产生新方向"的直觉。然后用 SymPy 处理四轮差速和轮足混合的更复杂情况。本章 76.5 节的练习提供了完整的符号计算框架。

---

Pfaffian 约束与现代 RL 控制的融合视角

约束感知的 RL 训练

当 RL 策略控制轮足机器人时，Pfaffian 约束不再以解析形式出现在控制器中，而是通过奖励函数和训练设计隐式编码。以下是三种约束编码方式的对比：

编码方式	实现	约束满足度	训练难度	部署风险
奖励惩罚	\(r_{constraint} = -w\|A(q)\dot{q}\|^2\)	软性（可违反）	低	中（可能学会违反）
动作投影	\(a_{safe} = \text{proj}(a, \text{null}(A))\)	硬性（不可违反）	中	低
课程学习	先在无约束环境训练，逐步加入约束	渐进	中	中

奖励惩罚的权重选择

约束惩罚权重 \(w\) 的选择至关重要。\(w\) 太小，策略忽视约束频繁打滑。\(w\) 太大，策略过于保守不敢运动。一个实用的自适应方法是：

\[ w(t) = w_0 \cdot \max\left(1, \frac{\|A(q)\dot{q}\|_{avg}}{\epsilon_{target}}\right) \]

当平均约束违反度超过目标阈值 \(\epsilon_{target}\) 时，自动增大权重。

跨领域类比：约束编码类似于法律体系中的"硬法"和"软法"。动作投影是"硬法"——不可违反，直接在执行前强制满足。奖励惩罚是"软法"——违反有代价但不被阻止。在实际社会中，最有效的治理通常是"少量硬法 + 大量软法"。机器人控制也是如此：关键安全约束（如关节限位）用硬性投影，性能约束（如能效）用奖励惩罚。

从单刚体到多刚体的 Pfaffian 推广

本章主要讨论了单底盘（单刚体）的 Pfaffian 约束。当轮足机器人的腿与轮子构成多刚体系统时，约束结构变得更复杂：

单底盘约束：

\[ A_{chassis}(q)\dot{q}_{chassis} = 0 \]

多刚体约束（轮足）：

\[ \begin{bmatrix} A_{chassis}(q) & 0 \\ A_{contact}(q) & J_{leg}(q) \end{bmatrix} \begin{bmatrix} \dot{q}_{chassis} \\ \dot{q}_{leg} \end{bmatrix} = 0 \]

其中 \(A_{contact}(q)\) 描述脚轮与地面的接触约束，\(J_{leg}(q)\) 是腿部雅可比。这个扩展矩阵的零空间定义了整个轮足系统的允许运动。

零空间维度的物理意义

\[ \dim(\text{null}(A_{extended})) = n_{total} - \text{rank}(A_{extended}) \]

对于四轮足机器人（\(n_{total} = 3 + 4 \times 3 = 15\) 维），如果所有轮子都在纯滚动状态（\(\text{rank} = 4 + 4 = 8\)），零空间维度为 7——系统有 7 个独立可控输入。当某条腿抬起时，对应的接触约束消失，rank 减少，零空间维度增加——系统获得了更多自由度，但也失去了该腿的支撑力。

---

延伸阅读

• 基础 ⭐⭐ Murray, Li, Sastry, A Mathematical Introduction to Robotic Manipulation, Chapter 7: 非完整约束与可控性的经典教科书推导
• 基础 ⭐⭐ Lynch & Park, Modern Robotics, Section 13.3: 移动机器人速度约束的几何推导，配有优秀的可视化
• 进阶 ⭐⭐⭐ Bjelonic et al., "Whole-Body MPC and Online Gait Sequence Generation for Wheeled-Legged Robots" (IROS 2021): ANYmal-on-wheels 的 WBC 和步态序列优化
• 进阶 ⭐⭐⭐ Bjelonic et al., "Rolling in the Deep: Hybrid Locomotion for Wheeled-Legged Robots Using Online Trajectory Optimization" (RAL 2020): 轮足在线轨迹优化
• 拓展 ⭐⭐⭐⭐ Pacejka, Tire and Vehicle Dynamics: 轮胎力模型的权威参考，工业级 Pacejka 公式的完整推导
• 拓展 ⭐⭐⭐ LaValle, Planning Algorithms, Chapter 15: 非完整系统的采样运动规划
• 工程 ⭐⭐ ROS2 nav2 文档, Kinematics Plugin Interface: nav2 运动学插件的 API 参考

---

本章核心概念的工程决策影响

本章的数学理论（Pfaffian 约束、Frobenius 定理、Chow 定理）直接影响以下工程决策：

工程决策	依赖的数学理论	决策影响
底盘类型选择	Pfaffian 约束维度	非完整底盘需要更复杂的规划器
运动规划算法	可控性等级（LARC）	等级高的系统需要更多规划步骤
导航精度要求	约束违反对定位的影响	滑移大时需要更多传感器融合
控制器设计	零空间维度	控制输入维度 = 零空间维度
模式切换策略	约束矩阵的模式依赖性	切换时约束结构突变需平滑过渡
安全约束设计	约束违反的严重程度	大违反意味着打滑，需要紧急处理

从数学到代码的映射

数学对象	代码实现	用途
\(A(q)\) 约束矩阵	def pfaffian_matrix(q):	运动学约束检查
\(\text{null}(A)\) 零空间	scipy.linalg.null_space(A)	允许运动方向
\([g_i, g_j]\) Lie bracket	sympy 符号计算	可控性验证
\(\text{rank}(\mathcal{L})\) LARC	sympy 秩计算	可控性等级确定
\(A(q)\dot{q}\) 约束残差	residual = A @ q_dot	滑移检测
\(\sigma_{lon}, \sigma_{lat}\) 滑移参数	pacejka(sigma, B, C, D, E)	高级运动模型

本质洞察：本章的价值不在于记住每个公式，而在于建立从"运动约束分析"到"工程决策"的思维链条。当你面对一个新的轮式或轮足机器人时，第一步应该是"写出 Pfaffian 约束矩阵"——它告诉你系统有多少自由度、能往哪个方向动、需要几步才能侧移。这些信息直接决定了规划算法、控制器和安全系统的设计。

---

跨章综合练习

综合题 1（需要复合/20 + 本章知识）：Go2+wheels 系统有 24 维广义速度。四轮全部在 wheel 模式下，写出统一 Pfaffian 约束矩阵 \(A_{\text{unified}}\) 的维度和秩。计算约束后的瞬时自由度。与纯足式（4 foot）模式的自由度对比，解释为什么轮模式允许更多运动方向。

综合题 2（需要足式/50 + 本章知识）：复合/20 中质量矩阵的分块结构 \(M_{bb}, M_{b\ell}, \ldots\) 是在关节空间定义的。现在加入轮约束 \(A(q)\dot{q} = 0\)，约束后的有效动力学方程是什么？（提示：使用约束空间动力学 \(M_c = P^T M P\)，其中 \(P\) 是 \(A\) 零空间的基底矩阵）

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关章节
残差长期偏置（\(r_{\text{roll}}\) 非零均值）	轮半径标定错误或坐标系不一致	1. 固定速度直行 2. 打印 \(r\omega\) 和 \(e_\parallel^T J_c v\) 3. 比较单位	76.3, 76.6
模式切换瞬间力矩尖峰	约束矩阵在切换瞬间不连续跳变	1. 画模式时间线 2. 检查约束行数变化 3. 添加平滑过渡窗口	76.7
仿真中轮子"拖拽"（不转但机器人前进）	轮模式仍使用 \(J_c v = 0\)（足模式约束）	1. 打印约束类型 2. 验证模式消息传递 3. 检查约束矩阵构造逻辑	76.7
低速时滑移比异常震荡	滑移比公式分母接近零	1. 打印 \(v_x\) 和 \(v_{\text{low}}\) 2. 确认低速阈值合理 3. 低速段切换为速度残差	76.6
坡面上法向残差 \(r_n\) 持续非零	滚动方向未投影到地形切平面	1. 打印 \(e_\parallel\) 与 \(n\) 的内积 2. 确认 build_contact_frame 正确 3. 检查地形法向估计	76.7

---

本章核心公式速查表

以下汇总本章最重要的公式及其物理含义，供复习和快速查阅。

公式	含义	适用场景
\(A(q)\dot{q} = 0\)	Pfaffian 约束的标准形式：速度必须在约束矩阵零空间内	所有非完整轮式/轮足系统
\(\dot{x}\sin\theta - \dot{y}\cos\theta = 0\)	差速底盘的侧向速度为零约束	差速/阿克曼底盘
\([f_1, f_2] = \frac{\partial f_2}{\partial q}f_1 - \frac{\partial f_1}{\partial q}f_2\)	Lie bracket：两个向量场的"交换子"	Frobenius 可积性判断、LARC 验证
\(\text{rank}(\text{Lie}(\Delta)) = n\)	Chow 定理条件：Lie bracket 生成的分布张成全空间	非完整系统可控性分析
$s_x = \frac{v_x - r\omega}{\max(	v_x	,
\(\alpha = \arctan(v_y / v_x)\)	侧偏角：衡量轮胎侧向滑移	高速转弯、侧滑检测
\(M_c = P^T M P\)	约束空间有效质量矩阵（\(P\) 为约束零空间基）	约束后动力学分析

使用建议：遇到轮足约束问题时，先确定当前模式（foot/wheel/mixed），写出对应的 \(A(q)\)，再检查零空间维度是否与预期自由度一致。如果不一致，通常是约束矩阵构造有误。

---

本章与后续章节的衔接

本章建立的 Pfaffian 约束框架是后续多个章节的基础：

• 复合/70 轮足混合 MPC：将本章的约束矩阵 \(A(q)\) 作为 MPC 优化问题的等式约束，约束残差成为 MPC 的可行性指标
• 复合/80 轮足 WBC：将滚动约束作为 WBC 的最高优先级任务，确保 QP 解满足非完整约束
• 复合/90 模式切换：本章 76.7 节的模式相关约束矩阵是模式切换策略设计的数学基础
• 足式/50 空间向量代数：本章的雅可比矩阵 \(J_c\) 与空间向量的关系在足式/50 中有详细推导

学习者在完成本章后，建议直接进入复合/70（轮足混合 MPC），在 MPC 的优化框架中看到 Pfaffian 约束如何变成"活的工程模块"——从纸面公式到实时求解的全过程。

核心收获回顾：本章的核心不是某一个具体公式，而是一种思维方式——任何轮式或轮足系统的运动分析，都从写出 Pfaffian 约束开始。约束矩阵 \(A(q)\) 决定了系统的瞬时自由度、可控性和规划空间的维度。掌握了这个起点，后续的运动学控制、MPC 和 WBC 都是在这个约束框架上的自然延伸。

如果你在后续章节中遇到约束相关的困惑， 随时回到本章的 76.3 节（Pfaffian 推导）和 76.5 节（Chow 定理）复习核心概念。

---

本章术语速查表

术语	英文	含义
Pfaffian 约束	Pfaffian Constraint	形如 \(A(q)\dot{q} = 0\) 的速度约束
完整约束	Holonomic Constraint	可积分为位形约束的速度约束
非完整约束	Nonholonomic Constraint	不可积分的速度约束，限制瞬时速度但不限制可达位形
刚体平面运动群	SE(2)	二维平面上的刚体运动：平移 + 旋转
Frobenius 定理	Frobenius Theorem	判断 Pfaffian 约束是否可积（完整 vs 非完整）的充要条件
Lie 括号	Lie Bracket	两个向量场的交换子 \([f, g]\)，用于 Frobenius 和 Chow 定理
Chow 定理	Chow's Theorem	如果 Lie 括号生成的分布的秩等于位形空间维度，则系统完全可控
LARC	Lie Algebra Rank Condition	Chow 定理的秩条件
差速底盘	Differential Drive	两个独立驱动轮的底盘，通过差速实现转向
阿克曼转向	Ackermann Steering	汽车式转向几何，内外轮转角不同
全向轮	Omnidirectional Wheel	可以在任意方向滚动的轮子（无非完整约束）
麦克纳姆轮	Mecanum Wheel	带 45 度斜辊的全向轮
滑移参数	Slip Ratio / Slip Angle	描述轮胎纵向滑移率和侧向滑移角
Pacejka 魔术公式	Pacejka Magic Formula	经验性的轮胎力-滑移关系模型
瞬时旋转中心	Instantaneous Center of Rotation (ICR)	刚体瞬时纯旋转的中心点
可控性	Controllability	系统从任意初始状态到达任意目标状态的能力
可控性等级	Controllability Degree	LARC 条件中需要的 Lie 括号阶数
零空间	Null Space	\(A(q)\) 的零空间定义了允许的瞬时速度方向
body frame	Body Frame	固定在机器人质心的坐标系
world frame	World Frame	固定在环境中的惯性坐标系

---

本章常见误解汇总

误解	正确理解
非完整约束限制了机器人能去的地方	非完整约束只限制瞬时速度方向，不限制可达位形（Chow 定理保证）
全向轮没有约束	全向轮没有非完整约束，但有关节速度限制和轮子间的运动学协调约束
Pfaffian 约束只对轮式机器人有用	Pfaffian 约束也描述腿足机器人的接触滚动约束
差速底盘是最简单的底盘	从运动学角度差速底盘最简单，但从控制角度它是非完整的，运动规划比全向底盘困难
Frobenius 定理只是数学游戏	Frobenius 定理直接决定了运动规划器需要几步才能实现侧移
滑移可以忽略	高速或粗糙地面上滑移显著改变运动学，忽略会导致导航误差累积
可控性等级越低越好控制	可控性等级高意味着需要更多次"倒车入库"式操作，运动规划更复杂
Pacejka 公式在所有场景通用	Pacejka 参数高度依赖轮胎材料、地面类型和速度范围

---

Pfaffian 约束在轮足RL控制中的角色

从解析约束到学习约束

传统运动学控制直接使用 Pfaffian 约束矩阵 \(A(q)\) 来约束运动规划。在 RL 控制中，策略不显式使用 \(A(q)\)，而是通过训练中的约束违反惩罚隐式学习遵守约束。

方法	约束处理	优点	缺点
解析约束（MPC/WBC）	显式 \(A(q)\dot{q} = 0\)	精确、可证明	需要精确模型
RL 惩罚	\(r_{slip} = -w \|A(q)\dot{q}\|^2\)	不需要精确模型	不保证严格满足
RL + 投影	策略输出投影到 \(\text{null}(A(q))\)	精确 + 灵活	需要实时计算 \(A(q)\)

本质洞察：Pfaffian 约束在 RL 控制中的角色从"硬约束"变成了"软正则"。RL 策略通过惩罚信号学会了大致遵守纯滚动约束，但不保证精确满足。对于轮足机器人，适度的"约束违反"（即滑移）有时反而是有用的——快速转弯时有意让轮子打滑可以提高敏捷性。关键是让策略学会"什么时候该遵守约束，什么时候可以违反"。

模式切换与约束矩阵的动态变化

轮足机器人在行走模式和滚动模式之间切换时，Pfaffian 约束矩阵发生离散变化。设行走模式的约束为 \(A_{walk}(q)\)，滚动模式为 \(A_{roll}(q)\)：

\[ A(q, m) = \begin{cases} A_{walk}(q) & m = \text{walk} \\ A_{roll}(q) & m = \text{roll} \end{cases} \]

模式切换瞬间，约束维度和方向可能都不同——滚动模式有纯滚动的非完整约束，行走模式则没有（脚掌可以自由放置）。

切换策略的设计准则

切换条件	从行走到滚动	从滚动到行走
地形坡度	\(< 15°\)（平坦）	\(> 25°\)（陡峭）
障碍物高度	\(< 5\) cm	\(> 10\) cm（台阶/路缘）
速度需求	\(> 1.5\) m/s	\(< 0.5\) m/s（精确定位）
能效权衡	滚动 COT 更低	行走可以原地转向

反事实推理：如果不设置切换缓冲区会怎样？假设切换条件是"坡度 > 15 度则行走，< 15 度则滚动"。在 15 度附近的斜坡上，由于坡度测量噪声，系统会频繁在两种模式之间切换——每次切换都伴随约束矩阵的突变和短暂的动态过渡。这种"抖动切换"不仅浪费能量，还可能导致在过渡期间失稳。正确做法是引入滞后（hysteresis）：上升阈值 > 下降阈值，例如"坡度 > 20 度切换到行走，坡度 < 10 度切换回滚动"。

nav2 中 Pfaffian 约束的实时使用

在 ROS2 nav2 框架中，运动学约束通过 CostmapModel 和 ControllerPlugin 接口传递给规划器。Pfaffian 约束矩阵 \(A(q)\) 的零空间定义了允许的瞬时速度方向：

\[ \dot{q} \in \text{null}(A(q)) \Rightarrow \dot{q} = G(q) u \]

其中 \(G(q)\) 是零空间基矩阵，\(u\) 是降维后的控制输入。规划器在 \(u\) 空间中搜索路径，而不是在完整位形空间中搜索。

对于差速底盘，\(u = [v, \omega]^T\)（线速度和角速度），规划器只需搜索二维空间。对于阿克曼转向，\(u = [v, \phi]^T\)（速度和转向角），规划器同样在二维空间中搜索，但需要考虑最小转弯半径约束。

import numpy as np


def pfaffian_to_input_basis(A: np.ndarray, q: np.ndarray) -> np.ndarray:
    """中文注释：从 Pfaffian 约束矩阵计算允许的输入基矩阵 G(q)。
    G(q) 的列向量张成 A(q) 的零空间。"""
    # 中文注释：SVD 分解求零空间
    U, S, Vt = np.linalg.svd(A)
    # 中文注释：奇异值接近零的右奇异向量是零空间基
    null_mask = S < 1e-10
    # 中文注释：如果 A 是 k × n，零空间维度是 n - rank(A)
    rank_A = np.sum(~null_mask)
    G = Vt[rank_A:].T  # 中文注释：零空间基矩阵
    return G


def check_constraint_violation(A: np.ndarray, q_dot: np.ndarray,
                                threshold: float = 0.01) -> tuple:
    """中文注释：检查当前速度是否违反 Pfaffian 约束。"""
    violation = A @ q_dot
    violation_norm = np.linalg.norm(violation)
    is_violated = violation_norm > threshold
    return is_violated, violation_norm

跨领域类比：Pfaffian 约束在运动规划中的作用类似于高速公路的车道线。车道线不限制你最终能到达哪里（可达性），但限制了你在任何时刻可以移动的方向（瞬时速度约束）。遵守车道线的驾驶（遵守 Pfaffian 约束的运动）是安全高效的；偶尔变道（有控制地违反约束）是允许的；完全无视车道线（大幅违反约束，即严重打滑）则是危险的。RL 策略学习"何时遵守何时违反"，就像经验丰富的司机知道何时该严格保持车道、何时可以灵活变道。

---

—— 本章终 ——