clone
Description
crée un clone profond de l'objet controller2
= clone(controller1
)controllerTEB
controller1
.
Exemples
Calculer les commandes de vitesse et la trajectoire optimale pour un robot à entraînement différentiel à l'aide d'un algorithme de bande élastique chronométré
Mettre en place un environnement de stationnement
Créez un objet occupancyMap
à partir d'une carte de parking et définissez la résolution de la carte sur 3 cellules par mètre.
load parkingMap.mat;
resolution = 3;
map = occupancyMap(map,resolution);
Visualisez la carte. La carte contient le plan d'étage d'un parking avec quelques emplacements de stationnement déjà occupés.
show(map) title("Parking Lot Map") hold on
Configurer et exécuter le planificateur global
Créez un validateur d'état validatorOccupancyMap
à l'aide de la définition stateSpaceSE2
. Spécifiez la carte et la distance pour interpoler et valider les segments de chemin.
validator = validatorOccupancyMap(stateSpaceSE2,Map=map); validator.ValidationDistance = 0.1;
Créez un planificateur de trajet RRT*. Augmentez la distance de connexion maximale.
rrtstar = plannerRRTStar(validator.StateSpace,validator); rrtstar.MaxConnectionDistance = 0.2;
Définissez les états de départ et d’objectif.
start = [2 9 0]; goal = [27 18 -pi/2];
Planifiez un chemin avec les paramètres par défaut.
rng(42,"twister") % Set random number generator seed for repeatable result. route = plan(rrtstar,start,goal); refpath = route.States;
RRT* utilise une orientation aléatoire, ce qui peut provoquer des virages inutiles.
headingToNextPose = headingFromXY(refpath(:,1:2));
Alignez l'orientation sur le chemin, sauf pour les états de départ et d'objectif.
refpath(2:end-1,3) = headingToNextPose(2:end-1);
Visualisez le chemin.
plot(refpath(:,1),refpath(:,2),"r-",LineWidth=2) hold off
Configurer et exécuter le planificateur local
Créez un objet local occupancyMap
d'une largeur et d'une hauteur de 15 mètres et de la même résolution que la carte globale.
localmap = occupancyMap(15,15,map.Resolution);
Créez un objet controllerTEB
en utilisant le chemin de référence généré par le planificateur global et la carte locale.
teb = controllerTEB(refpath,localmap);
Spécifiez les propriétés de l'objet 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;
Créez un clone profond de l'objet controllerTEB
.
teb2 = clone(teb);
Initialisez les paramètres.
curpose = refpath(1,:);
curvel = [0 0];
simtime = 0;
% Reducing timestep can lead to more accurate path tracking.
timestep = 0.1;
itr = 0;
goalReached = false;
Calculez les commandes de vitesse et la trajectoire optimale.
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
Arguments d'entrée
controller1
— Contrôleur TEB
Objet controllerTEB
Contrôleur TEB, spécifié comme objet controllerTEB
.
Arguments de sortie
controller2
— Clone du contrôleur TEB
Objet controllerTEB
Clone du contrôleur TEB, renvoyé sous forme d'objet controllerTEB
.
Historique des versions
Introduit dans R2023a
Commande MATLAB
Vous avez cliqué sur un lien qui correspond à cette commande MATLAB :
Pour exécuter la commande, saisissez-la dans la fenêtre de commande de MATLAB. Les navigateurs web ne supportent pas les commandes MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- 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)