| WO/2011/076365 | FAULT DETECTION METHODS |
| WO/1998/019176 | CAMERA GUIDE SYSTEM |
| JP2000028699 | SYSTEM AND METHOD FOR SPECIFICATION OF POSITION OF VEHICLE |
VASSILEV, Andrea (30 rue Ampère, Grenoble, Grenoble, F-38000, FR)
COMMISSARIAT A L'ENERGIE ATOMIQUE (25 rue Leblanc, Bâtiment "Le Ponant D", Paris, F-75015, FR)
BASSOMPIERE, Cindy (12 rue des Fleurs, Grenoble, Grenoble, F-38000, FR)
VASSILEV, Andrea (30 rue Ampère, Grenoble, Grenoble, F-38000, FR)
| REVENDICATIONS 1. 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 (ye) dudit objet selon trois axes de l'espace, caractérisé en ce qu'il comprend les étapes : A- prétraitement des dites mesures (VA, YM, YG) à un instant k pour détecter l'existence d'une perturbation dans lesdites mesures, ladite perturbation appartenant à un groupe comprenant une accélération propre de l'objet, un champ magnétique s'ajoutant au champ magnétique terrestre et un biais dans la mesure de la vitesse de rotation, et pour 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 {yA k , yMΛ , yGΛ ) à l'instant k issues de l'étape A. 2. Procédé selon la revendication 1 , dans lequel l'estimation de l'orientation d'un objet dans l'espace à l'instant k utilise uniquement 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 3. Procédé selon la revendication 1 , dans lequel l'étape A comporte : A1- un prétraitement des mesures de vitesse de rotation (YG), 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 , yM k ) est égale à la mesure à l'instant k, en cas de perturbation, la mesure non perturbée estimée à l'instant k (yA k , yM k ) est calculée sur la base de l'orientation estimée à l'instant précédent k-1. 4. Procédé selon la revendication 3, dans lequel l'étape A1 consiste à retrancher aux mesures de vitesse de rotation un biais moyen (bmoyen ) déterminé lors d'une étape préalable d'initialisation. 5. Procédé selon la revendication 4, dans lequel le biais moyen (bmoyen) 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. 6. Procédé selon la revendication 3, dans lequel l'étape A2 comprend : 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é (CÎA), 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 Ia 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é (CIM), 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. 7. Procédé selon la revendication 6, dans lequel on prévoit à l'étape A2.1 un test supplémentaire sur la perturbation estimée (âk_x ) à 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 (Go) à l'instant k est inférieure au seuil prédéterminé (C-A), 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é (βA), 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 (^-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 (H0) est inférieure au seuil prédéterminé (C-M), 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é (βivi), si ce test est positif, on considère que la perturbation magnétique est effectivement nulle à l'instant k. 8. Procédé selon la revendication 3, dans lequel une au moins des détections de l'étape A2 est effectuée sur une fenêtre temporelle (TA> TM) réglée par un utilisateur. 9. Procédé selon la revendication 8, dans lequel la détection de l'accélération propre est réalisé par un traitement de la forme: Si Bi, - T4 ; y A* - - G0 y, u = Φ Go i Sinon y AΛ = y Ai Fin Si 10. Procédé selon Ia revendication 8, dans lequel la détection de la perturbation magnétique est réalisée par un traitement de la forme : sinon JV* ~ y M, h Fin si dk=yM.k-qk~ι®H0®qk^ 11. Procédé selon la revendication 9, dans lequel on calcule en outre l'angle uk =angle(-yAk,yMk) en sortie de la détection de l'accélération propre et on réalise ensuite la détection de la perturbation magnétique par un traitement de la forme : Si présence d'une accélération propre else • M ~ I M_slow Fin si Si JV* H1 > αM ou U. -U1 > αu pour au moins une valeur tk telle que tke[tk-TM ; t J : sinon >'MΛ M.k Fin si cl ≈ y M, I- -qk^®H0Θqk^ 12. Procédé selon l'une des revendications 1 à 11, dans lequel l'observateur utilisé dans l'étape B est un filtre de Kalman étendu. 13. Procédé selon Ia revendication 12, 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 (ij ), à partir du vecteur d'état ( %kA ) estimé a posteriori à l'instant k-1 , l'estimation des mesures a priori à l'instant k (j>~), à partir de l'estimation du vecteur d'état a priori à l'instant k ( £j ), calcul du gain du filtre de Kalman étendu (Kk) et de l'innovation (l/c) en calculant la différence entre les mesures non perturbées estimées à l'instant k et les mesures estimées a priori ( yk~ ), 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. 14. Procédé selon la revendication 13, dans lequel le vecteur d'état utilisé dans le filtre de Kalman étendu contient les éléments de la vitesse angulaire et du quatemion d'orientation. 15. Procédé selon la revendication 14, dans lequel le vecteur d'état utilisé dans le filtre de Kalman étendu ne contient que les éléments du quatemion d'orientation. 16. Centrale d'attitude comportant des moyens aptes à fournir des mesures d'accélération (VA), des moyens de mesure de champ magnétique (VM), et des moyens de mesure de la vitesse de rotation (VQ) 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, ladite centrale étant caractérisée en ce que lesdits moyens d'estimation comprennent : 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, ladite perturbation appartenant à un groupe comprenant une accélération propre de l'objet, un champ magnétique s'ajoutant au champ magnétique terrestre et un biais dans la mesure de la vitesse de rotation et à délivrer des mesures accélérométriques non perturbées estimées (yAΛ ), des mesures magnétométriques non perturbées estimées {yux ), et de la vitesse de rotation non biaisée (y<3Λ ), 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. 17. . Centrale d'attitude selon la revendications 16, comprenant également des moyens de calcul d'un biais moyen (b „) des moyens de mesure de la vitesse de rotation lors d'une étape d'initialisation de la centrale. 18. Centrale d'attitude selon la revendication 16,, dans laquelle les moyens de prétraitement comprennent 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. 19. Centrale d'attitude selon la revendications 18, dans laquelle les 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 comprennent en outre des moyens pour effectuer ces détections dans une ou plusieurs fenêtres temporelles. 20. Centrale d'attitude selon la revendications 16, comprenant en outre 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. 21. Centrale d'attitude selon la revendication 16, dans laquelle l'observateur est un filtre de Kalman étendu. 22. Centrale d'attitude selon la revendication 16, dans laquelle les moyens aptes à fournir des mesures de l'accélération totale (yA), des mesures de champ magnétique (VM), et des mesures de la vitesse de rotation (yo) selon trois axes de l'espace sont des capteurs MEMS. |
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 œuvre un tel procédé. L'obtention de l'orientation nécessite généralement la mise en œuvre 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, dans le domaine de la navigation), ces capteurs MEMS ont pour inconvénients d'être relativement bruités et biaises.
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 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é. Il 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, II 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. Il existe des méthodes, dites d'optimisation, mettant en œuvre 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 œuvre 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 œuvre 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 œuvre 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 œuvre 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. 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 œuvre, une de ses applications à la capture de mouvement est notamment décrite dans le document « Quaternion-based extended Kalman filter for détermination orientation by inertial and magnetic sensing », SABATINI A. M., IEEE Transactions on Biomédical 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 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 accelerometers, magnetometers and gyroscope sensors for sensor network" de HARADA T., UCHINO H., MORI T., SATO T. IEEE Conférence on Multisensor Fusion and Intégration 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 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 œuvre 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 expérimental results of a quatemion-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 détermination of micro satellite », SU K., REN D.H., YOU Z, ZHOU Q., International Conférence 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 device based on accelerometers, magnetometers and gyroscope sensors for sensor network », HARADA T., UCHINO H., MORI T., SATO T. IEEE Conférence on Multisensor Fusion and Intégration 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 œuvre. 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 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 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 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. Pour cela, on prévoit d'utiliser l'orientation estimée à l'instant précédent comme information supplémentaire pour effectuer Ie 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 mouvement considéré. Cette méthode est par ailleurs simple de mise en œuvre 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 d'une perturbation dans lesdites mesures, ladite perturbation appartenant à un groupe comprenant une accélération propre de l'objet, un champ magnétique s'ajoutant au champ magnétique terrestre et un biais dans la mesure de la vitesse de rotation, et pour 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 : A1- 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 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 A1 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 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.
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. Dans des cas défavorables d'emploi où l'on sait que l'orientation estimée peut dériver, la robustesse de ces tests de comparaison des étapes A2.1 et A2.2 n'est cependant assurée que sur une fenêtre temporelle limitée, différente selon les cas. Les tests de comparaison sont donc avantageusement effectués sur des plages temporelles comme indiqué plus loin dans la description. Ainsi, en notant y A k , y M k , y GΛ les mesures obtenues à l'issue de l'étape de pré-traitement qui sont appelées mesures non perturbées estimées : - la détection d'accélérations propres s'effectue uniquement grâce à la norme de la mesure accélérométrique ; Si cette norme est différente de la norme de Go (gravité terrestre) sur au moins une des mesures d'une fenêtre glissante de durée T A alors la mesure à l'instant courant est considérée comme étant perturbée ; - la détection des perturbations magnétiques s'effectue de manière similaire : o si la norme de la mesure magnétométrique est différente de la norme de Ho (champ magnétique terrestre) sur au moins une des mesures d'une fenêtre glissante de durée T M o OU si l'angle entre la mesure magnétométrique et l'opposé de la mesure accélérométrique non perturbée - y A est différent de l'angle entre les vecteurs Go et H 0 , alors la mesure à l'instant courant est perturbée magnétiquement. TA pourra être un paramètre de valeur constante tandis que la valeur de T M pourra être liée à la rapidité du mouvement.
Grâce à l'emploi de cette variante, qui peut être implantée sur le même équipement et activée à l'initiative de l'utilisateur, on n'a pas à aller rechercher les valeurs des mesures à l'instant k-1 et on s'affranchit donc de la dérive.
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 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 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 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 é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 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 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 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.
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,
- 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 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é. II 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 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 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 :
avec YA : mesure tri-axe de l'accélération totale fournie par l'accéléro mètre,
YM : mesure tri-axe du champ magnétique fournie par le magnétomètre,
YG : mesure tri-axe de la vitesse de rotation fournie par le gyromètre,
R : matrice de rotation, G 0 : champ de gravité terrestre (vecteur 3x1 ), H 0 : champ magnétique terrestre (vecteur 3x1 ), ω : vitesse angulaire, a : accélérations propres, d : perturbations magnétiques, b : biais du gyromètre, v A : bruit de mesure de l'accéléromètre, v M : bruit de mesure du magnétomètre,
VQ : 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, le repère géocentrique est défini par les vecteurs
G 0 (0 ; 0 ; 1 ) et H 0 (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 mesures, chacune de ces mesures comporte respectivement une première partie
« -RGo », « R- Ho » et ω, 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 mesures, et enfin une troisième partie v A ,v M , VQ 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.
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 œuvre.
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 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 :
avec x : vecteur d'état ω : vitesse angulaire q : quaternion
T : 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 3*1 , et également pour le quaternion vectoriel [0, H 0 ] de dimension 4x1 , le vecteur H 0 étant également de dimension 3*1.
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 : x = f(x) ≈ q = -.q ®y G + w x (II), et
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 œuvre 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 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 ai 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 ai et di ; Soit on connaît l'orientation à l'état initial, dans ce cas on peut déduire ai et di . L'étape d'initialisation 100 de la centrale à k = 1 comporte en outre :
- le réglage des matrices de covariance Q, R et P 1 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 b moym . on maintient la centrale d'attitude immobile durant un temps déterminé, par exemple une seconde environ, et on calcule la moyenne des valeurs de sortie du gyromètre sur chaque axe. Cette estimation du biais moyen b moyen 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.
Cette estimation du biais moyen b moyen 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.
Lors de l'étape 200, les mesures délivrées par les trois capteurs sont prétraitées lors de trois étapes 210, 220 et 230.. Selon le contexte d'emploi du procédé et la précision souhaitée, plusieurs variantes de réalisation sont possibles.
La première étape 210 est identique dans tous les modes de réalisation. Lors de cette étape 210, représentée sur la figure 2B, a lieu le prétraitement de la mesure yβ ,k du gyromètre. Comme indiqué ci-dessus, ce prétraitement de la mesure yo ,k est obtenu en retranchant à la mesure réelle Ie biais moyen b mmen , on obtient en sortie une mesure prétraitée du gyromètre à l'instant k désignée par y G k .
Dans la description, l'accélération est donnée en multiple de Go (champ magnétique terrestre), et le champ magnétique est donné en multiple de Ho à des fins de simplification.
Dans un premier mode de réalisation, on effectue, avantageusement en parallèle, deux tests de détection de l'existence de perturbations accélérométriques (étape 220) et magnétométriques (étape 230). Lors de l'étape 220, a lieu le prétraitement des mesures délivrées par l'accéléromètre y Alk à 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. 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 y A k 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 y^ k à la norme du champ gravitationnel (pour rappel, on travaille en multiple de Go), on fait donc une comparaison par rapport à 1 : Si y * je ~ l < a A >
Avantageusement, en cas de test positif, on ajoute le test suivant :
A-I < β 'A La comparaison de la norme de l'accélération propre estimée à l'instant k-1 , â kA , à βA, 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 à βA, il est peu probable qu'à l'instant k l'accélération propre soit inférieure à β A . a A et β A 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 y Λ 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 à y^ λ -et est directement utilisable par l'observateur. â k = o y 4 , k = y Λ , k
Sinon, lors de l'étape 220.2, on construit une nouvelle mesure d'accélération grâce à l'orientation estimée q kA , 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 : .
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 = y u + £*-i ® G Q <S> q k _ ι , puisque y 4Â = y AΛ + â 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 YMM à 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 y M 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 Ia mesure y Mtk à la norme du champ magnétique (pour rappel, on travaille en multiple de H 0 ), on fait donc une comparaison par rapport à 1 :
Si Vi MJk -1 < a M '
Avantageusement, en cas de test positif de non existence d'une perturbation du champ magnétique, on ajoute le test suivant : d *-i < β M
La comparaison de la norme de la perturbation magnétique estimée à l'instant k-1 , d k _ λ , à PM. 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 à PM, il est peu probable qu'à l'instant k la perturbation magnétique soit inférieure à βy. a M 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 y M 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 d k est nulle à l'instant k. Lors de l'étape 230.2, la mesure estimée est alors égale à y^^et est directement utilisable par l'observateur. d k = 0 y M,k ~ y M jι
Sinon, lors de l'étape 230.2, on construit une nouvelle mesure de champ magnétique grâce à l'orientation estimée q kA , 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 : >W = ? t -i ® #o ® ?JM .
On peut également en déduire une valeur de la perturbation magnétique à l'instant k (étape 230.3), égale à : d k = JV* - ?*_, ® H 0 ® ? t _, , puisque j M t = J^ + d k Un deuxième mode de réalisation trouve particulèrement application dans des cas où l'on sait que l'estimation de l'orientation peut dériver de manière importante. Dans ces cas, il est avantageux de définir des fenêtres temporelles dans lesquelles les tests d'existence de perturbations accélérométriques et magnétométriques sont effectués. Dans cette variante, pour le test de mesures accélérométriques, on réalise la comparaison prévue à l'étape 220 sur une fenêtre se terminant à l'instant fo : si l'on détecte une accélération propre (norme des mesures accélérométriques différente de la norme de Go à a 4 près sur au moins une des mesures de la fenêtre [fc - T A ; t k ]) alors la mesure accélérométrique non perturbée est construite grâce à l'orientation estimée à l'instant précédent ; sinon, la mesure accélérométrique non perturbée est égale à la mesure entrant dans la phase de prétraitement (mesure capteur). La valeur de l'accélération propre est calculée à partir de la mesure capteur. Des valeurs typiques données uniquement à titre d'exemple pour a A et TA sont 0,2 g et 0,4 s.
Le test d'absence d'une accélération propre s'écrit alors :
Si L -P, \ «x A , Vt k e [t k - T 4 ; tj
= 0 y AI - y,
Sinon y 4 k = -q k . ι ® G 0 ® g k _ ι
Avantageusement, l'accélération propre peut être calculée systématiquement, même en cas de seuils non dépassés par la même formule que dans la première variante de réalisation :
On réalise ensuite ou en parallèle (cette deuxième option étant avantageuse puisqu'elle permet de gagner du temps) le test de détection de perturbations du signal magnétométrique prévu à l'étape 230 : si l'on détecte une perturbation magnétique (norme des mesures magnétométriques différente de la norme de H 0 à a M près sur au moins une des mesures de la fenêtre [t k -
TM ; t k ]) alors la mesure magnétométrique non perturbée est construite grâce à l'orientation estimée à l'instant précédent. Sinon, la mesure magnétométrique non perturbée est égale à la mesure entrant dans la phase de pré-traitement (mesure capteur). La valeur de la perturbation magnétique est ensuite calculée à partir de la mesure capteur. Des valeurs typiques données uniquement à titre d'exemple pour a M , et TM sont respectivement 0.1h et 0,5s.
Le test de non existence d'une perturbation s'écrit alors :
Si y M ^o m k -T M ; t k ) : d k -- = 0
5v k ~ y M ,u sinon y M , k ~ S ) H 0 ® <7*-.
Fin si
Avantageusement, la perturbation magnétique peut être calculée systématiquement, même en cas de seuils non dépassés par la même formule que dans la première variante de réalisation :
Dans ces deux premiers modes de réalisation, les étapes 202 et 203 trouvent avantage à être effectuées parallèlement.
En revanche, un troisième mode de réalisation permet d'améliorer la précision de la détection, lorsque cela est nécessaire et que l'on dispose dans le dispositif de moyens de calcul et de stockage suffisants pour utiliser des fonctions trîgonométriques. Dans ce cas on renonce à l'avantage procuré par Ie parallélisme des calculs de détection et il est plus avantageux d'effectuer le test de d'existence de perturbation magnétique après le test de présence d'une accélération propre. En sortie de ce premier calcul d'étape 202, on calcule également u k = angle(-y Λ k ,y u k ) et on note Uo l'angle mesuré entre les vecteurs Go et HQ. Ce paramètre peut être calculé lors de l'étape 100 d'initialisation. Puis on réalise le test de détection de perturbation magnétique de la manière suivante : si l'on détecte une perturbation magnétique (norme des mesures magnétométriques différente de la norme de H 0 à or M près ou angle u k différent de M 0 à a u près sur au moins une des mesures de la fenêtre [Î H -T M ; t k ]) alors la mesure magnétométrique non perturbée est construite grâce à l'orientation estimée à l'instant précédent. Sinon, la mesure magnétométrique non perturbée est égale à la mesure entrant dans la phase de pré-traitement (mesure capteur). La valeur de la perturbation magnétique est ensuite calculée à partir de la mesure capteur. Avantageusement, on utilisera deux valeurs différentes de TM selon qu'il y a ou pas présence d'une accélération propre (respectivement T M j as t et Des valeurs typiques données données uniquement à titre d'exemple pour a M , a u , TMjast et TM_SIOW sont respectivement 0.1 h, 10°, 0.5s et 3s.
Le test d' existence d'une perturbation s'écrit alors : Si présence d'une accélération propre
TM = TM fast else
Fin si
SSii JyM M J. k \ - HH r0 1 > > α« uM oOuU uM k4 -- uM 00 > α t pour au moins une valeur tk telle que t k e [t k -τ M ; h} : sinon y M,k ~ y M.k
Fin si d k --
Lors de l'étape 300, les mesures prétraitées y AΛ , y M k , y G k sont utilisées par l'observateur. On utilise par exemple un filtre de Kalman étendu sous sa forme factorisée.
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 K k du filtre de Kalman et de l'innovation l k , 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 : avec X^ 1 : estimation a posteriori du vecteur d'état à l'instant k-1 ,
%l : estimation a priori du vecteur d'état à l'instant k,
T e : 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) :
91 = h ( χ D
Lors de l'étape c), on calcule le gain K k et l'innovation l k , l'innovation est obtenue en soustrayant les mesures estimées a priori aux mesures prétraitées.
On obtient :
Lors de l'étape d), on corrige l'état estimé a priori avec le gain et l'innovation. Cette correction donne une estimation a posteriori de l'orientation q k à l'instant k.
Avantageusement, lors d'une étape e) ultérieure, on normalise le quaternion estimé q k = -^- , ce qui permet d'éviter une dérive à chaque étape du calcul.
9 k
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.
Il 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 œuvre 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.
Next Patent: METHOD FOR SEPARATING AROMATIC ALDEHYDE