第 82 章：底盘 + 臂联合规划——SE(2) 底座、冗余雅可比与 Nav2 + MoveIt2¶

本章定位：移动操作方向的入门核心章。研究对象：轮式底盘 + 机械臂，例如 Stretch、Ridgeback+UR5、Husky+UR、Mobile ALOHA、TIAGo、Go2-W+小臂。主线问题：底盘负责移动，机械臂负责操作，但末端任务需要两者共同完成。难度：⭐⭐⭐。预计时间：1.5 周，理论 10 小时，代码 10 小时，实验 8 小时。

82.0 前置自测¶

阅读前请回答：

#	问题	建议回顾
1	$SE(2)$ 位姿 $(x,y,\theta)$ 的速度输入如何写成差速底盘模型？	复合/60
2	机械臂雅可比 $J(q)$ 如何把关节速度映射到末端 twist？	机械臂/M03
3	OMPL 在构型空间规划的基本形式是什么？	机械臂/M07
4	MoveIt2 的 PlanningScene 管理什么？	机械臂/M14
5	Nav2 中全局规划器和局部控制器分别做什么？	SLAM/Nav2

自测标准：

5/5 正确：直接阅读本章。
3-4/5 正确：重点关注联合雅可比推导。
0-2/5 正确：建议先回顾机械臂 IK、OMPL 和 Nav2。

82.1 本章目标¶

学完本章，你应能：

写出底盘 + 臂的联合构型空间。
推导差速、全向和 Ackermann 底盘的运动学。
推导移动操作系统的联合雅可比。
使用零空间项表达“底盘少动、手先动”等偏好。
解释 Nav2 + MoveIt2 分层架构的优点和边界。
设计 base placement 和 inverse reachability map。
编写底盘臂联合速度 IK 框架。
分析统一 OCP 与分层规划的取舍。
调试导航到位但抓取失败、机械臂可达但底盘碰撞等故障。

本章知识导航¶

本章包含 5 组核心概念，建议按依赖顺序阅读：

知识块	包含内容	依赖	难度
A 建模	联合构型空间、三类底盘运动学、准速度	复合/60	⭐⭐
B 联合微分运动学	联合雅可比、约简雅可比、冗余度	A	⭐⭐⭐
C 求解器	阻尼伪逆、零空间、任务优先级 / HQP	B	⭐⭐⭐
D 系统落地	IRM、base placement、Nav2+MoveIt2、行为树	A、C	⭐⭐
E 进阶与前沿	统一 OCP、边走边操作、学习类路线	B–D	⭐⭐

推荐路径有两条：

理论线（想吃透数学）：$A\to B\to C\to E$，把 §82.6+ 约简雅可比与 §82.8+ 分层 IK 当作重点。
工程线（想先搭系统）：$A\to D\to C\to E$，先把 Nav2+MoveIt2 分层跑通，再回头补求解器与统一优化。

注意：本导航只展示**结构**，不展开内容。$\bar J$ 与 HQP 是本章两个“⭐⭐⭐ 锚点”，其余小节大多围绕它们展开。

前置知识桥接¶

本章站在三块前置内容之上：复合/60 给了轮式底盘的非完整约束与 Pfaffian 形式（本章 §82.4、§82.6+ 直接复用）；机械臂/M03 给了固定臂雅可比 $J(q)$（本章把它扩成 $[J_b\ J_a]$）；机械臂/M07、M14 给了 OMPL 与 MoveIt2（本章把单臂规划扩成“底盘作虚拟关节”的联合规划）。本章要解决的**新问题**是：当“基座”不再固定、而是一个可移动且可能受非完整约束的 $SE(2)$ 物体时，如何统一描述、求解并落地“底盘 + 臂共同完成末端任务”。

如果跳过本章会怎样¶

直接学复合/130 的 OCS2 mobile_manipulator 时，会看不懂状态为什么是 $(x_b,y_b,\theta_b,q_a)$、末端代价里的雅可比从哪来——因为联合雅可比与约简映射正是本章建立的。
部署真机移动抓取时，会反复踩“导航到位却抓不到”“底盘 footprint 安全但臂撞货架”的坑，而本章 §82.10、§82.18 正是为此而写。

预计阅读时间¶

阅读方式	时间	适合谁
精读（含推导与练习）	4 小时	要做移动操作研究 / 项目的读者
速读（跳过 §82.6+、§82.8+ 推导细节）	1.5 小时	已懂冗余 IK，只想了解移动操作特有问题
速查（只看表格、速查卡与故障手册）	20 分钟	遇到具体故障回来查

82.2 移动操作的基本矛盾 ⭐¶

固定基座机械臂的世界很清楚。

基座固定，机械臂关节决定末端位姿。

移动操作平台则多了一个移动底座。

底座移动会改变整条机械臂的工作空间。

机械臂运动又会影响底座停车位置是否合适。

若只用 Nav2 把底盘导航到桌前，再用 MoveIt2 抓杯子，流程简单。

但它有明显局限：

底盘停得太远，手够不到。
底盘停得太近，机械臂与桌子碰撞。
底盘朝向不合适，IK 进入奇异位形。
导航路径可行，但操作位姿不可达。
操作路径可行，但底盘在执行中挡住机械臂。

移动操作的核心不是“导航加抓取”。

它是“底盘位姿和机械臂构型共同服务末端任务”。

本质洞察：移动底座把机械臂的工作空间从一个固定集合变成了随底盘位姿移动的集合。规划问题从“在一个工作空间内找解”，变成“先把工作空间移动到合适位置，再在其中找解”。

82.3 联合构型空间 ⭐⭐¶

令底盘位姿：

\[ q_b=(x_b,y_b,\theta_b)\in SE(2) \]

令机械臂关节：

\[ q_a\in\mathbb{R}^{n_a} \]

联合构型：

\[ q=\begin{bmatrix}q_b\\q_a\end{bmatrix}\in SE(2)\times\mathbb{R}^{n_a} \]

若是 7-DOF 机械臂：

\[ \dim(q)=3+7=10 \]

末端位姿：

\[ T_{we}(q)=T_{wb}(q_b)T_{be}(q_a) \]

本文采用统一约定：$T_{ab}$ 表示“把 $b$ 坐标系中的点变换到 $a$ 坐标系”。

也就是：

\[ p_a=T_{ab}p_b \]

因此 $T_{wb}$ 是基座到世界的变换，也可理解为“基座在世界坐标系下的位姿”。

$T_{bw}=T_{wb}^{-1}$ 才是世界到基座的变换。

$T_{be}$ 是末端到基座的机械臂正运动学，也可理解为“末端在基座坐标系下的位姿”。

于是：

\[ p_w=T_{wb}p_b=T_{wb}T_{be}p_e \]

这就是 $T_{we}=T_{wb}T_{be}$ 的含义。

后续联合雅可比和 $SE(3)$ 误差都沿用这个方向，避免在代码中一会儿使用 $T_{wb}$、一会儿实际传入 $T_{bw}$。

固定基座机械臂只优化 $q_a$。

移动操作平台要同时优化 $q_b$ 和 $q_a$。

82.4 三类底盘运动学 ⭐⭐¶

差速底盘¶

差速底盘输入：

\[ u_b=\begin{bmatrix}v\\\omega\end{bmatrix} \]

运动学：

\[ \dot x_b=v\cos\theta_b \]

\[ \dot y_b=v\sin\theta_b \]

\[ \dot\theta_b=\omega \]

矩阵形式：

\[ \dot q_b= \begin{bmatrix} \cos\theta_b & 0\\ \sin\theta_b & 0\\ 0 & 1 \end{bmatrix} \begin{bmatrix}v\\\omega\end{bmatrix} \]

非完整约束：

\[ \dot x_b\sin\theta_b-\dot y_b\cos\theta_b=0 \]

它表示底盘不能瞬时侧移。回顾复合/60 轮式运动学与 Pfaffian 约束：这个约束是 Pfaffian 形式 $\mathbf{A}(\mathbf{q})\dot{\mathbf{q}} = 0$ 的最简单实例——$\mathbf{A} = (\sin\theta_b, -\cos\theta_b, 0)$，它约束速度空间但不约束可达位置空间。在联合规划中，这个约束意味着底盘在规划可行联合速度时，只能使用 $(v, \omega)$ 两个自由度，不能直接命令横向位移。对于需要横向微调末端位置的精细操作任务，差速底盘的这个限制会迫使机械臂承担更多的横向补偿，从而更快接近臂的工作空间边界。

全向底盘¶

全向底盘输入：

\[ u_b=\begin{bmatrix}v_x^B\\v_y^B\\\omega\end{bmatrix} \]

世界系速度：

\[ \begin{bmatrix}\dot x_b\\\dot y_b\end{bmatrix} =R(\theta_b)\begin{bmatrix}v_x^B\\v_y^B\end{bmatrix} \]

\[ \dot\theta_b=\omega \]

全向底盘对移动操作非常友好。

它可以在不改变朝向的情况下横向微调。这个能力在精细操作中价值极大——当末端需要横向平移 5 cm 来对准目标时，全向底盘可以直接侧移，而差速底盘必须先转弯、前进、再转回来，整个过程可能需要 3-5 秒。

代价是机械复杂度、地面适应性和轮胎打滑问题。麦克纳姆轮在光滑地面上效果好，但在地毯、碎石或户外地面上可能出现严重的横向滑移，导致实际运动与期望不符。

Ackermann 底盘¶

Ackermann 底盘状态：

\[ q_b=(x_b,y_b,\theta_b,\phi) \]

其中 $\phi$ 是转向角。

输入可取：

\[ u_b=(v,\dot\phi) \]

运动学：

\[ \dot x_b=v\cos\theta_b \]

\[ \dot y_b=v\sin\theta_b \]

\[ \dot\theta_b=\frac{v}{L}\tan\phi \]

\[ \dot\phi=u_\phi \]

Ackermann 底盘适合户外和重载。

但停车姿态约束更强。

对抓取任务而言，base placement 更难。

82.5 联合速度变量¶

为了写联合雅可比，我们定义可控速度：

\[ \nu=\begin{bmatrix}u_b\\\dot q_a\end{bmatrix} \]

差速底盘：

\[ \nu\in\mathbb{R}^{2+n_a} \]

全向底盘：

\[ \nu\in\mathbb{R}^{3+n_a} \]

Ackermann 底盘：

\[ \nu\in\mathbb{R}^{2+n_a} \]

注意：$\nu$ 不是简单的 $\dot q$。

对差速底盘，$\dot q_b$ 是由 $u_b$ 映射来的。

这与浮动基座中 $n_q$ 和 $n_v$ 不同但思想类似：配置变量和速度变量的维度不一定相同。

为什么准速度“不可积” ⭐⭐¶

“$\nu$ 不是 $\dot q$”这句话还可以追问一层：差速底盘明明只有 2 个准速度 $(v,\omega)$，位形却是 3 维 $(x_b,y_b,\theta_b)$；既然位置可达空间是完整的 3 维（底盘能开到平面任意位姿），为什么不能找到 2 个坐标，让它们的导数恰好是 $(v,\omega)$？答案触及非完整系统的本质——约束是不可积的。

形式上，若存在两个标量函数 $h_1,h_2$ 使得 $v=\dot h_1,\ \omega=\dot h_2$，则非完整约束 $\mathbf A(q_b)\dot q_b=0$ 就能积分成位形约束 $g(q_b)=\text{const}$，底盘的可达位形会被限制在一张 2 维曲面上。但现实是底盘能到达整个 3 维 $SE(2)$，所以这样的 $h_i$ 不存在——这正是“非完整”（nonholonomic）一词的定义：约束只限制速度方向，不限制可达位形。

判据是 Frobenius 定理：约束分布（由 $S$ 的两列张成）可积当且仅当它对**李括号封闭**。差速底盘的两个控制方向

\[ g_1=\begin{bmatrix}\cos\theta_b\\\sin\theta_b\\0\end{bmatrix}(\text{前进}),\qquad g_2=\begin{bmatrix}0\\0\\1\end{bmatrix}(\text{自转}) \]

的李括号

