La méthode de la fenêtre dynamique (Dynamic Window Aproach, DWA) constitue un algorithme efficace de planification de trajectoires locales, adapté à la navigation en temps réel des robots mobiles dans des environnements dynamiques. Elle calcule dynamiquement un espace de vitesses, évalue plusieurs trajectoires candidates et sélectionne le chemin optimal pour éviter les obstacles et atteindre la destination.

I. Principes de l'algorithme

1.1 Fondements de la DWA

1. Fenêtre dynamique : Échantillonnage des combinaisons de vitesses possibles dans l'espace (v, ω)
2. Prédiction de trajectoire : Simulation du mouvement du robot sur une courte durée
3. Évaluation de trajectoire : Comparaison des trajectoires selon trois critères
4. Sélection optimale : Exécution des commandes de vitese correspondant à la meilleure trajectoire

1.2 Modèle mathématique

• Modèle cinématique du robott :

x(t+Δt) = x(t) + v·cosθ·Δt
y(t+Δt) = y(t) + v·sinθ·Δt
θ(t+Δt) = θ(t) + ω·Δt

• Contraintes de la fenêtre dynamique :

V_r = [v_min, v_max] ∩ [v_c - a·Δt, v_c + a·Δt]
Ω_r = [ω_min, ω_max] ∩ [ω_c - α·Δt, ω_c + α·Δt]

• Fonction d'évaluation :

G(v,ω) = α·orientation(v,ω) + β·distance(v,ω) + γ·vitesse(v,ω)

II. Implémentation MATLAB

2.1 Programme principal

function planification_fenetre_dynamique()
    % Initialisation des paramètres
    config = initialiser_parametres();
    
    % Création de l'environnement
    [obstacles, but] = creer_environnement(config);
    
    % Initialisation de l'état du robot
    robot = initialiser_robot(config);
    
    % Enregistrement du chemin
    chemin = robot.position;
    
    % Boucle principale
    while norm(robot.position - but) > config.seuil_but
        % Calcul de la fenêtre dynamique
        [V, Omega] = calculer_fenetre_dynamique(robot, config);
        
        % Prédiction et évaluation des trajectoires
        meilleure_trajectoire = [];
        meilleur_score = -inf;
        
        for v = V(1):config.res_v:V(2)
            for omega = Omega(1):config.res_omega:Omega(2)
                % Prédiction de trajectoire
                traj = predire_trajectoire(robot, v, omega, config);
                
                % Évaluation de la trajectoire
                score = evaluer_trajectoire(traj, but, obstacles, config);
                
                % Sélection de la meilleure trajectoire
                if score > meilleur_score
                    meilleur_score = score;
                    meilleure_traj = traj;
                    meilleure_v = v;
                    meilleure_omega = omega;
                end
            end
        end
        
        % Exécution de la vitesse optimale
        robot = mettre_a_jour_etat(robot, meilleure_v, meilleure_omega, config);
        chemin = [chemin; robot.position];
        
        % Visualisation
        visualiser(robot, chemin, meilleure_traj, obstacles, but, config);
        pause(0.05);
    end
    
    % Affichage des résultats
    disp('Destination atteinte!');
    afficher_chemin_final(chemin, obstacles, but, config);
end

2.2 Initialisation des paramètres

function config = initialiser_parametres()
    % Paramètres du robot
    config.vitesse_max = 1.0;        % Vitesse linéaire maximale (m/s)
    config.vitesse_min = 0.0;        % Vitesse linéaire minimale (m/s)
    config.vitesse_angulaire_max = pi/2;  % Vitesse angulaire maximale (rad/s)
    config.vitesse_angulaire_min = -pi/2; % Vitesse angulaire minimale (rad/s)
    config.acceleration = 0.5;       % Accélération maximale (m/s²)
    config.acceleration_angulaire = pi/4; % Accélération angulaire maximale (rad/s²)
    config.rayon_robot = 0.2;        % Rayon du robot (m)
    config.resolution_vitesse = 0.01; % Résolution de vitesse (m/s)
    config.resolution_omega = 0.1;    % Résolution de vitesse angulaire (rad/s)
    config.pas_temps = 0.1;          % Pas de temps (s)
    config.temps_prediction = 3.0;   % Temps de prédiction (s)
    config.seuil_but = 0.3;          % Seuil d'arrivée au but (m)
    
    % Poids de la fonction d'évaluation
    config.poids_orientation = 0.7;  % Poids d'orientation
    config.poids_distance = 0.3;     % Poids de distance
    config.poids_vitesse = 0.1;      % Poids de vitesse
