FR2934043A1 - Procede d'estimation ameliore de l'orientation d'un objet et centrale d'attitude mettant en oeuvre un tel procede - Google Patents

Abstract

Procédé d'estimation de l'orientation d'un objet dans l'espace à l'instant k utilisant les mesures de l'accélération totale (yA), de champ magnétique (yM) et de la vitesse de rotation (yG) dudit objet selon trois axes de l'espace, comportant les étapes : A- prétraitement (200) desdites mesures (yA, yM, yG) à un instant k pour détecter l'existence d'une perturbation dans lesdites mesures et estimer des mesures non perturbées à l'instant k, B- estimation de l'orientation (qk) à l'instant k par un observateur à partir des mesures non perturbées (yA,k, yM,k, yG,k) à l'instant k issues de l'étape A.

Description

PROCÉDÉ D'ESTIMATION AMELIORÉ DE L'ORIENTATION D'UN OBJET ET CENTRALE D'ATTITUDE METTANT EN OEUVRE UN TEL PROCÉDÉ

La présente invention se rapporte à un procédé d'estimation de l'orientation d'un objet dans l'espace, avec ou sans accélération propre et avec ou sans perturbation magnétique, et à un dispositif apte à permettre l'estimation de l'orientation mettant en oeuvre un tel procédé.

L'obtention de l'orientation nécessite généralement la mise en oeuvre de plusieurs capteurs, faisant partie d'un ensemble désigné par dispositif de capture de mouvement, également désigné par centrale d'attitude. Des capteurs MEMS ( Micro-Electro-Mechanical Systems ou microsystèmes électroniques) peuvent être utilisés pour constituer cette centrale, ceux-ci présentent l'avantage d'être peu encombrants et de coût réduit. L'emploi de tels capteurs MEMS permet d'envisager l'utilisation de centrales d'attitude dans des domaines d'applications variés, notamment le domaine biomédical, pour la surveillance des personnes âgées à domicile, la rééducation fonctionnelle, dans le domaine sportif, pour l'analyse des mouvements des sportifs, dans les domaines automobile, robotique, de la réalité virtuelle, de l'animation en trois dimensions et plus généralement dans tout domaine dans lequel on cherche à déterminer ou à observer un mouvement. Cependant, par rapport à des capteurs non MEMS (utilisés par exemple, 20 dans le domaine de la navigation), ces capteurs MEMS ont pour inconvénients d'être relativement bruités et biaisés. Par ailleurs, il est connu qu'il existe des centrales d'attitude utilisant conjointement des accéléromètres et des magnétomètres, qui permettent de reconstruire les mouvements à trois degrés de liberté, c'est-à-dire les 25 mouvements dont les accélérations propres et les perturbations magnétiques sont respectivement négligeables par rapport au champ de gravité terrestre et au champ magnétique terrestre. Par contre, lorsque cette hypothèse n'est pas respectée, c'est-à-dire que l'on ne peut négliger l'accélération propre ou les perturbations magnétiques, les mouvements présentent six ou neuf degrés de liberté. II est alors impossible, à l'aide d'une centrale d'attitude utilisant uniquement des accéléromètres et magnétomètres d'estimer l'orientation de l'objet en mouvement. Or, la diversification des applications de la capture de mouvements impose de s'affranchir de ces contraintes. Il a alors été envisagé d'utiliser des capteurs supplémentaires, plus particulièrement d'utiliser conjointement des gyromètres, des accéléromètres et des magnétomètres. Les mesures issues de ces capteurs sont composées de deux parties : une partie informative directement liée à l'orientation de l'objet en mouvement et une partie perturbatrice dont la nature dépend du capteur considéré. Au premier ordre, il s'agit des accélérations propres pour les mesures fournies par les accéléromètres, des perturbations magnétiques pour les mesures délivrées par les magnétomètres et du biais pour les gyromètres. Ces perturbations entraînent une estimation erronée de l'orientation. Actuellement il existe plusieurs méthodes pour obtenir, à partir de mesures fournies par des accéléromètres, des magnétomètres et des gyromètres, une estimation de l'orientation de l'objet. II existe des méthodes, dites d'optimisation, mettant en oeuvre un ou plusieurs critères d'optimisation, or celles-ci sont relativement coûteuses en temps de calcul. De plus, lorsque le problème devient complexe, la définition du ou des critères d'optimisation est difficile. Il existe également des méthodes mettant en oeuvre des réseaux de neurones, ceux-ci nécessitent une phase d'apprentissage conséquente, notamment vis-à-vis de la taille de la base de données et du temps de calcul, pour obtenir une estimation précise. Par ailleurs, les méthodes d'optimisation et celles mettant en oeuvre des réseaux de neurones prennent difficilement en compte la notion d'évolution temporelle des états, ce qui les rends moins robustes.

II existe également des méthodes mettant en oeuvre un observateur, qui, contrairement aux méthodes précédemment citées, permettent de fusionner une information double : celle provenant des mesures fournies par les capteurs et celle provenant d'un modèle d'évolution, et ceci tout en maintenant un temps de calcul compatible avec une implémentation en temps réel.

