Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Planificación de la ruta en un escenario de almacén con evasión de obstáculos ocultos

Desde R2024b

Este ejemplo utiliza:

Este ejemplo muestra cómo construir un robot que sigue una ruta utilizando el algoritmo de banda elástica temporizada (TEB) para la navegación en un escenario de almacén con obstáculos invisibles. Este ejemplo utiliza datos del sensor LiDAR para detectar y evitar colisiones con obstáculos invisibles que no están representados en el mapa del escenario. El algoritmo TEB genera trayectorias suaves y óptimas en el tiempo que siguen las restricciones cinemáticas del robot.

En este ejemplo, usted

Cree un mapa de escenario de almacén con un área de almacenamiento y un área de embalaje utilizando la función auxiliar exampleHelperCreateScenario. Además, agregará paletas móviles al escenario para transportar mercancías desde el área de almacenamiento al área de embalaje.
Añade un robot móvil con sensor LiDAR al escenario.
Defina puntos de referencia y acciones para el robot móvil y genere una ruta de referencia para el robot utilizando el algoritmo de estrella híbrido A*.
Cree un controlador TEB utilizando la función controllerTEB para permitir que el robot se mueva desde una ubicación de inicio a una ubicación de destino siguiendo la ruta de referencia.
Utilice datos del sensor LiDAR para detectar obstáculos invisibles a lo largo de la ruta de referencia. En consecuencia, actualice los parámetros del controlador para ajustar la ruta local del robot con el fin de evitar la colisión con los obstáculos detectados.

El resto del ejemplo explica los pasos involucrados.

Crear un escenario de almacén

Crear un escenario de simulación de robot vacío. Especifique la tasa de actualización de la simulación y el número máximo de cuadros de poses de plataforma para almacenar en el escenario.

updateRate = 10;
scenario = robotScenario(UpdateRate=updateRate,HistoryBufferSize=320);

Agregue área de almacenamiento, área de embalaje y agregue paletas y cajas móviles al escenario de simulación de robot mediante la función auxiliar exampleHelperCreateScenario. El ayudante agrega las paletas móviles con cajas como objetos robotPlatform. Este ejemplo genera una malla corporal para las paletas móviles modificando un modelo de paleta descargado de aws-robomaker-small-warehouse-world. El modelo de paleta se almacena como archivo .mat.

scenario = exampleHelperCreateScenario(scenario)

scenario = 
  robotScenario with properties:

           UpdateRate: 10
             StopTime: Inf
    HistoryBufferSize: 320
    ReferenceLocation: [0 0 0]
         MaxNumFrames: 50
          CurrentTime: 0
            IsRunning: 0
        TransformTree: [1×1 transformTree]
       InertialFrames: ["ENU"    "NED"]
               Meshes: {[1×1 extendedObjectMesh]  [1×1 extendedObjectMesh]}
      CollisionMeshes: {1×0 cell}
            Platforms: [1×17 robotPlatform]

Visualice el escenario utilizando la función show3D. Marcar el área de almacenamiento y embalaje para fines de exhibición. Además, puede ajustar los ángulos de visión para generar nuevas vistas.

figure
show3D(scenario);

rectangle(Position=[3.7 -9.5 10.5 11])
t = text(5.5,-7,0.3,"Packaging Area");
t.Rotation = 7;
rectangle(Position=[-3 6 16 13.5])
t = text(3.5,7,0.3,"Storage Area");
t.Rotation = 7;

view(-10,30)
axis([-5 16,-10 20,0 8])
light

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 27 objects of type patch, rectangle, text. These objects represent link, link_mesh.

Agregar robot móvil al escenario

Cargue el modelo de árbol de cuerpo rígido para un robot móvil Jackal de Clearpath. El robot Jackal tiene una base móvil con accionamiento diferencial y la dimensión externa del robot es 508 mm $\times$ 430 mm $\times$ 250 mm.

[jackalRBT,jackalRBTInfo] = loadrobot("clearpathJackal");

