La traducción de esta página aún no se ha actualizado a la versión más reciente. Haga clic aquí para ver la última versión en inglés.

Modelado de control de trayectoria con cinemática inversa

Este ejemplo utiliza:

Abrir script en vivo

Este ejemplo de Simulink muestra cómo el bloque Inverse Kinematics puede dirigir un manipulador a lo largo de una trayectoria especificada. La trayectoria deseada se especifica como una serie de poses estrechamente espaciadas para el efector final del manipulador. La generación de trayectorias y la definición de waypoints representa numerosas aplicaciones robóticas, como operaciones pick-and-place, cálculo de trayectorias a partir de aceleración espacial y perfiles de velocidad, o incluso imitación de observaciones externas de marcos clave mediante cámaras y visión artificial. Una vez que se genera una trayectoria, se usa el bloque Inverse Kinematics para traducirla a una trayectoria en el espacio articular, que posteriormente puede utilizarse para simular la dinámica del manipulador y el controlador.

Visión general del modelo

Cargue el modelo para ver cómo se ha construido.

open_system('IKTrajectoryControlExample.slx');

El modelo se compone de cuatro operaciones primarias:

Generación de la pose objetivo
Cinemática inversa
Dinámica del manipulador
Medición de poses

Generación de la pose objetivo

Esta gráfica de Stateflow selecciona qué waypoint es el objetivo actual de manipulador. La gráfica ajusta el objetivo al siguiente waypoint una vez que el manipulador se encuentra dentro de un área de tolerancia del objetivo actual. La gráfica también convierte y ensambla los componentes del waypoint en una transformación homogénea mediante la función eul2tform. Cuando ya no se pueden seleccionar más waypoints, la gráfica finaliza la simulación.

Cinemática inversa

La cinemática inversa ha calculado un conjunto de ángulos de articulación para producir la pose deseada para el efector final. Use Inverse Kinematics con un modelo de rigidBodyTree y especifique la pose objetivo del efector final como una transformación homogénea. Especifique una serie de ponderaciones para las limitaciones de tolerancia relativa en la posición y orientación de la solución, y proporcione una estimación inicial de las posiciones de articulación. El bloque genera un vector de posiciones de articulación que producen la pose deseada para el modelo de rigidBodyTree especificado en los parámetros del bloque. Para garantizar la plena continuidad de las soluciones, la solución de configuración anterior se usa como la posición de partida para el solver. Esto también reduce la redundancia de los cálculos si la pose objetivo no se ha actualizado desde la última unidad de tiempo de la simulación.

Dinámica del manipulador

La dinámica del manipulador consta de dos componentes: un controlador para generar señales de par motor y un modelo de dinámica para modelar la dinámica del manipulador con las señales de par motor dadas. En este ejemplo, el controlador utiliza un componente prealimentado calculado mediante la dinámica inversa del manipulador y un componente de controlador PD por retroalimentación para corregir errores. El modelo del manipulador utiliza el bloque Forward Dynamics, que funciona con un objeto rigidBodyTree. Si desea utilizar técnicas de dinámica y visualización más sofisticadas, considere la posibilidad de utilizar herramientas del conjunto de bloques de Control Systems Toolbox™ y de Simscape Multibody™ para reemplazar el bloque Forward Dynamics.

Medición de poses

La medición de poses toma las lecturas de ángulos de las articulaciones del modelo del manipulador y las convierte en una matriz de transformación homogénea para utilizarlas como retroalimentación en la selección de waypoints.

Dinámica del manipulador

El manipulador utilizado para este ejemplo es el manipulador robótico Rethink Sawyer™. El objeto rigidBodyTree que describe el manipulador se importa desde un archivo URDF (formato de descripción de robot unificado) utilizando importrobot.

% Import the manipulator as a rigidBodyTree Object
sawyer = importrobot('sawyer.urdf');
sawyer.DataFormat = 'column';

% Define end-effector body name
eeName = 'right_hand';

% Define the number of joints in the manipulator
numJoints = 8;

% Visualize the manipulator
show(sawyer);
xlim([-1.00 1.00])
ylim([-1.00 1.00]);
zlim([-1.02 0.98]);
view([128.88 10.45]);

Generación de waypoints

En este ejemplo, el objetivo del manipulador es poder trazar los límites de las monedas detectadas en la imagen, coins.png. En primer lugar, la imagen se procesa para detectar los límites de las monedas.

I = imread('coins.png');
bwBoundaries = imread('coinBoundaries.png');

figure
subplot(1,2,1)
imshow(I,'Border','tight')
title('Original Image')

subplot(1,2,2)
imshow(bwBoundaries,'Border','tight')
title('Processed Image with Boundary Detection')

Figure contains 2 axes objects. Axes object 1 with title Original Image contains an object of type image. Axes object 2 with title Processed Image with Boundary Detection contains an object of type image.

Tras el procesamiento de la imagen, los bordes de las monedas se extraen como localizaciones de píxeles. Los datos se cargan desde el archivo MAT, boundaryData. boundaries es un arreglo de celdas en el que cada celda contiene un arreglo que describe las coordenadas de píxeles de uno de los límites detectados. Encontrará instrucciones más detalladas sobre cómo generar estos datos en el ejemplo "Trazado de límites en imágenes" (requiere Image Processing Toolbox).

load boundaryData.mat boundaries
whos boundaries

  Name             Size            Bytes  Class    Attributes

  boundaries      10x1             25376  cell

Para asignar estos datos al marco del mundo real, se debe definir dónde está situada la imagen y cuál es el escalado entre las coordenadas de píxeles y las coordenadas espaciales.

% Image origin coordinates
imageOrigin = [0.4,0.2,0.08];

% Scale factor to convert from pixels to physical distance
scale = 0.0015;