end

2.3 Création de l'environnement

function [obstacles, but] = creer_environnement(config)
    % Définition des obstacles (x, y, rayon)
    obstacles = [
        2.0, 2.0, 0.5;
        4.0, 5.0, 0.8;
        7.0, 3.0, 0.6;
        5.0, 8.0, 0.7;
        8.0, 7.0, 0.4;
        3.0, 7.0, 0.5
    ];
    
    % Position du but
    but = [10.0, 10.0];
end

2.4 Initialisation du robot

function robot = initialiser_robot(config)
    % Position initiale et orientation
    robot.position = [0.0, 0.0];
    robot.orientation = 0.0;  % Orientation (rad)
    robot.vitesse = 0.0;
    robot.vitesse_angulaire = 0.0;
end

2.5 Calcul de la fenêtre dynamique

function [V, Omega] = calculer_fenetre_dynamique(robot, config)
    % Contraintes de la fenêtre de vitesse
    V_min = max(config.vitesse_min, robot.vitesse - config.acceleration*config.pas_temps);
    V_max = min(config.vitesse_max, robot.vitesse + config.acceleration*config.pas_temps);
    V = [V_min, V_max];
    
    % Contraintes de la fenêtre de vitesse angulaire
    Omega_min = max(config.vitesse_angulaire_min, robot.vitesse_angulaire - config.acceleration_angulaire*config.pas_temps);
    Omega_max = min(config.vitesse_angulaire_max, robot.vitesse_angulaire + config.acceleration_angulaire*config.pas_temps);
    Omega = [Omega_min, Omega_max];
end

2.6 Prédiction de trajectoire

function trajectoire = predire_trajectoire(robot, v, omega, config)
    % Initialisation de la trajectoire
    trajectoire = zeros(floor(config.temps_prediction/config.pas_temps)+1, 2);
    x = robot.position(1);
    y = robot.position(2);
    theta = robot.orientation;
    
    % Simulation de la trajectoire
    for i = 1:size(trajectoire, 1)
        trajectoire(i, :) = [x, y];
        x = x + v * cos(theta) * config.pas_temps;
        y = y + v * sin(theta) * config.pas_temps;
        theta = theta + omega * config.pas_temps;
    end
end

2.7 Évaluation de trajectoire

function score = evaluer_trajectoire(traj, but, obstacles, config)
    % 1. Score d'orientation : alignement final avec le but
    dernier_point = traj(end, :);
    vecteur_but = but - dernier_point;
    angle_but = atan2(vecteur_but(2), vecteur_but(1));
    score_orientation = cos(angle_but);  % Plus la différence d'angle est faible, plus le score est élevé
    
    % 2. Score de distance : distance minimale aux obstacles
    dist_min = inf;
    for i = 1:size(traj, 1)
        point = traj(i, :);
        for j = 1:size(obstacles, 1)
            obs = obstacles(j, 1:2);
            rayon_obs = obstacles(j, 3);
            dist = norm(point - obs) - rayon_obs - config.rayon_robot;
            if dist < dist_min
                dist_min = dist;
            end
        end
    end
    
    % Éviter la division par zéro
    if dist_min < 0.1
        score_distance = 0;  % Risque de collision
    else
        score_distance = 1 / dist_min;  % Plus la distance est grande, plus le score est élevé
    end
    
    % 3. Score de vitesse : encouragement à avancer rapidement
    score_vitesse = norm([traj(2,1)-traj(1,1), traj(2,2)-traj(1,2)]) / config.pas_temps;
    
    % Score combiné
    score = config.poids_orientation * score_orientation + ...
            config.poids_distance * score_distance + ...
            config.poids_vitesse * score_vitesse;
end

2.8 Mise à jour de l'état du robot

function robot = mettre_a_jour_etat(robot, v, omega, config)
    % Mise à jour de la position et de l'orientation
    robot.position(1) = robot.position(1) + v * cos(robot.orientation) * config.pas_temps;
    robot.position(2) = robot.position(2) + v * sin(robot.orientation) * config.pas_temps;
    robot.orientation = robot.orientation + omega * config.pas_temps;
    
    % Mise à jour des vitesses
    robot.vitesse = v;
    robot.vitesse_angulaire = omega;
