MATLAB - 机器人机械臂设计轨迹规划器

系列文章目录

前言

本示例介绍了一种设计抓取和轨迹规划器的方法，该规划器可用于垃圾箱拣选系统。

在机器人技术中，垃圾箱拣选包括使用机械手从垃圾箱中取出物品。智能垃圾箱拣选是这一过程的高级版本，具有更强的自主性。使用摄像系统感知部件，规划器生成与场景相适应的无碰撞轨迹。

本示例展示了如何创建一个轨迹规划器组件，该组件将与料箱拣选系统配合使用。该组件作为独立模型存储，可作为线束中的引用模型调用。该示例还使用了一个简单的测试线束来验证规划器的行为。

一、查看模型

打开包含规划器模型的测试

open_system('TrajectoryPlanning_Harness')
open_system('TrajectoryPlanning_Harness/Planner Subsystem')

线束，并导航到规划器子系统以检查其内容。

规划器模块由几个部分组成：

计算逆运动学
规划无碰撞轨迹并优化运动学给定约束条件
缩放生成的轨迹，使其在速度和加速度约束条件下达到时间最优
在线性间隔内插值结果

本节将详细介绍这些步骤：

1.1 计算逆运动学

为了计算无碰撞轨迹，规划器必须知道四件事：起始和目标关节构型、机器人模型和环境模型。虽然起始构型是已知的（即机器人的当前构型），但目标构型往往是未知的。相反，运动规划器指令可能只传递一个目标姿势：末端执行器的笛卡尔空间目标。在这种情况下，规划器首先需要将姿态转换为达到该姿态的关节构型，这需要使用逆运动学块来完成。

1.2 规划无碰撞轨迹并优化给定的运动学约束条件

轨迹规划的大部分工作都在该区域的两个区块中进行。这两个块都是系统对象，是一种类似于 MATLAB 功能块的文件类型，可用于在 Simulink 中运行 MATLAB 代码。但与 MATLAB 功能块不同的是，系统对象可以在代码生成和解释模式下执行，执行起来更加灵活。

该例程使用两个系统对象：

MotionPlannerCHOMP - 这是操纵器CHOMP 对象的封装程序。系统对象接受起点和目标构型以及障碍物的详细信息。它还接受机器人和静态环境细节作为参数。在内部，大部分计算由辅助文件 exampleHelperCHOMPMotionPlanner 处理，该文件创建并执行规划器。为提高性能，该代码经过了混编，最终在系统对象中调用混编后的代码来代替原始的 MATLAB 代码。您可以使用以下命令查看系统对象或运动规划器代码：

edit('MotionPlannerCHOMP.m'); % System object
edit('exampleHelperCHOMPMotionPlanner.m'); % Planner creation & execution within the system object

Contopptraj - 这是 TOPP-RA 求解器 contopptraj 函数的系统对象包装器。该函数采用已知轨迹，并对其重新参数化，以便在遵守所提供的速度和加速度限制的前提下尽可能地穿越该轨迹。结果就是时间最优轨迹。您可以使用 this 命令检查这段代码。

edit('Contopptraj.m')

这两个模块会依次返回一个采样的无碰撞轨迹，在运动学约束条件下，该轨迹是时间最优的。不过，虽然 contopptraj 返回的是完整轨迹，但并不是均匀采样的。

如前所述，规划器代码的性能依赖于 mexed 版本，该版本附后。您可以使用以下命令自行生成该代码：

generateMEXforPlanner

1.3 使用均匀步长重新采样轨迹

一个包含插值器的系统对象用于获取非均匀采样的轨迹，并使用细粒度的均匀采样对其进行重新采样。然后，这个完整的轨迹就可以输出给目标控制器执行。

最后，将输出重新组合为输出总线格式。下面的接口部分将对此进行详细介绍。

二、总线概述

该模型接受机器人指令总线和运动规划响应总线，前者表明轨迹规划任务的性质，后者包含生成的轨迹和一些相关的解决方案数据。总线结构如下。

