FR2990027A1 - Procede d'estimation du tangage et du roulis et dispositif inertiel associe - Google Patents

Abstract

Les modes de réalisation de la présente invention décrivent un procédé d'estimation du tangage et du roulis d'un objet modélisé par un système dynamique comprenant les étapes suivantes : - on mesure des valeurs d'accélération de l'objet selon trois directions non coplanaires (101), - on mesure des valeurs de vitesse angulaire de l'objet autour de trois axes correspondant aux trois directions non coplanaires (101), - on définit un état du système dynamique lié au tangage et au roulis de l'objet, - on estime l'état du système dynamique par un observateur d'état dans lequel on calcule l'état du système dynamique à partir d'une part des valeurs mesurées de vitesse angulaire et d'autre part d'une estimation antérieure de l'état du système dynamique (104) et on corrige l'état calculé à partir des valeurs mesurées d'accélération (105), - on récupère les valeurs estimées de tangage et de roulis de l'objet à partir de l'état estimé du système dynamique, caractérisé en ce que l'étape d'estimation comprend les sous-étapes suivantes : - on détermine, à partir des valeurs mesurées d'accélération, une valeur de l'accélération propre de l'objet (102), - on pondère la correction des valeurs calculées de tangage et de roulis en fonction de la valeur de l'accélération propre déterminée (103).

Description

Procédé d'estimation du tangage et du roulis et dispositif inertiel associé La présente invention concerne le domaine des dispositifs inertiels aussi appelés inclinomètres.

Un dispositif inertiel est un dispositif visant à déterminer au moins partiellement l'attitude d'un objet auquel il est fixé, c'est-à-dire au moins un des trois angles correspondant au tangage (« pitch » en anglais), au roulis (« roll » en anglais) et au lacet (« yaw » en anglais), à partir de mesures de vitesse angulaire et d'accélération, voire d'intensité de champ magnétique ou autre mesure.

Suivant le domaine d'application, la complexité et le prix de revient d'un dispositif inertiel varie grandement. Ainsi, dans le domaine des jeux vidéos ou des téléphones portables, des dispositifs inertiels sont utilisés pour les manettes des jeux ou pour déterminer l'orientation de l'écran des téléphones ou des manettes. Dans ces applications, la précision recherchée sur les valeurs des mesures d'angles est très faible de sorte que des gyromètres de basse qualité ayant un prix de revient très faible suffisent. A l'inverse, dans le domaine aéronautique, la précision recherchée sur les mesures est très importante et différents types de capteurs de haute qualité, c'est à dire ayant notamment une dérive et un bruit très faibles, sont couplés et gérés par de puissant algorithmes pour obtenir une estimation très fiable de l'attitude. Dans le cadre d'applications industriels, comme par exemple les engins de chantier, la précision de mesure recherchée est supérieure à celle des applications de divertissement mais inférieure à celle de l'aéronautique. Par ailleurs, des contraintes environnementales comme les vibrations ou la présence de masses métalliques importantes empêchent l'utilisation de certains types de capteurs comme par exemple les magnétomètres.

Le but de la présente invention est donc de surmonter les inconvénients précités de l'état de la technique et de proposer une méthode et un équipement permettant d'obtenir une estimation fiable du tangage et du roulis en conditions industrielles, par exemple sur des engins de chantier, pour un coût de revient limité et sans utiliser de magnétomètre.

Ainsi, la présente invention concerne un procédé d'estimation du tangage et du roulis d'un objet modélisé par un système dynamique comprenant les étapes suivantes : - on mesure des valeurs d'accélération de l'objet selon trois directions non coplanaires, - on mesure des valeurs de vitesse angulaire de l'objet autour de trois axes correspondant aux trois directions non coplanaires, - on définit un état du système dynamique lié au tangage et au roulis de l'objet, - on estime l'état du système dynamique par un observateur d'état dans lequel on calcule l'état du système dynamique à partir d'une part des valeurs mesurées de vitesse 10 angulaire et d'autre part d'une estimation antérieure de l'état du système dynamique et on corrige l'état calculé à partir des valeurs mesurées d'accélération, - on récupère les valeurs estimées de tangage et de roulis de l'objet à partir de l'état estimé du système dynamique, dans lequel l'étape d'estimation comprend les sous-étapes suivantes : 15 - on détermine, à partir des valeurs mesurées d'accélération, une valeur de l'accélération propre de l'objet, - on pondère la correction des valeurs calculées de tangage et de roulis en fonction de la valeur de l'accélération propre déterminée. 20 Selon un aspect supplémentaire de la présente invention, l'observateur d'état est un filtrage dynamique. Selon un autre aspect de la présente invention, les trois directions non-coplanaires sont orthogonales. 25 Selon un autre aspect de la présente invention, on pondère la correction des valeurs calculées de tangage et de roulis en appliquant un facteur correctif à l'écart-type associé aux valeurs mesurées d'accélération. 30 Selon un aspect supplémentaire de la présente invention, les étapes de mesure des valeurs d'accélération et de vitesse angulaire sont réalisées selon un échantillonnage temporel et dans lequel le filtrage dynamique est un filtrage récursif dont la récurrence correspond à une période d'échantillonnage (te). L'état du système dynamique est alors décrit par un modèle dynamique à temps discret dans lequel les valeurs de vitesse angulaire sont considérées constantes pendant une période d'échantillonnage (te). Selon un aspect additionnel de la présent invention, l'état du système dynamique correspond à la projection de la gravité sur les axes d'un repère mobile lié au système dynamique.

Selon un autre aspect de la présente invention, la dynamique de l'état du système dynamique est linéarisée et les bruits associés à l'état et les bruits associés aux valeurs mesurées de vitesse angulaire sont additifs.

Selon un aspect additionnel de la présente invention, le système dynamique est décrit par des équations descriptives comprenant une matrice de covariance d'état (P), une matrice de covariance des bruits de mesure (R) et une matrice de covariance des bruits d'état (Q) et on utilise une forme factorisée de ces matrices de covariance afin d'obtenir un filtrage dynamique numériquement stable.

