FR2968395A1 - Systeme d'estimation d'une trajectoire - Google Patents

Abstract

Système d'estimation d'une trajectoire d'un élément ou d'un corps mobile comprenant un ensemble capteur (EC) comprenant un accéléromètre (A) et un gyromètre (G) liés de manière solidaire, comprenant des moyens de correction rétroactive (CORR_PPOS_A) d'une position angulaire (Position_A) obtenue à partir des mesures du gyromètre (G) à au moins un instant parmi une pluralité d'instants successifs (t t , ... t , t ) d'un intervalle de temps ([t ; t ])à partir d'une position cartésienne (Position_C) de l'accéléromètre (A) à au moins un desdits instants successifs et d'informations prédéterminées (INF_PRED_TRAJ) représentatives de la trajectoire de l'accéléromètre (A) durant ledit intervalle de temps ([t ; t ]),et à partir d'une rotation adaptée pour minimiser un écart entre lesdites informations prédéterminées (INF_PRED_TRAJ) et leur équivalent sur la trajectoire obtenue après rotation.

Description

Système d'estimation d'une trajectoire

L'invention porte sur un système d'estimation d'une trajectoire d'un élément ou d'un corps mobile comprenant un ensemble capteur comprenant un accéléromètre et un gyromètre liés de manière solidaire. La trajectoire d'un élément est un ensemble de positions cartésiennes Position _C successives de l'élément. L'invention concerne notamment l'estimation de cette trajectoire ou de l'orientation dans le cadre de la navigation piéton. Ce domaine est en 1 o pleine expansion, avec au moins une conférence internationale intégralement dédiée au domaine de la navigation et de la localisation à l'intérieur des bâtiments (International Conference on Indoor Positionning and Indoor Navigation - IPIN, articles publiés dans IEEE Xplore). L'invention se situe dans un contexte à deux modalités de 15 mesure, à savoir accéléromètres et gyromètres (AG). Sans un système externe permettant de déterminer la direction du Nord ou un magnétomètre pour mesurer le champ magnétique terrestre, il n'est pas possible de garantir que le repère de référence soit le repère Nord pour "North Frame" en langue anglaise, qui est un repère fixe dans le 20 référentiel terrestre. Ce repère est utilisé durant tout le mouvement (marche ou course). Il est considéré comme le repère de référence : les orientations (position angulaire) et positions (position cartésienne) sont mesurées par rapport à ce repère. Le repère Nord est défini par son origine ONF et ses axes 25 (xNF YNF ZNF )qui Se confondent avec les axes du repère terrestre, de directions l'Est, le Nord et la verticale ascendante. Son origine ONF peut correspondre à l'endroit où le mouvement débute ou à tout autre endroit. Par conséquent, dans cette partie, nous nous référons au repère de navigation NF, qui est lié au repère Nord par une rotation constante 30 autour de l'axe vertical. Dans la suite de la description, le repère de navigation NF est utilisé comme repère de référence, et les mesures données dans ce repère de navigation NF ont un exposant n. La trajectoire parcourue par l'ensemble capteur EC peut être récupérée par une double intégration de l'accélération propre aP en trois 35 dimensions ou 3D dans le repère de navigation NF.

Soit le repère BF un repère lié au capteur. Dans la suite, les vecteurs exprimés dans ce repère ont un exposant b. Les mesures de l'accéléromètre sont données dans le repère BF et la gravité (ou pesanteur) est superposée à l'accélération propre aP .

Avant de réaliser la double intégration il est donc nécessaire d'exprimer les mesures dans le repère NF et de compenser la gravité. Pour réaliser le passage du repère NF au repère BF, une formule de passage est nécessaire. Cette formule peut être obtenue si on connaît l'orientation du capteur. Dans le contexte AG, l'orientation est obtenue en 1 o intégrant la mesure du gyromètre. Cependant, cette intégration génère une dérive que nous proposons de limiter dans la présente invention. Dans la littérature, la dérive d'orientation est généralement exprimée comme une erreur d'orientation du repère local BF par rapport au repère NF. On peut alors considérer qu'une erreur ou dérive de l'orientation 15 estimée tourne les données du repère local BF, non pas vers le repère de navigation NF, mais vers un autre repère de navigation NF' affecté par la dérive et défini dans la suite de la description. Le repère NF', affecté par la dérive est défini par son orientation par rapport au repère de navigation ou repère global fixe lié au repère 20 terrestre NF. Cette orientation de NF' par rapport à NF est liée à la dérive de l'orientation estimée en intégrant le gyromètre. Ainsi, l'orientation du repère NF' par rapport au repère NF dérive lentement au fil du temps. Les mesures données dans ce repère NF' ont un exposant n'. Si on représente l'orientation du repère NF dans le repère NF' par 25 un quaternion qn,n , l'orientation du repère NF dans le repère BF par un quaternion qbn, l'orientation du repère NF' dans le repère BF par un quaternion qbn,, on peut alors écrire par l'équation suivante : qbn (t) - qbn qn n (t) le signe (D'étant le signe du produit associé aux quaternions. 30 Dans une approche classique, il est possible d'estimer la trajectoire d'une cheville d'un utilisateur à partir des données inertielles mesurées par l'accéléromètre et le gyromètre. Dans ce cas, (au niveau de la cheville), le cycle de marche peut être séparé en deux phases : la phase de vol pendant laquelle le pied se 35 déplace (appelée SW pour "SWing phase" en langue anglaise), et celle pendant laquelle le pied est au sol ou phase d'appui (appelée ST pour "STance phase" en langue anglaise). Un pas est composé de la succession des deux phases ST et SW et la marche est caractérisée par une succession de pas.