Lea el modelo de movimiento cinemático del robot. El robot Jackal sigue un modelo de movimiento cinemático con accionamiento diferencial.

robotKinematics = copy(jackalRBTInfo.MobileBaseMotionModel)

robotKinematics = 
  differentialDriveKinematics with properties:

      VehicleInputs: 'WheelSpeeds'
         TrackWidth: 0.3765
        WheelRadius: 0.0650
    WheelSpeedRange: [-61.5385 61.5385]

Este ejemplo utiliza un modelo de movimiento cinemático para simular el robot. El controlador TEB calcula una trayectoria óptima para el robot utilizando su postura actual y sus velocidades lineales y angulares actuales. El controlador utiliza las velocidades calculadas para encontrar la siguiente pose del robot. Por lo tanto, establezca la propiedad VehicleInputs del modelo de vehículo con transmisión diferencial en VehicleSpeedHeadingRate.

robotKinematics.VehicleInputs = "VehicleSpeedHeadingRate";

Establezca el rango de velocidad de las ruedas del vehículo en [-34 34];

robotKinematics.WheelSpeedRange = [-34 34];

Especifique la posición inicial del cuerpo del robot en el escenario.

robotInitLoc = [0,0,0];

Crea una plataforma robótica y agrégala al escenario. Especifique el modelo de árbol de cuerpo rígido, la posición base inicial y la orientación de la base para la plataforma del robot como entradas.

robot = robotPlatform("MobileRobot",scenario, ...
    RigidBodyTree=jackalRBT, ...
    InitialBasePosition=robotInitLoc, ...
    InitialBaseOrientation = eul2quat([0 0 0]));

Montar el sensor Lidar en el robot

Cree un modelo de sensor LiDAR y genere datos de nube de puntos organizados utilizando la función robotLidarPointCloudGenerator. Especifique estos parámetros para el modelo del sensor LiDAR .

Establezca el rango de distancia máximo para el sensor LiDAR en 12 m.
Establezca los límites del ángulo acimutal del sensor LiDAR en [-120 120]. Las unidades para los límites de acimut se expresan en grados.
Establezca la resolución de acimut en 0,2. Unidades en grados.
Establezca la tasa de actualización del sensor LiDAR en la tasa de actualización de simulación del escenario del robot.
Establezca los límites del ángulo de elevación del sensor LiDAR en [0 30]. Las unidades están en grados.
Establezca la resolución de elevación en 1. Las unidades están en grados.

Los parámetros para el modelo del sensor LiDAR se eligen experimentalmente para adaptarse al escenario del robot.

maxRange = 12;
lidarConfig = struct(angleLower=-120, ...
    angleUpper=120, ...
    angleStep=0.2, ...
    maxRange=maxRange, ...
    updateRate=updateRate);

lidarmodel = robotLidarPointCloudGenerator(...
    AzimuthResolution=lidarConfig.angleStep, ...
    AzimuthLimits=[lidarConfig.angleLower,lidarConfig.angleUpper], ...
    ElevationLimits=[0 30], ...
    ElevationResolution=1, ...
    MaxRange=lidarConfig.maxRange, ...
    UpdateRate=lidarConfig.updateRate, ...
    HasOrganizedOutput=true);

Agregue el sensor LiDAR al escenario del robot utilizando la función robotSensor.

lidar = robotSensor("Lidar",robot,lidarmodel,MountingBodyName="front_fender_link", ...
    MountingLocation=[0.25 0.1 0.2]);

Visualice el escenario con el robot.

figure
show3D(scenario);

rectangle(Position=[3.7 -9.5 10.5 11])
t = text(5.5,-7,0.3,"Packaging Area");
t.Rotation = 7;
rectangle(Position=[-3 6 16 13.5])
t = text(3.5,7,0.3,"Storage Area");
t.Rotation = 7;

view(-10,30)
axis([-5 16,-10 20,0 8])
light

Definición de puntos de referencia y acciones para el robot

Cree un mapa de ocupación binario a partir del escenario del robot.