Selon un aspect supplémentaire de la présente invention, la pondération de la correction des valeurs calculées de tangage et de roulis est réalisée en appliquant un facteur correctif à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération du système dynamique au niveau de la matrice de covariance des bruits de mesure (R).

Selon un autre aspect de la présente invention, on définit au moins un seuil d'accélération propre et si l'accélération propre est au-dessous du au moins un seuil, on applique un premier facteur correctif à la valeur à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération et si l'accélération propre est au-dessus du au moins un seuil, on applique un deuxième facteur correctif à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération. Selon un aspect supplémentaire de la présente invention, le facteur correctif appliqué à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération est défini en fonction de 5 l'accélération propre déterminée par une fonction continue prédéterminée. Selon un autre aspect de la présente invention, le filtrage dynamique est réalisé par un filtre de Kalman étendu, un filtre de Kalman « unscented » ou un filtre à particules. 10 Les modes de réalisation de la présente invention concernent également un programme d'ordinateur destiné à être associé à un dispositif inertiel comprenant trois accéléromètres et trois gyromètres dont les axes sensibles sont disposés de manière non coplanaire, ledit système inertiel étant fixé à un objet dont on désire estimer le tangage et le roulis, ledit programme d'ordinateur comprenant des instructions adaptées pour mettre en oeuvre les étapes 15 suivantes : - recevoir des valeurs mesurées d'accélération et de vitesse angulaire issues respectivement des accéléromètres et des gyromètres du dispositif inertiel, - modéliser l'objet par un système dynamique et définir un état du système dynamique lié au tangage et au roulis de l'objet, 20 - estimer l'état du système dynamique en appliquant un observateur d'état dans lequel l'état du système dynamique est calculé à partir d'une part des valeurs mesurées de vitesse angulaire et d'autre part d'une estimation antérieure de l'état du système dynamique et en corrigeant l'état calculé à partir des valeurs mesurées d'accélérations, - récupérer les valeurs estimées de tangage et de roulis d l'objet à partir de l'état estimé 25 du système dynamique, dans lequel l'étape d'estimation comprend les sous-étapes suivantes : - déterminer, à partir des valeurs mesurées d'accélération, une valeur de l'accélération propre de l'objet, - pondérer la correction des valeurs calculées de tangage et de roulis en fonction de la 30 valeur de l'accélération propre déterminée.

Les modes de réalisation de la présente invention concernent également un dispositif inertiel destiné à estimer le tangage et le roulis d'un objet, ledit dispositif inertiel comprenant : - trois gyromètres disposés selon trois directions non coplanaires, - trois accéléromètres disposés selon les trois directions non coplanaires, - des moyens de traitement configurés pour : - modéliser l'objet par un système dynamique et définir un état du système dynamique lié au tangage et au roulis de l'objet, - estimer l'état du système dynamique en appliquant un observateur d'état dans lequel l'état du système dynamique est calculé à partir d'une part des valeurs mesurées de vitesse angulaire fournies par les gyromètres et d'autre part d'une estimation antérieure de l'état du système dynamique et en corrigeant l'état calculé à partir des valeurs mesurées d'accélérations fournies par les accéléromètres, - récupérer les valeurs estimées de tangage et de roulis d l'objet à partir de l'état estimé du système dynamique, dans lequel les moyens de traitement sont également configurés pour : - déterminer, à partir des valeurs mesurées d'accélération fournies par les accéléromètres, une valeur de l'accélération propre de l'objet, - pondérer la correction des valeurs calculées de tangage et de roulis en fonction de la valeur de l'accélération propre déterminée. Les modes de réalisation de la présente invention concernent également un équipement motorisé comprenant des moyens d'actionnement permettant de faire varier le tangage et le roulis d'un élément associé à l'équipement motorisé caractérisé en ce que l'élément associé à l'équipement motorisé comprend un dispositif inertiel et en ce que les moyens d'actionnement sont pilotés en fonction des valeurs estimées de tangage et de roulis fournies par le dispositif inertiel. Selon un autre aspect de la présente invention, l'équipement motorisé est un engin de 30 manutention comprenant un bras articulé piloté par des vérins hydrauliques et l'élément associé est un outil de levage fixé sur le bras articulé, ledit engin de manutention comprenant un dispositif correcteur d'assiette qui pilote les vérins hydrauliques de manière à corriger l'orientation de l'outil de levage en fonction des valeurs estimées de tangage et de roulis fournies par le dispositif inertiel.

Selon un autre aspect de la présente invention, ledit équipement motorisé est un engin de manutention comprenant un bras articulé piloté par des vérins hydrauliques, l'élément associé est un outil de levage fixé sur le bras articulé, ledit engin de manutention comprenant un dispositif correcteur d'assiette qui pilote les vérins hydrauliques de manière à corriger l'orientation de l'outil de levage en fonction des valeurs estimées de tangage et de roulis fournies par le dispositif inertiel. D'autres caractéristiques et avantages de l'invention apparaîtront dans la description qui va maintenant en être faite, en référence aux dessins annexés qui en représentent, à titre 15 indicatif mais non limitatif, un mode de réalisation possible. Sur ces dessins: la figure 1 représente un organigramme des différentes étapes des modes de réalisation de la présente invention; 20 la figure 2 représente un schéma des différents écart-types en fonction de l'accélération propre mesurée selon un mode de réalisation de la présente invention; la figure 3 représente un exemple de fonction prédéfinie du facteur correctif à appliquer à l'écart-type en fonction de l'accélération propre mesurée ; la figure 4 représente un schéma d'un engin de chantier comprenant un correcteur 25 d'assiette utilisant un dispositif inertiel selon les modes de réalisation de la présente invention ; Sur ces dessins, les mêmes numéros de référence concernent des éléments similaires. 30 Dans la suite de la description, le terme « observateur d'état » d'un système désigne une extension d'un modèle représenté sous forme de représentation d'état du système. Lorsque l'état d'un système n'est pas mesurable, on conçoit un observateur qui permet de reconstruire l'état à partir d'un modèle du système dynamique et des mesures d'autres grandeurs.

