Main Content

Esta página es para la versión anterior. La página correspondiente en inglés ha sido eliminada en la versión actual.

Realizar un control de seguimiento de trayectoria segura utilizando bloques de manipuladores de robótica

En este ejemplo se muestra cómo utilizar Simulink® con Robotics System Toolbox™ bloques de algoritmos de manipulador para lograr un control de seguimiento de trayectoria seguro para un robot simulado que se ejecuta en Simscape™ Multibody™.

Tanto Robotics System Toolbox como Simscape Multibody son necesarios para ejecutar este ejemplo.

Introducción

En este ejemplo, ejecutará un modelo que implementa un controlador de par calculado con retroalimentación de posición y velocidad conjunta mediante bloques de algoritmo de manipulador. El controlador recibe información de posición y velocidad conjunta de un robot simulado (implementado con Simscape Multibody) y envía comandos de par para conducir al robot a lo largo de una trayectoria conjunta determinada. Un objeto plano se coloca delante del robot para que el efector final del brazo robótico colisione con él cuando se ejecute el control de seguimiento de trayectoria. Sin ninguna configuración adicional, el par creciente debido a la colisión con el objeto puede causar daño en el robot o el objeto. Para lograr un seguimiento de trayectoria seguro, se crea un bloque de escalado de trayectoria para ajustar la marca de tiempo al asignar el movimiento deseado al controlador. Puede ajustar algunos parámetros e interactuar con el robot mientras el modelo se está ejecutando y observar el efecto en el robot simulado.

Configurar el modelo de robot en el espacio de trabajo

Este ejemplo utiliza un modelo del Rethink Sawyer, un manipulador de robots de 7 grados de libertad. Llame para generar un modelo a partir de una descripción almacenada en un archivo de formato de descripción de robot unificado (URDF).importrobotrigidBodyTree Establece las propiedades y las propiedades para que sean coherentes con Simscape.DataFormatGravity

sawyer = importrobot('sawyer.urdf'); sawyer.removeBody('head'); sawyer.DataFormat = 'column'; sawyer.Gravity = [0, 0, -9.80665];

Generación de trayectoria y configuración relacionada

En primer lugar, asigne la hora de inicio y la duración de la trayectoria.

tStart = 0.5; tDuration = 3;

A continuación, asigne la configuración inicial y de destino. es la configuración inicial y es la configuración de destino.q0q1

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

Las siguientes figuras muestran la visualización del robot de la configuración inicial y la configuración de destino relacionada con la ubicación del objeto plano. El objeto plano se coloca para que el robot colisione con él durante el seguimiento de la trayectoria.

Se utiliza para generar la trayectoria de movimiento deseada para cada articulación. genera los coeficientes polinómicos de una trayectoria que satisface la posición de unión deseada, la velocidad cero y la aceleración cero en función de las configuraciones inicial y objetivo y la duración de la trayectoria.exampleHelperPolynomialTrajectoryexampleHelperPolynomialTrajectory

p = exampleHelperPolynomialTrajectory(q0,q1,tDuration);

Descripción general del modelo de Simulink

A continuación, abra el modelo Simulink. Las variables generadas anteriormente ya se almacenan en el espacio de trabajo del modelo de Simulink:

open_system('robotSafeTrajectoryTracking.slx');

El modelo implementa un controlador de par calculado con escalado de trayectoria para un seguimiento de trayectoria seguro.robotSafeTrajectoryTracking Hay tres subsistemas principales en este modelo:

  • Controlador de par computado

  • Escalado de trayectoria y movimiento deseado

  • Modelo Simscape Multibody con mecánica de contacto simple

En cada paso de tiempo, si el modificador de escala de trayectoria está activado, la marca de tiempo modificada se utiliza para evaluar la posición, velocidad y aceleración de la articulación deseada. A continuación, el controlador de par calculado utiliza los bloques de manipulador asociados con el modelo para realizar un seguimiento del movimiento deseado.RigidBodyTree La entrada de control derivada se introduce en el modelo Sawyer en Simscape Multibody (donde se incluye el objeto plano para interactuar con el robot).

Construir controlador de par computado utilizando bloques de manipulador de robótica

Para un manipulador con

<math display="inline">
<mrow>
<mi>n</mi>
</mrow>
</math>
juntas no fijas, la dinámica del sistema puede expresarse como:

<math display="block">
<mrow>
<mi>M</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
<mo>+</mo>
<mi>C</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo>,</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo stretchy="false">)</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo>+</mo>
<mi>G</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
<mo>=</mo>
<mi>u</mi>
</mrow>
</math>

Dónde