Les méthodes connues mettant en oeuvre un observateur reposent principalement sur l'utilisation d'un filtre de Kalman. L'avantage de cette technique est de permettre la fusion des données tout en tenant compte de la qualité de l'information apportée par les mesures fournies par les capteurs et de la qualité du modèle. ~o Plusieurs types de filtre de Kalman existent et sont bien connus de l'homme du métier : - le filtre de Kalman étendu ( Extended Kalman Filter ou EKF en terminologie anglo-saxonne), celui-ci est rapide et simple à mettre en oeuvre, une de ses applications à la capture de mouvement est notamment décrite 15 dans le document Quaternion-based extended Kalman filter for determination orientation by inertial and magnetic sensing , SABATINI A.M., IEEE Transactions on Biomedical Engineering, 2006, 53(7). - le filtre de Kalman UKF ( l'Unscented Kalman Filter en terminologie anglo-saxonne) est dédié aux problèmes fortement non-linéaires. Or dans le 20 cadre de la capture de mouvement, les problèmes rencontrés sont faiblement non-linéaires ; par conséquent il convient moins à l'estimation de l'orientation. Par ailleurs, le coût de calcul est fortement augmenté par rapport à un filtre EKF. Son intérêt est donc réduit par rapport à un filtre EKF. Par exemple, le document "Portable orientation estimation device based on 25 accelerometers, magnetometers and gyroscope sensors for sensor network" de HARADA T., UCHINO H., MORI T., SATO T. IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems, 2003, décrit un dispositif portable d'estimation de l'orientation dans lequel est effectuée la fusion de l'orientation absolue au moyen d'un accéléromètre et d'un 30 magnétomètre et de la vitesse de rotation au moyen d'un filtre UKF. - le filtre complémentaire de Kalman : dans ce cas de figure, le but est d'estimer l'erreur sur les états et non plus les états eux-mêmes, sa mise en oeuvre est très complexe. Outre le choix du filtre, la qualité des mesures injectées dans le filtre a une grande importance, et notamment la confiance qu'on accorde à leur valeur. En effet, comme cela a été mentionné précédemment, les mesures comportent une partie informative directement liée à l'orientation de l'objet en mouvement et une partie perturbatrice dont la nature dépend du capteur considéré. Au premier ordre, il s'agit des accélérations propres pour les mesures fournies par les accéléromètres, des perturbations magnétiques pour les mesures délivrées par le magnétomètre et le biais pour les gyromètres. Il faut également prendre en compte le bruit de mesure, cependant celui-ci est classiquement traité dans le filtre. Il existe actuellement plusieurs méthodes pour traiter les perturbations.

L'une d'elles consiste à considérer comme négligeables les perturbations et de fournir au filtre les mesures telles que délivrées par les capteurs, comme cela est le cas dans les documents Design, implementation and experimental results of a quaternion-based Kalman filter for human body motion tracking , YUN X., BACHMANN E.R.IEEE Transactions On Robotics, 2006, 22(6), et Application of MIMU/Magnetometer integrated system on the attitude determination of micro satellite , SU K., REN D.H., YOU Z., ZHOU Q., International Conference on Intelligent Mechatronics and Automation, August 2004, Chengdu, China. Par conséquent, dans le cas où une perturbation se produit effectivement sur l'un des capteurs, la mesure fournie au filtre est faussée mais celui-ci va la considérer comme exacte. L'estimation de l'orientation devient alors erronée. II n'est donc pas possible de négliger les perturbations pour obtenir une estimation avec une précision souhaitée, ceci quelque soit le nombre de degrés de liberté. Il y a donc une forte dégradation des performances, puisque les mesures perturbées sont fournies telles qu'elles à l'observateur.

Des méthodes d'estimation prévoient alors de tenir compte de l'imperfection ponctuelle de certaines mesures en détectant la présence de perturbations et en faisant évoluer la confiance accordée à ces mesures, ceci est par exemple décrit dans le document Portable orientation estimation de vice based on accelerometers, magnetometers and gyroscope sensors for sensor network , HARADA T., UCHINO H., MORI T., SATO T. IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems, 2003. Ce procédé prévoit une étape supplémentaire de détection de la perturbation dans les mesures. Lorsqu'une perturbation est détectée, la confiance dans la mesure correspondante est minimisée. L'information apportée par la mesure comportant une perturbation n'est donc pas prise en compte pour l'estimation de l'orientation. L'estimation de l'orientation ne repose alors que sur les mesures fournies par les autres capteurs. Or, dans le cas où les mesures de plusieurs capteurs présentent simultanément une perturbation, l'observateur n'a plus assez d'informations pour proposer une estimation correcte de l'orientation. Il existe enfin un dernier procédé décrit dans les documents Inertial and magnetic sensing of human motion , ROETENBERG D., Thèse de doctorat, université Twente, Pays-Bas, 2006, et Measuring orientation of human body segment using miniature gyroscopes and accelerometers , PhD Thesis, Inertial sensing of human movement, LUINGE H.J.n 2002b, dans lesquels on prévoit de détecter la présence de perturbations et de les estimer, grâce à l'observateur. Pour cela, le vecteur d'état est augmenté et les perturbations interviennent dans le modèle de mesure qui devient plus proche de la réalité. Cette technique semble, sur le principe, adaptée au cas des mouvements à six ou neuf degrés de liberté. Toutefois, l'estimation conjointe des perturbations et de l'orientation s'avère délicate du fait d'un manque d'observabilité. Elle nécessite en outre le réglage d'un grand nombre de paramètres, ce qui complexifie sa mise en oeuvre.

C'est par conséquent un but de la présente invention d'offrir un procédé d'estimation de l'orientation fournissant une estimation précise de l'orientation, en présence ou non d'accélérations propres et de perturbations magnétiques, et ceci de manière simplifiée par rapport aux procédés existants.