你可以选择建立一个完全不同的界面，但必须使用相同的总线进行通信。请注意，总线有许多字段，但只有少数字段是绝对必要的。

2.1 机器人指令总线

输入类型是机器人命令总线。通过初始化结构可以看到其结构：

busCmdToTest = motionPlannerCmdInitValue;

有几个关键参数：

请求 ID 是发送任务时的正数。在本模块中，只要计划器执行，请求 ID 就会一直传递。在父任务调度器中，请求 ID 与命令任务的 ID 进行比较，以验证任务是否已完成。
任务数组是详细说明每个具体任务的结构。规划指令的主要内容就在这里。两个补充属性描述了这些任务： MaxNumTasks 表示每个命令的任务数上限，NumTasks 表示该命令提供的任务数。不过，在本模块中，这些属性未被使用，只查询第一个任务。下面将进一步详细介绍该数组。
ObstacleObjects 属性允许用户输入静态环境中的障碍物。所提供的障碍物数量由 NumObstacleObjects 给出，最大值由 MaxNumObstacleObjects 指定。该参数为可选参数，当障碍物对机器人的工作空间无明显阻碍时，该参数通常不会被使用。
InitialPositions 属性表示给定轨迹计划的起始构型。
MaxIterations 属性定义规划器的最大迭代次数。
SampleTime 属性表示规划轨迹的采样时间。

2.2 可变大小命令

请注意，“任务 ”和 “障碍物对象 ”被初始化为 20 乘 1 的结构，但任务和所提供障碍物的数量可以变化。为适应部署要求，这些数组的大小是固定的，与设定的最大数量相对应。对于每个数组，相关参数 NumTasks 或 NumObstacleObjects 定义了规划器实际查询的项目数。

2.3 任务数组

每个任务都存储在一个任务数组中，格式如下：

busCmdToTest.Tasks(1)

ans = struct with fields:
                    Name: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x256 uint8)
                    Type: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x256 uint8)
               FrameName: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x256 uint8)
                       p: [3x1 double]
                  eulZYX: [3x1 double]
                       q: [6x1 double]
               MaxLinVel: 0
               MaxAngVel: 0
                  MaxVel: 0
                  Weight: 0
             MaxLinError: 0
             MaxAngError: 0
           MaxJointError: 0
    ConvergenceSpeedGain: 0
            SecurityDist: 0
           InfluenceDist: 0

该数组有几个重要字段：

名称“、”类型 “和 ”帧名 "属性用于跟踪任务细节。它们存储为固定长度的 uint8，但可以使用 char 函数转换为字符。例如，默认情况下，名称字段未指定，返回的都是空格：

char(busCmdToTest.Tasks(1).Name)

p 和 eulZYX 属性分别指定末端执行器的目标位置和方向（以欧拉 ZYX 序列表示）。这两个属性共同定义了末端执行器在给定计划中的目标姿态。提供这些值后，规划器随后将依靠逆运动学将末端执行器的姿态转换为目标构型。
q 属性可用于指定目标构型。不过，当前规划器未使用该属性。

其余参数为可选参数，可用于为规划器提供约束条件。

2.4 障碍物对象数组

障碍物对象数组为用户提供了另一种提供碰撞盒类型原始障碍物的方法。这个数组在本测试程序中未使用。要更深入地了解其用法，请打开以下辅助程序，该程序会根据结构化输入构建环境：

edit('exampleHelperGenerateCollisionEnvironmentInPlanner.m')

2.5 输入总线中未使用的属性

作为输入提供的总线设计用于多个不同的子系统。这意味着许多属性可能未被使用。例如，本计划模块做了几处简化：

只查询第一个任务。其他 19 项任务将被忽略。
规划器只查看目标姿态。传入的任何目标构型都将被忽略。
许多规划器参数（如最大线速度）与本规划器无关。