On nomme [ti ,t,'] l'intervalle de temps de la phase SW pour le kiè' pas détecté. Afin de limiter la dérive de position cartésienne qui est introduite par la double intégration de l'accélération, cette dernière est intégrée uniquement durant la phase SW de déplacement du pied, tandis que la vitesse est mise à zéro pendant la phase ST. Cet ajustement 1 o nécessite la détection précise des deux phases afin de déterminer si le pied d'un utilisateur est ou non en mouvement. La vitesse de la cheville d'un piéton peut être calculée pas par pas (après exécution d'une étape de détection), tandis que l'orientation ou position angulaire Position_A est déterminée à chaque indice de temps l 15 avec le quaternion qbn (qui définit l'orientation du repère global NF dans le repère local BF) calculé à partir des mesures de vitesses angulaires (ob(l) _ [cob(l) cob~l> cob(l) ]T et le quaternion d'orientation précédent qbn -1' comme dans l'équation qbn' = qb0,i-1> 0 qbn ~' Une fois que l'orientation dans le repère local BF est déterminée, 20 les mesures de l'accéléromètre sont simplement tournées par rotation dans le repère global NF, par la relation ans`' = Rn'' ?,ab'» dans laquelle Rn(Y) est la matrice de rotation associée au quaternion qn`b telle que définie par : 2gbn,0 -1+ 2gbn,1 2gbn,1 gbn,2 - 2gbn,0 gbn,3 2gbn,1 gbn,3 + 2gbn,0 gbn,2 R bn - 2gbn,1 gbn,2 + 2gbn,0 gbn,3 2gbn,0 -1+ 2gbn,2 2gbn,2 gbn,3 - 2gbn,0 gbn,l 2gbn,1 gbn,3 - 2gbn,0 gbn,2 2gbn,2 gbn,3 + 2gbn,0 gbn,l 2gbn 22 ,0 - 1+ 2gbn,3 25 Le quaternion qnb étant le conjugué de (Ibn et la matrice Rbn étant la matrice de transformation inverse de Rnb. Quand la mesure de l'accéléromètre dans le repère NF an a été calculée, la gravité doit être compensée pour calculer l'accélération propre : aP n = an + gn 30 En fait, le quaternion de positition angulaire qbn' dérive par rapport à sa "vraie" valeur en raison de la dérive et du bruit des mesures du gyromètre G. En outre, le quaternion de position angulaire qbn' , obtenu par la relation qbn' = fq (e,,-" ~b"-" ) = qbc(ol-" O e,,-" , nécessite la connaissance des conditions initiales. Ainsi, l'accélération propre est corrompue à cause de la dérive due à l'intégration du gyromètre pour obtenir l'orientation. Les mesures de l'accéléromètre, et ainsi même l'accélération 5 propre aP sont altérées par le bruit et parfois par un décalage ou "offset" en langue anglaise. Même si l'accélération propre est calculée avec un quaternion de position angulaire qbn' "parfait", la double intégration de ce décalage et de ce bruit conduit à des erreurs sur la position estimée, ainsi que sur la vitesse. 1 o En outre, en raison de la dérive du quaternion qbn' , le champ de gravité qui est compensé n'est pas égal à gr' = [0 0 -UT car on n'est pas dans le repère global NF, mais dans le repère NF'. Le thème de la navigation et de la localisation dans des bâtiments, i.e. en intérieur ou "indoor" en langue anglaise, est un problème 15 classique rencontré dans la littérature. En 2010, une conférence intégralement dédiée à la navigation et au positionnement indoor a été créée (IPIN pour "International Conference on Indoor Positioning and Indoor Navigation", IEEE). Parmi les articles présentés, neuf sont consacrés à la navigation piéton avec un système de capture de mouvement fixé au pied ou 20 "foot mounted pedestrian navigation" en langue anglaise. Ci-suit un résumé de ces communications ainsi que d'autres communications. Un premier article, "A High Precision Reference Data Set for Pedestrian Navigation using Foot-Mounted Inertial Sensors", de Michael Angermann, Patrick Robertson, Thomas Kemptner, et Mohammed Khider, 25 IPIN conference, Zurich, 2010, présente une méthodologie d'aquisition de données basée sur un système optique de référence et une unité de mesures inertielles fixée au pied, le problème de l'estimation de la trajectoire n'étant pas abordé. Un deuxième article, "Evaluation of Zero-Velocity Detectors for 30 Foot-Mounted Inertial avigation Systems", de Isaac Skog, John-Olof Nilsson, et Peter Hândel, IPIN conference, Zurich, 2010, s'intéresse au problème de la détection des phases de mouvement à vitesse nulle pour un pied équipé d'une centrale inertielle ("zero velocity detectors for a footmounted inertial sensor based pedestrian navigation system"). Quatre techniques de 35 détection de vitesse nulle sont comparées sur le critère d'erreur de position.

Le problème de dérive de position induit par les décalages et dérives des mesures n'est pas abordé. Un troisième article, " Comparison and Evaluation of Acceleration Based Step Length Estimators for Handheld Devices ", de Jasper Jahn, Jochen Seitz, Lucila Patino-Studencka, Ulrich Batzer, et Javier Gutiérrez Boronat, IPIN conference, Zurich, 2010, compare plusieurs techniques d'estimation de la longueur du pas d'un piéton à partir de mesures accélérométriques, sans aborder les problèmes de dérive et de recalage. Un quatrième article, "An improved shoe-mounted inertial navigation system", de Nadir Castaneda et Sylvie Lamy-Perbal, IPIN conference, Zurich, 2010, propose un système amélioré de suivi de piéton utilisant un capteur inertiel monté au niveau de la chaussure d'un utilisateur. Le système met en oeuvre une procédure de détection de la phase d'appui ou "STance phase" à base de logique floue. Un filtre de Kalman indirect IKF est implanté (l'état n'est pas directement estimé, mais une erreur sur l'état, comme dans un cinquième article, "Pedestrian Inertial Navigation with Gait Phase Detection Assisted Zero Velocity Updating Young Soo Suh, Sangkyung Park", 4th International Conference on Autonomous Robots and Agents, Feb 10-12, 2009, Wellington, New Zealand, où le filtre de Kalman indirect IKF est implanté dans le cadre de la navigation piéton, cet article mettant en oeuvre des capteurs inertiels/magnétiques et des capteurs de force positionnés respectivement sur la pointe de la chaussure et sous la chaussure). La constitution du système inertiel n'est pas explicitement décrit, et un filtre de Kalman est implanté.

Un sixième article, "Context-Adaptive Algorithms to Improve Indoor Positioning with Inertial Sensors", de Ulrich Walder, Thomas Bernoulli, Gerald Glanzer, et Thomas Wief3flecker, IPIN conference, Zurich, 2010, constate que la précision de positionnement pour les systèmes inertiels (montés sur le corps) pour la navigation piéton à l'intérieur de bâtiments est faible si on implante juste une double intégration des accélérations. Deux améliorations sont proposées respectivement sur la détection des phases à vitesse nulle et l'utilisation d'une carte pour recaler la trajectoire ou diminuer la dérive. Un septième article, "Dual IMU Indoor Navigation with Particle 35 Filter based Map-Matching on a Smartphone", de C. Ascher, C. Kessler, M.

Wankerl, G.F. Trommer, IPIN conference, Zurich, 2010, traite, de la navigation à l'intérieur de bâtiments avec recalage sur une carte embarquée dans un téléphone intelligent ou "smartphone" en langue anglaise. Deux unités de mesures inertielles à bas-coût, un compas électronique et un altimètre sont utilisés. Un filtre à particule est implanté pour fusionner les informations. La détection des phases à vitesse nulle est implantée. Un algorithme de localisation et de construction de carte conjointement (SLAM algorithm - Simultaneous Localisation And Mapping) est exploité, même si localement, les capteurs qui permettent de construre la carte peuvent être 1 o indisponibles. Dans un huitième article, "Design choices, filter parameter tuning, and calibration of zero-velocity update aided inertial navigation systems for pedestrian navigation", de John-Olof Nilsson, Isaac Skog, et Peter Hândel, IPIN conference, Zurich, 2010, est traité le problème de la conception d'un 15 système de navigation inertiel avec détection des phases à vitesse nulle (ZUPT pour "Zero velocity update"). Le système met en oeuvre un filtre de Kalman étendu (EKF) et la détection ZUPT. Les auteurs s'intéressent aux choix de conception (placement de capteurs, spécifications) et aux paramètres à ajuster pour le filtre de Kalman. Les résultats sur l'erreur 20 moyenne de position montrent de bons résultats par rapport à ceux rencontrés usuellement dans la littérature. Un neuvième article, "Pedestrian Indoor Navigation by aiding a Foot-mounted IMU with RFID Signal Strength Measurements", de Antonio R. Jiménez, Fernando Seco, J. Carlos Prieto et Jorge Guevara, IPIN 25 conference, Zurich, 2010, met en oeuvre une unité de mesures inertielles attaché au pied d'un piéton et des étiquettes RFID. La détection des phases à vitesse nulle est exploitée et un filtre de Kalman inverse est implanté pour compenser les erreurs de position, vitesse et attitude ou position angulaire. La technique implantée est valide pour différents types de mouvement 30 (latéral, arrière, marche, course) et ne nécessite pas de phase de calibration hors la ligne de la démarche, ni de l'atténuation des signaux radio dans l'environnement. La dérive de cap est réduite grâce à l'utilisation de magnétomètres. Un dixième article, "On the use of foot-mounted INS, UWB-35 ranging and opportunistic cooperation in high-accuracy indoor positioning systems", de Peter Strémbâck, Jouni Rantakokko, et Erika Emilsson, IPIN conference, Zurich, 2010, couple une centrale inertielle et de la radio ultra-large bande (IR-UWB) de manière à diminuer les erreurs de position. Le système utilisé pour la localisation est donc plus riche et plus complexe qu'un simple dispositif comprenant un accéléromètre et un gyromètre. Un onzième article, "A GPS/INS-based architecture for rescue team monitoring" d'Alberta Croci, Mattia De Agostino, et Ambrogio Manzino, IPIN conference, Zurich, 2010 porte sur une implantation d'un système mixte navigation par satellites/centrale inertielle : le système inertiel est utilisé pour 1 o compter les pas et mesurer la durée temporelle d'un pas, le magnétomètre et le gyromètre étant utilisés pour estimer la direction du déplacement. Les données GPS sont utilisées pour corriger le biais (de position) lorsqu'elles sont suffisamment fiables. D'autres articles, tel un douzième article, "Towards Real-Time 15 Camera Egomotion Estimation and Three-Dimensional Scene Acquisition from Monocular Image Streams", de Aufderheide et Werner Krybus, IPIN conference, Zurich, 2010, utilisent des systèmes de vision pour compenser la dérive introduite par la double intégration de l'accélération propre, ou bien des systèmes avec un champ magnétique externe alternatif comme décrit 20 dans un treizième article, "Position Estimation Using Artificial Generated Magnetic Fields", de Jérg Blankenbach et Abdelmoumen Norrdine, IPIN conference, Zurich, 2010. Pour résumer ces treize articles précédents, soit ils mettent en oeuvre un système complémentaire à la centrale inertielle (y compris carte, 25 GPS, UWB) pour compenser la dérive en position, soit ils implantent un filtre de Kalman (IKF, EKF), soit ils exploitent explicitement des mesures fournies par des magnétomètres. Le document FR 2 918 745 propose un dispositif d'assistance à la navigation d'une personne comprenant au moins un calculateur comprenant 30 en mémoire une carte numérisée du lieu dans lequel le trajet de la personne est prévu, une interface homme-machine, un ensemble de capteurs portés par la personne et délivrant les informations sur les mouvements de la personne. L'ensemble de capteurs comprend par exemple une centrale inertielle. Les capteurs portés par l'utilisateur sont par exemple des 35 accéléromètres, magnétomètres, gyromètres, et baromètres. Un calcul du chemin à parcourir est par exemple mis à jour en fonction d'objectifs intermédiaires identifiés du trajet atteint, ces objectifs étant repérés sur la carte numérisée. Dans ce cas, il y a un recalage de la dérive aux objectifs intermédiaires. Le dispositif de la présente invention ne nécessite pas de carte du lieu. Il ne nécessite pas non plus de "recalage manuel" par rapport à des points de passage intermédiaires identifités. Le document US20070018890 propose un dispositif de navigation interne et externe. La version portable (navigation piéton) fusionne les informations fournies par plusieurs capteurs, GPS, podomètres et/ou 1 o réalisées à partir d'accéléromètres à deux axes de mesure, et compas numérique. Des capteurs sont placés sur le dos d'un chien accompagnateur. La localisation interne s'opère en exploitant les connexions sans fil déjà disponibles dans de nombreux environnements du fait de l'existence des réseaux WiFi. La fusion est réalisée dans le cadre de la théorie de Dempster- 15 Shafer. Le système ne met pas en oeuvre de gyromètres. Le document EP1488197 combine un dispositif GPS et une unité de navigation à l'estime ou "Dead Reckoning Unit" en langue anglaise, le système de navigation étant destiné à des personnes mal voyantes. Dans le mode de réalisation proposé, le système contient également une caméra 20 numérique pour la capture d'images. Une carte numérisée de l'environnement est également exploitée. L'unité de navigation à l'estime ou DRU pour Dead Reckoning Unit contient un accéléromètre, un gyromètre et un micro-contrôleur ; elle prend le relais lors de pertes du signal GPS ou pour améliorer la précision de la localisation obtenue par le GPS. Le DRU permet 25 d'estimer la distance parcourue et le changement de direction, en couplant ces informations à une carte numérisée même en cas de perte du signal GPS. Il est possible de localiser l'utilisateur . La distance est estimée à partir du nombre de pas réalisés et de la longueur moyenne d'un pas. La direction relative de l'utilisateur est estimée à l'aide d'un gyroscope, car il est 30 insensible aux perturbations magnétiques et est précis. Les auteurs signalent que d'autres capteurs permettant d'estimer la distance parcourue et le changement relatif de direction peuvent être exploités et que leur dispositif ne se limite pas à l'utilisation d'un accéléromètre et d'un magnétomètre. La sortie du gyroscope est intégrée pour donner l'angle qui est codé, pour 35 l'exemple présenté sur quatre bits, ce qui donne 16 valeurs possibles de l'angle, et une résolution de 22.5°. Le problème de la dérive introduite par l'intégration du gyromètre n'est pas abordé, probablement dû à l'utilisation d'un dispositif intégrant un gyroscope de grande précision. Le brevet US6323807 présente un système de navigation en intérieur ou indoor avec des capteurs "passifs". Un premier modèle statistique est construit à partir des mesures faites en des endroits connus. Un second modèle statistique est obtenu à partir de mesures en des points inconnus. Le calculateur détermine la probabilité pour qu'une position cartésienne inconnue corresponde en fait à une position connue. Les 1 o capteurs incluent un accéléromètre triaxial, un magnétomètre triaxial, un détecteur de luminosité, et un capteur de température. Il est aussi possible d'ajouter une caméra à basse résolution, un capteur d'hygrométrie, un gyroscope pour mesurer l'inclinaison, un détecteur de gaz, et un altimètre. Le système nécessite une phase d'apprentissage, avec extraction de 15 caractéristiques. Il s'apparente ensuite à un système de classification. Un quatorzième article "Gait Assessment in Parkinson's Disease: Toward an Ambulatory System for Long-Term Monitoring Arash Salarian", de Heike Russmann, François J.G. Vingerhoets, Catherine Dehollain, Yves Blanc, Pierre R. Burkhard, et Kamiar Aminian, IEEE Transactions, 20 Biomedical Engineering, Vol. 51(8), pp. 1434-1443, 2004, divulgue notamment un système de mesure de la vitesse de rotation de certains segments du corps humain au moyen de gyroscopes, pour des patients parkinsoniens. Le problème de la dérive des mesures gyrométriques est traité par l'intermédiaire d'un filtrage passe-haut préliminaire à tout autre 25 traitement. La présente invention ne nécessite pas une telle phase de prétraitement par filtrage passe-haut. En outre, la présente invention utilise des accéléromètres, ce qui n'est pas décrit dans l'article. Un quinzième article, Estimation and Visualization of Sagittal Kinematicsof Lower Limbs Orientation Using Body-Fixed Sensors Hooman 30 Dejnabadi, Brigitte M. Jolies, Emilio Casanova, Pascal Fua, Kamiar Aminian, IEEE Transactions On Biomedical Engineering, Vol. 53(7), pp. 1385-1393, JULY 2006, divulgue un gyroscope et un accéléromètre à deux axes de mesure qui sont utilisés pour estimer l'orientation d'une jambe (le capteur est fixé sur le tibia). La fusion des données issues de ces capteurs exploite un 35 modèle de la marche et des contraintes biomécaniques. Le mode de traitement des données est totalement différent de celui de la présente invention. En effet, l'approche proposée nécessite de positionner précisément le capteur sur le tibia, de l'utilisateur, ce qui n'est pas le cas de la présente invention. Les auteurs prennent en compte le biais existant dans les mesures gyrométriques. L'angle du tibia par rapport à l'horizontale lorsque le pied est fixe est obtenu directement à partir des mesures fournies par l'accéléromètre bi-axial. A partir des mesures disponibles au niveau du tibia, sont calculées les mesures correspondantes qui devraient être mesurées au niveau de la cheville. L'estimation du biais sur l'angle introduit 1 o dans le calcul de l'angle par l'intégration de la mesure fournie par le gyroscope met en oeuvre un filtrage passe-bas par un filtre de Butterworth du second ordre qui est appliqué à l'angle obtenu par intégration aux instants auxquels on ne connait pas précisément cet angle. Ensuite, on applique une spline cubique d'interpolation ou "piecewise cubic hermite interpolation" aux 15 instants connus afin de retrouver une estimation de la dérive de l'angle sans dépassement ni oscillation, tout en préservant ses propriétés de monotonicité. La dérive d'angle estimée est ensuite soustraite à l'angle obtenu par intégration pour obtenir une estimation de l'angle sans dérive. Un seizième article, "A practical gait analysis system using 20 gyroscopes", de Kaiyu Tong et Malcolm H. Granat, Medical Engineering & Physics, Vol 21, p 87-94, 1999, traite de l'analyse de la marche en utilisant des gyromètres. Un gyromètre mono-axe est placé sur le tibia d'un utilisateur, à proximité de la cheville. L'inclinaison du tibia est obtenue par intégration des mesures fournies par le gyroscope. Le biais initial du 25 gyromètre, est obtenu en calculant la valeur moyenne du signal délivré par le gyromètre lorsque l'utilisateur est au repos, sur une fenêtre temporelle de cinq secondes. Des hypothèses sur une orientation particulière des segments de la jambe à un moment particulier du pas à savoir le milieu de la phase d'appui ou "mid-stance" en langue anglaise, permettent de remettre à 30 zéro l'angle, ce qui permet d'annuler la dérive d'angle à la fin du pas. Une autre méthode testée consiste à appliquer un filtrage passe-haut sur les signaux d'inclinaison du tibia pour limiter la dérive et enlever le décalage. Cet article est axé sur une estimation d'angles et non sur une estimation de la trajectoire. Ainsi la correction de la dérive d'angle ne tient pas compte de la 35 dérive sur la trajctoire.

Un dix-septième article, "Three dimensional inertial sensing of foot movements for automatic tuning of a two-channe) implantable drop-foot stimulator " de P.H. Veltink, P. Slycke, J. Hemssems, R. Buschman, G. Bultstra, et H. Hermens, Medical Engineering & Physics, Vol. 25, p 21-28, 2003, s'intéresse aux mouvements en trois dimensions du pied en vue d'implanter un stimulateur. La position cartésienne et l'orientation ou position angulaire du pied sont estimées à partir de mesures accélérométriques et magnétométriques triaxiales. Les mesures fournies par les gyromètres sont intégrées pour déterminer l'orientation, et la position est obtenue par la 1 o double intégration des accélérations ramenées dans le repère inertiel. Il est divulgué que ces phases d'intégration peuvent conduire à un résultat erroné pour l'estimation de l'orientation et de la position à cause de décalages mal connus et de gains mal estimés. Il est également décrit que pour la marche d'un être humain, des conditions initiales et finales particulières peuvent être 15 prises en compte, par exemple, un pied posé à plat sur le sol, une position verticale identique au début et à la fin d'un pas. En outre la durée d'intégration est limitée, sur la durée de un pas. La démarche proposée pour estimer la position et l'orientation du pied en trois dimensions est la suivante : l'orientation du capteur sur le pied est déterminée initialement, lors d'une 20 phase statique durant laquelle la personne ne bouge pas. Cela permet de mesurer le champ de gravité dans le repère du capteur. Un algorithme de détection de pas permet de définir le début et la fin d'un pas. A la fin de chaque pas, la mesure de vitesse angulaire est intégrée et la dérive est compensée en utilisant les conditions sur l'angle au début et la fin du pas. 25 L'orientation obtenue, en fonction du temps, est ensuite utilisée pour exprimer l'accélération dans le repère inertiel. Le champ de gravité est ensuite soustrait, ce qui fournit l'accélération propre. Cette dernière est intégrée deux fois pour déterminer la position, et les conditions sur le début et la fin du pas sont exploitées pour éliminer la dérive de position. Cette 30 démarche de correction de la dérive, illustrée par la figure 1 du dix-septième article, est totalement différente de celle de la présente invention. Un dix-huitième article "Self-contained Position Tracking of Human Movement Using Small Inertial/Magnetic Sensor Modules" de Yun Xiaoping, E.R. Bachmann, H. Moore, et J. Calusdian, IEEE International Conference 35 on Robotics and Automation, 2007, présente un système de navigation personnel ne nécessitant pas d'infrastructure particulière. La poursuite de la posture est obtenue par l'intermédiaire d'un ensemble de capteurs inertiels/magnétiques fixés sur les membres de la personne en déplacement. Les modules de capteurs considérés contiennent trois gyromètres, trois accéléromètres, et trois magnétomètres (ces capteurs sont triaxiaux et chacun des triaxes est monté à angle droit). Les auteurs notent la dérive introduite par les accéléromètres : ils explotent alors les propriétés de la marche (i.e. phases à vitesse nulle) afin de corriger les mesures délivrées par les accéléromètres. Le passage du repère mobile au repère fixe est 1 o réalisé en utilisant l'attitude (donnée sous la forme d'un quaternion) fournie directement par le module de capteurs, ce dernier intégrant un filtre de Kalman. La description ne donne pas d'information précise sur les capteurs utilisés pour calculer cette orentation. Un dix-neuvième article, "Spatio-temporal parameters of gait 15 measured by an ambulatory system using miniature gyroscopes", par K. Aminian, B. Najafia, C. Bülab, P. -F. Leyvrazc et Ph. Robert, dans Journal of Biomechanics, Vol. 35(5), pp. 689-699, 2002, décrit un système d'acquisition de données pour l'estimation des paramètres spatio-temporels de la marche. Pour cela, les auteurs exploitent la mesure des vitesses angulaires au niveau 20 des membres inférieurs. Le dispositif de mesure exploite des gyromètres et des capteurs de force pour la validation. L'estimation des paramètres est faite par l'intermédiaire de la transformée en ondelettes. On est donc ici très loin d'un système de navigation tel que celui proposé dans la présente invention. En outre, les auteurs n'abordent pas la dérive des gyromètres 25 puisqu'ils traitent de données prétraitées par la transformée en ondelette. Ces différents systèmes ont une précision limitée et un coût élevé. Ils ne proposent pas de résoudre le problème de la dérive de l'orientation de manière globale en tenant compte d'informations sur la trajctoire. Ils nécessitent souvent un nombre de capteurs supérieur à celui de la présente 30 invention. Il est proposé, selon un aspect de l'invention, un système d'estimation de trajectoire d'un élément ou d'un corps mobile, comprenant un ensemble capteur comprenant un accéléromètre et un gyromètre liés de manière solidaire. Le système comprend des moyens de correction 35 rétroactive d'une position angulaire obtenue à partir des mesures du gyromètre à au moins un instant parmi une pluralité d'instants successifs d'un intervalle de temps à partir de positions cartésiennes de l'accéléromètre à au moins deux desdits instants successifs et d'informations prédéterminées représentatives de la trajectoire de l'accéléromètre durant ledit intervalle de temps, et à partir d'une rotation adaptée pour minimiser un écart entre lesdites informations prédéterminées et leur équivalent sur la trajectoire obtenue après rotation. Les informations prédéterminées peuvent être relatives à l'orientation de la trajectoire. Cette orientation peut être calculée à partir de la trajectoire estimée. Par exemple, si l'information prédéterminée est l'orientation de la direction du pas par rapport au Nord (ou cap de la marche), alors son équivalent sur la trajectoire est calculé par exemple en prenant l'angle entre la direction Nord-Sud et la projection sur le plan horizontal du vecteur reliant le premier et le dernier point de la trajectoire du pas.

Un tel système permet de compenser la dérive en agissant sur les angles estimés en intégrant les mesures du gyromètre à partir de la trajectoire obtenue. Cela permet de gérer le problème de la dérive de manière globale. En outre, l'objet de la présente invention est d'obtenir une correction de trajectoire sans utiliser des données de mesure issues du magnétomètre. La correction est basée uniquement sur les mesures issues de l'accéléromètre et du gyromètre. Dans la présente invention, l'attitude du repère mobile est obtenu via l'intégration numérique des mesures gyrométriques, et la dérive est compensée non sur les mesures accélérométriques, mais sur l'attitude estimée, en tenant compte d'hypothèses sur la trajectoire, telle que par exemple, le déplacement du piéton dans un plan horizontal. L'intervalle de temps peut correspondre à une phase de vol d'un pied.

Dans un mode de réalisation, le système comprend, en outre : - des moyens de mémorisation de premières mesures délivrées par l'accéléromètre et de deuxièmes mesures délivrées par le gyromètre à ladite pluralité d'instants successifs dudit intervalle de temps, dans un repère mobile lié à l'ensemble capteur ; - des premiers moyens de détermination de ladite position angulaire du gyromètre définie par des angles de rotation par rapport à des positions de référence selon les axes d'un repère global fixe lié au repère terrestre, audits instants successifs, à partir desdites deuxièmes mesures ; et - des deuxièmes moyens de détermination de ladite position cartésienne de l'accéléromètre, dans ledit repère global fixe, auxdits instants successifs, à partir desdites premières mesures et de ladite position angulaire fournie par lesdits premiers moyens de détermination. Ainsi, la trajectoire peut être obtenue et la dérive en position peut 1 o être corrigée.

Plus particulièrement, le système de l'invention est apte à estimer une trajectoire d'un élément ou d'un corps mobile, en utilisant : o l'accéléromètre (A) apte à délivrer des mesures de données, le 15 gyromètre (G) apte à délivrer des mesures de vitesses angulaires, o des premiers moyens (DET_POS_A) d'estimation de la position angulaire (Position_A) à partir de la vitesse angulaire mesurée par le gyromètre (G), à au moins deux instants parmi une 20 pluralité d'instants successifs (t', t2, ... tN 1, tN) d'un intervalle de temps (kl;tz]), o des deuxièmes moyens (DET_POS_C) d'estimation de la trajectoire de l'élément ou du corps mobile comprenant, à au moins deux desdits instants successifs (t', t2, ... tN 1, tN), une 25 position cartésienne (Position_C) de l'accéléromètre (A), à partir des mesures de cet accéléromètre (A), et à partir de la position angulaire précédemment estimée, et o des troisièmes moyens d'estimation d'une rotation adaptée pour minimiser un écart entre des informations prédéterminées 30 (INF_PRED_TRAJ) représentatives de la trajectoire à estimer et leur équivalent sur la trajectoire tournée, obtenue après rotation de la trajectoire précédemment estimée o des moyens de correction rétroactive de la position angulaire (CORR POS A) auxdits au moins deux instants estimée par les premiers moyens d'estimation, à partir des résultats de la rotation estimée par les troisièmes moyens d'estimation, o des quatrièmes moyens d'estimation de la trajectoire corrigée de l'élément ou du corps mobile comprenant une position cartésienne corrigée de l'accéléromètre (A) à au moins deux desdits instants successifs (t', t2, ... Cl, tN), à partir des mesures de cet accéléromètre (A), et à partir de la position angulaire corrigée.

1 o Selon un mode de réalisation, lesdits moyens de correction sont adaptés pour corriger itérativement ladite position angulaire. Ainsi, la précision est améliorée.

Dans un mode de réalisation, lesdites informations 15 prédéterminées représentatives de la trajectoire de l'accéléromètre comprennent une trajectoire sensiblement planaire de l'accéléromètre entre un premier instant et un deuxième instant différents, parmi lesdits instants successifs dudit intervalle de temps. Ainsi, est prise en compte, une caractéristique communément 20 rencontrée dans les mouvements, notamment humains ou animaux ou robots, et ainsi la précision est améliorée. Par exemple le mouvement d'une cheville durant la marche est sensiblement planaire. Dans un mode de réalisation, l'orientation du plan de la trajectoire 25 est connue par rapport à la verticale ou au nord. Ainsi, la précision est améliorée. Selon un mode de réalisation, la trajectoire entre lesdits premier et deuxième instants comprend un cap connu et/ou une pente connue. Ainsi, il est possible de prendre en compte des informations 30 mémorisées concernant la pente du lieu où se trouve l'utilisateur ou le cap du déplacement (lors d'un déplacement dans un couloir par exemple), pour lesquels la position angulaire revient régulièrement à la même valeur. Dans un mode de réalisation, lesdits premier et deuxième instants sont les bornes dudit intervalle de temps.

Ainsi, dans le cas de mouvements cycliques, l'intervalle de temps correspond à un cycle ou à une partie du cycle (par exemple, dans le cas de la marche, l'intervalle de temps correspond à la phase séparant deux instants successifs de vitesse nulle d'un pied).

Par ailleurs, si l'intervalle de temps correspond à une phase de mouvement encadrée par deux phases d'immobilité (ou "MidStance" en langue anglaise dans le cas de la marche) comme dans le cas de la marche, l'hypothèse de mouvement planaire concerne tout le mouvement durant un pas. 1 o Selon un mode de réalisation, lesdites informations prédéterminées représentatives de la trajectoire de l'accéléromètre comprennent des positions cartésiennes possibles mémorisées. Ainsi, le calcul de la rotation de la trajectoire prend en compte ces points possibles de passage. 15 Dans un mode de réalisation, lesdits moyens de correction rétroactive sont adaptés pour répartir de manière prédéterminée la correction sur tout ou partie de ladite pluralité d'instants successifs. Par exemple, la correction est linéaire, voire équirépartie. Ainsi, la correction n'est pas la même à chaque instant et on peut 20 tenir compte de l'évolution temporelle possible de la dérive. Par exemple, lesdits moyens de correction sont adaptés pour faire croître linéairement la correction sur tout ou partie de ladite pluralité d'instants successifs. Une répartition linéaire de la correction permet de ne pas avoir d'a 25 priori sur les instants auxquels la correction doit être apportée, mais de tenir compte du fait que cette correction augmente au cours du temps ce qui est le cas lorsque le gyromètre possède un biais. Dans un mode de réalisation, le système est adapté pour être fixé à un utilisateur, à un terminal mobile, ou à un véhicule terrestre, aérien ou 30 maritime. Un utilisateur peut être vivant (humain ou animal), ou artificiel. Ainsi, la trajectoire du lieu où est fixé le système peut être estimée. Selon un mode de réalisation, le système comprend des moyens 35 de fixation à proximité d'une cheville d'un utilisateur.

Il est également proposé, selon un autre aspect de l'invention, un procédé d'estimation de trajectoire d'un élément ou d'un corps mobile, dans lequel on corrige de manière rétroactive, une position angulaire, à partir des résultats des étapes successives suivantes consistant à : - estimer une position angulaire obtenue à partir des mesures d'un gyromètre, à au moins un instant parmi une pluralité d'instants successifs d'un intervalle de temps, - estimer une position cartésienne d'un accéléromètre lié de manière solidaire au gyromètre à au moins un desdits instants successifs à partir des 1 o mesures de l'accéléromètre, et d'informations prédéterminées représentatives de la trajectoire de l'accéléromètre durant ledit intervalle de temps, et - estimer une rotation adaptée pour minimiser un écart entre lesdites informations prédéterminées et leur équivalent sur la trajectoire obtenue 15 après rotation.

L'invention sera mieux comprise à l'étude de quelques modes de réalisation décrits à titre d'exemples nullement limitatifs et illustrés par les dessins annexés sur lesquels : 20 - la figure 1 illustre un mode de réalisation d'un système selon un aspect de l'invention ; - les figures 2a et 2b illustrent un exemple liée à la marche d'un piéton ; - la figure 3 représente les repères global fixe NF et local BF ; - la figure 4 illustre les trajectoires dans les repères global fixe NF et 25 NF' - la figure 5 illustre la correction rétroactive de l'orientation ou position angulaire ; - les figures 6a, 6b et 6c illustrent les trajectoires estimées sans corriger la dérive (6a), en faisant une correction rétroactive de la dérive mais 30 en appliquant une correction angulaire constante pendant la durée de chaque pas (6b), et en faisant une correction récursive et une correction angulaire rétroactive linéaire (6c) ; et - les figures 7a et 7b illustrent les projections sur le plan horizontal des trajectoires des figures 6b et 6c : les courbes en trait gras représentent un aller et retour estimé, les courbes en traits fins représentent le trajet réellement réalisé.

Dans l'ensemble de figures, les éléments ayants les mêmes 5 références sont similaires. La présente invention propose, dès qu'un pas est détecté (grâce à un algorithme de détection de pas qui permet de déterminer le début et la fin d'un pas), d'intégrer les mesures du gyromètre pour déterminer une estimation de l'orientation, de ramener les mesures de l'accéléromètre dans 10 le repère inertiel, de compenser le champ de gravité et d'intégrer deux fois l'accélération propre obtenue à chaque instant d'échantillonnage pour obtenir une trajctoire estimée sur la durée du pas. Les connaissances a priori sur un pas sont utilisées et combinées à la trajectoire pour venir déterminer une correction d'orientation (qui 15 minimise l'écart entre les informations prédeterminées et leur équivalent sur la trajectoire estimée) qu'on applique sur les angles estimés initialement. Ensuite, en temps différé, on réutilise les mesures fournies par l'accéléromètre, ce qui permet d'estimer la trajectoire corrigée. Si l'écart entre les informations prédéterminées et leur équivalent sur la trajectoire 20 dépasse un seuil, on recommence le cycle d'estimation d'une correction d'angle puis la réestimation de la trajectoire. Sur la figure 1, un système d'estimation de trajectoire comprend un ensemble capteur EC muni d'un accéléromètre A à au moins un axe de mesure et un gyromètre G à au moins un axe de mesure liés de manière 25 solidaire. Le système comprend un module de correction rétroactive CORR_POS_A d'une position angulaire Position_A du gyromètre G parmi une pluralité d'instants successifs, correspondant aux instants d'échantillonnage sur un intervalle de temps, à partir des positions 3o cartésiennes Position _C de l'accéléromètre A à au moins deux desdits instants successifs et d'informations prédéterminées INF_PRED_TRAJ représentatives de la trajectoire de l'accéléromètre A durant l'intervalle de temps [tl;t2] par une rotation, qui, appliquée à la trajectoire, lui permet de vérifier au mieux lesdites informations prédéterminées, i.e. de minimiser un écart entre lesdites informations prédéterminées INF_PRED_TRAJ et leur équivalent sur la trajectoire obtenue après rotation. L'intervalle de temps est délimité par une première borne t, et par une deuxième borne ultérieure t2. Les échantillons sur l'intervalle de temps 5 [tl;tz] sont indicés ti de sorte que l varie entre 1 et N, et t' vaut t,, et tN vaut t2. On définit la trajectoire comme l'ensemble des positions cartésiennes Position _C successives. Les bornes t, et t2 peuvent correspondre à deux instants successifs de vitesse nulle d'un pied, délimitant une phase entre deux 1 o instants de vitesse nulle d'un pied ou "MidStance" en langue anglaise. Le système comprend, en outre, un module de mémorisation MEM_A de premières mesures ab délivrées par l'accéléromètre A, et un module de mémorisation MEM_G de deuxièmes mesures wb délivrées par le gyromètre G à ladite pluralité d'instants successifs dudit intervalle de temps, 15 dans un repère mobile BF lié à l'ensemble capteur EC. Un premier module de détermination DET_POS_A détermine la position angulaire Position_A du gyromètre G définie par des angles ou un quaternion représentant la rotation permettant de passer du repère NF au repère BF audits instants successifs t', t2, ... tN 1, tN, à partir des deuxièmes 20 mesures wb. Un deuxième module de détermination DEL POS C détermine la position cartésienne Position_C de l'accéléromètre A, dans le repère global fixe NF, auxdits instants successifs t', t2, ... tN 1, tN, à partir des premières mesures ab et de la position angulaire Position_A fournie par le premier 25 module de détermination DET_POS_A, après un changement de repère effectué par un module de changement de repère CR permettant de passer du repère local BF au repère NF (car après itérations, NF' tend à se confondre avec NF, comme décrit ultérieurement dans la description). Le module de correction rétroactive CORR_POS_A est adapté 30 pour corriger itérativement la position angulaire Position_A. La correction peut être appliquée soit à des vitesses angulaires (i.e. avant une intégration des mesures du gyromètre G, soit appliquée à des angles (i.e. après une intégration des mesures du gyromètre G) Le module de correction rétroactive CORR_POS_A est, en outre, adapté pour corriger la position cartésienne Position_C à au moins un desdits instants successifs t', t2, t", tn, Les informations prédéterminées INF_PRED_TRAJ peuvent comprendre une information de trajectoire sensiblement planaire de l'accéléromètre entre un premier instant et un deuxième instant différents, parmi lesdits instants successifs t', t2, ... t", tN dudit intervalle de temps [tl;t2]. L'orientation du plan autour duquel se situent les points de la 1 o trajectoire peut être connue par rapport à la verticale ou au nord. La trajectoire entre lesdits premier et deuxième instants peut comprendre un cap connu et/ou une pente connue. Les premier et deuxième instants peuvent être les bornes de l'intervalle de temps. 15 Les informations prédéterminées INF_PRED_TRAJ peuvent comprendre des positions cartésiennes possibles mémorisées. Le système peut être adapté pour être fixé à un utilisateur, à un terminal mobile, ou à un véhicule terrestre, aérien ou maritime. Par exemple, il peut être muni de moyens de fixation à proximité d'une cheville d'un 20 utilisateur. La présente invention est à mettre en comparaison avec une démarche classique d'estimation de la trajectoire d'un solide à partir de mesures fournies par un gyromètre et un accéléromètre comprenant quatre étapes : 25 - intégrer les mesures gyrométriques afin d'estimer l'orientation du repère BF lié à l'ensemble capteur EC par rapport au repère de référence NF ; - ramener les mesures accélérométriques du repère BF de l'ensemble capteur EC dans le repère de référence NF ; 30 - compenser la gravité pour obtenir l'accélération propre du solide dans le repère de référence ; et - effectuer une double intégration temporelle de cette accélération. Compte tenu des biais existant sur les mesures, il est notoirement connu que cette démarche ne peut pas à long terme fournir une trajectoire correcte. Aussi, des techniques sont connues pour essayer de corriger la dérive, ou "drift" en langue anglaise, de la trajectoire. Une originalité de la présente invention vient du fait qu'à partir de connaissances sur la trajectoire (la trajectoire est définie par l'ensemble des positions cartésiennes entre deux instants donnés), on détermine la rotation qui, en tournant la trajectoire, permet de satisfaire les connaissances sur la trajectoire, de venir corriger l'orientation ou position angulaire Position_A en se servant de cette rotation puis de recalculer la trajectoire. Notons que la correction est appliquée sur une fenêtre temporelle 10 donnée. En outre, la démarche de correction peut être itérative. Prenons l'exemple de la marche illustré sur les figures 2a et 2b. A partir d'hypothèses sur le mouvement, par exemple que l'altitude au début et à la fin du pas sont identiques, on déduit la correction d'angle 6 à appliquer 15 pour que les contraintes sur la trajectoire soient satisfaites, ici l'altitude à l'instant t1 est égale à l'altitude à l'instant t2. Cette correction Pos_A_C est répartie, d'une manière prédéterminée à définir, par exemple linéairement, par une division équirépartie de la correction en la divisant par le nombre d'échantillons N, sur 20 les attitudes ou positions angulaires Position_A déterminées à partir des mesures gyrométriques. Puis la trajectoire est recalculée. On définit ci-après les repères utilisés dans cette demande de brevet ainsi que le modèle des capteurs utilisés. On suppose que les unités de mesures inertielles ou IMU pour 25 "Inertial Measurement Units" en langue anglaise, i.e. l'ensemble capteur EC, mesurent des vecteurs en trois dimensions ou 3D. Tous ces vecteurs sont mesurés dans le repère BF de l'ensemble capteur EC, ce qui signifie que les mesures dépendent de l'attitude ou orientation du capteur. 30 L'ensemble capteur EC est fixé au corps mobile. Ainsi, la mesure du vecteur de rotation instantanée est une nécessité, à cause des changements d'attitude ou position angulaire de l'ensemble capteur EC au cours du temps. Tout d'abord, il faut définir quatre repères respectivement notés 35 NF, BF, SF et NF'.

Le repère Nord ou repère global fixe, ou NF pour "North Frame" en langue anglaise, est un repère fixe par rapport au référentiel terrestre. Il est utilisé durant tout le mouvement (marche ou course). Le repère Nord NF peut être considéré comme le repère de référence : les orientations ou positions angulaires et les positions ou positions cartésiennes sont mesurées dans ce repère. Le repère global fixe NF est défini par son origine ONF et ses axes (xNF, yNF , zNF ) . Les axes se référent au repère terrestre par les directions Est, Nord, et Haut. Son origine ONF peut correspondre à l'endroit où le mouvement débute ou à tout autre endroit. Les données exprimées 1 o dans ce repère sont notées avec un exposant n. Le repère local ou lié au capteur ou BF pour "Body Frame" en langue anglaise est le référentiel de l'ensemble capteur EC dans lequel les données brutes sont mesurées. Son centre est OBF . Les données dans ce repère sont notées avec un exposant b. Ses axes (xBF, yBF, zBF ) 15 correspondent aux axes des capteurs (dans le cas de capteurs triaxiaux) à chaque instant. Les coordonnées de son origine OBF dans le repère global fixe NF correspondent à l'emplacement du capteur dans le repère global fixe NF. Un repère intermédiaire ou SF pour "Step Frame" en langue 20 anglaise est défini. Dans la présente demande, le repère intermédiaire SF est un repère lié à la Terre, mais seulement pendant un intervalle de temps [tl,t2] (par exemple au cours d'un pas). Ce repère est utilisé pour des mouvements ayant moins de six degrés de liberté ou six DOF pour "Degrees Of Freedom" en langue anglaise. Par exemple, il est utilisé quand l'ensemble 25 capteur EC a un axe de rotation invariant et/ou un mouvement plan durant l'intervalle de temps [tl,t2]. xSF est l'axe de rotation et l'ensemble capteur EC est soumis à un mouvement plan dans le plan défini par (ySF, zSF) . Chaque fois que le plan est vertical, ySF est choisi pour être l'axe horizontal, tandis que l'axe zSF est 30 l'axe vertical. L'origine de ce repère SF correspond à l'emplacement du capteur au début de l'intervalle de temps (par exemple à t=t,). Les données exprimées dans ce repère sont notées avec un exposant s. Le repère NF' est un repère proche du repère NF qui tient compte de la dérive due à l'intégration du gyromètre. En supposant cette dérive nulle NF et NF' sont confondus. Sinon, le repère NF' est défini par les axes (xNF,,, yBF,,zBF,) et le centre OBF . Le repère NF' est le repère par rapport auquel sont exprimées avant correction la position angulaire Position_A obtenue en intégrant le gyromètre et la position cartésienne Position_C estimée en double intégrant l'accélération propre. La figure 3 représente les repères global fixe NF et local BF. Comme on le voit, le repère local BF est lié au repère global fixe NF par une translation et une rotation.

La rotation entre les repères nécessaire pour obtenir les données mesurées par l'ensemble capteur EC dans le repère NF doit être calculée. Pour ce faire, est définie une matrice de rotation Rnb qui fait passer un vecteur X, exprimé dans le repère BF lié à l'ensemble capteur, au même vecteur exprimé dans le repère global fixe NF par la relation suivante: 15 xn = Rnb xb (1) La matrice de transformation inverse est Rbn = Rnb = Rnb L'orientation du repère fixe NF dans le repère local BF peut être exprimée par le quaternion unité qbn , avec qbn 2 =1. Notons que dans la suite de la description, les position angulaires 20 ou attitudes et corrections angulaires sont exprimées par des quaternions, mais de manière non limitatives, ces valeurs pourraient être exprimées différemment, par exemple des angles d'Euler. Dans la présente demande, tous les quaternions sont, par exemple, des quaternions unitaires. 25 Le quaternion unité qbn est décrit avec l'angle de rotation v autour de l'axe de rotation défini par le vecteur unitaire de direction qbn à trois dimensions ou avec un vecteur unitaire à quatre dimensions comme suit : cos(W2) - [gbn,0 gbn,l gbn,2 gbn,3 T 2

5111(~~~/Y'2)qbn On peut noter que qnb est le conjugué de qbn , et que : cos(W2) T 3 - sin(v2)qbn_ 111('~ v2)q- [gbn,0 gbn,l gbn,2 gbn,3 bn La matrice de rotation Rnb est liée aux quaternions par la relation suivante: qbn 30 gnb 2gbn,0 - 1+ 2gbn,1 2gbn,1 gbn,2 - 2gbn,0 gbn,3 2gbn,1 gbn,3 + 2gbn,0 gbn,2 Rbn = 2gbn,1 gbn,2 + 2gbn,0 gbn,3 2gbn,0 - 1+ 2gbn,2 2gbn,2 gbn,3 - 2gbn,0 gbn,l (4) 2gbn,1 gbn,3 - 2gbn,0 gbn,2 2gbn,2 gbn,3 + 2gbn,0 gbn,l 2gbn,0 - 1+ 2gbn,3 On donne maintenant le modèle des mesures fournies par les capteurs utilisés dans un mode de réalisation pour lequel l'accéléromètre et 5 le gyromètre sont triaxiaux. L'accéléromètre mesure la somme des accélérations appliquées au capteur. Sur Terre, l'accéléromètre mesure, en sus de la gravité terrestre, l'accélération propre subie par le capteur. L'accélération propre ap est 10 définie comme l'accélération externe subie par le capteur, qui est égale à la dérivée temporelle de la vitesse du capteur. Ainsi, l'accélération ab fournie par le capteur dans le repère local BF est donnée par la relation suivante : ab (t) =Rbn (t)an (t) = Rbn (t)(ap (t) - gn ) (5) Dans laquelle t représente le temps, gn = [0 0 - g]T représente le vecteur de 15 gravité terrestre avec g = 9.81 m/s 2 et ap représente l'accélération propre dans le repère global fixe NF. Notons que cette équation est purement statique, contrairement à l'équation relative aux mesures du gyromètre qui est dynamique. Par ailleurs, du bruit peut corrompre les mesures. 20 Le gyromètre fournit des mesures permettant de déterminer un quaternion d'orientation. Soit qbn (t) _ [gbn,0 (t) gbn,l (t) gbn,2 (t) gbn,3 (t)]T le quaternion variable dans le temps décrivant l'orientation du repère local BF par rapport au repère global fixe NF. Il est connu que le modèle dynamique 25 du quaternion d'orientation qbn (t) suit l'équation différentielle du premier ordre suivante : gbn (t) - n b (t)q, (t) = 2 0 - w b (t) - w (t) - w (t) (,V b (t) o b (t) - b (t) w b (t) - w (t) 0 w b (t) w b (t) w b (t) - w b (t) 0 gb,,,0 (t) q,,l (t) qbn,, (t) _gbn ,3 (t)_ (6) dans laquelle (ab = [cob (t) wb (t) wb (t)]T désigne la vitesse angulaire donnée par le gyromètre exprimée dans le repère local BF.

Pour calculer le quaternion d'orientation (unitaire) qbn (t) , le premier module de détermination DET_POS_A de la position angulaire

Position_A du gyromètre G intègre l'équation différentielle (6) à partir de qbn (0) , où qbn (0) correspond à l'orientation initiale.

Comme la fonction continue Wb (t) n'est pas connue, mais seulement un échantillonnage d'échantillonscob(l), les échantillons qbn) de quaternion d'orientation peuvent être déterminés par intégration récursive de

l'équation (6). On suppose que la période d'échantillonnage T est suffisamment 1 o petite pour supposer que Wb (t) est constant entre les indices de temps (1-1) et (l) , et que (ob (t) est égal à cab') sur cet intervalle de temps. Ainsi qbn est calculé par l'intégration de l'équation différentielle (6), à partir de qbn ~' , pendant une durée TS :

qbn) - eXp(nb(l-1) Ts )qbn l) (7)

15 II est possible de montrer que le calcul de qbn' dans la relation (7) peut être obtenu à partir du produit O associé aux quaternions, par la relation suivante : (1) = f (l-1) ~b(l-1) =q-co 0 (1-1) (p) qbn q (qbn , ) - q-cu qbn c8 dans laquelle : b(l) _ b(l) b(l) b(l) b(l) T q-w - Lq-w,0 q-w,l q-w,2 q-w,3 cos( G)b(l) T /2) - 20 cos( uob(l) Ts/2) b(l) (9) sin(w 11TS/2) - sin((.0b(l) 11T /2) qw(l) - 0Wb(l) wb(l) désigne le quaternion de rotation d'angle W«» T autour du vecteur - gb.(l) Notons que les mesures du gyromètre sont corrompues par du bruit et une dérive (ou offset). Par conséquent, l'intégration directe de l'équation (6) (ou de sa version discrétisée) conduit à une estimation corrompue du quaternion

25 d'attitude, et donc de l'attitude ou position angulaire.

On peut remarquer que dans l'état de la technique, la dérive d'orientation est généralement exprimée comme une orientation erronée du repère local BF. Comme le principal problème d'orientation est de faire un

changement de repère pour exprimer les données d'accélération du repère 30 local BF au repère global fixe NF, on considère qu'un quaternion qnb erroné ne permet pas un changement de repère du repère local BF au repère global

fixe NF, mais au repère NF' ayant dérivé, i.e. tourné progressivement. Par conséquent, NF' est défini comme un repère ayant dévié par rapport au repère global fixe NF ; il est décrit par son orientation par rapport au repère global fixe NF donnée par le quaternion qn,n. Le repère NF' dérive lentement au cours du temps et son quaternion d'orientation qn,n varie donc lentement au cours du temps de façon que ces quaternions satisfassent l'équation suivante: qbn (e)= qbn' (t> 0 qn,n (t> . Ci-suit la description d'un exemple de réalisation, illustrée avec des quaternions, appliqué au cas de la marche d'un individu. 1 o On décrit maintenant la prise en compte de l'erreur sur le champ de gravité lors du calcul des accélérations propres. Soit [tl ,t2 ] l'intervalle de temps de la phase SW de déplacement ou de vol du pied (pied en l'air) de la foulée de l'étape de détection numéro K (correspondant au pas numéro K ). On suppose que qn,n (t) est invariant 15 dans le temps au cours de chaque phase de vol SW pour te [tl ,t2 ] . Un autre problème est la dérive de la position à cause de la double intégration temporelle des mesures d'accélération. Afin de limiter la dérive de position, l'accélération est intégrée uniquement durant la phase de vol SW, tandis que pendant le temps restant, 20 la vitesse est fixée à zéro. A partir de méthodes de détection de pas, il est possible de déterminer si le pied est ou non en mouvement. La vitesse de la cheville d'un piéton est calculée pas par pas, tandis que l'orientation est déterminée continûment à l'indice de temps /avec 25 le quaternion qbn), qui détermine l'orientation du repère NF' dans le repère local BF (et non le repère global fixe NF en raison de l'intégration du bruit et de la dérive existant sur les mesures du gyromètre). En l'absence d'observation de l'orientation, il est impossible de déterminer le quaternion de rotation qnn. C'est pourquoi on n'est pas en 30 mesure de déterminer qbn . Par conséquent, on se contente de calculer l'orientation du capteur dans le repère NF' par l'intégration de mesures de la vitesse angulaire en trois dimensions : (ob = [cab ,)b ,)b 1T , (10) 35 Comme dans la relation e,=q,g-1)oet,1) (11) dans laquelle, O représente le produit de deux quaternions et eg)=[cos(oby) T/2) sin(ob(l> T/2)gbwT' (12) T est la période d'échantillonnage des capteurs A et G, et gb.=-Wb/ Wb est l'axe de la rotation défini par le quaternion qbn,.

Une fois que l'orientation du repère local BF est déterminée, les termes d'accélération sont tout simplement exprimés dans le repère NF' qui a dérivé, comme dans la relation : = Rn(i,)b (13) dans laquelle Rniti est liée à q ib comme dans la relation (4). Cette étape est 1 o effectuée par le module de changement de repère CR. Une fois qu'on connaît an , la gravité doit être compensée pour calculer l'accélération propre. Le problème principal est que la direction verticale du repère NF' est différente de celle du repère global fixe NF. 15 Même si au début on commence avec une orientation de capteur connue, l'orientation calculée dérive dans le temps. Cela signifie que la gravité terrestre gn' ne peut pas être déterminée avec précision. Il n'est pas plus acceptable de supposer que gn' = g[0 0 -UT pour déduire la gravité à partir de l'accélération an'. 20 De ce fait, la relation suivante : aP =a"+g" pan +gn (14) montre que la gravité n'est pas la même dans les deux repères. En fait, une telle approximation corromprait l'accélération propre. Par conséquent, une erreur importante serait introduite dans l'estimation de 25 la position ou position cartésienne, après la double intégration de ce biais affectant l'accélération propre. Une autre façon de déterminer gr :consiste à utiliser l'accélération an (t) au cours de l'intervalle de temps [tl ,t2 ] . Plus précisément, on a vI (tl) = vl''(t2) = 0, vI désignant le vecteur vitesse en trois dimensions 30 obtenu à partir de l'intégration temporelle de aP . En supposant que le quaternion qnn est invariant dans le temps pour t E [tl ,t2 ] , on obtient la relation suivante : K K f Kan (t)dt = ~KRn,n(ap(t)-gn)dt rl rl = Rnn f K(a;(t)-gn)dt (15) = v' (t2) - v7 (tl ) - (t2 - tl )gn,

_ -(t; -tl )gn

dans laquelle Rn,n est lié à qnn, selon la relation (4) Selon la relation (15), la valeur moyenne de an' sur l'intervalle [tl ,t2 ] est égal à la gravité gn . Ainsi, il faut centrer an avant la double 5 intégration pour compenser la gravité terrestre, comme dans la relation suivante : K ap (t) = an (t) - K f Kan (t)dt (16) tK z - ti rl En fin de compte, l'accélération est intégrée deux fois, séparément pour chaque pas, afin de calculer, après tout, la trajectoire complète de la 1 o cheville. On a le système suivant : t n, v; (t) = v; (tI )+f K ap (t)dt tl (17) pRt) = p'Rtz-1)+f tK v;'(t)dt dans lequel pÎ =[x, yÎ z1']T désigne les coordonnées cartésiennes en trois dimensions de l'ensemble capteur EC dans NF', v'Rtl) = 0 /s, ex, et p'RO) est initialisé à o,, . 15 Comme l'accélération dans le repère NF' est centrée lors de chaque pas (16), alors vin«t2 ) = 0 m/s dK . Les étapes correspondant aux relations (14) à (17) sont réalisées par le deuxième module de détermination DET_POS_C de la position cartésienne Position _C de l'accéléromètre A.

