机械臂控制是现代机器人学的核心内容，涵盖从工业制造到医疗手术、从太空探索到家庭服务的广泛应用。本章将系统介绍机械臂的运动学、动力学建模，深入探讨力矩控制、阻抗控制等先进控制策略，并通过KUKA、Franka、达芬奇等实际系统案例，展示如何设计高性能的机械臂控制系统。我们将特别关注接触丰富任务中的控制挑战，以及如何通过巧妙的控制设计实现精密操作。
机械臂运动学描述关节变量与末端执行器位姿之间的几何关系。Denavit-Hartenberg（DH）参数提供了一种系统化的坐标系建立方法。
对于n自由度机械臂，第i个连杆的DH参数包括：
相邻坐标系之间的齐次变换矩阵：
\[^{i-1}T_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}\]正运动学通过连乘变换矩阵得到：
\[^0T_n = ^0T_1 \cdot ^1T_2 \cdots ^{n-1}T_n\]逆运动学求解给定末端位姿对应的关节角度，是机械臂控制的关键问题。
解析法：对于特殊结构（如球形腕、相交轴等），可得到封闭形式解。以6自由度机械臂为例，常用Pieper方法：
数值法：对于一般结构，使用雅可比迭代法：
\[\Delta q = J^{+}(q) \cdot \Delta x\]其中$J^{+}$是雅可比矩阵的伪逆。为提高收敛性，常用阻尼最小二乘法：
\[\Delta q = J^T(JJ^T + \lambda^2 I)^{-1} \Delta x\]雅可比矩阵建立关节速度与末端速度的线性映射：
\[\begin{bmatrix} v \\ \omega \end{bmatrix} = J(q) \dot{q}\]几何雅可比的计算：
奇异性分析至关重要：
Workspace Boundary
________________________
| |
| •---•---• | <- Singularity (fully extended)
| |
| • |
| /|\ | <- Normal configuration
| • • • |
| |
| • |
| | | <- Singularity (elbow lock)
| •---• |
|________________________|
机械臂动力学方程描述关节力矩与运动的关系。使用拉格朗日方法：
\[\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} = \tau_i\]其中拉格朗日量$L = K - P$，$K$为动能，$P$为势能。
标准形式的动力学方程：
\[M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau\]惯性矩阵的计算：
\[M_{ij} = \sum_{k=\max(i,j)}^n \text{Tr}\left(\frac{\partial T_k}{\partial q_i} I_k \frac{\partial T_k^T}{\partial q_j}\right)\]计算力矩控制通过动力学补偿实现线性化：
\[\tau = M(q)(\ddot{q}_d + K_v e_v + K_p e_p) + C(q,\dot{q})\dot{q} + G(q)\]其中$e_p = q_d - q$，$e_v = \dot{q}_d - \dot{q}$。
闭环动力学变为线性系统：
\[\ddot{e} + K_v \dot{e} + K_p e = 0\]通过选择增益矩阵$K_p$、$K_v$可实现期望的闭环响应。
实际系统存在模型不确定性，需要鲁棒性设计：
自适应控制： \(\tau = \hat{M}(q)u + \hat{C}(q,\dot{q})\dot{q} + \hat{G}(q)\) \(u = \ddot{q}_d + K_v \dot{e} + K_p e\) \(\dot{\hat{\theta}} = \Gamma Y^T(q,\dot{q},u) s\)
其中$s = \dot{e} + \Lambda e$是滑模变量，$Y$是回归矩阵。
滑模控制增强： \(\tau = M(q)(\ddot{q}_d + K_v \dot{e} + K_p e) + C(q,\dot{q})\dot{q} + G(q) + K_s \text{sgn}(s)\)
五次多项式插值：
\[q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5\]边界条件：位置、速度、加速度连续。
梯形速度规划：
velocity
^
| ___________
| / \
| / \
| / \
|/_________________\___> time
accel const decel
计算公式：
笛卡尔直线插值：
\[p(s) = p_0 + s(p_f - p_0), \quad s \in [0,1]\]姿态插值使用四元数SLERP：
\[q(s) = \frac{\sin((1-s)\theta)}{\sin\theta}q_0 + \frac{\sin(s\theta)}{\sin\theta}q_f\]其中$\cos\theta = q_0 \cdot q_f$。
给定路径$s(\tau)$，求解时间最优速度分布：
\[\min T = \int_0^1 \frac{d\tau}{\dot{s}(\tau)}\]约束条件：
| 速度限制：$ | \dot{q}_i | \leq v_{max,i}$ |
| 加速度限制：$ | \ddot{q}_i | \leq a_{max,i}$ |
| 力矩限制：$ | \tau_i | \leq \tau_{max,i}$ |
转化为相平面分析问题，使用动态规划或凸优化求解。
阻抗控制不直接控制力或位置，而是控制两者之间的动态关系。目标阻抗模型：
\[M_d(\ddot{x} - \ddot{x}_d) + D_d(\dot{x} - \dot{x}_d) + K_d(x - x_d) = F_{ext}\]其中：
基于力矩的阻抗控制律：
\(\tau = J^T(F_d + F_{imp}) + g(q)\) \(F_{imp} = \Lambda(q)[\ddot{x}_d - \ddot{x}] + \mu(q,\dot{q}) + p(q) - F_{ext}\)
其中$\Lambda$、$\mu$、$p$分别是笛卡尔空间的惯性、科里奥利和重力项。
导纳控制是阻抗控制的对偶形式，输入力输出位置：
\[M_d \ddot{x}_c + D_d \dot{x}_c + K_d x_c = F_{ext}\]实现框架：
对于约束任务，需要同时控制力和位置。使用选择矩阵$S$分解任务空间：
\[\tau = J^T[S F_f + (I-S) F_p]\]其中：
典型应用场景：
Surface Contact Task
Force Control (z)
↓ F_d
========■======== <- Surface
Robot
← → Position Control (x,y)
根据任务需求在线调整阻抗参数：
基于学习的阻抗调节： \(K_d(t) = K_{d,0} + \Delta K_d(t)\) \(\Delta K_d(t) = \alpha \int_0^t e_F(\tau) e_x^T(\tau) d\tau\)
基于能量的稳定性保证：
定义能量存储函数： \(V = \frac{1}{2}\dot{x}^T M_d \dot{x} + \frac{1}{2}(x-x_d)^T K_d (x-x_d)\)
能量耗散条件： \(\dot{V} = -\dot{x}^T D_d \dot{x} + \dot{x}^T F_{ext} \leq 0\)
对于冗余机械臂（$n > m$，关节数大于任务维度），逆运动学有无穷多解：
\[\dot{q} = J^{+}\dot{x} + (I - J^{+}J)\dot{q}_0\]其中$(I - J^{+}J)$是零空间投影矩阵，$\dot{q}_0$是任意关节速度。
多任务分层控制框架：
第一优先级任务： \(\dot{q}_1 = J_1^{+} \dot{x}_1\)
第二优先级任务（不影响第一任务）： \(\dot{q}_2 = \dot{q}_1 + (J_2 N_1)^{+}(\dot{x}_2 - J_2 \dot{q}_1)\)
其中$N_1 = I - J_1^{+}J_1$是第一任务的零空间投影。
递归形式： \(\dot{q}_k = \dot{q}_{k-1} + (J_k N_{k-1})^{+}(\dot{x}_k - J_k \dot{q}_{k-1})\) \(N_k = N_{k-1} - (J_k N_{k-1})^{+}(J_k N_{k-1})\)
利用冗余度优化次要目标：
可操作度最大化： \(\dot{q}_0 = k_w \nabla_q w(q)\) \(w(q) = \sqrt{\det(J(q)J^T(q))}\)
关节极限回避： \(\dot{q}_0 = -k_l \nabla_q H(q)\) \(H(q) = \sum_{i=1}^n \frac{1}{(q_i - q_{i,max})^2} + \frac{1}{(q_i - q_{i,min})^2}\)
障碍物回避： 使用排斥势场： \(\dot{q}_0 = -k_o \sum_{j} \nabla_q U_{rep}(d_j)\)
其中$d_j$是到第j个障碍物的距离。
为避免零空间运动对任务空间产生力的影响，使用动力学一致的雅可比伪逆：
\[\bar{J}^{+} = M^{-1}J^T(JM^{-1}J^T)^{-1}\]零空间投影： \(\bar{N} = I - \bar{J}^{+}J\)
此投影保证：$J\bar{N} = 0$且$\bar{N}^T J^T = 0$。
双臂系统的增广状态： \(q_{dual} = [q_{left}^T, q_{right}^T]^T\)
协调任务定义：
任务空间映射： \(x_{task} = \begin{bmatrix} x_{abs,L} \\ x_{abs,R} \\ x_{rel} \\ x_{obj} \end{bmatrix}\)
经典主从控制： \(\tau_{slave} = J_s^T K_p (x_{master} - x_{slave}) + J_s^T K_v (\dot{x}_{master} - \dot{x}_{slave})\)
双向力反馈： \(\tau_{master} = -\alpha F_{slave} + \tau_{human}\) \(\tau_{slave} = \beta F_{master} + \tau_{env}\)
稳定性条件（Llewellyn准则）： \(\alpha \beta < 1\)
对于刚性对象的协同搬运：
运动约束： \(\dot{x}_{left} = \dot{x}_{obj} + \omega_{obj} \times r_{left}\) \(\dot{x}_{right} = \dot{x}_{obj} + \omega_{obj} \times r_{right}\)
内力控制： 内力不改变对象运动，但影响抓取稳定性： \(F_{int} = \frac{1}{2}(F_{left} - F_{right})\)
控制律： \(F_{left} = F_{obj}/2 + F_{int,d}\) \(F_{right} = F_{obj}/2 - F_{int,d}\)
定义约束雅可比$J_c$描述运动约束： \(J_c \dot{q} = 0\)
投影到约束允许的运动空间： \(\dot{q} = P_c J^{+} \dot{x}_{task}\)
其中$P_c = I - J_c^{+}J_c$是约束空间投影矩阵。
KUKA iiwa是一款7自由度轻型协作机械臂，专为人机协作设计。其核心特性是关节力矩传感和笛卡尔阻抗控制。
系统架构：
阻抗控制实现：
KUKA iiwa的笛卡尔阻抗控制模式允许设置6个方向的刚度：
刚度范围：
- 平移方向：0.1-5000 N/m
- 旋转方向：0.1-300 Nm/rad
- 阻尼比：0.1-1.0（临界阻尼的比例）
控制律实现： \(\tau_{cmd} = J^T(K_x \Delta x + D_x \Delta \dot{x}) + \tau_{dyn}(q, \dot{q}) + \tau_{null}\)
其中$\tau_{null}$利用冗余度保持关节构型： \(\tau_{null} = (I - J^T J^{+T})(K_{null}(q_{null} - q) - D_{null}\dot{q})\)
安全功能：
应用案例：精密装配
在汽车工业中，iiwa用于精密零件装配：
Franka Panda是研究领域广泛使用的7自由度机械臂，以其开放的控制接口和优秀的力控性能著称。
独特的控制特性：
// Franka力控制示例
std::array<double, 7> tau_d_calculated;
for (size_t i = 0; i < 7; i++) {
tau_d_calculated[i] = coriolis[i] +
tau_J[i] + // 任务空间力
tau_nullspace[i]; // 零空间力矩
}
return tau_d_saturated(tau_d_calculated);
研究应用：接触丰富操作
MIT的研究团队使用Panda进行接触丰富的操作研究：
任务：USB插入
控制策略：
| 插入检测：力突变检测$ | \Delta F_z | > threshold$ |
性能指标：
达芬奇手术系统是医疗机器人的标杆，其精密的主从控制实现了微创手术的突破。
系统组成：
主从映射算法：
其中$\alpha$是缩放因子，$R_{align}$是视角对齐矩阵。
速度映射与颤抖滤波： \(\dot{x}_{slave} = LPF(\alpha \cdot \dot{x}_{master}, f_c = 10Hz)\)
力反馈（仅高端型号）： \(F_{master} = \beta \cdot F_{slave} + F_{gravity\_comp}\)
关键技术实现：
1. 远心点（RCM）约束： 手术器械必须绕切口点旋转，不能产生侧向力：
\[J_{RCM} = \begin{bmatrix} I_{3×3} & -[r_{tool}]_× \\ 0_{3×3} & I_{3×3} \end{bmatrix}\]控制律： \(\tau = J_{RCM}^T F_{task} + (I - J_{RCM}^T J_{RCM}^{+T})\tau_{secondary}\)
2. 工作空间限制： 使用势场法防止碰撞： \(V_{limit} = \begin{cases} \frac{k}{2}(d - d_{safe})^2 & d < d_{safe} \\ 0 & d \geq d_{safe} \end{cases}\)
3. 异常处理：
临床应用数据：
现代汽车装配线正从传统的位置控制向柔性控制转变，以适应人机协作和复杂装配任务。
案例：BMW人机协作装配站
系统配置：
控制策略分层：
力控制层（100Hz）： \(F_{desired} = 15N \pm 2N\) \(\tau = J^T(K_f(F_d - F_{measured}) + F_{feedforward})\)
轨迹适应层（10Hz）： 根据力误差调整轨迹： \(\Delta x_{normal} = \int K_i (F_d - F_{measured}) dt\)
过程监控层（1Hz）： 质量指标：
| 轨迹偏差：$ | x_{actual} - x_{nominal} | < 10mm$ |
实施效果：
本章系统介绍了机械臂控制的核心理论与实践方法：
运动学基础：
动力学与控制：
高级控制策略：
实际系统特点：
习题16.1 考虑一个平面2R机械臂，连杆长度均为$l$，推导其正运动学方程和雅可比矩阵。
提示：使用DH参数法，注意雅可比矩阵的几何意义。
习题16.2 设计一个五次多项式轨迹，使机械臂关节从$q_0 = 0$运动到$q_f = \pi/2$，时间$T = 2$秒，初始和最终速度、加速度均为零。
提示：列出6个边界条件，求解6个多项式系数。
习题16.3 一个7自由度机械臂执行6维笛卡尔任务，如何利用冗余度避免关节极限？写出具体的控制律。
提示：使用零空间投影和梯度投影法。
习题16.4 推导2自由度机械臂的重力补偿项$G(q)$，假设连杆质量为$m_1$、$m_2$，质心在连杆中点。
提示：使用拉格朗日法，先计算势能。
习题16.5 设计一个阻抗控制器，使机械臂末端在接触环境时表现出指定的动态特性。考虑环境刚度$K_e = 1000 N/m$，期望阻抗为$M_d = 1 kg$，$D_d = 20 Ns/m$，$K_d = 100 N/m$。分析闭环系统的稳定性。
提示：建立接触动力学模型，分析特征根。
习题16.6 双臂机器人协同搬运一个刚性箱子，左臂在位置$(0, 0.5, 0.3)$，右臂在$(0, -0.5, 0.3)$。设计控制律使箱子以0.1 m/s的速度沿x轴移动，同时保持10N的内力。
提示：分解为对象运动控制和内力控制。
习题16.7 机械臂末端需要沿着未知曲面滑动并保持恒定接触力。设计一个混合力/位置控制器，并考虑如何在线估计曲面法向量。
提示：使用选择矩阵分离力控和位置控制方向，用力传感器数据估计法向量。
习题16.8 考虑机械臂逆运动学的数值求解在奇异点附近的问题。比较雅可比转置法、阻尼最小二乘法和奇异值分解法的性能，给出选择准则。
提示：分析条件数、收敛速度和精度的权衡。
历史人物：John J. Craig (1986) - 其著作《Introduction to Robotics: Mechanics and Control》成为机器人学教育的标准教材，系统化了机械臂运动学、动力学和控制的教学方法，培养了整整一代机器人工程师。
前沿专题：接触丰富操作与触觉伺服控制
现代机械臂控制正向着更复杂的接触任务发展，包括：
这些前沿方向正推动机械臂从传统的位置控制设备转变为真正的智能操作系统。