Au delà des capacités de perception spécifiques du drone (susceptibles d'évoluer), c'est la problématique de localisation et navigation autonome multimodale dans un environnement complexe indoor/outdoor (mais approximativement connu) qui sera étudiée. La notion de mode désigne ici une instance spécifique de boucle Perception-Décision-Action (navigation par points de passage GPS, suivi de couloir, tracking de personne, SLAM, évitement d'obstacle, atterrissage d'urgence etc.) basée sur une estimation d'état associée (coordonnées dans un repère, position relative de la cible à tracker, distance par rapport à un obstacle etc.).

Nous savons par exemple que dans un environnement urbain, il est nécessaire de compenser la dégradation du signal GPS ou son indisponibilité par intermittence, en utilisant d'autres modes de localisation. Ces derniers pouvant être basés sur l'exploitation de capteurs extérieurs directement rattachés à l'environnement (par exemple en utilisant du motion capture avec des Vicon, une caméra fixe et des tags disposés sur le drone, etc.), soit sur l'exploitation des capteurs embarqués permettant une mesure locale des déplacements (SLAM, estimation de trajectoire à partir de centrale inertielle…).

La phase de transition d'un mode à l'autre est un sujet complexe encore peu abordé. Partant du principe que l'autonomie décisionnelle du drone ne s'étendra pas au delà du choix du (ou des) mode(s) à adopter, et qu'il reçoit en entrée les différents objectifs d'un plan de vol déterminé par un opérateur (ex: navigation par waypoint → recherche d'une ouverture dans un batiment → suivi de couloir..), ce sont les problèmes spécifiques aux phases de transition que nous allons étudier:

D'un point de vue « bas niveau » la problématique concerne la commande :

• Quelle méthode robuste permet le transfert d'un repère à un autre (estimations d'état de provenances, d'erreur et de précision multiples) tout en assurant un comportement sain sur la commande du drone.

A plus « haut niveau » c'est l'aspect supervision qui sera abordé:

• Anticiper la dégradation progressive des modes à l'abord d'un changement d'environnement (signal gps de moins en moins précis, limite du champ de vision des systèmes fixes..)
• Définir une stratégie de décision permettant d'utiliser à un instant donné la meilleure estimation disponible pour exécuter l'étape du plan de vol. (Plusieurs modes sont-ils amenés à être utilisés simultanément ? Ex: tracking + évitement d'obstacle). Utilisation d'un modèle décisionnel type mélange d'experts ?
• Choix du mode a adopter en fonction de l'étape du plan de vol ?

Bibliographie: Dans un premier temps l'objectif est d'acquérir un bon aperçu du paysage actuel dans le domaine de la navigation autonome des micro drones, ainsi que dans le domaine de la navigation multimodale. L'objectif de cette première étape consiste notamment à identifier des « familles » de modes et leur boucle perception-décision-action associée.

Mise en oeuvre des premiers moyens expérimentaux nécessaires:

Pour disposer de plusieurs modes de localisation, il serait intéressant de mettre rapidement en oeuvre ces deux moyens d'acquisition du couple attitude/coordonnées absolues : L'utilisation de caméras Vicon permettra d'établir un mode de référence indoor, étant donné qu'il s'agit du système le plus fiable et le plus rapide L'utilisation de tags disposés sur le drone ainsi que d'une caméra posée à une position connue (localisée par exemple dans le repère Vicon en utilisant une mire d'étalonnage classique sur laquelle on disposera des réflecteurs)

Dans un premier temps on pourra utiliser la configuration optimale pour étudier le passage du mode Vicon au mode Tag/Caméra en restant dans le repère commun Vicon. Dans un deuxième temps on pourra ajouter manuellement des erreurs de décalages entre les coordonnées du repère caméra et le repère Vicon afin d'étudier les effets produits et la robustesse nécessaire au système. Enfin il sera également possible de coupler ces deux modes de localisation avec un passage en outdoor pour ajouter le mode GPS. On simulera également l'utilisation d'un mode localisation basé sur les capteurs embarqués en bruitant les données fournies par le Vicon de façon à reproduire les dérives et erreurs caractéristiques de ce type de perception. Ces expérimentations seront effectuées sur une drone ENAC sous paparazzi.

Mots clés: Localisation multimodale, Navigation autonome, commande robuste, supervision.

Les référentiels de navigation:

Le référentiel ECEF et LTP : http://en.wikipedia.org/wiki/North_east_down

Passage d'un référentiel ENU à NED :

                          0  1  0
T enu-ned = T ned-enu =   1  0  0
                          0  0 -1

Angles d'Euler aéro : repère NED et rotation type “ 3(z / yaw) 2(y / pitch) 1(x / roll) ” ( R = Rx*Ry*Rz)

Besoins identifiés:

- Simulateur pour vol d'un quadrirotor (basé sur ?)

- Viewer 3D type GCS (dimension supplémentaire nécessaire à la navigation des quad) à intégrer sous paparazzi.

- Langage de description des plans de vol d'un quadrirotor en milieu indoor/outdoor et de ses différents modes.Compléter le format utilisé aujourd'hui pour les plans de vol paparazzi?

La chaîne de fonctionnement actuelle du drone en mode mocap:

Premières expériences à mettre en place:

- Commencer par faire fonctionner la boucle complète en transmettant les informations de position et attitude à f<10Hz. En interne les données d'attitude seront traitées par filtrage complémentaire avec les données fournies en embarqué par l'IMU. Attention les données injectées dans le filtre embarqué doivent être définies dans un référentiel NED !

- Suivi d'une trajectoire simple (ligne droite) par définitions de waypoints. Attention cette fois-ci les waypoints sont à définir dans un référentiel ENU

Problématiques émergeantes:

-Lors du passage d'un mode GPS à un mode indoor Vicon, on passe donc d'un référentiel LTP (défini par rapport à un point de départ en ECEF) à un référentiel local. Dans un premier temps on considérera les deux référentiels 'alignés' avec un simple offset sur le cap (NORD) qui sera calculé (depuis les coordonnées renvoyées par le Vicon) en ligne par le drone et directement réinjecté pour correction.

A terme, devons-nous considérer que nous avons une connaissance à priori de la transformation entre le repère local et le repère LTP utilisé pour le GPS ? Doit-on estimer en ligne la transformation entre le nouveau repère à diposition et l'ancien, que faire lorsqu'il n'existe pas de zone de recouvrement permettant d'estimer cette transformation ?

-De manière générale on cherche en permanence à se positionnner dans le repère le plus global connu.

L'effort fourni actuellement pour disposer du Mocap en tant que mode de localisation de référence (hauté précision, fréquence élevée) nous conduit à de multiples adaptations ad hoc. Les problématiques de transfert entre référentiels, et dans le cas présent le besoin de corriger le cap magnétique en indoor afin de l'aligner avec le repère local du mocap (repère HD décalé par rapport au nord magnétique), doivent être résolues dans le cadre d'une réflexion plus généraliste.

Partant du principe que certaines composantes de l'estimation d'état du drone (pose du drone dans un repère global connu, angle de lacet) ne sont pas toujours adaptées ou nécessaires à chacun des modes de navigation (ex: suivi de personne, évitement d'obstacle), nous souhaitons définir un mode comme une instance de boucle perception-commande fournissant à bas niveau les commandes d'assiette et de poussée strictement nécessaires à la réalisation du mode. Chaque mode peut donc fournir un jeu de commande plus ou moins complet, mais toujours homogène à une ou plusieurs des entrées du contrôleur bas niveau que sont le lacet, le roulis, le tangage et la poussée. En fonction du plan de vol plusieurs modes peuvent donc fournir des commandes “concurrentes” qui devront être gérées par un superviseur en fonction de leur priorité et de leur indice de confiance (incertitude de l'estimée?).

Même s'il existe un couplage mécanique entre les translations dans le plan (x,y) et la poussée qui commande la translation en z, l'hypothèse de vol quasi stationnaire (on considère que l'assiette reste trés faible pendant les déplacements) permet de considérer que l'altitude du drone ne dépend que de la poussée. Ces hypothèses sont confirmées dans la cas pratique par [Teulière01] et nous permettent de distinguer des modes agisssant uniquement sur l'assiette, et d'autres sur la poussée. Ceci permet de simplifier la gestion des commandes au niveau du superviseur.

La réflexion sur le choix de la commande en position ou en vitesse angulaire reste en cours.. (position pour roulis et tangage, vitesse pour lacet ?)

Bénéfices de cette méthode :

• Ne nécessite pas de traduire les données capteurs en une information exploitable dans un repère extérieur (LTP, repère local mocap etc)
• Résout la problématique de gestion de la commande en phase de transition ou lors de l'usage simultané de plusieurs modes par le biais du superviseur

Question: Les spécificités de l'espace de travail de chaque mode ne sont-elles pas reportées sur l'indice de confiance (incertitude) associé à chaque commande ? Autrement dit les commandes sont bien homogènes mais les incertitudes de chaque mode ne le sont pas. Comment les comparer ?

Schéma de fonctionnement général

[Teulière01] C. Teulière.- Approches déterministes et bayésiennes pour un suivi robuste: application à l'asservissement visuel d'un drone.- PhD. Thesis, Université de Rennes 1, 2010.

poster_workshop_microdrone_28012012_final.pdf

Exemple de tache de tracking: Simulation sous matlab de l'obtention de la pose du drone (caméra) après observation d'un modéle connu.

-Script de test-

%%paramètres intrinsèques caméra
%distances focales en unité pixel
syms fu fv real
%coordonnées pixelliques du point principal
syms pu pv real
 
MCi = [fu 0 pu 0;0 fv pv 0;0 0 1 0];
 
%%paramètres externes
% Phi(rot_x) Theta(rot_y) Psi(rot_z)
syms Phi Theta Psi real
syms tx ty tz real
 
Rx = [1 0 0 ; 0 cos(Phi) sin(Phi) ; 0 -sin(Phi) cos(Phi)];
Ry = [cos(Theta) 0 -sin(Theta) ; 0 1 0 ; sin(Theta) 0 cos(Theta)];
Rz = [cos(Psi) sin(Psi) 0; -sin(Psi) cos(Psi) 0; 0 0 1];
 
R = Rz*Ry*Rx;
 
%La pose caméra est donnée dans le repère monde donc rotation inverses 
% + Axe z repère caméra inversé par rapport a celui du repère monde
% (translation de tz et non -tz)
MCe = [ [R [-tx;-ty;tz]]; 0 0 0 1]; 
 
%C = MCi*MCe;
 
%Matrice caméra réelle
MCir = subs(MCi,{'fu','fv','pu','pv'},{500,500,0,0});
MCer = subs(MCe, {'tx','ty','tz', 'Phi', 'Theta', 'Psi'},{0,0,3,0,0,0});
C = MCir*MCer;
 
%Modèle pour tracking
Ptrack1 = [-1 -1 0 1];
Ptrack2 = [1 -1 0 1];
Ptrack3 = [1 1 0 1];
Ptrack4 = [-1 1 0 1];
 
Ptrack = [Ptrack1; Ptrack2; Ptrack3; Ptrack4];
 
% mov_x = 1;
% mov_y = -0.6;
% Pmoved = [Ptrack(:,1)+mov_x  Ptrack(:,2)+mov_y Ptrack(:,3) Ptrack(:,4)];
 
%Coordonées pixelliques du modèle en position de départ
Ptrack_uv = [(C*(Ptrack(1,:))')';(C*(Ptrack(2,:))')';(C*(Ptrack(3,:))')';(C*(Ptrack(4,:))')'];
for i = 1:4,
    Ptrack_uv(i,:) = Ptrack_uv(i,:)/Ptrack_uv(i,3);
end
 
% % %Coordonées pixelliques du modèle après mouvement de la caméra
% % Pmoved_uv = [(C*(Pmoved(1,:))')';(C*(Pmoved(2,:))')';(C*(Pmoved(3,:))')';(C*(Pmoved(4,:))')'];
% % for i = 1:4,
% %     Pmoved_uv(i,:) = Pmoved_uv(i,:)/Pmoved_uv(i,3);
% % end
Camera_pose = {1.0,0.6,3,pi/12,pi/10,0};
MCir = subs(MCi,{'fu','fv','pu','pv'},{500,500,0,0});
MCer = subs(MCe, {'tx','ty','tz', 'Phi', 'Theta', 'Psi'},Camera_pose);
C = MCir*MCer;
Pmoved_uv = [(C*(Ptrack(1,:))')';(C*(Ptrack(2,:))')';(C*(Ptrack(3,:))')';(C*(Ptrack(4,:))')'];
for i = 1:4,
    Pmoved_uv(i,:) = Pmoved_uv(i,:)/Pmoved_uv(i,3);
end
 
 
% Utilisation de la jacobienne pour lsqnonlin
%Calcul de la jacobienne de ComputeDistArray
dist_symb = ComputeDistArray([tx,ty,tz,Phi,Theta],MCir, MCe, Pmoved_uv, Ptrack,[]);
Jsymb = jacobian(dist_symb, [tx,ty,tz,Phi,Theta,Psi]);
 
optim=optimset('Jacobian','on','Display','off','TolX',1e-3,'TolFun',1e-3);
par0 = [0 0 3 0 0 0];
[pose_est,fval]=lsqnonlin(@(par)ComputeDistArray(par,MCir, MCe, Pmoved_uv, Ptrack, Jsymb),par0,[],[],optim);
 
% % utilisation par défaut de lsqnonlin
% optim=optimset('Display','off','TolX',1e-3,'TolFun',1e-3);
% par0 = [0 0 3 0 0 0];
% [pose_est,fval]=lsqnonlin(@(par)ComputeDistArray(par,MCir, MCe, Pmoved_uv, Ptrack, []),par0,[],[],optim);
 
 
figure
plot(Ptrack(:,1),Ptrack(:,2),'r+')
text(Ptrack(1,1),Ptrack(1,2),'  Pt1')
text(Ptrack(2,1),Ptrack(2,2),'  Pt2')
text(Ptrack(3,1),Ptrack(3,2),'  Pt3')
text(Ptrack(4,1),Ptrack(4,2),'  Pt4')
hold on
plot(Camera_pose{1},Camera_pose{2},'bo');
legend('Model in word coordinates','Camera Pose')
axis on;
axis([-3, 3, -3, 3]);
grid on;
hold off;
 
figure
plot(Ptrack_uv(:,1),Ptrack_uv(:,2),'r+')
text(Ptrack_uv(1,1),Ptrack_uv(1,2),'  Pt1')
text(Ptrack_uv(2,1),Ptrack_uv(2,2),'  Pt2')
text(Ptrack_uv(3,1),Ptrack_uv(3,2),'  Pt3')
text(Ptrack_uv(4,1),Ptrack_uv(4,2),'  Pt4')
hold on
plot(Pmoved_uv(:,1),Pmoved_uv(:,2),'bo')
legend('Model in pixel coordinates', 'Obervation from new Camera Pose')
axis([-400, 400, -300, 300]);
grid on;
hold off;

-Fonction à optimiser-

function [dist_array, Jcb] = ComputeDistArray(param, MCi, MCe, obs_pix_coord, model_w_coord, Jsymb)
 
    C = getCameraMatrix(param,MCi,MCe);
 
 
    for i = 1:length(model_w_coord(:,1)),
        model_pix_coord(i,:) = (C*(model_w_coord(i,:))')';
 
        %normalize for distance operation
        model_pix_coord(i,:) = model_pix_coord(i,:)/model_pix_coord(i,3);
        obs_pix_coord(i,:)   = obs_pix_coord(i,:)/obs_pix_coord(i,3);
 
        %J(i,:) = (jacobian((CSym*(model_w_coord(i,:))'),[tx,ty,tz,Phi,Theta,Psi]))';
        %display( jacobian((CSym*(model_w_coord(i,:))'),[tx,ty,tz,Phi,Theta,Psi]) );
 
    end
 
    if( ~isempty(Jsymb)),
        if nargout > 1,
            Jcb = subs(Jsymb,{'tx','ty','tz', 'Phi', 'Theta', 'Psi'},{param(1),param(2),param(3),param(4),param(5),0});
        end
    end
 
    %keep only u/v values
    dist_array = obs_pix_coord(:,1:2) - model_pix_coord(:,1:2);
 
end
 
function C = getCameraMatrix(param,MCi,MCe)
    MCe_iter = subs(MCe, {'tx','ty','tz', 'Phi', 'Theta', 'Psi'}, ...
               {param(1),param(2),param(3),param(4),param(5),0});
 
    C = MCi*MCe_iter;
end

Rappel sur l'évolution de la méthode de gestion des modes de navigation :

La définition de la tache de navigation autonome et multimodale comprend à l'origine trois objectifs:

• Être capable de réaliser plusieurs tâches de navigation reposant sur l'utilisation des mêmes degrés de liberté (mélange)
• La capacité de transition entre modes de navigation par le biais d'une commande continue (transition)
• la capacité de diagnostique et de décision autonome afin de sélectionner le ou les modes de navigation les plus appropriés en fonction du plan de vol, de l'environnement actuel, et de connaissances à priori.

Objectifs auxquels on peut ajouter une contrainte

• Intégrer ces fonctionnalités au sein d'un framework permettant d'ajouter facilement de nouvelles modalités de perception et de nouveaux modes de navigation.

(L'aspect planning a été ignoré jusqu'ici, la réalisation d'un plan de vol de façon autonome sous-entend également de disposer d'un superviseur permettant le séquencemment des tâches de navigation mais également la prise de décision. (→POMDPs ?) )

Dans ce but la première idée était de disposer de chaînes perception-commande indépendantes et de réaliser la fusion à bas niveau, en mélangeant les commandes. Cette méthode présente en théorie l'avantage de ne mélanger que des données homogènes, car toutes exprimées sur une partie ou sur l'ensemble de l'espace de commande.

Cette méthode étant rapidement et facilement mise en défaut (notamment le concept de mélange de commande semble abandonné depuis des années), l'idée consiste dorénavant à mélanger à plus haut niveau. En remontant au niveau de la couche de perception, on bénéficie d'un information plus riche et redondante, qui nous permet notamment de réaliser un des premiers objectifs de la thèse, la sélection du (ou des) mode(s) de perception le plus adapté pour les taches de navigation en cours.

Nouvelle approche:

Dans cette nouvelle approche, la partie mélange (que ce soit en terme de fusion de données ou pour la recherche de commande optimale multi-objectif) et gestion de la transition sont considérées comme acquises (ou ignorées pour l'instant en ce qui concerne la transition). La thématique de fusion de données constitue en soi une problématique très riche, et il est préférable dans un premier temps de se baser sur des méthodes classiques.

→Parmi les différents estimateurs à disposition, il serait toujours intéressant de travailler sur l'approche par minimisation (soutenue par Bertrand). Une fois la méthode de sélection établie et fonctionnelle (sous-entendant qu'on dispose d'un méthode d'évaluation de la pertinence d'un ensemble capteurs/estimateur), il sera automatiquement possible d'évaluer (éventuellement de prouver) l'intérêt de la méthode.

La capacité à assurer une commande continue pendant les phases de transition n'est plus vue comme nécessaire, et on s'autorise à effectuer un switch 'binaire' entre différents modes de navigation. Si cette problématique est toutefois abordée, elle sera plutôt intégrée dans les couches bas niveau pour la gestion de la commande finale. De même on considère la possibilité pour le drone de prendre l'initiative de s'arrêter pour effectuer un diagnostique de ses capacités de perception et des modes utilisables avant de reprendre le cours de son plan de vol.

On se focalise donc sur la fonction de sélection des modes de navigation ainsi que des modalités de perception sur lesquels ils se basent, en utilisant une architecture classique reposant sur des briques logicielles existantes (ce qui permet en théorie d'intégrer facilement de nouvelles modalités). Cette fonction, définie comme une tache de supervision régit les capacités de perception, d'estimation, ainsi que les modes de navigation en fonction de quatre inputs principaux:

• le plan de vol (séquence de tâches de navigation associées à une précision requise)
• les différentes estimations potentiellement fournies par différentes méthodes de filtrage ( ainsi que leur incertitude ).
• les données capteurs “utiles” ainsi que l'information provenant de capteurs dédiés, dans le but de définir un niveau de confiance pour chaque modalité de perception, plus simplement de déterminer s'il est pertinent d'utiliser un capteur dans l'environnement actuel, ou encore de choisir le capteur le plus approprié parmi plusieurs capteurs redondants.
• différents états internes du drone (batterie, capacité de calcul disponible, etc..)

- 1 - Dans un premier temps elle décide des capacités de perception à utiliser en cherchant les capteurs capables de fournir les données nécessaires pour assurer les taches de navigation en cours (états requis par la couche de commande (+ connaissance sur l'environnement)). Parmi ce sous-ensemble on peut opérer une seconde sélection en fonction de leur utilité ainsi que leur coût. L'utilité s'apparente (à priori) à la précision apportée sur l'état requis pour la tâche en cours. Dans ce but il paraît plus pertinent de se baser sur les incertitudes en sortie de la couche de filtrage (et de différents filtres si nous avons possibilité de choisir)

L'exploitation de l'information mutuelle (entre observation fournie par un capteur et l'état actuel) semble fournir un cadre théorique particulièrement approprié. Cette méthode, principalement utilisée dans les réseaux de capteurs à grande échelle, fournit un moyen de calculer l'utilité apportée par un capteur vis-à-vis de l'estimation d'état. En outre elle se complète facilement de paramètres de coût liés à l'utilisation d'un capteur, et pourrait donc s'intégrer facilement dans un framework dédié à la décision. Lien 1 : Maximum Mutual Information Principle for Dynamic Query Problems Lien 2 : Sensor Selection for Active Information Fusion

→Le niveau d'incertitude se mesure en sortie de la couche de filtrage, on peut également imaginer le mesurer en sortie de la couche commande si on dispose d'un modèle obtenu lors d'une phase d'identification de la précision réelle fournie par chaque contrôleur (?)

→Parmi ce sous-ensemble de capteurs la possibilité de choisir entre plusieurs capteurs redondants en fonction de leur modèle de bruit et de la précision requise pour effectuer la tâche en cours a été abordée. En terme d'estimation il est toujours pertinent d'intégrer le maximum de données capteur possible. Dans le cas ou l'un d'eux n'est plus fiable (perte de signal gps) un estimateur classique ne le prend plus en compte. Est-il nécessaire de procéder à cette discrimination avant d'utiliser la donnée ? Si nous faisons ce choix pour des raisons de coût (calcul, consommation etc..) est-ce que ces raisons sont vraiment pertinentes (ces contraintes ne seront pas toujours vraies), et surtout, dans le cas pratique va-ton vraiment disposer d'un choix (capacité limitée sur un drone)?

- 2 - Dans un deuxième temps il s'agit de choisir le traitement à appliquer aux données capteurs (fusion et filtrage pour l'estimation)

→Ce travail est-peut-être directement imbriqué avec la première phase de décision, si on prend la décision en se basant sur la sortie de la couche de filtrage plutôt que sur la donnée capteur ?

→En pratique disposera-t-on de plusieurs estimateurs ? Auquel cas comment aborder la transition lors du switch d'un estimateur vers l'autre ?

- 3 - Enfin, on fournit aux modes de navigation les données issues de la couche de filtrage. Il semblerait qu'on perde ici une capacité à mélanger les taches demandées, sauf si la réalisation simultanée (ex: tracking + évitement d'obstacle) de deux objectifs fait partie des blocs des modes de navigation à disposition.

De cette manière, la tâche de supervision établit de façon dynamique une instance de connexions entre la couche de perception, la couche d'estimation, et la couche de contrôle dans le but d'utiliser les informations disponibles pour réaliser au mieux la ou les tâches en cours.