end

2.9 Fonction de visualisation

function visualiser(robot, chemin, traj, obstacles, but, config)
    clf;
    hold on;
    axis equal;
    grid on;
    xlim([-1, 12]);
    ylim([-1, 12]);
    title('Planification de trajectoire avec fenêtre dynamique');
    xlabel('X (m)');
    ylabel('Y (m)');
    
    % Dessin des obstacles
    for i = 1:size(obstacles, 1)
        rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                              obstacles(i,2)-obstacles(i,3), ...
                              2*obstacles(i,3), 2*obstacles(i,3)], ...
                  'Curvature', [1,1], 'FaceColor', [0.8,0.2,0.2]);
    end
    
    % Dessin du point de destination
    plot(but(1), but(2), 'gp', 'MarkerSize', 15, 'MarkerFaceColor', 'g');
    
    % Dessin du chemin parcouru
    plot(chemin(:,1), chemin(:,2), 'b-', 'LineWidth', 1.5);
    
    % Dessin de la trajectoire prédite
    plot(traj(:,1), traj(:,2), 'm--', 'LineWidth', 1.2);
    
    % Dessin de la position actuelle du robot
    robot_plot = rectangle('Position', [robot.position(1)-config.rayon_robot, ...
                                         robot.position(2)-config.rayon_robot, ...
                                         2*config.rayon_robot, 2*config.rayon_robot], ...
                           'Curvature', [1,1], 'FaceColor', [0.2,0.4,0.8]);
    
    % Dessin de l'orientation du robot
    quiver(robot.position(1), robot.position(2), ...
           config.rayon_robot*cos(robot.orientation), config.rayon_robot*sin(robot.orientation), ...
           'Color', 'k', 'LineWidth', 2, 'MaxHeadSize', 2);
    
    % Ajout de la légende
    legend('Obstacles', 'Destination', 'Chemin parcouru', 'Trajectoire prédite', 'Robot', 'Orientation', 'Location', 'Best');
    
    drawnow;
end

2.10 Affichage du chemin final

