[Conocimientos matemáticos] Representación del ángulo de Euler de la matriz de rotación espacial tridimensional, representación de cuaterniones, conversión entre los dos, implementación del código Matlab

número de serie	contenido
1	[Conocimientos matemáticos] Grado de libertad y método de cálculo de grados de libertad
2	[Conocimientos matemáticos] Cuerpo rígido cuerpo rígido y el movimiento del cuerpo rígido
3	[Conocimientos matemáticos] Movimiento básico de cuerpos rígidos, traslación y rotación.
4	[Conocimientos matemáticos] Multiplicación de vectores, producto interno, producto externo, implementación de código matlab
5	[Conocimientos matemáticos] Método de mínimos cuadrados, comenzando con la regresión lineal, dando ejemplos numéricos y utilizando el método de mínimos cuadrados para resolver el modelo de regresión.
6	[Conocimientos matemáticos] Método de mínimos cuadrados, situación lineal general, proceso de representación matricial, proceso de fórmula de solución de parámetros óptimos
7	[Conocimiento matemático] Covarianza, la covarianza de variables aleatorias, la covarianza cuando las variables aleatorias son números únicos y vectores respectivamente
8	[Conocimiento matemático] Descomposición de valores singulares, entendida desde la perspectiva de la transformación lineal de datos
9	[Conocimientos matemáticos] El proceso de derivación de la matriz de rotación se basa en la rotación del vector, al tiempo que resuelve las limitaciones no lineales de la transformación euclidiana.
10	[Conocimientos matemáticos] Representación del ángulo de Euler de la matriz de rotación espacial tridimensional, representación de cuaterniones, conversión entre los dos, implementación del código Matlab
11	[Conocimiento matemático] Dadas las coordenadas de N> = 3 puntos en los momentos delantero y trasero, encuentre la matriz de traslación y la matriz de rotación del cuerpo rígido, y la distancia entre estos N> = 3 puntos siempre permanece sin cambios, lo que representa un cuerpo rígido

Hemos discutido las matrices de rotación antes. Nuevamente, el orden de rotación es importante y afectará el resultado final. Girar XX primero$Eje\; X$ , luego gire$Eje\; Y$ , rotación finalZZEl resultado obtenido en el$eje\; Z$$Eje\; Z$ , luego gire$Eje\; Y$ , última rotación$Los\; resultados\; obtenidos\; para\; el\; eje\; X$ son diferentes. Esta diferencia de orden da como resultado diferentes orientaciones y cambios en la orientación espacial. Es por eso que en aplicaciones prácticas necesitamos especificar explícitamente el orden de rotación para asegurarnos de obtener resultados correctos y consistentes.

Esta vez, basándonos en el espacio tridimensional, discutiremos dos métodos de representación de matrices de rotación, a saber, la representación del ángulo de Euler y la representación del cuaternión, así como la relación de conversión entre los dos.

En el espacio tridimensional, la matriz de rotación $Las\; dimensiones\; de\; R$ son$3\times 3$ , que es una matriz ortogonal con un determinante de$1$ .

1. Representación de los ángulos de Euler

Los ángulos de Euler suelen constar de tres ángulos.

• Ángulo de balanceo (rollo), el símbolo común es $\varphi$
• Ángulo de paso (paso), el símbolo común es $i$
• Ángulo de guiñada (guiñada), el símbolo común es $pag$

Estos tres ángulos describen los ángulos alrededor de $x,Y,$El ángulo de rotación del eje$Z.$

Alrededor del XXRotación del eje$X$$La\; matriz\; de\; rotación\; del\; ángulo\; \varphi$ es

