使用机器人操作器块执行安全轨迹跟踪控制

此示例使用：

打开生活的脚本

本示例演示如何将Simulink®与操纵器算法块一起使用，以实现模拟金宝app机器人的安全轨迹跟踪控制。

运行此示例需要Robotics System Toolbox™和Simscape Multibody™。

介绍

此示例提供了一个模型，该模型使用操纵器算法块实现了具有关节位置和速度反馈的计算扭矩控制器。该控制器接收来自模拟机器人（使用Simscape Multibody实现）的关节位置和速度信息，并发送扭矩命令以沿给定的关节轨迹驱动机器人。在机器人前面放置一个平面对象，以便在执行轨迹时，机器人手臂的末端执行器将与其碰撞。在没有任何附加设置的情况下，与物体碰撞产生的扭矩可能会对机器人或物体造成损坏。为了实现安全的轨迹跟踪，在将期望运动分配给控制器时，建立了一个轨迹缩放块来调整时间戳。您可以在模型运行时调整一些参数并与机器人交互，并观察对模拟机器人的影响。

建立机器人模型

这个例子使用了Rethink Sawyer的模型，一个七自由度机器人机械手。调用进口机器人产生rigidBodyTree模型来自存储在统一机器人描述格式(URDF)文件中的描述。设置DataFormat和重力属性与Simscape保持一致。Simu金宝applink模型从仿真中的工作空间访问该机器人模型。

