Description du calcul de l'azimut et de la pente à partir des données d'un capteur 6 axes. Préliminaires : La terre est munie d'un repère (XT, YT, ZT) s'appuyant sur les vecteurs G (accélération de la pesanteur) et H (Champ magnétique terrestre) : ZT est dans la même direction que G, XT est suivant la composante horizontale de H (Figure 1). Dans ce repère, G a pour composantes (0, 0, G) et celle de H sont (H*cos , 0, H*sin ), où est l'inclinaison du champ magnétique par rapport à l'horizontale. Le capteur est aussi muni d'un repère propre (X, Y, Z), qui s'appuie sur la direction de visée et la position de l'appareil : X est vers l'avant (sens de visée). Quand X et Y sont horizontaux et que Y est à droite de X, Z est vers le bas. Le repère du capteur (X, Y, Z) coïncide avec le repère terrestre (XT, YT, ZT) quand X est orienté vers le Nord et Z est vers le bas. Le capteur est composé de 6 petits capteurs (3 accéléromètres et trois magnétomètres) disposés sur les trois axes afin de mesurer les composantes de G(GX, GY, GZ) et H(HX, HY, HZ ) dans le repère du capteur. XT Sens de Visée X Figure 1 ZT H YT Nord Magnétique Y G Z TERRE CAPTEUR Le capteur peut être tenu dans n'importe quelle position. Celle ci est alors définie dans le repère terrestre par trois angles (Figure 2): On désigne par NX la projection de X sur le plan horizontal et par N Y sa perpendiculaire vers la droite dans le même plan (Intersection du plan (YZ) avec le plan horizontal). (Azimut) est l'angle horizontal entre la direction de visée X et le Nord magnétique H (rotation autour de ZT, angle (XT, NX), de 0° à 360°). (Inclinaison) est l'angle vertical que la direction de visée X fait avec le plan horizontal (rotation autour de NY, angle (X, NX), de -90° à +90°). (Roulis) est l'angle dans le plan (YZ) que la direction Y fait avec l'horizontale (rotation autour de X, angle (Y, NY), de 0° à 360°). Figure 2 Objets Verticaux Objets Horizontaux Direction quelconque X XT NX YT XYZ XT YT ZT NX NY ZT Y NY ZT = (XT, NX), = (X, NX), = (Y, NY). Plan Horizontal Z Le but est de trouver une expression de et en fonction des grandeurs mesurées, soit G X, GY, GZ, HX, HY et H Z. Expression du Roulis : Cet angle ne sert qu'a déterminer ensuite l'angle . On n'a pas besoin de le connaître, mais juste d'avoir l'expression de son sinus et de son cosinus. est l'angle dans le plan (YZ) entre Y et l'horizontale (figure 4). Dans ce plan, les composante de G sont : (GY, GZ). Si est positif, alors GY est négatif et vice versa. Y X Figure 3 GY Plan (YZ) NY GZ (GY²+GZ²) On a donc : SIN COS Z GY GY GZ 2 Equation 1 2 GZ GY GZ 2 Equation 2 2 Expression de l'inclinaison : est l'angle vertical entre l'horizontale et X (figure 3). Dans un plan vertical passant X et G et repéré par X et sa perpendiculaire (intersection avec le plan (YZ)), les composante de G sont : (GX, (GY²+GZ²)). Si est positif, alors GX est négatif et vice versa. X Figure 4 NY Plan (XZT) NX GX (GY²+GZ²) G On obtient : SIN GX G X GY GZ 2 2 GY GZ 2 COS 2 2 GX G 2 G X GY GZ 2 Equation 3 GY GZ 2 2 G 2 Equation 4 Comme varie de –90° à +90° on remarquera que cet angle est entièrement déterminé par la première équation. La seconde nous serra cependant utile pour calculer l'azimut. Si la formule pour la tangente est plus simple (un carré et une addition en moins), elle introduit cependant un problème de division par 0 quand est égal à –90° ou +90°, ce qui compliquerai l'algorithme de calcul. De plus il est toujours intéressant de connaître la valeur d'un invariant comme G pour contrôler les erreurs. Enfin, G devra obligatoirement être calculé pour obtenir l'azimut. Expression de l'azimut : est l'angle dans le plan horizontal entre la projection XT du Nord magnétique H et la projection NX de la direction de visée X (figure 5). Par abus de langage, nous désignerons par NX et NY les composantes de H dans le repère (NX, NY) du plan horizontal. De même que nous introduirons par la suite sa composante verticale N Z, à des fins de vérification. NX Figure 5 XT NX Plan horizontal (XTYT) ou (NXNY) ZT NY NY On a donc : Si NX = 0 (Excepté à la verticale du pôle magnétique, on ne peut avoir à la fois N X = 0 et NY = 0) : Si NY > 0 : 270 Equation 5 Si NY < 0 : 90 Equation 6 Si NX < 0 : NY NX 180 * Equation 7 NY NX 180 * Equation 8 180 arcTan Si NX > 0 : Si NY > 0 : 360 arcTan Si NY 0 : NY NX arcTan 180 * Equation 9 Il ne reste maintenant qu'à déterminer les valeur de NX et NY en fonction des composantes mesurée de H, c'est à dire en fonction de HX, HY et HZ. Pour cela, on commence par déterminer les composantes de H dans le plan (YZ) repéré par NY et la droite d'intersection avec le plan vertical passant par X (dirigée suivant Z quand = 0), (figure 6). Nous appellerons PZ la composante de H selon la droite d'intersection avec le plan vertical passant par X. HX n'intervient pas car il est normal à ce plan. X Y HY Figure 6 NY Plan (YZ) PZ HZ Z NY On a alors : N Y H Y * COS H Z * SIN Equation 10 PZ H Z * COS H Y * SIN On se place maintenant dans le plan vertical passant par X repéré par N X et la verticale descendante que nous appelons NZ, comme la composante de H selon cet axe (figure 7). Cette fois, c'est NY qui est normal est qui n'intervient plus. X HX NY Figure 7 NX Plan Vertical passant par X NX NZ PZ NZ On en déduit : N X H X * COS PZ * SIN Equation 11 N Z PZ * COS H X * SIN Il ne reste plus qu'à mixer les équations 1, 2, 3, 4, 10 et 11 : D'abord les 10 et 11 : N X H X * COS H Z * COS H Y * SIN * SIN N Y H Y * COS H Z * SIN N Z H Z * COS H Y * SIN * COS H X * SIN Equation 12 L'expression de NZ nous sert à vérifier qu'on a bien NX² + NY² + NZ² = HX² + HY² + HZ². Une fonction de l'appareil calculant H et à partir de ces équations permettrai d'ailleurs de contrôler qu'il n'y a pas d'anomalies du champ magnétique lors de la mesure. On insère maintenant les résultats des équations 1, 2, 3 et 4 : G G X GY G Z 2 2 H X GY G Z 2 NX NY 2 H G H G Y Y Z Z 2 2 GY G Z 2 G G X G Equation 13 H Y G Z H Z GY GY G Z 2 2 Et donc : Tan G H Y GZ H Z GY H X GY GZ 2 2 G X H Z GZ H Y GY Equation 14 RESUME : Procédure de calcul à utiliser pour déterminer et Les 6 capteurs (3 accéléromètres et trois magnétomètres) sont disposés sur trois axes afin de mesurer les composantes G(GX, GY, GZ) de la gravité et H(HX, HY, HZ ) du champ magnétique dans le repère (X, Y, Z) du capteur. X est vers l'avant (sens de visée). Quand X et Y sont horizontaux et que Y est à droite de X, Z est vers le bas. Calcul de G : G GX GY GZ 2 2 2 Equation 15 La racine pourra être déterminée au moyen d'un algorithme très simple. Calcul de : G X 180 G arcSin Equation 16 La fonction arc sinus pourra être déterminée par une table de valeurs inscrite en ROM. Calcul des composantes horizontales de H, puis détermination ou calcul de en fonction de leurs signes : En fait, il suffit de calculer : 2 2 N X H X GY G Z G X H Z G Z H Y GY Equation 17 N Y H Y G Z H Z GY Si NX' = 0 : Si NY ' > 0 : 270 Equation 18 90 Equation 19 Si NY '< 0 : Si NX' 0 : On calcule : G N 180 Y * NX arcTan Equation 20 Comme précédemment, la fonction arcTan pourra être déterminée à l'aide d'une table de correspondance inscrite en mémoire. Si NX' < 0 : 180 Equation 21 Si NX' > 0 : Si NY ' > 0 : 360 Equation 22 Si NY ' 0 : Equation 23 © Jérôme Ravier, 2000