【Simulink】使用简化机械臂系统动力学的抓取和放置任务及轨迹调度

abbSavedConfigs.mat 文件中的配置

文件中保存了多个关节角度配置，每个配置对应不同的机器人操作步骤。这些配置通常用于控制机器人在执行任务时的各个关键姿态和动作。

各个配置的功能解释：

configSequence (18x7 double):

• 功能: 包含了机器人执行任务的关节角度序列。这是一个 18 行 7 列的矩阵，每列对应于机器人的一个特定姿态或操作步骤。通常用于定义从开始到结束的完整动作路径，如从初始位置到抓取位置、放置位置等。

1. startingConfig (18x1 double):

   • 功能: 表示机器人的初始关节配置。这个配置是机器人在执行任务前的默认姿态，通常是所有操作的起点。
     

2. graspApproachConfig (18x1 double):

   • 功能: 表示机器人在接近抓取位置时的关节配置。这个配置用于将机器人移动到接近物体的姿态，准备进行抓取操作。

3. graspPoseConfig (18x1 double):

   • 功能: 表示机器人在实际抓取物体时的关节配置。这是一个非常关键的配置，确保机器人准确地抓住目标物体。
     

4. graspDepartConfig (18x1 double):

   • 功能: 表示机器人在抓取物体后离开抓取位置的关节配置。这个配置用于控制机器人抓住物体后从抓取位置移开的动作。

5. placeApproachConfig (18x1 double):

   • 功能: 表示机器人在接近放置位置时的关节配置。这个配置用于将机器人移动到放置物体的姿态，准备放下物体。
     

6. placeConfig (18x1 double):

   • 功能: 表示机器人在实际放置物体时的关节配置。这个配置确保机器人将物体准确地放置在目标位置。
     

7. placeDepartConfig (18x1 double):

   • 功能: 表示机器人在放置物体后离开放置位置的关节配置。这个配置用于控制机器人放下物体后从放置位置移开的动作。
     

commandLogic的功能是控制机器人执行一系列动作，涉及抓取和放置物体的操作。

通过 motionState 控制机器人的动作序列，并在每个状态下输出相应的关节角度和抓取器状态。代码还包括一个检测机器人是否到达目标状态的辅助函数。

function [state, closeGripper, t, wpts] = commandLogic(inputTime, configSequence, robotConfig, gripperStateReached)

% 持久化变量，用于保存运动状态和时间重置值
persistent motionState
persistent resetTimeValue

% 设置变量
posTgtThreshold = 0.015; % 位置目标的阈值，表示误差允许范围

% 初始化持久化变量
if isempty(motionState)
    motionState = 0; % 初始运动状态
end
if isempty(resetTimeValue)
    resetTimeValue = 0; % 初始时间重置值
end

% 输出状态和时间
state = motionState; % 当前运动状态
t = inputTime - resetTimeValue; % 从重置时间开始的相对时间

% 初始化输出到初始状态
wpts = configSequence(:,1:2); % 设置初始的关节角度路径
closeGripper = false; % 初始抓取器状态为打开

% 根据当前运动状态执行不同的动作
switch motionState
    case 1
        % 抓取：从接近位置移动到抓取位置，并打开抓取器
        wpts = configSequence(:,2:3);
        closeGripper = false;
    case 2
        % 关闭抓取器
        wpts = [configSequence(:,3) configSequence(:,3)];
        closeGripper = true;
    case 3
        % 抓取后移开
        wpts = configSequence(:,3:4);
        closeGripper = true;
    case 4
        % 移动到放置前的位置
        wpts = configSequence(:,4:5);
        closeGripper = true;
    case 5
        % 移动到放置位置
        wpts = configSequence(:,5:6);
        closeGripper = true;
    case 6
        % 打开抓取器
        wpts = [configSequence(:,6) configSequence(:,6)];
        closeGripper = false;
    case 7
        % 放置后移开
        wpts = configSequence(:,6:7);
        closeGripper = false;
    case 8
        % 循环回到抓取接近位置
        wpts = configSequence(:,[7 2]);
        closeGripper = false;
end
   
% 获取当前状态
trajEndConfigReached = endStateReached(robotConfig, wpts, posTgtThreshold);