Le but énoncé précédemment est atteint par un procédé d'estimation de l'orientation sur la base de mesures selon les trois axes de l'espace de l'accélération, du champ magnétique et de la vitesse de rotation, comportant : - une étape de prétraitement de ces mesures pour détecter l'existence 10 d'une perturbation et estimer des mesures non perturbées, et - une étape d'estimation de l'orientation sur la base des valeurs de mesures issues de l'étape de prétraitement. Le procédé ne néglige pas les perturbations, ce qui ne fausse pas l'estimation ; il les estime en permanence. Si elles existent, il ne rejette pas la 15 ou les mesures associées, comme c'est le cas dans d'autres procédés d'estimation. De plus, il ne les intègre pas dans le vecteur d'état ou dans le modèle de mesure, ce qui simplifie le modèle et ne conduit pas à des situations où l'estimation devient impossible. On prévoit donc d'estimer l'orientation, et éventuellement d'estimer les 20 perturbations, en deux étapes successives. On apporte donc à l'observateur des mesures des accéléromètres, magnétomètres et gyromètres les plus proches possibles de conditions idéales pour l'estimation de l'orientation : c'est-à-dire sans accélérations propres, sans perturbations magnétiques et sans biais respectivement. 25 Pour cela, on prévoit d'utiliser l'orientation estimée à l'instant précédent comme information supplémentaire pour effectuer le prétraitement des mesures. Le procédé d'estimation selon l'invention permet donc d'extraire des mesures des capteurs, l'orientation de l'objet de manière optimale, quelque soit le 30 mouvement considéré. Cette méthode est par ailleurs simple de mise en oeuvre et ne comporte qu'un faible nombre de paramètres de réglage.

L'observateur est avantageusement un filtre de Kalman étendu. On peut prévoir d'estimer les perturbations, notamment les accélérations propres, ce qui permet par intégration et double intégration de remonter à la vitesse et à la position de l'objet respectivement.

La présente invention a alors principalement pour objet un procédé d'estimation de l'orientation d'un objet dans l'espace à l'instant k utilisant les mesures de l'accélération totale, de champ magnétique et de la vitesse de rotation dudit objet selon trois axes de l'espace, comportant les étapes : A-prétraitement des dites mesures à un instant k pour détecter l'existence ~o d'une perturbation dans lesdites mesures et estimer des mesures non perturbées à l'instant k, B- estimation de l'orientation à l'instant k par un observateur à partir des mesures non perturbées estimées issues de l'étape A. L'étape A comporte avantageusement : 15 Alû un prétraitement des mesures de vitesse de rotation, A2û une détection de l'existence ou non d'une perturbation à l'instant k dans lesdites mesures de l'accélération totale et de champ magnétique, A3- en cas d'absence de perturbation à l'instant k, la mesure non perturbée estimée à l'instant k est égale à la mesure à l'instant k. En cas de 20 perturbation, la mesure non perturbée estimée à l'instant k est calculée sur la base de l'orientation estimée à l'instant précédent k-1. L'étape Al consiste à retrancher aux mesures de vitesse de rotation un biais moyen déterminé lors d'une étape préalable d'initialisation. Ce biais moyen peut être obtenu par immobilisation des moyens fournissant les mesures de 25 vitesse de rotation pendant un temps donné et calcul de la moyenne des valeurs des mesures de vitesse de rotation sur chaque axe. Dans le cas d'une centrale d'attitude portée par une personne, cette immobilisation consiste à retirer la centrale de la personne pour s'affranchir des tremblements inévitables de la personne. 30 L'étape A2 de prétraitement des mesures d'accélération et de champ magnétique peut comporter : - une étape A2.1 consistant en un test de comparaison de la norme des mesures de l'accélération totale à celle du champ gravitationnel, si la valeur absolue de la différence entre la norme des mesures accélérométriques à l'instant k et celle du champ gravitationnel est inférieure à un seuil prédéterminé, on considère que la perturbation accélérométrique est nulle, sinon on considère qu'il y a une perturbation, la perturbation étant égale, sur chaque axe, à la différence entre la mesure de l'accélération totale sur chaque axe à l'instant k et la mesure accélérométrique non perturbée estimée à l'instant k, et - une étape A2.2 consistant en un test de comparaison de la norme des mesures de champ magnétique à celle du champ magnétique terrestre, si la valeur absolue de la différence entre la norme des mesures de champ magnétique et celle du champ magnétique terrestre est inférieure à un seuil prédéterminé, on considère que la perturbation magnétique est nulle, sinon on considère que la perturbation magnétique est égale, sur chaque axe, à la différence entre la mesure de champ magnétique sur chaque axe à l'instant k et la mesure de champ magnétique non perturbée estimée sur chaque axe à l'instant k. On prévoit avantageusement à l'étape A2.1 un test supplémentaire sur la perturbation estimée à l'instant k-1 : dans le cas où la valeur absolue de la différence entre la norme des mesures de l'accélération totale et celle du champ gravitationnel à l'instant k est inférieure au seuil prédéterminé, on vérifie si la norme de la perturbation accélérométrique estimée à l'instant k-1 est inférieure à un seuil prédéterminé, si ce test est positif, on considère que la perturbation accélérométrique est effectivement nulle à l'instant k, et/ou on prévoit à l'étape A2.2 un test supplémentaire sur la perturbation magnétique estimée à l'instant k-1 : dans le cas où la valeur absolue de la différence entre la norme des mesures de champ magnétique et celle du champ magnétique terrestre est inférieure au seuil prédéterminé, on vérifie si la valeur absolue de la perturbation magnétique estimée à l'instant k-1 est inférieure à un seuil prédéterminé, si ce test est positif, on considère que la perturbation magnétique est effectivement nulle à l'instant k. Cette étape supplémentaire permet d'améliorer la précision du procédé d'estimation. L'observateur utilisé dans l'étape B est de préférence un filtre de Kalman étendu, qui est rapide et simple.

