step
Calcular comandos de velocidad y trayectoria óptima para pasos de tiempo posteriores.
Desde R2023a
Descripción
[ calcula los comandos de velocidad lineal y angular velcmds,timestamps,optPath] = step(controller,curState,curVel)velcmds, con su correspondiente timestamps y la ruta optimizada correspondiente optPath, para la pose actual especificada curState y la velocidad actual curVel de un robot.
[___, devuelve información adicional, extraInfo] = step(___)extraInfo, para evaluar la solución, además de todos los argumentos de la sintaxis anterior.
Ejemplos
Configurar el entorno del estacionamiento
Cree un objeto occupancyMap a partir de un mapa de estacionamiento y configure la resolución del mapa en 3 celdas por metro.
load parkingMap.mat;
resolution = 3;
map = occupancyMap(map,resolution);Visualiza el mapa. El mapa contiene el plano de un aparcamiento con algunas plazas ya ocupadas.
show(map) title("Parking Lot Map") hold on
![Figure contains an axes object. The axes object with title Parking Lot Map, xlabel X [meters], ylabel Y [meters] contains an object of type image.](../../examples/nav/win64/ComputeVelocityCmdsAndOptimalTrajUsingTEBAlgorithmExample_01.png)
Configurar y ejecutar el Planificador global
Cree un validador de estado validatorOccupancyMap utilizando la definición stateSpaceSE2. Especifique el mapa y la distancia para interpolar y validar segmentos de ruta.
validator = validatorOccupancyMap(stateSpaceSE2,Map=map); validator.ValidationDistance = 0.1;
Cree un planificador de rutas RRT*. Aumente la distancia máxima de conexión.
rrtstar = plannerRRTStar(validator.StateSpace,validator); rrtstar.MaxConnectionDistance = 0.2;
Establezca los estados de inicio y objetivo.
start = [2 9 0]; goal = [27 18 -pi/2];
Planifique una ruta con la configuración predeterminada.
rng(42,"twister") % Set random number generator seed for repeatable result. route = plan(rrtstar,start,goal); refpath = route.States;
RRT* utiliza una orientación aleatoria, lo que puede provocar giros innecesarios.
headingToNextPose = headingFromXY(refpath(:,1:2));
Alinee la orientación con la ruta, excepto en los estados inicial y objetivo.
refpath(2:end-1,3) = headingToNextPose(2:end-1);
Visualiza la ruta.
plot(refpath(:,1),refpath(:,2),"r-",LineWidth=2) hold off
![Figure contains an axes object. The axes object with title Parking Lot Map, xlabel X [meters], ylabel Y [meters] contains 2 objects of type image, line.](../../examples/nav/win64/ComputeVelocityCmdsAndOptimalTrajUsingTEBAlgorithmExample_02.png)
Configurar y ejecutar el planificador local
Crea un objeto local occupancyMap con un ancho y alto de 15 metros y la misma resolución que el mapa global.
localmap = occupancyMap(15,15,map.Resolution);
Cree un objeto controllerTEB utilizando la ruta de referencia generada por el planificador global y el mapa local.
teb = controllerTEB(refpath,localmap);
Especifique las propiedades del objeto controllerTEB.
teb.LookAheadTime = 10; % sec teb.ObstacleSafetyMargin = 0.4; % meters % To generate time-optimal trajectories, specify a larger weight value, % like 100, for the cost function, Time. To follow the reference path % closely, keep the weight to a smaller value like 1e-3. teb.CostWeights.Time = 100;
Crea un clon profundo del objeto controllerTEB.
teb2 = clone(teb);
Inicializar parámetros.
curpose = refpath(1,:);
curvel = [0 0];
simtime = 0;
% Reducing timestep can lead to more accurate path tracking.
timestep = 0.1;
itr = 0;
goalReached = false;Calcular comandos de velocidad y trayectoria óptima.
while ~goalReached && simtime < 200 % Update map to keep robot in the center of the map. Also update the % map with new information from the global map or sensor measurements. moveMapBy = curpose(1:2) - localmap.XLocalLimits(end)/2; localmap.move(moveMapBy,FillValue=0.5) syncWith(localmap,map) if mod(itr,10) == 0 % every 1 sec % Generate new vel commands with teb [velcmds,tstamps,curpath,info] = step(teb,curpose,curvel); goalReached = info.HasReachedGoal; feasibleDriveDuration = tstamps(info.LastFeasibleIdx); % If robot is far from goal and only less than third of trajectory % is feasible, then an option is to re-plan the path to follow to % reach the goal. if info.ExitFlag == 1 && ... feasibleDriveDuration < (teb.LookAheadTime/3) route = plan(rrtstar,curpose,[27 18 -pi/2]); refpath = route.States; headingToNextPose = headingFromXY(refpath(:,1:2)); refpath(2:end-1,3) = headingToNextPose(2:end-1); teb.ReferencePath = refpath; end timestamps = tstamps + simtime; % Show the updated information input to or output % from controllerTEB clf show(localmap) hold on plot(refpath(:,1),refpath(:,2),".-",Color="#EDB120", ... DisplayName="Reference Path") quiver(curpath(:,1),curpath(:,2), ... cos(curpath(:,3)),sin(curpath(:,3)), ... 0.2,Color="#A2142F",DisplayName="Current Path") quiver(curpose(:,1),curpose(:,2), ... cos(curpose(:,3)),sin(curpose(:,3)), ... 0.5,"o",MarkerSize=20,ShowArrowHead="off", ... Color="#0072BD",DisplayName="Start Pose") end simtime = simtime+timestep; % Compute the instantaneous velocity to be sent to the robot from the % series of timestamped commands generated by controllerTEB velcmd = velocityCommand(velcmds,timestamps,simtime); % Very basic robot model, should be replaced by simulator. statedot = [velcmd(1)*cos(curpose(3)) ... velcmd(1)*sin(curpose(3)) ... velcmd(2)]; curpose = curpose + statedot*timestep; if exist("hndl","var") delete(hndl) end hndl = quiver(curpose(:,1),curpose(:,2), ... cos(curpose(:,3)),sin(curpose(:,3)), ... 0.5,"o",MarkerSize=20,ShowArrowHead="off", ... Color="#D95319",DisplayName="Current Robot Pose"); itr = itr + 1; drawnow end legend
![Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 5 objects of type image, line, quiver. These objects represent Reference Path, Current Path, Start Pose, Current Robot Pose.](../../examples/nav/win64/ComputeVelocityCmdsAndOptimalTrajUsingTEBAlgorithmExample_03.png)
Argumentos de entrada
Controlador TEB, especificado como un objeto controllerTEB.
Pose actual del robot, especificada como un vector de tres elementos con la forma [x y theta]. x y y especifican la posición del robot en metros. theta especifica la orientación del robot en radianes.
Tipos de datos: single | double
Velocidad actual del robot, especificada como un vector de dos elementos con la forma [v w]. v especifica la velocidad lineal del robot en metros por segundo. w especifica la velocidad angular del robot en radianes por segundo.
Tipos de datos: single | double
Argumentos de salida
Comandos de velocidad, devueltos como una matriz N por 2. La primera columna es la velocidad lineal en metros por segundo y la segunda columna es la velocidad angular en radianes por segundo.
Tipos de datos: double
Marcas de tiempo correspondientes a los comandos de velocidad, devueltos como un vector columna de elemento N.
Tipos de datos: double
Ruta optimizada, devuelta como una matriz N por 3. Cada fila tiene el formato [x y theta], que define la posición xy y el ángulo de orientación theta en un punto de la ruta.
N se ve afectado por las propiedades ReferenceDeltaTime y LookAheadTime de controller. El algoritmo intenta mantener la diferencia entre dos timestamps consecutivos cerca de ReferenceDeltaTime. Si la brecha entre un par de marcas de tiempo consecutivas es mayor que ReferenceDeltaTime, la función agrega poses y marcas de tiempo a la ruta. Si el espacio es menor que ReferenceDeltaTime, la función elimina las poses y las marcas de tiempo de la ruta. Además, el algoritmo intenta mantener el valor final de timestamps cerca de LookAheadTime, por lo que al aumentar LookAheadTime también aumenta N.
Tipos de datos: double
Información adicional, devuelta como una estructura. Los campos de la estructura son:
| Campo | Descripción |
|---|---|
LastFeasibleIdx | Índice que especifica un elemento en la ruta optimizada y las salidas de marca de tiempo hasta el cual la trayectoria es factible. Más allá de este índice, el valor de |
DistanceFromStartPose | Distancia de cada pose en |
HasReachedGoal | Indica si el robot ha alcanzado con éxito la última pose en ReferencePath dentro de una tolerancia y retorna como |
TrajectoryCost | Coste de la trayectoria optimizada para funciones de coste en el algoritmo Timed Elastic Band. |
ExitFlag | Valor escalar que indica la condición de salida de la función
|
Tipos de datos: struct
Historial de versiones
Introducido en R2023aEl argumento de salida extraInfo ahora devuelve un campo adicional ExitFlag que especifica más información sobre los comandos de velocidad, las marcas de tiempo y la ruta devueltos por la función step.
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Seleccione un país/idioma
Seleccione un país/idioma para obtener contenido traducido, si está disponible, y ver eventos y ofertas de productos y servicios locales. Según su ubicación geográfica, recomendamos que seleccione: .
También puede seleccionar uno de estos países/idiomas:
Cómo obtener el mejor rendimiento
Seleccione China (en idioma chino o inglés) para obtener el mejor rendimiento. Los sitios web de otros países no están optimizados para ser accedidos desde su ubicación geográfica.
América
- América Latina (Español)
- Canada (English)
- United States (English)
Europa
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)