介绍

在本例中，将运行一个模型实现与关节位置，并且使用机械手算法块速度反馈的计算转矩控制器。该控制器接收来自模拟机器人（使用的Simscape多体实现）关节的位置和速度信息，并将扭矩指令，以沿给定的关节轨迹驱动机器人。的平面物体被放置在机器人的前面，使得机器人臂的端部执行将被执行的轨迹跟踪控制时与它发生碰撞。没有任何额外的设置，由于与物体发生碰撞的增加的扭矩可以引起在机器人或所述物体的损坏。实现安全轨迹跟踪，轨迹缩放块内置分配到控制器的期望的运动时调整时间戳。该模式运行时，您可能会与机器人调整一些参数和互动，并观察对仿真机器人的效果。

设立在工作区中的机器人模型

本例使用重新思考锯工，7度的自由度的机器人操纵器的模型。呼叫importrobot以产生rigidBodyTree模型从存储在统一机器人描述格式（URDF）文件中的描述。设置DATAFORMAT和重力性能与的Simscape一致。

锯工= importrobot（<小号p一个ñ小号Ťyle="color:#A020F0">'sawyer.urdf'）;sawyer.removeBody（<小号p一个ñ小号Ťyle="color:#A020F0">'头'）;sawyer.DataFormat =<小号p一个ñ小号Ťyle="color:#A020F0">'柱';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）;

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

用exampleHelperPolynomialTrajectory以产生用于每个关节的期望的运动轨迹。exampleHelperPolynomialTrajectory生成的轨迹，其满足所需的关节位置，零速度和基于所述初始和目标配置和轨迹持续时间零加速度的多项式系数。

P = exampleHelperPolynomialTrajectory（Q0，Q1，tDuration）;

万博1manbetxSimulink模型概述

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

open_system（<小号p一个ñ小号Ťyle="color:#A020F0">'robotSafeTrajectoryTracking.slx'）;

该robotSafeTrajectoryTracking模型实现了一个计算力矩控制器轨迹缩放安全轨迹跟踪。还有在这个模型中三个主要的子系统：

在每个时间步骤中，如果轨迹缩放开关接通时，所述修改的时间戳被用于评估所期望的关节位置，速度和加速度。然后，所计算的扭矩控制器使用与相关联的操纵器块RigidBodyTree模型来跟踪所要求的运动。所导出的控制输入被馈送到在多体的Simscape的锯工模型（其中包括用于与机器人交互的平面对象）。

构建使用机器人机械臂块计算力矩控制器

对于机械手<小号p一个ñclass="inlineequation"> $ñ$非固定接头，系统动力学可以表示为：

