空间机械臂力位协调控制仿真

空间机械臂力与位置协调控制仿真代码

在空间站舱外维修、非合作目标捕获或深空探测器对接等任务中，机械臂不仅要精准地移动到指定位置，还必须“感知”接触过程中的反作用力，避免因刚性碰撞导致航天器姿态失稳甚至结构损伤。这种对 运动精度 和 交互柔顺性 的双重需求，使得传统纯位置控制显得力不从心。

一个典型的场景是：当机械臂试图抓取一颗失控翻滚的卫星时，如果末端以固定轨迹强行逼近，哪怕路径规划再精确，一旦发生接触，不可预测的接触力可能引发剧烈反弹，甚至让整个服务航天器失去平衡。这时候，我们需要的不是“硬碰硬”，而是一种能像人类操作一样“边摸边走”的智能控制策略——这就是 力与位置协调控制 （Hybrid Force/Position Control）的核心思想。

它不像传统方法那样在整个空间统一施加控制模式，而是根据任务需求，在不同自由度上分别执行力控制或位置控制。比如，在接触面法线方向上主动调节施加的压力（力控），而在切向保持轨迹跟踪（位控）。这种“分而治之”的策略，既保证了操作的安全性，又维持了必要的运动精度。

要实现这一目标，仅靠控制器设计远远不够。由于空间机械臂安装在无固定支撑的航天器上，其运动将引起基座漂移，形成强烈的动力学耦合。这意味着关节一动，整个平台都会“晃”。因此，任何有效的控制算法都必须建立在高保真动力学模型之上，并通过闭环仿真验证其鲁棒性。

本文不堆砌公式推导，也不罗列抽象理论，而是聚焦于如何构建一套 可运行、可调试、可扩展 的MATLAB/Simulink仿真框架，帮助开发者真正把力/位协调控制从概念落地为代码。我们将逐步拆解关键模块的技术细节，并提供核心函数实现，力求让读者不仅能理解原理，更能动手复现和改进。

---

空间机械臂的本质是一个 欠驱动、强耦合、非线性 的动力系统。它的基座悬浮在轨道上，没有外部支点，因此机械臂自身的运动必然扰动航天器的姿态与质心位置。这种“自我干扰”特性决定了我们不能像处理地面固定基座机器人那样简化建模。

常用的建模方法基于 空间矢量代数 （Spatial Vector Algebra），利用递归牛顿-欧拉算法高效计算整个系统的动力学响应。最终得到的标准形式如下：

$$
\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}
$$

其中 $\mathbf{q}$ 包含了所有关节变量以及描述基座位姿的虚拟广义坐标。质量矩阵 $\mathbf{H}$ 不仅包含臂体自身惯性，还体现了臂与基座之间的动态耦合；科里奥利项 $\mathbf{C}$ 反映了速度相关的离心力和哥氏力效应；$\mathbf{g}$ 则包含了重力梯度力矩等保守力项。

这个模型的重要性在于：它是设计前馈补偿、自适应控制器乃至阻抗控制的基础。如果没有准确的动力学信息，仅靠PID反馈很难应对大范围运动下的参数时变问题。例如，在伸展动作中，机械臂的有效惯量变化极大，若不进行补偿，会导致控制超调或响应迟滞。

更进一步，实际仿真中还需考虑诸如关节摩擦、传动间隙、柔性关节振动等因素。虽然这些细节会增加模型复杂度，但在高精度操作任务中不可或缺。幸运的是，借助Simscape Multibody这类多体动力学工具，我们可以直接构建三维物理模型，自动求解上述方程，省去手动推导的繁琐过程。

---

雅可比矩阵是连接 关节空间 与 任务空间 的桥梁。对于控制而言，我们通常关心的是末端执行器的位置、速度和受力状态，而不是每个关节的具体角度。因此，必须通过雅可比矩阵 $\mathbf{J}(\mathbf{q})$ 将两者关联起来：

$$
\mathbf{v} = \mathbf{J}(\mathbf{q}) \dot{\mathbf{q}}
$$

其中 $\mathbf{v} \in \mathbb{R}^6$ 是末端的线速度与角速度组合，$\dot{\mathbf{q}}$ 是关节速度向量。在控制律设计中，我们常常需要其伪逆 $\mathbf{J}^\dagger$ 来求解逆运动学：