索耶= importrobot (<年代p一个n年代tyle="color:#A020F0">“索耶，乌尔德夫”); 索耶·雷莫博迪(<年代p一个n年代tyle="color:#A020F0">“头”）;索耶。DataFormat =<年代p一个n年代tyle="color:#A020F0">“列”；索耶。Gravity = [0, 0, -9.80665];

轨迹生成

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

tStart = 0.5;tDuration = 3;

接下来，分配初始和目标配置。q0处是初始配置和第一季度是目标配置。

q0 = [0;-1.18;0;2.18;0;-1.0008;3.3161);q1 = 0 (7, 1);

下图显示了与平面物体位置相关的初始构型和目标构型的机器人可视化。放置平面物体，使机器人在轨迹跟踪时与之碰撞。

在Simuli金宝appnk模型中<一个href="//www.tatmou.com/au/au/help/robotics/ref/polynomialtrajectory.html" class="a">多项式的轨迹Block使用五阶多项式计算机器人在轨迹中任意时刻的位置、速度和加速度。

金宝app仿真软件模型概述

接下来，打开Simulink模金宝app型。以上生成的变量已经存储在Simulink模型工作区中:金宝app

开放式系统(<年代p一个n年代tyle="color:#A020F0">“robotSafeTrajectoryTracking.slx”）;

的robotSafeTrajectoryTracking该模型实现了基于轨迹缩放的计算转矩控制器，实现了安全的轨迹跟踪。这个模型中有三个主要的子系统:

计算转矩控制器
轨迹缩放和期望运动
具有简单接触力学的Simscape多体模型

在每个时间步，如果轨迹缩放开关打开，则修改后的时间戳用于评估所需的关节位置、速度和加速度。然后，计算出的扭矩控制器使用与r相关联的操纵器块igidBodyTree模型来跟踪期望的运动。导出的控制输入被输入Simscape多体中的Sawyer模型，其中包括用于与机器人交互的平面对象。

建立计算扭矩控制器

对于具有<年代p一个nclass="inlineequation"> $n$ 非固定接头，系统动力学可表示为：

$米（问）问_{}^{¨} + C （问，问_{}^{˙} ）问_{}^{˙} + G （问）＝ u$

在哪里<年代p一个nclass="inlineequation"> $问$ ，<年代p一个nclass="inlineequation"> $问_{}^{˙}$ ，<年代p一个nclass="inlineequation"> $问_{}^{¨}$ ，<年代p一个nclass="inlineequation"> $\in R^{n}$ 为广义坐标的位置、速度和加速度，<年代p一个nclass="inlineequation"> $u \in R^{n}$ 为控制输入(转矩)，<年代p一个nclass="inlineequation"> $米（问）$ 是关节空间质量矩阵，<年代p一个nclass="inlineequation"> $C （问，问_{}^{˙} ）问_{}^{˙}$ 是速度与扭矩的乘积，<年代p一个nclass="inlineequation"> $G （问）$ 为重力力矩。以期望的位置沿期望的关节轨迹进行跟踪<年代p一个nclass="inlineequation"> $问_{d}$ 、速度<年代p一个nclass="inlineequation"> ${问_{}^{˙}}_{d}$ 和加速度<年代p一个nclass="inlineequation"> ${问_{}^{¨}}_{d}$ ，在给定机器人动力学变量的情况下，计算转矩控制器计算获得给定构型和速度所需的转矩<年代p一个nclass="inlineequation"> $米（问）$ ，<年代p一个nclass="inlineequation"> $C （问，问_{}^{˙} ）问_{}^{˙}$ ,<年代p一个nclass="inlineequation"> $G （问）$ ．在Si金宝appmulink中，这些变量可以很容易地使用机器人系统工具箱中的机器人操作器块来设计以下计算扭矩控制器:

$u ＝米（问）（ {问_{}^{¨}}_{d} - K_{d} {问_{}^{˙}}_{e} - K_{p} 问_{e} ） + C （问，问_{}^{˙} ）问_{}^{˙} + G （问）$

在哪里<年代p一个nclass="inlineequation"> $问_{e} ＝问 - 问_{d}$ 是位置误差和<年代p一个nclass="inlineequation"> ${问_{}^{˙}}_{e} ＝问_{}^{˙} - {问_{}^{˙}}_{d}$ 为速度误差。有了这个控制器输入，系统动力学就变成了一个二阶ODE:

${问_{}^{¨}}_{e} + K_{d} {问_{}^{˙}}_{e} + K_{p} 问_{e} ＝ 0$

选择<年代p一个nclass="inlineequation"> $K_{d}$ 和<年代p一个nclass="inlineequation"> $K_{p}$ 正确的，跟踪误差<年代p一个nclass="inlineequation"> $问_{e}$ 当时间接近无穷大时，将收敛到零。

的<年代trong class="emphasis bold">计算转矩控制器子系统是使用三个机器人机械手块构建的:关节空间质量矩阵，速度的产品转矩,重力力矩。请注意，关联的rigidBodyTree模型中,索耶，在所有这些块中指定，并且需要将配置和速度指定为列向量。

开放式系统(<年代p一个n年代tyle="color:#A020F0">“robotSafeTrajectoryTracking /计算力矩控制器”）;

内部<年代trong class="emphasis bold">计算转矩控制器，有两个可调参数(用色块表示):

获得金伯利进程：修正机器人配置时的比例增益
获得杜兰特：修正机器人配置时的衍生增益

一种标准的方法来确定金伯利进程和杜兰特是这样计算的:

$K_{p} ＝ ω_{n}^{2}$

$K_{d} ＝ 2 ω_{n} ξ$

在哪里<年代p一个nclass="inlineequation"> $ω_{n}$ 和<年代p一个nclass="inlineequation"> $ξ$ 是二阶常微分方程的固有频率和阻尼比。在本例中，默认值为金伯利进程和杜兰特通过将固有频率和阻尼比设置为<年代p一个nclass="inlineequation"> $ω_{n} ＝ 10$ 和<年代p一个nclass="inlineequation"> $ξ ＝ 1$ 使二阶常微分方程成为临界阻尼系统。

轨道伸缩

该子系统中有两个主要模块：

轨道伸缩
期望运动

轨道伸缩是本例中用于安全轨迹跟踪的主要块。在每个时间步<年代p一个nclass="inlineequation"> $t_{我}$ ，则计算原始时间戳为<年代p一个nclass="inlineequation"> $t_{我} ＝ t_{我 - 1} + Δ t$ ．然而，当机器人与一个意想不到的物体碰撞时，增加的扭矩和偏离计划轨迹的偏差对机器人和物体都是破坏性的。轨迹缩放的主要思想是计算时间戳为<年代p一个nclass="inlineequation"> $t_{我} ＝ t_{我 - 1} + f_{年代} （问_{d} ， {问_{}^{˙}}_{d} ， {问_{}^{¨}}_{d} ， τ_{米 e 一个} ） Δ t$ 通过介绍<年代p一个nclass="inlineequation"> $f_{年代} （问_{d} ， {问_{}^{˙}}_{d} ， {问_{}^{¨}}_{d} ， τ_{米 e 一个} ） \in ［ - 1 ， 1 ］$ ，这是所需运动和测量扭矩的函数<年代p一个nclass="inlineequation"> $τ_{米 e 一个}$ ．这个函数<年代p一个nclass="inlineequation"> $f_{年代} （）$ 控制机器人的运动速度，并根据机器人感受到的干涉来确定。如果测得的扭矩大于预期，<年代p一个nclass="inlineequation"> $f_{年代} （）$ 减小扭矩，使机器人减速，甚至向后移动，直到达到所需扭矩。这些价值观<年代p一个nclass="inlineequation"> $f_{年代} （）$ 对机器人的运动有以下影响:

$f_{年代} （） > 0$ 时，机器人向前移动(<年代p一个nclass="inlineequation"> $f_{年代} （）＝ 1$ 是正常速度）。
$f_{年代} （）＝ 0$ ，机器人停下来。
$f_{年代} （） < 0$ ，机器人向后移动。

在<年代trong class="emphasis bold">轨道伸缩块，则需要估计外部扭矩<年代p一个nclass="inlineequation"> ${τ_{}^{ˆ}}_{e x t} ＝ τ_{米 e 一个} - τ_{}^{ˆ}$ 计算<年代p一个nclass="inlineequation"> $f_{年代} （）$ 哪里<年代p一个nclass="inlineequation"> $τ_{米 e 一个}$ 是来自Simscape模型的测量扭矩，并且<年代p一个nclass="inlineequation"> $τ_{}^{ˆ}$ 为期望运动在上一个时间戳上的期望转矩。在模型的外部转矩观测器部分逆动力学块计算从测量扭矩减去的期望扭矩。期望转矩:<年代p一个nclass="inlineequation"> $τ_{}^{ˆ} ＝米（问_{d} ）（ {问_{}^{¨}}_{d} ） + C （问_{d} ， {问_{}^{˙}}_{d} ） {问_{}^{˙}}_{d} + G （问_{d} ）$ ．

在<年代trong class="emphasis bold">期望运动面积，轨迹缩放算法的输出作为输入输入到a<年代trong class="emphasis bold">多项式的轨迹块。这个块计算一个已知值的五次多项式轨迹q0处和第一季度以及速度和加速度为零的边界条件。它输出位置、速度和加速度:<年代p一个nclass="inlineequation"> $问_{d} （ t ）$ ，<年代p一个nclass="inlineequation"> ${问_{}^{˙}}_{d} （ t ）$ ,<年代p一个nclass="inlineequation"> ${问_{}^{¨}}_{d} （ t ）$ ，它们被喂给<年代trong class="emphasis bold">计算转矩控制器子系统。

Simscape多体机器人模型与简单接触力学

Simscape多体机器人模型也是从该模型导入的.urdf文件使用smimport，其中增加了一组扭矩执行器和用于关节扭矩、关节位置和速度的传感器。接触机构块以球体和平面的形式模拟末端执行器与障碍物之间的反作用力，其中采用了简单的线性弹簧阻尼模型。

注意:接触机制仅在末端效应器和平面对象之间实现。因此，机械臂的其他部分仍可能通过障碍物。

运行模型

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

首先，单击Simscape模型块左侧的作用域图标，打开查看器。该显示器显示信号，包括关节力矩，末端执行器与平面物体之间的反作用力，以及计算所需运动轨迹跟踪的时间戳。

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

使用以下命令或单击<年代trong class="emphasis bold">运行开始模拟。

sim卡(<年代p一个n年代tyle="color:#A020F0">“robotSafeTrajectoryTracking.slx”，<年代p一个n年代tyle="color:#A020F0">“StopTime”，<年代p一个n年代tyle="color:#A020F0">'5'）;

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

现在，切换轨迹缩放开关到<年代trong class="emphasis bold">在然后重新运行模型。

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

当模拟运行时，调整滑块，使物体朝向或远离机器人。机器人应该对其位置做出反应，同时仍然试图安全地执行轨迹。

总结

该实例展示了如何利用Simulink中的机器人机械臂块设计计算力矩控制器，并将其与Simscape Multibody中的轨金宝app迹缩放和动态仿真相结合，实现安全轨迹跟踪。合力扭矩、反作用力和时间戳证明了轨迹缩放的能力，以执行安全的轨迹跟踪。