<math display="inline">
<mrow>
<mi>q</mi>
</mrow>
</math>
,
<math display="inline">
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
,
<math display="inline">
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
</math>
,
<math display="inline">
<mrow>
<mo></mo>
<msup>
<mrow>
<mi>R</mi>
</mrow>
<mrow>
<mi>n</mi>
</mrow>
</msup>
</mrow>
</math>
son la posición, velocidad y aceleración de la coordenada generalizada,
<math display="inline">
<mrow>
<mi>u</mi>
<mo></mo>
<msup>
<mrow>
<mi>R</mi>
</mrow>
<mrow>
<mi>n</mi>
</mrow>
</msup>
</mrow>
</math>
es la entrada de control (par),
<math display="inline">
<mrow>
<mi>M</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
es la matriz de masa espacial conjunta,
<math display="inline">
<mrow>
<mi>C</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo>,</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo stretchy="false">)</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
es el par de velocidad del producto,
<math display="inline">
<mrow>
<mi>G</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
es el par de gravedad. Para realizar un seguimiento a lo largo de una trayectoria de unión deseada con la posición deseada
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</math>
Velocidad
<math display="inline">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</math>
y la aceleración
<math display="inline">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</math>
, el controlador de par calculado calcula el par necesario para obtener una configuración y velocidad determinadas, siempre que las variables de dinámica del robot
<math display="inline">
<mrow>
<mi>M</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
,
<math display="inline">
<mrow>
<mi>C</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo>,</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo stretchy="false">)</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
Y
<math display="inline">
<mrow>
<mi>G</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
. En Simulink, estas variables se pueden derivar fácilmente utilizando bloques manipuladores de robótica de Robotics System Toolbox para diseñar el siguiente controlador de par calculado:

