第16章：机械臂与操作控制

机械臂控制是现代机器人学的核心内容，涵盖从工业制造到医疗手术、从太空探索到家庭服务的广泛应用。本章将系统介绍机械臂的运动学、动力学建模，深入探讨力矩控制、阻抗控制等先进控制策略，并通过KUKA、Franka、达芬奇等实际系统案例，展示如何设计高性能的机械臂控制系统。我们将特别关注接触丰富任务中的控制挑战，以及如何通过巧妙的控制设计实现精密操作。

16.1 机械臂运动学

16.1.1 正运动学与DH参数

机械臂运动学描述关节变量与末端执行器位姿之间的几何关系。Denavit-Hartenberg（DH）参数提供了一种系统化的坐标系建立方法。

对于n自由度机械臂，第i个连杆的DH参数包括：

$a_i$：连杆长度（沿$x_i$轴从$z_{i-1}$到$z_i$的距离）
$\alpha_i$：连杆扭转（绕$x_i$轴从$z_{i-1}$到$z_i$的角度）
$d_i$：连杆偏移（沿$z_{i-1}$轴从$x_{i-1}$到$x_i$的距离）
$\theta_i$：关节角（绕$z_{i-1}$轴从$x_{i-1}$到$x_i$的角度）

相邻坐标系之间的齐次变换矩阵：

$$^{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$$

16.1.2 逆运动学求解

逆运动学求解给定末端位姿对应的关节角度，是机械臂控制的关键问题。

解析法：对于特殊结构（如球形腕、相交轴等），可得到封闭形式解。以6自由度机械臂为例，常用Pieper方法：

位置解耦：利用腕心点位置求解前3个关节
姿态解耦：根据腕部姿态求解后3个关节

数值法：对于一般结构，使用雅可比迭代法： $$\Delta q = J^{+}(q) \cdot \Delta x$$ 其中$J^{+}$是雅可比矩阵的伪逆。为提高收敛性，常用阻尼最小二乘法： $$\Delta q = J^T(JJ^T + \lambda^2 I)^{-1} \Delta x$$

16.1.3 雅可比矩阵与奇异性分析

雅可比矩阵建立关节速度与末端速度的线性映射： $$\begin{bmatrix} v \\ \omega \end{bmatrix} = J(q) \dot{q}$$ 几何雅可比的计算：

线速度列：$J_{v_i} = z_{i-1} \times (p_n - p_{i-1})$（旋转关节）
角速度列：$J_{\omega_i} = z_{i-1}$（旋转关节）

奇异性分析至关重要：

边界奇异：机械臂完全伸展或收缩
内部奇异：两个或多个关节轴共线
奇异性度量：可操作度$w = \sqrt{\det(JJ^T)}$

        Workspace Boundary
    ________________________
   |                        |
   |    •---•---•          |  <- Singularity (fully extended)

   |    •---•---•          |  <- Singularity (fully extended)
   |                        |
   |         •              |
   |        /|\             |  <- Normal configuration
   |       • • •            |
   |                        |
   |    •                   |
   |    |                   |  <- Singularity (elbow lock)
   |    •---•               |
   |________________________|

16.2 机械臂动力学与力矩控制

16.2.1 拉格朗日动力学建模

机械臂动力学方程描述关节力矩与运动的关系。使用拉格朗日方法： $$\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(q)$：惯性矩阵（对称正定）
$C(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)$$

16.2.2 计算力矩控制

计算力矩控制通过动力学补偿实现线性化： $$\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$可实现期望的闭环响应。

16.2.3 鲁棒性增强

实际系统存在模型不确定性，需要鲁棒性设计：

自适应控制： $$\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)$$

16.3 轨迹规划与路径生成

16.3.1 关节空间轨迹规划

五次多项式插值： $$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

计算公式：

加速时间：$t_a = v_{max}/a_{max}$
匀速时间：$t_c = (q_f - q_0 - v_{max}^2/a_{max})/v_{max}$
总时间：$T = t_a + t_c + t_d$

