Modelado de control de trayectoria con cinemática inversa
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 de robótica, 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 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')
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.
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. El número de monedas que traza este código en las imágenes son tres.
% 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 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');
Visualizar los resultados
El modelo genera dos conjuntos de datos que pueden utilizarse con fines de visualización después de 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