L'étape B d'estimation de l'orientation à partir des mesures estimées à l'instant k peut comporter : - l'estimation du vecteur d'état a priori à l'instant k à partir du vecteur d'état estimé a posteriori à l'instant k-1, - l'estimation des mesures a priori à l'instant k à partir de l'estimation a priori du vecteur d'état a priori à l'instant k, dite estimation a priori des mesures, - calcul du gain du filtre de Kalman étendu et de l'innovation en calculant la différence entre les mesures non perturbées estimées à l'instant k et les mesures estimées a priori, - calcul de l'orientation estimée à l'instant k par correction du vecteur d'état 15 estimé a priori à l'instant k par le gain et l'innovation. Le vecteur d'état utilisé dans le filtre de Kalman peut contenir les éléments de la vitesse angulaire et du quaternion d'orientation. Le vecteur d'état utilisé dans le filtre de Kalman étendu ne contient avantageusement que les éléments du quaternion d'orientation, ce qui 20 permet de simplifier la structure des modèles d'état et de mesure. La présente invention a également pour objet une centrale d'attitude comportant des moyens aptes à fournir des mesures d'accélération, des moyens de mesure de champ magnétique, et des moyens de mesure de la vitesse de rotation selon trois axes de l'espace, lesdits moyens étant 25 destinés à être solidaires en mouvement d'un objet, et des moyens d'estimation d'une orientation à l'instant k sur la base des mesures fournies par lesdits moyens de mesure, lesdits moyens d'estimation comportant : - des moyens de prétraitement des dites mesures d'accélération, de champ magnétique, et de la vitesse de rotation, lesdits moyens de prétraitement 30 étant aptes à détecter l'existence d'une perturbation dans lesdites mesures et à délivrer des mesures accélérométriques non perturbées estimées, des mesures magnétométriques non perturbées estimées, et de la vitesse de rotation non biaisée, - des moyens d'estimation de l'orientation à un instant k par un observateur à partir des mesures fournies par les moyens de prétraitement. Cet 5 observateur peut être un filtre de Kalman étendu. La centrale d'attitude selon la présente invention peut comporter également des moyens de calcul d'un biais moyen des moyens de mesure de la vitesse de rotation lors d'une étape d'initialisation de la centrale. Les moyens de prétraitement peuvent comporter des moyens de détection 10 de l'existence d'une accélération propre dans les mesures d'accélération et des moyens de détection de l'existence de perturbations magnétiques dans les mesures de champ magnétique. La centrale d'attitude selon l'invention peut également comporter des moyens d'estimation de l'accélération propre et de calcul de la vitesse et de 15 la position de l'objet. Les moyens aptes à fournir des mesures de l'accélération totale, des mesures de champ magnétique, et des mesures de la vitesse de rotation selon trois axes de l'espace sont avantageusement des capteurs MEMS. 20 La présente invention sera mieux comprise à l'aide de la description qui va suivre et des dessins en annexe, sur lesquels : - la figure 1 est un organigramme du procédé selon la présente invention à l'instant k, 25 - les figures 2A à 2C représentent des organigrammes détaillés d'une étape de prétraitement des mesures d'un accéléromètre, d'un gyromètre et d'un magnétomètre respectivement, selon la présente invention.

On souhaite obtenir l'orientation d'un objet se déplaçant dans l'espace, par 30 exemple l'orientation d'une personne. Pour cela, on utilise une centrale d'attitude comportant des capteurs aptes à fournir des mesures de l'accélération totale, de champ magnétique et de la vitesse de rotation selon les trois axes de l'espace. Les capteurs sont avantageusement des capteurs MEMS offrant un prix de revient réduit et un encombrement limité. Il peut s'agir, pour la mesure de l'accélération, par exemple d'un accéléromètre tri axe ou de trois accéléromètres mono axe fournissant une mesure sur chacun des axes. De même pour les mesures de champ magnétique, il peut s'agir d'un magnétomètre tri axe ou de trois magnétomètres mono axe. Pour la mesure de la vitesse de rotation, il peut s'agir, par exemple, de trois ~o gyromètres mono axe ou avantageusement de deux gyromètres bi axe. Les triaxes peuvent être alignés ou non, dans ce dernier cas l'orientation relative entre les axes doit être connue. Dans la suite de la description, à des fins de simplicité, nous désignerons le ou les accéléromètres, par un accéléromètre, le ou les magnétomètres par 15 un magnétomètre et le ou les gyromètres par un gyromètre. Ces capteurs sont attachés à l'objet dont on souhaite connaître l'orientation. Nous ne disposons que des mesures y, que nous modélisons par : y4 =ùR.Go +a+vA yM = R.Ho + d + vM (I) ,yG=w+b+vG 20 avec yA : mesure tri-axe de l'accélération totale fournie par l'accéléromètre, yM : mesure tri-axe du champ magnétique fournie par le magnétomètre, 25 yG : mesure tri-axe de la vitesse de rotation fournie par le gyromètre, R : matrice de rotation, Go : champ de gravité terrestre (vecteur 3x1), Ho : champ magnétique terrestre (vecteur 3x1), 30 w : vitesse angulaire, a : accélérations propres, d : perturbations magnétiques, b : biais du gyromètre, vA : bruit de mesure de l'accéléromètre, vM : bruit de mesure du magnétomètre, vG : bruit de mesure du gyromètre.