mapOrig = binaryOccupancyMap(scenario,MapHeightLimits=[0 5], ...
    GridOriginInLocal=[-5 -10], ...
    MapSize=[20 30]);

Marque el límite del mapa como ubicaciones ocupadas para evitar que el robot se mueva fuera de la región del mapa. Utilice la función auxiliar exampleHelperDefineMapBoundary para marcar los límites del mapa como regiones ocupadas. En este ejemplo, la región del mapa dentro de 1 m de los límites del mapa se considera como límite del mapa y se marca como regiones ocupadas.

globalMap = exampleHelperDefineMapBoundary(mapOrig);

Muestra el mapa de ocupación para inspeccionar las regiones ocupadas del mapa. Se puede observar que junto con los límites, los estantes en el área de empaque también están marcados como ubicaciones ocupadas. Las ubicaciones de las paletas móviles en el mapa no están marcadas como ubicaciones ocupadas. Las paletas móviles y otros objetos en las regiones del mapa son obstáculos temporales que el robot debe detectar y evitar durante la navegación.

figure
show(globalMap)

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains an object of type image.

Especifique la posición inicial y la posición final del robot.

robotInitPose = robotInitLoc;
robotFinalPose = [6 8 pi/2];

Especifique la ubicación de recogida y entrega de un pallet que se va a mover.

palletPickLoc = [2 13 pi/2];
palletDropLoc = [6 -7 0];

El robot debe realizar un giro para poder moverse desde el lugar de recogida de pallets al lugar de entrega. Especifica la ubicación del robot después de realizar el giro. En este ejemplo, el robot gira 180 grados para realizar un giro.

robotLocAfterTurn = [2 13 -pi/2];

Especifique los puntos de referencia para que el robot recoja el palé y navegue desde su posición inicial hasta la posición final.

robotWaypoints = [robotInitPose; palletPickLoc; ... % Robot motion from start to the pallet pickup location
    robotLocAfterTurn; palletDropLoc; ... % Robot taking pallet to the drop location
    robotFinalPose]; % Robot motion to the final location

Crea un arreglo booleana para especificar en qué punto de referencia el robot debe recoger la paleta. Establezca el valor como true para el punto de referencia donde el robot debe recoger la paleta.

pickPallet = [false;false;true;false;false];

Crea un arreglo booleana para especificar en qué punto de referencia el robot debe realizar un giro. Establezca el valor como true para el punto de referencia donde el robot debe realizar un giro de 180 grados.

rotateRobotInPlace = [false;true;false;false;false];

Encuentre la ruta de referencia usando el Planificador global

Establezca el valor inicial para asegurarse de generar los mismos resultados.

rng("default")

Valide los estados en el mapa de entrada y calcule una ruta de referencia entre dos puntos de referencia consecutivos utilizando el algoritmo de planificación híbrido A*.

validator = validatorOccupancyMap(stateSpaceSE2,Map=globalMap);
validator.ValidationDistance = 0.1;
planner = plannerHybridAStar(validator);

numMotionSeq = size(robotWaypoints,1)-1;
refPath = cell(1,numMotionSeq);

for i=1:numMotionSeq
    start = robotWaypoints(i,:);
    goal = robotWaypoints(i+1,:);
    if rotateRobotInPlace(i)
        refPath{i} = [start;start;goal];
    else
        path = plan(planner,start,goal);
        refPath{i} = path.States;
    end
end

Mostrar el escenario. Grafique las rutas de referencia calculadas para cada par de inicio y objetivo.

figure
show3D(scenario);
view(-10,30)
axis([-5 16,-10 20,0 8])
light
hold on
for i=1:numMotionSeq
    plot(refPath{i}(:,1),refPath{i}(:,2),'*')
end
hold off

Crear un controlador TEB e inicializar los parámetros del controlador

Cree un mapa de ocupación para definir el campo de visión local del robot. El ancho y la altura del campo local se establecen en 15 m.

localMap = occupancyMap(15,15,globalMap.Resolution);