function afficher_chemin_final(chemin, obstacles, but, config)
    figure;
    hold on;
    axis equal;
    grid on;
    xlim([-1, 12]);
    ylim([-1, 12]);
    title('Résultat final de la planification de trajectoire');
    xlabel('X (m)');
    ylabel('Y (m)');
    
    % Dessin des obstacles
    for i = 1:size(obstacles, 1)
        rectangle('Position', [obstacles(i,1)-obstacles(i,3), ...
                              obstacles(i,2)-obstacles(i,3), ...
                              2*obstacles(i,3), 2*obstacles(i,3)], ...
                  'Curvature', [1,1], 'FaceColor', [0.8,0.2,0.2]);
    end
    
    % Dessin du point de destination
    plot(but(1), but(2), 'gp', 'MarkerSize', 15, 'MarkerFaceColor', 'g');
    
    % Dessin du chemin
    plot(chemin(:,1), chemin(:,2), 'b-', 'LineWidth', 2);
    plot(chemin(1,1), chemin(1,2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
    
    % Ajout des étiquettes
    text(chemin(1,1)+0.2, chemin(1,2)+0.2, 'Départ', 'FontSize', 12);
    text(but(1)+0.2, but(2)+0.2, 'Destination', 'FontSize', 12);
    
    % Ajout de la grille et du cadre
    grid on;
    box on;
end

III. Optimisation et extension de l'algorithme

3.1 Ajustement adaptatif des paramètres

function config = ajuster_parametres_adaptatifs(robot, but, obstacles, config)
    % Ajustement du temps de prédiction en fonction de la distance
    distance_but = norm(robot.position - but);
    config.temps_prediction = min(3.0, max(1.0, distance_but/2));
    
    % Ajustement des poids en fonction de la densité des obstacles
    densite_obstacles = compter_obstacles_proches(robot.position, obstacles, 3.0);
    if densite_obstacles > 3
        config.poids_distance = 0.6;  % Augmentation du poids d'évitement
        config.poids_orientation = 0.3;
    else
        config.poids_distance = 0.3;
        config.poids_orientation = 0.7;
    end
end

function count = compter_obstacles_proches(pos, obstacles, rayon)
    count = 0;
    for i = 1:size(obstacles, 1)
        if norm(pos - obstacles(i,1:2)) < rayon
            count = count + 1;
        end
    end
end

3.2 Support de points de destination multiples

function [prochain_but, but_atteint] = obtenir_prochain_but(robot, buts, index_actuel)
    but_actuel = buts(index_actuel, :);
    if norm(robot.position - but_actuel) < 0.5
        if index_actuel < size(buts, 1)
            prochain_but = buts(index_actuel+1, :);
            but_atteint = false;
        else
            prochain_but = but_actuel;
            but_atteint = true;
        end
    else
        prochain_but = but_actuel;
        but_atteint = false;
    end
end

3.3 Gestion des obstacles dynamiques

function deplacer_obstacles(obstacles)
    % Déplacement simple des obstacles (exemple)
    for i = 1:size(obstacles, 1)
        obstacles(i, 1) = obstacles(i, 1) + 0.02 * randn();
        obstacles(i, 2) = obstacles(i, 2) + 0.02 * randn();
        
        % Conservation des obstacles dans les limites
        obstacles(i, 1) = max(0.5, min(9.5, obstacles(i, 1)));
        obstacles(i, 2) = max(0.5, min(9.5, obstacles(i, 2)));
    end
end

3.4 Lissage du chemin

function chemin_lisse = lisser_chemin(chemin, poids_donnees, poids_lissage, tolerance)
    % Algorithme de lissage de chemin (descente de gradient)
    chemin_lisse = chemin;
    variation = tolerance;
    
    while variation >= tolerance
        variation = 0;
        for i = 2:size(chemin, 1)-1
            for j = 1:2
                aux = chemin_lisse(i, j);
                chemin_lisse(i, j) += poids_donnees * (chemin(i, j) - chemin_lisse(i, j)) + ...
                                    poids_lissage * (chemin_lisse(i+1, j) + chemin_lisse(i-1, j) - 2*chemin_lisse(i, j));
                variation += abs(aux - chemin_lisse(i, j));
            end
        end
    end
end

IV. Évaluation des performances et comparaisons

4.1 Calcul des indicateurs de performance

function evaluer_performances(chemin, but, obstacles, config)
    % Longueur du chemin
    longueur_chemin = 0;
    for i = 2:size(chemin, 1)
        longueur_chemin = longueur_chemin + norm(chemin(i,:) - chemin(i-1,:));
    end
    
    % Temps de planification
    temps_planification = toc;
    
    % Lissage (variation de courbure)
    courbure = 0;
    for i = 2:size(chemin, 1)-1
        dx1 = chemin(i,1) - chemin(i-1,1);
        dy1 = chemin(i,2) - chemin(i-1,2);
        dx2 = chemin(i+1,1) - chemin(i,1);
        dy2 = chemin(i+1,2) - chemin(i,2);
        angle_diff = acos((dx1*dx2 + dy1*dy2)/(norm([dx1,dy1])*norm([dx2,dy2])));
        courbure = courbure + angle_diff;
    end
    lissage = courbure / (size(chemin,1)-2);
    
    % Sécurité (distance minimale aux obstacles)
    dist_min = inf;
    for i = 1:size(chemin, 1)
        for j = 1:size(obstacles, 1)
            dist = norm(chemin(i,:) - obstacles(j,1:2)) - obstacles(j,3) - config.rayon_robot;
            if dist < dist_min
                dist_min = dist;
            end
        end
    end
    
    % Affichage des résultats
    fprintf('===== Résultats d''évaluation des performances =====\n');
    fprintf('Longueur du chemin: %.2f m\n', longueur_chemin);
    fprintf('Temps de planification: %.2f s\n', temps_planification);
    fprintf('Lissage du chemin: %.4f rad/m\n', lissage);
    fprintf('Distance minimale aux obstacles: %.2f m\n', dist_min);
end

4.2 Comparaison avec d'autres algorithmes

function comparer_algorithmes()
    algorithmes = {'DWA', 'A*', 'RRT'};
    resultats = zeros(length(algorithmes), 4); % [Longueur chemin, Temps planification, Lissage, Marge sécurité]
    
    for i = 1:length(algorithmes)
        [chemin, temps, lissage, dist_min] = executer_algorithme(algorithmes{i});
        resultats(i, :) = [longueur_chemin(chemin), temps, lissage, dist_min];
    end
    
    % Affichage des résultats de comparaison
    figure;
    subplot(2,2,1);
    bar(resultats(:,1));
    set(gca, 'XTickLabel', algorithmes);
    title('Comparaison de la longueur des chemins');
    ylabel('Longueur (m)');
    
    subplot(2,2,2);
    bar(resultats(:,2));
    set(gca, 'XTickLabel', algorithmes);
    title('Comparaison des temps de planification');
    ylabel('Temps (s)');
    
    subplot(2,2,3);
    bar(resultats(:,3));
    set(gca, 'XTickLabel', algorithmes);
    title('Comparaison du lissage des chemins');
    ylabel('Variation de courbure (rad/m)');
    
    subplot(2,2,4);
    bar(resultats(:,4));
    set(gca, 'XTickLabel', algorithmes);
    title('Comparaison des marges de sécurité');
    ylabel('Distance minimale (m)');
end

V. Cas d'application réels

5.1 Robot de logistique entrepôt

function simulation_robot_entrepot()
    % Configuration de l'environnement d'entrepôt
    obstacles = [
        % Rayonnages
        2, 2, 0.5; 2, 4, 0.5; 2, 6, 0.5; 2, 8, 0.5;
        4, 2, 0.5; 4, 4, 0.5; 4, 6, 0.5; 4, 8, 0.5;
        6, 2, 0.5; 6, 4, 0.5; 6, 6, 0.5; 6, 8, 0.5;
        8, 2, 0.5; 8, 4, 0.5; 8, 6, 0.5; 8, 8, 0.5;
        
        % Piliers
        5, 5, 0.3; 7, 3, 0.3
    ];
    
    % Points de départ et d'arrivée
    position_depart = [0.5, 0.5];
    position_but = [9.5, 9.5];
    
    % Exécution de l'algorithme DWA
    executer_simulation_dwa(position_depart, position_but, obstacles);
end

5.2 Navigation intérieure de drone

function simulation_navigation_drone()
    % Configuration de l'environnement intérieur
    obstacles = [
        % Murs
        0, 5, 0.2; 10, 5, 0.2; 5, 0, 0.2; 5, 10, 0.2;
        
        % Meubles
        3, 3, 0.4; 7, 3, 0.4; 3, 7, 0.4; 7, 7, 0.4;
        2, 5, 0.3; 8, 5, 0.3; 5, 2, 0.3; 5, 8, 0.3
    ];
    
    % Points de départ et d'arrivée
    position_depart = [1, 1, 1];  % Position 3D
    position_but = [9, 9, 2];
    
    % Exécution de l'algorithme DWA 3D
    executer_simulation_dwa_3d(position_depart, position_but, obstacles);
end

VI. Conclusion et extensions

6.1 Caractéristiques de l'algorithme DWA

1. Temps réel : Haute efficacité de calcul, adapté aux applications en temps réel
2. Réactif : Réponse rapide aux changements d'environnement
3. Complétude : Trouve une solution faisable en temps fini
4. Sensibilité aux paramètres : Performance dépendante de l'optimisation des paramètres

6.2 Directions d'extension

1. Planification 3D : Extension au mouvement sur l'axe Z
2. Collaboration multi-robots : Gestion des collisions inter-robots
3. Fusion de capteurs : Intégration de lidar, vision, etc.
4. Optimisation par apprentissage automatique : Utilisation de renforcement pour optimiser les paramètres

6.3 Guide d'optimisation des paramètres

Paramètre	Valeur recommandée	Influence
poids_orientation	0.6-0.8	Orientation vers l'objectif
poids_distance	0.2-0.4	Sécurité d'évitement
poids_vitesse	0.05-0.15	Vitesse de déplacement
temps_prediction	2-5s	Vision前瞻性
rayon_robot	Dimensions réelles	Distance de sécurité

6.4 Solutions aux problèmes courants

1. Minimum local : Ajout de perturbations aléatoires ou basculement vers un planificateur global
2. Comportement oscillant : Augmentation des contraintes de vitesse angulaire ou ajustement de la fonction d'évaluation
3. Chemin non lisse : Algorithme de lissage post-traitement
4. Obstacles dynamiques : Ajout d'un module de prédiction d'obstacles