使用机器人机械手块进行安全轨迹跟踪控制

这个示例使用:

打开生活的脚本

这个例子向您展示了如何使用Simulink®与机械手算法块来实现对模万博1manbetx拟机器人的安全轨迹跟踪控制。

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

介绍

该实例提供了一个利用机械臂算法块实现具有关节位置和速度反馈的计算力矩控制器的模型。该控制器从模拟机器人(使用Simscape Multibody实现)接收关节位置和速度信息，并发送扭矩命令驱动机器人沿给定的关节轨迹运行。在机器人的前方放置一个平面物体，这样机器人手臂的末端执行器在执行轨迹时就会与机器人手臂发生碰撞。如果没有任何额外的设置，与物体碰撞产生的扭矩会对机器人或物体造成损害。为了实现安全的轨迹跟踪，在将目标运动分配给控制器时，通过建立轨迹缩放块来调整时间戳。当模型运行时，可以调整一些参数并与机器人进行交互，观察对模拟机器人的影响。

建立机器人模型

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

索耶= importrobot (<年代p一个n年代tyle="color:#A020F0">“sawyer.urdf”）;sawyer.removeBody (<年代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万博1manbetxnk模型中<一个href="//www.tianjin-qmedu.com/help/robotics/ref/polynomialtrajectory.html" class="a">多项式的轨迹Block使用五阶多项式计算机器人在轨迹中任意时刻的位置、速度和加速度。

万博1manbetx仿真软件模型概述

接下来，打开Simulink模万博1manbetx型。以上生成的变量已经存储在Simulink模型工作区中:万博1manbetx

open_system (<年代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万博1manbetxmulink中，这些变量可以很容易地使用机器人系统工具箱中的机器人操作器块来设计以下计算扭矩控制器:

$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模型中,索耶，在所有这些块中分配，并且配置和速度需要指定为列向量。

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

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

获得Kp:修正机器人配置时的比例增益
获得Kd:修正机器人构型时的导数增益

确定。的标准方法Kp和Kd是这样计算的:

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

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

在哪里<年代p一个nclass="inlineequation"> $ω_{n}$ 和<年代p一个nclass="inlineequation"> $ξ$ 为二阶ODE的固有频率和阻尼比。在本例中，默认值为Kp和Kd是通过设置固有频率和阻尼比为<年代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中的轨万博1manbetx迹缩放和动态仿真相结合，实现安全轨迹跟踪。合力扭矩、反作用力和时间戳证明了轨迹缩放的能力，以执行安全的轨迹跟踪。