Le concept de quaternions est fascinant avec diverses applications, mais en robotique, nous nous soucions des quaternions principalement en tant que paramétrisation très efficace de l’orientation 3D, principalement celle de l’effecteur final du robot. Il existe des dizaines de présentations approfondies sur les quaternions, mais aucune n’est destinée aux programmeurs de robots industriels. Pour combler le vide, ce tutoriel présente le strict minimum sur les quaternions que les utilisateurs de robots industriels doivent connaître.
Quaternions comme extension des nombres complexes
Les quaternions ont été inventés par le mathématicien irlandais W. R. Hamilton en 1843. Ils sont essentiellement une extension des nombres complexes. Comme vous l’avez appris au lycée, un nombre complexe est représenté par
a + bi,
où a et b sont des nombres réels, et i est un nombre imaginaire dit unitaire tel que i2 = −1.
« Les quaternions sont essentiellement une extension des nombres complexes. »
De la même manière, un quaternion est représenté par
q = q1 + q2 i + q3 j + q4 k,
où i2 = j2 = k2 = −1, ij = k = −ji, jk = i = −kj, and ki = j = −ki. Souvent, nous désignons simplement les quaternions comme le quadruple {q1, q2, q3, q4}.
En utilisant les identités qui viennent d’être données, si p = {p1, p2, p3, p4} est un autre quaternion, on peut facilement montrer que le produit de q fois p, appelé produit de Hamilton, est défini par
en 1775, le scientifique suisse Leonhard Euler a prouvé que toute réorientation d’un corps rigide équivaut à une seule rotation autour d’un axe. Ainsi, toute orientation dans l’espace 3D peut être représentée par un vecteur unitaire u le long de cet axe et l’angle de rotation, θ, où le sens de rotation est déterminé par la règle de la main droite. Nous utiliserons donc désormais le terme d’axe dirigé défini par un vecteur unitaire et ne répéterons pas que le sens des rotations positives autour de cet axe dirigé se mesure par la règle de la main droite. Ainsi, si u et θ représentent une orientation dans l’espace, alors −u et −θ représentent la même orientation, ainsi que −u et 360°−θ.
« Toute réorientation d’un corps rigide équivaut à une simple rotation autour d’un axe. »
Imaginez que vous déplacez l’effecteur final de votre robot en mode de réorientation autour de son TCP (point central de l’outil). Même après une longue séquence de rotations autour des axes x, y et z du cadre de référence de l’outil, vous pouvez toujours revenir à votre orientation d’origine en utilisant une seule rotation de moins de 180° autour d’un axe passant par le TCP. En fait, si vous exécutez une commande de mouvement linéaire (MoveLin, dans le cas des robots Mecademic) avec la même position inchangée du TCP mais l’orientation d’origine du référentiel de l’outil comme arguments, l’effecteur effectuera exactement cette rotation unique. de moins de 180° autour d’un axe passant par le TCP. Nous reviendrons plus tard sur cette interpolation dite SLERP.
Quaternions comme rotations dans l’espace 3D
Un quaternion unitaire, q, tel que |q| = q12 + q22 + q32 + q42 = 1, peut également être représenté par
q = {cos(θ/2), ux sin(θ/2), uy sin(θ/2), uz sin(θ/2)},
où ux, uy et uz sont les composantes d’un vecteur unitaire u et θ est un angle. On peut montrer, bien que ce ne soit pas trivial, que si nous supposons que le quaternion unitaire ci-dessus représente une rotation de θ autour d’un axe dirigé défini par le vecteur u, nous nous retrouverons avec quelques propriétés élégantes.
Par exemple, puisque la même orientation peut être représentée par une rotation de 360°−θ autour d’un axe dirigé défini par le vecteur −u, on peut facilement montrer que
En d’autres termes, si nous nions tous les termes d’un quaternion, le nouveau et l’ancien représentent la même orientation.
Le référentiel 1 est obtenu en faisant tourner le référentiel 0 d’un angle θ autour de l’axe directeur défini par le vecteur unitaire u
Quaternions unitaires en tant que transformations
Maintenant, soit Rq la matrice de rotation 3 × 3, que vous connaissez bien, représentant la même orientation que le quaternion q, et soit Rp la matrice de rotation représentant la même orientation qu’un autre quaternion unitaire p. Alors, l’orientation représentée par la matrice correspondant au produit RqRp est la même que l’orientation représentant le produit quaternionique qp (qui a été définie au tout début). De même, l’orientation représentée par la matrice correspondant au produit RpRq est la même que l’orientation représentée par le produit quaternionique pq.
Ainsi, alors que le calcul de la transformation correspondant à deux rotations successives nécessite 27 multiplications et 18 additions/soustractions lorsqu’on utilise des matrices de rotation, la même opération ne nécessite que 16 multiplications et 12 additions/soustractions lorsqu’on utilise des quaternions.
Comme vous vous en souvenez probablement, l’inverse d’une matrice de rotation est identique à sa transposée :
Rq−1 = RqT
L’inverse d’un quaternion n’est pas seulement plus facile à calculer, il est aussi plus intuitif. L’inverse d’une rotation de θ autour d’un axe dirigé défini par le vecteur unitaire u est simplement une rotation de −θ autour du même axe dirigé. En d’autres termes, l’inverse du quaternion unitaire q est :
q−1 = {cos(−θ/2), ux sin(−θ/2), uy sin(−θ/2), uz sin(−θ/2)} = {cos(θ/2), −ux sin(θ/2), −uy sin(θ/2), −uz sin(θ/2)}.
Ainsi, nous avons
q−1q = qq−1 = {1, 0, 0, 0},
qui ne correspond à aucune orientation (similaire à la matrice identité).
Enfin, vous connaissez sûrement la façon dont les coordonnées exprimées dans un référentiel sont exprimées dans un autre, à l’aide de matrices de rotation. Cette opération est équivalente au produit triple quaternion suivant :
qvq−1,
où v = {0, vx, vy, vz} est un quaternion dit pur, et vx, vy et vz sont les coordonnées à transformer.
Multiplier une matrice de rotation par un vecteur de position nécessite 9 multiplications et 6 additions/soustractions. L’opération équivalente utilisant des quaternions nécessite plus du double : 18 multiplications et 13 additions/soustractions.
Comme nous venons de le voir, le quaternion unitaire peut représenter une rotation, tandis qu’un quaternion pur peut représenter une translation. Mais comment les combiner pour effectuer des transformations dans l’espace, comme on le fait avec des matrices homogènes 4×4 ? Cette opération est réalisée à l’aide de ce qu’on appelle des doubles quaternions.
Dans ce didacticiel, nous ne présenterons pas de quaternions doubles, car ils ne sont guère utiles au programmeur de robots typique. Cependant, il convient de mentionner leurs avantages par rapport aux matrices homogènes. En termes d’efficacité de calcul, les deux méthodes sont similaires, mais les quaternions doubles sont plus compacts et plus robustes numériquement.
Conversion entre matrices de rotation et quaternions unitaires
Si les quaternions sont très utiles — et parfois même indispensables — lorsqu’il s’agit d’orientations 3D, pour définir le quaternion correspondant à l’orientation d’un référentiel par rapport à un autre, il faut généralement calculer au préalable la matrice de rotation correspondante. Nous avons donc besoin de la formule suivante pour calculer les composantes d’un quaternion, q, à partir des éléments d’une matrice de rotation, R :
q1 = 1/2 √1 + r1,1 + r2,2 + r3,3,
q2 = 1/2sgn(r3,2 − r2,3)√1 + r1,1 − r2,2 − r3,3,
q3 = 1/2sgn(r1,3 − r3,1)√1 − r1,1 + r2,2 − r3,3,
q4 = 1/2sgn(r2,1 − r1,2)√1 − r1,1 − r2,2 + r3,3,
où
R =
r1,1
r1,2
r1,3
r2,1
r2,2
r2,3
r3,1
r3,2
r3,3
.
Bien sûr, rappelons que si q est le quaternion unitaire correspondant à la matrice de rotation R, alors −q est l’autre quaternion correspondant à la même orientation.
De même, si vous avez un quaternion unitaire q et que vous souhaitez le convertir en une matrice de rotation R, la formule est :
R =
2q12 + 2q22 − 1
2q2q3−2q1q4
2q2q4 + 2q1q3
2q2q3 + 2q1q4
2q12 + 2q32 − 1
2q3q4 − 2q1q2
2q2q4 − 2q1q3
2q3q4 − 2q1q2
2q12 + 2q42 − 1
.
Utilisation des quaternions unitaires pour mesurer les déplacements de rotation
Comme nous l’avons vu dans notre tutoriel sur les angles d’Euler, ces angles sont presque toujours la seule façon dont l’orientation est définie par les utilisateurs et leur est représentée dans le cas des robots industriels. Ainsi, c’est une erreur très courante de faire un parallèle entre la nature des coordonnées de position et les angles d’Euler lorsqu’il s’agit de mesurer un déplacement relatif, ou une « distance ». En fait, même la norme ISO 9283, qui dicte comment les performances du robot doivent être mesurées et représentées, stipule que la précision d’orientation est essentiellement mesurée avec exactement la même formule que la précision de position, mais avec des angles d’Euler remplaçant les coordonnées de position x, y et z. Pourtant, à l’instar des singularités de représentation des angles d’Euler (communément appelées effet de verrouillage Gimbal), les angles d’Euler peuvent varier considérablement, même si l’orientation ne change que légèrement.
Cela n’a aucun sens d’utiliser les angles d’Euler lors de la mesure de la précision d’orientation et de la répétabilité d’un robot industriel
La seule et unique mesure du déplacement de rotation, c’est-à-dire la « distance » entre une orientation initiale et la finale, peut être facilement calculée à l’aide de quaternions unitaires. En effet, soit q le quaternion unitaire représentant l’orientation du référentiel outil à la pose 1, et p le quaternion unitaire représentant l’orientation du référentiel outil à la pose 2. L’orientation du référentiel outil à la pose 2 par rapport à le cadre de l’outil à la pose 1 est
où wx, wy et wz sont les composantes d’un vecteur unitaire w. En d’autres termes, pour passer de l’orientation du cadre d’outil à la pose 1 à l’orientation à la pose 2, le cadre d’outil doit tourner φ autour d’un axe dirigé défini par le vecteur w. Maintenant, puisque nous ne nous intéressons qu’à la rotation la plus courte possible, nous voulons 0 < φ ≤ 180°. Par conséquent, si nous voyons que la première composante du quaternion résultant d est négative, nous devrions simplement nier le quaternion. (Rappelez-vous que d = −d.) Ensuite, pour calculer la rotation la plus courte équivalente entre les deux orientations, nous devons calculer le cosinus inverse de la première composante de d et le multiplier par 2.
De même, beaucoup supposent à tort que la vitesse angulaire d’un corps dans l’espace est le vecteur des dérivées des angles d’Euler. Le vecteur vitesse angulaire, ω, est en fait le vecteur unitaire correspondant à l’axe de rotation instantané dirigé multiplié par la vitesse de rotation autour de cet axe.
Utilisation de quaternions pour interpoler entre les orientations
Comme nous venons de le voir, le moyen le plus rapide pour passer d’une orientation à une autre est de tourner autour de l’axe de rotation équivalent d’un angle ne dépassant pas 180°. Ce chemin de rotation est souvent appelé chemin de couple minimal. Ainsi, pour obtenir une série « équidistante » d’orientations qui vous font passer d’une orientation représentée par le quaternion q, à une orientation représentée par le quaternion p, nous devons utiliser la formule simple suivante :
Slerp(q,p,t) = q(q−1p)t,
où t est un paramètre variant de 0 (quand à l’orientation q) à 1 (quand à l’orientation p). La procédure ci-dessus est appelée interpolation linéaire sphérique, ou SLERP.
Élever le quaternion d = q−1p = {cos(φ/2), wx sin(φ/2), wy sin(−φ/2), wz sin(−φ/2)} à la puissance t équivaut à tournant t fois d’un angle φ, donc
Ainsi, si t = 0, 1/n, 2/n, …, 1, SLERP « ajoute » essentiellement une rotation de φ/n à chaque étape de discrétisation, où n − 1 est le nombre d’orientations intermédiaires. Bien sûr, φ ne doit pas être supérieur à 180°, sinon il faut annuler l’un des deux quaternions et refaire les calculs.
On peut montrer que la formule SLERP ci-dessus est équivalente à l’équation suivante, qui est un peu plus rapide à calculer :
où φ est tel qu’il vient d’être défini, c’est-à-dire l’angle de rotation entre q et p. Notez que les deux fonctions SLERP sont équivalentes et que le résultat est un quaternion unitaire pour tout t.
A faster way to interpolate between orientations along the same rotation path about the equivalent axis of rotation is using the following simple formula:
Lerp(q,p,t) = (1 − t)q + tp,
where t is again a parameter varying between 0 and 1. However, this so-called LERP interpolation works relatively well only for orientations that are close to each other (e.g., φ < 90°). This is because the reorientation is not performed at constant angular speed. Besides, with LERP, you need to normalize the resulting quaternion at each step t (then the method is actually called NLERP).
Most industrial robots use a SLERP interpolation when reorienting the end-effector along a linear path. Obviously, if the start and end orientations are 180° apart, then there would be two identical minimum-torque paths. The robot controller can arbitrarily choose one of them, but that means that the user cannot anticipate the motion of the end-effector. Thus, industrial robots always have some kind of limits on the displacements along a linear path. For example, Mecademic robots simply refuse a linear motion towards a pose with an orientation that is exactly 180° away.
Quaternions vs angles d’Euler
Chaque fois que je postais sur les angles d’Euler, il y avait toujours des gens qui les critiquaient et louaient les quaternions. En effet, les angles d’Euler – et toute représentation à trois paramètres de l’orientation dans l’espace – ont des singularités de représentation par lesquelles une orientation peut être représentée avec une infinité de triplets. (Ceci est similaire au problème de la représentation des coordonnées aux pôles Nord ou Sud sur Terre – la latitude n’est pas définie de manière unique aux deux pôles.)
Bonne chance pour calculer à la main le quaternion représentant l’orientation de l’image 1 par rapport à l’image 0
Les angles d’Euler sont sûrement insuffisants pour faire face à des orientations qui varient. Cependant, en robotique, les angles d’Euler sont généralement utilisés pour définir un cadre de référence fixe par rapport à un autre, tel que le cadre de référence de l’outil par rapport au cadre de référence de la bride. Comme le montre la figure ci-dessus, il est généralement impossible de calculer à la main le quaternion correspondant à l’orientation d’un référentiel par rapport à un autre, alors qu’il est souvent facile de deviner les angles d’Euler par tâtonnements.
« Les angles d’Euler sont sûrement insuffisants pour faire face à des orientations qui varient. »
Il existe cependant des situations où il est impossible de deviner également les angles d’Euler, et vous devez les calculer à l’aide de formules trigonométriques impliquant les éléments d’une matrice de rotation. Par exemple, dans la figure ci-dessus, pour l’orientation du repère 1 par rapport au repère 0, les angles d’Euler selon la convention mobile XYZ utilisée par Mecademic Robotics sont {0°, 45°, 40°}, mais les angles d’Euler selon à la convention mobile ZYX utilisée par KUKA sont {49,879°, 32,798°, 32,732°}, et les quaternions utilisés par ABB sont {0,868163, 0,130885, 0,359605, 0,315986}.
Néanmoins, il n’est pas surprenant que pratiquement tous les robots industriels demandent aux utilisateurs de définir des orientations à l’aide d’angles d’Euler, car ceux-ci sont plus faciles à comprendre, mais pas triviaux. ABB est une rare exception, où les quaternions sont le principal moyen d’entrer des orientations, bien que les angles d’Euler soient également une option.
Cela dit, tous les robots industriels utilisent des quaternions dans leurs contrôleurs, que ce soit pour l’interpolation d’orientation, le calcul de transformations ou à d’autres fins.
Résumé
Pour récapituler, en tant que programmeur de robots industriels, vous devez connaître les quaternions pour au moins les raisons suivantes :
être capable d’anticiper la réorientation de l’effecteur du robot le long d’une trajectoire linéaire (l’interpolation SLERP)
savoir comment mesurer correctement la précision d’orientation et la répétabilité
mieux comprendre les inconvénients des angles d’Euler
savoir utiliser les quaternions lorsqu’ils sont disponibles
Ilian Bonev, Ph.D., Eng.
Le concept de quaternions est fascinant avec diverses applications, mais en robotique, nous nous soucions des quaternions principalement en tant que paramétrisation très efficace de l’orientation 3D, principalement celle de l’effecteur final du robot. Il existe des dizaines de présentations approfondies sur les quaternions, mais aucune n’est destinée aux programmeurs de robots industriels. Pour combler le vide, ce tutoriel présente le strict minimum sur les quaternions que les utilisateurs de robots industriels doivent connaître.
Quaternions comme extension des nombres complexes
Les quaternions ont été inventés par le mathématicien irlandais W. R. Hamilton en 1843. Ils sont essentiellement une extension des nombres complexes. Comme vous l’avez appris au lycée, un nombre complexe est représenté par
a + bi,
où a et b sont des nombres réels, et i est un nombre imaginaire dit unitaire tel que i2 = −1.
De la même manière, un quaternion est représenté par
q = q1 + q2 i + q3 j + q4 k,
où i2 = j2 = k2 = −1, ij = k = −ji, jk = i = −kj, and ki = j = −ki. Souvent, nous désignons simplement les quaternions comme le quadruple {q1, q2, q3, q4}.
En utilisant les identités qui viennent d’être données, si p = {p1, p2, p3, p4} est un autre quaternion, on peut facilement montrer que le produit de q fois p, appelé produit de Hamilton, est défini par
qp = (q1p1 – q2p2 – q3p3 – q4p4) + (q1p2 + q2p1 + q3p4 – q4p3)i + (q1p3 + q3p1 – q2p4 + q4p2)j + (q1p4 + q4p1 + q2p3 – q3p2)k.
Il peut également être facilement démontré que la multiplication de quaternions n’est pas commutative, ce qui signifie qu’en général qp ≠ pq.
En revanche, l’addition de quaternions est commutative et s’effectue par composants :
q + p = p + q = (q1 + p1) + (q2 + p2)i + (q3 + p3)j + (q4 + p4)k.
Théorème de rotation d’Euler
en 1775, le scientifique suisse Leonhard Euler a prouvé que toute réorientation d’un corps rigide équivaut à une seule rotation autour d’un axe. Ainsi, toute orientation dans l’espace 3D peut être représentée par un vecteur unitaire u le long de cet axe et l’angle de rotation, θ, où le sens de rotation est déterminé par la règle de la main droite. Nous utiliserons donc désormais le terme d’axe dirigé défini par un vecteur unitaire et ne répéterons pas que le sens des rotations positives autour de cet axe dirigé se mesure par la règle de la main droite. Ainsi, si u et θ représentent une orientation dans l’espace, alors −u et −θ représentent la même orientation, ainsi que −u et 360°−θ.
« Toute réorientation d’un corps rigide équivaut à une simple rotation autour d’un axe. »
Imaginez que vous déplacez l’effecteur final de votre robot en mode de réorientation autour de son TCP (point central de l’outil). Même après une longue séquence de rotations autour des axes x, y et z du cadre de référence de l’outil, vous pouvez toujours revenir à votre orientation d’origine en utilisant une seule rotation de moins de 180° autour d’un axe passant par le TCP. En fait, si vous exécutez une commande de mouvement linéaire (MoveLin, dans le cas des robots Mecademic) avec la même position inchangée du TCP mais l’orientation d’origine du référentiel de l’outil comme arguments, l’effecteur effectuera exactement cette rotation unique. de moins de 180° autour d’un axe passant par le TCP. Nous reviendrons plus tard sur cette interpolation dite SLERP.
Quaternions comme rotations dans l’espace 3D
Un quaternion unitaire, q, tel que |q| = q12 + q22 + q32 + q42 = 1, peut également être représenté par
q = {cos(θ/2), ux sin(θ/2), uy sin(θ/2), uz sin(θ/2)},
où ux, uy et uz sont les composantes d’un vecteur unitaire u et θ est un angle. On peut montrer, bien que ce ne soit pas trivial, que si nous supposons que le quaternion unitaire ci-dessus représente une rotation de θ autour d’un axe dirigé défini par le vecteur u, nous nous retrouverons avec quelques propriétés élégantes.
Par exemple, puisque la même orientation peut être représentée par une rotation de 360°−θ autour d’un axe dirigé défini par le vecteur −u, on peut facilement montrer que
q = {cos(180°−θ/2), −ux sin(180°−θ/2), −uy sin(180°−θ/2), −uz sin(180°−θ/2)}
= {−cos(θ/2), −ux sin(θ/2), −uy sin(θ/2), −uz sin(θ/2)) = −q.
En d’autres termes, si nous nions tous les termes d’un quaternion, le nouveau et l’ancien représentent la même orientation.
Le référentiel 1 est obtenu en faisant tourner le référentiel 0 d’un angle θ autour de l’axe directeur défini par le vecteur unitaire u
Quaternions unitaires en tant que transformations
Maintenant, soit Rq la matrice de rotation 3 × 3, que vous connaissez bien, représentant la même orientation que le quaternion q, et soit Rp la matrice de rotation représentant la même orientation qu’un autre quaternion unitaire p. Alors, l’orientation représentée par la matrice correspondant au produit RqRp est la même que l’orientation représentant le produit quaternionique qp (qui a été définie au tout début). De même, l’orientation représentée par la matrice correspondant au produit RpRq est la même que l’orientation représentée par le produit quaternionique pq.
Ainsi, alors que le calcul de la transformation correspondant à deux rotations successives nécessite 27 multiplications et 18 additions/soustractions lorsqu’on utilise des matrices de rotation, la même opération ne nécessite que 16 multiplications et 12 additions/soustractions lorsqu’on utilise des quaternions.
Comme vous vous en souvenez probablement, l’inverse d’une matrice de rotation est identique à sa transposée :
Rq−1 = RqT
L’inverse d’un quaternion n’est pas seulement plus facile à calculer, il est aussi plus intuitif. L’inverse d’une rotation de θ autour d’un axe dirigé défini par le vecteur unitaire u est simplement une rotation de −θ autour du même axe dirigé. En d’autres termes, l’inverse du quaternion unitaire q est :
q−1 = {cos(−θ/2), ux sin(−θ/2), uy sin(−θ/2), uz sin(−θ/2)} = {cos(θ/2), −ux sin(θ/2), −uy sin(θ/2), −uz sin(θ/2)}.
Ainsi, nous avons
q−1q = qq−1 = {1, 0, 0, 0},
qui ne correspond à aucune orientation (similaire à la matrice identité).
Enfin, vous connaissez sûrement la façon dont les coordonnées exprimées dans un référentiel sont exprimées dans un autre, à l’aide de matrices de rotation. Cette opération est équivalente au produit triple quaternion suivant :
qvq−1,
où v = {0, vx, vy, vz} est un quaternion dit pur, et vx, vy et vz sont les coordonnées à transformer.
Multiplier une matrice de rotation par un vecteur de position nécessite 9 multiplications et 6 additions/soustractions. L’opération équivalente utilisant des quaternions nécessite plus du double : 18 multiplications et 13 additions/soustractions.
Comme nous venons de le voir, le quaternion unitaire peut représenter une rotation, tandis qu’un quaternion pur peut représenter une translation. Mais comment les combiner pour effectuer des transformations dans l’espace, comme on le fait avec des matrices homogènes 4×4 ? Cette opération est réalisée à l’aide de ce qu’on appelle des doubles quaternions.
Dans ce didacticiel, nous ne présenterons pas de quaternions doubles, car ils ne sont guère utiles au programmeur de robots typique. Cependant, il convient de mentionner leurs avantages par rapport aux matrices homogènes. En termes d’efficacité de calcul, les deux méthodes sont similaires, mais les quaternions doubles sont plus compacts et plus robustes numériquement.
Conversion entre matrices de rotation et quaternions unitaires
Si les quaternions sont très utiles — et parfois même indispensables — lorsqu’il s’agit d’orientations 3D, pour définir le quaternion correspondant à l’orientation d’un référentiel par rapport à un autre, il faut généralement calculer au préalable la matrice de rotation correspondante. Nous avons donc besoin de la formule suivante pour calculer les composantes d’un quaternion, q, à partir des éléments d’une matrice de rotation, R :
q1 = 1/2 √1 + r1,1 + r2,2 + r3,3,
q2 = 1/2sgn(r3,2 − r2,3)√1 + r1,1 − r2,2 − r3,3,
q3 = 1/2sgn(r1,3 − r3,1)√1 − r1,1 + r2,2 − r3,3,
q4 = 1/2sgn(r2,1 − r1,2)√1 − r1,1 − r2,2 + r3,3,
où
Bien sûr, rappelons que si q est le quaternion unitaire correspondant à la matrice de rotation R, alors −q est l’autre quaternion correspondant à la même orientation.
De même, si vous avez un quaternion unitaire q et que vous souhaitez le convertir en une matrice de rotation R, la formule est :
Utilisation des quaternions unitaires pour mesurer les déplacements de rotation
Comme nous l’avons vu dans notre tutoriel sur les angles d’Euler, ces angles sont presque toujours la seule façon dont l’orientation est définie par les utilisateurs et leur est représentée dans le cas des robots industriels. Ainsi, c’est une erreur très courante de faire un parallèle entre la nature des coordonnées de position et les angles d’Euler lorsqu’il s’agit de mesurer un déplacement relatif, ou une « distance ». En fait, même la norme ISO 9283, qui dicte comment les performances du robot doivent être mesurées et représentées, stipule que la précision d’orientation est essentiellement mesurée avec exactement la même formule que la précision de position, mais avec des angles d’Euler remplaçant les coordonnées de position x, y et z. Pourtant, à l’instar des singularités de représentation des angles d’Euler (communément appelées effet de verrouillage Gimbal), les angles d’Euler peuvent varier considérablement, même si l’orientation ne change que légèrement.
Cela n’a aucun sens d’utiliser les angles d’Euler lors de la mesure de la précision d’orientation et de la répétabilité d’un robot industriel
La seule et unique mesure du déplacement de rotation, c’est-à-dire la « distance » entre une orientation initiale et la finale, peut être facilement calculée à l’aide de quaternions unitaires. En effet, soit q le quaternion unitaire représentant l’orientation du référentiel outil à la pose 1, et p le quaternion unitaire représentant l’orientation du référentiel outil à la pose 2. L’orientation du référentiel outil à la pose 2 par rapport à le cadre de l’outil à la pose 1 est
ré = q−1p = {cos(φ/2), wx sin(φ/2), wy sin(φ/2), wz sin(φ/2)},
où wx, wy et wz sont les composantes d’un vecteur unitaire w. En d’autres termes, pour passer de l’orientation du cadre d’outil à la pose 1 à l’orientation à la pose 2, le cadre d’outil doit tourner φ autour d’un axe dirigé défini par le vecteur w. Maintenant, puisque nous ne nous intéressons qu’à la rotation la plus courte possible, nous voulons 0 < φ ≤ 180°. Par conséquent, si nous voyons que la première composante du quaternion résultant d est négative, nous devrions simplement nier le quaternion. (Rappelez-vous que d = −d.) Ensuite, pour calculer la rotation la plus courte équivalente entre les deux orientations, nous devons calculer le cosinus inverse de la première composante de d et le multiplier par 2.
De même, beaucoup supposent à tort que la vitesse angulaire d’un corps dans l’espace est le vecteur des dérivées des angles d’Euler. Le vecteur vitesse angulaire, ω, est en fait le vecteur unitaire correspondant à l’axe de rotation instantané dirigé multiplié par la vitesse de rotation autour de cet axe.
Utilisation de quaternions pour interpoler entre les orientations
Comme nous venons de le voir, le moyen le plus rapide pour passer d’une orientation à une autre est de tourner autour de l’axe de rotation équivalent d’un angle ne dépassant pas 180°. Ce chemin de rotation est souvent appelé chemin de couple minimal. Ainsi, pour obtenir une série « équidistante » d’orientations qui vous font passer d’une orientation représentée par le quaternion q, à une orientation représentée par le quaternion p, nous devons utiliser la formule simple suivante :
Slerp(q,p,t) = q(q−1p)t,
où t est un paramètre variant de 0 (quand à l’orientation q) à 1 (quand à l’orientation p). La procédure ci-dessus est appelée interpolation linéaire sphérique, ou SLERP.
Élever le quaternion d = q−1p = {cos(φ/2), wx sin(φ/2), wy sin(−φ/2), wz sin(−φ/2)} à la puissance t équivaut à tournant t fois d’un angle φ, donc
dt = {cos(tφ/2), wx sin(tφ/2), wy sin(−tφ/2), wz sin(−tφ/2)}.
Ainsi, si t = 0, 1/n, 2/n, …, 1, SLERP « ajoute » essentiellement une rotation de φ/n à chaque étape de discrétisation, où n − 1 est le nombre d’orientations intermédiaires. Bien sûr, φ ne doit pas être supérieur à 180°, sinon il faut annuler l’un des deux quaternions et refaire les calculs.
On peut montrer que la formule SLERP ci-dessus est équivalente à l’équation suivante, qui est un peu plus rapide à calculer :
Slerp(q,p,t) =
sin((1 − t)φ/2)
sin(φ/2) q +
sin(tφ/2)
sin(φ/2) p,
où φ est tel qu’il vient d’être défini, c’est-à-dire l’angle de rotation entre q et p. Notez que les deux fonctions SLERP sont équivalentes et que le résultat est un quaternion unitaire pour tout t.
A faster way to interpolate between orientations along the same rotation path about the equivalent axis of rotation is using the following simple formula:
Lerp(q,p,t) = (1 − t)q + tp,
where t is again a parameter varying between 0 and 1. However, this so-called LERP interpolation works relatively well only for orientations that are close to each other (e.g., φ < 90°). This is because the reorientation is not performed at constant angular speed. Besides, with LERP, you need to normalize the resulting quaternion at each step t (then the method is actually called NLERP).
Most industrial robots use a SLERP interpolation when reorienting the end-effector along a linear path. Obviously, if the start and end orientations are 180° apart, then there would be two identical minimum-torque paths. The robot controller can arbitrarily choose one of them, but that means that the user cannot anticipate the motion of the end-effector. Thus, industrial robots always have some kind of limits on the displacements along a linear path. For example, Mecademic robots simply refuse a linear motion towards a pose with an orientation that is exactly 180° away.
Quaternions vs angles d’Euler
Chaque fois que je postais sur les angles d’Euler, il y avait toujours des gens qui les critiquaient et louaient les quaternions. En effet, les angles d’Euler – et toute représentation à trois paramètres de l’orientation dans l’espace – ont des singularités de représentation par lesquelles une orientation peut être représentée avec une infinité de triplets. (Ceci est similaire au problème de la représentation des coordonnées aux pôles Nord ou Sud sur Terre – la latitude n’est pas définie de manière unique aux deux pôles.)
Bonne chance pour calculer à la main le quaternion représentant l’orientation de l’image 1 par rapport à l’image 0
Les angles d’Euler sont sûrement insuffisants pour faire face à des orientations qui varient. Cependant, en robotique, les angles d’Euler sont généralement utilisés pour définir un cadre de référence fixe par rapport à un autre, tel que le cadre de référence de l’outil par rapport au cadre de référence de la bride. Comme le montre la figure ci-dessus, il est généralement impossible de calculer à la main le quaternion correspondant à l’orientation d’un référentiel par rapport à un autre, alors qu’il est souvent facile de deviner les angles d’Euler par tâtonnements.
« Les angles d’Euler sont sûrement insuffisants pour faire face à des orientations qui varient. »
Il existe cependant des situations où il est impossible de deviner également les angles d’Euler, et vous devez les calculer à l’aide de formules trigonométriques impliquant les éléments d’une matrice de rotation. Par exemple, dans la figure ci-dessus, pour l’orientation du repère 1 par rapport au repère 0, les angles d’Euler selon la convention mobile XYZ utilisée par Mecademic Robotics sont {0°, 45°, 40°}, mais les angles d’Euler selon à la convention mobile ZYX utilisée par KUKA sont {49,879°, 32,798°, 32,732°}, et les quaternions utilisés par ABB sont {0,868163, 0,130885, 0,359605, 0,315986}.
Néanmoins, il n’est pas surprenant que pratiquement tous les robots industriels demandent aux utilisateurs de définir des orientations à l’aide d’angles d’Euler, car ceux-ci sont plus faciles à comprendre, mais pas triviaux. ABB est une rare exception, où les quaternions sont le principal moyen d’entrer des orientations, bien que les angles d’Euler soient également une option.
Cela dit, tous les robots industriels utilisent des quaternions dans leurs contrôleurs, que ce soit pour l’interpolation d’orientation, le calcul de transformations ou à d’autres fins.
Résumé
Pour récapituler, en tant que programmeur de robots industriels, vous devez connaître les quaternions pour au moins les raisons suivantes :