controllerPurePursuit

Cree un controlador para seguir un conjunto de waypoints

expandir todo en la página

Descripción

controllerPurePursuit System object™ crea un objeto de controlador para que un vehículo tipo automóvil o con tracción diferencial siga un conjunto de waypoints. El objeto calcula la velocidad lineal y la curvatura del vehículo a partir de la pose actual dada. Las subsiguientes llamadas al objeto con poses actualizadas proporcionan comandos de velocidad actualizados para el vehículo. Utilice las propiedades MaxCurvature y DesiredLinearVelocity para actualizar la curvatura y la velocidad basándose en el rendimiento del vehículo.

La propiedad LookaheadDistance calcula un punto lookahead en la trayectoria, que es un objetivo local para el vehículo. El comando de curvatura se calcula a partir de este punto. Cambiar LookaheadDistance tiene un impacto significativo en el rendimiento del algoritmo. Una mayor distancia lookahead da como resultado una trayectoria más suave, pero puede provocar que el vehículo tome un atajo durante la trayectoria. Una distancia lookahead menor puede resultar en oscilaciones en el seguimiento de la trayectoria, lo que causaría un comportamiento inestable. Para obtener más información sobre el algoritmo Pure Pursuit, consulte Controlador de Pure Pursuit.

Para calcular los comandos de control de velocidad lineal y curvatura:

  1. Cree el objeto controllerPurePursuit y configure sus propiedades.

  2. Llame al objeto con argumentos, como si fuera una función.

Para más información sobre el funcionamiento de los System objects, consulte ¿Qué son los System Objects?

Creación

Sintaxis

controller = controllerPurePursuit
controller = controllerPurePursuit(Name,Value)

Descripción

controller = controllerPurePursuit crea un objeto Pure Pursuit que utiliza el algoritmo de Pure Pursuit para calcular las entradas de velocidad lineal y curvatura para un vehículo tipo automóvil o con tracción diferencial.

ejemplo

controller = controllerPurePursuit(Name,Value) crea un objeto de Pure Pursuit con opciones adicionales especificadas por uno o más pares Name,Value. Name es el nombre de la propiedad y Value es el valor correspondiente. Name debe aparecer entre comillas simples (' '). Puede especificar varios argumentos de par nombre-valor en cualquier orden como Name1,Value1,...,NameN,ValueN. Las propiedades no especificadas conservan sus valores predeterminados.

Ejemplo: controller = controllerPurePursuit('DesiredLinearVelocity', 0.5)

Propiedades

expandir todo

Velocidad lineal constante deseada, especificada como un escalar en metros por segundo. El controlador asume que el vehículo avanza a una velocidad lineal constante y que la curvatura calculada es independiente de la velocidad lineal.

Tipos de datos: double

Distancia lookahead, especificada como un escalar en metros. La distancia lookahead cambia la respuesta del controlador. Un vehículo con una distancia lookahead mayor produce trayectorias sin obstáculos pero realiza giros más abiertos en las esquinas. Un vehículo con una distancia lookahead menor sigue fielmente la trayectoria y realiza giros bruscos, pero podría crear oscilaciones en la trayectoria.

Tipos de datos: double

Curvatura máxima, especificada como escalar real positivo en 1/metros. El controlador satura la salida de curvatura absoluta al valor especificado.

Tipos de datos: double

Waypoints, especificados como un arreglo de n por 2 de pares [x y], donde n es el número de waypoints. Puede generar los waypoints a partir de la clase mobileRobotPRM o de otro origen.

Tipos de datos: double

Uso

Sintaxis

[vel,curvature] = controller(pose)
[vel,curvature,lookaheadpoint] = controller(pose)

Descripción

[vel,curvature] = controller(pose) procesa la posición y la orientación del vehículo, pose, y genera la velocidad lineal, vel, y la curvatura, curvature.

ejemplo

[vel,curvature,lookaheadpoint] = controller(pose) devuelve el punto lookahead, que es una ubicación en la trayectoria usada para calcular los comandos de velocidad y curvatura. El objeto de controlador calcula esta ubicación en la trayectoria utilizando la propiedad LookaheadDistance.

Argumentos de entrada

expandir todo

La posición y la orientación del vehículo, especificadas como un vector de 3 por 1 en la forma [x y theta]. La pose del vehículo es una posición de x y y con una orientación angular θ (en radianes) medida a partir del eje x.

Argumentos de salida

expandir todo

Velocidad lineal, devuelta como un escalar en metros por segundo.

Tipos de datos: double

Curvatura, devuelta como escalar real positivo en 1/metros. Puede calcular la velocidad angular multiplicando la curvatura por la velocidad lineal.