哪里<小号p一个ñclass="inlineequation"> $q$，<小号p一个ñclass="inlineequation"> $\underset{}{\overset{\dot{}}{q}}$，<小号p一个ñclass="inlineequation"> $\underset{}{\overset{¨}{q}}$，<小号p一个ñclass="inlineequation"> $\in {\mathrm{[R}}^{ñ}$是广义坐标的位置，速度和加速度，<小号p一个ñclass="inlineequation"> $ü\in {\mathrm{[R}}^{ñ}$为控制输入（转矩），<小号p一个ñclass="inlineequation"> $\mathrm{中号}（q）$是关节空间质量矩阵，<小号p一个ñclass="inlineequation"> $C（q，\underset{}{\overset{\dot{}}{q}}）\underset{}{\overset{\dot{}}{q}}$是速度产物扭矩，<小号p一个ñclass="inlineequation"> $G（q）$是重力力矩。沿着与所希望的位置所希望的关节轨迹跟踪<小号p一个ñclass="inlineequation"> $q_d$，速度<小号p一个ñclass="inlineequation"> ${\underset{}{\overset{\dot{}}{q}}}_d$和加速度<小号p一个ñclass="inlineequation"> ${\underset{}{\overset{¨}{q}}}_d$，所计算的扭矩控制器计算，以获得给定的配置和速度所需要的转矩，所提供的机器人动力学变量<小号p一个ñclass="inlineequation"> $\mathrm{中号}（q）$，<小号p一个ñclass="inlineequation"> $C（q，\underset{}{\overset{\dot{}}{q}}）\underset{}{\overset{\dot{}}{q}}$和<小号p一个ñclass="inlineequation"> $G（q）$。在Si万博1manbetxmulink，这些变量可以很容易地使用从机器人系统工具箱机器人操纵器块设计以下衍生的计算力矩控制器：

哪里<小号p一个ñclass="inlineequation"> $q_{Ë}=q-q_d$是位置误差和<小号p一个ñclass="inlineequation"> ${\underset{}{\overset{\dot{}}{q}}}_{Ë}=\underset{}{\overset{\dot{}}{q}}-{\underset{}{\overset{\dot{}}{q}}}_d$是速度误差。与此控制器的输入，该系统动力学成为二阶ODE：

通过选择<小号p一个ñclass="inlineequation"> ${ķ}_d$和<小号p一个ñclass="inlineequation"> ${ķ}_p$正确的，跟踪误差<小号p一个ñclass="inlineequation"> $q_{Ë}$将收敛，当时间趋于无穷大到零。

该<小号Ťrong class="emphasis bold">计算力矩控制器子系统使用三个机器人机械手模块构建：联合太空质量矩阵，速度产品扭力和重力扭矩。需要注意的是相关的rigidBodyTree模型，索耶被分配在所有这些块，并且配置和速度需要被插入作为列向量。

open_system（<小号p一个ñ小号Ťyle="color:#A020F0">“robotSafeTrajectoryTracking /计算转矩控制器”）;

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

一个标准的方法来确定KP和KD是计算它们为：

哪里<小号p一个ñclass="inlineequation"> ${\omega }_{ñ}$和<小号p一个ñclass="inlineequation"> $\xi$是固有频率和第二次ODE的阻尼比。在这个例子中，默认值KP和KD通过设置的固有频率和阻尼比为衍生<小号p一个ñclass="inlineequation"> ${\omega }_{ñ}=10$和<小号p一个ñclass="inlineequation"> $\xi =1$使二阶ODE一个临界阻尼系统。

轨迹缩放

还有在这个子系统两大块：

轨迹缩放在这个例子中部署安全轨迹跟踪的主要街区。在每个时间步<小号p一个ñclass="inlineequation"> ${Ť}_{\mathrm{一世}}$，原始时间戳被计算为<小号p一个ñclass="inlineequation"> ${Ť}_{\mathrm{一世}}={Ť}_{\mathrm{一世}-1}+\Delta Ť$。然而，当与一个意想不到的目的，增加的扭矩和偏差从计划轨迹机器人碰撞可能是破坏性的机器人与物体两者。轨迹缩放的主要思想是计算时间标记为<小号p一个ñclass="inlineequation"> ${Ť}_{\mathrm{一世}}={Ť}_{\mathrm{一世}-1}+F_{\mathrm{小号}}（q_d，{\underset{}{\overset{\dot{}}{q}}}_d，{\underset{}{\overset{¨}{q}}}_d，{\tau }_{米Ë\mathrm{一个}}）\Delta Ť$通过引入<小号p一个ñclass="inlineequation"> $F_{\mathrm{小号}}（q_d，{\underset{}{\overset{\dot{}}{q}}}_d，{\underset{}{\overset{¨}{q}}}_d，{\tau }_{米Ë\mathrm{一个}}）\in [-1，1]$，这是所希望的运动和测量的扭矩的函数<小号p一个ñclass="inlineequation"> ${\tau }_{米Ë\mathrm{一个}}$。功能<小号p一个ñclass="inlineequation"> $F_{\mathrm{小号}}（）$控制机器人的运动的速度和基于由所述机器人感受到的干扰被确定。如果测得的扭矩大于预期，<小号p一个ñclass="inlineequation"> $F_{\mathrm{小号}}（）$降低到使机器人减慢或甚至直至所需的转矩来实现向后移动。这些值<小号p一个ñclass="inlineequation"> $F_{\mathrm{小号}}（）$对机器人的运动以下效果：

在里面<小号Ťrong class="emphasis bold">轨迹缩放块，它是估计的外部扭矩所需<小号p一个ñclass="inlineequation"> ${\underset{}{\overset{}{\tau }}}_{ËXŤ}={\tau }_{米Ë\mathrm{一个}}-\underset{}{\overset{}{\tau }}$计算<小号p一个ñclass="inlineequation"> $F_{\mathrm{小号}}（）$，其中<小号p一个ñclass="inlineequation"> ${\tau }_{米Ë\mathrm{一个}}$是从所述的Simscape模型所测得的扭矩，并<小号p一个ñclass="inlineequation"> $\underset{}{\overset{}{\tau }}$是在以前的时间标记所要求的运动的期望转矩。在该模型中的外部转矩观测器部，所述逆动力学块计算其从测量转矩中减去预期的转矩。预期的扭矩是：<小号p一个ñclass="inlineequation"> $\underset{}{\overset{}{\tau }}=\mathrm{中号}（q_d）（{\underset{}{\overset{¨}{q}}}_d）+C（q_d，{\underset{}{\overset{\dot{}}{q}}}_d）{\underset{}{\overset{\dot{}}{q}}}_d+G（q_d）$。

在里面<小号Ťrong class="emphasis bold">期望的运动块，多项式系数被给予生成具有给定的期望的轨迹的timeStamp输入，其可基于轨迹缩放算法来调整。该<小号p一个ñclass="inlineequation"> $q_d（Ť）$，<小号p一个ñclass="inlineequation"> ${\underset{}{\overset{\dot{}}{q}}}_d（Ť）$和<小号p一个ñclass="inlineequation"> ${\underset{}{\overset{¨}{q}}}_d（Ť）$是基于轨迹多项式输出和被馈送到计算转矩控制器子系统。

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

在多体的Simscape机器人模型是从同一进口.urdf使用文件smimport其中一组扭矩致动器和传感器，用于关节扭矩，关节的位置和速度的相加。如下面所示的触点机构块被添加到模拟端部执行器和所述障碍物为球体和一个平面上，其中使用简单的线性弹簧 - 阻尼器模型之间的反作用力。

注意：接触机构只被端部执行器和所述平面对象之间实现。因此，机器人臂的其它部分仍然可以穿过障碍物。

运行模型

运行模型，观察机器人仿真和互动与它索耶的行为。

摘要

这个例子说明了如何使用机器人操纵块在Simulink设计一个计算力矩控制，并在多体的Simscape它的轨迹缩放和动态模拟集万博1manbetx成，实现安全轨迹跟踪。由此产生的转矩，反作用力和时间戳还证明轨迹缩放的能力，用于执行安全轨迹跟踪。