Le terme « filtrage dynamique » correspond à un modèle de prévision de l'évolution de l'état d'un système dynamique à partir de mesures qui sont liées à l'état de ce système et dont on connait l'incertitude; Le terme « accélération propre » d'un objet définit l'accélération de cet objet par 10 rapport à un référentiel terrestre. Le terme « système dynamique » définit un modèle permettant de décrire l'évolution au cours du temps d'un objet. 15 L'expression « trois directions non coplanaires » définit trois directions qui n'appartiennent pas à un même plan de sorte que la troisième direction ne peut être obtenue par une combinaison linéaire des deux autres directions et de sorte que les trois directions constituent une base de l'espace à trois dimensions le. 20 Le terme « modèle à temps discret » correspond à un modèle dans lequel les variables ne sont connues qu'aux instants d'échantillonnage et correspond donc à un modèle numérique. Le lien entre un tel modèle et une environnement réel (à temps continu) se fait par l'introduction de bloqueurs qui sont habituellement d'ordre zéro. Le bloqueur d'ordre zéro maintient constante la valeur de l'échantillon sur toute la période d'échantillonnage. La 25 détermination de la fréquence d'échantillonnage du modèle est déterminée par la dynamique du système et les objectifs du modèle (simulation, commande...) et également ici par l'hypothèse des vitesses angulaires constantes entre deux pas d'échantillonnage. Les modes de réalisation de la présente invention concernent un procédé d'estimation 30 du tangage (« pitch » en anglais) et du roulis (« roll » en anglais) d'un objet à partir des mesures effectuées sur trois accéléromètres et trois gyromètres d'un dispositif inertiel fixé sur ledit objet dans lequel l'importance accordée aux valeurs mesurées par les accéléromètres dans la détermination du tangage et du roulis de l'objet est pondérée en fonction de la valeur déterminée de l'accélération propre de l'objet.