También es necesario definir los ángulos de Euler para la orientación del efector final deseada en cada punto.

eeOrientation = [0, pi, 0];

En este ejemplo, la orientación se ha seleccionado de modo que el efector final esté siempre perpendicular al plano de la imagen.

Una vez que se ha definido esta información, cada uno de los conjuntos de coordenadas y ángulos de Euler deseados se puede compilar en un waypoint. Cada waypoint se representa como un vector de seis elementos cuyos primeros tres elementos se corresponden con las posiciones del manipulador xyz- deseadas en el marco del mundo real. Los últimos tres elementos se corresponden con los ángulos de Euler ZYX de la orientación deseada.

$Waypoint = [\begin{matrix} X & Y & Z & ϕ_{z} & ϕ_{y} & ϕ_{x} \end{matrix}]$

Los waypoints se concatenan para formar un arreglo de n por 6 en el que n es el número total de poses de la trayectoria. Cada fila del arreglo se corresponde con uno de los waypoints de la trayectoria.

% Clear previous waypoints and begin building wayPoint array
clear wayPoints

% Start just above image origin
waypt0 = [imageOrigin + [0 0 .2],eeOrientation];

% Touch the origin of the image
waypt1 = [imageOrigin,eeOrientation];

% Interpolate each element for smooth motion to the origin of the image
for i = 1:6
    
    interp = linspace(waypt0(i),waypt1(i),100);
    wayPoints(:,i) = interp';
    
end

Hay 10 monedas en total. Para simplificar y acelerar el proceso, se puede trazar un subconjunto de monedas más pequeño limitando el número total que se transfiere a los waypoints. En el siguiente ejemplo, numTraces = 3 define la cantidad de monedas a trazar en la imagen.

% Define the number of coins to trace
numTraces = 3;

% Assemble the waypoints for boundary tracing
for i = 1:min(numTraces, size(boundaries,1))
    
    %Select a boundary and map to physical size
    segment = boundaries{i}*scale;
    
    % Pad data for approach waypoint and lift waypoint between boundaries
    segment = [segment(1,:); segment(:,:); segment(end,:)];
    
    % Z-offset for moving between boundaries
    segment(1,3) = .02;
    segment(end,3) = .02;
    
    % Translate to origin of image
    cartesianCoord = imageOrigin + segment;
    
    % Repeat desired orientation to match the number of waypoints being added
    eulerAngles = repmat(eeOrientation,size(segment,1),1);
    
    % Append data to end of previous wayPoints 
    wayPoints = [wayPoints;
                 cartesianCoord, eulerAngles]; 
end

Este arreglo es la entrada primaria al modelo.

Configuración del modelo

Se deben inicializar varios parámetros antes de ejecutar el modelo.

% Initialize size of q0, the robot joint configuration at t=0. This will
% later be replaced by the first waypoint.
q0 = zeros(numJoints,1);

% Define a sampling rate for the simulation.
Ts = .01;

% Define a [1x6] vector of relative weights on the orientation and 
% position error for the inverse kinematics solver.
weights = ones(1,6);

% Transform the first waypoint to a Homogenous Transform Matrix for initialization
initTargetPose = eul2tform(wayPoints(1,4:6));
initTargetPose(1:3,end) = wayPoints(1,1:3)';

% Solve for q0 such that the manipulator begins at the first waypoint
ik = inverseKinematics('RigidBodyTree',sawyer);
[q0,solInfo] = ik(eeName,initTargetPose,weights,q0);

Simular el movimiento del manipulador

Para simular el modelo, use el comando sim. El modelo genera el conjunto de datos de salida, jointData, y muestra el progreso en dos gráficas:

La gráfica X Y muestra una vista cenital de los movimientos de trazado del manipulador. Las líneas entre los círculos se trazan a medida que el manipulador pasa del contorno de una moneda al siguiente.
La gráfica Waypoint Tracking (Seguimiento de waypoints) permite visualizar el progreso en 3D. El punto verde indica la posición objetivo. El punto rojo indica la posición real del efector final lograda por el efector usando el control por retroalimentación.

% Close currently open figures 
close all

% Open & simulate the model
open_system('IKTrajectoryControlExample.slx');
sim('IKTrajectoryControlExample.slx');

MATLAB figure

Visualizar los resultados

El modelo genera dos conjuntos de datos que pueden utilizarse con fines de visualización tras la simulación. Las configuraciones de articulaciones se muestran como jointData. Las poses del efector final del robot se generan como poseData.

% Remove unnecessary meshes for faster visualization
clearMeshes(sawyer);

% Data for mapping image
[m,n] = size(I);

[X,Y] = meshgrid(0:m,0:n);
X = imageOrigin(1) + X*scale;
Y = imageOrigin(2) + Y*scale;

Z = zeros(size(X));
Z = Z + imageOrigin(3);

% Close all open figures
close all

% Initialize a new figure window
figure;
set(gcf,'Visible','on');

% Plot the initial robot position
show(sawyer, jointData(1,:)');
hold on

% Initialize end effector plot position
p = plot3(0,0,0,'.');
warp(X,Y,Z,I');

% Change view angle and axis
view(65,45)
axis([-.25 1 -.25 .75 0 0.75])

% Iterate through the outputs at 10-sample intervals to visualize the results
for j = 1:10:length(jointData)
    % Display manipulator model
    show(sawyer,jointData(j,:)', 'Frames', 'off', 'PreservePlot', false);
    
    % Get end effector position from homoegenous transform output
    pos = poseData(1:3,4,j);
    
    % Update end effector position for plot
    p.XData = [p.XData pos(1)];
    p.YData = [p.YData pos(2)];
    p.ZData = [p.ZData pos(3)];
    
    % Update figure
    drawnow
end