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
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
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
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)