\[ [g_1,g_2]=\frac{\partial g_1}{\partial q_b}g_2-\frac{\partial g_2}{\partial q_b}g_1 =\begin{bmatrix}\sin\theta_b\\-\cos\theta_b\\0\end{bmatrix} \]

恰好指向**被约束禁止的横向方向**，且不在 $\{g_1,g_2\}$ 张成的空间内。分布对李括号不封闭，故不可积——这从微分几何上证明了准速度无法被积分成位形坐标。

本质洞察：李括号 $[g_1,g_2]$ 指向横向，正是“反复前进+自转能侧向挪车”的数学根源。你不能瞬时侧移，但“前进一点、转一点、后退一点、再转回来”的组合能净产生横向位移——这就是平行泊车，也是 $[g_1,g_2]\ne0$ 的物理含义。它同时说明：差速底盘虽**瞬时**受约束，却**长期**可达全空间（小时间局部可控）；所以约简雅可比只在速度级损失自由度，不在可达性上损失。

这也回答了一个常被混淆的问题：差速底盘的“冗余度少 1”是速度级现象，不影响它最终能停到任意位姿——§82.10 的 base placement 之所以仍能枚举任意朝向 $\theta_b$，正依赖于这种长期可达性。

82.6 联合雅可比的推导 ⭐⭐⭐¶

末端 twist 记为：

\[ V_{ee}^w\in\mathbb{R}^6 \]

它由底盘速度和机械臂速度共同贡献：

\[ V_{ee}^w=J_b(q)u_b+J_a(q)\dot q_a \]

合并：

\[ V_{ee}^w=J(q)\nu \]

其中：

\[ J(q)=\begin{bmatrix}J_b(q)&J_a(q)\end{bmatrix} \]

差速底盘部分¶

设末端相对基座的位置在世界系为：

\[ p_{be}^w=R_{wb}p_{be}^b \]

底盘向前速度 $v$ 对末端线速度贡献：

\[ R_{wb}e_x v \]

底盘偏航角速度 $\omega$ 对末端线速度贡献：

\[ \omega e_z\times p_{be}^w \]

对末端角速度贡献：

\[ \omega e_z \]

因此差速底盘雅可比：

\[ J_b= \begin{bmatrix} R_{wb}e_x & e_z\times p_{be}^w\\ 0_{3\times1} & e_z \end{bmatrix} \in\mathbb{R}^{6\times2} \]

这里 $e_z\times p$ 也可写作：

\[ -[p]_\times e_z \]

符号取决于 twist 约定。

工程中必须与 Pinocchio 或 Eigen 实现统一。

全向底盘部分¶

全向底盘多一个横向速度列：

\[ J_b= \begin{bmatrix} R_{wb}e_x & R_{wb}e_y & e_z\times p_{be}^w\\ 0 & 0 & e_z \end{bmatrix} \in\mathbb{R}^{6\times3} \]

这让冗余度增加 1。

同样末端任务下，全向底盘更容易保持机械臂姿态舒适。

82.6+ 非完整底盘的约简雅可比与准速度 ⭐⭐⭐¶

上一节把 $J_b$ 直接写成了对 $u_b=(v,\omega)$ 的 $6\times2$ 矩阵，看起来和全向底盘只差了一列。这容易让人忽略一个本质区别：全向底盘的 $J_b$ 是“把 $\dot q_b$ 写成末端 twist”，而差速底盘的 $J_b$ 已经把非完整约束“吸收”进去了。理解这一点是把非完整底盘正确放进联合规划的关键，也是工程里最容易写错的地方。本节给出严格的推导。

动机：为什么不能直接对 $\dot q_b$ 求伪逆¶

一个自然但错误的做法是：先写出“全自由度”的几何雅可比

\[ J_{\text{full}}(q)=\begin{bmatrix}J_{b,\text{full}}(q)&J_a(q)\end{bmatrix}\in\mathbb{R}^{6\times(3+n_a)} \]

其中 $J_{b,\text{full}}$ 把 $\dot q_b=(\dot x_b,\dot y_b,\dot\theta_b)$ 这三个分量都当作可控量，然后对 $J_{\text{full}}$ 求伪逆得到 $\dot q$。问题在于：差速底盘根本不能独立命令 $\dot y_b^B$（车体系横向速度）。伪逆解会给出一个一般含有非零横向分量的 $\dot q_b$，把它丢给底盘执行时，底盘要么忽略横向分量（于是末端任务无法满足），要么用滑移近似（于是产生与模型不符的误差）。

⚠️ 概念误区：把非完整约束当成“事后裁剪”

新手想法：“先按全向求伪逆，再把横向速度分量设零就行。” 实际上：把横向分量直接置零**不是**约束流形上的投影，它会破坏末端任务的满足度——置零后的 $\dot q_b$ 配上原来的 $\dot q_a$，乘回 $J_{\text{full}}$ 已经不等于 $V_d$ 了。正确做法：在“求解之前”就把可控速度限制在非完整约束允许的子空间内，即使用**约简雅可比**（reduced Jacobian）。这与复合/60 中把 Pfaffian 约束写进速度空间是同一思想。

准速度与约简映射¶

回顾差速底盘运动学的矩阵形式：

\[ \dot q_b=S(q_b)\,u_b,\qquad S(q_b)=\begin{bmatrix}\cos\theta_b&0\\\sin\theta_b&0\\0&1\end{bmatrix}\in\mathbb{R}^{3\times2} \]

矩阵 $S(q_b)$ 的两列张成的正是非完整约束 $\mathbf{A}(q_b)\dot q_b=0$ 的零空间——满足 $\mathbf{A}S=0$，其中 $\mathbf{A}=(\sin\theta_b,-\cos\theta_b,0)$。把这里的 $u_b=(v,\omega)$ 称为**准速度**（quasi-velocity）：它不是任何位形坐标的时间导数（不存在函数 $h$ 使得 $v=\dot h$），但它完整参数化了底盘所有可行的瞬时运动。这正是 §82.5 强调“$\nu$ 不是简单的 $\dot q$”的数学根源。

把 $S$ 推广到全身，定义**约简选择矩阵**：

\[ \bar S(q)=\begin{bmatrix}S(q_b)&0\\0&I_{n_a}\end{bmatrix}\in\mathbb{R}^{(3+n_a)\times(2+n_a)} \]

于是全身位形速度与可控准速度 $\nu=(u_b,\dot q_a)$ 的关系是 $\dot q=\bar S(q)\,\nu$。

约简雅可比¶

把这一关系代入“全自由度”几何雅可比，得到**约简雅可比**（reduced Jacobian）：

\[ \boxed{\;\bar J(q)=J_{\text{full}}(q)\,\bar S(q)\in\mathbb{R}^{6\times(2+n_a)}\;} \]

展开它的底盘部分就能验证它与 §82.6 直接推出的 $J_b$ 完全一致：

\[ J_{b,\text{full}}\,S =\begin{bmatrix}I_3&-[p_{be}^w]_\times\\0&I_3\end{bmatrix} \!\!\underbrace{\begin{bmatrix}R_{wb}e_x&0\\R_{wb}e_y&0\\0&0\\\hline 0&0\\0&0\\0&1\end{bmatrix}}_{\text{$S$ 嵌入 6 维 twist}} =\begin{bmatrix}R_{wb}e_x&e_z\times p_{be}^w\\0&e_z\end{bmatrix} \]

（上式中 $J_{b,\text{full}}$ 取“底座 twist 到末端 twist”的伴随形式，$S$ 把 $(v,\omega)$ 嵌成底座 twist $(v\cos\theta_b,v\sin\theta_b,0,0,0,\omega)$。）这说明 §82.6 那个看似“凑”出来的 $J_b$，其实是 $J_{\text{full}}\bar S$ 的严格结果——非完整约束已经通过右乘 $S$ 被结构性地吸收了。

本质洞察：约简雅可比把“约束”从求解器的负担变成了变量定义的一部分。一旦用 $\bar J$ 替代 $J_{\text{full}}$，后面所有伪逆、零空间、QP 公式都可以原封不动套用，而求出的 $\nu$ 天然可行——不需要任何事后裁剪。代价是：求出 $\nu$ 后若想得到 $\dot q_b$，必须再左乘 $S$；横向速度永远是 $v\sin\theta_b\,$（耦合量），而非独立自由度。

这对冗余度计数的影响¶

用约简雅可比后，冗余度的定义回到标准形式 $r=\dim(\nu)-m$，其中 $m$ 是任务维度。差速底盘 + 7 轴臂、6 维任务时 $r=(2+7)-6=3$，与 §82.7 一致。注意这里 $r$ 计的是**准速度**的冗余，不是位形速度的冗余——差速底盘虽有 3 个位形坐标，但只贡献 2 个可控方向。这也解释了为什么差速底盘在“纯横向”微调任务上比全向底盘“少一个直接手段”：它的约简雅可比在该方向上必须借道偏航 $\omega$ 与臂关节耦合实现。

可操作性的非完整版本¶

把 §82.11 的 Yoshikawa 可操作性指标推广到非完整移动操作时，必须用约简雅可比而非全自由度雅可比：

\[ w_{\text{nh}}(q)=\sqrt{\det\!\big(\bar J\bar J^\top\big)} \]

Bayle 与 Fourquet 等人指出，正是这个“约简可操作性”而不是固定臂的 $w(q_a)$，才正确刻画了移动操作平台在当前位形下产生末端运动的能力——它同时包含了底盘朝向 $\theta_b$ 的影响（因为 $\bar J$ 含 $R_{wb}$）。一个直接推论：差速底盘**朝向**会进入可操作性，这是固定臂场景没有的现象，也是 §82.10 的 base placement 必须把 $\theta_b$ 当成关键决策量的根本原因。

由此还能得到一个有用的比较命题：在**同一臂构型**下，全向底盘的约简可操作性恒不低于差速底盘。理由是全向底盘的约简雅可比 $\bar J_{\text{omni}}$ 比差速的 $\bar J_{\text{diff}}$ 多一列（横向 $R_{wb}e_y$），即 $\bar J_{\text{diff}}$ 的列空间是 $\bar J_{\text{omni}}$ 列空间的子集。多一列只会让 $\bar J\bar J^\top$ 的特征值不减（半正定矩阵叠加一个秩一半正定项），故

\[ w_{\text{nh}}^{\text{omni}}(q)\ge w_{\text{nh}}^{\text{diff}}(q). \]

这把 §82.4“全向底盘对操作更友好”从定性印象升格为可证不等式：全向底盘的“友好”本质上是它在**任意**位形下都拥有不小于差速底盘的末端运动能力。代价仍是 §82.4 指出的机械复杂度与打滑——可操作性的优势只在轮子不打滑时才真正兑现。

82.7 冗余度 ⭐⭐¶

为什么冗余度是移动操作的关键概念？ 固定基座的 6-DOF 机械臂在给定末端位姿后通常有唯一解或有限解（最多 8 个）。但移动操作平台有"多余"的自由度——底盘和臂加起来超过了末端任务所需的 6 个自由度。这些多余的自由度不是浪费，而是巨大的工程优势——它们让系统在完成末端任务的同时，还能满足额外的偏好和约束。

末端完整位姿任务是 6 维。

差速底盘 + 7 轴臂的速度维度：

\[ 2+7=9 \]

冗余度：

\[ r=9-6=3 \]

全向底盘 + 7 轴臂：

\[ r=10-6=4 \]

冗余度可以用来做次级目标：

底盘尽量少动。
机械臂远离关节限位。
末端保持远离奇异构型。
底盘朝向保持面向目标。
机械臂肘部远离桌面。
载荷尽量靠近车体。

82.8 阻尼伪逆和零空间 ⭐⭐⭐¶

给定期望末端 twist：

\[ V_d \]

求联合速度：

\[ \nu^\star=\arg\min_\nu \|J\nu-V_d\|^2+\lambda^2\|\nu\|^2 \]

解为阻尼伪逆：

\[ \nu^\star=J^\top(JJ^\top+\lambda^2I)^{-1}V_d \]

加入次级目标：

\[ \nu^\star=J^+V_d+(I-J^+J)\nu_0 \]

$\nu_0$ 位于零空间中。

它不会一阶影响末端任务。

如果希望底盘少动：

