Know About Euler Angles and Quaternion
2023-5-28 14:59:43
文章目录
Euler Angles
The Euler angles are three angles introduced by Leonhard Euler to describe the orientation of a rigid body with respect to a fixed coordinate system(Wikipedia). In classic Euler angles that a zero degrees represent the vertical orientation while in the alternative form introduced by Peter Guthrie Tait and George H. Bryan that zero degrees represent the horizontal position.
![img](https://upload.wikimedia.org/wikipedia/commons/thumb/8/85/Euler2a.gif/170px-Euler2a.gif)
Three composed elemental rotations(rotations about the axes of a coordinates system) are sufficient to reach any target frame, and called as **extrinsic **if the rotation is with respect to the motionless original coordinate xyz, or **intrinsic **if the coordinate XYZ rotates with the moving body.
Euler angles are typically denoted as α \alpha α, β \beta β, γ \gamma γ or ψ \psi ψ, θ \theta θ, φ \varphi φ, depends. There are 12 different conventions for definition of the rotation axes:
Proper Euler angles: ( z − x − z , x − y − x , y − z − y , z − y − z , x − z − x , y − x − y ) (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y) (z−x−z,x−y−x,y−z−y,z−y−z,x−z−x,y−x−y)
Tait-Bryan angles: ( x − y − z , y − z − x , z − x − y , x − z − y , z − y − x , y − x − z ) (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z) (x−y−z,y−z−x,z−x−y,x−z−y,z−y−x,y−x−z)
Tait-Bryan angles are also called Cardan angles, nautical angles; heading, elevation, and bank; or yaw(bearing), pitch(elevation) and roll(bank). The line of nodes(N) is defined as the intersection of planes xy and XY or common perpendicular to the axes z and Z. Then the N can be written as N = z × Z N=z\times Z N=z×Z1. The Euler angles can be defined as:
![img](https://upload.wikimedia.org/wikipedia/commons/thumb/7/72/Gimbaleuler.svg/180px-Gimbaleuler.svg.png)
![img](https://upload.wikimedia.org/wikipedia/commons/thumb/3/34/Gimbaleuler2.svg/160px-Gimbaleuler2.svg.png)
-
α \alpha α(or ψ \psi ψ) is the signed angle between x and N(x-convention, or y-convention defined between y and N).
-
β \beta β(or θ \theta θ) is the signed angle between z and Z axis.
-
γ \gamma γ(or φ \varphi φ) is the signed angle between the N and X axis(x-convention).
Angles follow the right-hand rule, which has positive values when representing a rotation clockwise.
Gimbal Lock: When two circular arms rotate on the same plane, for example: xy and the XY planes are identical(z axis and the Z axis have the same or opposite directions), as shown in the figure
The rotation sequence is hierarchical, therefore the upper-level rotation will change the direction of the axis in the lower level.
Now we can not turn the arrow down unless we rotate all axis in several steps. The solution of the gimbal is to change the rotation order, but just solve the problem temporarily, in where the gimbal changed direction. There are six combinations of gimbals, xyz, zxy, yxz, xzy, zyx and yzx.
The best way to avoid gimbal is to find the least possible direction. Or, using quaternion.
Quaternion
Quaternions are generally represented in the form:
KaTeX parse error: Undefined control sequence: \label at position 12: a+bi+cj+dk\̲l̲a̲b̲e̲l̲{1}
where
a
a
a,
b
b
b,
c
c
c and
d
d
d are real numbers and
1
1
1,
i
i
i,
j
j
j, and
k
k
k are the basis vectors or basis elements. Quaternions are used to avoid the gimbal lock in the Euler angle, because it can transform between Euler angles and rotation matrix2. The quaternion satisfied those constraint:
i
2
=
j
2
=
k
2
=
−
1
i
j
=
−
j
i
=
k
j
k
=
−
k
j
=
i
k
i
=
−
i
k
=
j
i^2=j^2=k^2=-1\\ ij=-ji=k\\ jk=-kj=i\\ ki=-ik=j
i2=j2=k2=−1ij=−ji=kjk=−kj=iki=−ik=j
We can rewrite the equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{1} as the sum of scalar and vector:
q
=
a
+
b
i
+
c
j
+
d
k
=
s
+
v
q = a+bi+cj+dk = s+v
q=a+bi+cj+dk=s+v
If
q
1
=
s
1
+
v
1
q_1=s_1+v_1
q1=s1+v1,
q
2
=
s
2
+
v
2
q_2=s_2+v_2
q2=s2+v2, then
KaTeX parse error: Undefined control sequence: \label at position 65: …+ v_1\times v_2\̲l̲a̲b̲e̲l̲{4}
If a quaternion
q
=
s
+
v
q=s+v
q=s+v and another quaternion
q
~
=
s
−
v
\tilde{q}=s-v
q~=s−v, then
q
~
\tilde{q}
q~ called conjugate quaternions. The product of a quaternion and its conjugate quaternion equals to the dot product of the quaternion itself, that is square of length of the quaternion.
q
q
~
=
q
~
q
=
q
∙
q
=
∣
q
∣
2
=
q
2
q\tilde{q} = \tilde{q}q = q\bullet q = {\left | q \right |}^2 =q^2
qq~=q~q=q∙q=∣q∣2=q2
The inverse of quaternion
q
q
q is
q
−
1
=
q
~
q
2
q^{-1}=\frac{\tilde{q}}{q^2}
q−1=q2q~, and
q
q
−
1
=
1
qq^{-1}=1
qq−1=1.
Reference
Scalar product(dot product) and vector product:
Let A A A and B B B be vectors, then the Scalar product is :
A
∙
B
=
∣
A
∣
∣
B
∣
cos
θ
A\bullet B = \left | A \right | \left | B \right | \cos\theta
A∙B=∣A∣∣B∣cosθ
The result of the scalar product is a number, which means the length of
A
A
A project on
B
B
B. The equation is derived from the law of cosines. While the result of a vector product is still the vector, and its module is:
∣
A
×
B
∣
=
∣
A
∣
∣
B
∣
sin
θ
\left | A\times B \right | = \left | A \right | \left | B \right | \sin\theta
∣A×B∣=∣A∣∣B∣sinθ
Considering the direction and module:
KaTeX parse error: Expected 'EOF', got '&' at position 2: &̲ A\times B=det(…
which is a vector that perpendicular to the plane of
A
A
A and
B
B
B, its length equals to the area of the rhomboid consist of
A
A
A and
B
B
B.
Quaternion and rotation matrix
This deduce refer to those links:
From dot and vector product to rotation matrix
First of all, think about the coordinate of a point in the polar coordinates
x
x
x and
y
y
y. Then they can be calculated from:
KaTeX parse error: Expected 'EOF', got '&' at position 2: &̲ x = r\cos\thet…
Ok. Now given two n dimensions’ vector
P
P
P and
Q
Q
Q. As illustrated earlier, their dot product is:
P
∙
Q
=
∣
P
∣
∣
Q
∣
cos
α
P\bullet Q = \left | P \right | \left | Q \right | \cos\alpha
P∙Q=∣P∣∣Q∣cosα
Where
α
\alpha
α is the angle between vectors.
Therefore, the vector
P
P
P project on
Q
Q
Q (a vector, the result of dot product is a scalar, need to multiplied a unit vector)is:
KaTeX parse error: Undefined control sequence: \label at position 53: … \right |}^2 }Q\̲l̲a̲b̲e̲l̲{11}
Write as the matrix product(assume
P
P
P and
Q
Q
Q are column vectors):
KaTeX parse error: Undefined control sequence: \label at position 207: …z \end{bmatrix}\̲l̲a̲b̲e̲l̲{12}
The vector from
P
P
P perpendicular to
Q
Q
Q is:
p
r
o
p
Q
P
=
P
−
p
r
o
j
Q
P
=
P
−
P
∙
Q
∣
Q
∣
2
Q
prop_QP = P-proj_QP=P-\frac{P\bullet Q}{{\left | Q \right |}^2 }Q
propQP=P−projQP=P−∣Q∣2P∙QQ
If
P
P
P and
Q
Q
Q are 3 dimensions, then their vector product is:
P
×
Q
=
<
P
y
Q
z
−
P
z
Q
y
,
P
z
Q
x
−
P
x
Q
z
,
P
x
Q
y
−
P
y
Q
x
>
P\times Q = <P_yQ_z-P_zQ_y, P_zQ_x-P_xQ_z,P_xQ_y-P_yQ_x>
P×Q=<PyQz−PzQy,PzQx−PxQz,PxQy−PyQx>
Write as matrix product:
KaTeX parse error: Undefined control sequence: \label at position 145: …z \end{bmatrix}\̲l̲a̲b̲e̲l̲{15}
Now we have a vector
P
=
<
P
x
,
P
y
>
=
<
x
,
y
>
P=<P_x, P_y>=<x,y>
P=<Px,Py>=<x,y>, and rotate it 90
∘
^{\circ}
∘.
Then the rotated vector
Q
=
<
−
P
y
,
P
x
>
=
<
−
y
,
x
>
Q=<-P_y, P_x>=<-y,x>
Q=<−Py,Px>=<−y,x>.
As shown in the figure, if
P
′
P'
P′ is the vector after
P
P
P rotate
θ
\theta
θ, then according to the polar coordinates illustrated in equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{9}, the coordinates of
P
′
P'
P′ is :
KaTeX parse error: Undefined control sequence: \label at position 29: …ta +Q\sin\theta\̲l̲a̲b̲e̲l̲{16}
Substitute
P
P
P and
Q
Q
Q, then:
P
x
′
=
P
x
cos
θ
−
P
y
sin
θ
P
y
′
=
P
y
cos
θ
+
P
x
sin
θ
P_x'=P_x\cos\theta - P_y\sin\theta\\ P_y'=P_y\cos\theta + P_x\sin\theta
Px′=Pxcosθ−PysinθPy′=Pycosθ+Pxsinθ
That is:
P
′
=
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
P
P'= \begin{bmatrix} \cos\theta& -\sin\theta\\ \sin\theta&\cos\theta \end{bmatrix}P
P′=[cosθsinθ−sinθcosθ]P
This rotation matrix can be extended to 3 dimensions, if rotate about z axis for
θ
\theta
θ (just keeping the z coordinate same):
R
z
(
θ
)
=
[
cos
θ
−
sin
θ
0
sin
θ
cos
θ
0
0
0
1
]
R_z(\theta)= \begin{bmatrix} \cos\theta& -\sin\theta & 0\\ \sin\theta& \cos\theta & 0\\ 0& 0& 1 \end{bmatrix}
Rz(θ)=
cosθsinθ0−sinθcosθ0001
Similarly, the rotation matrix about x and y axis:
R x ( θ ) = [ 1 0 0 0 c o s θ − sin θ 0 sin θ cos θ ] R_x(\theta)= \begin{bmatrix} 1& 0& 0\\ 0& cos\theta& -\sin\theta \\ 0& \sin\theta& \cos\theta \end{bmatrix} Rx(θ)= 1000cosθsinθ0−sinθcosθ
R x ( θ ) = [ c o s θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_x(\theta)= \begin{bmatrix} cos\theta& 0& \sin\theta\\ 0& 1& 0\\ -\sin\theta& 0& \cos\theta \end{bmatrix} Rx(θ)= cosθ0−sinθ010sinθ0cosθ
Now we want to make the rotation matrix more generally, let
P
P
P rotates about axis
A
A
A for
θ
\theta
θ, where
A
A
A is a unit vector. The projection
P
P
P to
A
A
A according to equationKaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{11}is:
p
r
o
j
A
P
=
(
A
∙
P
)
A
proj_AP=(A\bullet P)A
projAP=(A∙P)A
Similarly, the vector from
P
P
P perpendicular to
A
A
A is
p
r
o
p
A
P
=
P
−
(
A
∙
P
)
A
prop_AP=P-(A\bullet P)A
propAP=P−(A∙P)A
The vector after rotation consists of the projection on A axis and the vector
p
r
o
p
A
P
prop_AP
propAP after rotation. Remind that the A is the rotation axis, therefore the projection
p
r
o
j
A
P
proj_AP
projAP doesn’t change. As shown in the upper figure, the length of
p
r
o
p
A
P
prop_AP
propAP is
∣
P
∣
sin
α
\left | P \right |\sin\alpha
∣P∣sinα . How to construct the rotated
P
P
P vector? We need a vector perpendicular to
A
A
A and
P
P
P, combined with
p
r
o
p
A
P
prop_AP
propAP. This perpendicular vector could be obtained by vector product:
A
×
P
A\times P
A×P. As shown in the figure, vector
A
×
P
A\times P
A×P and
p
r
o
p
A
P
prop_AP
propAP construct a new coordinate. We should mind, because
A
A
A is unit vector, therefore the length of
A
×
P
A\times P
A×P is equal to
∣
P
∣
sin
α
\left | P \right |\sin\alpha
∣P∣sinα, that is
p
r
o
p
A
P
prop_AP
propAP. Also, the length of
P
′
P'
P′ also equals to
p
r
o
p
A
P
prop_AP
propAP, now the coordinate is actually same with equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{16}
Therefore, the coordinate of rotated p r o p A P prop_AP propAP is:
[
P
−
(
A
∙
P
)
A
]
cos
θ
+
(
A
×
P
)
sin
θ
[P-(A\bullet P)A]\cos\theta + (A\times P)\sin\theta
[P−(A∙P)A]cosθ+(A×P)sinθ
Then the vector of rotated
P
P
P ,
P
′
P'
P′ is combination of
p
r
o
j
A
P
proj_AP
projAP and rotated
p
r
o
p
A
P
prop_AP
propAP:
KaTeX parse error: Undefined control sequence: \label at position 139: …)(1-\cos\theta)\̲l̲a̲b̲e̲l̲{25}
According to equations KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{12} and KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{15}, substitute
A
×
P
A\times P
A×P and
(
A
∙
P
)
A
(A\bullet P)A
(A∙P)A (
A
A
A is unit vector):
P
′
=
[
1
0
0
0
1
0
0
0
1
]
P
cos
θ
+
[
0
−
A
z
A
y
A
z
0
−
A
x
−
A
y
A
x
0
]
P
sin
θ
+
[
A
x
2
A
x
A
y
A
x
A
z
A
x
A
y
A
y
2
A
y
A
z
A
x
A
z
A
y
A
z
A
z
2
]
P
(
1
−
cos
θ
)
P'=\begin{bmatrix} 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1 \end{bmatrix} P\cos\theta + \begin{bmatrix} 0& -A_z& A_y\\ A_z& 0& -A_x\\ -A_y& A_x& 0 \end{bmatrix} P\sin\theta + \begin{bmatrix} {A_x}^2& A_xA_y& A_xA_z\\ A_xA_y& {A_y}^2& A_yA_z\\ A_xA_z& A_yA_z& {A_z}^2 \end{bmatrix} P(1-\cos\theta)
P′=
100010001
Pcosθ+
0Az−Ay−Az0AxAy−Ax0
Psinθ+
Ax2AxAyAxAzAxAyAy2AyAzAxAzAyAzAz2
P(1−cosθ)
Quaternion represents rotation
Assume the rotation in 3-dimension space is a function
ϕ
\phi
ϕ, to keep the vector’s length angle and handedness during rotation, this function should satisfy:
∣
ϕ
(
P
)
∣
=
∣
P
∣
ϕ
(
P
1
)
∙
ϕ
(
P
2
)
=
P
1
∙
P
2
ϕ
(
P
1
)
×
ϕ
(
P
2
)
=
ϕ
(
P
1
×
P
2
)
\left | \phi(P)\right | = \left | P\right |\\ \phi(P_1)\bullet \phi(P_2) = P_1\bullet P_2\\ \phi(P_1)\times \phi(P_2) = \phi(P_1\times P_2)
∣ϕ(P)∣=∣P∣ϕ(P1)∙ϕ(P2)=P1∙P2ϕ(P1)×ϕ(P2)=ϕ(P1×P2)
Also because
ϕ
(
s
+
v
)
=
s
+
ϕ
(
v
)
\phi(s+v)=s+\phi(v)
ϕ(s+v)=s+ϕ(v) (inner product space mapping to itself),
ϕ
(
P
1
)
∙
ϕ
(
P
2
)
=
ϕ
(
P
1
∙
P
2
)
\phi(P_1)\bullet \phi(P_2) = \phi(P_1\bullet P_2)
ϕ(P1)∙ϕ(P2)=ϕ(P1∙P2)
Therefore, according to equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{4}, let
P
P
P be a quaternion with
s
=
0
s=0
s=0, then
KaTeX parse error: Undefined control sequence: \label at position 132: … = \phi(P_1P_2)\̲l̲a̲b̲e̲l̲{29}
for both dot product and vector product. The function satisfied equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{29} is called homomorphism.
Now we assume a function
ϕ
q
(
P
)
=
q
P
q
−
1
\phi_q(P)=qPq^{-1}
ϕq(P)=qPq−1 is a quaternion presentation of the rotation matrix. It can be proved that this function satisfied the rotation properties and also homomorphism. To be easy to prove, we assume
q
=
s
+
v
q=s+v
q=s+v is unit quaternion, its inverse is
q
=
s
−
v
q=s-v
q=s−v. Give a vector
P
P
P, we have
KaTeX parse error: Undefined control sequence: \label at position 74: …times P\times v\̲l̲a̲b̲e̲l̲{30}
according to theorem: Given two 3-D vector
P
P
P,
Q
Q
Q, then
P
×
(
Q
×
P
)
=
P
×
Q
×
P
=
P
2
Q
−
(
P
∙
Q
)
P
P\times (Q\times P)=P\times Q\times P=P^2Q-(P\bullet Q)P
P×(Q×P)=P×Q×P=P2Q−(P∙Q)P. Substitute into KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{30} and let
v
=
t
A
v=tA
v=tA(
A
A
A is unit vector as rotation axis), we have
q
P
q
−
1
=
(
s
2
−
t
2
)
P
+
2
s
t
A
×
P
+
2
t
2
(
A
∙
P
)
A
qPq^{-1}=(s^2-t^2)P+2stA\times P+2t^2(A\bullet P)A
qPq−1=(s2−t2)P+2stA×P+2t2(A∙P)A
compare to equationKaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{25}, we can have
t
=
sin
θ
2
s
=
cos
θ
2
t=\sin\frac{\theta}{2}\\ s=\cos\frac{\theta}{2}
t=sin2θs=cos2θ
Therefore, we find a quaternion
q
q
q, which could construct the rotation matrix about axis
A
A
A for
θ
\theta
θ:
q
=
s
+
v
=
s
+
t
A
=
cos
θ
2
+
A
sin
θ
2
q=s+v\\ =s+tA\\ =\cos\frac{\theta}{2} + A\sin\frac{\theta}{2}
q=s+v=s+tA=cos2θ+Asin2θ
Now we could use a quaternion
q
q
q to rotate a vector
P
P
P:
P
′
=
q
P
q
−
1
P'=qPq^{-1}
P′=qPq−1
Quaternion to rotation matrix
We write equation (31) to matrix according to dot product and vector product:
KaTeX parse error: Undefined control sequence: \label at position 350: … \end{bmatrix}P\̲l̲a̲b̲e̲l̲{35}
We rewrite
q
=
s
+
t
A
q=s+tA
q=s+tA to quaternion
q
=
<
w
,
x
,
y
,
z
>
q=<w,x,y,z>
q=<w,x,y,z>, where
w
=
s
,
x
=
t
A
x
,
y
=
t
A
y
,
z
=
t
A
z
w=s, x=tA_x, y=tA_y, z=tA_z
w=s,x=tAx,y=tAy,z=tAz, and
x
2
+
y
2
+
z
2
=
t
2
A
2
=
t
2
x^2+y^2+z^2=t^2A^2=t^2
x2+y2+z2=t2A2=t2, now equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{35}become:
q
P
q
−
1
=
[
w
2
−
x
2
−
y
2
−
t
2
w
2
−
x
2
−
y
2
−
t
2
w
2
−
x
2
−
y
2
−
t
2
]
P
+
[
0
−
2
w
z
2
w
y
2
w
z
0
−
2
w
x
−
2
w
y
2
w
x
0
]
P
+
[
2
x
2
2
t
2
A
x
A
y
2
x
z
2
x
y
2
y
2
2
y
z
2
x
y
2
y
z
2
z
2
]
P
qPq^{-1}= \begin{bmatrix} w^2-x^2-y^2-t^2& & \\ & w^2-x^2-y^2-t^2& \\ & & w^2-x^2-y^2-t^2 \end{bmatrix}P + \begin{bmatrix} 0& -2wz& 2wy\\ 2wz& 0& -2wx\\ -2wy& 2wx& 0 \end{bmatrix}P + \begin{bmatrix} 2x^2& 2t^2A_xA_y& 2xz\\ 2xy& 2y^2& 2yz\\ 2xy& 2yz& 2z^2 \end{bmatrix}P
qPq−1=
w2−x2−y2−t2w2−x2−y2−t2w2−x2−y2−t2
P+
02wz−2wy−2wz02wx2wy−2wx0
P+
2x22xy2xy2t2AxAy2y22yz2xz2yz2z2
P
also,
w
2
−
x
2
−
y
2
−
z
2
=
1
−
2
x
2
−
2
y
2
−
2
z
2
w^2-x^2-y^2-z^2=1-2x^2-2y^2-2z^2
w2−x2−y2−z2=1−2x2−2y2−2z2
Finally, according to equationKaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{38}, we can get rotation matrix:
KaTeX parse error: Undefined control sequence: \label at position 138: …2 \end{bmatrix}\̲l̲a̲b̲e̲l̲{38}
Now if we have the quaternion and rotation axis, we could get
w
,
x
,
y
,
z
w, x, y, z
w,x,y,z, and
R
q
R_q
Rq.
Rotation matrix to Quaternion
This deduce refer to 三维旋转:欧拉角、四元数、旋转矩阵、轴角之间的转换
If we have a rotation matrix, we should keep in mind a rotation matrix should be orthogonal. From equation KaTeX parse error: Undefined control sequence: \eqref at position 1: \̲e̲q̲r̲e̲f̲{38}, we know:
m
32
−
m
23
=
4
x
w
m
13
−
m
31
=
4
y
w
m
21
−
m
12
=
4
z
w
m_{32}-m_{23} = 4xw\\ m_{13}-m_{31} = 4yw\\ m_{21}-m_{12} = 4zw
m32−m23=4xwm13−m31=4ywm21−m12=4zw
also:
t
r
(
R
q
)
=
m
11
+
m
22
+
m
33
=
3
−
4
(
x
2
+
y
2
+
z
2
)
=
4
(
1
−
(
x
2
+
y
2
+
z
2
)
)
−
1
=
4
w
2
−
1
tr(R_q)=m_{11}+m_{22}+m_{33}\\ =3-4(x^2+y^2+z^2)\\ =4(1-(x^2+y^2+z^2))-1\\ =4w^2-1
tr(Rq)=m11+m22+m33=3−4(x2+y2+z2)=4(1−(x2+y2+z2))−1=4w2−1
Therefore:
w
=
t
r
(
R
q
)
+
1
2
x
=
m
32
−
m
23
4
w
y
=
m
13
−
m
31
4
w
z
=
m
21
−
m
12
4
w
w=\frac{\sqrt{tr(R_q)+1}}{2}\\ x =\frac{m_{32}-m_{23}}{4w}\\ y =\frac{m_{13}-m_{31}}{4w}\\ z =\frac{m_{21}-m_{12}}{4w}
w=2tr(Rq)+1x=4wm32−m23y=4wm13−m31z=4wm21−m12
Euler angles and rotation matrix
Euler angles to rotation matrix
**How to transform Euler angle to rotation matrix? Just multiply three elemental rotation matrices. **
Rotation matrix to Euler angle
For ZXY order(according to the yaw pitch roll angle range):
- Y axis yaw angle:
α = a t a n 2 ( m 13 , m 33 ) \alpha = atan2(m_{13}, m_{33}) α=atan2(m13,m33)
- Y axis pitch angle:
β = a r c s i n ( − m 23 ) \beta = arcsin(-m_{23}) β=arcsin(−m23)
- Z axis roll angle:
γ = a t a n 2 ( m 21 , m 22 ) \gamma = atan2(m_{21}, m_{22}) γ=atan2(m21,m22)
If gimbal lock occur,
m
23
=
−
sin
β
=
±
1
m_{23}=-\sin\beta=\pm 1
m23=−sinβ=±1, or
cos
β
=
0
\cos\beta = 0
cosβ=0 these equations become questionable. At the time,
β
=
±
π
2
\beta=\pm \frac{\pi}{2}
β=±2π, the rotation matrix is:
R
(
α
,
β
,
γ
)
=
[
cos
(
α
±
γ
)
sin
(
α
±
γ
)
0
0
0
±
1
−
sin
(
α
±
γ
)
−
cos
(
α
±
γ
)
0
]
R(\alpha, \beta, \gamma) = \begin{bmatrix} \cos (\alpha \pm \gamma) & \sin (\alpha \pm \gamma) & 0 \\ 0 & 0 & \pm 1 \\ -\sin (\alpha \pm \gamma) & -\cos (\alpha \pm \gamma) & 0 \end{bmatrix}
R(α,β,γ)=
cos(α±γ)0−sin(α±γ)sin(α±γ)0−cos(α±γ)0±10
if
β
=
−
π
2
\beta=- \frac{\pi}{2}
β=−2π,
R
(
α
,
β
,
γ
)
=
[
cos
(
α
±
γ
)
sin
(
α
±
γ
)
0
0
0
1
−
sin
(
α
±
γ
)
−
cos
(
α
±
γ
)
0
]
α
+
γ
=
a
t
a
n
2
(
−
m
12
,
m
11
)
R(\alpha, \beta, \gamma) = \begin{bmatrix} \cos (\alpha \pm \gamma) & \sin (\alpha \pm \gamma) & 0 \\ 0 & 0 & 1 \\ -\sin (\alpha \pm \gamma) & -\cos (\alpha \pm \gamma) & 0 \end{bmatrix}\\ \alpha+\gamma=atan2(-m_{12}, m_{11})
R(α,β,γ)=
cos(α±γ)0−sin(α±γ)sin(α±γ)0−cos(α±γ)010
α+γ=atan2(−m12,m11)
if
β
=
π
2
\beta=\frac{\pi}{2}
β=2π,
R
(
α
,
β
,
γ
)
=
[
cos
(
α
±
γ
)
sin
(
α
±
γ
)
0
0
0
−
1
−
sin
(
α
±
γ
)
−
cos
(
α
±
γ
)
0
]
α
−
γ
=
a
t
a
n
2
(
m
12
,
m
11
)
R(\alpha, \beta, \gamma) = \begin{bmatrix} \cos (\alpha \pm \gamma) & \sin (\alpha \pm \gamma) & 0 \\ 0 & 0 & -1 \\ -\sin (\alpha \pm \gamma) & -\cos (\alpha \pm \gamma) & 0 \end{bmatrix}\\ \alpha-\gamma=atan2(m_{12}, m_{11})
R(α,β,γ)=
cos(α±γ)0−sin(α±γ)sin(α±γ)0−cos(α±γ)0−10
α−γ=atan2(m12,m11)
Give one value to
α
\alpha
α or
γ
\gamma
γ to calculate.