16.3.2 笛卡尔空间规划

笛卡尔直线插值： $$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$。

16.3.3 时间最优轨迹

给定路径$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}$

转化为相平面分析问题，使用动态规划或凸优化求解。

16.4 阻抗控制与力控制

16.4.1 阻抗控制原理

阻抗控制不直接控制力或位置，而是控制两者之间的动态关系。目标阻抗模型： $$M_d(\ddot{x} - \ddot{x}_d) + D_d(\dot{x} - \dot{x}_d) + K_d(x - x_d) = F_{ext}$$ 其中：

$M_d$：期望惯性矩阵
$D_d$：期望阻尼矩阵
$K_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$分别是笛卡尔空间的惯性、科里奥利和重力项。

16.4.2 导纳控制

导纳控制是阻抗控制的对偶形式，输入力输出位置： $$M_d \ddot{x}_c + D_d \dot{x}_c + K_d x_c = F_{ext}$$ 实现框架：

测量接触力$F_{ext}$
计算期望加速度：$\ddot{x}_c = M_d^{-1}(F_{ext} - D_d \dot{x}_c - K_d x_c)$
积分得到位置指令
位置控制器跟踪$x_c$

16.4.3 混合力/位置控制

对于约束任务，需要同时控制力和位置。使用选择矩阵$S$分解任务空间： $$\tau = J^T[S F_f + (I-S) F_p]$$ 其中：

$F_f = F_d + K_f(F_d - F_{measured})$：力控制
$F_p = K_p(x_d - x) + K_v(\dot{x}_d - \dot{x})$：位置控制

典型应用场景：

   Surface Contact Task

   Force Control (z)
        ↓ F_d
    ========■========  <- Surface
        Robot
    ← → Position Control (x,y)

16.4.4 变阻抗控制

根据任务需求在线调整阻抗参数：

基于学习的阻抗调节： $$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$$

16.5 零空间控制与冗余度解析

16.5.1 冗余机械臂运动学

对于冗余机械臂（$n > m$，关节数大于任务维度），逆运动学有无穷多解： $$\dot{q} = J^{+}\dot{x} + (I - J^{+}J)\dot{q}_0$$ 其中$(I - J^{+}J)$是零空间投影矩阵，$\dot{q}_0$是任意关节速度。

16.5.2 任务优先级控制

多任务分层控制框架：

第一优先级任务： $$\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})$$

16.5.3 零空间优化

利用冗余度优化次要目标：

可操作度最大化： $$\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个障碍物的距离。

16.5.4 动力学一致性零空间

为避免零空间运动对任务空间产生力的影响，使用动力学一致的雅可比伪逆： $$\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$。

16.6 双臂协调控制

16.6.1 双臂系统建模

双臂系统的增广状态： $$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}$$

16.6.2 主从控制模式

经典主从控制： $$\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$$

16.6.3 协同操作控制

对于刚性对象的协同搬运：

运动约束： $$\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}$$

16.6.4 约束空间控制

定义约束雅可比$J_c$描述运动约束： $$J_c \dot{q} = 0$$ 投影到约束允许的运动空间： $$\dot{q} = P_c J^{+} \dot{x}_{task}$$ 其中$P_c = I - J_c^{+}J_c$是约束空间投影矩阵。

16.7 深度案例分析

16.7.1 KUKA iiwa协作机械臂阻抗控制

KUKA iiwa是一款7自由度轻型协作机械臂，专为人机协作设计。其核心特性是关节力矩传感和笛卡尔阻抗控制。

系统架构：

关节力矩传感器：每个关节集成高精度力矩传感器
控制频率：位置控制1kHz，力矩控制3kHz
通信接口：KUKA Sunrise Cabinet，支持FRI（Fast Robot Interface）

阻抗控制实现：

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})$$ 安全功能：