L'orientation est estimée par rapport à un repère de référence, entièrement défini par la donnée des vecteurs Go et Ho. Par exemple, 10 le repère géocentrique est défini par les vecteurs

Go (0 ;0; 1) et Ho(0.5;0; 2 )

A des fins de simplicité, nous ne distinguerons pas les mesures selon les trois directions de l'espace. Comme cela apparaît clairement au vu de la définition mathématique (I) des 15 mesures, chacune de ces mesures comporte respectivement une première partie -RG0 , R.Ho et w, qui contient l'information permettant d'obtenir une estimation de l'orientation, une deuxième partie a, d et b qui représente les perturbations éventuelles pouvant apparaître, de façon aléatoire dans les 20 mesures, et enfin une troisième partie VA ,VM, VG représentant le bruit de mesure au niveau de chaque capteur. Dans le procédé selon la présente invention, on prévoit après la collecte des mesures, une étape de prétraitement avant leur utilisation dans une étape de traitement destinée à fournir une estimation de l'orientation. 25 Sur la figure 1, on peut voir un organigramme général du procédé selon la présente invention. Dans la description du procédé qui va suivre, on prend comme exemple l'estimation de l'orientation à l'instant k, k étant un entier naturel supérieur ou égal 2.

Le procédé selon la présente invention comporte une étape 100 d'initialisation de la centrale d'attitude, une étape 200 de prétraitement des mesures fournies par les capteurs et une troisième étape de traitement 300 par l'observateur. Chacune des étapes sera décrite en détail dans la suite de la description. Avant de décrire en détail les différentes étapes du procédé selon l'invention, nous allons décrire l'observateur. De manière avantageuse, l'observateur utilisé dans l'étape de traitement des mesures est un filtre de Kalman étendu, qui est simple, robuste et rapide à mettre en oeuvre.

Ce filtre est largement connu de l'homme du métier et ne sera pas décrit de manière détaillée. Nous donnerons uniquement les expressions mathématiques du modèle d'état et du modèle de mesure. Un filtre de Kalman comporte un modèle d'état définissant l'évolution temporelle et dynamique des états, et un modèle de mesure qui permet de 15 relier les mesures de capteurs et les états. Le vecteur d'état du filtre de Kalman est composé selon une première modélisation des trois éléments de la vitesse angulaire et des quatre éléments du quaternion définissant l'orientation. Les modèles d'état et de mesure associés peuvent être respectivement : 20 =f(x)= + wr ïo. ~q. y=h(x)= i w -q G0 q q H0Oq \v ) r YG avec x : vecteur d'état 25 w : vitesse angulaire q : quaternion r : constante de temps du modèle d'évolution de la vitesse angulaire y : mesures w, : bruit de modélisation

A des fins de simplification, on identifie le quaternion vectoriel [0, Go] de dimension 4x1 avec le vecteur Go de dimension 3X1, et également pour le quaternion vectoriel [0, Ho] de dimension 4x1, le vecteur Ho étant également de dimension 3X1. De manière avantageuse, selon une deuxième modélisation, on peut utiliser un vecteur d'état ne contenant que les éléments du quaternion, celui-ci n'est plus que de dimension 4, alors qu'il est de dimension 7 dans la première modélisation. La mesure gyrométrique est alors injectée directement dans le modèle d'état et le vecteur de mesure ne contient que les mesures de l'accéléromètre et du magnétomètre. Le modèle d'état et le modèle de mesure peuvent alors s'écrire : 1 =f(x)=9= .q Yc+w (Il), et _q \ G° q\ ( + vA (III) q H° q) VM) i YA y=h(x)= GYM Cette deuxième modélisation permet de simplifier la structure des modèles d'état et de mesure puisque leur dimension est directement réduite. Par ailleurs, le nombre de paramètres de réglage, notamment les éléments des matrices de covariance du bruit de modélisation, du bruit de mesure et de l'erreur d'estimation du vecteur d'état, est également restreint, ce qui facilite la mise en oeuvre de cette méthode. Les résultats d'estimation obtenus de cette manière sont de précision similaire à ceux obtenus en employant la première modélisation. Nous allons maintenant décrire en détail les étapes 100, 200 et 300 selon la 25 présente invention. L'étape 100 d'initialisation prévoit d'estimer la perturbation moyenne du gyromètre. Cette perturbation b, qui est en fait le biais du gyromètre varie entre deux valeurs extrêmes.