% 根据状态和时间重置情况推进运动状态
if trajEndConfigReached && gripperStateReached
    % 推进状态并重置时间
    motionState = mod(motionState, 8) + 1;
    resetTimeValue = inputTime;
end

end



% 辅助函数：判断机器人左右臂是否到达目标状态
function isReached = endStateReached(config, wpts, tgtThreshold)

    % 初始化输出
    isReached = false;

    leftReached = false;
    rightReached = false;

    % 定义左臂和右臂关节索引
    leftArmIdx = 1:7;  % 左臂关节索引（不包括抓取器）
    rightArmIdx = 10:16; % 右臂关节索引（不包括抓取器）

    % 提取左臂的当前配置和目标配置
    leftArmConfig = config(leftArmIdx,:);
    leftArmTarget = wpts(leftArmIdx, 2);

    % 检查左臂状态是否达到目标路径点
    diffLeftWpts = abs(leftArmConfig - leftArmTarget);
    leftErrorStatus = diffLeftWpts < tgtThreshold; % 判断误差是否在阈值范围内
    if all(leftErrorStatus)
        leftReached = true; % 如果左臂所有关节都在误差范围内，则认为到达目标
    end

    % 提取右臂的当前配置和目标配置
    rightArmConfig = config(rightArmIdx,:);
    rightArmTarget = wpts(rightArmIdx, 2);

    % 检查右臂状态是否达到目标路径点
    diffRightWpts = abs(rightArmConfig - rightArmTarget);
    rightErrorStatus = diffRightWpts < tgtThreshold; % 判断误差是否在阈值范围内
    if all(rightErrorStatus)
        rightReached = true; % 如果右臂所有关节都在误差范围内，则认为到达目标
    end

    % % 根据返回的结果来做进一步的判断
    % % 打开一个文件用于写入调试信息
    % fileID = fopen('debug_log.txt', 'a'); % 'a' 表示追加模式

    % 判断左右臂是否到达目标位置，并将结果写入文件
    if leftReached && rightReached
        % fprintf(fileID, 'Both arms have reached the target positions.\n');
        isReached = true;
    end

    % % 关闭文件
    % fclose(fileID);

end

代码功能解释：

1. commandLogic 函数：

   • 这个函数根据机器人当前的运动状态和时间来确定机器人下一步的动作。动作包括抓取、移动、放置等操作。motionState 保存当前的运动阶段，resetTimeValue 用于跟踪时间的重置。
   • 函数返回当前的运动状态（state）、抓取器的状态（closeGripper）、相对时间（t），以及关节角度路径（wpts）。

2. endStateReached 函数：

   • 这是一个辅助函数，用于检查机器人当前的关节角度配置是否到达了目标状态。通过计算配置和目标之间的差异，并与阈值进行比较，判断是否到达目标。

主要流程：

• 机器人在各个状态间切换，依次执行抓取、移动和放置的动作。
• 在每个状态中，根据 motionState 确定机器人应该执行的具体动作，并生成对应的关节角度路径。
• 使用 endStateReached 函数判断当前动作是否完成，并在完成后推进到下一个状态。

场景描述

机器人的任务是从桌子上抓取一个物体，然后将其移动到另一个位置并放下。

在这个例子中，我们的 configSequence 包含了一系列的关节角度配置，每个配置对应于机器人的一个特定姿态。

代码执行流程

1. 初始状态（motionState = 0）初始位置->抓取前的接近位置：

   • 机器人开始时处于初始状态。代码会输出初始的 wpts 和 closeGripper 状态。
   • 此时，wpts 为 configSequence 的第1列和第2列，对应于初始位置到抓取前的接近位置。

2. 状态1：抓取前的接近位置->抓取位置：

   • 当机器人接近物体时，motionState 被设置为1。
   • 代码中的 wpts 被设置为 configSequence 的第2列和第3列，这表示机器人从接近位置移动到抓取位置。
   • closeGripper = false 表示抓取器保持打开状态。

3. 状态2：闭合夹爪抓取物体：

   • 机器人到达抓取位置后，motionState 被设置为2。
   • 代码中的 wpts 被设置为 configSequence 的第3列（抓取位置），并且 closeGripper = true，表示抓取器闭合抓住物体。