碰撞检测：基于外力矩估计$\tau_{ext} = \tau_{measured} - \tau_{model}$
反应策略：检测到碰撞后切换到零重力模式
虚拟墙：笛卡尔空间约束$F_{wall} = K_{wall} \cdot \text{penetration}$

应用案例：精密装配

在汽车工业中，iiwa用于精密零件装配：

搜索阶段：低刚度（100 N/m），探索插孔位置
对准阶段：选择性刚度，z方向柔顺（50 N/m），xy方向刚硬（2000 N/m）
插入阶段：力控制，保持恒定插入力（30N）

16.7.2 Franka Emika Panda精密操作控制

Franka Panda是研究领域广泛使用的7自由度机械臂，以其开放的控制接口和优秀的力控性能著称。

独特的控制特性：

直接力矩接口： - 实时Linux内核，1kHz控制频率 - libfranka C++接口直接访问 - 延迟小于1ms
力控制实现：

// 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);

笛卡尔运动生成器：内置运动规划，保证加加速度连续： $$j(t) = j_{max} \cdot \text{profile}(t)$$ 研究应用：接触丰富操作

MIT的研究团队使用Panda进行接触丰富的操作研究：

任务：USB插入

挑战：6自由度对准，公差小于0.1mm
方法：混合力/位置控制+螺旋搜索

控制策略：

接近阶段：视觉伺服到预插入位置
接触建立：z方向力控制（2N），xy位置控制
螺旋搜索： $$x(t) = x_0 + r(t)\cos(\omega t)$$ $$y(t) = y_0 + r(t)\sin(\omega t)$$ $$r(t) = r_0 + v_r t$$
插入检测：力突变检测$|\Delta F_z| > threshold$
完成插入：位置控制推进

性能指标：

成功率：95%（100次试验）
平均搜索时间：3.2秒
插入力峰值：<10N

16.7.3 达芬奇手术机器人主从控制

达芬奇手术系统是医疗机器人的标杆，其精密的主从控制实现了微创手术的突破。

系统组成：

主控台：2个7自由度主手
从端：3-4个手术臂（含内窥镜臂）
控制频率：1.5kHz
运动缩放：1:1到10:1可调

主从映射算法：

位置映射： $$x_{slave} = \alpha \cdot R_{align} \cdot x_{master}$$ 其中$\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. 异常处理： - 主从脱离：位置偏差超限时暂停从端 - 紧急停止：检测异常力矩立即锁定 - 故障降级：单臂故障不影响其他臂

临床应用数据：

手术精度：<1mm
手部颤抖消除：>95%
操作时间缩短：30-40%（相比传统腹腔镜）

16.7.4 工业应用：汽车装配线柔性控制

现代汽车装配线正从传统的位置控制向柔性控制转变，以适应人机协作和复杂装配任务。

案例：BMW人机协作装配站

系统配置：

机器人：Universal Robots UR10e
任务：车门密封条安装
挑战：非线性材料特性，需要恒力控制

控制策略分层：

力控制层（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）：质量指标：

力一致性：$\sigma_F < 3N$
轨迹偏差：$|x_{actual} - x_{nominal}| < 10mm$

实施效果：

装配质量提升：缺陷率从2.3%降至0.4%
生产效率：单件时间减少18%
人体工程学改善：工人疲劳度降低40%

本章小结

本章系统介绍了机械臂控制的核心理论与实践方法：

运动学基础：

正运动学通过DH参数系统化建模机械臂几何关系
逆运动学求解采用解析法（特殊结构）或数值迭代法（一般结构）
雅可比矩阵建立关节速度与末端速度的线性映射，奇异性分析至关重要

动力学与控制：

动力学方程：$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau$
计算力矩控制通过动力学补偿实现系统线性化
鲁棒性增强通过自适应控制或滑模控制实现

高级控制策略：

阻抗控制：$M_d(\ddot{x} - \ddot{x}_d) + D_d(\dot{x} - \dot{x}_d) + K_d(x - x_d) = F_{ext}$
零空间控制：$\dot{q} = J^{+}\dot{x} + (I - J^{+}J)\dot{q}_0$
任务优先级控制实现多目标协调