Tipos de datos: double

Punto lookahead en la trayectoria, devuelto como un vector [x y]. Este valor se calcula a partir de la propiedad LookaheadDistance.

Funciones del objeto

Para usar una función de objeto, especifique el System object como el primer argumento de entrada. Por ejemplo, para liberar recursos de sistema de un System object llamado obj, utilice la siguiente sintaxis:

release(obj)

expandir todo

infoCharacteristic information about controllerPurePursuit object
stepEjecutar el algoritmo System object
releaseRelease resources and allow changes to System object property values and input characteristics
resetReset internal states of System object

Ejemplos

contraer todo

Abrir script en vivo

Utilice el método info para obtener más información sobre un objeto controllerPurePursuit. La función info devuelve dos campos, RobotPose y LookaheadPoint, que se corresponden con la posición y la orientación actuales del robot y el punto en la trayectoria utilizado para calcular las salidas desde la última llamada del objeto.

Cree un objeto controllerPurePursuit.

pp = controllerPurePursuit;

Asigne waypoints.

pp.Waypoints = [0 0;1 1];

Calcule comandos de control utilizando el objeto pp con la pose inicial [x y theta] dada como entrada.

[v,k] = pp([0 0 0]);

Obtenga más información.

s = info(pp)
s = struct with fields:
         RobotPose: [0 0 0]
    LookaheadPoint: [0.7071 0.7071]
Abrir script en vivo

Guíe a un vehículo de tracción diferencial por un conjunto de waypoints utilizando un controlador Pure Pursuit.

Para configurar las restricciones físicas del vehículo para la simulación, defina los parámetros de un vehículo de tracción diferencial. Estos parámetros determinan la velocidad con la que se mueve el vehículo y con cuánta precisión gira.

wheelRadius   = 0.05;           % [m]   Radius of each wheel
trackWidth    = 0.2;            % [m]   Distance between left and right wheels
maxWheelSpeed = 100*2*pi/60;    % [rad/s] Maximum wheel speed (100 RPM converted to rad/s)

Para simular el movimiento del vehículo, cree un modelo de cinemática de tracción diferencial y especifique los parámetros relevantes. Para un robot de tracción diferencial, establezca el rango de velocidad de las ruedas, el ancho de vía y el radio de las ruedas.

vehicle = differentialDriveKinematics(VehicleInputs="VehicleSpeedHeadingRate");
vehicle.WheelSpeedRange = [-1 1]*maxWheelSpeed; 
vehicle.TrackWidth = trackWidth;
vehicle.WheelRadius = wheelRadius;

Para crear la trayectoria del vehículo, especifique un conjunto de waypoints como arreglo de coordenadas [x y]. Elija la velocidad lineal deseada para establecer la velocidad con la que se desplaza el vehículo entre los waypoints.

waypoints = [0 0;     % Start position [x y] in meters
              1 0;
              1 1.5;
              4 1.5;
              4 0;
              5 0];   % End position

desiredSpeed = 0.2;   % [m/s]

Para permitir que el vehículo siga los waypoints, configure el controlador Pure Pursuit. Especifique los waypoints, la velocidad deseada, la curvatura máxima y la distancia lookahead. Una distancia lookahead menor permite al vehículo seguir la trayectoria de forma más precisa, pero puede provocar oscilaciones. Una distancia lookahead mayor genera trayectorias más suaves, pero puede producir curvaturas más grandes cerca de las esquinas.

controller = controllerPurePursuit;
controller.Waypoints = waypoints;                                              % Path waypoints to follow
controller.DesiredLinearVelocity = desiredSpeed;                               % Target speed [m/s]

% Calculate maximum angular velocity based on vehicle physical limits.
% For a differential drive vehicle, maximum angular velocity occurs when
% the wheels rotate in opposite directions at maximum speed.
maxAngularVelocity = wheelRadius*2*maxWheelSpeed/trackWidth;                   % rad/s

% Set maximum curvature the controller can command 
% (curvature = angular velocity / linear velocity). 
controller.MaxCurvature = maxAngularVelocity/desiredSpeed;                     % [1/m] 

% Set lookahead distance based on desired lookahead time.
lookaheadTime = 2;                                                             % [seconds]
controller.LookaheadDistance = lookaheadTime*controller.DesiredLinearVelocity; % [meters]

Para preparar el entorno de simulación, establezca los parámetros de simulación, inicialice la pose del vehículo y preasigne arreglos para almacenar la pose del vehículo y los comandos de velocidad.