Especifique el primer par de inicio y objetivo entre los cuales debe navegar el robot. Lea la ruta de referencia calculada para el par de inicio y objetivo especificados.

waypointIdx = 1;
referencePath = refPath{waypointIdx};

Cree un objeto controlador TEB para calcular los comandos de velocidad lineal y angular para que el robot siga la ruta de referencia especificada.

teb = controllerTEB(referencePath,localMap);

Especifique estos parámetros del controlador.

Límite de velocidad lineal y límite de velocidad angular para el robot móvil.
Mire hacia el futuro. Seleccione este valor según el rango máximo del sensor y la frecuencia de ejecución del controlador. El controlador genera comandos de velocidad y optimiza la trayectoria hasta que el controlador alcanza el tiempo de anticipación. Un tiempo de anticipación más alto genera comandos de velocidad más en el futuro. Esto permite que el robot reaccione antes ante obstáculos invisibles, pero aumenta el tiempo de ejecución del controlador. Por el contrario, un tiempo de anticipación más corto reduce el tiempo disponible para reaccionar ante obstáculos nuevos y desconocidos, pero permite que el controlador funcione a un ritmo más rápido. Las unidades están en segundos.
Margen de seguridad de obstáculos. Las unidades se establecen en metros.
Tolerancia de objetivos.

teb.MaxVelocity = [2 1];
teb.LookAheadTime = 3;
teb.ObstacleSafetyMargin = 0.1;
teb.GoalTolerance = [0.2 0.2 0.2];

Simular la navegación del robot

Establecer el escenario del robot para la simulación. Para la inicialización, establezca la pose actual del robot en la pose inicial.

setup(scenario)
currentPose = robotInitPose;

Especifique la frecuencia de ejecución del controlador como 0,5, de modo que el planificador se ejecute cada 0.5 segundos del tiempo de simulación.

controllerExecutionFreq = 0.5;

Durante la simulación, el mapa global calculado utilizando el algoritmo híbrido A* se proporciona como ruta de referencia al controlador. Si se detectan obstáculos temporales a lo largo de la trayectoria del robot, sus posiciones se marcan como ubicaciones ocupadas en el mapa global. Esto permitirá que el controlador calcule una ruta alternativa alrededor de los obstáculos. En consecuencia, el mapa global se actualiza y se sincroniza con el mapa local.

Especifique la dimensión del robot y el pallet en metros. Las dimensiones del palet están ligeramente infladas para evitar que se acerque demasiado a los obstáculos.

palletDimension = [0.88 0.8];
robotSize = [.508 .43];

Muestra el escenario con el cuerpo del robot colocado en la ubicación [0, 0, 0]. Utilice la lista desplegable para seleccionar un ángulo de visión de la cámara. La función auxiliar exampleHelperSetScenarioView muestra el escenario del robot según la vista de cámara elegida. Para una visualización clara de la evasión de colisiones durante la navegación del robot, puede seleccionar la vista superior.

figure
[ax,plotFrames] = show3D(scenario);
rectangle(Position=[3.7 -9.5 10.5 11])
t = text(5.5,-7,0.3,"Packaging Area");
t.Rotation = 7;
rectangle(Position=[-3 6 16 13.5])
t = text(3.5,7,0.3,"Storage Area");
t.Rotation = 7;
view(-10,30)
axis([-5 16,-10 20,0 8])
light
cameraView = "Default View";
exampleHelperSetScenarioView(ax,cameraView);

Simule la navegación del robot siguiendo estos pasos:

Paso 1: Dado un punto de referencia, verifique si el robot debe recoger el palé.

En caso afirmativo, establezca la propiedad RobotInformation del objeto controllerTEB en la dimensión de la paleta. Además, establezca la propiedad ObstacleSafetyMargin del objeto controllerTEB en 0.4. Un margen de seguridad mayor ayuda a evitar colisiones al mantener una zona de amortiguación mayor alrededor del robot.
Si no, establezca la propiedad RobotInformation del objeto controllerTEB en la dimensión del robot. Además, establezca la propiedad ObstacleSafetyMargin del objeto controllerTEB en 0,1. Un margen de seguridad más pequeño permite que el robot navegue más cerca de los obstáculos, optimizando la planificación de la ruta y la eficiencia del movimiento.