Pour l'étape d'initialisation (k=1) : Soit on connaît l'accélération propre al et la perturbation magnétique di à l'instant initial, alors l'étape d'initialisation comporte la détermination du vecteur d'état xi : la vitesse angulaire est supposée nulle à l'instant initial, le quaternion est déterminé par optimisation à l'aide des mesures d'accélération et de champ magnétique corrigées des perturbations a, et di ; Soit on connaît l'orientation à l'état initial, dans ce cas on peut déduire al et di. L'étape d'initialisation 100 de la centrale à k = 1 comporte en outre : ~o - le réglage des matrices de covariance Q, R et PI respectivement liées au bruit de modélisation, au bruit de mesure et à l'erreur d'estimation du vecteur d'état initial ; - le calcul d'une estimation du biais b, que l'on note &,0,,eä: on maintient la centrale d'attitude immobile durant un temps déterminé, par exemple une 15 seconde environ, et on calcule la moyenne des valeurs de sortie du gyromètre sur chaque axe. Cette estimation du biais moyen b,,,o,e,, est, par la suite, retranchée à chaque mesure du gyromètre lors de l'étape de prétraitement 200, ce qui permet de minimiser l'influence du biais et ainsi d'améliorer la précision des résultats obtenus. 20 Cette estimation du biais moyen bn,oy,en a lieu de préférence au début de chaque acquisition. On peut également prévoir de renouveler cette estimation lors de périodes d'immobilité. Sur les figures 2A à 2C, on peut voir le détail des étapes du procédé selon l'invention. 25 Lors de l'étape 200, les mesures délivrées par les trois capteurs sont prétraitées indépendamment lors de trois étapes 210, 220 et 230. Ces trois étapes 210, 220 et 230 sont réalisées avantageusement simultanément pour augmenter la vitesse d'estimation de l'orientation. Lors de l'étape 210, représentée sur la figure 2B, a lieu le prétraitement de la 30 mesure YG,k du gyromètre. Comme indiqué ci-dessus, ce prétraitement de la mesure YG,k est obtenu en retranchant à la mesure réelle le biais moyen b,,,o,.e71, on obtient en sortie une mesure prétraitée du gyromètre à l'instant k désignée par ÿG,k . Dans la description, l'accélération est donnée en multiple de Go (champ 5 magnétique terrestre), et le champ magnétique est donné en multiple de Ho à des fins de simplification. Lors de l'étape 220, a lieu le prétraitement des mesures délivrées par l'accéléromètre YA,k à l'instant k. Cette étape 220 comporte une première sous-étape 220.1 de détection de l'existence ou non d'une perturbation, i.e. ~o d'une accélération propre a, et une deuxième sous-étape 220.2 de construction d'une mesure prétraitée de l'accélération . Ak sur la base de l'orientation estimée à l'instant précédent k-1. Lors de l'étape 220.1, pour détecter l'existence ou non d'une accélération propre a, on compare la norme de la mesure yA,k à la norme du champ 15 gravitationnel (pour rappel, on travaille en multiple de Go), on fait donc une comparaison par rapport à 1 : Si ti'.4.k -1 <aA, Avantageusement, en cas de test positif, on ajoute le test suivant : ak-I < PA 20 La comparaison de la norme de l'accélération propre estimée à l'instant k-1, âk_, , à (3A, permet avantageusement d'exclure des cas particuliers pour lesquels le premier test ne suffirait pas. En effet, on considère que si à l'instant k-1 l'accélération propre a une valeur élevée, i.e. supérieure à r3A, il est peu probable qu'à l'instant k l'accélération propre soit inférieure à 13A. aA 25 et /JA sont par exemple égaux à 0,04 et 0,2 respectivement. Ce deuxième test améliore donc la précision de l'estimation de la mesure non perturbée ÿA k et donc de l'estimation de l'orientation.

Si les deux tests ci-dessus sont positifs, on décide alors que l'accélération propre âk est nulle à l'instant k. Lors de l'étape 220.2, la mesure estimée est alors égale à YA,k et est directement utilisable par l'observateur. âk =0 YA.k = YA.k Sinon, lors de l'étape 220.2, on construit une nouvelle mesure d'accélération grâce à l'orientation estimée qk_, , estimée à l'instant précédent. La mesure accélérométrique non perturbée estimée à l'instant k s'écrit alors à l'aide du modèle de mesure : YA,k = -qk-I GO qk + On peut également en déduire une valeur de l'accélération propre âk à l'instant k (étape 220.3), ce qui permet par intégration et double intégration respectivement d'en déduire la vitesse et la position de l'objet. L'accélération propre est égale à : âk = Ya,k + qk_, Go 0 qk_, , puisque YA,k = YA,k + âk Lors de l'étape 230, similaire à l'étape 220, a lieu le prétraitement des mesures délivrées par le magnétomètre YM,k à l'instant k. Cette étape 230 comporte une première sous-étape 230.1 de détection de l'existence ou non d'une perturbation magnétique d, et une deuxième sous-étape 230.2 de construction d'une mesure prétraitée du champ magnétique ÿAf,k sur la base de l'orientation estimée à l'instant précédent kù1. Lors de l'étape 230.1, pour détecter l'existence ou non d'une perturbation magnétique d, on compare la norme de la mesure YM,k à la norme du champ magnétique (pour rappel, on travaille en multiple de Ho), on fait donc une comparaison par rapport à 1 : Si YAl ,k -1 < a%f , Avantageusement, en cas de test positif, on ajoute le test suivant : dk-I <RM La comparaison de la norme de la perturbation magnétique estimée à l'instant k-1, dk_, , à f3M, permet avantageusement d'exclure des cas particuliers pour lesquels le premier test ne suffirait pas. En effet, on considère que si à l'instant k-1 la perturbation magnétique a une valeur élevée, i.e. supérieure à [3M, il est peu probable qu'à l'instant k la perturbation magnétique soit inférieure à 13M. aM et /M sont par exemple égaux à 0,04 et 0,2 respectivement. Ce deuxième test améliore donc la précision de l'estimation de la mesure non perturbée yA, k et donc de l'estimation de l'orientation.

Si les deux tests ci-dessus sont positifs, on décide alors que la perturbation magnétique dk est nulle à l'instant k. Lors de l'étape 230.2, la mesure estimée est alors égale à yM,k et est directement utilisable par l'observateur. dk = 0 . .41.k = Ynf,k Sinon, lors de l'étape 230.2, on construit une nouvelle mesure de champ 15 magnétique grâce à l'orientation estimée qk_1, estimée à l'instant précédent. La mesure magnétométrique non perturbée estimée à l'instant k s'écrit alors à l'aide du modèle de mesure : Y,%1,k = qk-1 Ho 9k-, On peut également en déduire une valeur de la perturbation magnétique à 20 l'instant k (étape 230.3), égale à : dk = YM.k ù qk-1 0 Ho 0 qk_1 , puisque YA,,k = YM,k + dk Lors de l'étape 300, les mesures prétraitées ÿA k YM,k YG,k sont utilisées par l'observateur. On utilise par exemple un filtre de Kalman étendu sous sa forme factorisée. 25 Nous allons maintenant expliquer les étapes de calcul effectuées par le filtre.

Nous supposerons que nous utilisons la deuxième modélisation. Mais il est bien entendu que la première modélisation peut être utilisée de manière similaire. L'étape 300 comporte les étapes suivantes : a) Estimation a priori du vecteur d'état, b) Estimation a priori des mesures, c) Calcul du gain Kk du filtre de Kalman et de l'innovation Ik, d) Correction de l'état estimé a priori. Les étapes a) à d) vont être décrites en détail ci-dessous.