% Define simulation parameters
sampleTime = 0.1;                        % Time step for simulation [seconds]
maxSimTime = 50;                         % Maximum simulation time [seconds]
simSteps = ceil(maxSimTime/sampleTime);  % Total number of simulation steps
timeVec = (0:simSteps-1)*sampleTime;


% Initialize poses
initialPose = [0 0 0];                   % Initial pose: [x y theta]
currentPose = initialPose;               % Set current pose to initial pose
poses = zeros(simSteps, 3);              % Preallocate array to record robot trajectory

% Initialize arrays to record velocity commands
vCmds = zeros(simSteps,1);               % Linear velocity commands
wCmds = zeros(simSteps,1);               % Angular velocity commands

% Reset the pure pursuit controller to clear any previous states from
% previous runs
reset(controller)

Cree gráficas para visualizar la trayectoria y los comandos de velocidad del vehículo durante la simulación.

figure(Name="Pure Pursuit Waypoint Following With Velocity Commands");

% Subplots 1 and 3: Plot XY trajectory (pose)
subplot(2,2,[1 3])
hold on
plot(waypoints(:,1),waypoints(:,2),"k--o",LineWidth=1.5);               % Waypoints
vehicleMarker = plannerLineSpec.state(MarkerSize=5);                    % Vehicle marker
hVehicle = plot(initialPose(1),initialPose(2),vehicleMarker{:});       
hTraj = plot(initialPose(1),initialPose(2),plannerLineSpec.path{:});    % Trajectory
xlabel("X [m]") 
ylabel("Y [m]")
title('XY Pose (Trajectory)')
axis equal 
grid on

% Subplot 2: Linear velocity command
subplot(2,2,2)
hV = plot(0,0,"r"',LineWidth=1.5);
xlabel("Time [s]")
ylabel("Linear Velocity [m/s]")
title("Linear Velocity Command")
grid on
xlim([0 maxSimTime])
ylim([desiredSpeed-0.1 desiredSpeed+0.1])

% Subplot 4: Angular velocity command
subplot(2,2,4);
hW = plot(0,0,"g",LineWidth=1.5);
xlabel("Time [s]")
ylabel("Angular Velocity [rad/s]")
title("Angular Velocity Command")
grid on
xlim([0 maxSimTime])

Para simular que el vehículo siga los waypoints, ejecute una simulación de lazo cerrado. En cada unidad de tiempo, calcule los comandos de control, actualice la pose del vehículo y actualice la gráfica.

for idx = 1:simSteps
    % Get the linear velocity (vCmd) and curvature (kappaCmd) commands for the current pose.
    [vCmd,kappaCmd] = controller(currentPose);

    % Use the curvature command and velocity command to calculate the angular velocity command.
    wCmd = kappaCmd*vCmd;

    % Record the linear and angular velocity commands for later analysis or plotting.
    vCmds(idx) = vCmd;
    wCmds(idx) = wCmd;

    % Compute the velocity vector of the vehicle using the differential drive kinematic model.
    vel = derivative(vehicle,currentPose,[vCmd wCmd]);

    % Update the vehicle pose using Euler integration.
    currentPose = currentPose + vel'*sampleTime;
    poses(idx,:) = currentPose;

    % Update XY pose (merged plot).
    subplot(2,2,[1 3])
    set(hVehicle,XData=currentPose(1),YData=currentPose(2))
    set(hTraj,XData=poses(1:idx,1),YData=poses(1:idx,2))

    % Update linear velocity plot.
    subplot(2,2,2)
    set(hV,XData=timeVec(1:idx),YData=vCmds(1:idx))

    % Update angular velocity plot.
    subplot(2,2,4)
    set(hW,XData=timeVec(1:idx),YData=wCmds(1:idx))
    drawnow limitrate  % Update the animation

    % Stop the simulation if the vehicle is close to the last waypoint within a tolerance of 0.1 m.
    if norm(currentPose(1:2)-waypoints(end,:)) < 0.1
        break
    end
end

% Add legend to the  XY plot
subplot(2,2,[1 3])
legend("Waypoints","Vehicle Position","Trajectory")
hold off

Figure Pure Pursuit Waypoint Following With Velocity Commands contains 3 axes objects. Axes object 1 with title XY Pose (Trajectory), xlabel X [m], ylabel Y [m] contains 3 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, Vehicle Position, Trajectory. Axes object 2 with title Linear Velocity Command, xlabel Time [s], ylabel Linear Velocity [m/s] contains an object of type line. Axes object 3 with title Angular Velocity Command, xlabel Time [s], ylabel Angular Velocity [rad/s] contains an object of type line.

Capacidades ampliadas

expandir todo

Historial de versiones

Introducido en R2019b

expandir todo

Consulte también

| (Navigation Toolbox) |

Temas