实际系统特点：

KUKA iiwa：关节力矩传感+笛卡尔阻抗控制
Franka Panda：开放的实时控制接口
达芬奇手术机器人：精密主从控制+RCM约束

练习题

基础题

习题16.1 考虑一个平面2R机械臂，连杆长度均为$l$，推导其正运动学方程和雅可比矩阵。

提示：使用DH参数法，注意雅可比矩阵的几何意义。

答案

正运动学： $$x = l\cos(q_1) + l\cos(q_1 + q_2)$$ $$y = l\sin(q_1) + l\sin(q_1 + q_2)$$ 雅可比矩阵： $$J = \begin{bmatrix} -l\sin(q_1) - l\sin(q_1+q_2) & -l\sin(q_1+q_2) \\ l\cos(q_1) + l\cos(q_1+q_2) & l\cos(q_1+q_2) \end{bmatrix}$$ 奇异配置：当$q_2 = 0$或$\pi$时（完全伸展或折叠）。

习题16.2 设计一个五次多项式轨迹，使机械臂关节从$q_0 = 0$运动到$q_f = \pi/2$，时间$T = 2$秒，初始和最终速度、加速度均为零。

提示：列出6个边界条件，求解6个多项式系数。

答案

边界条件：

$q(0) = 0$, $q(T) = \pi/2$
$\dot{q}(0) = 0$, $\dot{q}(T) = 0$
$\ddot{q}(0) = 0$, $\ddot{q}(T) = 0$

解得系数： $$q(t) = \frac{\pi}{2} \cdot \left[10\left(\frac{t}{T}\right)^3 - 15\left(\frac{t}{T}\right)^4 + 6\left(\frac{t}{T}\right)^5\right]$$ 最大速度发生在$t = T/2$：$\dot{q}_{max} = 15\pi/(16T)$

习题16.3 一个7自由度机械臂执行6维笛卡尔任务，如何利用冗余度避免关节极限？写出具体的控制律。

提示：使用零空间投影和梯度投影法。

答案

控制律： $$\dot{q} = J^{+}\dot{x}_d + (I - J^{+}J)\dot{q}_0$$ 其中零空间速度用于关节极限回避： $$\dot{q}_0 = -k \nabla_q H(q)$$ $$H(q) = \sum_{i=1}^7 \left[\frac{1}{(q_i - q_{i,max})^2} + \frac{1}{(q_{i,min} - q_i)^2}\right]$$ 梯度计算： $$\frac{\partial H}{\partial q_i} = \frac{2}{(q_i - q_{i,max})^3} - \frac{2}{(q_{i,min} - q_i)^3}$$

习题16.4 推导2自由度机械臂的重力补偿项$G(q)$，假设连杆质量为$m_1$、$m_2$，质心在连杆中点。

提示：使用拉格朗日法，先计算势能。

答案

势能： $$P = m_1 g \frac{l_1}{2}\sin(q_1) + m_2 g[l_1\sin(q_1) + \frac{l_2}{2}\sin(q_1+q_2)]$$ 重力项： $$G_1 = \frac{\partial P}{\partial q_1} = \left(\frac{m_1}{2} + m_2\right)gl_1\cos(q_1) + \frac{m_2 l_2 g}{2}\cos(q_1+q_2)$$ $$G_2 = \frac{\partial P}{\partial q_2} = \frac{m_2 l_2 g}{2}\cos(q_1+q_2)$$

挑战题

习题16.5 设计一个阻抗控制器，使机械臂末端在接触环境时表现出指定的动态特性。考虑环境刚度$K_e = 1000 N/m$，期望阻抗为$M_d = 1 kg$，$D_d = 20 Ns/m$，$K_d = 100 N/m$。分析闭环系统的稳定性。

提示：建立接触动力学模型，分析特征根。

答案