Lors de l'étape a), on effectue une estimation a priori du vecteur d'état à l'instant k à partir de l'estimation a posteriori du vecteur d'état à l'instant k-1. L'estimation du vecteur d'état a priori est donnée par : h =tek_,+Te.f(xk l) avec xk_, : estimation a posteriori du vecteur d'état à l'instant 15 k-1, xk : estimation a priori du vecteur d'état à l'instant k, Te : période d'échantillonnage, Lors de l'étape b), on réalise, à l'aide du modèle de mesure (III), une estimation des mesures à l'aide du vecteur d'état estimé à l'étape a) : 20 .Yk = h(Xk ) Lors de l'étape c), on calcule le gain Kk et l'innovation Ik, l'innovation est obtenue en soustrayant les mesures estimées a priori aux mesures prétraitées. On obtient : 25 Ik = _ÿAk @h1.k _yk Lors de l'étape d), on corrige l'état estimé a priori avec le gain et l'innovation. Xk = Xk + Kk.Ik Cette correction donne une estimation a posteriori de l'orientation k à l'instant k. Avantageusement, lors d'une étape e) ultérieure, on normalise le quaternion estimé q, = qk , ce qui permet d'éviter une dérive à chaque étape du calcul. qk Le procédé selon la présente invention offre l'avantage de fournir à l'observateur des mesures proches de mesures non perturbées, cohérentes avec le modèle de mesure. L'influence des perturbations sur l'estimation de l'orientation est donc fortement réduite.

II permet, en outre, d'apporter en permanence à l'observateur l'information provenant de chacun des capteurs même en présence de perturbations, en maintenant une confiance constante sur chaque mesure. En effet, l'utilisation conjointe des mesures de l'accéléromètre, du magnétomètre et du gyromètre permet de réduire l'influence des erreurs de mesure (bruit de mesure, perturbations restantes, biais gyromètre restant) sur l'orientation estimée. La méthode selon la présente invention permet d'estimer l'orientation, mais également les accélérations propres et les perturbations magnétiques à chaque pas d'échantillonnage, quel que soit le mouvement réalisé jusqu'à neuf degrés de liberté. La mise en oeuvre de ce procédé est très simple puisqu'elle repose sur l'utilisation de briques élémentaires : tests de valeurs, calculs analytiques, filtre de Kalman étendu.25

Claims (16)