<math display="block">
<mrow>
<mi>u</mi>
<mo>=</mo>
<mi>M</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>p</mi>
</mrow>
</msub>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
<mo>+</mo>
<mi>C</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo>,</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo stretchy="false">)</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo>+</mo>
<mi>G</mi>
<mo stretchy="false">(</mo>
<mi>q</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>

Dónde

<math display="inline">
<mrow>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo>=</mo>
<mi>q</mi>
<mo>-</mo>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</math>
es el error de posición y
<math display="inline">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo>=</mo>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
<mo>-</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</math>
es el error de velocidad. Con esta entrada del controlador, la dinámica del sistema se convierte en un ODE de segundo orden:

<math display="block">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>p</mi>
</mrow>
</msub>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>0</mn>
</mrow>
</math>

Al elegir

<math display="inline">
<mrow>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
</mrow>
</math>
Y
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>p</mi>
</mrow>
</msub>
</mrow>
</math>
correctamente, el error de seguimiento
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>e</mi>
</mrow>
</msub>
</mrow>
</math>
convergerá a cero cuando el tiempo se acerca al infinito.

El subsistema se construye utilizando tres bloques manipuladores de robótica: , , y .Computed Torque ControllerJoint Space Mass MatrixVelocity Product TorqueGravity Torque Tenga en cuenta que el modelo asociado, , se asigna en todos esos bloques, y la configuración y la velocidad deben insertarse como vectores de columna.rigidBodyTreesawyer

open_system('robotSafeTrajectoryTracking/Computed Torque Controller');

Dentro del Controlador de Par Computado, hay dos parámetros ajustables (indicados por bloques de colores):

  • Ganar:Kp La ganancia proporcional al corregir la configuración del robot

  • Ganar:Kd La ganancia derivada al corregir la configuración del robot

Una forma estándar de determinar y es calcularlos como:KpKd

<math display="block">
<mrow>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>p</mi>
</mrow>
</msub>
<mo>=</mo>
<msubsup>
<mrow>
<mi>ω</mi>
</mrow>
<mrow>
<mi>n</mi>
</mrow>
<mrow>
<mn>2</mn>
</mrow>
</msubsup>
</mrow>
</math>

<math display="block">
<mrow>
<msub>
<mrow>
<mi>K</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<msub>
<mrow>
<mi>ω</mi>
</mrow>
<mrow>
<mi>n</mi>
</mrow>
</msub>
<mi>ξ</mi>
</mrow>
</math>

Dónde

<math display="inline">
<mrow>
<msub>
<mrow>
<mi>ω</mi>
</mrow>
<mrow>
<mi>n</mi>
</mrow>
</msub>
</mrow>
</math>
Y
<math display="inline">
<mrow>
<mi>ξ</mi>
</mrow>
</math>
son la frecuencia natural y la relación de amortiguación de la ODE de segundo orden. En este ejemplo, el valor predeterminado de y se derivan estableciendo la frecuencia natural y la relación de amortiguación comoKpKd
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>ω</mi>
</mrow>
<mrow>
<mi>n</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>1</mn>
<mn>0</mn>
</mrow>
</math>
Y
<math display="inline">
<mrow>
<mi>ξ</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
</math>
para hacer de la ODE de segundo orden un sistema amortiguado crítico.

Escalado de trayectoria

Hay dos bloques principales en este subsistema:

  • Escalado de trayectoria

  • Movimiento deseado

es el bloque principal desplegado para el seguimiento de trayectoria segura en este ejemplo.Escalado de trayectoria En cada paso de tiempo

<math display="inline">
<mrow>
<msub>
<mrow>
<mi>t</mi>
</mrow>
<mrow>
<mi>i</mi>
</mrow>
</msub>
</mrow>
</math>
, la marca de tiempo original se calcula como
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>t</mi>
</mrow>
<mrow>
<mi>i</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mrow>
<mi>t</mi>
</mrow>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<mi>Δ</mi>
<mi>t</mi>
</mrow>
</math>
. Sin embargo, cuando el robot choca con un objeto inesperado, el par creciente y la desviación de la trayectoria planificada pueden ser destructivos tanto para el robot como para el objeto. La idea principal del escalado de trayectoria es calcular la marca de tiempo como
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>t</mi>
</mrow>
<mrow>
<mi>i</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mrow>
<mi>t</mi>
</mrow>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mrow>
<mi>f</mi>
</mrow>
<mrow>
<mi>s</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<mi>τ</mi>
</mrow>
<mrow>
<mi>m</mi>
<mi>e</mi>
<mi>a</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
<mi>Δ</mi>
<mi>t</mi>
</mrow>
</math>
introduciendo
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>f</mi>
</mrow>
<mrow>
<mi>s</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<mi>τ</mi>
</mrow>
<mrow>
<mi>m</mi>
<mi>e</mi>
<mi>a</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
<mo></mo>
<mo stretchy="false">[</mo>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mn>1</mn>
<mo stretchy="false">]</mo>
</mrow>
</math>
, que es una función del movimiento deseado y el par medido
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>τ</mi>
</mrow>
<mrow>
<mi>m</mi>
<mi>e</mi>
<mi>a</mi>
</mrow>
</msub>
</mrow>
</math>
. La función
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>f</mi>
</mrow>
<mrow>
<mi>s</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mo stretchy="false">)</mo>
</mrow>
</math>
controla la velocidad del movimiento del robot y se determina en función de la interferencia que siente el robot. Si los pares medidos son mayores de lo esperado,
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>f</mi>
</mrow>
<mrow>
<mi>s</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mo stretchy="false">)</mo>
</mrow>
</math>
se reduce para hacer que el robot se ralentice o incluso se mueva hacia atrás hasta que se alcancen los pares deseados. Estos valores de
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>f</mi>
</mrow>
<mrow>
<mi>s</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mo stretchy="false">)</mo>
</mrow>
</math>
tienen los siguientes efectos en el movimiento del robot:

  • <math display="inline">
    <mrow>
    <msub>
    <mrow>
    <mi>f</mi>
    </mrow>
    <mrow>
    <mi>s</mi>
    </mrow>
    </msub>
    <mo stretchy="false">(</mo>
    <mo stretchy="false">)</mo>
    <mo>></mo>
    <mn>0</mn>
    </mrow>
    </math>
    , el robot avanza (
    <math display="inline">
    <mrow>
    <msub>
    <mrow>
    <mi>f</mi>
    </mrow>
    <mrow>
    <mi>s</mi>
    </mrow>
    </msub>
    <mo stretchy="false">(</mo>
    <mo stretchy="false">)</mo>
    <mo>=</mo>
    <mn>1</mn>
    </mrow>
    </math>
    es la velocidad normal).

  • <math display="inline">
    <mrow>
    <msub>
    <mrow>
    <mi>f</mi>
    </mrow>
    <mrow>
    <mi>s</mi>
    </mrow>
    </msub>
    <mo stretchy="false">(</mo>
    <mo stretchy="false">)</mo>
    <mo>=</mo>
    <mn>0</mn>
    </mrow>
    </math>
    , el robot se detiene.

  • <math display="inline">
    <mrow>
    <msub>
    <mrow>
    <mi>f</mi>
    </mrow>
    <mrow>
    <mi>s</mi>
    </mrow>
    </msub>
    <mo stretchy="false">(</mo>
    <mo stretchy="false">)</mo>
    <mo><</mo>
    <mn>0</mn>
    </mrow>
    </math>
    , el robot se mueve hacia atrás.

En el bloque, es necesario estimar el par externoEscalado de trayectoria