接触动力学： $$M_d \ddot{x} + D_d \dot{x} + (K_d + K_e)x = F_d$$ 特征方程： $$M_d s^2 + D_d s + (K_d + K_e) = 0$$ $$s^2 + 20s + 1100 = 0$$ 特征根： $$s = -10 \pm j\sqrt{1000} \approx -10 \pm j31.6$$ 系统稳定（实部为负），阻尼比： $$\zeta = \frac{D_d}{2\sqrt{M_d(K_d + K_e)}} = \frac{20}{2\sqrt{1100}} \approx 0.3$$ 系统欠阻尼但稳定，会有振荡收敛。

习题16.6 双臂机器人协同搬运一个刚性箱子，左臂在位置$(0, 0.5, 0.3)$，右臂在$(0, -0.5, 0.3)$。设计控制律使箱子以0.1 m/s的速度沿x轴移动，同时保持10N的内力。

提示：分解为对象运动控制和内力控制。

答案

对象速度： $$v_{obj} = [0.1, 0, 0]^T, \quad \omega_{obj} = [0, 0, 0]^T$$ 两臂末端速度（假设刚性连接）： $$v_{left} = v_{right} = v_{obj} = [0.1, 0, 0]^T$$ 力分配（沿y轴的内力）： $$F_{left} = [F_x/2, 10, F_z/2]^T$$ $$F_{right} = [F_x/2, -10, F_z/2]^T$$ 其中$F_x$、$F_z$由对象动力学决定。

控制律： $$\tau_{left} = J_{left}^T F_{left} + J_{left}^T K_v(v_{obj} - v_{left})$$ $$\tau_{right} = J_{right}^T F_{right} + J_{right}^T K_v(v_{obj} - v_{right})$$

习题16.7 机械臂末端需要沿着未知曲面滑动并保持恒定接触力。设计一个混合力/位置控制器，并考虑如何在线估计曲面法向量。

提示：使用选择矩阵分离力控和位置控制方向，用力传感器数据估计法向量。

答案

法向量估计（基于接触力方向）： $$\hat{n} = \frac{F_{measured}}{||F_{measured}||}$$ 滤波更新（避免噪声）： $$\hat{n}_{filtered} = (1-\alpha)\hat{n}_{prev} + \alpha\hat{n}_{current}$$ 选择矩阵： $$S = \hat{n}\hat{n}^T$$（力控制方向）$$I - S$$（位置控制方向）

控制律： $$F_{cmd} = S(F_d + K_f(F_d - ||F_{measured}||)) + (I-S)K_p(x_d - x)$$ 其中切向运动指令$x_d$在曲面切平面内： $$x_d = x_{current} + (I - \hat{n}\hat{n}^T)v_{tangent}\Delta t$$

习题16.8 考虑机械臂逆运动学的数值求解在奇异点附近的问题。比较雅可比转置法、阻尼最小二乘法和奇异值分解法的性能，给出选择准则。

提示：分析条件数、收敛速度和精度的权衡。

答案

雅可比转置法： $$\Delta q = \alpha J^T \Delta x$$

优点：计算简单，总是有解
缺点：收敛慢，奇异点附近几乎不动

阻尼最小二乘（DLS）： $$\Delta q = J^T(JJ^T + \lambda^2 I)^{-1}\Delta x$$

优点：奇异点附近仍能运动
缺点：引入跟踪误差

奇异值分解（SVD）： $$J = U\Sigma V^T$$ $$\Delta q = V\Sigma^{+}U^T\Delta x$$ 其中$\sigma_i^{+} = \begin{cases} 1/\sigma_i & \sigma_i > \epsilon \\ 0 & \sigma_i \leq \epsilon \end{cases}$

选择准则：

远离奇异点（$\kappa(J) < 100$）：使用伪逆
接近奇异点（$100 < \kappa(J) < 1000$）：使用DLS，$\lambda = 0.01$
奇异点处（$\kappa(J) > 1000$）：使用SVD截断或重新规划路径

