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.

Modelado de Control de Trayectoria con Cinemática Inversa

En este ejemplo se muestra cómo el bloque puede controlar un manipulador a lo largo de una trayectoria especificada.Inverse Kinematics La trayectoria deseada se especifica como una serie de poses estrechamente espaciadas para el efector final del manipulador. La generación de trayectoria y la definición de waypoint representan muchas aplicaciones robóticas como la operación de picking y colocación, el cálculo de trayectorias a partir de perfiles de aceleración espacial y velocidad, o incluso imitar observaciones externas de fotogramas clave utilizando cámaras y la visión por computadora. Una vez que se genera una trayectoria, el bloque se utiliza para traducir esto a una trayectoria de espacio conjunto, que luego se puede utilizar para simular la dinámica del manipulador y controlador.Inverse Kinematics

Descripción general del modelo

El modelo se compone de cuatro operaciones primarias:

  • Generación de Pose objetivo

  • Cinemática Inversa

  • Dinámica de manipuladores

  • Medición de pose

Generación de Pose objetivo

Este gráfico Stateflow selecciona qué waypoint es el objetivo actual para el manipulador. El gráfico ajusta el objetivo al siguiente waypoint una vez que el manipulador llega a una tolerancia del objetivo actual. El gráfico también convierte y ensambla los componentes del waypoint en una transformación homogénea a través de la función.eul2tform Una vez que no hay más waypoints para seleccionar, el gráfico termina la simulación.

Cinemática Inversa

La cinemática inversa calculó un conjunto de ángulos de unión para producir una pose deseada para un efector final. Utilice el modelo con un modelo y especifique la pose de destino del efecto final como una transformación homogénea.Inverse KinematicsrigidBodyTree Especifique una serie de ponderaciones para las restricciones de tolerancia relativa en la posición y orientación de la solución y proporcione una estimación inicial de las posiciones de la unión. El bloque genera un vector de posiciones de unión que producen la pose deseada a partir del modelo especificado en los parámetros de bloque.rigidBodyTree Para garantizar una continuidad fluida de las soluciones, la solución de configuración anterior se utiliza como posición inicial para el solucionador. Esto también reduce la redundancia de los cálculos si la pose de destino no se ha actualizado desde el último paso de tiempo de simulación.

Dinámica de manipuladores

La dinámica del manipulador consta de dos componentes, un controlador para generar señales de par y un modelo dinámico para modelar la dinámica del manipulador dadas estas señales de par. El controlador en el ejemplo utiliza un componente de avance de avance de avance de avance calculado a través de la dinámica inversa del manipulador y un controlador de DP de retroalimentación para corregir el error. El modelo del manipulador utiliza el bloque que funciona con un objeto.Forward DynamicsrigidBodyTree Para obtener técnicas de visualización y dinámicas más sofisticadas, considere la posibilidad de utilizar herramientas de Control Systems Toolbox™ blockset y Simscape Multibody™ reemplazar el bloque Forward Dynamics.

Medición de pose

La medición de la pose toma las lecturas del ángulo de la articulación del modelo del manipulador y las convierte en una matriz de transformación homogénea que se utilizará como retroalimentación en la sección.Waypoint Selection

Definición del manipulador

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

% 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 Waypoint

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 encontrar 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')

Después del procesamiento de la imagen, los bordes de las monedas se extraen como ubicaciones de píxeles. Los datos se cargan desde el archivo MAT, . es una matriz de celdas donde cada celda contiene una matriz que describe las coordenadas de píxeles para un único límite detectado.boundaryDataboundaries Una vista más completa de cómo generar estos datos se puede encontrar en el ejemplo, "Seguimiento de límites en imágenes" (requiere cuadro de herramientas de procesamiento de imágenes).

load boundaryData.mat boundaries whos boundaries
  Name             Size            Bytes  Class    Attributes    boundaries      10x1             25456  cell                

Para asignar estos datos al marco del mundo, necesitamos definir dónde se encuentra la imagen y la escala 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 deben definirse los ángulos de Euler para la orientación del efector final deseado en cada punto.

eeOrientation = [0, pi, 0];

En este ejemplo se elige la orientación de tal manera que el efector final siempre sea perpendicular al plano de la imagen.

Una vez definida esta información, cada conjunto de coordenadas deseadas y los ángulos de Euler se pueden compilar en un waypoint. Cada waypoint se representa como un vector de seis elementos cuyos tres primeros elementos corresponden a las posiciones deseadas del manipulador en el marco del mundo.xyz- Los tres últimos elementos corresponden a los ángulos ZYX Euler de la orientación deseada.

<math display="block">
<mrow>
<mi mathvariant="normal">Waypoint</mi>
<mo>=</mo>
<mtext></mtext>
<mrow>
<mo>[</mo>
<mtable>
<mtr>
<mtd>
<mrow>
<mi mathvariant="italic">X</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi mathvariant="italic">Y</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi mathvariant="italic">Z</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mi>ϕ</mi>
</mrow>
<mrow>
<mi mathvariant="italic">z</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mi>ϕ</mi>
</mrow>
<mrow>
<mi mathvariant="italic">y</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mi>ϕ</mi>
</mrow>
<mrow>
<mi mathvariant="italic">x</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>]</mo>
</mrow>
</mrow>
</math>

Los waypoints se concatenan para formar una matriz -by-6, donde está el número total de poses en la trayectoria.nn Cada fila de la matriz corresponde a un waypoint en 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   % Assemble the waypoints for boundary tracing for i = 1: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

Esta matriz es la entrada principal del modelo.

Configuración del modelo

Se deben inicializar varios parámetros antes de que se pueda 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, utilice el comando.sim El modelo genera el dataset de salida y muestra el progreso en dos trazados:jointData

  • Muestra una vista de arriba hacia abajo de los movimientos de trazado del manipulador.X Y Plot Las líneas entre los círculos se producen a medida que el manipulador pasa de un contorno de moneda al siguiente.

  • La gráfica visualiza el progreso en 3D.Waypoint Tracking El punto verde indica la posición de destino. El punto rojo indica la posición real del efector final alcanzada por el efector final mediante el control de retroalimentación.

% Close currently open figures  close all  % Open & simulate the model open_system('IKTrajectoryControlExample.slx'); sim('IKTrajectoryControlExample.slx');

Visualizar los resultados

El modelo genera dos conjuntos de datos que se pueden utilizar para la visualización después de la simulación. Las configuraciones de juntas se proporcionan como .jointData Las posturas del efector final del robot se emiten 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