【添加核心机械臂动力学】Model and Control a Manipulator Arm with Robotics and Simscape

机械臂动力学

为了设计控制器，机械臂动力学必须表示给定力矩输入的关节位置。这在机械臂动力学子系统中通过一个前向动力学块实现，该块将关节力矩转换为给定当前状态的关节加速度，然后通过两次积分得到完整的关节配置。积分器初始化为q0和dq0，即初始关节位置和速度。

此外，夹爪控制子系统将关节控制力矩覆盖到夹爪执行器上，应用10牛顿的力来打开或关闭夹爪。
需要注意的是，第二个积分器是饱和的。

虽然设计良好的位置控制器下的机械臂通常不会达到关节极限，但由于夹爪动作的开环力的添加，需要关节极限以确保响应的现实性。对于更精确的模型，可以将关节饱和连接到速度以重置积分，但对于此模型，这一精度级别已足够。

夹爪传感器

该模型还添加了一个更详细的夹爪传感器，以检查夹爪是否确实打开或关闭。夹爪传感器从关节配置的最后两个值中提取数据（这些值对应于夹爪的位置），并在MATLAB函数块Gripper Logic中将其与closeGripper命令给出的预期夹爪位置进行比较。当夹爪关节的位置与closeGripper命令给定的期望状态相匹配时，Gripper Status返回1。当夹爪尚未达到这些状态时，Gripper Status返回0。这与早期简化模型中的夹爪模型行为相匹配。
在这里插入图片描述

用于控制机械臂夹爪（Gripper）的控制模块

该模块的主要功能是通过控制输入信号来修改施加在机械臂关节上的力矩，以实现夹爪的开合操作。
在这里插入图片描述

模块功能说明

Controller Joint Torques and Forces (控制器关节力矩和力)
- 输入端口 1：接收来自控制器的关节力矩和力，这些力矩和力是控制器根据机械臂的当前状态和期望状态计算出来的，主要用于驱动机械臂的每个关节。
- 这些信号在后续的处理过程中可能会被修改，以便实现夹爪的开合操作。
Demux 块 (tau_Demux)
- 功能：Demux（Demultiplexer）块用于将输入的关节力矩信号解复用为多个独立的信号。在这个模型中，它将18个关节的力矩信号分解开，以便对每个关节的力矩单独处理。
- 输出：分解后的信号将被送入各个加法器（Sum 块）进行进一步的处理。
Sum 块 (加法器)
- 功能：加法器用于将输入的关节力矩信号与可能的修正量进行相加或相减。在这个模型中，每个关节的力矩都经过一个加法器，其中部分关节力矩可能会根据夹爪的控制信号进行修改。
- 夹爪控制信号（Gripper Close Command）用于确定是否需要对夹爪对应的关节力矩进行修正。
Gripper Close Command (夹爪关闭命令)
- 输入端口 2：接收一个控制信号，用于指示夹爪的状态。如果信号为1，表示需要关闭夹爪；如果信号为0，表示需要打开夹爪。
- Double 块：将输入的控制信号转换为适当的数据类型，以便后续处理。
- 乘法器 (Gain 块)：将控制信号与固定值（如10）相乘，生成一个修正力矩，用于施加在夹爪的关节上。
- 加法器：将修正后的力矩信号与原始力矩信号相加，以调整夹爪的动作。
Modified Joint Torques and Forces (修改后的关节力矩和力)
- 输出端口：将修改后的力矩信号输出，这些信号会用于实际驱动机械臂的各个关节，包括夹爪的动作。
- 最终的力矩信号包括了控制器计算出的原始力矩，以及根据夹爪状态所做的修正。

总结

这个模块的核心功能是通过接收控制器计算的关节力矩信号，并根据夹爪的控制命令对相关的关节力矩进行调整。具体来说，当夹爪需要关闭时，模块会在夹爪对应的关节力矩上叠加一个修正力矩，从而实现夹爪的动作。调整后的力矩信号最终输出，用于驱动机械臂完成包括夹爪操作在内的所有任务。

该模块名为 Manipulator Forward Dynamics，用于计算机械臂在给定关节力矩和状态下的关节加速度。这个模块在机器人动力学仿真中非常关键，因为它能够准确地模拟机械臂的运动。

Manipulator Forward Dynamics模块基于给定的关节力矩和机械臂的当前状态，计算出各个关节的加速度。

这个过程被称为前向动力学（Forward Dynamics），是机器人动力学的核心计算之一。

输入参数:

Config: 当前的关节位置配置。该输入是一个 Nx1 的向量，N 表示机械臂的非固定关节的数量。它包含了每个关节的当前位置（通常以弧度为单位）。
JointVel: 当前的关节速度。该输入也是一个 Nx1 的向量，表示每个关节的当前速度（通常以弧度/秒为单位）。
JointTorq: 施加在关节上的力矩。这个输入是一个 Nx1 的向量，包含了每个关节的当前力矩或力（根据关节类型不同）。
FExt: 外部作用力矩。这个输入是一个 6xM 的矩阵，M 是机械臂中刚体的数量。该矩阵用于表示外部作用在每个刚体上的力和力矩。