\[ \nu_0= \begin{bmatrix} -k_b u_b\\ 0 \end{bmatrix} \]

如果希望机械臂回到舒适姿态：

\[ \nu_0= \begin{bmatrix} 0\\ -k_a(q_a-q_a^{nom}) \end{bmatrix} \]

如果希望底盘朝向目标：

\[ \nu_0= \begin{bmatrix} 0\\ k_\theta\mathrm{wrap}(\theta_{target}-\theta_b)\\ 0 \end{bmatrix} \]

本质洞察：移动操作的冗余不是”多余自由度”，而是工程偏好的入口。同一个末端速度可以由底盘动、臂动或两者共同完成。零空间项就是把”我更希望谁动”写进数学问题。

数值实例：差速底盘 + 3-DOF 臂的阻尼伪逆 IK¶

为了建立直觉，我们用一个可以手算的简化系统来演示阻尼伪逆 IK 的完整计算过程。

系统配置：差速底盘（2 输入：前进速度 $v$、偏航角速度 $\omega$）+ 3-DOF 平面臂（关节角 $q_1, q_2, q_3$）。只考虑末端在平面内的位置跟踪（2D 任务），因此联合速度维度为 $2 + 3 = 5$，任务维度为 2，冗余度 $r = 3$。

当前配置：底盘朝向 $\theta_b = 0$，臂末端在基座坐标系中的位置为 $p_{be}^b = (0.5, 0.3)^T$ m。

联合雅可比（简化为 2D 位置任务）：

\[ J = \begin{bmatrix} \underbrace{1}_{v \rightarrow \dot{x}} & \underbrace{-0.3}_{{\omega \rightarrow \dot{x}}} & \underbrace{-0.5}_{q_1} & \underbrace{-0.3}_{q_2} & \underbrace{-0.1}_{q_3} \\ \underbrace{0}_{v \rightarrow \dot{y}} & \underbrace{0.5}_{\omega \rightarrow \dot{y}} & \underbrace{0.4}_{q_1} & \underbrace{0.2}_{q_2} & \underbrace{0.05}_{q_3} \end{bmatrix} \]

第一列来自底盘前进速度（$\theta_b = 0$ 时只贡献 $\dot{x}$），第二列来自底盘偏航角速度（通过 $e_z \times p_{be}^w$ 贡献末端线速度），后三列是臂的雅可比（具体数值取决于臂的连杆长度和当前关节角）。

期望末端速度：$V_d = (0.1, 0.05)^T$ m/s（向前右方移动）。

Step 1：计算 $JJ^T$

\[ JJ^T = \begin{bmatrix} 1^2 + 0.3^2 + 0.5^2 + 0.3^2 + 0.1^2 & -0.3 \cdot 0 + \ldots \\ \ldots & 0^2 + 0.5^2 + 0.4^2 + 0.2^2 + 0.05^2 \end{bmatrix} \]

\[ = \begin{bmatrix} 1.44 & -0.03 \\ -0.03 & 0.4525 \end{bmatrix} \]

（注：对角项是雅可比行的范数平方，非对角项是行的内积。）

Step 2：加入阻尼

取 $\lambda = 0.05$（阻尼因子），则：

\[ JJ^T + \lambda^2 I = \begin{bmatrix} 1.4425 & -0.03 \\ -0.03 & 0.455 \end{bmatrix} \]

Step 3：求逆并计算阻尼伪逆解

\[ (JJ^T + \lambda^2 I)^{-1} V_d \approx \begin{bmatrix} 0.0694 \\ 0.1105 \end{bmatrix} \]

\[ \nu^* = J^T \begin{bmatrix} 0.0694 \\ 0.1105 \end{bmatrix} = \begin{bmatrix} 1 \cdot 0.0694 + 0 \cdot 0.1105 \\ -0.3 \cdot 0.0694 + 0.5 \cdot 0.1105 \\ -0.5 \cdot 0.0694 + 0.4 \cdot 0.1105 \\ -0.3 \cdot 0.0694 + 0.2 \cdot 0.1105 \\ -0.1 \cdot 0.0694 + 0.05 \cdot 0.1105 \end{bmatrix} = \begin{bmatrix} 0.069 \\ 0.034 \\ 0.010 \\ 0.001 \\ -0.001 \end{bmatrix} \]

解读：

底盘前进速度 $v = 0.069$ m/s 承担了大部分前进任务。

底盘偏航 $\omega = 0.034$ rad/s 配合横向位移。

臂关节几乎不动。

这是因为阻尼伪逆天然选择最小范数解——底盘通过一个大幅度运动就能完成大部分末端位移，臂只需要做微量调整。

这个结果在物理上是直觉的：底盘一个小幅度的前进就能让末端整体平移，而臂关节需要多个关节协调才能产生同样的末端位移。最小范数解倾向于用"效率最高"的自由度来完成任务。

Step 4：加入零空间”底盘少动”偏好

如果希望底盘尽量少动（比如在狭窄空间中），设 $\nu_0 = (-k_b v, -k_b \omega, 0, 0, 0)^T$，$k_b = 5$。零空间投影 $(I - J^+ J)\nu_0$ 会在不影响末端速度的前提下，重新分配底盘和臂的贡献——底盘分量减小，臂分量相应增大。具体数值需要计算完整的投影矩阵，留作练习。

⚠️ 概念误区：阻尼因子 $\lambda$ 不是越小越好。$\lambda \to 0$ 时阻尼伪逆退化为 Moore-Penrose 伪逆，在雅可比秩亏（奇异位形）时速度会发散到无穷大。工程上 $\lambda$ 的取值通常在 $10^{-3}$ 到 $10^{-1}$ 之间，更精细的做法是让 $\lambda$ 随最小奇异值自适应变化——奇异值越小，$\lambda$ 越大，避免过度放大。回顾复合/60 轮式运动学与 Pfaffian 约束中讨论的阻尼逆在 Pfaffian 约束投影中的应用，思想完全一致。

82.8+ 任务优先级与分层 IK ⭐⭐⭐¶

§82.8 的“主任务 + 零空间次级目标”只有**两级**：末端 twist 是硬的，其余偏好挤在零空间里软性叠加。但真实移动操作往往需要**多级严格优先级**：例如“末端位置 $\succ$ 末端姿态 $\succ$ 避关节限位 $\succ$ 底盘少动 $\succ$ 回中”。把它们全塞进一个加权和会出现“权重打架”——调大避限位权重会污染末端跟踪。本节给出系统化的解法。

动机：加权和的根本缺陷¶

考虑把 $k$ 个任务写成一个加权最小二乘：

\[ \nu^\star=\arg\min_\nu\sum_{i=1}^{k}w_i\|J_i\nu-\dot x_i^d\|^2 \]

当两个任务**冲突**（无法同时精确满足）时，解会落在由权重 $w_i$ 决定的折中点上。这带来两个问题：(1) 折中比例对权重数值极其敏感，标定困难；(2) 没有“硬优先级”概念——再小的低优先任务也会以 $O(w_i/w_1)$ 的量级污染高优先任务。Siciliano 与 Slotine 提出的**任务优先级**框架（task-priority）用嵌套零空间投影替代加权，从结构上杜绝了低优先级对高优先级的污染。

递归零空间投影（等式任务）¶

设任务按优先级排序 $1\succ2\succ\dots\succ k$，每个任务有雅可比 $J_i$ 和期望速度 $\dot x_i^d$。递归构造：

\[ \nu_1=J_1^{+}\dot x_1^d,\qquad N_1=I-J_1^{+}J_1 \]

\[ \nu_i=\nu_{i-1}+\big(J_iN_{i-1}\big)^{+}\big(\dot x_i^d-J_i\nu_{i-1}\big),\quad i=2,\dots,k \]

其中 $N_{i-1}$ 是“前 $i-1$ 个任务联合零空间”的投影矩阵，可递归更新

\[ N_i=N_{i-1}-\big(J_iN_{i-1}\big)^{+}\big(J_iN_{i-1}\big). \]

每一级只在“不破坏所有更高优先级任务”的子空间里求解自己的目标。$\nu_k$ 就是最终联合速度。当某级任务与高优先级**完全冲突**时，$J_iN_{i-1}$ 秩亏，该级在可行子空间内做最小二乘近似，而高优先级**严格不受影响**——这正是加权和做不到的。

本质洞察：零空间投影把“优先级”编码成了几何包含关系。第 $i$ 级的解永远活在前 $i-1$ 级的零空间里，所以它对高优先级任务的一阶影响**恒等于零**，与它自己的“权重”无关。加权和是“用数值大小排座次”，任务优先级是“用子空间维度排座次”——后者才是真正的硬优先级。

为什么移动操作更需要 HQP¶

上面的递归投影只能处理**等式任务**，无法直接表达“关节限位 $q_{\min}\le q\le q_{\max}$”“底盘速度限幅”“避障距离 $\ge d_{\text{safe}}$”这类**不等式约束**。而移动操作几乎每一级都带不等式：底盘速度有上限、臂关节有行程、相机杆不能撞货架。Escande、Mansard 与 Wieber 于 2014 年提出的**分层二次规划**（Hierarchical Quadratic Programming, HQP）解决了这个问题：它把每一级写成一个带等式/不等式的 QP，按优先级**逐级求解**，并把上一级的最优**残差**作为下一级的硬约束传下去。

第 $i$ 级的 QP（概念形式）：

\[ \begin{aligned} \min_{\nu,\,w_i}\quad& \tfrac12\|w_i\|^2\\ \text{s.t.}\quad& J_i\nu-\dot x_i^d= w_i&&\text{(本级松弛)}\\ & A_i\nu\le b_i&&\text{(本级不等式)}\\ & J_j\nu-\dot x_j^d= w_j^\star,\ j<i&&\text{(锁定更高优先级的最优残差)}\\ & A_j\nu\le b_j,\ j<i&&\text{(继承更高优先级不等式)} \end{aligned} \]

求得本级最优松弛 $w_i^\star$ 后冻结，进入下一级。文献报告：HQP 求解纯等式层级比迭代投影法快约 10 倍，且能在任意层级插入不等式，仍可在全身规模问题上以控制频率（kHz 级）运行——这使它成为人形/移动操作整机控制的主流后端。本项目足式方向 240_WBC 用的就是同一类分层求解思想，区别仅在那里是动力学级（力/力矩），这里是运动学级（速度）。

移动操作的典型任务栈¶

把上述框架落到一台差速底盘 + 7 轴臂上，一个常见的任务栈（priority stack）如下：

优先级	任务	类型	雅可比 / 约束
P0	关节/底盘速度限幅、避障距离	不等式（最硬）	$A\nu\le b$
P1	末端位置跟踪	等式	$\bar J_{\text{pos}}\,\nu=\dot p^d$
P2	末端姿态跟踪	等式	$\bar J_{\text{ori}}\,\nu=\omega^d$
P3	远离关节限位 / 提升可操作性	等式（梯度方向）	$\nabla_\nu(\cdot)$
P4	底盘少动 / 臂回中	等式（最软）	$\nu\to\nu_0$

注意这里用的是 §82.6+ 的**约简雅可比** $\bar J$，所以非完整约束已在变量定义层面满足，不需要再单列一条等式约束。把避障与限幅放在最高优先级 P0，保证了任何情况下安全约束都不会被末端任务“挤掉”——这与“宁可够不到，也不能撞”的工程直觉一致。

🧠 思维陷阱：层数越多越好

新手想法：“每个偏好都给一层严格优先级，系统最讲道理。” 实际上：层数越多，可行子空间逐级收缩得越快，低优先级任务可能根本没有自由度可用（$N_{i-1}$ 已退化为零空间维度 0），更糟的是**优先级切换**（任务进出栈）会引起速度不连续，导致抖动。正确思维：移动操作通常 3-4 级足够（安全 / 末端 / 限位 / 名义），其余偏好用同级加权或零空间软项处理。Recursive Hierarchical Projection 等工作专门研究优先级**平滑过渡**以消除切换抖动——这说明“多层”本身是有工程代价的。

可操作性梯度作为零空间目标¶