<math display="inline">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>τ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo>ˆ</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>e</mi>
<mi>x</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mrow>
<mi>τ</mi>
</mrow>
<mrow>
<mi>m</mi>
<mi>e</mi>
<mi>a</mi>
</mrow>
</msub>
<mo>-</mo>
<munderover accent="true">
<mrow>
<mi>τ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo>ˆ</mo>
</mrow>
</munderover>
</mrow>
</math>
para calcular
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>f</mi>
</mrow>
<mrow>
<mi>s</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mo stretchy="false">)</mo>
</mrow>
</math>
Dónde
<math display="inline">
<mrow>
<msub>
<mrow>
<mi>τ</mi>
</mrow>
<mrow>
<mi>m</mi>
<mi>e</mi>
<mi>a</mi>
</mrow>
</msub>
</mrow>
</math>
es el par medido del modelo Simscape, y
<math display="inline">
<mrow>
<munderover accent="true">
<mrow>
<mi>τ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo>ˆ</mo>
</mrow>
</munderover>
</mrow>
</math>
es el par esperado del movimiento deseado en la marca de tiempo anterior. En la sección Observador de par externo del modelo, el bloque calcula el par esperado que se resta del par de medida.Inverse Dynamics El par esperado es:
<math display="inline">
<mrow>
<munderover accent="true">
<mrow>
<mi>τ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo>ˆ</mo>
</mrow>
</munderover>
<mo>=</mo>
<mi>M</mi>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
<mo>+</mo>
<mi>C</mi>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo>+</mo>
<mi>G</mi>
<mo stretchy="false">(</mo>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">)</mo>
</mrow>
</math>
.

En el bloque, los coeficientes polinómicos se dan para generar la trayectoria deseada con la entrada dada, que se puede ajustar en función del algoritmo de escalado de trayectoria.Desired MotiontimeStamp el

<math display="inline">
<mrow>
<msub>
<mrow>
<mi>q</mi>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mi>t</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
,
<math display="inline">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mi>t</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
Y
<math display="inline">
<mrow>
<msub>
<mrow>
<munderover accent="true">
<mrow>
<mi>q</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">¨</mo>
</mrow>
</munderover>
</mrow>
<mrow>
<mi>d</mi>
</mrow>
</msub>
<mo stretchy="false">(</mo>
<mi>t</mi>
<mo stretchy="false">)</mo>
</mrow>
</math>
son salidas basadas en el polinomio de trayectoria y se alimentan al subsistema del Controlador de Par Computado.

Modelo de robot multicuerpo Simscape y mecánica de contacto simple

El Simscape Multibody Robot Model se importa desde el mismo archivo utilizando , donde se añade un conjunto de actuadores de par y sensores para par de unión, posición de la junta y velocidad..urdfsmimport Se añade un bloque de mecanismo de contacto como se muestra a continuación para simular la fuerza de reacción entre el efector final y el obstáculo como una esfera y un plano, donde se utiliza un modelo lineal simple de amortiguador de resorte.

El mecanismo de contacto solo se ha implementado entre el efector final y el objeto plano.Nota: Por lo tanto, otras partes del brazo robótico todavía pueden pasar a través del obstáculo.

Ejecutar el modelo

Ejecute el modelo y observe el comportamiento de Sawyer en el simulador de robots e interactúe con él.

  • En primer lugar, abre el visor haciendo clic en el icono de ámbito que se muestra a continuación a la izquierda del bloque de modelos de Simscape. Muestra señales que incluyen los pares de unión, la fuerza de contacto de reacción entre el efector final y el objeto plano, y la marca de tiempo para calcular el movimiento deseado para el seguimiento de la trayectoria.

  • Cambie el interruptor de escala de trayectoria a "Off".

  • Haga clic en el botón Reproducir de Simulink para iniciar la simulación. Debería ver el brazo chocar con el objeto produciendo altas entradas de par y una gran fuerza de reacción. Tenga en cuenta en este caso la marca de tiempo original. Detenga la simulación después.

  • Ahora, cambie el interruptor de escalado de trayectoria a "Activado" y vuelva a ejecutar el modelo. Observe las diferencias en los pares calculados y la fuerza de reacción reducida después de la colisión.

  • Mientras se ejecuta la simulación, ajuste el control deslizante para mover el objeto hacia o lejos del robot. El robot debe reaccionar a su posición mientras sigue tratando de ejecutar la trayectoria de forma segura.

  • Haga clic en el botón Detener de Simulink para detener la simulación.

Resumen

Este ejemplo mostró cómo utilizar bloques manipuladores de robótica en Simulink para diseñar un controlador de par calculado e integrarlo con el escalado de trayectoria y la simulación dinámica en Simscape Multibody para lograr un seguimiento de trayectoria seguro. El par, la fuerza de reacción y la marca de tiempo resultantes también demostraron la capacidad de escalado de trayectoria para realizar un seguimiento de trayectoria seguro.