$$
\dot{\mathbf{q}} = \mathbf{J}^\dagger \mathbf{v} d + [\mathbf{I} - \mathbf{J}^\dagger \mathbf{J}] \mathbf{q} {\text{null}}
$$

第二项用于在冗余自由度中优化次级目标，如避开关节限位、最小化能耗或提升灵巧度。

值得注意的是，雅可比矩阵必须随构型实时更新。一旦机械臂进入奇异位形（如完全伸直或折叠），矩阵秩亏，伪逆结果将剧烈震荡。此时应引入阻尼最小二乘法（DLS）或其他正则化手段稳定求解：

$$
\mathbf{J}^\dagger_\lambda = \mathbf{J}^T (\mathbf{J}\mathbf{J}^T + \lambda^2 \mathbf{I})^{-1}
$$

下面是一段适用于平面两连杆机械臂的解析雅可比计算示例，可用于快速搭建二维仿真原型：

% 计算解析雅可比矩阵（以RR机械臂为例）
function J = analytic_jacobian(q, L1, L2)
    % q: [q1, q2]
    % L1, L2: 连杆长度

    s1 = sin(q(1)); 
    c1 = cos(q(1));
    s12 = sin(q(1)+q(2));
    c12 = cos(q(1)+q(2));

    J = [
        -L1*s1 - L2*s12,   -L2*s12;
         L1*c1 + L2*c12,    L2*c12;
         0,                 0;
         0,                 0;
         0,                 0;
         1,                 1
    ];
end

尽管这只是简化模型，但它清晰展示了雅可比的构成逻辑：前三行对应平移自由度，后三行对应旋转。在三维空间机械臂中，可通过DH参数逐连杆累加旋量来构建完整雅可比。

---

真正的挑战出现在机械臂与环境发生接触的那一刻。此时，外部接触力 $\mathbf{F}_{\text{ext}}$ 开始作用于末端，传统的轨迹跟踪控制可能会产生巨大误差甚至激发振荡。这时就需要启用 力与位置协调控制 。

其核心思想非常直观：将任务空间分解为两个正交子空间——一部分用于控制位置，另一部分用于控制力。例如，在抓取操作中，我们希望在接触面法线方向上施加稳定的预紧力（力控），同时在其余方向上继续跟踪预定轨迹（位控）。

为此，引入一个对角选择矩阵 $\mathbf{S} \in \mathbb{R}^{6\times6}$，其对角元素为0或1，表示该自由度是否采用力控制。控制目标可表述为：

$$
\begin{cases}
\mathbf{S} (\mathbf{F}_{\text{ext}} - \mathbf{F}_d) = 0 \
(\mathbf{I}-\mathbf{S})(\mathbf{x} - \mathbf{x}_d) = 0
\end{cases}
$$

对应的控制律通常设计为：

$$
\boldsymbol{\tau} = \mathbf{J}^T \left[ \mathbf{S} \mathbf{K} f (\mathbf{F}_d - \mathbf{F} {\text{ext}}) + (\mathbf{I}-\mathbf{S}) \mathbf{K}_p (\mathbf{x}_d - \mathbf{x}) - (\mathbf{I}-\mathbf{S}) \mathbf{K}_d \dot{\mathbf{x}} \right] + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g}
$$

这里加入了速度反馈项以增强阻尼，同时补充了重力与离心力补偿项，提升动态性能。

以下是该控制律的MATLAB实现：

% Hybrid Force/Position Control Law
function tau = hybrid_control(q, dq, x, dx, F_ext, xd, Fd, J, H, C, g, Kp, Kd, Kf, S)
    % 输入参数：
    % q, dq: 关节角度与速度
    % x, dx: 末端位姿与速度（6维）
    % F_ext: 外部接触力（6维）
    % xd, Fd: 期望位姿与期望力
    % J: 任务雅可比矩阵
    % Kp, Kd, Kf: 增益矩阵
    % S: 选择矩阵 (6x6), 对角线上1表示力控方向，0表示位置控方向

    err_x = xd - x;           % 位置误差
    err_F = Fd - F_ext;       % 力误差

    % 计算任务空间控制指令
    F_task = (eye(6)-S) * Kp * err_x ...     % 位置控制部分
           - (eye(6)-S) * Kd * dx ...        % 阻尼项
           + S * Kf * err_F;                 % 力控制部分

    % 映射到关节空间
    tau = J' * F_task;

    % 加入动力学补偿
    tau = tau + C*dq + g;