§82.11 用可操作性 $w$ 做 base placement 的**离线**打分；在**在线**速度 IK 里，更常见的是把“提升可操作性、远离奇异”写成一个零空间速度。其方向是可操作性对位形的梯度：

\[ \nu_0^{(w)}=k_w\,\nabla_q\, w_{\text{nh}}(q),\qquad \frac{\partial w}{\partial q_j}=w\cdot\operatorname{tr}\!\Big(\big(\bar J\bar J^\top\big)^{-1}\frac{\partial(\bar J\bar J^\top)}{\partial q_j}\Big) \]

把 $\nu_0^{(w)}$ 投进零空间 $(I-\bar J^{+}\bar J)\nu_0^{(w)}$，系统就会在完成末端任务的同时，自动朝“更灵巧”的位形漂移，远离奇异。Haviland 与 Corke 的纯反应式（purely-reactive）控制器把这一项与避障、限位一起写成一个 QP，证明无需全局规划也能让移动操作平台持续保持高可操作性——这是把 base placement 的“离线选位”思想下沉到“在线连续调整”的代表性工作，正好对应 §82.16 中 L1 $\to$ L2 的过渡。

⚠️ 概念误区：可操作性梯度需要解析二阶导

新手想法：“$\partial(\bar J\bar J^\top)/\partial q_j$ 要手推每个关节的雅可比导数，太复杂，工程里没法用。” 实际上：绝大多数实现用**数值差分**或自动微分（Pinocchio 的 computeJointJacobiansTimeVariation / CasADi）得到该梯度，根本不手推。在 10-DOF 移动操作上，一次梯度评估只是若干次正运动学，远低于一个控制周期的预算。手推解析式只在追求极致实时性时才必要。

82.9 速度级联合 IK 代码框架¶

#include <algorithm>

#include <Eigen/Dense>

struct MobileIkWeights {
  double damping = 1e-3;
  double base_regularization = 1.0;
  double arm_regularization = 0.1;
  int base_dof = 2;
};

class MobileManipulatorVelocityIk {
 public:
  Eigen::VectorXd solve(const Eigen::MatrixXd& J,
                        const Eigen::VectorXd& desired_twist,
                        const Eigen::VectorXd& nominal_velocity,
                        const MobileIkWeights& weights) const {
    const int nv = static_cast<int>(J.cols());
    const int base_dof = std::min(weights.base_dof, nv);

    Eigen::MatrixXd R = Eigen::MatrixXd::Zero(nv, nv);
    for (int i = 0; i < nv; ++i) {
      const bool is_base = i < base_dof;
      R(i, i) = is_base ? weights.base_regularization
                        : weights.arm_regularization;
    }

    const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(nv, nv);

    // 中文注释：求解带速度偏好的阻尼最小二乘问题。
    const Eigen::MatrixXd H =
        J.transpose() * J + R + weights.damping * weights.damping * I;
    const Eigen::VectorXd rhs =
        J.transpose() * desired_twist + R * nominal_velocity;

    return H.ldlt().solve(rhs);
  }
};

这个框架只解决速度级任务。

它没有处理碰撞、关节限位和底盘不可达。在实际工程中，这三个问题都必须加入约束处理——碰撞约束通过 MoveIt2 的 PlanningScene 检查，关节限位通过 QP 的不等式约束保证，底盘不可达通过 Nav2 的 costmap 验证。速度级 IK 只是移动操作规划的第一步——它给出"如果没有任何障碍和限制，底盘和臂应该怎么动"的理想解。后续章节（复合/130_OCS2_mobile_manipulator）会把这个理想解放进带约束的统一 OCP 框架中。

这段代码对应的优化问题是：

\[ \min_\nu \frac12\|J\nu-V_d\|^2 +\frac12(\nu-\nu_0)^\top R(\nu-\nu_0) +\frac12\lambda^2\|\nu\|^2 \]

其中 $R$ 的底盘对角项由 base_regularization 给出，机械臂对角项由 arm_regularization 给出。

当 nominal_velocity 对应分量为 0 时，调大 base_regularization 会让求解器少用底盘速度，调大 arm_regularization 会让求解器少用关节速度。

当 nominal_velocity 不为 0 时，这两个权重表达的是“更贴近该名义速度”，而不是简单地压低速度幅值。

工程中要把它包装成 QP：

\[ \min_\nu \|J\nu-V_d\|_Q^2+\|\nu-\nu_0\|_R^2 \]

约束：

\[ \nu_{\min}\le\nu\le\nu_{\max} \]

\[ q_{a,\min}\le q_a+\dot q_a\Delta t\le q_{a,\max} \]

\[ A_{\mathrm{collision}}\nu\le b_{\mathrm{collision}} \]

82.10 底盘停车位姿与 IRM ⭐⭐¶

移动操作中，底盘停在哪里决定机械臂能不能抓。

给定目标抓取位姿 $T_g^w$。

我们需要找底盘位姿 $q_b$，使得存在臂构型 $q_a$：

\[ FK(q_b,q_a)=T_g^w \]

同时满足：

底盘不碰撞。
机械臂不碰撞。
关节不超限。
IK 远离奇异。
视野不被遮挡。
底盘可由导航到达。

Inverse Reachability Map 预计算机械臂在基座坐标系下可达的末端位姿集合。

离线步骤：

采样大量臂关节构型 $q_a$。
计算末端位姿 $T_{be}$。
把 $T_{be}$ 反变换成“底盘相对目标”的候选位姿。
统计每个相对位姿的可达性质量。
存成查表结构。

在线步骤：

目标抓取位姿 $T_g^w$ 输入。
从 IRM 查出候选底盘相对位姿。
转成世界系底盘候选 $q_b^w$。
用 Nav2 costmap 检查底盘可达和碰撞。
用 MoveIt2 检查臂 IK 和碰撞。
选择综合代价最低的停车位姿。

IRM 的形式化：从可达图到反向可达图 ⭐⭐⭐¶

上面的流程描述背后是一套可以写清楚的离散化与变换。理解它，才能解释 IRM 为什么能把“在线找停车点”从一次次昂贵的 IK 搜索，降到一次查表。

第一步：可达图（Reachability Map, RM）。 把基座坐标系下的末端位姿空间离散成 6D 体素（voxel）。位置 $(x,y,z)$ 按分辨率 $\Delta_p$（典型 5 cm）网格化，姿态按某种均匀采样（如 SO(3) 上的 $\Delta_R$）离散。对每个体素 $V$，离线随机采样大量臂构型 $q_a$、正运动学得到 $T_{be}(q_a)$，落入哪个体素就给它累加一次命中。每个体素存一个**可达性质量**

\[ \rho(V)=\max_{q_a:\,T_{be}(q_a)\in V}\ w(q_a),\qquad w(q_a)=\sqrt{\det\!\big(J_aJ_a^\top\big)} \]

即落入该体素的所有构型中**最高的 Yoshikawa 可操作性**（也可用命中率或最小到限位距离）。$\rho$ 高的体素表示“臂在这里既够得到、又灵巧、远离奇异”。

第二步：取反得到反向可达图（IRM）。 RM 回答“给定底座，末端能到哪”；base placement 要的是反问题“给定末端目标，底座该在哪”。关键观察是这对变换互为逆：若末端相对底座的位姿是 $T_{be}$，则**底座相对末端**的位姿就是

\[ T_{eb}=T_{be}^{-1}. \]

于是把 RM 中每个体素 $T_{be}$ 取逆、连同其质量 $\rho$ 一起存进“以末端为原点”的新体素表，就得到 IRM。在线时给定世界系目标抓取位姿 $T_g^w$，候选底座世界位姿直接由

\[ T_{b}^{w}=T_g^{w}\,T_{eb}=T_g^{w}\,T_{be}^{-1} \]

枚举 IRM 中所有高质量 $T_{eb}$ 即得——不需要在线解任何 IK。再把 $T_b^w$ 投影到 $SE(2)$（取 $x,y$ 与绕 $z$ 的偏航，忽略高度与滚转俯仰，因为底盘只在平面运动）就是候选 $q_b^w$。

本质洞察：IRM 把“在线 IK 搜索”换成了“离线 FK 采样 + 一次矩阵求逆查表”。正运动学是廉价的单向计算，逆运动学是昂贵的搜索；IRM 用大量离线 FK 采样把 IK 的答案“预烤”进表里。这与图形学里用预计算光照贴图替代实时光线追踪是同一种工程哲学：把贵的搜索搬到离线。

⚠️ 概念误区：IRM 可以直接给出可执行停车点

新手想法：“从 IRM 查到高质量 $T_{eb}$，转成 $q_b^w$ 就能去停。” 实际上：IRM 只编码了**臂的运动学可达性**，它不知道现场障碍、底盘能否导航到该点、相机是否被遮挡。正确做法：IRM 给出的是**按可达质量排序的候选集**，必须再经 §82.11 的 costmap、IK 复核、视野代价过滤——IRM 负责“快速缩小搜索范围”，不负责“最终拍板”。

离散化的代价。 体素分辨率是精度与内存的权衡：$\Delta_p$ 太粗，候选停车点与真实最优偏差大、可能 IK 复核全失败；太细，6D 体素数量爆炸（$O(\Delta_p^{-3}\Delta_R^{-3})$），离线构建与内存都吃不消。工程上常只对**任务相关的位姿子集**（如桌面高度 $\pm 10$ cm、末端朝下的抓取姿态）建图，把 6D 退化成 3–4D，显著压缩规模。这也解释了为何通用 IRM 少见、而“面向某类抓取任务的定制 IRM”更实用。

82.11 base placement 代价¶

一个停车候选的代价可写为：

\[ J(q_b,q_a)= w_d d_{\mathrm{nav}}(q_b) +w_c c_{\mathrm{costmap}}(q_b) +w_i c_{\mathrm{ik}}(q_a) +w_l c_{\mathrm{limit}}(q_a) +w_v c_{\mathrm{view}}(q_b) \]

各项含义：

代价	含义
$d_{\mathrm{nav}}$	底盘到候选点的导航距离
$c_{\mathrm{costmap}}$	候选点附近障碍代价
$c_{\mathrm{ik}}$	IK 残差或奇异性
$c_{\mathrm{limit}}$	关节接近限位程度
$c_{\mathrm{view}}$	相机视角和遮挡

奇异性可用 manipulability：

\[ w(q_a)=\sqrt{\det(J_aJ_a^\top)} \]

代价：

\[ c_{\mathrm{ik}}=\frac{1}{w(q_a)+\epsilon} \]

82.12 Nav2 + MoveIt2 分层架构¶

经典流水线：

任务目标
  │
  ▼
抓取候选生成
  │
  ▼
base placement
  │
  ├── Nav2 检查底盘可达
  └── MoveIt2 检查臂可达
  ▼
Nav2 导航到底盘位姿
  │
  ▼
MoveIt2 规划机械臂抓取
  │
  ▼
执行抓取与放置

优点：

工程成熟。
模块边界清楚。
Nav2 和 MoveIt2 各自可替换。
适合低速服务机器人。
故障容易定位。

缺点：

导航和操作时间上分离。
无法自然处理边走边抓。
底盘误差会传给机械臂。
MoveIt2 规划时常假设底盘固定。
对连续移动操作不够紧耦合。

82.13 行为树编排¶

Nav2 使用行为树编排导航。

移动操作任务也可以用行为树管理阶段。

示例：

Sequence
  ├── DetectObject
  ├── GenerateGraspCandidates
  ├── ChooseBasePlacement
  ├── NavigateToBasePose
  ├── UpdatePlanningScene
  ├── PlanArmToPreGrasp
  ├── ServoToGrasp
  ├── CloseGripper
  ├── AttachObject
  ├── PlanArmToLift
  └── NavigateToPlacePose

行为树的作用不是替代规划器。

它负责把多个规划器和控制器串起来。

当 PlanArmToPreGrasp 失败时，可以回到 ChooseBasePlacement 重新选停车点。

当 NavigateToBasePose 失败时，可以换一个候选点。

82.14 MoveIt2 侧接口¶

MoveIt2 需要知道：

机械臂 URDF/SRDF。
关节限位。
碰撞模型。
当前 planning scene。
目标末端位姿。
可用规划器。
执行控制器。

底盘可以作为虚拟关节加入规划模型。

