Main Content

Seguimiento de rutas para un robot de tracción diferencial

Este ejemplo muestra cómo controlar un robot para que siga la ruta deseada utilizando un simulador de robot. El ejemplo utiliza el controlador de seguimiento de rutas Pure Pursuit para dirigir un robot simulado a lo largo de una ruta predeterminada. La ruta deseada es un conjunto de waypoints definido explícitamente o calculado mediante un planificador de rutas (consulte Planificación de rutas en entornos de diferente complejidad). Se crea el controlador de seguimiento de rutas Pure Pursuit para un robot de tracción diferencial, que calcula los comandos de control para seguir una ruta determinada. Los comandos de control calculados se usan para dirigir el robot simulado a lo largo de la trayectoria deseada con el fin de seguir la ruta basada en el controlador Pure Pursuit.

Nota: A partir de la versión R2016b, en lugar de usar el método de pasos para realizar la operación definida por el System object™, puede llamar al objeto con argumentos, como si fuera una función. Por ejemplo, y = step(obj,x) y y = obj(x) realizan operaciones equivalentes.

Definir waypoints

Defina un conjunto de waypoints para la ruta que desea que siga el robot.

path = [2.00    1.00;
        1.25    1.75;
        5.25    8.25;
        7.25    8.75;
        11.75   10.75;
        12.00   10.00];

Establezca la ubicación actual y la ubicación objetivo del robot según lo definido en la ruta.

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

Asuma una orientación inicial del robot (la orientación del robot es el ángulo entre la dirección del robot y el eje X positivo, medido en el sentido contrario a las agujas del reloj).

initialOrientation = 0;

Defina la pose actual del robot [x y theta].

robotCurrentPose = [robotInitialLocation initialOrientation]';

Crear un modelo cinemático de robot

Inicialice el modelo de robot y asígnele una pose inicial. El robot simulado dispone de ecuaciones cinemáticas para mover un robot de tracción diferencial de dos ruedas. Las entradas a este robot simulado son las velocidades lineales y angulares.

robot = differentialDriveKinematics("TrackWidth", 1, "VehicleInputs", "VehicleSpeedHeadingRate");

Visualice la ruta deseada

figure
plot(path(:,1), path(:,2),'k--d')
xlim([0 13])
ylim([0 13])

Definir el controlador de seguimiento de rutas

En función de la ruta definida anteriormente y el modelo de movimiento del robot, necesitará un controlador de seguimiento de rutas para dirigir el robot a lo largo de la ruta. Cree el controlador de seguimiento de rutas utilizando el objeto controllerPurePursuit.

controller = controllerPurePursuit;

Utilice la ruta definida anteriormente para establecer los waypoints que desea usar para el controlador.

controller.Waypoints = path;

Configure los parámetros del controlador de seguimiento de rutas. La velocidad lineal deseada se establece en 0.6 metros/segundo en este ejemplo.

controller.DesiredLinearVelocity = 0.6;

La velocidad angular máxima actúa como límite de saturación para la velocidad de rotación, que en este ejemplo está establecida en 2 radianes/segundo.

controller.MaxAngularVelocity = 2;

En general, la distancia look-ahead debe ser mayor que la velocidad lineal deseada para que el robot se desplace por la ruta sin problemas. El robot podría tomar un atajo si la distancia look-ahead es grande. Por el contrario, una distancia look-ahead pequeña podría provocar que el robot se comportara de forma inestable al seguir la ruta. Para este ejemplo se ha elegido un valor de 0.3 m.

controller.LookaheadDistance = 0.3;

Uso del controlador de seguimiento de rutas para dirigir el robot a través de los waypoints deseados

El controlador de seguimiento de rutas proporciona señales de control de entrada para el robot, que las utiliza para autodirigirse a lo largo de la ruta deseada.

Defina un radio objetivo, que es el umbral de distancia deseado entre la ubicación final del robot y la ubicación objetivo. Una vez que el robot se encuentre a esta distancia del objetivo, se detendrá. Asimismo, debe calcular la distancia real entre la ubicación del robot y la ubicación objetivo. Esta distancia se compara continuamente con el radio objetivo, y el robot se detiene cuando esta distancia es inferior al radio objetivo.

Tenga en cuenta que un valor de radio objetivo demasiado pequeño puede provocar que el robot no alcance el objetivo y muestre un comportamiento inesperado al aproximarse al objetivo.

goalRadius = 0.1;
distanceToGoal = norm(robotInitialLocation - robotGoal);

El objeto controllerPurePursuit calcula los comandos de control del robot. Dirija al robot utilizando estos comandos de control hasta que alcance el radio objetivo. Si utiliza un simulador externo o un robot físico, deberá aplicar las salidas del controlador al robot. Es posible que se necesite un sistema de localización para actualizar la pose del robot. El controlador se ejecuta a 10 Hz.

% Initialize the simulation loop
sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);

% Initialize the figure
figure

% Determine vehicle frame size to most closely represent vehicle with plotTransforms
frameSize = robot.TrackWidth/0.8;

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    
    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    hold all
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 13])
    ylim([0 13])
    
    waitfor(vizRate);
end

Uso del controlador de seguimiento de rutas con un PRM

Si el conjunto de waypoints deseado se calcula mediante un planificador de rutas, el controlador de seguimiento de rutas puede utilizarse de forma similar. En primer lugar, visualice el mapa.

load exampleMaps
map = binaryOccupancyMap(simpleMap);
figure
show(map)

Puede calcular path usando el algoritmo de planificación de rutas PRM. Consulte Planificación de rutas en entornos de diferente complejidad para obtener más detalles.

mapInflated = copy(map);
inflate(mapInflated, robot.TrackWidth/2);
prm = robotics.PRM(mapInflated);
prm.NumNodes = 100;
prm.ConnectionDistance = 10;

Identifique una ruta entre la ubicación inicial y la final. Tenga en cuenta que path será diferente debido al carácter probabilístico del algoritmo PRM.

startLocation = [4.0 2.0];
endLocation = [24.0 20.0];
path = findpath(prm, startLocation, endLocation)
path = 8×2

    4.0000    2.0000
    3.1703    2.7616
    7.0797   11.2229
    8.1337   13.4835
   14.0707   17.3248
   16.8068   18.7834
   24.4564   20.6514
   24.0000   20.0000

Muestre el mapa inflado, las hojas de ruta y la ruta final.

show(prm);

Ya se ha definido un controlador de seguimiento de rutas; lo puede reutilizar para calcular los comandos de control de un robot en este mapa. Para reutilizar el controlador y redefinir los waypoints sin modificar el resto de la información, utilice la función release.

release(controller);
controller.Waypoints = path;

Establezca la ubicación inicial y el objetivo del robot según se haya definido en la ruta.

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

Asuma una orientación inicial del robot.

initialOrientation = 0;

Defina la pose actual del movimiento del robot [x y theta].

robotCurrentPose = [robotInitialLocation initialOrientation]';

Calcule la distancia a la ubicación objetivo.

distanceToGoal = norm(robotInitialLocation - robotGoal);

Defina un radio objetivo.

goalRadius = 0.1;

Dirija al robot utilizando la salida del controlador en el mapa dado hasta que alcance el objetivo. El controlador se ejecuta a 10 Hz.

reset(vizRate);

% Initialize the figure
figure

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    show(map);
    hold all

    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'Parent', gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 27])
    ylim([0 26])
    
    waitfor(vizRate);
end

Consulte también