Planear la trayectoria con jerk mínimo para el brazo robótico

Este ejemplo muestra cómo planificar una trayectoria polinomial con jerk mínimo para un manipulador robótico. El ejemplo muestra cómo cargar un modelo de robot, planificar una trayectoria para el modelo de robot en un entorno con obstáculos, generar una trayectoria con jerk mínimo a partir de la trayectoria y visualizar las trayectorias generadas y el movimiento del robot.

Configurar el entorno y el modelo de robot

Este ejemplo usa un modelo del KUKA LBR iiwa, un manipulador robótico con 7 grados de libertad. Use loadrobot para cargar el modelo de robot en el área de trabajo como un objeto rigidBodyTree. Establezca el formato de salida para las configuraciones en "row".

robot = loadrobot("kukaIiwa14",DataFormat="row");

Genere el entorno para el robot. Cree objetos de colisión y especifique sus poses respecto a la base del robot. Visualice el entorno.

env = {collisionBox(0.5,0.5,0.05,Pose=trvec2tform([0 0 -0.05])), ...
       collisionSphere(0.3,Pose=trvec2tform([0.1 0.2 0.8]))};
env{1}.Pose(3,end) = -0.05;
env{2}.Pose(1:3,end) = [0.1 0.2 0.8];

show(robot);
hold on
showCollisionArray(env);
axis([-1 1 -1 1 -0.1 1.75])
hold off

Planificar una trayectoria usando manipulatorRRT

Cree el planificador RRT para el modelo de robot usando manipulatorRRT. Configure la propiedad ValidationDistance para aumentar el número de estados intermedios al interpolar la trayectoria.

rrt = manipulatorRRT(robot,env);
rrt.ValidationDistance = 0.2;
rrt.SkippedSelfCollisions = "parent";

Especifique una configuración de inicio y una de objetivo.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Planifique la trayectoria. Debido a la aleatoriedad del algoritmo RRT, establezca la semilla rng para la repetibilidad.

rng(0)
path = plan(rrt,startConfig,goalConfig);

Interpole la trayectoria y recupere los waypoints.

interpPath = interpolate(rrt,path);
wpts = interpPath';

Generar una trayectoria polinomial con jerk mínimo

El planificador devuelve una trayectoria como un conjunto ordenado de waypoints. Para pasarlos a un robot, primero debe determinar una trayectoria a través de ellos. La función minjerkpolytraj crea una trayectoria sin obstáculos con jerk mínimo que llega a todos los waypoints especificados.

Especifique una estimación inicial de los puntos de tiempo en que el brazo robótico llega a los waypoints.

initialGuess = linspace(0,size(wpts,2)*0.2,size(wpts,2));

Especifique el número de muestras a tomar al estimar la trayectoria.

numSamples = 100;

Calcule la trayectoria polinomial con jerk mínimo.

[q,qd,qdd,qddd,pp,tpts,tSamples] = minjerkpolytraj(wpts,initialGuess,numSamples);

Visualizar trayectorias de articulación y waypoints

Represente las trayectorias y los waypoints a lo largo del tiempo.

minJerkPath = q';
figure
plot(tSamples,q)
hold on
scatter(tpts,wpts,40)
title("Joint Trajectories and Waypoints")
xlabel("Time")
ylabel("Joint Angle (rad)")

Visualizar el movimiento del robot

Use la función de objeto show para animar el movimiento resultante. Esta visualización permite actualizaciones rápidas para garantizar una animación fluida.

figure
ax = show(robot,startConfig);
hold on
showCollisionArray(env);
axis([-1 1 -1 1 -0.1 1.75])
title(["Animation of Robot Arm Following","Minimum Jerk Trajectory"])
for i = 1:size(minJerkPath,1)
    show(robot,minJerkPath(i,:),PreservePlot=false,FastUpdate=true);
    drawnow;
end
hold off