Paso 2: Inicializar los parámetros del controlador TEB. Establezca los comandos de velocidad, la marca de tiempo y la velocidad actual en cero. Especifique la ruta de referencia para el punto de referencia dado como entrada al controlador TEB.

Paso 3: Actualice la vista local del controlador a medida que el robot se mueve a lo largo de la ruta de referencia. Utilice la pose actual del robot y los límites del mapa local para cambiar la vista local.

Paso 4: Lea los datos del sensor LiDAR y detecte cualquier obstáculo desconocido a lo largo de la ruta de referencia utilizando la función auxiliar exampleHelperUpdateMapWithLidarReading. Marque la ubicación de los obstáculos detectados como ubicaciones ocupadas y actualice el mapa local con los nuevos datos.

Paso 5: Verifique si el robot puede continuar moviéndose hacia su próximo objetivo sin realizar un giro y si el tiempo de simulación es un múltiplo de la frecuencia de ejecución del controlador. En caso afirmativo, calcule los comandos de velocidad y la trayectoria óptima para los pasos de tiempo subsiguientes utilizando el controlador TEB.

Paso 6: Dado el punto de referencia, verifique si el robot está llevando el palé y ha alcanzado la meta. En caso afirmativo, separe el robot del palé. Comprueba si la trayectoria del robot es válida. Si no es válido, utilice la función auxiliar exampleHelperTEBRecovery para generar una ruta y comandos válidos. Si el robot no transporta el pallet, vaya al paso 7.

Paso 7: Comprueba si el robot debe girar 180 grados en el punto de referencia actual. Si es así, utilice la función auxiliar exampleHelperRotateRobot para calcular los comandos necesarios para lograr la orientación deseada antes de proceder al siguiente punto de referencia. Si el robot no tiene que realizar un giro, vaya al paso 8.

Paso 8: Calcule la próxima pose y velocidad del robot utilizando la función auxiliar exampleHelperGetRobotCurrentPose. La función auxiliar utiliza la función derivative (Robotics System Toolbox) para calcular la siguiente pose y velocidad a partir de la velocidad instantánea y la cinemática del robot.

Paso 9: Mueva la plataforma del robot en el escenario a la siguiente pose. Actualiza el valor de la pose actual a la siguiente pose.

En cada paso de tiempo durante la simulación, verifique si el robot ha alcanzado una ubicación objetivo.

Si el robot ha llegado a la ubicación de destino y lleva el palé, separe el palé del robot y llévelo al siguiente punto de referencia siguiendo los pasos 1 a 9.
Si el robot ha llegado a la ubicación de destino pero no lleva el palé, llévelo al siguiente punto de referencia siguiendo los pasos 1 a 9.
Si el robot no ha llegado a la ubicación objetivo, continúe navegando a lo largo de la ruta de referencia utilizando los pasos 3 a 9.

Continúe ejecutando la simulación hasta que el robot haya cubierto todos los puntos de referencia.

