介绍

在此示例中，您将运行一种模型，该模型实现具有使用操纵器算法块的接合位置和速度反馈的计算扭矩控制器。控制器从模拟机器人接收联合位置和速度信息（使用Simscape Multibody实现），并发送扭矩命令以沿着给定的关节轨迹驱动机器人。平面物体放置在机器人的前面，使得当执行轨迹跟踪控制时，机器人臂的末端执行器将与其碰撞。如果没有任何额外的设置，则由于与物体碰撞而导致的扭矩可能会导致机器人或物体损坏。为了实现安全的轨迹跟踪，构建轨迹缩放块以在将所需的运动分配给控制器时调整时间戳。您可以调整一些参数并在模型运行时与机器人交互，并观察对模拟机器人的影响。

在工作空间中建立机器人模型

本例使用了Rethink Sawyer的模型，这是一个7自由度的机器人操作器。调用importrobot生成一个刚性小组细胞从存储在统一机器人描述格式（URDF）文件中的描述中的模型。设定dataformat.和重力属性与simscape一致。

Sawyer = ImportRobot（“sawyer.urdf”）;sawyer.removeBody (“头”）;Sawyer.DataFormat ='柱子';sawyer.gravity = [0,0，-9.80665];

轨迹生成及相关设置

首先，指定轨迹的开始时间和持续时间。

tstart = 0.5;tduration = 3;

接下来，分配初始配置和目标配置。Q0是初始配置和Q1是目标配置。

Q0 = [0;-1.18;0;2.18;0;-1.0008;3.3161];Q1 =零（7,1）;

下图显示了与平面对象的位置相关的初始配置和目标配置的机器人可视化。放置平面对象，使得机器人在轨迹跟踪期间将碰撞到它。

在Simuli金宝appnk模型中，a多项式轨迹块用于使用第五阶多项式在轨迹中的任何瞬间计算机器人的位置，速度和加速度。

金宝appSimulink模型概述

接下来，打开Simulink模金宝app型。上面生成的变量已经存储在Simulink Model工作空间中：金宝app

open_system ('robotsafetrableatorytracking.slx'）;

这robotsafeTrableatorytracking.模型实现了具有用于安全轨迹跟踪的轨迹缩放的计算扭矩控制器。此模型中有三个主要子系统：

在每个时间步长上，如果轨迹缩放开关打开，则使用修改后的时间戳来评估期望的关节位置、速度和加速度。然后，计算的力矩控制器使用与RigidBodyTree模型跟踪所需的运动。导出的控制输入被馈送到Simscape Multibody中的Sawyer模型（其中包括与机器人交互的平面对象）。

利用机器人操作机块构建计算力矩控制器

对于一个操纵者 $N$非固定关节，系统动力学可表示为:

在哪里 $\mathrm{问：}$那 $\underset{}{\overset{\dot{}}{\mathrm{问：}}}$那 $\underset{}{\overset{¨}{\mathrm{问：}}}$那 $\in {\mathrm{R.}}^N$是广义坐标的位置，速度和加速度， $你\in {\mathrm{R.}}^N$是控制输入（扭矩）， $m（\mathrm{问：}）$是关节空间质量矩阵， $C（\mathrm{问：}那\underset{}{\overset{\dot{}}{\mathrm{问：}}}）\underset{}{\overset{\dot{}}{\mathrm{问：}}}$是速度和扭矩的乘积， $G（\mathrm{问：}）$是重力力矩。以期望的位置沿着期望的关节轨迹跟踪 ${\mathrm{问：}}_{\mathrm{D.}}$， 速度 ${\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}$和加速度 ${\underset{}{\overset{¨}{\mathrm{问：}}}}_{\mathrm{D.}}$，计算的扭矩控制器计算获得给定配置和速度所需的扭矩，提供了机器人动态变量 $m（\mathrm{问：}）$那 $C（\mathrm{问：}那\underset{}{\overset{\dot{}}{\mathrm{问：}}}）\underset{}{\overset{\dot{}}{\mathrm{问：}}}$， 和 $G（\mathrm{问：}）$．在Si金宝appmulink中，可以使用从机器人系统工具箱的机器人机械手块轻松导出这些变量，以设计以下计算扭矩控制器：

在哪里 ${\mathrm{问：}}_{\mathrm{E.}}=\mathrm{问：}-{\mathrm{问：}}_{\mathrm{D.}}$是位置错误和 ${\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{E.}}=\underset{}{\overset{\dot{}}{\mathrm{问：}}}-{\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}$是速度错误。使用此控制器输入，系统动态成为二阶焦点：

通过选择 ${\mathrm{K.}}_{\mathrm{D.}}$和 ${\mathrm{K.}}_{\mathrm{P.}}$正确，跟踪错误 ${\mathrm{问：}}_{\mathrm{E.}}$会在时间趋近于无穷时收敛于零。

这计算转矩控制器子系统使用三个机器人机械手块构建：关节空间质量矩阵那速度产品扭矩， 和重力扭矩．注意，关联的刚性小组细胞模型，索耶，分配在所有这些块中，并且需要将配置和速度作为列向量插入。

open_system ('RobotSafeTrojectoryTracking / Computed Torque Controller'）;

在计算的扭矩控制器内部，有两个可调参数（由彩色块表示）：

一种标准的方法来确定Kp和Kd是计算它们为：

在哪里 ${\mathrm{\omega .}}_N$和 $\xi$为二阶ODE的固有频率和阻尼比。在本例中，默认值为Kp和Kd设固有频率和阻尼比为 ${\mathrm{\omega .}}_N=10.$和 $\xi =1$使二阶ODE成为临界阻尼系统。

轨迹缩放

在这个子系统中有两个主要模块:

轨迹缩放是本例中为安全轨迹跟踪而部署的主要块。在每一个时间步 ${\mathrm{T.}}_{\mathrm{一世}}$，原始时间戳计算为 ${\mathrm{T.}}_{\mathrm{一世}}={\mathrm{T.}}_{\mathrm{一世}-1}+\Delta \mathrm{T.}$．然而，当机器人与意外物体碰撞时，对于机器人和对象来说，从计划轨迹的增加的扭矩和偏差可能是破坏性的。轨迹缩放的主要思想是计算时间戳 ${\mathrm{T.}}_{\mathrm{一世}}={\mathrm{T.}}_{\mathrm{一世}-1}+F_{\mathrm{S.}}（{\mathrm{问：}}_{\mathrm{D.}}那{\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}那{\underset{}{\overset{¨}{\mathrm{问：}}}}_{\mathrm{D.}}那{\mathrm{\tau .}}_{m\mathrm{E.}\mathrm{一种}}）\Delta \mathrm{T.}$通过引入 $F_{\mathrm{S.}}（{\mathrm{问：}}_{\mathrm{D.}}那{\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}那{\underset{}{\overset{¨}{\mathrm{问：}}}}_{\mathrm{D.}}那{\mathrm{\tau .}}_{m\mathrm{E.}\mathrm{一种}}）\in [-1那1]$，它是期望运动和测量扭矩的函数 ${\mathrm{\tau .}}_{m\mathrm{E.}\mathrm{一种}}$．功能 $F_{\mathrm{S.}}（）$控制机器人运动的速度，并根据机器人感觉到的干涉来确定。如果测量的扭矩大于预期， $F_{\mathrm{S.}}（）$减小使机器人减速或甚至向后移动，直到所需的扭矩达到。这些值的 $F_{\mathrm{S.}}（）$对机器人的运动有以下影响：

在里面轨迹缩放块，需要估计外部扭矩 ${\underset{}{\overset{ˆ}{\mathrm{\tau .}}}}_{\mathrm{E.}X\mathrm{T.}}={\mathrm{\tau .}}_{m\mathrm{E.}\mathrm{一种}}-\underset{}{\overset{ˆ}{\mathrm{\tau .}}}$计算 $F_{\mathrm{S.}}（）$,在那里 ${\mathrm{\tau .}}_{m\mathrm{E.}\mathrm{一种}}$是来自Simscape模型的测量扭矩， $\underset{}{\overset{ˆ}{\mathrm{\tau .}}}$是前一次邮票上所需运动的预期扭矩。在模型的外部扭矩观察片段中，逆动力学块计算从测量扭矩中减去的预期扭矩。预期扭矩是： $\underset{}{\overset{ˆ}{\mathrm{\tau .}}}=m（{\mathrm{问：}}_{\mathrm{D.}}）（{\underset{}{\overset{¨}{\mathrm{问：}}}}_{\mathrm{D.}}）+C（{\mathrm{问：}}_{\mathrm{D.}}那{\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}）{\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}+G（{\mathrm{问：}}_{\mathrm{D.}}）$．

在里面想要运动区域，轨迹缩放算法的输出被馈送为输入到一个输入多项式轨迹块。该块计算给定q0和q1的已知值和它们的边界条件——零速度和加速度的五次多项式轨迹。它输出位置、速度和加速度: ${\mathrm{问：}}_{\mathrm{D.}}（\mathrm{T.}）$那 ${\underset{}{\overset{\dot{}}{\mathrm{问：}}}}_{\mathrm{D.}}（\mathrm{T.}）$和 ${\underset{}{\overset{¨}{\mathrm{问：}}}}_{\mathrm{D.}}（\mathrm{T.}）$，它们被提供给计算扭矩控制器子系统。

Simscape Multibody机器人模型和简单的接触力学

Simscape多体机器人模型从同一模型导入.urdf.文件使用小运动，添加了一组扭矩致动器和用于关节扭矩，关节位置和速度的传感器。添加如下所示的接触机构块以模拟末端执行器和障碍物之间的反作用力作为球体和平面，其中使用简单的线性弹簧阻尼模型。

笔记：接触机构只在末端执行器与平面物体之间实现。因此，机器人手臂的其他部分仍可能通过障碍物。

运行模型

在机器人模拟器中运行模型，观察Sawyer的行为并与之交互。

首先，单击SimsceeD模型块左侧下显示的范围图标，打开查看器。它显示包括联合扭矩，末端执行器和平面物体之间的反应接触力的信号，以及计算轨迹跟踪的所需运动的时间戳。

将轨迹缩放开关切换到“关闭”。

使用以下命令或Simulink中的“Run”按钮来启动模拟。金宝app

SIM（'robotsafetrableatorytracking.slx'那'停止'那“5”）;

你应该看到手臂与物体碰撞产生高扭矩输入和大反作用力。注意，在这种情况下使用的是原始的时间戳。之后停止模拟。

现在，将轨迹缩放开关切换到“开”并重新运行模型。

SIM（'robotsafetrableatorytracking.slx'那'停止'那“5”）;

注意在碰撞后计算的扭矩和减小的反作用力的差异。

虽然模拟正在运行，调整滑块以将物体移向或远离机器人。机器人应该在其位置反应，同时仍然试图安全地执行轨迹。

概括

此示例显示了如何在Simulink中使用机器人机械手块来设计计算的扭矩控制器，并将其与Simsceum Cultibody中金宝app的轨迹缩放和动态模拟集成，以实现安全的轨迹跟踪。所得到的扭矩，反作用力和时间戳还证明了用于执行安全轨迹跟踪的轨迹缩放的能力。