$$R_{}\left(\varphi \right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \mathrm{porque}\left(\varphi \right)& -\mathrm{pecado}\left(\varphi \right)\\ & \mathrm{pecado}(\varphi & \mathrm{porque}(\varphi \end{array}\right]$$

绕 Y Y Rotación del eje$Y$$La\; matriz\; de\; rotación\; del\; ángulo\; \theta$ es

$$R_{}\left(yo\right)=\left[\begin{array}{ccc}\mathrm{porque}\left(\theta \right)& 0& \mathrm{pecado}\left(\theta \right)\\ 0& 1& 0\\ -\mathrm{pecado}(\theta & & \mathrm{porque}(\theta \end{array}\right]$$

ZZ _Rotación del eje$Z$$La\; matriz\; de\; rotación\; del\; ángulo\; \psi$ es

$$R_{}\left(pag\right)=\left[\begin{array}{ccc}\mathrm{porque}\left(\psi \right)& -\mathrm{pecado}\left(\psi \right)& 0\\ \mathrm{pecado}\left(\psi \right)& \mathrm{porque}\left(\psi \right)& 0\\ & & \end{array}\right]$$

Por ejemplo, para un eje fijo $La\; representación\; del\; ángulo\; de\; Euler\; de\; XYZ$ , su matriz de rotación es

$$\begin{array}{rl}R& =R_{}\left(\varphi \right)R_{}\left(yo\right)R_{}\left(pag\right)\\ & =\left[\begin{array}{ccc}\mathrm{porque}\left(\theta \right)\mathrm{porque}\left(\psi \right)& \mathrm{pecado}\left(\varphi \right)\mathrm{pecado}\left(\theta \right)\mathrm{porque}\left(\psi \right)-\mathrm{porque}\left(\varphi \right)\mathrm{pecado}\left(\psi \right)& \mathrm{porque}\left(\varphi \right)\mathrm{pecado}\left(\theta \right)\mathrm{porque}\left(\psi \right)+\mathrm{pecado}\left(\varphi \right)\mathrm{pecado}\left(\psi \right)\\ \mathrm{porque}\left(\theta \right)\mathrm{pecado}\left(\psi \right)& \mathrm{pecado}\left(\varphi \right)\mathrm{pecado}\left(\theta \right)\mathrm{pecado}\left(\psi \right)+\mathrm{porque}\left(\varphi \right)\mathrm{porque}\left(\psi \right)& \mathrm{porque}\left(\varphi \right)\mathrm{pecado}\left(\theta \right)\mathrm{pecado}\left(\psi \right)-\mathrm{pecado}\left(\varphi \right)\mathrm{porque}\left(\psi \right)\\ -\mathrm{pecado}(\theta & \mathrm{pecado}\left(\varphi \right)\mathrm{porque}(\theta & \mathrm{porque}\left(\varphi \right)\mathrm{porque}(\theta \end{array}\right]\end{array}$$

Esta matriz representa la primera alrededor del XXRotación del eje$X$$Ángulo\; \varphi$ , luego alrededor deYYRotación del eje$Y$$ángulo\; \theta$ , luego alrededorde ZZRotación del eje$Z$ ψ \psiEl efecto de rotación total del$ángulo\; \psi \; .$

Para obtener más información sobre la derivación y detalles de los ángulos de Euler, consulte el artículo: Capítulo 3 - Conceptos básicos del conocimiento matemático -> El proceso de derivación de la transformación de coordenadas y la matriz de rotación [conocimiento matemático], basado en la rotación de vectores, mientras se resuelve La no linealidad de las limitaciones de la transformación euclidiana .

% 给定欧拉角 phi theta psi
phi   = deg2rad(10);  % 示例：10度，记得转换为弧度
theta = deg2rad(22);  % 示例：22度
psi   = deg2rad(35);  % 示例：35度

R_x = [ 1  0         0
        0  cos(phi) -sin(phi)
        0  sin(phi)  cos(phi)];

R_y = [ cos(theta)  0  sin(theta)
        0           1  0
       -sin(theta)  0  cos(theta)];

R_z = [ cos(psi) -sin(psi)  0
        sin(psi)  cos(psi)  0
        0         0         1];

R = R_z * R_y * R_x;

R = [cos(theta)*cos(psi)  sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)  cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)
     cos(theta)*sin(psi)  sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi)  cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)
    -sin(theta)           sin(phi)*cos(theta)                             cos(phi)*cos(theta)];

R =
    0.7595   -0.5116    0.4018
    0.5318    0.8440    0.0694
   -0.3746    0.1610    0.9131

---

% 给定点坐标
point_1 = [ 10
            22
            35];
point_2 = R * point_1;

figure()
scatter3(point_1(1), point_1(2), point_1(3), 150, 'r'); hold on;
scatter3(point_2(1), point_2(2), point_2(3), 150, 'b');

---

2. Representación del cuaternión

El cuaternión está compuesto por $1$ número real más$Está\; compuesto\; por\; 3$ números complejos y generalmente se puede expresar como

$$q=q_{}+q_{}\text{i}+q_{}\text{j}+q_{}\text{k}$$

其中$q_{},q_{},q_{},q_{}$Todos son números reales, $\text{i, j, k}$ son las primitivas de los cuaterniones y satisfacen la relación de multiplicación como se muestra a continuación

• ${\text{i}}^2={\text{j}}^2={\text{k}}^2=\text{yo}\text{jk}\text{\_}=-1$
• ${\text{i}}^0={\text{j}}^0={\text{k}}^0=1$

También se puede considerar que los cuaterniones consisten en un escalar y un vector, donde $q_{}$es la parte escalar del cuaternión, $q_{},q_{},q_{}$Constituye la parte vectorial del cuaternión.

---

Supongamos que hay dos cuaterniones respectivamente $q_{}=(q_w,[q_{x1},q_y,q_{con}])$ ,$q_{}=(q_w,[q_{x2},q_{y2\_},q_z])$ , y sea$v_{}=[q_{x1},q_y,q_{con}]$，$v_{}=[q_{x2},q_{y2\_},q_z]$ , entonces existe el siguiente algoritmo

• Suma de cuaterniones: $q_{}+q_{}=(q_w+q_w,(v_{}+v_{}))$
• 四元数乘法: $q_{}{q}_{}=q_w{q}_w-v_{}\cdot v_{}+q_w{v}_{}+q_w{v}_{}+v_{}\times v_{}=(q_w{q}_w-v_{}\cdot v_{},(q_w{v}_{}+q_w{v}_{}+v_{}\times v_{}))$
• Módulo del cuaternión: $\Vert q{\_}_{}\Vert =\sqrt{q_{w1}^{}+q_{x1\_}^{}+q_{y1}^{}+q_{con1}^{}}$
• Cuaternión unitario: $\Vert q{\_}_{}\Vert =1$
• Conjugado de cuaternión: $q_1^{}=(q_w,-v{\_}_{})$
• Inverso del cuaternión: $q_1^{-}=\frac{q_1^{}}{\Vert q{\_}_{}\Vert }$

---

Los cuaterniones son un sistema de números complejos extendido que se usa comúnmente para representar rotaciones en un espacio tridimensional.

Un cuaternión unitario (longitud $1$ ) Puede representar la rotación en el espacio 3D. Se puede rotar un punto a otra posición mediante la multiplicación de cuaterniones.

Alrededor del XXRotación del eje$X$ ϕ \phiEl cuaternión del ángulo$\varphi \; es$

$$q_{}=\left(\mathrm{porque}\right(\frac{}{2}),\mathrm{pecado}(\frac{}{2}),0,0)$$

绕 Y Y Rotación del eje$Y$ θ \thetaEl cuaternión del ángulo$\theta \; es$

$$q_{}=\left(\mathrm{porque}\right(\frac{}{2}),0,\mathrm{pecado}(\frac{}{2}),0,0)$$

ZZ _Rotación del eje$Z$ ψ \psiEl cuaternión del$ángulo\; \psi \; es$

$$q_{}=\left(\mathrm{porque}\right(\frac{}{2}),0,0,\mathrm{pecado}(\frac{}{2}))$$

El cuaternión rotado total es el producto de estos tres cuaterniones. La multiplicación de cuaterniones no es la multiplicación escalar habitual, tiene reglas de multiplicación específicas.

Dado un cuaternión $q$ (la longitud del módulo es 1, existe la relación$\sqrt{q_w^{}+q_X^{}+q_y^{}+q_z^{}}=1$ ), asumiendo que el orden de rotación adoptado es$XYZ$ , su correspondiente matriz de rotación$R$ se puede expresar como

$$\begin{array}{rl}& =\left[\begin{array}{ccc}1-2(q_y^{}+q_z^{})& 2(q_{}{q}_{}-q_{}{q}_{})& 2(q_{}{q}_{}+q_{}{q}_{})\\ 2(q_{}{q}_{}+q_{}{q}_{})& 1-2(q_X^{}+q_z^{})& 2(q_{}{q}_{}-q_{}{q}_{})\\ 2(q_{}{q}_{}-q_{}{q}_{}& 2(q_{}{q}_{}+q_{}{q}_{}& 1-2(q_X^{}+q_y^{}\end{array}\right]\end{array}$$

Los cuaterniones unitarios tienen algunas ventajas al describir rotaciones 3D, que no se ven afectadas por el problema del "bloqueo del cardán" en los ángulos de Euler.

% 给定四元数
quaternion = [0.9376  0.0244  0.2070  0.2782];  

q_w = quaternion(1);
q_x = quaternion(2);
q_y = quaternion(3);
q_z = quaternion(4);

% 计算旋转矩阵 R
R(1,1) = 1 - 2*(q_y^2 + q_z^2);
R(1,2) = 2*(q_x*q_y - q_w*q_z);
R(1,3) = 2*(q_x*q_z + q_w*q_y);
R(2,1) = 2*(q_x*q_y + q_w*q_z);
R(2,2) = 1 - 2*(q_x^2 + q_z^2);
R(2,3) = 2*(q_y*q_z - q_w*q_x);
R(3,1) = 2*(q_x*q_z - q_w*q_y);
R(3,2) = 2*(q_y*q_z + q_w*q_x);
R(3,3) = 1 - 2*(q_x^2 + q_y^2);

R =
    0.7595   -0.5116    0.4017
    0.5318    0.8440    0.0694
   -0.3746    0.1609    0.9131

---

3. Convertir cuaterniones a ángulos de Euler

La conversión de cuaterniones a ángulos de Euler no es única, ya que para ciertas rotaciones existen múltiples representaciones de los ángulos de Euler. Sin embargo, para la mayoría de las aplicaciones prácticas, se puede calcular un conjunto específico de ángulos de Euler a partir de un cuaternión específico.

Construya $q=(q_{},q_{},q_{},q_{})$ , si desea convertirlo aXYZ XYZÁngulos de Euler de orden$X$$Y$$Z$$(\varphi ,yo,\psi )$ . Aquí se explica cómo convertir de cuaterniones a ángulos de Euler

$$\begin{array}{rl}\varphi & =\text{atan2}\left(2\right(q_{}{q}_{}+q_{}{q}_{}),1-2(q_X^{}+q_y^{}))\\ i& =\mathrm{arcosen}\left(2\right(q_{}{q}_{}-q_{}{q}_{}))\\ & =\text{atan2}\left(2\right(q_{}{q}_{}+q_{}{q}_{}),1-2(q_y^{}+q_z^{})\end{array}$$

donde $\text{atan2}\left(\right)$ no es$\mathrm{arctán}\left(\right)$。

Por ejemplo, si usa $\arctan (y/x)$ , su valor de retorno está en$[-\pi /2,\pi /2]$ , porque no puede distinguir entre$Los\; valores\; positivos\; y\; negativos\; de\; x$ . Y$\text{atan2}(y,x)$ , su valor de retorno está en$[-\pi ,\pi ]$ , puedes distinguir$El\; signo\; de\; x$ , por lo que resulta más práctico, sobre todo a la hora de calcular ángulos de Euler. Más importante aún,$\text{atan2}(y,x)$ puede manejar$X=0$ , que es útil al calcular ángulos o ángulos de Euler.

Tenga en cuenta que debido al uso de $\arcsin \left(\right)$ , este$\theta$ está cerca de$A\; \pm 90°$ , puede producirse inestabilidad numérica. Esto se debe a que en estos casos extremos el rumbo y el balanceo se vuelven indistinguibles, lo que se conoce como problema de bloqueo del cardán.

% 给定四元数
quaternion = [0.9376  0.0244  0.2070  0.2782];  

q_w = quaternion(1);
q_x = quaternion(2);
q_y = quaternion(3);
q_z = quaternion(4);

% 转换四元数到欧拉角
phi   = atan2(2*(q_w*q_x + q_y*q_z), 1 - 2*(q_x^2 + q_y^2));
theta = asin(2*(q_w*q_y - q_z*q_x));
psi   = atan2(2*(q_w*q_z + q_x*q_y), 1 - 2*(q_y^2 + q_z^2));

% 如果需要角度形式而不是弧度，可以转换为度
phi_deg   = rad2deg(phi);
theta_deg = rad2deg(theta);
psi_deg   = rad2deg(psi);

phi_deg =
    9.9953

theta_deg =
   21.9990

psi_deg =
   34.9983

---

4. Convertir los ángulos de Euler en cuaternión

Dados tres ángulos de Euler $\varphi ,yo,\psi$ , el cuaternión correspondiente es

$$\begin{array}{rl}{q}_{}& =\mathrm{porque}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)+\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\\ q_{}& =\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)-\mathrm{porque}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\\ q_{}& =\mathrm{porque}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)+\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\\ q_{}& =\mathrm{porque}\left(\frac{}{2}\right)\mathrm{porque}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)-\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{pecado}\left(\frac{}{2}\right)\mathrm{porque}(\frac{}{2}\end{array}$$

El cuaternión resultante es $(q_{},q_{},q_{},q_{})$。

phi   = deg2rad(10);  % 示例：10度，记得转换为弧度
theta = deg2rad(22);  % 示例：22度
psi   = deg2rad(35);  % 示例：35度

% 计算四元数
q_w = cos(phi/2) * cos(theta/2) * cos(psi/2) + sin(phi/2) * sin(theta/2) * sin(psi/2);
q_x = sin(phi/2) * cos(theta/2) * cos(psi/2) - cos(phi/2) * sin(theta/2) * sin(psi/2);
q_y = cos(phi/2) * sin(theta/2) * cos(psi/2) + sin(phi/2) * cos(theta/2) * sin(psi/2);
q_z = cos(phi/2) * cos(theta/2) * sin(psi/2) - sin(phi/2) * sin(theta/2) * cos(psi/2);

quaternion = [q_w, q_x, q_y, q_z];

quaternion =
    0.9376    0.0244    0.2070    0.2782

---

Árbitro

1. Matriz de rotación - Wikipedia
2. Colección de información útil: ángulos de Euler, matrices de rotación y cuaterniones.