简介

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

建立机器人模型

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

Sawyer = 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金宝appnk模型中<一个href="//www.tatmou.com/in/in/help/robotics/ref/polynomialtrajectory.html" data-docid="robotics_ref#mw_59e05d0a-835f-4d56-85a6-ee509b10771a" class="a">多项式的轨迹Block使用五阶多项式计算机器人在轨迹上任意时刻的位置、速度和加速度。

金宝appSimulink模型概述

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

open_system (<年代p一个n年代tyle="color:#A020F0">“robotSafeTrajectoryTracking.slx”）;

的robotSafeTrajectoryTracking模型实现了一种具有轨迹缩放的计算扭矩控制器，用于安全的轨迹跟踪。在这个模型中有三个主要子系统:

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

构建计算转矩控制器

对于一个操纵器<年代p一个nclass="inlineequation"> $n$非固定节点时，系统动力学可表示为:

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

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

通过选择<年代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">“robotsafe轨迹跟踪/计算扭矩控制器”）;

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

一种确定的标准方法Kp而且Kd就是将它们计算为:

在哪里<年代p一个nclass="inlineequation"> ${\omega }_n$而且<年代p一个nclass="inlineequation"> $\xi$为二阶ODE的固有频率和阻尼比。在本例中，的默认值Kp而且Kd设固有频率和阻尼比为<年代p一个nclass="inlineequation"> ${\omega }_n＝10$而且<年代p一个nclass="inlineequation"> $\xi ＝1$使二阶ODE成为临界阻尼系统。

轨道伸缩

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

轨道伸缩是本例中为安全轨迹跟踪而部署的主块。在每个时间步<年代p一个nclass="inlineequation"> $t_{我}$时，原时间戳计算为<年代p一个nclass="inlineequation"> $t_{我}＝t_{我-1}+\Delta t$．然而，当机器人与一个意想不到的物体碰撞时，不断增加的扭矩和偏离计划的轨迹对机器人和物体都是毁灭性的。轨迹缩放的主要思想是将时间戳计算为<年代p一个nclass="inlineequation"> $t_{我}＝t_{我-1}+f_{\mathrm{年代}}（{问}_d，{\underset{}{\overset{\dot{}}{问}}}_d，{\underset{}{\overset{¨}{问}}}_d，{\tau }_{米e\mathrm{一个}}）\Delta t$通过引入<年代p一个nclass="inlineequation"> $f_{\mathrm{年代}}（{问}_d，{\underset{}{\overset{\dot{}}{问}}}_d，{\underset{}{\overset{¨}{问}}}_d，{\tau }_{米e\mathrm{一个}}）\in ［-1，1］$，这是期望运动和测量扭矩的函数<年代p一个nclass="inlineequation"> ${\tau }_{米e\mathrm{一个}}$．这个函数<年代p一个nclass="inlineequation"> $f_{\mathrm{年代}}（）$控制机器人运动的速度，根据机器人感受到的干扰来确定。如果测量到的扭矩大于预期，<年代p一个nclass="inlineequation"> $f_{\mathrm{年代}}（）$减小，以使机器人减速甚至向后移动，直到达到所需的扭矩。这些值<年代p一个nclass="inlineequation"> $f_{\mathrm{年代}}（）$对机器人的运动有以下影响:

在<年代trong class="emphasis bold">轨道伸缩挡块时，需要估计外转矩<年代p一个nclass="inlineequation"> ${\underset{}{\overset{ˆ}{\tau }}}_{ext}＝{\tau }_{米e\mathrm{一个}}-\underset{}{\overset{ˆ}{\tau }}$计算<年代p一个nclass="inlineequation"> $f_{\mathrm{年代}}（）$,在那里<年代p一个nclass="inlineequation"> ${\tau }_{米e\mathrm{一个}}$是Simscape模型的测量扭矩，和<年代p一个nclass="inlineequation"> $\underset{}{\overset{ˆ}{\tau }}$是先前时间戳上期望运动的期望扭矩。在模型的“外部扭矩观测器”部分中，逆动力学Block计算从测量扭矩中减去的期望扭矩。期望扭矩为:<年代p一个nclass="inlineequation"> $\underset{}{\overset{ˆ}{\tau }}＝米（{问}_d）（{\underset{}{\overset{¨}{问}}}_d）+C（{问}_d，{\underset{}{\overset{\dot{}}{问}}}_d）{\underset{}{\overset{\dot{}}{问}}}_d+G（{问}_d）$．

在<年代trong class="emphasis bold">想要运动区域，轨迹缩放算法的输出作为输入馈送到<年代trong class="emphasis bold">多项式的轨迹块。该块计算一个已知值的五次多项式轨迹q0处而且第一季度它们的边界条件是0速度和加速度。它输出位置、速度和加速度:<年代p一个nclass="inlineequation"> ${问}_d（t）$，<年代p一个nclass="inlineequation"> ${\underset{}{\overset{\dot{}}{问}}}_d（t）$,<年代p一个nclass="inlineequation"> ${\underset{}{\overset{¨}{问}}}_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迹缩放和动态仿真相结合，实现安全的轨迹跟踪。由此产生的扭矩、反作用力和时间戳证明了轨迹缩放执行安全轨迹跟踪的能力。