4. 状态3：抬起物体：抓取位置->抓取后的离开位置-：

   • 抓取完成后，motionState 被设置为3。
   • wpts 被设置为 configSequence 的第3列和第4列，这表示机器人从抓取位置抬起物体并移动到中间位置。
   • closeGripper = true 表示抓取器保持闭合状态。

5. 状态4：抓取后的离开位置->放置前的位置：

   • 机器人开始移动物体，motionState 被设置为4。
   • wpts 被设置为 configSequence 的第4列和第5列，表示机器人移动到放置前的位置。
   • closeGripper = true 表示抓取器继续保持闭合状态。

6. 状态5：放置前的位置->放置位置：

   • 机器人移动到最终放置位置，motionState 被设置为5。
   • wpts 被设置为 configSequence 的第5列和第6列，表示机器人移动到放置位置。
   • closeGripper = true 表示抓取器仍然闭合。

7. 状态6：打开夹爪放下物体：

   • 机器人放下物体，motionState 被设置为6。
   • wpts 被设置为 configSequence 的第6列（放置位置），并且 closeGripper = false，表示抓取器打开，物体被放下。

8. 状态7：放置位置->放置后的位置：

   • 放置完成后，motionState 被设置为7。
   • wpts 被设置为 configSequence 的第6列和第7列，表示机器人离开放置位置。
   • closeGripper = false，表示抓取器保持打开状态。

9. 状态8：循环回到抓取前的接近位置：

   • 机器人完成一次循环操作，motionState 被设置为8。
   • wpts 被设置为 configSequence 的第7列和第2列，表示机器人回到抓取前的接近位置，为下一次抓取做准备。
   • closeGripper = false 表示抓取器继续保持打开状态。

总结

这个流程展示了机器人如何通过状态机逐步执行抓取和放置操作。每个状态对应于机器人的一个动作步骤，状态的推进是通过检查机器人是否到达了目标姿态（endStateReached 函数）以及抓取器状态是否达成来实现的。整个过程是自动化的，机器人可以根据预设的 configSequence 依次完成复杂的操作任务。

初始状态 (motionState = 0)

当 motionState 的初始值为 0 时，代码逻辑在 motionState 为 0 时并没有定义任何具体的动作（即 switch 语句中没有对 motionState = 0 的处理）。这意味着，在初始状态 motionState = 0 下，机器人不会执行任何特定的运动。

示例流程更新

假设机器人在 motionState = 0 的初始状态下开始任务，然后任务流程如下：

步骤 0: 初始状态（motionState = 0）

• 机器人状态: 在这个状态下，机器人可能处于待机或初始化状态，等待接收到执行任务的命令。
• 输出:
• wpts = configSequence(:,1:2)，表示机器人在 configSequence 的第1列到第2列的关节角度配置之间进行移动。
• closeGripper = false，表示抓取器保持打开状态。

检查是否到达初始目标

• 逻辑: 函数 endStateReached 检查机器人是否已经到达初始位置。
• 动作: 一旦机器人到达 configSequence 的第2列位置且抓取器状态符合要求 (gripperStateReached = true)，则 motionState 从 0 递增到 1，并且 resetTimeValue 被重置为当前时间。这将启动实际的任务序列。

步骤 1: 接近抓取位置（motionState = 1）

• 动作: 机器人将其关节角度调整到 configSequence 的第2列到第3列之间的位置，开始接近抓取位置。
• 抓取器状态: 抓取器保持打开状态 (closeGripper = false)。

后续任务

• 随着任务的进行，motionState 从 1 到 7 逐步递增，每个状态对应一个任务步骤，如抓取、移动、放置等。
• 在每个状态下，机器人检查是否到达目标位置，并根据需要执行相应的动作。

总结

• motionState = 0: 机器人处于初始化状态，等待进入任务流程。当机器人到达初始位置时，状态会递增到 1，开始任务。
• 这种设计使得机器人在启动时有一个准备阶段，可以用于初始化系统，确保在进入实际任务前一切就绪。

状态0：仿真时间1s

状态0(+状态1)：仿真时间2s

状态0+状态1：仿真时间3s

状态0+状态1：仿真时间3.9s

状态0+状态1+状态2(+状态3)：仿真时间4s

状态0+状态1+状态2+状态3：仿真时间5s