20 Ici, la position calculée peut encore être sujette à dérive à cause de qnn (t) . Pour limiter la dérive et corriger la trajectoire, il est possible d'inclure des informations supplémentaires sur les caractéristiques de la marche.

Par ailleurs, certaines caractéristiques de la trajectoire de la

25 cheville sont exploitées. Par exemple, on peut admettre que la trajectoire de la cheville est inclus dans un plan (mouvement plan). En supposant que le plan de la trajectoire du vrai pas (plan de la marche) est vertical, alors toute dérive d'orientation inclinerait le plan vertical.

Supposons que les hypothèses suivantes soient remplies, correspondant aux informations prédéterminées INF_PRED_TRAJ - le piéton marche sur un plancher horizontal: zÎ (tl) = zÎ (tz) ; et - le plan de la marche est vertical: zsF = zNF = [001]T .

Soit q ve le quaternion qui tourne la trajectoire calculée pour satisfaire au mieux les hypothèses précédentes, la figure 4 illustre le cas d'une dérive de trajectoire (pour un pas) et la trajectoire correspondante que nous supposons être la "trajectoire réelle" ou "vraie". Le quaternion q,e permet de corriger la trajectoire de l'étape. 1 o Considérons (e~',ez ,e'à les trois vecteurs qui sont liés au plan de la marche pour un pas dans le repère NF', calculé à la fin de l'étape (t = t2) , on a le système suivant : rv- rv rv rv , et - el,x e1,Y ehz T -XSnF ez=[ez;x ez;y ez;z]T_ p7'(tz)-pq'(t() (18) p'T(tn-p'l« ) eT =[et e y é3,z]T =e' Ae2 p représentant les positions cartésiennes calculée par la double intégration 15 de l'accélération propre. XSF fait référence à l'axe de rotation invariant dans le temps durant le pas, p7' est fourni par le système (17) et A désigne le produit vectoriel. Pour satisfaire les hypothèses précédentes, ces vecteurs exprimés dans le repère fixe NF peuvent être écrits selon le système suivant 20 n _ T et [e1n ,x en ez = [en 2,x 2,y en 0]T (19) e3 = ei h ez Comme on ignore le "vrai" cap, il est impossible de déterminer la valeur exacte du cap dans le repère global fixe NF. En d'autres termes, on est en mesure de corriger la trajectoire indépendamment du cap car on n'est 25 pas en mesure d'ajuster le cap de la foulée. En l'espèce, on considère dans le repère NF le même cap de foulée que celui dans le repère NF'. On peut alors écrire : n n e~,x = e2,y et (20) n n e = hy -e2 x La dérive d'orientation peut être représentée par la matrice de rotation Rc ou de façon équivalente, avec le quaternion q~ . Pour déterminer le quaternion de rotation q~ ou la matrice de rotation Rc il est uniquement nécessaire de connaître la valeur de deux vecteurs non colinéaires dans le repère NF' et leurs valeurs dans le repère global fixe NF. Notons que les vecteurs fel,e,,e3} vérifient les caractéristiques de rotation (produit scalaire et norme). On suppose également que ces vecteurs 1 o sont linéairement indépendants, sinon le quaternion de rotation n'est pas unique. Aussi, la matrice de rotation de correction Rc est donnée par la relation suivante : Rc =[ei en2 e3][ei (21) Aussi q,e peut être déduit de Rc par des calculs classiques 15 d'inversion de la formule (4). Une autre manière de calculer q~ est de déterminer l'axe de rotation qc et l'angle de rotation sans calculer la matrice de rotation. Dans ce cas, on obtient les relations suivantes : (el" -el» ) n (e~ - e~ ) q- el -el (22) 20 et yrc = sign (I [qc e;' e;''] I) atan (el,' n qc) n (el n qc) (23) ~ (el,' ^ ci, ) (el,' ^ qc) (x.y) désignant le produit scalaire de deux vecteurs, correspondant au déterminant d'une matrice carrée, et Il Il correspondant à la norme euclidienne. 25 L'angle donné par atanO dans la relation (23) est définie dans l'intervalle [0 7c] . Les étapes correspondant aux relations (18) à (23) sont réalisées par le module de correction rétroactive CORR_POS_A. n' e2 y i+ (e2,y )2 Au cours d'un déplacement d'un piéton dans un espace couvert ou "indoor" en langue anglaise, il est fréquent de pouvoir supposer une marche horizontale (ou une inclinaison fixe correspondant à des escaliers). Ainsi, on peut tourner la trajectoire calculée de manière à avoir un déplacement d'inclinaison prédéfinie, par exemple horizontal. Dans le présent scénario, en plus de l'hypothèse d'un déplacement horizontal, on impose une trajectoire de cheville sensiblement dans un plan de mouvement vertical. En plus de ces hypothèses, deux hypothèses sur l'évolution de la dérive du repère NF' par rapport au repère NF sont possibles. 1 o La première suppose que q, , (t) est invariant dans le temps pour de courtes durées, plus précisément au cours de la phase de vol de chaque pas. Ainsi, pour chaque étape, le repère NF' est supposé être lié au repère global fixe NF par une rotation constante. Si q, , (t) est invariant dans le temps pour tE [ti ,t2 , alors le quaternion q,, = qn- est déterminé pour chaque pas. 15 Ensuite, la trajectoire est recalculée avec une correction qui est appliquée pas par pas comme explicité dans le système suivant : â- (t)=RKai)«tIK) for tE[ti ,tz] v~(t)=v~(t~ )+ f t â t p(t)dt=RKv~«t) (24) K 1 Py(t)=py(tz-l)+f Î-y(t)dt dans lequel RK est la matrice de rotation de correction associée au x1ème pas. 20 Toutefois, le repère NF' dérive continuellement dans le temps et même pendant la phase de vol du pied. On suppose que l'erreur d'orientation ou de position angulaire est due à l'intégration des biais des mesures du gyromètre G au cours du temps. Ainsi, l'intégration d'un biais constant fait dériver linéairement l'orientation. 25 Par conséquent, il est commode non seulement de corriger l'orientation finale, mais de réinjecter et de distribuer cette correction tout au long de la trajectoire comme illustré sur la figure 5. Si le repère NF' dérive à cause d'un biais constant du gyromètre G, alors l'angle de la rotation donnée par qn,n croît linéairement dans le 30 temps. Si le biais du gyromètre G varie lentement dans le temps, alors les dérives de l'angle de rotation qn,n peuvent encore être considérées linéaires pour une courte durée (par exemple pendant la phase de vol du pied).

Comme le montre la figure 1, il peut être ajouté deux étapes supplémentaires par rapport à l'état de l'art :

- une première étape calcule l'orientation du quaternion de correction - une deuxième étape détermine les quaternions corrigés gbn(t) grâce à une correction, qui augmente de manière linéaire, de l'ensemble des quaternions d'orientation gbn,(t)qui détermine l'orientation du repère NF' dans le repère local BF, en commençant par le dernier indice temporel du pas précédemment corrigé te [t2-1,t2] . Ensuite, gbn(t) est 1 o calculé et la trajectoire de la cheville est recalculée. Enfin, une boucle est proposée permettant d'améliorer le calcul de qbn (t) et de sorte que la trajectoire de la cheville vérifie les contraintes ci-dessus. Pour être plus précis, on donne les équations qui sont utilisées 15 pour estimer la trajectoire corrigée. Soit q+" =qbn le quaternion de la dernière orientation "vraie" connue, à la fin du pas précédent. Puis L

échantillons de vitesse angulaire sont intégrés avant d'observer la position à la fin du pas présent, dont le quaternion associé est noté gbnL) .

Sans commande de correction, l'orientation qbn), calculée à partir 20 de l'intégration des mesures du gyromètre dérive dans le temps. Un but de l'invention est de calculer l'orientation corrigée qbn`' par

application d'une correction q l~ variant linéairement au cours du temps à L

l'orientation calculée (en l'espèce la correction croît linéairement au cours du temps), de manière à compenser la dérive de qbn), en appliquant la relation 25 suivante : qbn() = qbn 0q for le 112-1 :12 } (25) LY/c.

dans laquelle : CL) - b(l-1) 0...0 b(l2 1+1) +C12 1) qbn' - q-w q-w q (26) q [cos(Lyr~l2) -sin(L12)q~]T L Dans la relation (25), qbn est corrigé linéairement, de manière à 30 avoir une correction continue de la l'orientation au début du pas gbn(0) = qo à l'orientation à la fin du pas qbn (L) = qv O qbn' , ce qui est équivalent à gbn(L)=gbn,~q-,Vc La trajectoire est recalculée à partir de l'orientation corrigée qbn . Pour ce faire, le système suivant est d'abord calculé : an (t) = Rnb (t)ab (t) (27) ân (t) = an (t) + gn dans lequel la grandeur â (respectivement g) représente la quantité a (respectivement g ) calculée à partir de l'hypothèse de croissance linéaire de la dérive et donc de la correction linéaire. La matrice Rnbi' est obtenue à partir de q b`' comme dans la relation (4), et 10 on a la relation suivante : n - - ft -tin (t) (28) g tb -ti - Aussi, la vitesse et la position de l'ensemble capteur EC sont calculées par le système suivant : v;(t) = v~ (t~ ) + f Ka p (t)dt (29) r~ r p';(t) = p';(tz-1) + f Kv';(t)dt 15 Une fois la trajectoire de la cheville calculée, il est possible de recalculer dans une autre boucle un nouveau quaternion de correction q,e à partir de cette dernière trajectoire de la cheville. Ensuite, on effectue à nouveau une boucle pour calculer un nouveau quaternion qbn et un nouvelle position pÎ.

20 On peut remarquer que dans la relation (25), qbn`' peut être écrit de manière récursive en fonction de qbn`-l' selon la relation suivante : +(l' b(1-1' O +(1-1' (30) qbn q + (Ibn dans laquelle qb(+ désigne le quaternion de vitesse angulaire corrigé et est (31) 25 défini par la relation suivante : b(l) '» +(l) bO (l) (l) q'» = q-w q 1 = q-w qbn' q 1 qn~b auquel cas la correction est équirépartie, la relation (31) étant obtenue par la relation suivante : qbn ) qbn' ® q d LV gbw(l-1) l-1) ® q l ® q l-1 W - qbn LV _ L gbw(l-1) l-1) ® q 1 gbn'1) )~ q 1-1 (32) - gbn -LV~ - L V b(i)® (i)® ® ('i) g-w gbn' g 1 gn'b \ Lac / b(l-1) X +(1-1) g_w+ O qbn L'invention proposée est testée et comparée à une approche classique différente à partir de données expérimentales identiques. Un piéton muni d'un ensemble capteur 3A3G, pour accéléromètre triaxial et gyromètre triaxial, fixés solidairement sur sa cheville, marche à l'intérieur d'un bâtiment où le sol est supposé plan et horizontal, le plan de la marche étant supposé vertical. Les figures 7a, 7b, et 7c représentent la trajectoire en trois dimensions ou 3D de la cheville du piéton. Sur la figure 7a, aucune correction n'est effectuée, sur la figure 7b, la correction est faite avec la première hypothèse d'angle constant entre NF et NF' (par l'équation (24) et les précédentes) et sur la figure 7c, la correction est faite avec la deuxième hypothèse de variation linéaire de l'angle entre NF et NF'. Contrairement à la trajectoire calculée sans correction, qui dérive comme le montre la figure 7a, il est clair que les corrections proposées garantissent les contraintes, comme illustré sur les figures 7b et 7c. Comme explicité précédemment, le quaternion de correction q,e n'est pas en mesure de corriger la dérive de la position. On note v la vitesse 20 horizontale, et Ob la direction du déplacement par rapport au Nord biaisée donnée en utilisant la première hypothèse (i.e. dérive d'orientation invariante dans le temps). On note v la vitesse horizontale et Ob la direction du déplacement par rapport au Nord données en utilisant la seconde hypothèse (i.e. dérive d'orientation linéaire dans le temps).

25 On a les relations suivantes : Vlv7x)2+(vIy)2 q )b atan V1'y (33) \vI y / g+( 1) ®bn et v = ,x)2 +(v~ y) ~ -n atan VI y (34) vI _ [v~nx v~ y Vlnz ]T et vI = [VI x VI y v~ z ]T sont donnes respectivement dans les relations (24) et (29). Les figures 8a et 8b montrent respectivement les mêmes trajectoires que celles des figures 6b et 6c dans le plan horizontal. En l'espèce, les directions du déplacement par rapport au Nord Ob et Ob sont biaisées, mais on peut remarquer sur Ob que la correction d'orientation avec i o la deuxième hypothèse réduit clairement cette dérive Ob .