一种方式是使用 planar joint 表示底盘：

<joint name="virtual_base_joint" type="planar">
  <parent link="world"/>
  <child link="base_link"/>
</joint>

这种表示适合全向或离线联合规划。

对差速底盘，planar joint 会允许横向移动。

因此需要额外加入非完整约束，或只把底盘位姿用于采样，不直接执行横向速度。

82.15 Nav2 侧接口¶

Nav2 需要：

地图或 costmap。
当前底盘位姿。
目标底盘位姿。
机器人 footprint。
运动学模型。
局部控制器参数。
障碍物更新。

移动操作中的 footprint 不是固定不变的。

机械臂伸出时，机器人占用空间变大。

如果 Nav2 仍使用收起姿态的 footprint，底盘可能贴近桌子，导致机械臂或负载碰撞。

解决办法：

导航时要求机械臂收起。
根据臂姿态动态更新 footprint。
用 3D 碰撞检查替代 2D footprint。
在操作阶段冻结底盘或降低速度。

82.16 联合规划的三种层次 ⭐⭐¶

层次	方法	适用场景
L0 分层串行	Nav2 到位，再 MoveIt2 抓取	低速、静态环境
L1 松耦合联合	base placement 联合检查可达性	服务机器人常用
L2 统一优化	底盘和臂同时进入 OCP	连续移动操作

L0 最容易部署。

L1 是工程最常见折中。

L2 最优雅，但成本最高。

统一优化的状态：

\[ x=\begin{bmatrix}x_b&y_b&\theta_b&q_a\end{bmatrix}^\top \]

输入：

\[ u=\begin{bmatrix}v&\omega&\dot q_a\end{bmatrix}^\top \]

动力学：

\[ \dot x= \begin{bmatrix} v\cos\theta_b\\ v\sin\theta_b\\ \omega\\ \dot q_a \end{bmatrix} \]

末端代价：

\[ \ell_{ee}=\frac12\|\log(T_{we}^{d^{-1}}T_{we}(x))^\vee\|_{Q}^2 \]

输入代价：

\[ \ell_u=\frac12 u^\top R u \]

约束包括：

关节限位。
底盘速度限幅。
自碰撞。
外部障碍距离。
底盘非完整约束。

82.17 分层与统一的决策树¶

任务是否需要边移动边操作？
  ├── 否 → 使用 Nav2 + MoveIt2 分层
  └── 是
      ├── 速度低、可接受短暂停顿 → 使用 L1 松耦合
      └── 速度高、末端需连续跟踪
          ├── 模型简单、状态维度低 → 使用 OCS2 统一 OCP
          └── 接触丰富、动力学强耦合 → MPC + WBC 或学习策略

不要为了“统一”而统一。

若机器人只是去桌边拿杯子，分层架构足够好。

若机器人要边移动边擦桌子，统一 OCP 更合适。

若机器人要推门、拉抽屉、搬重物，动力学和接触力必须进入规划。

82.17+ 第三条路：端到端学习与近年工作 ⭐⭐¶

L0–L2（分层 / 松耦合 / 统一 OCP）共享一个前提：底盘与臂的运动学、可达性、碰撞都用**显式模型**写出来，再交给规划或优化求解。这条路解释性强、可调试，但对“高度接触、长视野、语义任务”力不从心。2022 年以来出现了第三条路——用学习直接获得底盘与臂的协调策略。这一节梳理它与经典方法的关系，帮助你在读相关论文时把它“挂”到本章的框架上，而不是当成另起炉灶的黑盒。

为什么经典分层在某些任务上失效¶

回顾 §82.12 分层架构的缺点：导航与操作时间分离、底盘误差传给臂、MoveIt2 常假设底盘固定。对“开冰箱门并取物”“边走边整理桌面”这类任务，底盘和臂必须**同时连续协调**，且接触力、关节顺从性、视觉反馈强耦合，显式建模成本极高。学习类方法的卖点正是：把协调策略从数据里学出来，绕开手写的可达性图、base placement 代价和零空间权重。

系统分类：学习如何介入¶

学习并不总是“端到端替换一切”。按介入深度可分三档（这是理解近年工作的关键坐标系）：

档位	学什么	经典部分保留什么	代表工作
A 学底盘、模型控臂	RL 学“给定末端任务的可行底盘运动”	臂仍用 §82.8 的模型化 IK / 任务空间控制	N$^2$M$^2$
B 端到端学整机	一个策略同时输出底盘 + 臂指令	仅保留底层关节/速度跟踪	HarmonicMM
C 模仿学习整机	从人类全身遥操作示范克隆	仅保留遥操作映射与底层执行	Mobile ALOHA

本质洞察：这三档与 §82.16 的 L0–L2 不是替代而是“正交维度”。 L0–L2 回答“用多紧的耦合求解”，A–C 回答“用模型还是数据获得策略”。 N$^2$M$^2$（档 A）本质上是“用 RL 替换 §82.10 的 base placement + 跟随”，臂部分仍是经典任务空间控制——它是“学习版的松耦合 L1”，而非彻底的黑盒。

档 A：N$^2$M$^2$——学底盘、保留任务空间臂控制¶

Honerkamp、Welschehold 与 Valada 的 N$^2$M$^2$（Neural Navigation for Mobile Manipulation, TRO 2023）把移动操作分解为两件事：(1) 一个**简化的末端任务空间运动生成器**（直接给出末端该往哪走，类似一条 servo 参考），(2) 一个**强化学习智能体**专门为移动底盘生成运动，以保证整条末端轨迹的**运动学可行性**——即始终把末端目标维持在臂的可达范围内。它能在数千步、长达五分钟的长视野任务上实时执行，对未见任务与环境零样本泛化，并对动态障碍即时反应（无规划耗时）。这恰好是本章 §82.10“底盘要停/走到让臂够得着的地方”这一问题的**学习化连续版本**：经典方法离线建反向可达图，N$^2$M$^2$ 用 RL 在线生成可行底盘运动。

档 B：HarmonicMM——端到端联合优化导航与操作¶

Yuan 等人的 HarmonicMM（Harmonic Mobile Manipulation, IROS 2024）走得更彻底：一个端到端策略**同时**输出导航与操作指令，输入是机器人本体感知 + 多视角视觉（视觉经冻结的 DINOv2 编码器 + 小型 CNN，再送入 GRU 与策略头）。它在仿真与真实环境验证，无需额外微调即可适应未见公寓。与档 A 的区别在于：HarmonicMM 不再保留显式的任务空间臂控制，底盘与臂的协调完全由策略内部隐式完成——对应本章语言，就是“连联合雅可比和零空间投影都交给网络学”。代价是可解释性和安全保证下降，需要大量交互数据与仿真到真实迁移。

档 C：Mobile ALOHA——从全身遥操作示范模仿¶

Fu、Zhao 与 Finn 的 Mobile ALOHA（CoRL 2024）代表模仿学习路线：在 ALOHA 双臂系统上加移动底座与**全身遥操作**接口采集数据，用行为克隆训练策略，并与已有静态 ALOHA 数据集**联合训练**（co-training）。每个任务仅 50 条示范，联合训练即可把成功率最多提升约 90%，完成炒虾、开双门橱柜放重锅等需要整机协调的长任务，整套硬件约 3.2 万美元。它的启示是：当显式建模代价过高时，“人示范 + 模仿”是获得底盘-臂协调的另一条捷径，但需要可靠的全身遥操作映射与可观规模的数据采集。

桥接：学习方法没有废弃本章的概念¶

读这些论文时容易产生“运动学、雅可比、可达性都过时了”的错觉。事实相反：

可行性的定义没变。N$^2$M$^2$ 的 RL 奖励本质上奖励“末端落在可达集合内”——这正是 §82.6 联合雅可比与 §82.10 可达性的语言，只是从硬约束变成了 reward shaping。
冗余仍在。端到端策略输出的底盘 + 臂指令依然活在 $SE(2)\times\mathbb{R}^{n_a}$ 里，冗余度 $r$ 决定了策略有多大“协调空间”可学。
安全仍需显式兜底。多数真机部署在学习策略外层套一层 §82.18 的碰撞检查 / 速度限幅，作为硬安全层——这与 §82.8+ 把安全约束放在 HQP 最高优先级 P0 是同一思想。

🧠 思维陷阱：端到端 = 不需要懂运动学

新手想法：“既然 HarmonicMM 端到端学，我只要会调网络就行，本章这些雅可比白学了。” 实际上：设计观测、奖励、动作空间、仿真器时，处处要用到运动学——动作空间是 $(v,\omega,\dot q_a)$（§82.5），奖励里的“可达”“奇异”“限位”全是本章概念，仿真器里底盘是差速还是全向决定了动作维度（§82.4）。正确思维：学习方法是“求解器”的替换，不是“问题定义”的替换。不懂运动学，连观测和奖励都写不对。

选型小结¶

任务特征	推荐路线	理由
静态环境、到位后抓取	L0/L1 经典分层	成熟、可调试、无需数据
连续移动、模型已知、低接触	L2 统一 OCP（见复合/130）	显式最优、可加约束
长视野导航 + 可达性，臂任务清晰	档 A（N$^2$M$^2$ 式）	学底盘、臂仍可解释
高接触、语义复杂、难显式建模	档 B/C（端到端/模仿）	绕开手写模型，但需数据与仿真

不存在“最好”的路线，只有“与任务接触复杂度、数据可得性、安全要求匹配”的路线。本章后续 §82.33 的边走边操作、复合/130 的 OCS2 统一 OCP，分别是 L2 路线的两个落点；学习路线则把同一组运动学概念搬进了奖励与网络里。

82.18 碰撞问题的两个空间¶

底盘碰撞通常在 2D costmap 中检查。

机械臂碰撞通常在 3D PlanningScene 中检查。

移动操作需要同时处理两者。

问题在于：

2D costmap 不知道机械臂伸出来。

3D PlanningScene 不一定知道底盘全局路径。

常见工程方案：

导航阶段机械臂收起，用 2D footprint。
操作阶段底盘静止，用 3D PlanningScene。
连续移动操作阶段，使用 3D 包络体或 ESDF。

这里的 ESDF（Euclidean Signed Distance Field，欧氏符号距离场）值得单独点出，因为它是把碰撞统一进 §82.16 L2 优化的关键工具。ESDF 在每个体素上存“到最近障碍的有符号距离” $d(\mathbf{x})$：障碍外为正、内为负。它对连续移动操作有两个不可替代的优点：一是**可微**——优化器可以直接用 $\nabla d$ 把“离障碍至少 $d_{\text{safe}}$”写成软/硬约束 $d(\mathbf{x})\ge d_{\text{safe}}$ 并求梯度，而 2D costmap 或离散碰撞查询不可微；二是**底盘与臂统一**——把机器人整体（底盘 + 臂连杆 + 负载）采样成一组检查点 $\{\mathbf{x}_k\}$，对每个点查同一个 ESDF，就同时覆盖了原本分属 2D 与 3D 的两套碰撞，从根上消除“footprint 安全但臂撞货架”的割裂。代价是 ESDF 需要在线增量维护（环境变化时更新距离场），这在快速变化或大尺度场景里是主要计算负担——所以静态、慢速场景仍偏好简单的分阶段碰撞模型。

陷阱框：二维安全不等于三维安全

底盘 footprint 不碰撞，只说明车体投影安全。机械臂、相机杆、负载和夹爪都可能碰到桌面或货架。移动操作规划必须明确“当前阶段使用哪种碰撞模型”。

82.19 视觉伺服的桥接作用¶

Nav2 和 MoveIt2 都偏规划。

真实抓取末端常需要闭环微调。

视觉伺服输入图像误差：

\[ e=s-s^\star \]

控制律：

\[ v_c=-\lambda L^+e \]

移动操作中，视觉伺服可输出联合速度：

\[ \nu=J^+V_c+(I-J^+J)\nu_0 \]

这样末端误差由相机闭环修正。

底盘和机械臂共同执行微调。

低速抓取阶段常用：

Nav2 导航到近处。
MoveIt2 规划到预抓取。
视觉伺服闭环到抓取。
夹爪闭合。