其他规划器模块可以使用这些输入。使用该线束，您可以类似地验证

2.6 运动规划响应总线

轨迹规划完成后，必须进行输出。首先要将轨迹组合成一个结构，提供完整的采样轨迹：

motionPlannerResponseInitValue.JointTrajectory

ans = struct with fields:
       NumPoints: 0
    MaxNumPoints: 30000
            Time: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ... ] (1x30000 double)
        JointPos: [6x30000 double]
        JointVel: [6x30000 double]
        JointAcc: [6x30000 double]
        JointTau: [6x30000 double]

这里的 MaxNumPoints 再次表示轨迹大小的上限。所有属性都很重要：

NumPoints 属性设置了轨迹实际使用的点数。例如，对于以 1000 Hz 频率采样的 5 秒轨迹，只会使用前 5000 个索引。
时间属性表示轨迹的采样时间。该属性初始化为一个 1-by-MaxNumPoints 的零矩阵，计划时间占第一个 NumPoints 元素、
JointPos、JointVel、JointAcc 和 JointTau 分别包含关节构型、速度、加速度和挺举。它们被初始化为 N-by-MaxNumPoints 矩阵的零，但计划轨迹占据了前 N-by-NumPoints 索引。这里的 N 是指机器人的非移动关节数。对于 UR5e cobot，N 等于 6。

最后，关节轨迹总线被插入运动规划器响应总线。该总线主要是一组标志：

motionPlannerResponseInitValue

motionPlannerResponseInitValue = struct with fields:
          RequestID: -1
             Status: -1
    JointTrajectory: [1x1 struct]
         IsPosValid: -1
         IsVelValid: -1
         IsAccValid: -1
         IsTauValid: -1

总线具有以下属性：

RequestID 是任务的请求 ID。通常情况下，调用调度程序用它来验证所请求的任务是否就是被执行的任务。
Status（状态）为 1，表示计划已完成并提供了插值轨迹。
JointTrajectory 属性包含先前指定的计划轨迹。

最后四个属性设置为 1 分别表示计划的关节位置、速度、加速度和挺举有效

三、用简单的规划任务验证控制器

我们可以使用简单的测试线束来验证运动规划模块。该工具包旨在测试该模块必须实现的两个主要功能：

给定当前关节构型和可到达的目标姿势，计算两个姿势之间的无碰撞轨迹

给定当前关节构型和指定姿态下的物体，计算一条无碰撞轨迹，将机器人定位在合适的抓取姿态上

3.1 初始化测试线束的输入

所提供的线束通过交替执行的两个独立子系统来实现这一功能。首先加载系统。

open_system('TrajectoryPlanning_Harness');

接下来，初始化参数并为输入总线分配有意义的值：

busCmdToTest = motionPlannerCmdInitValue;
busCmdToTest.RequestID = int32(1);
busCmdToTest.NumTasks = uint32(4);
busCmdToTest.InitialPositions = homePosition(:);
busCmdToTest.MaxIterations = uint32(300);
busCmdToTest.SampleTime = 0.2;
busCmdToTest.NumObstacleObjects = uint32(2); % This value should be nonzero to ensure the planner runs

具体功能在任务结构中定义。创建了两个任务。首先，定义了提取任务：

pickTask = busCmdToTest.Tasks(1);
pickTask.Name(1:length('TooltipTaskSE3')) = uint8('TooltipTaskSE3');
pickTask.Type(1:length('Pick')) = uint8('Pick');
pickTask.FrameName(1:length('Bellow')) = uint8('Bellow');
pickTask.p = [0.485; -0.0032; -0.0368];
pickTask.eulZYX = [1.2041; pi; 0];
pickTask.MaxLinVel = 0.1;
pickTask.MaxAngVel = 0.4;
pickTask.Weight = 1;
pickTask.MaxLinError = 0.01;
pickTask.MaxAngError = 0.0349;

接下来，通过复制采摘任务并对目标构型进行一些小的编辑，创建一个放置任务：