自适应阻尼： $$\lambda = \begin{cases} 0 & w > w_{threshold} \\ \lambda_0(1 - w/w_{threshold})^2 & w \leq w_{threshold} \end{cases}$$ 其中$w = \sqrt{\det(JJ^T)}$是可操作度。

常见陷阱与错误

1. 运动学建模错误

DH参数混淆：Modified DH和Standard DH的区别
坐标系定义不一致：确保所有变换在同一参考系下
解决方案：使用仿真软件验证正运动学

2. 奇异性处理不当

问题：在奇异点附近雅可比矩阵病态
症状：关节速度突变、振荡
解决方案：
提前检测奇异性（监控条件数）
使用阻尼最小二乘或任务空间规划避开奇异点

3. 动力学补偿不准确

模型误差：质量、惯量参数不准
计算延迟：动力学计算耗时导致相位滞后
解决方案：
参数辨识实验
使用高效递归算法
自适应控制补偿模型误差

4. 力控制不稳定

硬接触不稳定：环境刚度高时容易振荡
传感器噪声：力传感器噪声放大
解决方案：
降低控制增益
增加阻尼
使用低通滤波器（注意相位延迟）

5. 轨迹规划不连续

加速度突变：引起振动和跟踪误差
路径-速度不协调：转弯处速度过快
解决方案：
使用高阶多项式或样条
考虑动力学约束的时间最优规划

6. 实时性问题

控制周期不稳定：导致性能下降
通信延迟：主从控制的同步问题
解决方案：
使用实时操作系统
优化代码，预计算复杂项
时间戳同步和预测补偿

最佳实践检查清单

系统设计阶段

[ ] 明确任务需求（精度、速度、力要求）
[ ] 选择合适的机械臂配置（自由度、工作空间）
[ ] 确定控制模式（位置/力/阻抗）
[ ] 评估安全需求（协作等级、防护措施）

建模与仿真

[ ] 建立准确的运动学模型
[ ] 验证工作空间和奇异性
[ ] 识别动力学参数
[ ] 仿真验证控制算法

控制器实现

[ ] 选择合适的控制频率（位置1kHz，力矩>3kHz）
[ ] 实现安全限制（关节极限、速度限制、力矩限制）
[ ] 添加异常处理（奇异性、碰撞检测）
[ ] 考虑实时性要求

轨迹规划

[ ] 保证位置、速度、加速度连续性
[ ] 考虑动力学约束
[ ] 添加障碍物检测与回避
[ ] 优化时间或能量

力控制应用

[ ] 正确选择力传感器位置和量程
[ ] 标定传感器偏置和增益
[ ] 设计合适的阻抗参数
[ ] 实现稳定的接触转换

系统集成

[ ] 通信协议可靠性（实时性、错误处理）
[ ] 标定工具和工件坐标系
[ ] 实现人机界面和监控
[ ] 建立维护和诊断机制

性能优化

[ ] 优化控制增益（响应速度vs稳定性）
[ ] 减少稳态误差（积分控制、前馈补偿）
[ ] 提高轨迹跟踪精度（动力学补偿）
[ ] 降低能耗（最优轨迹、重力补偿）

安全验证

[ ] 测试紧急停止功能
[ ] 验证碰撞检测灵敏度
[ ] 检查速度和力矩限制
[ ] 评估故障模式和降级策略

历史人物：John J. Craig (1986) - 其著作《Introduction to Robotics: Mechanics and Control》成为机器人学教育的标准教材，系统化了机械臂运动学、动力学和控制的教学方法，培养了整整一代机器人工程师。

前沿专题：接触丰富操作与触觉伺服控制

现代机械臂控制正向着更复杂的接触任务发展，包括：

学习型阻抗控制：通过强化学习自动调整阻抗参数
多模态感知融合：结合视觉、力觉、触觉的控制
柔性操作：线缆、织物等可变形物体的操作
微操作：亚毫米精度的装配和手术应用