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.

Planifique y ejecute trayectorias libres de colisión utilizando el manipulador KINOVA Gen3

En este ejemplo se muestra cómo planificar trayectorias de robot libres de colisiones de bucle cerrado desde una postura inicial a una pose de efecto final deseada mediante el control predictivo de modelo no lineal. Las trayectorias resultantes se ejecutan utilizando un modelo de simulación de robot de baja fidelidad y control de par calculado. Los obstáculos pueden ser estáticos o dinámicos, y pueden establecerse como primitivos (esferas, cilindros, cajas) o como mallas personalizadas.

Descripción del robot y poses

Cargue el modelo de árbol de cuerpo rígido (RBT) KINOVA Gen3.

robot = loadrobot('kinovaGen3', 'DataFormat', 'column');

Consigue el número de articulaciones.

numJoints = numel(homeConfiguration(robot));

Especifique el marco del robot donde está conectado el efector final.

endEffector = "EndEffector_Link"; 

Especifique las posturas iniciales y deseadas del efecto final. Utilice la cinemática inversa para resolver la configuración inicial del robot dada una postura deseada.

% Initial end-effector pose taskInit = trvec2tform([[0.4 0 0.2]])*axang2tform([0 1 0 pi]);  % Compute current robot joint configuration using inverse kinematics ik = inverseKinematics('RigidBodyTree', robot); ik.SolverParameters.AllowRandomRestart = false; weights = [1 1 1 1 1 1]; currentRobotJConfig = ik(endEffector, taskInit, weights, robot.homeConfiguration); currentRobotJConfig = wrapToPi(currentRobotJConfig);  % Final (desired) end-effector pose taskFinal = trvec2tform([0.35 0.55 0.35])*axang2tform([0 1 0 pi]);   anglesFinal = rotm2eul(taskFinal(1:3,1:3),'XYZ'); poseFinal = [taskFinal(1:3,4);anglesFinal']; % 6x1 vector for final pose: [x, y, z, phi, theta, psi]

Mallas y obstáculos de colisión

Inicializar una función auxiliar que se utiliza para comprobar si hay colisiones entre los cuerpos del robot y el mundo de la colisión.checkCollision Establecer la propiedad para habilitar la comprobación de colisiones para todos los sólidos.ExhaustiveCheckingtrue

collisionHelper = helperManipCollsionsKINOVA(robot); collisionHelper.ExhaustiveChecking = true;

Para comprobar y evitar colisiones durante el control, debe configurar una colisión como un conjunto de objetos de colisión.world En este ejemplo se utilizan objetos como obstáculos a evitar.collisionSphere Cambie el siguiente booleano para planificar utilizando obstáculos estáticos en lugar de mover.

isMovingObst = true;

Los tamaños y las ubicaciones de los obstáculos se inicializan en la siguiente función auxiliar. Para agregar más obstáculos estáticos, agregue objetos de colisión en la matriz.world

helperCreateObstaclesKINOVA;

Visualice el robot en la configuración inicial. Usted debe ver los obstáculos en el medio ambiente, así.

x0 = [currentRobotJConfig', zeros(1,numJoints)]; helperInitialVisualizerKINOVA;

Especifique una distancia de seguridad lejos de los obstáculos. Este valor se utiliza en la función de restricción de desigualdad del controlador MPC no lineal.

safetyDistance = 0.01; 

Diseñar controlador predictivo de modelo no lineal

Puede diseñar el controlador predictivo de modelo no lineal mediante el siguiente archivo auxiliar, que crea un objeto de controlador.nlmpc (Model Predictive Control Toolbox) Para ver el archivo, escriba .edit helperDesignNLMPCobjKINOVA

helperDesignNLMPCobjKINOVA;

El controlador está diseñado sobre la base del siguiente análisis. El número máximo de iteraciones para el solucionador de optimización se establece en 5. Los límites inferior y superior para la posición de la articulación y las velocidades (estados) y las aceleraciones (entradas de control) se establecen explícitamente.

  • El modelo de juntas robóticas es descrito por los integradores dobles. Los estados del modelo son

    <math display="block">
    <mrow>
    <mi>x</mi>
    <mo>=</mo>
    <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>
    </mrow>
    </math>
    , donde las 7 posiciones conjuntas se denotan por
    <math display="inline">
    <mrow>
    <mi mathvariant="italic">q</mi>
    </mrow>
    </math>
    y sus velocidades son denotadas por
    <math display="block">
    <mrow>
    <munderover accent="true">
    <mrow>
    <mi>q</mi>
    </mrow>
    <mrow></mrow>
    <mrow>
    <mo stretchy="false">˙</mo>
    </mrow>
    </munderover>
    </mrow>
    </math>
    . Las entradas del modelo son las aceleraciones conjuntas
    <math display="block">
    <mrow>
    <mi>u</mi>
    <mo>=</mo>
    <munderover accent="true">
    <mrow>
    <mi>q</mi>
    </mrow>
    <mrow></mrow>
    <mrow>
    <mo stretchy="false">¨</mo>
    </mrow>
    </munderover>
    </mrow>
    </math>
    . La dinámica del modelo se da por

<math display="block">
<mrow>
<mover accent="true">
<mrow>
<mi mathvariant="italic">x</mi>
</mrow>
<mrow>
<mo>˙</mo>
</mrow>
</mover>
<mo>=</mo>
<mrow>
<mo>[</mo>
<mtable>
<mtr>
<mtd>
<mrow>
<mn>0</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">I</mi>
</mrow>
<mrow>
<mn>7</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>0</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>]</mo>
</mrow>
<mi mathvariant="italic">x</mi>
<mo>+</mo>
<mrow>
<mo>[</mo>
<mtable>
<mtr>
<mtd>
<mrow>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">I</mi>
</mrow>
<mrow>
<mn>7</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>]</mo>
</mrow>
<mi mathvariant="italic">u</mi>
</mrow>
</math>

Dónde

<math display="inline">
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">I</mi>
</mrow>
<mrow>
<mn>7</mn>
</mrow>
</msub>
</mrow>
</math>
denota que el
<math display="inline">
<mrow>
<mn>7</mn>
<mo>×</mo>
<mn>7</mn>
</mrow>
</math>
matriz de identidad. La salida del modelo se define como

<math display="inline">
<mrow>
<mi mathvariant="italic">y</mi>
<mo>=</mo>
<mrow>
<mo>[</mo>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">I</mi>
</mrow>
<mrow>
<mn>7</mn>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>]</mo>
</mrow>
<mi mathvariant="italic">x</mi>
</mrow>
</math>
.

Por lo tanto, el controlador predictivo del modelo no lineal ( ) tiene 14 estados, 7 salidas y 7 entradas.nlobj

  • La función de coste para es una función de coste no lineal personalizada, que se define de una manera similar a un coste de seguimiento cuadrático más un coste de terminal.nlobj

<math display="inline">
<mrow>
<mi mathvariant="italic">J</mi>
<mo>=</mo>
<mrow>
<msubsup>
<mrow>
<mo></mo>
</mrow>
<mrow>
<mn>0</mn>
</mrow>
<mrow>
<mi mathvariant="italic">T</mi>
</mrow>
</msubsup>
<mrow>
<msup>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">p</mi>
</mrow>
<mrow>
<mi mathvariant="normal">ref</mi>
</mrow>
</msub>
<mo>-</mo>
<mi mathvariant="italic">p</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">q</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo></mo>
</mrow>
</msup>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">r</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">p</mi>
</mrow>
<mrow>
<mi mathvariant="normal">ref</mi>
</mrow>
</msub>
<mo>-</mo>
<mi mathvariant="italic">p</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">q</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msup>
<mrow>
<mi mathvariant="italic">u</mi>
</mrow>
<mrow>
<mo></mo>
</mrow>
</msup>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
<mo>)</mo>
</mrow>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">u</mi>
</mrow>
</msub>
<mi mathvariant="italic">u</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
<mo>)</mo>
</mrow>
<mtext></mtext>
</mrow>
</mrow>
<mi mathvariant="normal">dt</mi>
<mo>+</mo>
<msup>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">p</mi>
</mrow>
<mrow>
<mi mathvariant="normal">ref</mi>
</mrow>
</msub>
<mo>-</mo>
<mi mathvariant="italic">p</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">q</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">T</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo></mo>
</mrow>
</msup>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">p</mi>
</mrow>
<mrow>
<mi mathvariant="normal">ref</mi>
</mrow>
</msub>
<mo>-</mo>
<mi mathvariant="italic">p</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">q</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">T</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msup>
<mrow>
<mover accent="true">
<mrow>
<mi mathvariant="italic">q</mi>
</mrow>
<mrow>
<mo>˙</mo>
</mrow>
</mover>
</mrow>
<mrow>
<mo></mo>
</mrow>
</msup>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">T</mi>
</mrow>
<mo>)</mo>
</mrow>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">v</mi>
</mrow>
</msub>
<mover accent="true">
<mrow>
<mi mathvariant="italic">q</mi>
</mrow>
<mrow>
<mo>˙</mo>
</mrow>
</mover>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">T</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</math>