控制律里的 $L$ 是**交互矩阵**（image Jacobian），它把相机速度 $v_c$ 映射到图像特征变化率 $\dot s$，即 $\dot s=Lv_c$。它依赖特征深度 $Z$（点特征的 $L$ 含 $1/Z$ 项），这是视觉伺服在移动操作中需要格外小心的地方：底盘移动会改变相机到目标的距离，从而改变 $L$。若用固定深度估计，底盘大幅移动后 $L$ 会失配，伺服增益等效变化，可能引起超调或发散。工程上要么实时估计深度（RGB-D），要么把底盘速度限到使 $L$ 在一个控制周期内近似不变的范围——这与 §82.33 “接近目标时底盘降速、视觉伺服接管”是同一条工程约束的两个侧面。把交互矩阵接到联合速度 $\nu=\bar J^{+}V_c+(I-\bar J^{+}\bar J)\nu_0$ 后，相机闭环误差由底盘与臂共同消化，但**安全限幅仍应作为最高优先级**保留，防止伺服在特征丢失瞬间输出过大速度。

82.20 底盘误差对抓取的影响¶

底盘定位误差 $\delta q_b$ 会传到末端位姿。

一阶近似：

\[ \delta x_{ee}\approx J_b\delta q_b+J_a\delta q_a \]

若机械臂规划假设底盘已精确到位，但实际底盘有 5 cm 偏差，末端也会有同量级误差。

解决：

抓取前重新定位目标。
使用视觉伺服。
使用力控或导纳控制处理轻微接触。
停车时保留机械臂可调余量。
base placement 代价中惩罚靠近臂工作空间边界。

误差传播的定量刻画 ⭐⭐¶

“5 cm 底盘偏差导致同量级末端误差”这句话可以做得更精确，从而指导停车容差与可达裕量的设计。设底盘定位误差服从零均值高斯 $\delta q_b\sim\mathcal{N}(0,\Sigma_b)$，臂构型误差忽略，则末端位姿误差的一阶协方差由雅可比传播：

\[ \Sigma_{ee}\approx J_b\,\Sigma_b\,J_b^\top \]

这与卡尔曼滤波中“协方差经线性化系统传播”是同一公式。它告诉我们两件事：

误差放大由 $J_b$ 决定，而 $J_b$ 含 $e_z\times p_{be}^w$ 项。末端离底盘旋转中心越远（$\|p_{be}^w\|$ 越大），底盘偏航误差 $\delta\theta_b$ 被放大得越厉害——一个 $1^\circ$ 的底盘朝向误差，在 0.8 m 臂展处就是约 $0.8\times\tfrac{\pi}{180}\approx1.4$ cm 的末端横向偏移。这解释了为何 §82.11 的 base placement 偏好“底盘离目标近一些”：不只为可达，也为降低朝向误差的杠杆。
各方向误差不等价。$\Sigma_{ee}$ 一般是各向异性的椭球。沿“底盘前进方向”的误差可由臂前后微调廉价补偿，但绕末端轴的姿态误差可能落在臂的奇异方向上，难以补偿。因此停车容差不该是各向同性的单一数值，而应按 $\Sigma_{ee}$ 的主轴分配——这正是 §82.22 中 goal_tolerance_xy 与 goal_tolerance_yaw 分开设的深层原因。

本质洞察：底盘误差能否被容忍，取决于它落在末端任务的“敏感方向”还是“冗余方向”。落在冗余方向（臂能廉价补偿）的误差几乎免费；落在敏感方向（接近奇异、或姿态强约束）的误差直接变成抓取失败。这就是“停车时保留机械臂可调余量”的数学含义——余量必须留在 $\Sigma_{ee}$ 的大主轴方向上。

跨章综合练习（证明题）¶

结合本节误差传播与机械臂/M03 的可操作性椭球，证明：当末端处于臂的奇异位形（$\det(J_aJ_a^\top)\to0$）时，存在一个底盘误差方向 $\delta q_b$，使得末端位姿误差**无法**由任何臂关节微调 $\delta q_a$ 补偿。提示：考虑 $J_a$ 的列空间在奇异处坍缩，而 $J_b\delta q_b$ 可能落在该列空间的正交补里；请在草稿纸上写出“可补偿”的代数条件 $J_b\delta q_b\in\operatorname{range}(J_a)$，再说明奇异处为何该条件可被破坏。

82.21 接口设计：BaseArmCoordinator¶

#include <Eigen/Geometry>

#include <optional>
#include <string>
#include <vector>

struct Pose2D {
  double x = 0.0;
  double y = 0.0;
  double yaw = 0.0;
};

struct GraspCandidate {
  Eigen::Isometry3d grasp_pose;
  double score = 0.0;
};

struct BaseArmPlan {
  Pose2D base_goal;
  std::vector<double> arm_pregrasp;
  std::vector<double> arm_grasp;
  std::string explanation;
};

class BaseArmCoordinator {
 public:
  std::optional<BaseArmPlan> planPick(
      const std::vector<GraspCandidate>& grasps) {
    for (const auto& grasp : grasps) {
      const auto base_candidates = sampleBasePlacements(grasp);
      for (const auto& base : base_candidates) {
        if (!isBaseReachable(base)) {
          continue;
        }
        const auto ik = solveArmIk(base, grasp);
        if (!ik.has_value()) {
          continue;
        }
        if (!isArmCollisionFree(*ik)) {
          continue;
        }
        BaseArmPlan plan;
        plan.base_goal = base;
        plan.arm_pregrasp = ik->pregrasp;
        plan.arm_grasp = ik->grasp;
        plan.explanation = "底盘可达且机械臂 IK 与碰撞检查通过";
        return plan;
      }
    }
    return std::nullopt;
  }

 private:
  struct IkSolution {
    std::vector<double> pregrasp;
    std::vector<double> grasp;
  };

  std::vector<Pose2D> sampleBasePlacements(const GraspCandidate&) const {
    // 中文注释：实际工程中这里查询 IRM 或围绕目标采样底盘位姿。
    return {};
  }

  bool isBaseReachable(const Pose2D&) const {
    // 中文注释：调用 Nav2 规划器或 costmap 检查候选停车点。
    return true;
  }

  std::optional<IkSolution> solveArmIk(const Pose2D&,
                                       const GraspCandidate&) const {
    // 中文注释：调用 MoveIt2、Pinocchio 或专用 IK 求解器。
    return std::nullopt;
  }

  bool isArmCollisionFree(const IkSolution&) const {
    // 中文注释：调用 PlanningScene 检查机械臂和环境碰撞。
    return true;
  }
};

这个类表达的是协调逻辑。

不要把 Nav2、MoveIt2、感知和夹爪控制全部塞进一个函数。

每个检查函数都应可单独测试。

82.22 参数配置建议¶

base_arm_planning:
  base_sampling:
    radial_min_m: 0.45
    radial_max_m: 1.10
    radial_step_m: 0.05
    yaw_step_deg: 10.0
    max_candidates: 200

  reachability:
    min_manipulability: 0.03
    joint_limit_margin_rad: 0.15
    prefer_arm_nominal_weight: 1.0
    prefer_base_short_path_weight: 0.5

  navigation:
    goal_tolerance_xy_m: 0.03
    goal_tolerance_yaw_rad: 0.05
    require_final_rotation: true

  manipulation:
    pregrasp_distance_m: 0.10
    retreat_distance_m: 0.12
    allow_visual_servo: true

这里的容差会互相影响。

Nav2 停车误差越大，MoveIt2 抓取前需要的视觉伺服余量越大。

机械臂越接近工作空间边界，对底盘停车误差越敏感。

82.23 故障排查：导航到位但抓不到¶

现象：

Nav2 成功到达目标。
MoveIt2 IK 失败。
或 IK 成功但路径碰撞。

常见原因：

base placement 没有检查机械臂可达性。
底盘停车容差过宽。
目标物体位姿在导航后发生更新。
机械臂 SRDF 禁用了必要碰撞对或误启用了碰撞对。
预抓取距离设置不合理。

排查步骤：

记录底盘实际最终位姿。
用实际位姿重新求 IK。
检查目标物体在 PlanningScene 中的位置。
可视化机械臂候选构型。
降低 Nav2 目标容差。
增加视觉伺服阶段。
修改 base placement 代价，远离工作空间边界。

82.24 故障排查：IK 成功但执行撞桌¶

原因：

PlanningScene 中桌面位置错误。
深度点云延迟。
机械臂模型碰撞几何太粗或太细。
目标物体附着后没有更新碰撞体。
底盘定位漂移导致执行时场景不一致。

排查：

在 RViz 中显示 collision objects。
检查点云到 world 的 TF。
在抓取后立即 attach object。
执行前重新同步 PlanningScene。
加大桌面安全距离。
低速执行，打开碰撞预警。

82.25 故障排查：底盘频繁微调¶

现象：

抓取前底盘反复小范围移动。
机械臂规划成功率低。
任务时间过长。

原因：

base placement 代价对底盘路径过于敏感。
IK 解选择不稳定。
目标检测抖动。
停车容差小于实际定位精度。

处理：

对目标位姿做时间滤波。
加入停车位姿滞回。
若新候选提升很小，保持原候选。
停车后用机械臂和视觉伺服微调，不再频繁移动底盘。
设置最大重选次数。

82.26 故障排查：统一规划不收敛¶

统一 OCP 失败常见原因：

末端权重太高。
底盘和臂速度权重尺度不一致。
初始轨迹离目标太远。
障碍约束过硬。
末端姿态误差用欧拉角导致不连续。
差速底盘被错误建模为全向。

处理：

先只优化位置，不优化姿态。
降低末端权重，增加输入正则。
用 Nav2+MoveIt2 解作为初值。
把障碍约束先作为软约束。
使用 $SE(3)$ 对数误差。
确认底盘运动学模型正确。

82.27 练习¶

练习 A：联合雅可比手推¶

给定差速底盘和 6 轴机械臂。

要求：

写出 $T_{we}=T_{wb}T_{be}$。
推导底盘 $v,\omega$ 对末端线速度的贡献。
写出 $J_b\in\mathbb{R}^{6\times2}$。
与数值差分结果比较。

练习 B：零空间偏好¶

实现联合速度 IK。

要求：

主任务为末端直线速度。
次级任务为底盘少动。
再加入臂关节回中。
比较不同权重下底盘移动距离。
画出末端误差和底盘速度曲线。

练习 C：base placement¶

围绕桌面目标采样底盘位姿。

要求：

每 10 度采样朝向。
每 5 cm 采样距离。
用 costmap 过滤碰撞。
用 IK 过滤不可达。
用 manipulability 排序。

练习 D：Nav2 + MoveIt2 编排¶

搭建一个简化任务：

导航到桌前。
更新 PlanningScene。
规划到预抓取。
视觉伺服到目标。
闭合夹爪。
抬起物体。

要求：

每个阶段输出成功或失败原因。
抓取失败时返回重新选择停车位姿。

82.28 累积项目：Ridgeback + UR5 抓取管线¶

项目目标：完成一个可回放的移动抓取系统。

模块：

目标物体位姿输入。
抓取候选生成。
IRM 或围绕目标采样底盘位姿。
Nav2 可达性检查。
MoveIt2 IK 与碰撞检查。
行为树编排。
视觉伺服微调。
日志与失败恢复。

交付：

一张系统数据流图。
一份 base placement 参数文件。
一个联合雅可比验证脚本。
一段仿真抓取视频或日志。
一份失败案例分析。

成功标准：

候选停车点能解释排序原因。
导航失败能换候选。
IK 失败能换候选。
抓取前有视觉闭环或目标重定位。
执行日志能重放每个阶段。

82.29 本章速查¶