状态0+状态1+状态2+状态3：仿真时间6s//状态0+状态1+状态2+状态3+(状态4)：仿真时间6.1s

状态0+状态1+状态2+状态3+状态4：仿真时间7s

状态0+状态1+状态2+状态3+状态4：仿真时间8s//状态0+状态1+状态2+状态3+状态4(+状态5)：仿真时间8.1s

状态0+状态1+状态2+状态3+状态4+状态5：仿真时间9s

状态0+状态1+状态2+状态3+状态4+状态5：仿真时间10s//状态0+状态1+状态2+状态3+状态4+状态5+（状态6）：仿真时间10.1s

状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7：仿真时间11s

状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7：仿真时间12s//状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7（+状态8）：仿真时间12.2s

状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8：仿真时间13s

状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8：仿真时间14s//状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8+(状态1)：仿真时间14.2s

状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8+状态1：仿真时间15s

状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8+状态1+状态2+状态3：仿真时间16s//状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8+状态1+状态2：仿真时间16.1s//状态0+状态1+状态2+状态3+状态4+状态5+状态6+状态7+状态8+状态1+状态2(+状态3)：仿真时间16.2s

仿真时间运行到20s时正处于状态4（18.2s–20.2s）,目标位置为placeApproachConfig（config5）

motionState = 8 的条件

官方提供的 commandLogic 函数中，motionState 的值通过以下逻辑进行更新：

motionState = mod(motionState, 7) + 1;

在 case 语句中，定义了 case 8 的情况，但根据更新公式 motionState = mod(motionState, 7) + 1;，motionState 只能循环取值为 1, 2, 3, 4, 5, 6, 7，不会等于 8。

所以，实际上，motionState 永远不会等于 8。这意味着代码中的 case 8 语句永远不会被执行到。

修改代码以允许 motionState 等于 8

想让 motionState 循环到 8 然后重新开始，可以这样做：

motionState = mod(motionState, 8) + 1;

这样，motionState 的取值范围就会变成 1 到 8，当 motionState 等于 7 时，下一次会变为 8，然后再循环回 1。此时，case 8 中的代码就会被执行。

Trapezoidal Velocity Profile Trajectory模块

该截图显示的是 Simulink 中 “Trapezoidal Velocity Profile Trajectory” 模块的参数设置窗口。这个模块用于生成通过多个路径点（waypoints）的轨迹，并使用梯形速度曲线来进行运动规划。以下是这个模块的详细功能解释：

模块功能

Trapezoidal Velocity Profile Trajectory 模块生成通过多个路径点的轨迹，使用梯形速度曲线来控制运动。梯形速度曲线具有加速、恒速和减速三个阶段，在这种曲线下，系统能够平稳地从一个路径点过渡到下一个路径点。

参数解释

1. Waypoint source (路径点来源)

   • External (外部)：此选项表示路径点将从模块的输入端口提供，允许用户动态地传递路径点到模块中。
   • 当选择 “External” 时，用户需要在外部为模块提供一个路径点矩阵，路径点的矩阵大小为 N*P，其中N是轴的数量，P 是路径点的数量。

2. Parameters (参数)

   • Number of parameters (参数数量)：这个下拉菜单用于设置梯形速度曲线的参数数量。用户可以选择一到多个参数，这些参数用于控制速度曲线的形状，例如结束时间、最大速度、最大加速度等。
   • Parameter 1: End Time (参数1：结束时间)：此参数指定每段轨迹的结束时间。
     • Parameter source (参数来源)：设置参数值的来源，可以是 Internal（内部），即由用户在模块中手动指定参数，或者是 External（外部），由外部输入端口提供参数值。
     • End time (结束时间)：指定内部结束时间，单位为秒。

3. 仿真方式 (Simulation mode)

   • 解译执行 (Interpreted Execution)：表示模块将在 MATLAB 环境下解释执行，这通常比生成代码执行要快，但在硬件实时执行或代码生成场景下可能需要切换为生成代码的模式。

总结

这个模块通过生成梯形速度曲线来控制运动系统的路径跟踪，确保系统在加速、恒速和减速过程中的速度平滑变化。路径点可以从外部输入提供，而速度曲线的形状可以通过内部或外部参数来控制。