Aquí

<math>
<mi mathvariant="italic">p</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">q</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</math>
transforma las posiciones conjuntas
<math display="inline">
<mrow>
<mi mathvariant="italic">q</mi>
<mrow>
<mo>(</mo>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</math>
al marco del efector final utilizando la cinemática delantera y , ygetTransform
<math display="inline">
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">p</mi>
</mrow>
<mrow>
<mi mathvariant="normal">ref</mi>
</mrow>
</msub>
</mrow>
</math>
denota la pose de efecto final deseada.

Las matrices

<math>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">r</mi>
</mrow>
</msub>
</math>
,
<math display="inline">
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">u</mi>
</mrow>
</msub>
</mrow>
</math>
,
<math display="inline">
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">t</mi>
</mrow>
</msub>
</mrow>
</math>
Y
<math display="inline">
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">Q</mi>
</mrow>
<mrow>
<mi mathvariant="italic">v</mi>
</mrow>
</msub>
</mrow>
</math>
son matrices de peso constante.

  • Para evitar colisiones, el controlador tiene que satisfacer las siguientes restricciones de desigualdad.

<math display="inline">
<mrow>
<msub>
<mrow>
<mi mathvariant="italic">d</mi>
</mrow>
<mrow>
<mi mathvariant="italic">i</mi>
<mo>,</mo>
<mi mathvariant="italic">j</mi>
</mrow>
</msub>
<mo></mo>
<msub>
<mrow>
<mi mathvariant="italic">d</mi>
</mrow>
<mrow>
<mi mathvariant="normal">safe</mi>
</mrow>
</msub>
</mrow>
</math>

Aquí

<math>
<msub>
<mrow>
<mi mathvariant="italic">d</mi>
</mrow>
<mrow>
<mi mathvariant="italic">i</mi>
<mo>,</mo>
<mi mathvariant="italic">j</mi>
</mrow>
</msub>
</math>
denota la distancia desde el
<math display="inline">
<mrow>
<mi mathvariant="italic">i</mi>
</mrow>
</math>
-th cuerpo robot a la
<math display="inline">
<mrow>
<mi mathvariant="italic">j</mi>
</mrow>
</math>
-ésimo obstáculo, calculado utilizando .checkCollision

En este ejemplo,

<math display="inline">
<mrow>
<mi mathvariant="italic">i</mi>
</mrow>
</math>
pertenece a
<math display="inline">
<mrow>
<mrow>
<mo>{</mo>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mo>,</mo>
<mn>3</mn>
<mo>,</mo>
<mn>4</mn>
<mo>,</mo>
<mn>5</mn>
<mo>,</mo>
<mn>6</mn>
</mrow>
<mo>}</mo>
</mrow>
</mrow>
</math>
(se excluyen los cuerpos robóticos de base y efecto final), y
<math display="inline">
<mrow>
<mi mathvariant="italic">j</mi>
</mrow>
</math>
pertenece a
<math display="inline">
<mrow>
<mrow>
<mo>{</mo>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
</mrow>
<mo>}</mo>
</mrow>
</mrow>
</math>
(se utilizan 2 obstáculos).

Los jacobinos de la función de estado, la función de salida, la función de costo y las restricciones de desigualdad se proporcionan para que el modelo de predicción mejore la eficiencia de la simulación. Para calcular la restricción de desigualdad jacobiana, utilice la función y la aproximación jacobiana en [1].geometricJacobian

Planificación de la trayectoria de bucle cerrado

Simular el robot para un máximo de 50 pasos con las condiciones iniciales correctas.

maxIters = 50; u0 = zeros(1,numJoints); mv = u0; time = 0; goalReached = false;

Inicializar la matriz de datos para el control.

positions = zeros(numJoints,maxIters); positions(:,1) = x0(1:numJoints)';  velocities = zeros(numJoints,maxIters); velocities(:,1) = x0(numJoints+1:end)';  accelerations = zeros(numJoints,maxIters); accelerations(:,1) = u0';  timestamp = zeros(1,maxIters); timestamp(:,1) = time;

Utilice la función para la generación de trayectoria de bucle cerrado.nlmpcmove (Model Predictive Control Toolbox) Especifique las opciones de generación de trayectoria utilizando un objeto.nlmpcmoveopt (Model Predictive Control Toolbox) Cada iteración calcula la posición, la velocidad y la aceleración de las articulaciones para evitar obstáculos a medida que se mueven hacia la meta. El script comprueba si el robot ha alcanzado la meta.helperCheckGoalReachedKINOVA El script mueve las ubicaciones de los obstáculos en función del paso de tiempo.helperUpdateMovingObstacles

options = nlmpcmoveopt; for timestep=1:maxIters     disp(['Calculating control at timestep ', num2str(timestep)]);     % Optimize next trajectory point      [mv,options,info] = nlmpcmove(nlobj,x0,mv,[],[], options);     if info.ExitFlag < 0         disp('Failed to compute a feasible trajectory. Aborting...')         break;     end     % Update states and time for next iteration     x0 = info.Xopt(2,:);     time = time + nlobj.Ts;     % Store trajectory points     positions(:,timestep+1) = x0(1:numJoints)';     velocities(:,timestep+1) = x0(numJoints+1:end)';     accelerations(:,timestep+1) = info.MVopt(2,:)';     timestamp(timestep+1) = time;     % Check if goal is achieved      helperCheckGoalReachedKINOVA;     if goalReached         break;     end     % Update obstacle pose if it is moving     if isMovingObst         helperUpdateMovingObstaclesKINOVA;     end end
Calculating control at timestep 1 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Calculating control at timestep 2 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Calculating control at timestep 3 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Calculating control at timestep 4 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Calculating control at timestep 5 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Calculating control at timestep 6 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Calculating control at timestep 7 Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard. Target configuration reached. 

Ejecutar la trayectoria planificada utilizando el control y la simulación de robots de baja fidelidad

Recorte los vectores de trayectoria en función de los pasos de tiempo calculados a partir del plan.

tFinal = timestep+1; positions = positions(:,1:tFinal); velocities = velocities(:,1:tFinal); accelerations = accelerations(:,1:tFinal); timestamp = timestamp(:,1:tFinal);  visTimeStep = 0.2;

Utilice a para realizar un seguimiento de la trayectoria con el control de par calculado.jointSpaceMotionModel La función genera las entradas derivadas para la función para modelar la trayectoria del robot calculado.helperTimeBasedStateInputsKINOVAode15s

motionModel = jointSpaceMotionModel('RigidBodyTree', robot);  % Control robot to target trajectory points in simulation using low-fidelity model initState = [positions(:,1);velocities(:,1)]; targetStates = [positions;velocities;accelerations]';     [t,robotStates] = ode15s(@(t,state) helperTimeBasedStateInputsKINOVA(motionModel, timestamp, targetStates, t, state), [timestamp(1):visTimeStep:timestamp(end)], initState); 

Visualice el movimiento del robot.

helperFinalVisualizerKINOVA;

[1] Schulman, J., et al. "Planificación del movimiento con optimización convexa secuencial y comprobación de colisiones convexas." 33.9 (2014):The International Journal of Robotics Research 1251-1270.