输出参数:

JointAccel: 输出的关节加速度。这是一个 Nx1 的向量，表示每个关节在给定力矩和状态下产生的加速度。

刚体树模型:

Rigid body tree: 该模块依赖于一个刚体树（rigidBodyTree）对象，这个对象描述了机械臂的结构，包括关节的连接方式、各个部件的质量、惯性等物理属性。该对象在 MATLAB 的工作区中定义并传递给模块。

模块在系统中的作用

前向动力学计算:
- 通过前向动力学计算，模块能够基于当前的关节位置和速度以及施加的力矩，求解出每个关节的加速度。这对于精确模拟机械臂在各种力和外力作用下的运动至关重要。
仿真和控制:
- 该模块输出的关节加速度可以用于进一步的仿真，或者用于更新机械臂在下一时刻的关节速度和位置。它是机械臂动态模型的核心部分，确保了仿真系统的精确性和物理一致性。

Gripper Sensor用于检测机械臂夹爪（Gripper）的状态，确定夹爪是否已经达到预期的开合位置。

1. Gripper Sensor 模块

功能概述:
- 该模块的主要功能是通过测量夹爪的实际位置，并将其与预期的目标位置进行比较，从而判断夹爪是否已经到达预期的状态。最终的输出是一个布尔值，表示夹爪是否到达目标位置。
输入端口:
1. U (18): 这个输入代表机械臂的关节位置，包含18个关节的位置信息。通过 Demux 分解后提取出夹爪相关的关节位置信息。
2. 1/z: 表示一个状态保持器，用来存储上一个时间步的夹爪控制信号，通常用于检测状态变化。
输出端口:
- Gripper Status (夹爪状态): 表示夹爪是否已经达到目标位置。如果到达，输出 1，否则输出 0。

2. Gripper Logic 模块

功能概述:
- Gripper Logic 模块接收夹爪的位置和控制信号，判断夹爪是否已到达目标状态（完全打开或关闭）。内部执行的逻辑由一个 MATLAB 函数实现。

3. MATLAB 函数 `isGripperStateReached`

代码解释:

function isReached = isGripperStateReached(gripperPos, closeGripper)

    isReached = false;

    % 定义关键值
    posTgtThreshold = 0.01; % 位置误差容许范围
    openValue = 0.025;      % 夹爪打开时的位置值
    closeValue = 0;         % 夹爪关闭时的位置值
    
    % 根据控制信号确定目标位置
    if closeGripper
        gripperTarget = closeValue;
    else
        gripperTarget = openValue;
    end
    
       % 计算左臂和右臂夹爪的误差
      diffWptsLeft = abs(gripperPos(1:2) - [gripperTarget; gripperTarget]);
      diffWptsRight = abs(gripperPos(3:4) - [gripperTarget; gripperTarget]);
  
      % 判断左臂和右臂夹爪是否都已到达目标位置
      posErrorStatusLeft = all(diffWptsLeft < posTgtThreshold);
      posErrorStatusRight = all(diffWptsRight < posTgtThreshold);
      
      if posErrorStatusLeft && posErrorStatusRight
          isReached = true;
      end
    
end

函数功能:
- isReached: 返回值 isReached 是一个布尔变量，用于指示夹爪是否已经到达目标状态。如果到达，返回 true，否则返回 false。
- posTgtThreshold: 定义了一个误差容忍范围，表示夹爪位置与目标位置之间的允许偏差。在这个范围内，位置被认为是达到了目标位置。
- openValue 和 closeValue: 分别表示夹爪在完全打开和完全关闭时的目标位置值。
- gripperTarget: 根据 closeGripper 的输入值（表示是否需要关闭夹爪），确定当前的目标位置是 openValue 还是 closeValue。
- posErrorStatus: 计算当前夹爪位置与目标位置之间的差异，并判断该差异是否在容忍范围内。如果是，则认为夹爪已经到达目标位置。

Selector模块用于从输入信号向量中选择特定元素或一组元素进行输出。

该模块在 Simulink 模型中非常有用，特别是在处理多维数组或信号向量时，可以提取所需的部分进行后续处理。
在这里插入图片描述

模块的主要功能：

选择输入向量的特定元素：
- Selector 模块允许用户从输入的向量信号中选择特定的元素。你可以指定索引位置来提取单个元素或者一组连续的元素。
输入维度和索引选择：
- 输入维度: 该参数表示输入信号的维度。在此例中，输入端口大小为18，意味着输入信号是一个长度为18的向量。
- 索引模式: “从1开始” 表示索引是基于 MATLAB 语言的索引系统，从1开始计数。
- 索引选择: 通过选择索引，可以指定要提取的元素范围。
输出：
- 模块根据设定的索引选择，输出指定的信号分量。