placeTask = pickTask;
placeTask.Type(1:length('Place')) = uint8('Place');
placeTask.p = [0.1 0.6 0.07]';
placeTask.eulZYX = [1.2041; pi; 0];

3.2 使用测试线束为每个测试任务规划路径

为每个任务运行测试线束，存储输出结果：

busCmdToTest.Tasks(1) = pickTask;
pickout = sim('TrajectoryPlanning_Harness.slx');

busCmdToTest.Tasks(1) = placeTask;
busCmdToTest.InitialPositions = [-0.2850 -1.5035 2.0352 -2.1025 -1.5708 0.0817]';
placeout = sim('TrajectoryPlanning_Harness.slx');

3.3 分析和验证结果

为了验证模拟结果，我们将在 MATLAB 中将结果可视化。这需要使用本脚本底部定义的两个辅助函数。这两个辅助函数只是将存储的模型输出转换为可视化的有意义数据。

% Create the environment in which the robot is to be visualized
task_obst = exampleHelperGenerateCollisionEnvironmentInPlanner(uint8(targetEnv),binLength, binWidth, binHeight, binCenterPosition, binRotation, ...
    1, zeros(1,4));

3.4 可视化采摘任务结果

[pick_status, pick_task_traj, picktargetRegion] = helperReadModelData(pickout);

figure;
title('Animate the Picking Task');
show(ur5e, pick_task_traj(:,1)');
hold on
show(picktargetRegion)
showCollisionArray(task_obst);
axis equal

完整轨迹动画

rc = rateControl(25);
for i = 1:size(pick_task_traj,2)
    drawnow;
    show(ur5e, pick_task_traj(:,i)', FastUpdate=true, PreservePlot=false);
    waitfor(rc);
end

3.5 可视化放置任务成果

[task_status, place_task_traj, placetargetRegion] = helperReadModelData(placeout);

figure;
title('Animate the Placing Task');
show(ur5e, place_task_traj(:,1)',Collisions="on");
hold on
show(placetargetRegion)
showCollisionArray(task_obst);
axis equal
rc = rateControl(25);
for i = 1:size(place_task_traj,2)
    drawnow;
    show(ur5e, place_task_traj(:,i)', FastUpdate=true, PreservePlot=false,Collisions="on");
    waitfor(rc);
end

3.6 辅助功能

function [task_status, task_traj, targetRegion] = helperReadModelData(out)

% Read input data
tasks = out.yout{4}.Values.Tasks(1);
tgtPoses = [tasks.p.Data; tasks.eulZYX.Data];

% Read output data
reqs = out.yout{1}.Values.Data;
status = out.yout{2}.Values.Data;
trajs = out.yout{3}.Values;

% Determine the intervals where the request changes and sort into indices
reqchanges = find(diff(reqs)~=0); %Better than unique because duplicates are not filtered out
reqidx = [[1; reqchanges+1] [reqchanges; numel(reqs)]];

% Get the data corresponding to the first req
taskIDX = 1;
taskind = reqidx(taskIDX+1,:);

% Get the task status and trajectory corresponding to the task
task_status = status(taskind(1):taskind(2));
task_trajsize = trajs.NumPoints.Data(taskind(1));
task_traj = trajs.JointPos.Data(:,1:task_trajsize,taskind(1));

% Create a target region to indicate where the robot trajectory is supposed
% to end. Since this planner treats the target orientation as Z only, the
% valid region is set as a rotation about Z.
targetPoseVec = tgtPoses(:,:,taskind(1));
targetPoseTF = [axang2rotm([0 0 1 targetPoseVec(6)]) targetPoseVec(1:3); 0 0 0 1];
targetRegion = workspaceGoalRegion('tcp',ReferencePose=targetPoseTF);
targetRegion.Bounds(4,:) = [-pi pi]; % Not strictly required but shows that with suction, this is really a region and not a specific target

end