Esta página aún no se ha traducido para esta versión. Puede ver la versión más reciente de esta página en inglés.

Seguimiento de ruta para un robot de accionamiento diferencial

En este ejemplo se muestra cómo controlar un robot para seguir la ruta deseada mediante un simulador de robot. En el ejemplo se utiliza el controlador de seguimiento de ruta de persecución pura para conducir un robot simulado a lo largo de una ruta predeterminada. Una ruta deseada es un conjunto de waypoints definidos explícitamente o calculados mediante un planificador de rutas (consulte ).Planificación de rutas en entornos de diferente complejidad Se crea el controlador de ruta de acceso Pure Pursuit siguiente para un robot de unidad diferencial simulado y calcula los comandos de control para seguir una ruta determinada. Los comandos de control calculados se utilizan para conducir el robot simulado a lo largo de la trayectoria deseada para seguir la ruta deseada basada en el controlador Pure Pursuit.

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

Definir waypoints

Defina un conjunto de waypoints para el camino deseado para 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 del objetivo del robot según lo definido por la ruta.

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

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

initialOrientation = 0;

Definir la pose actual para el robot [x y theta]

robotCurrentPose = [robotInitialLocation initialOrientation]';

Crear un modelo de robot cinemático

Inicializar el modelo de robot y asignar una pose inicial. El robot simulado tiene ecuaciones cinemáticas para el movimiento de un robot de accionamiento diferencial de dos ruedas. Las entradas de este robot simulado son velocidades lineales y angulares.

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

Visualizar la ruta deseada

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

Definir el controlador de seguimiento de ruta

Basado en la ruta definida anteriormente y un modelo de movimiento robot, necesita un controlador de seguimiento de ruta para conducir el robot a lo largo de la ruta. Cree la ruta de acceso siguiente controlador mediante el objeto.controllerPurePursuit

controller = controllerPurePursuit;

Utilice la ruta definida anteriormente para establecer los waypoints deseados para el controlador

controller.Waypoints = path;

Establezca la ruta de acceso siguiendo los parámetros del controlador. La velocidad lineal deseada se establece en 0,3 metros/segundo para este ejemplo.

controller.DesiredLinearVelocity = 0.6;

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

controller.MaxAngularVelocity = 2;

Como regla general, la distancia de búsqueda anticipada debe ser mayor que la velocidad lineal deseada para una trayectoria suave. El robot podría cortar las esquinas cuando la distancia de búsqueda anticipada es grande. Por el contrario, una pequeña distancia de búsqueda anticipada puede dar lugar a un comportamiento de seguimiento de ruta inestable. Se eligió un valor de 0,5 m para este ejemplo.

controller.LookaheadDistance = 0.3;

Usando el controlador Path Following, conduzca el robot sobre los waypoints deseados

El controlador de seguimiento de ruta proporciona señales de control de entrada para el robot, que el robot utiliza para conducirse a sí mismo a lo largo de la ruta deseada.

Defina un radio de objetivo, que es el umbral de distancia deseado entre la ubicación final del robot y la ubicación del objetivo. Una vez que el robot está a esta distancia de la meta, se detendrá. Además, calcula la distancia actual entre la ubicación del robot y la ubicación del objetivo. Esta distancia se comprueba continuamente contra el radio objetivo y el robot se detiene cuando esta distancia es menor que el radio del objetivo.

Tenga en cuenta que el valor demasiado pequeño del radio de la meta puede hacer que el robot se pierda la meta, lo que puede resultar en un comportamiento inesperado cerca de la meta.

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

El objeto calcula los comandos de control para el robot.controllerPurePursuit Conduce al robot usando estos comandos de control hasta que alcance dentro del radio objetivo. Si está utilizando un simulador externo o un robot físico, entonces las salidas del controlador deben aplicarse al robot y un sistema de localización puede ser necesario para actualizar la pose del robot. El controlador funciona 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 Path Following junto con PRM

Si un planificador de rutas calcula el conjunto de waypoints deseado, la ruta de acceso siguiente controlador se puede utilizar de la misma manera. Primero, visualice el mapa

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

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

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

Busque una ruta entre la ubicación inicial y final. Tenga en cuenta que el será diferente debido a la naturaleza probabilística del algoritmo PRM.path

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, los mapas de carreteras y la ruta final.

show(prm);

Definió una ruta de acceso siguiendo el controlador por encima de la cual puede volver a utilizar para calcular los comandos de control de un robot en este mapa. Para reutilizar el controlador y redefinir los waypoints manteniendo la otra información igual, utilice la función.release

release(controller); controller.Waypoints = path;

Establecer la ubicación inicial y el objetivo del robot según lo definido por la ruta

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

Asumir una orientación inicial del robot

initialOrientation = 0;

Definir la pose actual para el movimiento del robot [x y theta]

robotCurrentPose = [robotInitialLocation initialOrientation]';

Calcular la distancia a la ubicación del objetivo

distanceToGoal = norm(robotInitialLocation - robotGoal);

Definir un radio de meta

goalRadius = 0.1;

Conduzca el robot usando la salida del controlador en el mapa dado hasta que alcance la meta. El controlador funciona 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

Ver también