Claims (13)

1. REVENDICATIONS1. Système d'estimation d'une trajectoire d'un élément ou d'un corps mobile, comprenant un ensemble capteur (EC) comprenant un accéléromètre (A) et un gyromètre (G) liés de manière solidaire, caractérisé en ce qu'il comprend des moyens de correction rétroactive (CORR_POS_A) d'une position angulaire (Position_A) obtenue à partir des mesures du gyromètre (G) à au moins un instant parmi une pluralité d'instants successifs (t1, t2, ... tN1, tN ) d'un intervalle de temps ([tl;t2]) à partir d'une position cartésienne (Position_C) de l'accéléromètre (A) à au moins un desdits instants successifs et d'informations prédéterminées (INF PRED TRAJ) représentatives de la trajectoire de l'accéléromètre (A) durant ledit intervalle de temps ([tl; t2 ]), et à partir d'une rotation adaptée pour minimiser un écart entre lesdites informations prédéterminées (INF_ PRED_ TRAJ) et leur équivalent sur la trajectoire obtenue après rotation.
2. 2. Système selon la revendication 1, comprenant, en outre : - des moyens de mémorisation (MEM_A, MEM_G) de premières mesures (ab) délivrées par l'accéléromètre (A) et de deuxièmes mesures ((Job) délivrées par le gyromètre (G) à ladite pluralité d'instants successifs (t1, t2, ... tN 1, tN ) dudit intervalle de temps ([tl;t2]), dans un repère mobile (BF) lié à l'ensemble capteur (EC) ; - des premiers moyens de détermination (DET_POS_A) de ladite position angulaire (Position_A) du gyromètre (G) définie par des angles de rotation par rapport à des positions de référence selon les axes d'un repère global fixe (NF) lié au repère terrestre, audits instants successifs (t1, t2, ... tN 1, tN ), à partir desdites deuxièmes mesures ((Job) ; et - des deuxièmes moyens de détermination (DET_POS_C) de ladite position cartésienne (Position_C) de l'accéléromètre (A), dans ledit repère global fixe (NF), auxdits instants successifs (t1, t2, ... tN 1, tN ), à partir desdites premières mesures (ab) et de ladite position angulaire(Position_A) fournie par lesdits premiers moyens de détermination (DET POS A).
3. 3. Système selon la revendication 1 ou 2, dans lequel lesdits moyens de correction rétroactive (CORR_POS_A) sont adaptés pour corriger itérativement ladite position angulaire (Position_A).
4. 4. Système selon l'une des revendications 1 à 3, dans lequel lesdites informations prédéterminées (INF_PRED_TRAJ) 1 o représentatives de la trajectoire de l'accéléromètre (A) comprennent une trajectoire sensiblement planaire de l'accéléromètre entre un premier instant et un deuxième instant différents, parmi lesdits instants successifs (t1, t2, ... tN 1, tN ) dudit intervalle de temps ([tl;t2]). 15
5. 5. Système selon la revendication 4, dans lequel l'orientation du plan de la trajectoire est connue par rapport à la verticale ou au nord.
6. 6. Système selon la revendication 4 ou 5, dans lequel la 20 trajectoire entre lesdits premier et deuxième instants comprend un cap connu et/ou une pente connue.
7. 7. Système selon l'une des revendications 4 à 6, dans lequel lesdits premier et deuxième instants sont les bornes (t1, t2) dudit 25 intervalle de temps ( [tl; t2 ]).
8. 8. Système selon des revendications 1 à 7, dans lequel lesdites informations prédéterminées (INF_PRED_TRAJ) représentatives de la trajectoire de l'accéléromètre (A) comprennent des positions 30 cartésiennes (Position_C) possibles mémorisées.
9. 9. Système selon l'une des revendications précédentes, dans lequel lesdits moyens de correction rétroactive (CORR_POS_A) sont adaptés pour répartir de manière prédéterminée une correction(Pos_A_C) sur tout ou partie de ladite pluralité d'instants successifs (t1, t2, tN-1 ... , tN).
10. 10. Système selon la revendication 9, dans lequel lesdits moyens de correction sont adaptés pour faire croître linéairement la correction (Pos_A_C) sur tout ou partie de ladite pluralité d'instants successifs (t1, t2, tN-1 tN)
11. 11. Système selon l'une des revendications précédentes, 1 o adapté pour être fixé à un utilisateur, à un terminal mobile, ou à un véhicule terrestre, aérien ou maritime.
12. 12. Système selon la revendication 11, comprenant des moyens de fixation à proximité d'une cheville d'un utilisateur. 15
13. 13. Procédé d'estimation d'une trajectoire d'un élément ou d'un corps mobile, comprenant les étapes successives suivantes : - une première étape de mesure de vitesse angulaire par un gyromètre - une deuxième étape d'estimation de la position angulaire à partir de la 20 vitesse angulaire mesurée par le gyromètre (G), à au moins deux instants parmi une pluralité d'instants successifs (t1, t2, ... tN 1, tN) d'un intervalle de temps ([tl; t2 ]), - une troisième étape de mesure de données issues d'un accéléromètre lié de manière solidaire au gyromètre (G), 25 - une quatrième étape d'estimation de la trajectoire de l'élément ou du corps mobile en estimant, à au moins deux desdits instants successifs (t1, t2, tN 1, tN), une position cartésienne (Position_C) d'un accéléromètre (A), à partir des mesures de cet accéléromètre (A), en utilisant la position angulaire précédemment estimée, et, 30 - une cinquième étape d'estimation d'une rotation adaptée pour minimiser un écart entre des informations prédéterminées (INF_PRED_TRAJ) représentatives de la trajectoire à estimer et leur équivalent sur la trajectoire tournée, obtenue après rotation de la trajectoire précédemment estimée,- une sixième étape de correction rétroactive de la position angulaire auxdits au moins deux instants estimée à la deuxième étape, à partir des résultats de la rotation estimée à la cinquième étape, - une septième étape d'estimation de la trajectoire corrigée de l'élément ou du corps mobile en estimant, à au moins deux desdits instants successifs (t', t2, ... Cl, tN), une position cartésienne corrigée de l'accéléromètre (A), à partir des mesures de cet accéléromètre (A), en utilisant la position angulaire corrigée à la sixième étape.10