移动操作的构型空间是 $SE(2)\times\mathbb{R}^{n_a}$。
差速底盘不能瞬时侧移。
全向底盘更利于操作微调。
Ackermann 底盘停车约束更强。
联合雅可比由底盘列和机械臂列拼接。
底盘角速度会给末端线速度带来 $e_z\times p$ 项。
冗余度是次级任务的入口。
阻尼伪逆能缓解奇异。
零空间项可以表达底盘少动。
IRM 用于快速找可达底盘位姿。
base placement 必须同时看导航和操作。
Nav2 + MoveIt2 分层最实用。
分层架构不擅长边走边抓。
统一 OCP 适合连续移动操作。
2D footprint 安全不代表机械臂 3D 安全。
机械臂伸出会改变底盘有效占用空间。
底盘停车误差会传给末端。
视觉伺服能补偿底盘和目标误差。
目标位姿抖动会导致底盘频繁微调。
行为树适合编排多阶段任务。
MoveIt2 管机械臂场景和规划。
Nav2 管底盘地图和控制。
PlanningScene 必须及时更新。
抓取后要 attach object。
统一规划应使用 $SE(3)$ 对数误差。
差速底盘不能用全向速度直接执行。
初值对统一 OCP 收敛很重要。
分层解可作为统一 OCP 初值。
base placement 应远离臂工作空间边界。
移动操作失败通常是跨模块接口失败。

82.30 延伸阅读¶

材料	难度	阅读重点
Khatib, Operational Space Control	⭐⭐⭐	末端任务、冗余度和零空间控制的基础思想
Yamamoto 与 Yun 的 mobile manipulator 协调控制工作	⭐⭐⭐	底盘与机械臂如何共同贡献末端运动
MoveIt2 官方教程	⭐⭐	PlanningScene、碰撞检查、attach object 与执行接口
Nav2 官方教程	⭐⭐	costmap、行为树、恢复行为与导航失败诊断
复合/130_OCS2_mobile_manipulator	⭐⭐⭐	将本章联合运动学放进统一 OCP 的代价与约束结构

82.31 检查清单¶

001 底盘模型是否正确标明差速、全向或 Ackermann。
002 联合构型空间是否包含底盘 yaw。
003 末端任务是否使用统一世界坐标。
004 TF 树是否从 map 到 base 到 tool 连通。
005 底盘实际停车误差是否记录。
006 base placement 是否检查导航可达。
007 base placement 是否检查机械臂 IK。
008 base placement 是否检查机械臂碰撞。
009 base placement 是否惩罚关节限位。
010 base placement 是否惩罚奇异构型。
011 候选停车点是否有排序解释。
012 Nav2 footprint 是否与臂姿态假设一致。
013 导航阶段机械臂是否收起。
014 操作阶段底盘是否锁定或限速。
015 PlanningScene 是否包含桌面。
016 PlanningScene 是否包含目标物体。
017 抓取后是否 attach object。
018 放置后是否 detach object。
019 点云到 world 的 TF 是否延迟补偿。
020 目标检测是否滤波。
021 IK 求解器是否支持多解排序。
022 预抓取距离是否足够。
023 退让方向是否避开障碍。
024 视觉伺服是否有速度限幅。
025 视觉伺服失败是否回退规划。
026 行为树是否记录失败节点。
027 Nav2 失败是否换候选停车点。
028 MoveIt2 失败是否换抓取候选。
029 统一 OCP 是否有良好初值。
030 末端姿态误差是否用 Lie 群形式。
031 底盘速度权重和臂速度权重是否同尺度。
032 障碍约束是否先软后硬调试。
033 关节限位裕量是否足够。
034 机械臂负载是否影响速度限制。
035 夹爪开合状态是否进入场景。
036 目标物体是否有可抓取姿态集合。
037 导航目标是否留出机械臂操作空间。
038 底盘绕目标采样是否覆盖后退路径。
039 失败日志是否保存图像、点云和状态。
040 系统是否能从抓取失败返回重新规划。

82.32 小结¶

本章把移动操作的基础问题拆成三层。

第一层是联合运动学：底盘速度和臂关节速度共同产生末端 twist。

第二层是联合可达性：底盘停车点必须同时满足导航和操作。

第三层是系统编排：Nav2、MoveIt2、视觉伺服和行为树必须围绕失败恢复组织起来。

下一章将深入 OCS2 mobile_manipulator 示例，用统一 OCP 的方式把本章的运动学、代价和约束放进一个 MPC 框架。

82.33 动态抓取与边走边操作 ⭐⭐⭐¶

上面的讨论主要面向"到位后操作"的场景。但移动操作还有一类重要任务是边走边操作。

动机：为什么固定底盘后抓取不够¶

物流配送、家庭整理和工业巡检中，机器人需要连续经过多个工位，每个工位执行一次抓取或放置。如果每次都先完全停车、再规划臂、再执行，效率极低。边走边操作让底盘保持低速移动，机械臂在移动过程中完成操作。

核心困难¶

底盘运动使机械臂的工作基准持续变化。末端 twist 中底盘贡献不断变化方向和幅值。如果规划器假设底盘速度为零或常数，实际执行时末端会漂移。

联合雅可比 $J(q)$ 是构型相关的。底盘旋转 $\theta_b$ 改变 $J_b$ 中的 $R_{wb}$ 和 $e_z\times p_{be}^w$。因此在底盘持续转向时，雅可比的变化速度更快，阻尼伪逆需要更频繁更新。

工程方案¶

底盘提供缓慢、平滑的全局位移。
MPC 或速度 IK 以 50-200 Hz 频率更新联合速度。
抓取最后阶段底盘降速或暂停。
视觉伺服在末端接近目标时接管闭环。
底盘速度限幅在操作阶段收紧。

反事实推理：如果底盘不降速直接抓取，底盘定位误差、IMU 延迟和末端反馈延迟会叠加。1 m/s 底盘速度配合 50 ms 延迟就是 5 cm 末端偏差——对精细抓取不可接受。因此边走边操作通常在接近目标时把底盘速度降到 0.1-0.3 m/s。

跨章综合练习¶

结合本章的联合雅可比和第 83 章（130_OCS2_mobile_manipulator.md）的 OCS2 末端代价，设计一个边走边抓的 MPC 问题：

底盘沿直线以 0.3 m/s 前进。
机械臂末端要在 2 秒内到达固定世界位姿目标。
写出状态、输入和代价函数。
分析底盘速度对末端跟踪误差的影响。
比较底盘在抓取前停止 vs 不停止的方案。

82.34 🔧 故障排查手册¶

症状	可能原因	排查步骤	相关小节
导航到位但 IK 失败	base placement 没检查臂可达性	1. 记录底盘实际位姿 2. 用实际位姿重新求 IK 3. 可视化候选构型	82.23
IK 成功但执行撞桌	PlanningScene 桌面位置错误	1. RViz 显示 collision objects 2. 检查点云 TF 3. attach object	82.24
底盘频繁微调	目标检测抖动或候选代价过敏感	1. 目标位姿时间滤波 2. 停车位姿滞回 3. 限制最大重选次数	82.25
统一规划不收敛	末端权重太高或初值差	1. 先只优化位置 2. 降低末端权重 3. 用分层解作初值	82.26
机械臂伸出碰到门框	2D footprint 不包含臂	1. 动态更新 footprint 2. 导航阶段收臂 3. 使用 3D 碰撞	82.18
视觉伺服不收敛	相机外参或目标特征错误	1. 检查 hand-eye 标定 2. 降低伺服增益 3. 打印图像误差	82.19
阻尼伪逆把横向分量丢给差速底盘	用了全自由度雅可比而非约简雅可比	1. 确认变量是 $(v,\omega,\dot q_a)$ 而非 $(\dot x_b,\dot y_b,\dot\theta_b,\dot q_a)$ 2. 用 $\bar J=J_{\text{full}}S$ 重写 3. 检查 $S$ 的列是否满足 $\mathbf{A}S=0$	82.6+
低优先级任务污染末端跟踪	用加权和而非任务优先级/HQP	1. 把安全约束移到最高优先级 2. 末端任务用零空间投影隔离低优先级 3. 减少层数到 3-4	82.8+
末端在底盘转向时漂移	雅可比含 $R_{wb}$，更新频率不足	1. 提高 IK 频率到 100-200 Hz 2. 底盘转向时降速 3. 接近目标用视觉伺服接管	82.33

82.35 本章常见误解汇总¶

#	常见误解	正确理解	对应节
1	移动操作 = 导航 + 抓取两步拼起来	底盘位姿与臂构型共同服务末端任务；底盘把臂的工作空间“搬”到合适位置	§82.2
2	差速底盘雅可比只比全向少一列、本质相同	差速 $J_b$ 已把非完整约束吸收进准速度，是 $J_{\text{full}}S$ 的结果，不能独立命令横向速度	§82.6+
3	非完整约束可以“先按全向求解、再把横向置零”	置零不是约束流形上的投影，会破坏末端任务满足度；必须用约简雅可比在求解前限制变量	§82.6+
4	冗余自由度是“多余的浪费”	冗余是工程偏好的入口；零空间项把“我更希望谁动”写进数学	§82.7–82.8
5	阻尼因子 $\lambda$ 越小越精确	$\lambda\to0$ 在奇异处速度发散；应随最小奇异值自适应	§82.8
6	多个任务用加权和即可，调权重就行	加权和无硬优先级，低优先级以 $O(w_i/w_1)$ 污染高优先级；任务优先级用零空间投影杜绝污染	§82.8+
7	任务优先级层数越多越好	层数多则可行子空间收缩快、切换抖动；移动操作 3–4 级足够	§82.8+
8	可操作性只能离线给 base placement 打分	其梯度可作零空间速度，在线连续远离奇异（反应式控制）	§82.8+
9	端到端学习后运动学就没用了	学习替换的是“求解器”不是“问题定义”；观测/奖励/动作空间全靠运动学	§82.17+
10	底盘 footprint 不碰撞就安全	2D 投影安全 $\ne$ 3D 安全；臂、相机杆、负载都可能撞	§82.18
11	base placement 只看距离够不够	必须同时看导航可达、IK 可达、碰撞、限位、奇异、视野；底盘朝向进入可操作性	§82.10–82.11
12	MoveIt2 用 planar joint 就能正确表达差速底盘	planar joint 允许横向，对差速会规划出不可执行的侧移；需额外非完整约束或只用于采样	§82.14

82.36 本章小结附加组件¶

符号表¶

符号	含义	首次出现
$q_b=(x_b,y_b,\theta_b)$	底盘位姿，属 $SE(2)$	§82.3
$q_a\in\mathbb{R}^{n_a}$	机械臂关节角向量	§82.3
$T_{we}=T_{wb}T_{be}$	末端在世界系的位姿（$T_{ab}$：$b\to a$ 变换）	§82.3
$u_b$	底盘控制输入（差速 $(v,\omega)$ / 全向 $(v_x,v_y,\omega)$）	§82.4
$\nu=(u_b,\dot q_a)$	可控准速度向量（非 $\dot q$）	§82.5
$S(q_b)$	差速底盘约简映射，列张成非完整约束零空间，$\mathbf{A}S=0$	§82.6+
$\bar S(q)$	全身约简选择矩阵，$\dot q=\bar S\nu$	§82.6+
$\mathbf{A}(q_b)$	Pfaffian 约束矩阵 $(\sin\theta_b,-\cos\theta_b,0)$	§82.4
$J_b,\,J_a$	联合雅可比的底盘块与机械臂块	§82.6
$\bar J=J_{\text{full}}\bar S$	约简雅可比（$6\times(2+n_a)$，约束已吸收）	§82.6+
$r=\dim(\nu)-m$	冗余度（$m$ 为任务维度）	§82.7
$\lambda$	阻尼伪逆阻尼因子	§82.8
$N_i=I-(J_iN_{i-1})^{+}(J_iN_{i-1})$	前 $i$ 级任务联合零空间投影矩阵	§82.8+
$w_{\text{nh}}=\sqrt{\det(\bar J\bar J^\top)}$	非完整移动操作的 Yoshikawa 可操作性	§82.6+
$\mathrm{IRM}$	Inverse Reachability Map，反向可达图	§82.10
$V_d,\,V_{ee}^w$	期望 / 实际末端 twist（$\in\mathbb{R}^6$）	§82.6,82.8
$L$	视觉伺服交互矩阵（image Jacobian），$\dot s=Lv_c$，含 $1/Z$	§82.19
$\Sigma_{ee}=J_b\Sigma_b J_b^\top$	底盘误差到末端误差的一阶协方差传播	§82.20

定理速查表¶