Les gyromètres sont disposés selon trois axes non coplanaires et préférentiellement orthogonaux et les accéléromètres sont également orientés selon les mêmes trois axes. Le procédé d'estimation est basé sur un observateur d'état, par exemple un filtrage dynamique, dans lequel les valeurs de vitesse angulaire et d'accélération mesurées respectivement par les gyromètres et les accéléromètres sont combinées par un algorithme de fusion pour réduire 10 l'effet des dérives et des erreurs des différents capteurs (accéléromètres et gyromètres). Dans ce procédé, les valeurs mesurées par les gyromètres ou mesures de vitesse angulaire sont utilisées pour établir un calcul, aussi appelé prédiction, du tangage et du roulis de l'objet et les valeurs mesurées par les accéléromètres sont utilisées pour corriger la valeur calculée ou prédiction. En effet, en l'absence d'accélération propre de l'objet, les 15 accéléromètres mesurent la projection de la gravité sur leur axe de mesure ce qui permet d'obtenir une estimation des valeurs de tangage et de roulis de l'objet. Cependant, la présence d'une accélération propre de l'objet introduit une erreur dans cette estimation. Il convient donc de pondérer l'importance accordée aux valeurs mesurées par les accéléromètres en présence d'accélération propre de l'objet. L'importance donnée aux valeurs mesurées par les 20 accéléromètres étant d'autant plus faible que l'accélération propre de l'objet est grande. La figure 1 décrit les différentes étapes du procédé d'estimation du tangage et du roulis selon les modes de réalisation de la présente invention. - La première étape 101 correspond à l'étape de mesure des valeurs d'accélération et de vitesse 25 angulaire selon trois directions non coplanaires par respectivement les trois accéléromètres et les trois gyromètres du dispositif inertiel. Les mesures sont effectuées par le biais d'un convertisseur analogique-numérique qui échantillonne les signaux issus des gyromètres et des accéléromètres à une fréquence donnée. Cette fréquence est définie en fonction de l'application et de la dynamique de l'objet. Par ailleurs, de manière préférentielle, les trois 30 directions sont orthogonales. Cependant, à partir de trois directions non coplanaires permettant de définir l'espace 1R3, il est possible de se ramener aux valeurs correspondant à trois directions orthogonales, par exemple par une décomposition QR de la matrice de mesures. - La deuxième étape 102 correspond à la détermination de l'accélération propre de l'objet à partir des valeurs mesurées par les accéléromètres, en combinant les valeurs mesurées par les trois accéléromètres. En effet, connaissant les axes sensibles des accéléromètres, correspondant, par exemple aux trois directions orthogonales, et les valeurs mesurées des accélérations selon ces trois directions, on peut déterminer (par somme vectoriel des trois accélérations fournies par les trois accéléromètres) une valeur mesurée de l'accélération propre (l'accélération propre mesurée correspond en réalité à la somme de l'accélération de la gravité et de l'accélération propre de l'objet). Il est à noter que cette détermination ne peut être réalisée de manière précise du fait que la valeur mesurée par les accéléromètres correspond la valeur d'une accélération résultant de la combinaison de l'accélération propre de l'objet et de l'accélération de la gravité. Ainsi, si la valeur mesurée par les accéléromètres est différente de la valeur de l'accélération de la gravité, on peut en conclure la présence d'accélération propre de l'objet mais si la valeur mesurée des accéléromètres correspond à la valeur de l'accélération de la gravité, on ne peut conclure à l'absence d'accélération propre de l'objet. Par exemple, pour une accélération propre de l'objet égale à deux fois la valeur de l'accélération de la gravité mais dans une direction opposée, la valeur mesurée par les accéléromètres correspondra à la valeur de l'accélération de la gravité. Ainsi à partir des valeurs mesurées par les accéléromètres, on peut conclure que plus la valeur mesurée est différente de la valeur absolue de l'accélération de la gravité (correspondant à 1G), plus l'accélération propre sera importante. - La troisième étape 103 correspond à la pondération associée aux valeurs mesurées par les accéléromètres en fonction de la valeur de l'accélération propre déterminée à l'étape 102. Cette pondération est réalisée par l'ajustement de la valeur de l'écart-type lié au bruit des valeurs mesurées par les accéléromètres de sorte que cet écart-type varie en fonction de la valeur de l'accélération propre déterminée. Cette variation peut être discrète ou continue. Dans le cas d'une variation discrète, on définit un ou plusieurs seuils d'accélération et on définit différentes valeurs d'écarts-types (ou différents facteurs correctifs d'écarts-types) en fonction de la position respective de l'accélération propre mesurée par rapport à ces seuils. La figure 2 montre un exemple de variation discrète dans lequel trois seuils notés Si, S2 et S3 et correspondant respectivement aux valeurs d'accélérations 0,5G, 1,5G et 3G. Ainsi, pour des valeurs mesurées d'accélération propre comprises entre 0,5 et 1,5G, on applique un écart-type a 1, pour des valeurs d'accélération propre mesurée comprises entre 0 et 0,5G ou entre 1,5 et 3G on applique un deuxième écart-type a2 (avec a2> a 1) et pour des valeurs supérieures à 3G, on applique un troisième écart-type a3 (avec a3> a2). En pratique, un écart-type est donné par l'incertitude de mesure des accéléromètres et un facteur correctif est associé à cet écart-type en fonction de la valeur de l'accélération propre mesurée, ce facteur correctif sera par exemple de 1 pour obtenir al, de 2 pour obtenir a2 et de 3 pour obtenir a3. La valeur de l'écart-type peut même être infinie au delà d'un certain seuil, ce qui revient à n'utiliser que les valeurs mesurées par les gyromètres dans l'estimation du tangage et du roulis. Le nombre et la valeur des seuils sont déterminés en fonction de l'application. D'autre part, la variation peut être continue, dans ce cas, on a une fonction continue (croissante) qui définit la valeur de l'écart-type (ou du facteur correctif) en fonction de la valeur mesurée de l'accélération propre comme représenté sur la figure 3. Ainsi, plus la valeur de l'écart-type est grand, plus l'importance accordée aux valeurs 20 mesurées par les accéléromètres dans l'estimation du tangage et du roulis sera faible. Dans notre cas, plus l'accélération propre mesurée sera différente de 1G, plus le facteur correctif sera grand. - La quatrième étape 104 correspond au calcul ou à la prédiction du tangage et du roulis de 25 l'objet à partir des valeurs mesurées par les gyromètres et d'une estimation antérieure. Une étape préliminaire d'initialisation ou de calibration peut être effectuée à la mise en marche du dispositif inertiel pour effectuer la première estimation. Par ailleurs, dans le cas d'un filtrage dynamique, on calcule non seulement une valeur du tangage et du roulis mais aussi l'incertitude associée au tangage et au roulis calculés. Cette incertitude est déterminée à partir 30 de l'incertitude de mesure des gyromètres. D'autre part, en réalité, afin de simplifier les calculs, on n'estime pas directement le tangage et le roulis de l'objet mais on modélise l'objet par un système dynamique. On définit un état du système dynamique lié au tangage et au roulis et on calcule ou on prédit l'état du système dynamique à partir des valeurs mesurées de vitesse angulaire et d'accélération par un observateur d'état comme par exemple un filtrage dynamique. Cette modélisation sera décrite plus en détails par la suite. - La cinquième étape 105 concerne la correction du calcul ou de la prédiction de l'étape 104 par les valeurs d'accélération mesurées par les accéléromètres. Cette correction étant pondérée en fonction de la valeur de l'écart-type déterminé à l'étape 103. Cette étape est également réalisée par l'observateur d'état du système dynamique. On récupère ensuite les valeurs de tangage et de roulis de l'objet à partir de l'état estimé du système dynamique. En pratique, ces étapes sont réalisées de manière quasi-simultanée et le processus est récursif avec une période correspondant à la période d'échantillonnage d'un convertisseur analogique-numérique associé aux capteurs (gyromètres et accéléromètres) de sorte que ces étapes sont réalisées à chaque période d'échantillonnage. La période d'échantillonnage étant définie en fonction de l'application (dynamique de l'objet) et éventuellement des capacités du convertisseur analogique-numérique et des capacités de traitement disponibles.

La modélisation et l'estimation du tangage et du roulis de l'objet vont maintenant être décrits plus en détails dans le cas d'un filtrage dynamique réalisé par un filtre de Kalman. Il est à noter que d'autre types de filtrage dynamique comme, par exemple, le filtrage à particules (ou filtrage particulaire) ou filtre de Kalman « unscented » (« Unscented Kalman filter (UKF) » en anglais) peuvent également être utilisés. Dans le cadre du filtrage dynamique, l'objet est donc modélisé par un système dynamique, la dynamique du système correspondant à la dynamique de l'objet. Les valeurs d'accélération mesurées par les accéléromètres sont notés y et sont définies par = M.(a+ao) (1) avec M la matrice de rotation pour passer d'un repère terrestre fixe au repère mobile lié au système dynamique, a l'accélération propre et ao la gravité terrestre. La matrice M vérifie l'équation différentielle par rapport au temps : M=S(w)M (2) avec S(w)= [ 0 co w -coz 0 (ox wy "--(0x 0 avec wx, co,, et coz les valeurs des vitesses de rotation ou vitesse angulaires par rapport aux axes x, y et z du repère terrestre fixe mesurées par les gyromètres. On suppose que l'on se trouve en régime quasi-statique, c'est-à-dire que l'accélération propre du système est nulle de sorte que : -sin (0(t)) I v =M. aoqcos(0 (t)) sin(V(t)) (3) cos (0 (t))cos(V(t)) avec 0 le tangage et Y le roulis de l'objet modélisé par le système dynamique. On définit alors l'état du système x(t) comme la projection de la gravité sur les axes du repère mobile lié à l'objet. x(t)=[cos(0 (t))sin(11)(t)) (4) cos(0(t))cos(4)(t)) cela revient à faire un changement de variable entre 0 et I' d'une part et x d'autre part. Ce changement de variable permet d'obtenir des équations linéaires pour définir la dynamique du système en régime quasi-statique. La dynamique du système est défini par : X=S(w(t))x(t) Co(t)=co(t)+b(t)+e(t) v(t)=- x(t)+ n(t) (5) avec n(t) est un bruit modélisant les imperfections des accéléromètres et les erreurs de modèle induites par l'équation (3) lorsqu'une accélération propre existe, b(t) représente le biais éventuel des gyromètres et e(t) un second bruit de mesure modélisant les imperfections des gyromètres. Ainsi, avec le choix de l'état de l'équation (4), les non-linéarités du filtrage dynamique ne proviennent que de la non-linéarité des capteurs (gyromètres).

L'estimation de l'état x(t) à partir des valeurs mesurées par les gyromètres et par les accéléromètres relève alors d'un processus de filtrage dynamique dans lequel on établit une prédiction ou un calcul de l'état à partir d'une estimation antérieure et des valeurs de vitesse angulaire mesurées par les gyromètres puis on corrige la prédiction par les valeurs d'accélération mesurées par les accéléromètres. On utilise également des paramètres de réglage pour adapter au contexte d'utilisation, en particulier, on spécifie la matrice de covariance des bruits de l'équation de mesure, notée R par la suite, à chaque instant d'échantillonnage de manière à prendre en compte la présence d'accélération propre.

On suppose que la dynamique de l'état est linéaire et que les bruits sur l'état et les mesures sont additifs. Le modèle dynamique linéaire est alors de la forme : f+1=Atxt+ B,e,} (6) avec xt et xt+i l'état aux instants t et t+1, et le bruit d'état à l'instant t, y, les mesures à l'instant t et Et le bruit de mesure à l'instant t, l'estimateur linéaire de Kalman est alors donné par : 1+ t-1Q ip,1-1 ,-,t-i T } pour la prédiction (7) f 2t= ilt-i'*i-i P ,=,4,_i tI P_A,T B avec £, et P, la prédiction de l'état et la covariance de l'état prédit à l'instant t et Q,-, la matrice de covariance du bruit d'état à l'instant t-1, à l'instant t-1, Bt., une matrice de pondération, et IG,=P,C7,'[C,P,C7,. + D, R, D,T]i pour l'estimation (8) .1=---.'?t+Gt[Yt-Ct5iti avec 5cs, l'estimation de l'état, Pt la covariance de l'état estimé, Gt le gain de Kalman, C, une matrice de couplage de l'état et de la sortie et Dt une matrice de pondération.

Ces formules étant numériquement instables du fait de la soustraction dans le calcul de la covariance de l'état estimé alors que cette matrice de covariance est définie positive ce qui .Pt_l l'estimation de la covariance de l'état Pt Gt Ci Pt peut conduire à une divergence du filtre de Kalman. En effet, les capacités des microprocesseurs assurant les calculs étant limitées, la précision des calculs est également limitée de sorte que des erreurs sont induites par les capacités du microprocesseur. Suite à ces erreurs de calcul (arrondi par exemple une valeur <10 sera considéré nulle), la matrice de covariance de l'état estimé peut ne plus être symétrique et positive. Afin d'éviter toute instabilité et améliorer la précision des calculs, on utilise une forme factorisée dans laquelle les matrices de covariance sont symétriques et définies positives par construction, la précision de calcul obtenue est alors doublée. Les factorisations des matrices Qt, Rt et Pt sont définies par : (c/2)TQ,u2=Q,,(Rii/2)T =Rtet(p1112)T pill2=pf Ces factorisations peuvent être obtenues par exemple par l'algorithme de Cholesky. L'estimateur de Kalman s'exprime alors par : H =[ p,1/21 p1/2 AT pour la prédiction 0 t -1 1-1- (9) avec H une matrice de triangularisation telle que pour toute matrice M, H(M)=V où V est une 15 matrice triangulaire supérieure. On a alors Mr1M=VTV et FITH=I avec I la matrice identité car H est orthogonale et, H Rtii2D1T I=[ p2ctT p!112 U Z opr1/2 pour l'estimation (10) ±-1=jer1+G1[Yt-Ct1] avec UG,T=Z Par ailleurs, si la prise en compte de certains défauts des gyromètres (biais, dérive) s'avère 20 nécessaire, alors le modèle dynamique sera non linéaire et traité par un filtrage de Kalman étendu (« Extended Kalman Filter (EKF) » en anglais). Le système dynamique est désormais de la forme : Ixt+i=f (xt, ut, wt)} yt=h(x'y,) 25 avec ut l'entrée du système, c'est-à-dire la commande fournie au système, w, le bruit d'état de moyennelT =0 et de covariance Q, et v, le bruit de mesure de moyenne V=0 et de covariance Le principe du filtre de Kalman étendu consiste à appliquer, pour le calcul des covariances, la formulation d'un filtre de Kalman standard au problème linéaire : lx,+1=A,xt+B,w,} yi=C,x,+Dtv, avec A,=a f(x,)l(x,)Br=a f (w ,) 1(w g= 17v) a xf awg a xt et Di= eh(vt)1(vr=V) a v, les équations de filtrage sont alors dans leur forme classique : f(xg-1,14g-pw) Ar_i î)t- AfT_I+ BfT-i .fg= pour la prédiction (13) Et fP:=Pi-G,C,13, G, =P,c,T[C,P,c,T+D,R,D,1]-1 pour l'estimation, (14) et dans leur forme factorisée : .x.." g= f (xt_put_pw) (12) HP ji»112 AT [V = t 0 fuzipt, iT [bill pour la prédiction et (15) He Rtl / 2 - D 0 = U plt/2C,T p tin 0 p i/2 .ft=.5é,+Gt[Yt-h(it,V)] avec UGiT =Z pour l'estimation (16) Dans le cas de notre problème à 3 gyromètres et 3 accéléromètres, en reprenant l'équation (5) et en replaçant co(t) par sa valeur, on obtient : {X=S(6)(t))x(t)+S(x(t))b(t)+S(x(t))e(t)} (17) v (t)= x(t)+n(t) Le bruit de mesure des accéléromètres n(t) et des gyromètres e(t) correspondent respectivement aux matrices ae2I eta0,2I avec ae l'écart-type de mesure des accéléromètres, a, l'écart-type de mesure des gyromètres et I la matrice identité. Pour le biais sur les valeurs mesurées par les gyromètres b(t), on prend comme modèle dynamique : h(t)=0+fl(t) avec j3(t) un bruit blanc de covariance de la forme ad2, ce qui correspond à la modélisation d'une dérive. En prenant pour état (t)= bx tt E 1R6, le modèle dynamique s'écrit alors : (t)_[S(6)(t)-b(t)) (t)+ S(x(t))e(t)i} 0 O (t) (18) v(t)=[I 0](t)+n(t) r" I. p On discrétise le problème avec une période ou pas d'échantillonnage constant te et on considère que l'on a un modèle à temps discret et que x(t) et b(t) sont constants au cours d'un pas ou période d'échantillonnage te, on obtient alors : ,,1expl[s(- bt) °0. el t_r 5(xr3tt )e1i _i M e) °]+[s (xr3,t ) e , I (19) v(t)=[I 0](t)+n(t) ° il avec M(w, t)=exP(S(w(t)))=[i+ sin (at) s (.)± 1 -cos(at) S2(W)} et (1=11(01121a norme a a2 euclidienne de oméga.

Cependant, l'équation d'état (15) n'est pas linéaire du fait du terme b, dans l'argument de M. En posant : Ati1=[I+sina(ÇLt)s(6),t 1-cos (a t)2(3 b1)]=1/±C1 5(011-b1)+C252(Ûjt-bt)] 2 cL (20) on a alors 1axt±i =AH t Ô xt axt+,=Al2= ô r4xJ=aab {xt±c1s(6..ii)x1±cis(x1)br+,2(s(03 s(b1))2] ab, t ab L ce qui conduit à un système linéarisé : A1 1J1 Ati+.8,11t 0 I I.o / (3, v(t)=[/ O](t)l-n(t)=Ctt+n On obtient ainsi un système dont la dynamique est linéaire et correspond à la dynamique choisie à l'équation (6) de sorte que le filtrage de Kalman décrit aux équations (15) et (16) peut être appliqué à ce système. Par ailleurs, comme dans le cas présent on n'observe pas le lacet, le filtrage va être appliqué en pratique à un système réduit à 5 états obtenu en supprimant la dernière composante de correspondant à la perturbation sur le troisième gyromètre (selon l'axe z). La nouvelle matrice A, est obtenue à partir de l'équation (22) en supprimant sa dernière ligne et sa dernière colonne. Pour Ct, on supprime une colonne de zéro et pour 131 sa dernière colonne. Ainsi, en appliquant le filtrage de Kalman décrit dans les équations (15) et (16) sur le système décrit par l'équation (22), on peut estimer à chaque instant l'état du système dynamique et la covariance de l'état, c'est-à-dire le couple (.2t,11)2) Pour récupérer les valeurs des angles de tangage et de roulis ainsi que leur incertitude, on utilise la définition de l'état xt défini à l'équation (4), ce qui donne : cos(o(t))cose , cos(0 (t))sin (ip(t)) =VI12 11.Î111, à -sin(0(t)) I 1 {pi lp(t)) Y (23) avec p, 6 et y les composantes du vecteur d'état xt on en déduit alors : (21) (22) Idt=-asin( Il :(1 1 tî), = atan ( ) La matrice de covariance des angles C est approchée par l'équation : (24) (25) P (p2+ Ô2 4_ y2) \1,52+ y2 Ô2 "2 P Y (p2+b2±y2)\/82±y2 -ô 82+ y2 avec Ji= - La matrice de covariance de l'état étant connue sous forme factorisée, l'équation (25) devient : " t /2 y P1 J 1/2 7"; que l'on peut noté sous la forme Z ZT cji p Si on note par uiT et u2T les lignes de alors les écarts-types de mesure associés au tangage et au roulis sont donnés par : f00=11/11112 o= u2112 10 Ainsi, la définition de l'état (équation (4)) équivaut à un changement de variables qui permet d'obtenir une dynamique d'état linéaire en l'absence de biais ou dérive (gyromètres parfaits) et d'autant plus non linéaire que la qualité et les performances des gyromètres diminuent. En conséquence le modèle linéarisé (équation (22)) est d'autant plus fidèle au modèle non linéaire que les gyromètres sont performants ce qui permet de tirer le meilleur parti possible 15 de la technique de filtrage dynamique EKF. Les valeurs de vitesse angulaire mesurées par les gyromètres interviennent directement dans l'équation d'état (équation (22)) et sont donc le support à la prédiction de l'attitude. Leur précision ou incertitude découle de celle des composantes de l'état, c'est-à-dire de la matrice 20 de covariance P, élaborée par le filtre de Kalman. Cette covariance est plus grande que celle de l'estimation à l'instant précédent (cf. Équation (7)) et est proportionnelle à la durée du pas d'échantillonnage (ou période d'échantillonnage) de sorte que si on ne corrigeait pas la prédiction à l'aide des valeurs mesurées par les accéléromètres, on connaitrait de moins en moins bien les valeurs de tangage et de roulis de l'objet avec le temps. (26) L'estimation à partir des valeurs mesurées par les accéléromètres fait « décroitre » la matrice de covariance 151pour donner Pt (cf. Équation (8)). Cette « décroissance » ou « diminution » est paramétrée par la matrice de covariance 11, qui reflète la précision ou l'incertitude des accéléromètres. En conséquence, si on pose Rt=c52, / avec d, l'écart-type corrigé en fonction de la valeur de l'accélération propre mesurée, les valeurs mesurées de vitesse angulaire et d'accélération fournies respectivement par les gyromètres et les accéléromètres seront alors gérées de façon à prendre en compte la présence d'accélération propre dans l'estimation du tangage et du roulis et rendre cette estimation plus fiable.

Afin de mieux comprendre la présente invention un exemple de réalisation d'un dispositif de correction d'assiette d'un support d'un équipement motorisé de manutention utilisant un dispositif inertiel tel que décrit précédemment et qui fournit, à chaque instant, une estimation du tangage et du roulis. L'équipement motorisé de manutention correspond ici à un engin de chantier 1 représenté sur la figure 4. L'engin de chantier 1 comprend notamment un transpalette 3 monté sur un porte-outil 5 de l'engin de chantier 1 situé à l'extrémité d'un bras articulé 7 et piloté par un ensemble de vérins ou actionneurs 9, par exemple des vérins hydrauliques. Afin de permettre de garder le transpalette 3 horizontal, lors d'un chargement, on utilise un dispositif inertiel situé, par exemple, au niveau du porte-outil 5 de manière à connaître le tangage et le roulis du transpalette 3 lors des manipulations du bras articulé 7 par un opérateur et pouvoir ainsi corriger l'assiette du transpalette 3 si le tangage ou le roulis deviennent trop important pour maintenir le transpalette horizontal. Le dispositif inertiel comprend trois gyromètres et trois accéléromètres de type MEMS (« MicroElectroMechanical Systems » en anglais), un microcontrôleur qui permet l'acquisition des mesures, un microprocesseur pour réaliser le filtrage dynamique, une mémoire flash pour sauvegarder les différents paramètres et notamment les valeurs ou fonctions associées au facteur correctif pour l'écart-type associé aux mesures accélérométriques en fonction de l'accélération propre. Lors de la mise en route de l'engin de chantier 1, une phase d'initialisation, lorsque l'engin 1 30 est à l'arrêt et que le bras articulé 7 est immobile, est effectuée pour initialiser le dispositif inertiel. En effet, en l'absence de mouvement de l'engin 1 et du bras articulé 7, les accéléromètres du dispositif inertiel ne mesurent que l'accélération due à la gravité et peuvent ainsi réaliser une première estimation fiable du tangage et du roulis du transpalette 3. Après la phase d'initialisation, lorsque le bras articulé 7 est piloté par l'opérateur, par exemple par le biais de commandes hydrauliques qui pilotent les vérins 9 ou lors du déplacement de l'engin 1 sur le chantier, le dispositif inertiel détermine à chaque instant le tangage et le roulis du transpalette 3 et envoie ces mesures vers un dispositif correcteur d'assiette. Ce dernier corrige alors le pilotage des vérins hydrauliques 9 pour effectuer une correction automatique d'horizontalité du transpalette 3. Ainsi, à chaque instant, le dispositif inertiel détermine l'accélération propre du transpalette 3 et le facteur correctif à appliquer à l'écart-type associé à ces mesures accélérométriques et utilise l'écart-type corrigé pour déterminer la matrice de covariance associée aux mesures accélérométriques dans le filtrage dynamique permettant d'estimer le tangage et le roulis du transpalette 3. Les valeurs de tangage et de roulis estimées par le dispositif inertiel sont alors transmises au dispositif correcteur d'assiette qui peut alors corriger le pilotage des vérins pour maintenir le transpalette 3 horizontal lorsqu'il est chargé. Ce type de dispositif inertiel peut être utilisé pour toute application dans laquelle on veut déterminer le tangage et le roulis à partir de trois gyromètres et trois accéléromètres comme 20 par exemple dans un chariot élévateur ou un robot. Ainsi, l'utilisation d'un dispositif inertiel comprenant trois gyromètres et trois accéléromètres dans lequel les mesures des gyromètres et des accéléromètres sont fusionnées par un observateur d'état comme par exemple un filtrage dynamique et dans lequel 25 l'accélération propre de l'objet auquel est fixé le dispositif inertiel est prise en compte pour pondérer l'importance des mesures accélérométriques dans le filtrage dynamique permet d'améliorer la fiabilité de l'estimation du tangage et du roulis et ainsi d'utiliser des gyromètres ou accéléromètres à bas coût présentant une incertitude relativement importante. Des gyromètres et accéléromètres de type MEMS peuvent donc être utilisés pour fournir un 30 dispositif inertiel donnant une estimation du tangage et du roulis d'un objet dont l'incertitude est suffisante pour des applications industrielles. De plus, le type de filtrage dynamique utilisé peut être adapté en fonction de la non-linéarité des gyromètres. Ainsi, des gyromètres de moindre qualité présentant des non-linéarités seront compensés par une puissance de calcul plus importante, par exemple un filtre de Kalman « unscented » pour obtenir la fiabilité désirée. Par ailleurs, un tel dispositif inertiel ne comprend pas de magnétomètre et peut ainsi être utilisé dans des environnements comprenant de nombreux métaux et étant hautement magnétique.

Claims (18)

1. REVENDICATIONS1. Procédé d'estimation du tangage et du roulis d'un objet modélisé par un système dynamique comprenant les étapes suivantes : - on mesure des valeurs d'accélération de l'objet selon trois directions non coplanaires (101), - on mesure des valeurs de vitesse angulaire de l'objet autour de trois axes correspondant aux trois directions non coplanaires (101), - on définit un état du système dynamique lié au tangage et au roulis de l'objet, - on estime l'état du système dynamique par un observateur d'état dans lequel on calcule l'état du système dynamique à partir d'une part des valeurs mesurées de vitesse angulaire et d'autre part d'une estimation antérieure de l'état du système dynamique (104) et on corrige l'état calculé à partir des valeurs mesurées d'accélération (105), - on récupère les valeurs estimées de tangage et de roulis de l'objet à partir de l'état estimé du système dynamique, caractérisé en ce que l'étape d'estimation comprend les sous-étapes suivantes : - on détermine, à partir des valeurs mesurées d'accélération, une valeur de l'accélération propre de l'objet (102), - on pondère la correction des valeurs calculées de tangage et de roulis en fonction de la valeur de l'accélération propre déterminée (103).
2. 2. Procédé selon la revendication 1 dans lequel l'observateur d'état est un filtrage dynamique.
3. 3. Procédé selon la, revendication 1 ou 2 dans lequel les trois directions non coplanaires sont orthogonales.
4. 4. Procédé selon l'une des revendications précédentes dans lequel on pondère lacorrection des valeurs calculées de tangage et de roulis en appliquant un facteur correctif à l'écart-type (a) associé aux valeurs mesurées d'accélération.
5. 5. Procédé selon l'une des revendications 2 à 4 dans lequel les étapes de mesure des valeurs d'accélération et de vitesse angulaire sont réalisées selon un échantillonnage temporel et dans lequel le filtrage dynamique est un filtrage récursif dont la récurrence correspond à une période d'échantillonnage (te).
6. 6. Procédé selon l'une des revendications précédentes dans lequel l'état du système dynamique correspond à la projection de la gravité sur les axes d'un repère mobile lié au système dynamique.
7. 7. Procédé selon la revendication 6 dans lequel la dynamique de l'état du système dynamique est linéarisé et dans lequel les bruits associés à l'état et les bruits associés aux valeurs mesurées de vitesse angulaire sont additifs.
8. 8. Procédé selon la revendication 6 ou 7 en combinaison avec la revendication 2 dans lequel le système dynamique est décrit par des équations descriptives comprenant une matrice de covariance d'état (P), une matrice de covariance des bruits de mesure (R) et une matrice de covariance des bruits d'état (Q) et dans lesquelles on utilise une forme factorisée de ces matrices de covariance afin d'obtenir un filtrage dynamique numériquement stable.
9. 9. Procédé selon les revendications 4 et 8 dans lequel la pondération de la correction des valeurs calculées de tangage et de roulis est réalisée en appliquant un facteur correctif à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération du système dynamique au niveau de la matrice de covariance des bruits de mesure (R).
10. 10. Procédé selon la revendication 9 dans lequel on définit au moins un seuil d'accélération propre et si l'accélération propre est au-dessous du au moins un seuil,on applique un premier facteur correctif à la valeur à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération et si l'accélération propre est au-dessus du au moins un seuil, on applique un deuxième facteur correctif à l'écart-type (a) associé au bruit des valeurs mesurées d'accélération.
11. 11. Procédé selon la revendication 9 dans lequel le facteur correctif appliqué à l'écart-type (G) associé au bruit des valeurs mesurées d'accélération est défini en fonction de l'accélération propre déterminée par une fonction continue prédéterminée.
12. 12. Procédé selon l'une des revendications 2 à 11 dans lequel le filtrage dynamique est réalisé par un filtre de Kalman étendu.
13. 13. Procédé selon l'une des revendications 2 à 11 dans lequel le filtrage dynamique est réalisé par un filtre de Kalman « unscented ».
14. 14. Procédé selon la revendication 2 à 11 dans lequel le filtrage dynamique est réalisé par un filtre à particules.
15. 15. Programme d'ordinateur destiné à être associé à un dispositif inertiel comprenant trois accéléromètres et trois gyromètres dont les axes sensibles sont disposés de manière non coplanaire, ledit système inertiel étant fixé à un objet dont on désire estimer le tangage et le roulis, ledit programme d'ordinateur comprenant des instructions adaptées pour mettre en oeuvre les étapes suivantes : - recevoir des valeurs mesurées d'accélération et de vitesse angulaire issues respectivement des accéléromètres et des gyromètres du dispositif inertiel, - modéliser l'objet par un système dynamique et définir un état du système dynamique lié au tangage et au roulis de l'objet, - estimer l'état du système dynamique en appliquant un observateur d'état dans lequel l'état du système dynamique est calculé à partir d'une part des valeurs mesurées de vitesse angulaire et d'autre part d'une estimation antérieure de l'état du système dynamique et en corrigeant l'état calculé à partir des valeurs mesurées d'accélérations(105), - récupérer les valeurs estimées de tangage et de roulis d l'objet à partir de l'état estimé du système dynamique, caractérisé en ce que l'étape d'estimation comprend les sous-étapes suivantes : - déterminer, à partir des valeurs mesurées d'accélération, une valeur de l'accélération propre de l'objet (102), - pondérer la correction des valeurs calculées de tangage et de roulis en fonction de la valeur de l'accélération propre déterminée (103).
16. 16. Dispositif inertiel destiné à estimer le tangage et le roulis d'un objet, ledit dispositif inertiel comprenant : - trois gyromètres disposés selon trois directions non coplanaires, - trois accéléromètres disposés selon les trois directions non coplanaires, - des moyens de traitement configurés pour : - modéliser l'objet par un système dynamique et définir un état du système dynamique lié au tangage et au roulis de l'objet, - estimer l'état du système dynamique en appliquant un observateur d'état dans lequel l'état du système dynamique est calculé à partir d'une part des valeurs mesurées de vitesse angulaire fournies par les gyromètres et d'autre part d'une estimation antérieure de l'état du système dynamique et en corrigeant l'état calculé à partir des valeurs mesurées d'accélérations fournies par les accéléromètres (105), - récupérer les valeurs estimées de tangage et de roulis de l'objet à partir de l'état estimé du système dynamique, caractérisé en ce que les moyens de traitement sont également configurés pour : - déterminer, à partir des valeurs mesurées d'accélération fournies par les accéléromètres, une valeur de l'accélération propre de l'objet (102), - pondérer la correction des valeurs calculées de tangage et de roulis en fonction de la valeur de l'accélération propre déterminée (103).
17. 17. Equipement motorisé comprenant des moyens d'actionnement permettant de faire varier le tangage et le roulis d'un élément associé à l'équipement motorisécaractérisé en ce qu'un dispositif inertiel selon la revendication 16 est fixé à l'élément associé à l'équipement motorisé et en ce que les moyens d'actionnement sont pilotés en fonction des valeurs estimées de tangage et de roulis fournies par le dispositif inertiel.
18. 18. Equipement motorisé selon la revendication 17 caractérisé en ce que ledit équipement motorisé est un engin de manutention comprenant un bras articulé piloté par des vérins hydrauliques, en ce que l'élément associé est un outil de levage fixé sur le bras articulé, ledit engin de manutention comprenant un dispositif correcteur d'assiette qui pilote les vérins hydrauliques de manière à corriger l'orientation de l'outil de levage en fonction des valeurs estimées de tangage et de roulis fournies par le dispositif inertiel.15