continueSimulation = true;
while continueSimulation
    %---- STEP 1 ----%
    if pickPallet(waypointIdx)
        % Modify the dimension of the robot to include the pallet dimension
        robotInfo = struct(Dimension=palletDimension,Shape="Rectangle");
        attach(robot,"rack2b","mid_mount","ChildToParentTransform",eul2tform([pi/2 0 0]));
        teb.ObstacleSafetyMargin = 0.4;
    else
        robotInfo = struct(Dimension=robotSize,Shape="Rectangle");
        teb.ObstacleSafetyMargin = 0.1;
    end
    %---Add robot information to TEB controller---%
    teb.RobotInformation = robotInfo;

    %---- STEP 2 ----%
    %---Initialize TEB controller parameters---%
    velcmds = zeros(1,2); % velocity commands [linear velocity, angular velocity]
    tstamps = 0;
    validTimeStamp = 0;
    currentVel = [0 0];
    %---Add reference path to TEB controller---%
    referencePath = refPath{waypointIdx};
    teb.ReferencePath = referencePath;

    %---Navigate robot along the reference path---%
    robotTaskInProgress = true;
    while robotTaskInProgress
        %---- STEP 3 ----%
        %---Shift local map view as the robot moves---%
        moveMapBy = currentPose(1:2)-localMap.XLocalLimits(end)/2;
        move(localMap,moveMapBy);

        %---Update local map with data from global map---%
        syncWith(localMap,globalMap);

        %---Read data from lidar sensor---%
        [isUpdated,~,pointCloudA] = read(lidar);
        %---- STEP 4 ----%
        %--Update the local map based on lidar sensor readings---%
        robotDimension = teb.RobotInformation.Dimension;
        exampleHelperUpdateMapWithLidarReading(lidarConfig,pointCloudA,localMap,currentPose,robotDimension,palletDimension);

        %---- STEP 5 ----%
        runPlanner = ~rotateRobotInPlace(waypointIdx)&&(mod(scenario.CurrentTime,controllerExecutionFreq) == 0);
        if runPlanner
            [velcmds,tstamps,currentPath,info] = step(teb,currentPose,currentVel);
            robotTaskInProgress = ~info.HasReachedGoal;

            if ~robotTaskInProgress && pickPallet(waypointIdx)
                detach(robot); % Detach the pallet when the robot has reached the goal
            end
            %---- STEP 6 ----%
            % If timestamps are inconsistent or there is a collision along the trajectory
            % and if only a small part of it is valid, then find a new reference path.
            if any(diff(tstamps) < 0) || info.ExitFlag == 1
                % Do recovery when output from controllerTEB is invalid.
                [velcmds,tstamps,currentPath] = exampleHelperTEBRecovery(teb,velcmds,tstamps,currentPath,info,planner);
            end
            if ~isempty(velcmds)
                validTimeStamp = tstamps + scenario.CurrentTime;
            end
            [hndl1,hndl2] = exampleHelperPlotPath(ax,teb.ReferencePath,currentPath);
        end
         %---- STEP 7 ----%
        % Check if the robot needs to be rotated by 180 degrees at the same
        % position
        if rotateRobotInPlace(waypointIdx)
            goalPose = robotWaypoints(waypointIdx+1,:);
            [velcmds,tstamps,isRotated] = exampleHelperRotateRobot(currentPose,goalPose,teb.MaxVelocity);
            if isRotated
                robotTaskInProgress = false;
            end
            validTimeStamp = tstamps + scenario.CurrentTime;
        end
        %---- STEP 8 ----%
        %---Compute next pose and velocity for the robot---%
        velcmd = velocityCommand(velcmds,validTimeStamp,scenario.CurrentTime);
        timeStep = 1/updateRate;
        [currentPose,currentVel] = exampleHelperGetRobotCurrentPose(robotKinematics,currentPose,velcmd,timeStep);

        %---Display the scenario with updated robot position---%
        show3D(scenario,FastUpdate=true,Parent=ax);
        legend([hndl1,hndl2],"Reference Path","Controller Output",Location="North")
        % Refresh all plot data and visualize
        refreshdata
        drawnow limitrate
        %---- STEP 9 ----%
        %---Move robot platform in the scenario to the next pose---%
        move(robot,"base",[currentPose(1),currentPose(2),0, ...
            zeros(1,6),eul2quat([currentPose(3),0,0]), ...
            zeros(1,3)]);

        %--- Advance scenario simulation time---%
        advance(scenario);
        % Update all sensors in the scenario
        updateSensors(scenario);
    end
    if waypointIdx == numMotionSeq
        continueSimulation = false;
    end
    waypointIdx = waypointIdx + 1;
end

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 62 objects of type patch, rectangle, text, line, quiver. These objects represent Reference Path, Controller Output.

Consulte también