1. REVENDICATIONS1. Procédé d'estimation de l'orientation d'un objet dans l'espace à l'instant k utilisant les mesures de l'accélération totale (y4, de champ magnétique (yM) et de la vitesse de rotation (YG) dudit objet selon trois axes de l'espace, comportant les étapes : A- prétraitement des dites mesures (yA, YM, YG) à un instant k pour détecter l'existence d'une perturbation dans lesdites mesures et estimer des mesures non perturbées à l'instant k, B- estimation de l'orientation à l'instant k par un observateur à partir des 10 mesures non perturbées estimées (ÿA,k , YM,k , YG,k) à l'instant k issues de l'étape A.
2. 2. Procédé selon la revendication 1, dans lequel l'étape A comporte : Alû un prétraitement des mesures de vitesse de rotation (yG), 15 A2û une détection de l'existence ou non d'une perturbation à l'instant k dans lesdites mesures de l'accélération totale et de champ magnétique (YA, YM), A3-en cas d'absence de perturbation à l'instant k, la mesure non perturbée estimée à l'instant k (YA,k , YAI,k) est égale à la mesure à 20 l'instant k, en cas de perturbation, la mesure non perturbée estimée à l'instant k (Y,, k , YM.k) est calculée sur la base de l'orientation estimée à l'instant précédent k-1.
3. 3. Procédé selon la revendication précédente, dans lequel l'étape Al 25 consiste à retrancher aux mesures de vitesse de rotation un biais moyen (b,,,o,eä) déterminé lors d'une étape préalable d'initialisation.
4. 4. Procédé selon la revendication précédente, dans lequel le biais moyen est obtenu par immobilisation des moyens fournissant les mesures de vitesse de rotation pendant un temps donné et calcul de la moyenne des valeurs des mesures de vitesse de rotation sur chaque axe.
5. 5. Procédé selon l'une des revendications 2 à 4, dans lequel l'étape A2 comporte : A2.1 - un test de comparaison de la norme des mesures de l'accélération totale à celle du champ gravitationnel (Go), si la valeur absolue de la différence entre la norme des mesures accélérométriques à l'instant k et celle du champ gravitationnel (Go) est inférieure à un seuil prédéterminé (aA), on considère que la perturbation accélérométrique est nulle, sinon on considère qu'il y a une perturbation, la perturbation étant égale, sur chaque axe, à la différence entre la mesure de l'accélération totale à l'instant k et la mesure accélérométrique non perturbée estimée à l'instant k, A2.2 - un test de comparaison de la norme des mesures de champ magnétique à celle du champ magnétique terrestre (Ho), si la valeur absolue de la différence entre la norme des mesures de champ magnétique et celle du champ magnétique terrestre est inférieure à un seuil prédéterminé (am), on considère que la perturbation magnétique est nulle, sinon on considère que la perturbation magnétique est égale, sur chaque axe, à la différence entre la mesure de champ magnétique à l'instant k et la mesure de champ magnétique non perturbée estimée à l'instant k.
6. 6. Procédé selon la revendication précédente, dans lequel on prévoit à l'étape A2.1 un test supplémentaire sur la perturbation estimée (âk_,) à l'instant k-1 : dans le cas où la valeur absolue de la différence entre la norme des mesures de l'accélération totale et celle du champ 30 gravitationnel (Go) à l'instant k est inférieure au seuil prédéterminé (aA), on vérifie si la norme de la perturbation accélérométrique estimée à l'instant k-1 est inférieure à un seuil prédéterminé ((3A), si ce test est positif, on considère que la perturbation accélérométrique est effectivement nulle à l'instant k, et/ou on prévoit à l'étape A2.2 un test supplémentaire sur la perturbation magnétique estimée à l'instant k-1 (ak_, ), dans le cas où la valeur absolue de la différence entre la norme des mesures de champ magnétique et celle du champ magnétique terrestre (Ho) est inférieure au seuil prédéterminé (am), on vérifie si la valeur absolue de la perturbation magnétique estimée à l'instant k-1 est inférieure à un seuil prédéterminé (Pm), si ce test est positif, on considère que la perturbation magnétique est effectivement nulle à l'instant k.
7. 7. Procédé selon l'une des revendications 1 à 6, dans lequel l'observateur utilisé dans l'étape B est un filtre de Kalman étendu.
8. 8. Procédé selon la revendication 7, dans lequel l'étape B d'estimation de l'orientation à partir des mesures non perturbées estimées à l'instant k comporte : l'estimation du vecteur d'état a priori à l'instant k (xk ), à partir du vecteur d'état (xk_) estimé a posteriori à l'instant k-1, - l'estimation des mesures a priori à l'instant k (vk ), à partir de l'estimation du vecteur d'état a priori à l'instant k (xk ), calcul du gain du filtre de Kalman étendu (Kk) et de l'innovation (Ik) en calculant la différence entre les mesures non perturbées estimées à l'instant k et les mesures estimées a priori (ÿk ), calcul de l'orientation estimée à l'instant k (qk) par correction du vecteur d'état estimé a priori à l'instant k par le gain et l'innovation.
9. 9. Procédé selon la revendication 8, dans lequel le vecteur d'état utilisé dans le filtre de Kalman étendu contient les éléments de la vitesse angulaire et du quaternion d'orientation.
10. 10. Procédé selon la revendication 8, dans lequel le vecteur d'état utilisé dans le filtre de Kalman étendu ne contient que les éléments du quaternion d'orientation.
11. 11. Centrale d'attitude comportant des moyens aptes à fournir des mesures d'accélération (yA), des moyens de mesure de champ magnétique (yM), et des moyens de mesure de la vitesse de rotation (yG) selon trois axes de l'espace, lesdits moyens étant destinés à être solidaires en mouvement d'un objet, et des moyens d'estimation d'une orientation à l'instant k sur la base des mesures fournies par lesdits moyens de mesure, lesdits moyens d'estimation comportant : des moyens de prétraitement des dites mesures d'accélération (yA), de champ magnétique (yM), et de la vitesse de rotation (yG), lesdits moyens de prétraitement étant aptes à détecter l'existence d'une perturbation dans lesdites mesures et à délivrer des mesures accélérométriques non perturbées estimées (.YA.k ), des mesures magnétométriques non perturbées estimées (h" ), et de la vitesse de rotation non biaisée (Y".k ) des moyens d'estimation de l'orientation à un instant k par un observateur à partir des mesures accélérométriques non perturbées estimées, des mesures magnétométriques non perturbées estimées et des mesures de vitesse de rotation non biaisées fournies par les moyens de prétraitement.
12. 12. Centrale d'attitude selon la revendication 11, comportant également des moyens de calcul d'un biais moyen (bnoy,Q1) des moyens de mesure de la vitesse de rotation lors d'une étape d'initialisation de la centrale.
13. 13. Centrale d'attitude selon la revendication 11 ou 12, dans laquelle les moyens de prétraitement comportent des moyens de détection de l'existence d'une accélération propre dans les mesures d'accélération et des moyens de détection de l'existence de perturbations magnétiques dans les mesures de champ magnétique.
14. 14. Centrale d'attitude selon l'une des revendications 11 à 13, comportant des moyens d'estimation de l'accélération propre, de la perturbation magnétique, et de calcul de la vitesse et de la position de l'objet.
15. 15. Centrale d'attitude selon l'une des revendications 11 à 14, dans laquelle l'observateur est un filtre de Kalman étendu.
16. 16. Centrale d'attitude selon l'une des revendications 11 à 15, dans laquelle les moyens aptes à fournir des mesures de l'accélération totale (yA), des mesures de champ magnétique (yM), et des mesures de la vitesse de rotation (yG) selon trois axes de l'espace sont des capteurs MEMS.25