定理 / 公式	一句话说明	对应节
联合构型空间 $SE(2)\times\mathbb{R}^{n_a}$	移动操作的状态既含底盘 $SE(2)$ 又含臂关节，维度 $3+n_a$	§82.3
联合雅可比 $V_{ee}^w=[J_b\ J_a]\nu$	末端 twist 是底盘列与臂列的线性叠加	§82.6
约简雅可比 $\bar J=J_{\text{full}}\bar S$	右乘 $\bar S$ 把非完整约束结构性吸收，伪逆/零空间公式可原样套用	§82.6+
阻尼伪逆 $\nu^\star=J^\top(JJ^\top+\lambda^2I)^{-1}V_d$	用 $\lambda$ 换取奇异处数值稳定，最小范数解	§82.8
零空间投影 $\nu=J^+V_d+(I-J^+J)\nu_0$	次级目标 $\nu_0$ 不一阶影响末端任务	§82.8
递归任务优先级 $N_i=N_{i-1}-(J_iN_{i-1})^{+}(J_iN_{i-1})$	每级活在更高级零空间内，硬优先级、不靠权重	§82.8+
HQP 逐级 QP	每级带等式/不等式，残差传下级；可在任意层级加不等式且实时	§82.8+
可操作性 $w=\sqrt{\det(\bar J\bar J^\top)}$	衡量当前位形产生末端运动的能力；其梯度可在线避奇异	§82.11,82.8+
base placement 代价 $J=\sum w_i c_i$	综合导航距离、costmap、IK、限位、视野的加权停车评分	§82.11

知识点总表¶

#	知识点	核心要点	对应节	难度
1	移动操作基本矛盾	底盘移动改变臂工作空间	§82.2	⭐
2	联合构型空间	$SE(2)\times\mathbb{R}^{n_a}$	§82.3	⭐⭐
3	三类底盘运动学	差速/全向/Ackermann	§82.4	⭐⭐
4	准速度与约简映射	$\nu\ne\dot q$，$S$ 吸收约束	§82.5,82.6+	⭐⭐⭐
5	联合雅可比	底盘列 + 臂列拼接	§82.6	⭐⭐⭐
6	约简雅可比	$\bar J=J_{\text{full}}\bar S$	§82.6+	⭐⭐⭐
7	冗余与零空间	次级目标入口	§82.7,82.8	⭐⭐
8	阻尼伪逆	奇异鲁棒最小范数解	§82.8	⭐⭐⭐
9	任务优先级 / HQP	嵌套零空间、逐级 QP	§82.8+	⭐⭐⭐
10	IRM 与 base placement	离线可达图 + 在线评分	§82.10,82.11	⭐⭐
11	Nav2 + MoveIt2 分层	成熟、模块清晰、不擅边走边抓	§82.12	⭐⭐
12	三层耦合 L0–L2	串行 / 松耦合 / 统一 OCP	§82.16	⭐⭐
13	学习类路线 A–C	N$^2$M$^2$ / HarmonicMM / Mobile ALOHA	§82.17+	⭐⭐
14	两个碰撞空间	2D costmap vs 3D PlanningScene	§82.18	⭐⭐
15	边走边操作	雅可比时变、底盘降速、视觉接管	§82.33	⭐⭐⭐

82.37 本章与后续章节的关系¶

后续章节	与本章的关系	本章哪个知识点为其铺垫
复合/130 OCS2 mobile_manipulator	把本章联合运动学、末端代价、约束放进统一 OCP/MPC	§82.6 联合雅可比、§82.16 L2、§82.33 边走边
复合/20 浮动基座臂动力学	从“速度级运动学”升级到“力/力矩级动力学”，处理接触与负载	§82.5 准速度（$n_q\ne n_v$ 思想）、§82.6+ 约简映射
足式/240 WBC	同一套分层求解思想下沉到力矩级整机控制	§82.8+ 任务优先级 / HQP
机械臂/M14 MoveIt2	本章把单臂 MoveIt2 接口扩展到“底盘作虚拟关节”的联合规划	§82.14 planar joint、§82.18 碰撞空间
具身智能方向	端到端 / 模仿学习策略的动作空间、奖励、可行性定义	§82.17+ 学习路线 A–C

82.38 研究实践建议¶

给新手的建议：

先把“到位后抓取”跑通再碰边走边抓。L0 分层（Nav2 到位后再用 MoveIt2 抓取）是一切的基线，能暴露 80% 的接口与 TF 问题。不要一上来就上统一 OCP。
永远记录底盘实际停车位姿。绝大多数“IK 失败 / 撞桌”问题，用实际位姿（而非目标位姿）重算一遍就能定位（§82.23）。
把安全约束写在最硬的优先级。无论用 QP、HQP 还是学习策略外层，速度限幅与避障都应是“不可被任务挤掉”的硬层（§82.8+ P0）。
数值差分验证联合雅可比。手推 $\bar J$ 后，务必用 $\bar J\nu$ 与正运动学有限差分对照（练习 A），符号/约定错误几乎都在此暴露。

给有经验者的建议：

用约简雅可比统一非完整与全向代码路径。把底盘类型抽象成 $S(q_b)$ 一个矩阵，差速 / 全向 / Ackermann 只换 $S$，上层 IK/QP 代码完全复用（§82.6+）。
在线可操作性梯度优于离线 IRM 重查。当任务连续、环境缓变时，把 §82.11 的离线打分换成 §82.8+ 的在线可操作性零空间项，避免底盘频繁重选（§82.25）。
分层解是统一 OCP 的最佳初值。复合/130 的统一 OCP 收敛性高度依赖初值；直接拿 L1 的 base placement + MoveIt2 轨迹做 warm start（§82.26）。
评估学习方法时先定位它在 A–C 哪一档。读论文先问“底盘学还是臂学、模型保留多少”，再判断它的安全兜底与数据成本，避免被“端到端”标签误导（§82.17+）。

82.39 版本信息速查¶

工具 / 库	推荐版本	在本章中的角色
ROS 2	Humble / Jazzy (LTS)	Nav2 与 MoveIt2 的运行时基座
Nav2	与 ROS 2 发行版匹配	底盘 costmap、行为树、导航与恢复
MoveIt2	与 ROS 2 发行版匹配	臂 PlanningScene、IK、碰撞、执行
Pinocchio	$\ge$ 2.6	联合 / 约简雅可比、可操作性梯度、$SE(3)$ 对数误差
Eigen	$\ge$ 3.4	伪逆、`ldlt` 求解、几何变换
OSQP / qpOASES	OSQP $\ge$ 0.6	速度级 QP / HQP 后端求解
OCS2	主线（见复合/130）	L2 统一 OCP / MPC 框架

说明：Nav2 / MoveIt2 的版本应严格跟随所用 ROS 2 发行版，跨版本混用易导致消息/接口不兼容。本章代码片段以 C++17 + Eigen 表达，不依赖特定 ROS 2 小版本。

符号	含义	首次出现
\(q_b=(x_b,y_b,\theta_b)\)	底盘位姿，属 \(SE(2)\)	§82.3
\(q_a\in\mathbb{R}^{n_a}\)	机械臂关节角向量	§82.3
\(T_{we}=T_{wb}T_{be}\)	末端在世界系的位姿（\(T_{ab}\)：\(b\to a\) 变换）	§82.3
\(u_b\)	底盘控制输入（差速 \((v,\omega)\) / 全向 \((v_x,v_y,\omega)\)）	§82.4
\(\nu=(u_b,\dot q_a)\)	可控准速度向量（非 \(\dot q\)）	§82.5
\(S(q_b)\)	差速底盘约简映射，列张成非完整约束零空间，\(\mathbf{A}S=0\)	§82.6+
\(\bar S(q)\)	全身约简选择矩阵，\(\dot q=\bar S\nu\)	§82.6+
\(\mathbf{A}(q_b)\)	Pfaffian 约束矩阵 \((\sin\theta_b,-\cos\theta_b,0)\)	§82.4
\(J_b,\,J_a\)	联合雅可比的底盘块与机械臂块	§82.6
\(\bar J=J_{\text{full}}\bar S\)	约简雅可比（\(6\times(2+n_a)\)，约束已吸收）	§82.6+
\(r=\dim(\nu)-m\)	冗余度（\(m\) 为任务维度）	§82.7
\(\lambda\)	阻尼伪逆阻尼因子	§82.8
\(N_i=I-(J_iN_{i-1})^{+}(J_iN_{i-1})\)	前 \(i\) 级任务联合零空间投影矩阵	§82.8+
\(w_{\text{nh}}=\sqrt{\det(\bar J\bar J^\top)}\)	非完整移动操作的 Yoshikawa 可操作性	§82.6+
\(\mathrm{IRM}\)	Inverse Reachability Map，反向可达图	§82.10
\(V_d,\,V_{ee}^w\)	期望 / 实际末端 twist（\(\in\mathbb{R}^6\)）	§82.6,82.8
\(L\)	视觉伺服交互矩阵（image Jacobian），\(\dot s=Lv_c\)，含 \(1/Z\)	§82.19
\(\Sigma_{ee}=J_b\Sigma_b J_b^\top\)	底盘误差到末端误差的一阶协方差传播	§82.20

#	问题	建议回顾
1	\(SE(2)\) 位姿 \((x,y,\theta)\) 的速度输入如何写成差速底盘模型？	复合/60
2	机械臂雅可比 \(J(q)\) 如何把关节速度映射到末端 twist？	机械臂/M03
3	OMPL 在构型空间规划的基本形式是什么？	机械臂/M07
4	MoveIt2 的 PlanningScene 管理什么？	机械臂/M14
5	Nav2 中全局规划器和局部控制器分别做什么？	SLAM/Nav2

优先级	任务	类型	雅可比 / 约束
P0	关节/底盘速度限幅、避障距离	不等式（最硬）	\(A\nu\le b\)
P1	末端位置跟踪	等式	\(\bar J_{\text{pos}}\,\nu=\dot p^d\)
P2	末端姿态跟踪	等式	\(\bar J_{\text{ori}}\,\nu=\omega^d\)
P3	远离关节限位 / 提升可操作性	等式（梯度方向）	\(\nabla_\nu(\cdot)\)
P4	底盘少动 / 臂回中	等式（最软）	\(\nu\to\nu_0\)

代价	含义
\(d_{\mathrm{nav}}\)	底盘到候选点的导航距离
\(c_{\mathrm{costmap}}\)	候选点附近障碍代价
\(c_{\mathrm{ik}}\)	IK 残差或奇异性
\(c_{\mathrm{limit}}\)	关节接近限位程度
\(c_{\mathrm{view}}\)	相机视角和遮挡

定理 / 公式	一句话说明	对应节
联合构型空间 \(SE(2)\times\mathbb{R}^{n_a}\)	移动操作的状态既含底盘 \(SE(2)\) 又含臂关节，维度 \(3+n_a\)	§82.3
联合雅可比 \(V_{ee}^w=[J_b\ J_a]\nu\)	末端 twist 是底盘列与臂列的线性叠加	§82.6
约简雅可比 \(\bar J=J_{\text{full}}\bar S\)	右乘 \(\bar S\) 把非完整约束结构性吸收，伪逆/零空间公式可原样套用	§82.6+
阻尼伪逆 \(\nu^\star=J^\top(JJ^\top+\lambda^2I)^{-1}V_d\)	用 \(\lambda\) 换取奇异处数值稳定，最小范数解	§82.8
零空间投影 \(\nu=J^+V_d+(I-J^+J)\nu_0\)	次级目标 \(\nu_0\) 不一阶影响末端任务	§82.8
递归任务优先级 \(N_i=N_{i-1}-(J_iN_{i-1})^{+}(J_iN_{i-1})\)	每级活在更高级零空间内，硬优先级、不靠权重	§82.8+
HQP 逐级 QP	每级带等式/不等式，残差传下级；可在任意层级加不等式且实时	§82.8+
可操作性 \(w=\sqrt{\det(\bar J\bar J^\top)}\)	衡量当前位形产生末端运动的能力；其梯度可在线避奇异	§82.11,82.8+
base placement 代价 \(J=\sum w_i c_i\)	综合导航距离、costmap、IK、限位、视野的加权停车评分	§82.11