控制算法与抓取规划是灵巧手系统的”大脑”，决定了机械手如何精确、稳定、智能地完成复杂操作任务。本章将从底层的关节控制开始，逐步上升到高层的任务规划，系统介绍灵巧手控制的完整技术栈。我们将深入探讨经典控制理论在灵巧手中的应用，包括阻抗控制如何实现柔顺交互，抓取规划如何处理几何与物理约束，以及现代机器学习方法如何让灵巧手具备自主学习能力。通过本章学习，读者将掌握从PID到强化学习的多层次控制方法，理解从抓取到操作的完整规划流程，并能够设计适应不同任务需求的控制系统。
关节空间控制是最基础的控制层，直接作用于每个关节的电机或执行器。对于一个n自由度的灵巧手，关节空间控制器的目标是让关节角度向量 $\mathbf{q} = [q_1, q_2, …, q_n]^T$ 跟踪期望轨迹 $\mathbf{q}_d(t)$。
最常用的关节控制器是PID控制器，其控制律为：
\[\tau_i = K_{p,i}(q_{d,i} - q_i) + K_{d,i}(\dot{q}_{d,i} - \dot{q}_i) + K_{i,i}\int_0^t (q_{d,i} - q_i)dt\]其中 $\tau_i$ 是第i个关节的控制力矩，$K_{p,i}$、$K_{d,i}$、$K_{i,i}$ 分别是比例、微分、积分增益。
对于灵巧手这样的多关节系统，需要考虑关节间的耦合效应。完整的动力学方程为：
\[\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{G}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}^T\mathbf{F}_{ext}\]其中：
为了处理动力学耦合，可以使用计算力矩控制（Computed Torque Control）：
\[\boldsymbol{\tau} = \mathbf{M}(\mathbf{q})[\ddot{\mathbf{q}}_d + \mathbf{K}_d(\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) + \mathbf{K}_p(\mathbf{q}_d - \mathbf{q})] + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{G}(\mathbf{q})\]这种控制器通过动力学模型的逆运算来线性化和解耦系统，但需要精确的动力学模型。
笛卡尔空间控制直接控制末端执行器（指尖）的位置和姿态，更符合任务描述的自然方式。
对于位置控制，使用雅可比矩阵建立关节速度与笛卡尔速度的关系：
\[\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}\]通过雅可比伪逆计算关节速度指令：
\[\dot{\mathbf{q}} = \mathbf{J}^{\dagger}(\dot{\mathbf{x}}_d + \mathbf{K}_p(\mathbf{x}_d - \mathbf{x}))\]其中 $\mathbf{J}^{\dagger} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T)^{-1}$ 是雅可比伪逆。
当雅可比矩阵接近奇异时，需要使用阻尼最小二乘法：
\[\mathbf{J}^{\dagger} = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T + \lambda^2\mathbf{I})^{-1}\]其中 $\lambda$ 是阻尼因子，可以根据可操作度 $w = \sqrt{\det(\mathbf{J}\mathbf{J}^T)}$ 自适应调整：
\[\lambda = \begin{cases} 0 & \text{if } w > w_{threshold} \\ \lambda_0(1 - \frac{w}{w_{threshold}})^2 & \text{if } w \leq w_{threshold} \end{cases}\]灵巧手的多个手指需要协调运动来完成抓取任务。协调控制的关键是处理运动学和动力学约束。
将被抓物体视为虚拟连杆，多指系统形成闭链机构：
物体
/ | \
/ | \
指1 指2 指3
| | |
掌部基座
闭链约束方程：
\[\mathbf{G}(\mathbf{q})\dot{\mathbf{q}} = \mathbf{0}\]结合开链动力学，得到约束动力学方程：
\[\begin{bmatrix} \mathbf{M} & \mathbf{G}^T \\ \mathbf{G} & \mathbf{0} \end{bmatrix} \begin{bmatrix} \ddot{\mathbf{q}} \\ \boldsymbol{\lambda} \end{bmatrix} = \begin{bmatrix} \boldsymbol{\tau} - \mathbf{h} \\ -\dot{\mathbf{G}}\dot{\mathbf{q}} \end{bmatrix}\]其中 $\boldsymbol{\lambda}$ 是拉格朗日乘子，表示约束力。
阻抗控制不直接控制位置或力，而是控制位置与力之间的动态关系，实现柔顺交互。期望的阻抗模型为：
\[\mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{D}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{F}_{ext}\]其中 $\mathbf{M}_d$、$\mathbf{D}_d$、$\mathbf{K}_d$ 分别是期望的惯性、阻尼和刚度矩阵。
控制律设计：
\[\boldsymbol{\tau} = \mathbf{J}^T[\mathbf{M}_d\ddot{\mathbf{x}}_r + \mathbf{D}_d\dot{\mathbf{x}}_e + \mathbf{K}_d\mathbf{x}_e - \mathbf{F}_{ext}] + \mathbf{g}(\mathbf{q})\]其中：
导纳控制是阻抗控制的对偶形式，输入力输出位置：
\[\ddot{\mathbf{x}} = \mathbf{M}_d^{-1}[\mathbf{F}_{ext} - \mathbf{D}_d\dot{\mathbf{x}} - \mathbf{K}_d\mathbf{x}]\]导纳控制适用于位置控制精度高但力控制能力有限的系统。
实现框架：
根据任务需求动态调整阻抗参数：
\[\mathbf{K}_d = \mathbf{K}_0 + \alpha(\mathbf{F}_{ext})\mathbf{K}_1\]其中 $\alpha$ 是基于接触力的调节函数。例如，在自由空间运动时降低刚度以提高速度，接触时增加刚度以提高精度。
使用强化学习优化阻抗参数：
\[J = \sum_{t=0}^T [w_1||\mathbf{x}_e||^2 + w_2||\mathbf{F}_{ext} - \mathbf{F}_d||^2 + w_3||\mathbf{K}_d||^2]\]| 通过策略梯度方法学习最优的阻抗调节策略 $\pi(\mathbf{K}_d | \mathbf{s})$。 |
点接触模型分类：
接触约束矩阵：
\[\mathbf{W} = \begin{bmatrix} \mathbf{W}_1 & \mathbf{W}_2 & ... & \mathbf{W}_k \end{bmatrix}\]其中 $\mathbf{W}_i$ 是第i个接触点的约束矩阵。
力闭合判据： 抓取矩阵 $\mathbf{G} = \mathbf{W}$ 的秩为6，且原点在接触力凸包内部。
Q1度量（最小扳手）：
\[Q_1 = \min_{\mathbf{w} \in \partial B} \max_{\mathbf{f} \in FC} \{\lambda : \lambda\mathbf{w} = \mathbf{Gf}\}\]其中 $\partial B$ 是单位扳手球面，$FC$ 是摩擦锥约束集。
Q2度量（最大内切球）：
\[Q_2 = \max\{r : B_r \subseteq GWS\}\]其中 $GWS$ 是抓取扳手空间。
基于物体几何形状的解析计算：
优化问题：
\[\begin{align} \max_{\mathbf{p}_i, \mathbf{n}_i} & \quad Q(\mathbf{p}_1, ..., \mathbf{p}_k) \\ \text{s.t.} & \quad \mathbf{p}_i \in \partial\Omega \\ & \quad \mathbf{n}_i \perp T_{\mathbf{p}_i}\partial\Omega \end{align}\]均匀采样：
for i = 1 to N:
p = sample_surface_point()
n = compute_normal(p)
approach = sample_approach_direction(n)
grasp = compute_grasp_pose(p, n, approach)
quality = evaluate_grasp(grasp)
if quality > threshold:
add_to_grasp_set(grasp)
启发式采样：
网络架构：
输入：深度图 + 抓取候选
↓
CNN特征提取
↓
空间变换网络（对齐抓取方向）
↓
全连接层
↓
输出：抓取成功概率
训练数据生成：
直接从图像预测抓取位姿：
\[\mathbf{g} = f_{\theta}(\mathbf{I})\]其中 $\mathbf{g} = (x, y, z, \theta, w, q)$ 包含位置、角度、开口宽度和质量分数。
损失函数：
\[L = \lambda_1 L_{pos} + \lambda_2 L_{angle} + \lambda_3 L_{width} + \lambda_4 L_{quality}\]重抓取（Regrasping）是通过改变抓取配置来调整物体位姿的过程。
节点：稳定抓取配置 边：可行的抓取转换
G1 --放置--> 桌面 --抓取--> G2
| |
└-------手内操作----------→┘
转换可行性判断：
A*搜索：
\[f(n) = g(n) + h(n)\]其中：
RRT-Connect： 适用于高维配置空间的概率完备规划器。
手内操作（In-hand Manipulation）通过手指的协调运动改变物体位姿。
运动学约束：
混合整数规划（MIP）formulation：
\[\begin{align} \min & \quad \sum_i ||\mathbf{f}_i||^2 + \sum_j c_j \\ \text{s.t.} & \quad \text{静力平衡} \\ & \quad \text{摩擦锥约束} \\ & \quad \text{接触模式约束} \end{align}\]类似于多足机器人的步态，手指交替移动：
时刻1: [接触, 接触, 移动]
时刻2: [接触, 移动, 接触]
时刻3: [移动, 接触, 接触]
稳定性约束：始终保持足够的接触点维持力闭合。
定义基本操作原语库：
原语组合实现复杂任务：
def reorient_object(target_pose):
current = get_object_pose()
if large_rotation_needed(current, target):
finger_gaiting(intermediate_pose)
pivot_rotation(align_axis)
fine_adjustment(target_pose)
直接从专家演示学习策略：
\[\pi_{\theta} = \arg\min_{\theta} \mathbb{E}_{(\mathbf{s}, \mathbf{a}) \sim \mathcal{D}} [||\pi_{\theta}(\mathbf{s}) - \mathbf{a}||^2]\]数据收集方法：
从演示中学习奖励函数：
\[R_{\psi}^* = \arg\max_{\psi} \mathbb{E}_{\tau \sim \pi_E}[R_{\psi}(\tau)] - \mathbb{E}_{\tau \sim \pi_{\psi}}[R_{\psi}(\tau)]\]其中 $\pi_E$ 是专家策略，$\pi_{\psi}$ 是学习到的策略。
状态空间：
动作空间：
奖励设计：
\[r_t = r_{task} + r_{contact} + r_{smooth} + r_{safety}\]近端策略优化（PPO）的目标函数：
\[L^{CLIP}(\theta) = \mathbb{E}_t[\min(r_t(\theta)\hat{A}_t, \text{clip}(r_t(\theta), 1-\epsilon, 1+\epsilon)\hat{A}_t)]\]| 其中 $r_t(\theta) = \frac{\pi_{\theta}(a_t | s_t)}{\pi_{\theta_{old}}(a_t | s_t)}$ 是重要性采样比。 |
逐步增加任务难度：
成功率阈值触发：
\[\text{if } \text{success\_rate} > \tau \text{ then } \text{level} += 1\]随机化参数：
def randomize_simulation():
friction = uniform(0.5, 1.5)
mass = uniform(0.8, 1.2) * nominal_mass
delay = uniform(0, 50) # ms
noise = normal(0, 0.01)
return env_with_params(friction, mass, delay, noise)
使用对抗训练对齐仿真和真实数据分布：
\[\begin{align} L_{task} &= \mathbb{E}_{s \sim p_{sim}}[L_{RL}(\pi(s))] \\ L_{disc} &= \mathbb{E}_{s \sim p_{real}}[\log D(s)] + \mathbb{E}_{s \sim p_{sim}}[\log(1-D(s))] \\ L_{total} &= L_{task} - \lambda L_{disc} \end{align}\]OpenAI在2019年展示的单手解魔方系统是灵巧操作的里程碑。
硬件配置：
控制架构：
视觉感知 → 状态估计 → 高层规划 → 低层控制
↓ ↓ ↓ ↓
魔方识别 位姿跟踪 Kociemba算法 关节控制
动态调整随机化范围：
if success_rate > threshold_upper:
increase_randomization_range()
elif success_rate < threshold_lower:
decrease_randomization_range()
随机化维度：
高层策略：输出目标魔方朝向 低层策略：执行单个旋转动作
训练策略：
关键创新：
扩散模型通过学习数据分布的逆扩散过程生成高质量轨迹。
前向过程（加噪）：
\[q(\mathbf{x}_t|\mathbf{x}_{t-1}) = \mathcal{N}(\mathbf{x}_t; \sqrt{1-\beta_t}\mathbf{x}_{t-1}, \beta_t\mathbf{I})\]逆向过程（去噪）：
\[p_{\theta}(\mathbf{x}_{t-1}|\mathbf{x}_t) = \mathcal{N}(\mathbf{x}_{t-1}; \mu_{\theta}(\mathbf{x}_t, t), \Sigma_{\theta}(\mathbf{x}_t, t))\]给定任务描述 $\mathbf{c}$（如目标位姿、语言指令），生成操作轨迹：
\[p(\tau|\mathbf{c}) = \int p(\tau|\mathbf{x}_0)p(\mathbf{x}_0|\mathbf{c})d\mathbf{x}_0\]网络架构：
任务编码器：
语言/图像 → Transformer → 任务嵌入
轨迹去噪器：
[噪声轨迹, 任务嵌入, 时间步] → U-Net → 预测噪声
输出：
去噪后的关节轨迹
在扩散过程中引入物理约束：
\[\mathbf{x}_{t-1} = \text{Denoise}(\mathbf{x}_t) + \lambda\nabla_{\mathbf{x}}C(\mathbf{x}_t)\]其中 $C(\mathbf{x})$ 是约束满足度（如接触保持、避障）。
训练损失：
\[L = ||\epsilon - \epsilon_{\theta}(\mathbf{x}_t, t, \mathbf{c})||^2 + \lambda_{physics}L_{constraints}\]使用DDIM（Denoising Diffusion Implicit Models）加速：
\[\mathbf{x}_{t-1} = \sqrt{\alpha_{t-1}}\left(\frac{\mathbf{x}_t - \sqrt{1-\alpha_t}\epsilon_{\theta}}{\sqrt{\alpha_t}}\right) + \sqrt{1-\alpha_{t-1}}\epsilon_{\theta}\]可以跳过中间步骤，将1000步降至50步。
生成后使用轨迹优化平滑：
\[\min_{\tau} \sum_t ||\tau_t - \tau_t^{diffusion}||^2 + \lambda_{smooth}||\tau_{t+1} - 2\tau_t + \tau_{t-1}||^2\]subject to：运动学约束、碰撞避免
语言条件操作生成： 输入：”旋转物体90度然后放在桌子左边” 输出：完整的操作轨迹序列
多模态条件： 结合视觉目标和触觉约束生成抓取策略
风格迁移： 学习不同操作风格（快速/谨慎、有力/轻柔）
性能指标：
本章系统介绍了灵巧手的控制算法与抓取规划技术：
核心概念：
关键技术要点：
实践指导：
通过本章学习，读者应能够：
下一章将探讨系统集成与实际应用，将这些算法落地到真实的灵巧手系统中。
8.1 PID参数调节 一个单关节系统的传递函数为 $G(s) = \frac{1}{s^2 + 2s + 1}$，设计PID控制器使闭环系统的超调量小于10%，调节时间小于2秒。
8.2 雅可比矩阵计算 对于平面2R机械臂，连杆长度为 $l_1 = l_2 = 0.5m$，关节角度 $q_1 = 30°$，$q_2 = 45°$，计算末端执行器的雅可比矩阵和可操作度。
8.3 阻抗控制参数选择 设计一个阻抗控制器，使机械手在自由空间快速运动（低阻抗），接触时保持 $F = 10N$ 的接触力（高刚度）。给出刚度矩阵 $\mathbf{K}_d$ 的切换策略。
8.4 抓取质量评估 三指抓取一个半径 $r = 5cm$ 的圆柱体，接触点均匀分布在圆周上，摩擦系数 $\mu = 0.5$。计算该抓取的力闭合性和Q1质量度量。
8.5 多指协调控制设计 设计一个三指灵巧手的协调控制器，实现对球形物体的操作。要求：
8.6 强化学习训练策略 使用PPO算法训练一个灵巧手完成”瓶盖拧开”任务。设计：
8.7 扩散模型轨迹生成 实现一个条件扩散模型，根据语言描述生成操作轨迹。例如输入”轻柔地拿起杯子并倒水”，输出相应的关节轨迹。
8.8 开放性思考题 讨论如何将大语言模型（LLM）集成到灵巧手控制系统中，实现自然语言指令到复杂操作的端到端执行。考虑以下方面：