end

这段代码的关键在于 S 矩阵的设计灵活性。例如，在垂直壁面贴附任务中，可设置 $S = \text{diag}([0,0,1,0,0,0])$，即仅在z方向（法向）启动力控，其余五个自由度仍保持位置跟踪。这种配置允许机械臂在不破坏表面的前提下平稳压紧。

但要注意，错误的选择矩阵可能导致“力-位置冲突”。比如在同一方向同时要求跟踪特定轨迹并施加恒定力，这在刚性接触下是不可能实现的。因此，$\mathbf{S}$ 的设定必须与实际接触约束一致，理想情况下应由接触几何自动识别生成。

---

在真实系统中，力传感器信号往往伴随噪声和延迟，直接使用容易引发高频抖动。建议在输入端加入一阶低通滤波器或卡尔曼滤波进行预处理：

% 简单低通滤波示例
alpha = 0.8;  % 滤波系数
F_ext_filtered = alpha * F_ext_prev + (1-alpha) * F_ext_raw;

此外，接触初始阶段常出现冲击峰值，可设计渐进式力加载策略：先以位控方式接近目标，检测到接触后缓慢 ramp up 期望力值，避免突加负载。

另一个常见问题是基座扰动。由于缺乏外部锚点，机械臂施加的力矩会反作用于航天器本体，造成姿态漂移。一种解决方案是在控制律中加入基座姿态补偿，例如通过飞轮或推力器抵消反作用力矩。在仿真中，可通过扩展状态向量将基座动力学显式建模，评估不同控制策略对整体稳定性的影响。

对于冗余机械臂，还可以利用多余自由度主动抑制基座扰动。例如定义一个“扰动最小化”目标函数，在逆运动学求解时优先选择对基座影响小的构型。这类高级策略虽增加了计算负担，但在长期在轨操作中极具价值。

---

完整的仿真系统应包含以下闭环模块：

[轨迹规划] --> [逆运动学求解] --> [控制器] <-- [动力学模型]
                    ↑                   ↓
              [雅可比计算]      [关节驱动器]
                    ↓                   ↑
             [状态反馈模块] <--- [传感器数据]
                     |
               [力传感器 | IMU | 关节编码器]

典型工作流程如下：

1. 初始化机械臂DH参数、质量属性及初始位姿；
2. 实时读取当前关节角度，计算末端位姿与雅可比；
3. 根据力传感器判断是否进入接触阶段；
4. 若未接触，设 $\mathbf{S}=0$，全自由度位控；
5. 若已接触，根据法向估计设置 $\mathbf{S}$，切换至混合控制模式；
6. 调用 hybrid_control 函数生成关节力矩；
7. 将力矩输入动力学模型，推进仿真时间步；
8. 循环执行直至任务完成。

为了提升仿真真实性，可逐步添加以下因素：
- 关节摩擦模型（库仑+粘滞）
- 传动间隙非线性
- 通信延迟（模拟星地链路）
- 外部扰动（如太阳光压、微流星撞击）

这些非理想特性不仅能检验控制算法的鲁棒性，也为后续硬件在环测试打下基础。

---

这套仿真框架的价值不仅限于学术研究。中国的“天和”核心舱机械臂、加拿大Space Station Remote Manipulator System（SSRMS）等实际在轨系统，均依赖类似的控制逻辑完成精密操作。更重要的是，该方法论同样适用于地面场景：核电站内部检修机器人需轻触管道而不划伤表面；手术机械臂要在组织上施加可控压力；工业装配线上的柔性插接任务也都离不开力/位协调机制。

未来的发展方向正朝着 智能化 和 自主化 演进。例如结合机器学习在线识别接触刚度，自适应调整力控增益；融合视觉伺服实时估计接触面法向，动态重构选择矩阵；甚至发展多臂协同控制架构，实现复杂空间作业任务。

当前提供的代码只是一个起点。你可以在此基础上集成阻抗控制、自适应律或强化学习策略，探索更高层次的自主操作能力。毕竟，真正的太空机器人不应只是“按图索骥”的执行者，而应是懂得“察言观色”、灵活应变的智能伙伴。