目录
注:文章涉及的坐标系都为右手系
1. 旋转矩阵
1.1. 旋转矩阵 -> 四元数
假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0+q1i+q2j+q3k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。
p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp
q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗
L
(
q
)
R
(
q
∗
)
=
[
s
−
v
T
v
s
I
3
×
3
+
[
v
×
]
]
[
s
v
T
−
v
s
I
3
×
3
+
[
v
×
]
]
=
[
a
b
c
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
]
\mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})= \begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \\{}\\= \begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}
L(q)R(q∗)=[sv−vTsI3×3+[v×]][s−vvTsI3×3+[v×]]=[acbvvT+s2I+2s[v×]+(v×)2]
因为
p
\mathbf p
p和
p
′
\mathbf p'
p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R
=
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2
R=vvT+s2I+2s[v×]+(v×)2
代入
s
=
q
0
,
v
=
q
1
i
+
q
2
j
+
q
3
k
s=q_0,\mathbf v = q_1\mathbf i + q_2\mathbf j + q_3\mathbf k
s=q0,v=q1i+q2j+q3k.
根据旋转矩阵的表达式,可以由旋转矩阵得到四元数:
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
[
2
(
q
0
2
+
q
1
2
)
−
1
2
(
q
1
q
2
−
q
0
q
3
)
2
(
q
1
q
3
+
q
0
q
1
)
2
(
q
1
q
2
+
q
0
q
3
)
2
(
q
0
2
+
q
2
2
)
−
1
2
(
q
2
q
3
−
q
0
q
1
)
2
(
q
1
q
3
−
q
0
q
1
)
2
(
q
2
q
3
+
q
0
q
1
)
2
(
q
0
2
+
q
3
2
)
−
1
]
R =\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right] =\left[\begin{array}{lll} 2(q_0^2 + q_1^2)-1 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_1) \\ 2(q_1q_2+q_0q_3) & 2(q_0^2 + q_2^2)-1 & 2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_1) & 2(q_2q_3+q_0q_1) & 2(q_0^2 + q_3^2)-1 \end{array}\right]
R=
r11r21r31r12r22r32r13r23r33
=
2(q02+q12)−12(q1q2+q0q3)2(q1q3−q0q1)2(q1q2−q0q3)2(q02+q22)−12(q2q3+q0q1)2(q1q3+q0q1)2(q2q3−q0q1)2(q02+q32)−1
q 0 = 1 + r 11 + r 22 + r 33 2 q_{0}=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2} q0=21+r11+r22+r33
q 1 = r 32 − r 23 4 q 0 q_{1}=\frac{r_{32}-r_{23}}{4 q_{0}} q1=4q0r32−r23
q 2 = r 13 − r 31 4 q 0 q_{2}=\frac{r_{13}-r_{31}}{4 q_{0}} q2=4q0r13−r31
q 3 = r 21 − r 12 4 q 0 q_{3}=\frac{r_{21}-r_{12}}{4 q_{0}} q3=4q0r21−r12
Eigen::Quaterniond quaternion(rotation_matrix);
或者
Eigen::Quaterniond quaternion;
quaternion = rotation_matrix;
1.2. 旋转矩阵 -> 欧拉角
R x ( θ ) = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R_{x}(\theta)=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta\end{array}\right] Rx(θ)= 1000cosθsinθ0−sinθcosθ
R y ( θ ) = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_{y}(\theta)=\left[\begin{array}{ccc}\cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta\end{array}\right] Ry(θ)= cosθ0−sinθ010sinθ0cosθ
R z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_{z}(\theta)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] Rz(θ)= cosθsinθ0−sinθcosθ0001
R
=
R
z
(
ϕ
)
R
y
(
θ
)
R
x
(
ψ
)
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi)= \left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]=
R=Rz(ϕ)Ry(θ)Rx(ψ)=
r11r21r31r12r22r32r13r23r33
=
[
cos
θ
cos
ϕ
sin
ψ
sin
θ
cos
ϕ
−
cos
ψ
sin
ϕ
cos
ψ
sin
θ
cos
ϕ
+
sin
ψ
sin
ϕ
cos
θ
sin
ϕ
sin
ψ
sin
θ
sin
ϕ
+
cos
ψ
cos
ϕ
cos
ψ
sin
θ
sin
ϕ
−
sin
ψ
cos
ϕ
−
sin
θ
sin
ψ
cos
θ
cos
ψ
cos
θ
]
\left[\begin{array}{ccc}\cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta\end{array}\right]
cosθcosϕcosθsinϕ−sinθsinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθcosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ
则对于绕ZYX定轴得到的旋转矩阵可以如下表示欧拉角:
θ
x
=
atan
2
(
r
32
,
r
33
)
θ
y
=
atan
2
(
−
r
31
,
r
32
2
+
r
33
2
)
θ
z
=
atan
2
(
r
21
,
r
11
)
\begin{array}{c} \theta_{x}=\operatorname{atan} 2\left(r_{32}, r_{33}\right) \\ \theta_{y}=\operatorname{atan} 2\left(-r_{31}, \sqrt{r_{32}^{2}+r_{33}^{2}}\right) \\ \theta_{z}=\operatorname{atan} 2\left(r_{21}, r_{11}\right) \end{array}
θx=atan2(r32,r33)θy=atan2(−r31,r322+r332)θz=atan2(r21,r11)
Eigen::Vector3d R_ZYX = R.eulerAngles(2,1,0);
std::cout << yaw, pitch, roll" << R_ZYX.transpose() *180/ M_PI << std::endl;
// 或者
double yaw = atan2(R(1,0),R(0,0)) * 180/ M_PI;
1.3. 旋转矩阵->轴角
利用罗德里格旋转公式可以获得绕某过原点一轴
n
=
[
r
x
,
r
y
,
r
z
]
\mathbf n = \left[r_{x}, r_{y}, r_{z}\right]
n=[rx,ry,rz] 旋转某一角度
θ
\theta
θ 的旋转矩阵
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
^
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
R=cos\theta \mathbf I+(1-cos\theta )\mathbf n\mathbf n^T+sin\theta \mathbf n \hat{}=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]=
R=cosθI+(1−cosθ)nnT+sinθn^=
r11r21r31r12r22r32r13r23r33
=
[
r
x
2
(
1
−
cos
θ
)
+
cos
θ
r
x
r
y
(
1
−
cos
θ
)
−
r
z
s
i
n
θ
r
x
r
z
(
1
−
cos
θ
)
+
r
y
s
i
n
θ
r
x
r
y
(
1
−
cos
θ
)
+
r
z
s
i
n
θ
r
y
2
(
1
−
cos
θ
)
+
cos
θ
r
y
r
z
(
1
−
cos
θ
)
−
r
x
s
i
n
θ
r
x
r
z
(
1
−
cos
θ
)
−
r
y
s
i
n
θ
r
y
r
z
(
1
−
cos
θ
)
+
r
x
s
i
n
θ
r
z
2
(
1
−
cos
θ
)
+
c
o
s
θ
]
\left[\begin{array}{ccc} r_{x}^{2}(1-\cos \theta) + \cos \theta& r_{x} r_{y}(1-\cos \theta)-r_zsin\theta & r_{x} r_{z}(1-\cos \theta)+r_ysin\theta \\ r_{x} r_{y}(1-\cos \theta)+r_zsin\theta & r_{y}^{2}(1-\cos \theta) +\cos \theta& r_{y} r_{z}(1-\cos \theta)-r_xsin\theta \\ r_{x} r_{z}(1-\cos \theta)-r_ysin\theta & r_{y} r_{z}(1-\cos \theta)+r_xsin\theta & r_{z}^2(1-\cos \theta)+cos\theta \end{array}\right]
rx2(1−cosθ)+cosθrxry(1−cosθ)+rzsinθrxrz(1−cosθ)−rysinθrxry(1−cosθ)−rzsinθry2(1−cosθ)+cosθryrz(1−cosθ)+rxsinθrxrz(1−cosθ)+rysinθryrz(1−cosθ)−rxsinθrz2(1−cosθ)+cosθ
θ = a c o s r 11 + r 22 + r 33 − 1 2 \theta = acos\frac{r_{11}+ r_{22}+r_{33}-1}{2} θ=acos2r11+r22+r33−1
r ⃗ = 1 2 s i n θ [ r 32 − r 23 r 13 − r 31 r 21 − r 12 ] \vec r = \frac{1}{2sin\theta}\left[\begin{array}{lll} r_{32} - r_{23} \\ r_{13} - r_{31} \\ r_{21} - r_{12} \end{array}\right] r=2sinθ1 r32−r23r13−r31r21−r12
Eigen::AngleAxisd angel_axisd(rotation_matrix);
2. 旋转向量
2.1. 旋转向量->轴角
Eigen::AngleAxisd angel_axisd;
angel_axisd =
Eigen::AngleAxisd(extrinsic_params[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(extrinsic_params[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(extrinsic_params[2], Eigen::Vector3d::UnitX());
或
Eigen::Vector3d vec(jvalue[sensor_type][j]["extrinsic_param"]["rotation"][0].asDouble(),
jvalue[sensor_type][j]["extrinsic_param"]["rotation"][1].asDouble(),
jvalue[sensor_type][j]["extrinsic_param"]["rotation"][2].asDouble());
Eigen::AngleAxisd rot_vec(vec.norm(), vec.normalized());
2.2. 旋转向量->旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
或
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
2.3. 旋转向量->欧拉角
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
2.4. 旋转向量->四元数
Eigen::Quaterniond quaternion(rotation_vector);
3. 轴角
3.1. 轴角->旋转向量
轴角和旋转向量本质上是一个东西, 轴角用四个元素表达旋转, 其中的三个元素用来描述旋转轴, 另外一个元素描述旋转的角度,如 下所示:
r
=
[
x
,
y
,
z
,
θ
]
r=[x, y, z, \theta]
r=[x,y,z,θ]
其中单位向量
n
⃗
=
[
x
,
y
,
z
]
\vec{n}=[x, y, z]
n=[x,y,z]对应的是旋转轴
θ
\theta
θ 对应的是旋转角度。旋转向量与轴角相同, 只是旋转向量用三个元素来描述旋转, 它把
θ
\theta
θ 角 乘到了旋转轴上, 如下:
r
v
=
[
x
∗
θ
,
y
∗
θ
,
z
∗
θ
]
r_{v}=[x * \theta, y * \theta, z * \theta]
rv=[x∗θ,y∗θ,z∗θ]
3.2. 轴角 -> 四元数
假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0+q1i+q2j+q3k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。
p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp
q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗
L
(
q
)
R
(
q
∗
)
=
[
s
−
v
T
v
s
I
3
×
3
+
[
v
×
]
]
[
s
v
T
−
v
s
I
3
×
3
+
[
v
×
]
]
=
[
a
b
c
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
]
\mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})= \begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \\{}\\= \begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}
L(q)R(q∗)=[sv−vTsI3×3+[v×]][s−vvTsI3×3+[v×]]=[acbvvT+s2I+2s[v×]+(v×)2]
因为
p
\mathbf p
p和
p
′
\mathbf p'
p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R
=
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2
R=vvT+s2I+2s[v×]+(v×)2
对上式两侧求迹,得到
t
r
(
R
)
=
t
r
(
v
v
T
)
+
s
2
t
r
(
I
3
×
3
)
+
2
s
t
r
(
[
v
×
]
)
+
t
r
(
(
v
×
)
2
)
=
q
1
2
+
q
2
2
+
q
3
2
+
3
s
3
−
2
q
1
2
−
2
q
2
2
−
2
q
3
2
=
3
s
3
−
(
q
1
2
+
q
2
2
+
q
3
2
)
=
3
s
3
−
(
1
−
s
2
)
=
4
s
2
−
1
tr(R)= tr(\mathbf v \mathbf v^T) + s^2 tr(\mathbf I_{3\times3}) +2s tr([\mathbf v\times]) +tr((\mathbf v\times)^2)\\=q_1^2+q_2^2+q_3^2+3s^3-2q_1^2-2q_2^2-2q_3^2 \\=3s^3-(q_1^2+q_2^2+q_3^2) \\=3s^3-(1-s^2) \\=4s^2-1
tr(R)=tr(vvT)+s2tr(I3×3)+2str([v×])+tr((v×)2)=q12+q22+q32+3s3−2q12−2q22−2q32=3s3−(q12+q22+q32)=3s3−(1−s2)=4s2−1
v v T = [ q 1 q 2 q 3 ] [ q 1 q 2 q 3 ] = [ q 1 2 q 1 q 2 q 1 q 3 q 2 q 1 q 2 2 q 2 q 3 q 3 q 1 q 3 q 2 q 3 2 ] \mathbf v \mathbf v^T= \begin{bmatrix}q_1\\q_2\\q_3\end{bmatrix} \begin{bmatrix}q_1&q_2&q_3\end{bmatrix}= \begin{bmatrix}q_1^2&q_1q_2&q_1q_3\\q_2q_1&q_2^2 &q_2q_3\\q_3q_1&q_3q_2&q_3^2\end{bmatrix} vvT= q1q2q3 [q1q2q3]= q12q2q1q3q1q1q2q22q3q2q1q3q2q3q32
( v × ) 2 = [ 0 − q 3 q 2 q 3 0 − q 1 − q 2 q 1 0 ] [ 0 − q 3 q 2 q 3 0 − q 1 − q 2 q 1 0 ] = [ − q 2 2 − q 3 2 q 1 q 2 q 1 q 3 q 1 q 2 − q 2 2 − q 3 2 q 2 q 3 q 1 q 3 q 2 q 3 − q 1 2 − q 2 2 ] (\mathbf v\times)^2=\begin{bmatrix}0&-q_3&q_2\\q_3&0&-q_1\\-q_2&q_1&0\end{bmatrix}\begin{bmatrix}0&-q_3&q_2\\q_3&0&-q_1\\-q_2&q_1&0\end{bmatrix} = \begin{bmatrix}-q_2^2-q_3^2&q_1q_2&q_1q_3\\q_1q_2&-q_2^2-q_3^2&q_2q_3\\q_1q_3&q_2q_3&-q_1^2-q_2^2\end{bmatrix} (v×)2= 0q3−q2−q30q1q2−q10 0q3−q2−q30q1q2−q10 = −q22−q32q1q2q1q3q1q2−q22−q32q2q3q1q3q2q3−q12−q22
又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系:
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
[
n
×
]
\mathbf R = cos\theta \mathbf I + (1-cos\theta)\mathbf n \mathbf n^T+sin\theta[\mathbf n\times]
R=cosθI+(1−cosθ)nnT+sinθ[n×]
对上式子求迹可得:
t
r
(
R
)
=
c
o
s
θ
t
r
(
I
)
+
(
1
−
c
o
s
θ
)
t
r
(
n
n
T
)
+
s
i
n
θ
[
(
n
×
]
)
=
3
c
o
s
θ
+
(
1
−
c
o
s
θ
)
+
0
=
1
+
2
c
o
s
θ
=
>
θ
=
a
r
c
c
o
s
t
r
(
R
)
−
1
2
=
a
r
c
c
o
s
(
2
s
2
−
1
)
=
>
c
o
s
θ
=
2
c
o
s
2
θ
2
−
1
=
2
s
2
−
1
=
>
θ
=
2
a
r
c
c
o
s
(
s
)
tr(\mathbf R) = cos\theta tr(\mathbf I) + (1-cos\theta)tr(\mathbf n \mathbf n^T)+sin\theta[(\mathbf n\times]) \\= 3cos\theta + (1-cos\theta) + 0 \\ = 1 + 2cos\theta \\=>\theta = arccos\frac{tr(\mathbf R)-1}{2}=arccos(2s^2-1) \\ => cos\theta = 2cos^2\frac{\theta}{2}-1 =2s^2-1 \\ => \theta = 2 arccos(s)
tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθ[(n×])=3cosθ+(1−cosθ)+0=1+2cosθ=>θ=arccos2tr(R)−1=arccos(2s2−1)=>cosθ=2cos22θ−1=2s2−1=>θ=2arccos(s)
轴角绕单位轴
u
\mathbf{u}
u 旋转
θ
\theta
θ 的四元数是:
至于旋转轴,如果在四元数转换的时候用
q
\mathbf q
q的虚部代替
p
\mathbf p
p,易知
q
\mathbf q
q的虚部组成的向量在旋转时是不动的,即构成旋转轴。于是只要将它除以它的模长,即得。
u
=
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
s
i
n
θ
2
\mathbf{u}=[n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin\frac{\theta}{2}
u=[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
所以,轴角绕单位轴 u \mathbf{u} u 旋转 θ \theta θ 的四元数是:
q ( w , v ) = ( cos θ 2 , u sin θ 2 ) \mathbf{q}(w, \mathbf{v})=\left(\cos \frac{\theta}{2}, \mathbf{u} \sin \frac{\theta}{2}\right) q(w,v)=(cos2θ,usin2θ)
3.3. 轴角->旋转矩阵
罗德里格斯形式旋转角(轴角)使用一个单位旋转轴k和绕轴旋转的角度
θ
\theta
θ(正方向右手定则)来描述
如上图,我们可以知道,向量
v
v
v 在
k
k
k 的作用下其实只旋转了垂直于旋转轴的分量,我们将
v
v
v 做分解,有
v
=
v
∥
+
v
⊥
v=v_{\|}+v_{\perp}
v=v∥+v⊥
其中平行分量使用投影和旋转轴表示为:(
v
⋅
k
v \cdot k
v⋅k等价于将v向量在k向量上的投影(
∣
v
∣
∗
∣
k
∣
∗
c
o
s
θ
|v|*|k|*cos\theta
∣v∣∗∣k∣∗cosθ),k是一个长度为1的旋转轴)
v
∥
=
(
v
⋅
k
)
k
v_{\|}=(v \cdot k) k
v∥=(v⋅k)k
垂直分量表示为(
−
-
−表示方向相反)
v
⊥
=
v
−
v
∥
=
v
−
(
v
⋅
k
)
k
=
−
k
×
(
k
×
v
)
v_{\perp}=v-v_{\|}=v-(v \cdot k) k=-k \times(k \times v)
v⊥=v−v∥=v−(v⋅k)k=−k×(k×v)
平行于轴的分量在旋转时不会改变幅度和方向(如图所示),
v
∥
r
o
t
=
v
∥
v_{\| r o t}=v_{\|}
v∥rot=v∥
垂直于轴的分量只会改变方向,但不会改变大小
∣
v
⊥
r
o
t
∣
=
∣
v
⊥
∣
v
⊥
=
cos
θ
v
⊥
+
sin
θ
k
×
v
⊥
\begin{gathered} \left|v_{\perp r o t}\right|=\left|v_{\perp}\right| \\ v_{\perp}=\cos \theta v_{\perp}+\sin \theta k \times v_{\perp} \end{gathered}
∣v⊥rot∣=∣v⊥∣v⊥=cosθv⊥+sinθk×v⊥
代入分解式
k
×
v
⊥
=
k
×
(
v
−
v
∥
)
=
k
×
v
−
k
×
v
∥
=
k
×
v
k \times v_{\perp}=k \times\left(v-v_{\|}\right)=k \times v-k \times v_{\|}=k \times v
k×v⊥=k×(v−v∥)=k×v−k×v∥=k×v
因此上式可以修改为
v
⊥
=
cos
θ
v
⊥
+
sin
θ
k
×
v
v_{\perp}=\cos \theta v_{\perp}+\sin \theta k \times v
v⊥=cosθv⊥+sinθk×v
则
v
r
o
t
=
v
∥
+
cos
(
θ
)
v
⊥
+
sin
(
θ
)
k
×
v
=
v
∥
+
cos
(
θ
)
(
v
−
v
∥
)
+
sin
(
θ
)
k
×
v
=
cos
(
θ
)
v
+
(
1
−
cos
θ
)
v
∥
+
sin
(
θ
)
k
×
v
=
cos
(
θ
)
v
+
(
1
−
cos
θ
)
(
k
⋅
v
)
k
+
sin
(
θ
)
k
×
v
\begin{aligned} \mathbf{v}_{\mathrm{rot}} &=\mathbf{v}_{\|}+\cos (\theta) \mathbf{v}_{\perp}+\sin (\theta) \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}_{\|}+\cos (\theta)\left(\mathbf{v}-\mathbf{v}_{\|}\right)+\sin (\theta) \mathbf{k} \times \mathbf{v} \\ &=\cos (\theta) \mathbf{v}+(1-\cos \theta) \mathbf{v}_{\|}+\sin (\theta) \mathbf{k} \times \mathbf{v} \\ &=\cos (\theta) \mathbf{v}+(1-\cos \theta)(\mathbf{k} \cdot \mathbf{v}) \mathbf{k}+\sin (\theta) \mathbf{k} \times \mathbf{v} \end{aligned}
vrot=v∥+cos(θ)v⊥+sin(θ)k×v=v∥+cos(θ)(v−v∥)+sin(θ)k×v=cos(θ)v+(1−cosθ)v∥+sin(θ)k×v=cos(θ)v+(1−cosθ)(k⋅v)k+sin(θ)k×v
我们将叉乘部分按照矩阵分量形式表示出来
[
(
k
×
v
)
x
(
k
×
v
)
y
(
k
×
v
)
z
]
=
[
k
y
v
z
−
k
z
v
y
k
z
v
x
−
k
x
v
z
k
x
v
y
−
k
y
v
x
]
=
[
0
−
k
z
k
y
k
z
0
−
k
x
−
k
y
k
x
0
]
[
v
x
v
y
v
z
]
\left[\begin{array}{c} (\mathbf{k} \times \mathbf{v})_{x} \\ (\mathbf{k} \times \mathbf{v})_{y} \\ (\mathbf{k} \times \mathbf{v})_{z} \end{array}\right]=\left[\begin{array}{l} k_{y} v_{z}-k_{z} v_{y} \\ k_{z} v_{x}-k_{x} v_{z} \\ k_{x} v_{y}-k_{y} v_{x} \end{array}\right]=\left[\begin{array}{ccc} 0 & -k_{z} & k_{y} \\ k_{z} & 0 & -k_{x} \\ -k_{y} & k_{x} & 0 \end{array}\right]\left[\begin{array}{l} v_{x} \\ v_{y} \\ v_{z} \end{array}\right]
(k×v)x(k×v)y(k×v)z
=
kyvz−kzvykzvx−kxvzkxvy−kyvx
=
0kz−ky−kz0kxky−kx0
vxvyvz
为了表达方便,我们定义一个叉乘矩阵
K
\mathbf{K}
K
K
=
[
0
−
k
z
k
y
k
z
0
−
k
x
−
k
y
k
x
0
]
\mathbf{K}=\left[\begin{array}{ccc} 0 & -k_{z} & k_{y} \\ k_{z} & 0 & -k_{x} \\ -k_{y} & k_{x} & 0 \end{array}\right]
K=
0kz−ky−kz0kxky−kx0
将叉乘简化为
K
v
=
k
×
v
\mathbf{K} \mathbf{v}=\mathbf{k} \times \mathbf{v}
Kv=k×v
由于
k
k
k 为单位向量,因此有单位 2-范数
∥
K
∥
2
=
1
\|K\|_{2}=1
∥K∥2=1
我们回到之前的
v
r
o
t
v_{r o t}
vrot 表达式上
v
rot
=
v
cos
θ
+
(
k
×
v
)
sin
θ
+
k
(
k
⋅
v
)
(
1
−
cos
θ
)
=
v
cos
θ
+
(
k
×
v
)
sin
θ
+
(
v
−
v
⊥
)
(
1
−
cos
θ
)
=
v
cos
θ
+
(
k
×
v
)
sin
θ
+
(
v
+
k
×
(
k
×
v
)
)
(
1
−
cos
θ
)
=
v
cos
θ
+
(
k
×
v
)
sin
θ
+
(
v
+
K
(
K
v
)
)
(
1
−
cos
θ
)
=
v
(
cos
θ
+
1
−
cos
θ
)
+
(
k
×
v
)
sin
θ
+
K
(
K
v
)
(
1
−
cos
θ
)
\begin{aligned} \mathbf{v}_{\text {rot }} &=\mathbf{v} \cos \theta+(\mathbf{k} \times \mathbf{v}) \sin \theta+\mathbf{k}(\mathbf{k} \cdot \mathbf{v})(1-\cos \theta) \\ &=\mathbf{v} \cos \theta+(\mathbf{k} \times \mathbf{v}) \sin \theta+\left(\mathbf{v}-\mathbf{v}_{\perp}\right)(1-\cos \theta) \\ &=\mathbf{v} \cos \theta+(\mathbf{k} \times \mathbf{v}) \sin \theta+(\mathbf{v}+\mathbf{k} \times(\mathbf{k} \times \mathbf{v}))(1-\cos \theta) \\ &=\mathbf{v} \cos \theta+(\mathbf{k} \times \mathbf{v}) \sin \theta+(\mathbf{v}+\mathbf{K}(\mathbf{K} \mathbf{v}))(1-\cos \theta) \\ &=\mathbf{v}(\cos \theta+1-\cos \theta)+(\mathbf{k} \times \mathbf{v}) \sin \theta+\mathbf{K}(\mathbf{K} \mathbf{v})(1-\cos \theta) \end{aligned}
vrot =vcosθ+(k×v)sinθ+k(k⋅v)(1−cosθ)=vcosθ+(k×v)sinθ+(v−v⊥)(1−cosθ)=vcosθ+(k×v)sinθ+(v+k×(k×v))(1−cosθ)=vcosθ+(k×v)sinθ+(v+K(Kv))(1−cosθ)=v(cosθ+1−cosθ)+(k×v)sinθ+K(Kv)(1−cosθ)
这里我们就得到了
v
r
o
t
=
v
+
(
sin
θ
)
K
v
+
(
1
−
cos
θ
)
K
2
v
,
∥
K
∥
2
=
1
\mathbf{v}_{\mathrm{rot}}=\mathbf{v}+(\sin \theta) \mathbf{K} \mathbf{v}+(1-\cos \theta) \mathbf{K}^{2} \mathbf{v}, \quad\|\mathbf{K}\|_{2}=1
vrot=v+(sinθ)Kv+(1−cosθ)K2v,∥K∥2=1
那么旋转矩阵就可以使用上式得到(
v
r
o
t
=
R
v
=
>
R
\mathbf{v}_{\mathrm{rot}}=Rv=>R
vrot=Rv=>R)
R
=
I
+
sin
θ
K
+
(
1
−
cos
θ
)
K
2
\mathbf{R}=\mathbf{I}+\sin \theta \mathbf{K}+(1-\cos \theta) \mathbf{K}^{2}
R=I+sinθK+(1−cosθ)K2
展开之后有
R
(
k
,
θ
)
=
[
cos
θ
+
k
x
2
(
1
−
cos
θ
)
−
sin
θ
k
z
+
(
1
−
cos
θ
)
k
x
k
y
sin
θ
k
y
+
(
1
−
cos
θ
)
k
x
k
z
sin
θ
k
z
+
(
1
−
cos
θ
)
k
x
k
y
cos
θ
+
k
y
2
(
1
−
cos
θ
)
−
sin
θ
k
x
+
(
1
−
cos
θ
)
k
y
k
z
−
sin
θ
k
y
+
(
1
−
cos
θ
)
k
x
k
z
sin
θ
k
x
+
(
1
−
cos
θ
)
k
y
k
x
cos
θ
+
k
z
2
(
1
−
cos
θ
)
]
\mathbf{R}(k, \theta)=\left[\begin{array}{ccc} \cos \theta+k_{x}^{2}(1-\cos \theta) & -\sin \theta k_{z}+(1-\cos \theta) k_{x} k_{y} & \sin \theta k_{y}+(1-\cos \theta) k_{x} k_{z} \\ \sin \theta k_{z}+(1-\cos \theta) k_{x} k_{y} & \cos \theta+k_{y}^{2}(1-\cos \theta) & -\sin \theta k_{x}+(1-\cos \theta) k_{y} k_{z} \\ -\sin \theta k_{y}+(1-\cos \theta) k_{x} k_{z} & \sin \theta k_{x}+(1-\cos \theta) k_{y} k_{x} & \cos \theta+k_{z}^{2}(1-\cos \theta) \end{array}\right]
R(k,θ)=
cosθ+kx2(1−cosθ)sinθkz+(1−cosθ)kxky−sinθky+(1−cosθ)kxkz−sinθkz+(1−cosθ)kxkycosθ+ky2(1−cosθ)sinθkx+(1−cosθ)kykxsinθky+(1−cosθ)kxkz−sinθkx+(1−cosθ)kykzcosθ+kz2(1−cosθ)
Eigen::AngleAxisd rot_vec(vec.norm(), vec.normalized());
Eigen::Matrix3d M = rot_vec.toRotationMatrix();
4. 欧拉角
Eigen::Vector3d eulerAngle(yaw,pitch,roll);
4.1. 欧拉角 -> 四元数
把旋转外参表示为绕ZYX定轴分解得到连乘形式:
q = q z ( ψ ) ⊗ q y ( θ ) ⊗ q x ( ϕ ) \mathbf{q}= \mathbf{q}_{z}(\psi) \otimes \mathbf{q}_{y}(\theta) \otimes \mathbf{q}_{x}(\phi) q=qz(ψ)⊗qy(θ)⊗qx(ϕ)
=
[
cos
(
ψ
/
2
)
0
0
sin
(
ψ
/
2
)
]
[
cos
(
θ
/
2
)
0
sin
(
θ
/
2
)
0
]
[
cos
(
ϕ
/
2
)
sin
(
ϕ
/
2
)
0
0
]
\begin{aligned} =\left[\begin{array}{c}\cos (\psi / 2) \\ 0 \\ 0 \\ \sin (\psi / 2)\end{array}\right]\left[\begin{array}{c}\cos (\theta / 2) \\ 0 \\ \sin (\theta / 2) \\ 0\end{array}\right]\left[\begin{array}{c}\cos (\phi / 2) \\ \sin (\phi / 2) \\ 0 \\ 0\end{array}\right] \\ &\end{aligned}
=
cos(ψ/2)00sin(ψ/2)
cos(θ/2)0sin(θ/2)0
cos(ϕ/2)sin(ϕ/2)00
=
[
cos
(
ϕ
/
2
)
cos
(
θ
/
2
)
cos
(
ψ
/
2
)
+
sin
(
ϕ
/
2
)
sin
(
θ
/
2
)
sin
(
ψ
/
2
)
sin
(
ϕ
/
2
)
cos
(
θ
/
2
)
cos
(
ψ
/
2
)
−
cos
(
ϕ
/
2
)
sin
(
θ
/
2
)
sin
(
ψ
/
2
)
cos
(
ϕ
/
2
)
sin
(
θ
/
2
)
cos
(
ψ
/
2
)
+
sin
(
ϕ
/
2
)
cos
(
θ
/
2
)
sin
(
ψ
/
2
)
cos
(
ϕ
/
2
)
cos
(
θ
/
2
)
sin
(
ψ
/
2
)
−
sin
(
ϕ
/
2
)
sin
(
θ
/
2
)
cos
(
ψ
/
2
)
]
\begin{aligned}=\left[\begin{array}{l}\cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)\end{array}\right] \end{aligned}
=
cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)−cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)−sin(ϕ/2)sin(θ/2)cos(ψ/2)
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
4.2. 欧拉角 -> 旋转矩阵
R x ( θ ) = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R_{x}(\theta)=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta\end{array}\right] Rx(θ)= 1000cosθsinθ0−sinθcosθ
R y ( θ ) = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_{y}(\theta)=\left[\begin{array}{ccc}\cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta\end{array}\right] Ry(θ)= cosθ0−sinθ010sinθ0cosθ
R z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_{z}(\theta)=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] Rz(θ)= cosθsinθ0−sinθcosθ0001
所以,欧拉角转旋转矩阵如下:
R = R z ( ϕ ) R y ( θ ) R x ( ψ ) = R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi) = R=Rz(ϕ)Ry(θ)Rx(ψ)=
[ cos θ cos ϕ sin ψ sin θ cos ϕ − cos ψ sin ϕ cos ψ sin θ cos ϕ + sin ψ sin ϕ cos θ sin ϕ sin ψ sin θ sin ϕ + cos ψ cos ϕ cos ψ sin θ sin ϕ − sin ψ cos ϕ − sin θ sin ψ cos θ cos ψ cos θ ] \left[\begin{array}{ccc}\cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta\end{array}\right] cosθcosϕcosθsinϕ−sinθsinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθcosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ
附录: 其他转换顺序的欧拉角转旋转矩阵
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
Eigen::Matrix3d rotationMatrix = q.matrix();
也可以
Eigen::Matrix3d Rx_angle(double angle)
{
Eigen::Matrix3d R;
R << 1,0,0,0, cos(angle),-sin(angle),0,sin(angle),cos(angle);
return R;
}
Eigen::Matrix3d Ry_angle(double angle)
{
Eigen::Matrix3d R;
R << cos(angle),0,sin(angle),0,1,0,-sin(angle),0,cos(angle);
return R;
}
Eigen::Matrix3d Rz_angle(double angle)
{
Eigen::Matrix3d R;
R << cos(angle),-sin(angle),0,sin(angle),cos(angle),0,0,0,1;
return R;
}
Eigen::Matrix3d R_angle_ = Rz_angle(angle[i][0]/ 180 *M_PI) * Ry_angle(angle[i][1]/ 180 *M_PI) * Rx_angle(angle[i][2]/ 180 *M_PI);
- 验证
R
x
(
θ
)
,
R
y
(
θ
)
,
R
z
(
θ
)
R_{x}(\theta),R_{y}(\theta),R_{z}(\theta)
Rx(θ),Ry(θ),Rz(θ)
我们首先假定坐标系为常用的右手坐标系:
那么 R x ( θ ) R_{x}(\theta) Rx(θ)就表示为从y轴旋转到z轴(明确逆时针为正,判断顺时针还是逆时针需要把轴朝向自己后再判断)
对应到下图,x轴相当于y轴,y轴相当与z轴。
我们可以建立公式:
x
′
=
r
c
o
s
(
θ
+
ϕ
)
=
r
cos
(
ϕ
)
cos
(
θ
)
−
r
sin
(
ϕ
)
sin
(
θ
)
=
x
cos
(
θ
)
−
y
sin
(
θ
)
y
′
=
r
s
i
n
(
θ
+
ϕ
)
=
r
sin
(
ϕ
)
cos
(
θ
)
+
r
cos
(
ϕ
)
sin
(
θ
)
=
x
sin
(
θ
)
+
y
cos
(
θ
)
\begin{array}{l} x^{\prime}=rcos(\theta + \phi )=r \cos (\phi) \cos (\theta)-r \sin (\phi) \sin (\theta)=x \cos (\theta)-y \sin (\theta) \\ y^{\prime}=rsin(\theta + \phi )=r \sin (\phi) \cos (\theta)+r \cos (\phi) \sin (\theta)=x \sin (\theta)+y \cos (\theta) \end{array}
x′=rcos(θ+ϕ)=rcos(ϕ)cos(θ)−rsin(ϕ)sin(θ)=xcos(θ)−ysin(θ)y′=rsin(θ+ϕ)=rsin(ϕ)cos(θ)+rcos(ϕ)sin(θ)=xsin(θ)+ycos(θ)
矩阵形式为:
R
x
(
θ
)
=
[
x
′
y
′
]
=
[
cos
(
θ
)
−
sin
(
θ
)
sin
(
θ
)
cos
(
θ
)
]
[
x
y
]
R_{x}(\theta)= \left[\begin{array}{l} x^{\prime} \\ y^{\prime} \end{array}\right]=\left[\begin{array}{cc} \cos (\theta) & -\sin (\theta) \\ \sin (\theta) & \cos (\theta) \end{array}\right]\left[\begin{array}{l} x \\ y \end{array}\right]
Rx(θ)=[x′y′]=[cos(θ)sin(θ)−sin(θ)cos(θ)][xy]
R x ( θ ) = [ 1 0 0 0 cos ( θ ) − sin ( θ ) 0 sin ( θ ) cos ( θ ) ] [ x y z ] R_{x}(\theta)=\left[\begin{array}{cc} 1&0&0\\ 0&\cos (\theta) & -\sin (\theta) \\ 0&\sin (\theta) & \cos (\theta) \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right] Rx(θ)= 1000cos(θ)sin(θ)0−sin(θ)cos(θ) xyz
R y ( θ ) R_{y}(\theta) Ry(θ)就表示为从z轴旋转到x轴
对应到下图,x轴相当于z轴,y轴相当与x轴。
矩阵形式为:
R
x
(
θ
)
=
[
x
′
y
′
]
=
[
cos
(
θ
)
−
sin
(
θ
)
sin
(
θ
)
cos
(
θ
)
]
[
x
y
]
R_{x}(\theta)= \left[\begin{array}{l} x^{\prime} \\ y^{\prime} \end{array}\right]=\left[\begin{array}{cc} \cos (\theta) & -\sin (\theta) \\ \sin (\theta) & \cos (\theta) \end{array}\right]\left[\begin{array}{l} x \\ y \end{array}\right]
Rx(θ)=[x′y′]=[cos(θ)sin(θ)−sin(θ)cos(θ)][xy]
明确:二维的x对应三维的z,二维的y对应三维的x
R
y
(
θ
)
=
[
cos
(
θ
)
0
sin
(
θ
)
0
1
0
−
sin
(
θ
)
0
cos
(
θ
)
]
[
x
y
z
]
R_{y}(\theta)=\left[\begin{array}{cc} \cos (\theta)&0&\sin (\theta)\\ 0& 1& 0 \\ -\sin (\theta) &0& \cos (\theta) \end{array}\right]\left[\begin{array}{l} x \\ y \\ z \end{array}\right]
Ry(θ)=
cos(θ)0−sin(θ)010sin(θ)0cos(θ)
xyz
R
z
(
θ
)
R_{z}(\theta)
Rz(θ)同理
4.3. 欧拉角 -> 轴角
Eigen::AngleAxisd angel_axisd =
Eigen::AngleAxisd(yaw角(弧度制) , Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(pitch角(弧度制)], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll角(弧度制)], Eigen::Vector3d::UnitX());
5. 四元数
5.1. 单位四元数 -> 旋转矩阵
假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0+q1i+q2j+q3k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。
p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp
q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗
L
(
q
)
R
(
q
∗
)
=
[
s
−
v
T
v
s
I
3
×
3
+
[
v
×
]
]
[
s
v
T
−
v
s
I
3
×
3
+
[
v
×
]
]
=
[
a
b
c
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
]
\mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})= \begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \\{}\\= \begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}
L(q)R(q∗)=[sv−vTsI3×3+[v×]][s−vvTsI3×3+[v×]]=[acbvvT+s2I+2s[v×]+(v×)2]
因为
p
\mathbf p
p和
p
′
\mathbf p'
p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R
=
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
=
[
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
−
2
q
3
q
0
2
q
1
q
3
+
2
q
2
q
0
2
q
1
q
2
+
2
q
3
q
0
1
−
2
q
1
2
−
2
q
3
2
2
q
2
q
3
−
2
q
1
q
0
2
q
1
q
3
−
2
q
2
q
0
2
q
2
q
3
+
2
q
1
q
0
1
−
2
q
1
2
−
2
q
2
2
]
R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2 \\= \left[\begin{array}{ccc} 1-2 q_{2}^{2}-2 q_{3}^{2} && 2 q_{1} q_{2}-2 q_{3} q_{0} && 2 q_{1} q_{3}+2 q_{2} q_{0} \\ 2 q_{1} q_{2}+2 q_{3} q_{0} && 1-2 q_{1}^{2}-2 q_{3}^{2} && 2 q_{2} q_{3}-2 q_{1} q_{0} \\ 2 q_{1} q_{3}-2 q_{2} q_{0} && 2 q_{2} q_{3}+2 q_{1} q_{0} && 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right]
R=vvT+s2I+2s[v×]+(v×)2=
1−2q22−2q322q1q2+2q3q02q1q3−2q2q02q1q2−2q3q01−2q12−2q322q2q3+2q1q02q1q3+2q2q02q2q3−2q1q01−2q12−2q22
其中
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}
q=q0+q1i+q2j+q3k
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
5.2. 四元数 -> 欧拉角
q = q z ( ψ ) ⊗ q y ( θ ) ⊗ q x ( ϕ ) \mathbf{q}= \mathbf{q}_{z}(\psi) \otimes \mathbf{q}_{y}(\theta) \otimes \mathbf{q}_{x}(\phi) q=qz(ψ)⊗qy(θ)⊗qx(ϕ)
=
[
cos
(
ψ
/
2
)
0
0
sin
(
ψ
/
2
)
]
[
cos
(
θ
/
2
)
0
sin
(
θ
/
2
)
0
]
[
cos
(
ϕ
/
2
)
sin
(
ϕ
/
2
)
0
0
]
\begin{aligned} =\left[\begin{array}{c}\cos (\psi / 2) \\ 0 \\ 0 \\ \sin (\psi / 2)\end{array}\right]\left[\begin{array}{c}\cos (\theta / 2) \\ 0 \\ \sin (\theta / 2) \\ 0\end{array}\right]\left[\begin{array}{c}\cos (\phi / 2) \\ \sin (\phi / 2) \\ 0 \\ 0\end{array}\right] \\ &\end{aligned}
=
cos(ψ/2)00sin(ψ/2)
cos(θ/2)0sin(θ/2)0
cos(ϕ/2)sin(ϕ/2)00
=
[
cos
(
ϕ
/
2
)
cos
(
θ
/
2
)
cos
(
ψ
/
2
)
+
sin
(
ϕ
/
2
)
sin
(
θ
/
2
)
sin
(
ψ
/
2
)
sin
(
ϕ
/
2
)
cos
(
θ
/
2
)
cos
(
ψ
/
2
)
−
cos
(
ϕ
/
2
)
sin
(
θ
/
2
)
sin
(
ψ
/
2
)
cos
(
ϕ
/
2
)
sin
(
θ
/
2
)
cos
(
ψ
/
2
)
+
sin
(
ϕ
/
2
)
cos
(
θ
/
2
)
sin
(
ψ
/
2
)
cos
(
ϕ
/
2
)
cos
(
θ
/
2
)
sin
(
ψ
/
2
)
−
sin
(
ϕ
/
2
)
sin
(
θ
/
2
)
cos
(
ψ
/
2
)
]
\begin{aligned}=\left[\begin{array}{l}\cos (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \sin (\phi / 2) \cos (\theta / 2) \cos (\psi / 2)-\cos (\phi / 2) \sin (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)+\sin (\phi / 2) \cos (\theta / 2) \sin (\psi / 2) \\ \cos (\phi / 2) \cos (\theta / 2) \sin (\psi / 2)-\sin (\phi / 2) \sin (\theta / 2) \cos (\psi / 2)\end{array}\right] \end{aligned}
=
cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)−cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)−sin(ϕ/2)sin(θ/2)cos(ψ/2)
则有
[
ϕ
θ
ψ
]
=
[
arctan
2
(
q
0
q
1
+
q
2
q
3
)
1
−
2
(
q
1
2
+
q
2
2
)
arcsin
(
2
(
q
0
q
2
−
q
3
q
1
)
)
arctan
2
(
q
0
q
3
+
q
1
q
2
)
1
−
2
(
q
2
2
+
q
3
2
)
]
\left[\begin{array}{c}\phi \\ \theta \\ \psi\end{array}\right]=\left[\begin{array}{c}\arctan \frac{2\left(q_{0} q_{1}+q_{2} q_{3}\right)}{1-2\left(q_{1}^{2}+q_{2}^{2}\right)} \\ \arcsin \left(2\left(q_{0} q_{2}-q_{3} q_{1}\right)\right) \\ \arctan \frac{2\left(q_{0} q_{3}+q_{1} q_{2}\right)}{1-2\left(q_{2}^{2}+q_{3}^{2}\right)}\end{array}\right]
ϕθψ
=
arctan1−2(q12+q22)2(q0q1+q2q3)arcsin(2(q0q2−q3q1))arctan1−2(q22+q32)2(q0q3+q1q2)
arctan 和arcsin的结果是是 [ − π 2 , π 2 ] \left[-\frac{\pi}{2}, \frac{\pi}{2}\right] [−2π,2π], 这并不能覆盖所有朝向,因此需要用atan2来代替arctan。
注:
-
arctan(y / x)求出的取值范围是[-PI/2, PI/2]。
-
atan2(y , x) 求出的θ取值范围是[-PI, PI]。
[ ϕ θ ψ ] = [ atan 2 ( 2 ( q 0 q 1 + q 2 q 3 ) , 1 − 2 ( q 1 2 + q 2 2 ) ) asin ( 2 ( q 0 q 2 − q 3 q 1 ) ) atan 2 ( 2 ( q 0 q 3 + q 1 q 2 ) , 1 − 2 ( q 2 2 + q 3 2 ) ) ] \left[\begin{array}{c}\phi \\ \theta \\ \psi\end{array}\right]=\left[\begin{array}{c}\operatorname{atan} 2\left(2\left(q_{0} q_{1}+q_{2} q_{3}\right), 1-2\left(q_{1}^{2}+q_{2}^{2}\right)\right) \\ \operatorname{asin}\left(2\left(q_{0} q_{2}-q_{3} q_{1}\right)\right) \\ \operatorname{atan} 2\left(2\left(q_{0} q_{3}+q_{1} q_{2}\right), 1-2\left(q_{2}^{2}+q_{3}^{2}\right)\right)\end{array}\right] ϕθψ = atan2(2(q0q1+q2q3),1−2(q12+q22))asin(2(q0q2−q3q1))atan2(2(q0q3+q1q2),1−2(q22+q32))
// 方法1:
double roll = atan2(2 * (quaternion.w()* quaternion.x() + quaternion.y() * quaternion.z()) , 1 - 2 * (pow( quaternion.x() + quaternion.y(),2 )));
double pitch = asin(2 * (quaternion.w()* quaternion.y() -quaternion.z() * quaternion.x()) );
double yaw = atan2(2 * (quaternion.w()* quaternion.z() + quaternion.x() * quaternion.y()) , 1 - 2 * (pow( quaternion.y() + quaternion.z(),2 )));
// 方法2:
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
5.3. 四元数 -> 轴角
假设有单位四元数 q = q 0 + q 1 i + q 2 j + q 3 k = s + v \mathbf q= q_0 + q_1\mathbf i + q_2\mathbf j + q_3\mathbf k =s+ \mathbf v q=q0+q1i+q2j+q3k=s+v,三维点 p \mathbf p p经过旋转之后变为 p ′ \mathbf p' p′,如果使用矩阵描述,那么有 p ′ = R p \mathbf p'=\mathbf R \mathbf p p′=Rp 。 如果用四元数描述旋转,则表示为 p ′ = q p q − 1 \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1} p′=qpq−1。
p ′ = q p q − 1 = L ( q ) R ( q − 1 ) p = R p \mathbf p'=\mathbf q \mathbf p\mathbf q^{-1}=\mathcal L(\mathbf q) \mathcal R(\mathbf q^{-1})\mathbf p = \mathbf R \mathbf p p′=qpq−1=L(q)R(q−1)p=Rp
q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \mathbf q^{-1}=\mathbf q^*/||\mathbf q||^2 q−1=q∗/∣∣q∣∣2,所以单位四元数的逆即为 q ∗ \mathbf q^* q∗
L
(
q
)
R
(
q
∗
)
=
[
s
−
v
T
v
s
I
3
×
3
+
[
v
×
]
]
[
s
v
T
−
v
s
I
3
×
3
+
[
v
×
]
]
=
[
a
b
c
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
]
\mathcal L(\mathbf q) \mathcal R(\mathbf q^{*})= \begin{bmatrix} s &-\mathbf v^T\\ \mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \begin{bmatrix} s &\mathbf v^T\\ -\mathbf v & s\mathbf I_{3\times3} +[\mathbf v\times]\end{bmatrix} \\{}\\= \begin{bmatrix} a &b\\c & \mathbf v \mathbf v^T+s^2 \mathbf I+2s [\mathbf v\times]+(\mathbf v\times)^2\end{bmatrix}
L(q)R(q∗)=[sv−vTsI3×3+[v×]][s−vvTsI3×3+[v×]]=[acbvvT+s2I+2s[v×]+(v×)2]
因为
p
\mathbf p
p和
p
′
\mathbf p'
p′都是虚四元数,所以该矩阵的右下角即为从四元数到旋转矩阵的变换关系:
R
=
v
v
T
+
s
2
I
+
2
s
[
v
×
]
+
(
v
×
)
2
R= \mathbf v \mathbf v^T + s^2 \mathbf I +2s [\mathbf v\times] +(\mathbf v\times)^2
R=vvT+s2I+2s[v×]+(v×)2
对上式两侧求迹,得到
t
r
(
R
)
=
t
r
(
v
v
T
)
+
s
2
t
r
(
I
3
×
3
)
+
2
s
t
r
(
[
v
×
]
)
+
t
r
(
(
v
×
)
2
)
=
q
1
2
+
q
2
2
+
q
3
2
+
3
s
3
−
2
q
1
2
−
2
q
2
2
−
2
q
3
2
=
3
s
3
−
(
q
1
2
+
q
2
2
+
q
3
2
)
=
3
s
3
−
(
1
−
s
2
)
=
4
s
2
−
1
tr(R)= tr(\mathbf v \mathbf v^T) + s^2 tr(\mathbf I_{3\times3}) +2s tr([\mathbf v\times]) +tr((\mathbf v\times)^2)\\=q_1^2+q_2^2+q_3^2+3s^3-2q_1^2-2q_2^2-2q_3^2 \\=3s^3-(q_1^2+q_2^2+q_3^2) \\=3s^3-(1-s^2) \\=4s^2-1
tr(R)=tr(vvT)+s2tr(I3×3)+2str([v×])+tr((v×)2)=q12+q22+q32+3s3−2q12−2q22−2q32=3s3−(q12+q22+q32)=3s3−(1−s2)=4s2−1
v v T = [ q 1 q 2 q 3 ] [ q 1 q 2 q 3 ] = [ q 1 2 q 1 q 2 q 1 q 3 q 2 q 1 q 2 2 q 2 q 3 q 3 q 1 q 3 q 2 q 3 2 ] \mathbf v \mathbf v^T= \begin{bmatrix}q_1\\q_2\\q_3\end{bmatrix} \begin{bmatrix}q_1&q_2&q_3\end{bmatrix}= \begin{bmatrix}q_1^2&q_1q_2&q_1q_3\\q_2q_1&q_2^2 &q_2q_3\\q_3q_1&q_3q_2&q_3^2\end{bmatrix} vvT= q1q2q3 [q1q2q3]= q12q2q1q3q1q1q2q22q3q2q1q3q2q3q32
( v × ) 2 = [ 0 − q 3 q 2 q 3 0 − q 1 − q 2 q 1 0 ] [ 0 − q 3 q 2 q 3 0 − q 1 − q 2 q 1 0 ] = [ − q 2 2 − q 3 2 q 1 q 2 q 1 q 3 q 1 q 2 − q 2 2 − q 3 2 q 2 q 3 q 1 q 3 q 2 q 3 − q 1 2 − q 2 2 ] (\mathbf v\times)^2=\begin{bmatrix}0&-q_3&q_2\\q_3&0&-q_1\\-q_2&q_1&0\end{bmatrix}\begin{bmatrix}0&-q_3&q_2\\q_3&0&-q_1\\-q_2&q_1&0\end{bmatrix} = \begin{bmatrix}-q_2^2-q_3^2&q_1q_2&q_1q_3\\q_1q_2&-q_2^2-q_3^2&q_2q_3\\q_1q_3&q_2q_3&-q_1^2-q_2^2\end{bmatrix} (v×)2= 0q3−q2−q30q1q2−q10 0q3−q2−q30q1q2−q10 = −q22−q32q1q2q1q3q1q2−q22−q32q2q3q1q3q2q3−q12−q22
又有了罗德里格斯公式可以得到旋转向量到旋转矩阵的转换关系:
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
[
n
×
]
\mathbf R = cos\theta \mathbf I + (1-cos\theta)\mathbf n \mathbf n^T+sin\theta[\mathbf n\times]
R=cosθI+(1−cosθ)nnT+sinθ[n×]
对上式子求迹可得:
t
r
(
R
)
=
c
o
s
θ
t
r
(
I
)
+
(
1
−
c
o
s
θ
)
t
r
(
n
n
T
)
+
s
i
n
θ
[
(
n
×
]
)
=
3
c
o
s
θ
+
(
1
−
c
o
s
θ
)
+
0
=
1
+
2
c
o
s
θ
=
>
θ
=
a
r
c
c
o
s
t
r
(
R
)
−
1
2
=
a
r
c
c
o
s
(
2
s
2
−
1
)
=
>
c
o
s
θ
=
2
c
o
s
2
θ
2
−
1
=
2
s
2
−
1
=
>
θ
=
2
a
r
c
c
o
s
(
s
)
tr(\mathbf R) = cos\theta tr(\mathbf I) + (1-cos\theta)tr(\mathbf n \mathbf n^T)+sin\theta[(\mathbf n\times]) \\= 3cos\theta + (1-cos\theta) + 0 \\ = 1 + 2cos\theta \\=>\theta = arccos\frac{tr(\mathbf R)-1}{2}=arccos(2s^2-1) \\ => cos\theta = 2cos^2\frac{\theta}{2}-1 =2s^2-1 \\ => \theta = 2 arccos(s)
tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθ[(n×])=3cosθ+(1−cosθ)+0=1+2cosθ=>θ=arccos2tr(R)−1=arccos(2s2−1)=>cosθ=2cos22θ−1=2s2−1=>θ=2arccos(s)
轴角绕单位轴
u
\mathbf{u}
u 旋转
θ
\theta
θ 的四元数是:
至于旋转轴,如果在四元数转换的时候用
q
\mathbf q
q的虚部代替
p
\mathbf p
p,易知
q
\mathbf q
q的虚部组成的向量在旋转时是不动的,即构成旋转轴。于是只要将它除以它的模长,即得。
u
=
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
s
i
n
θ
2
\mathbf{u}=[n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/sin\frac{\theta}{2}
u=[nx,ny,nz]T=[q1,q2,q3]T/sin2θ
q ( w , v ) = ( cos θ 2 , u sin θ 2 ) \mathbf{q}(w, \mathbf{v})=\left(\cos \frac{\theta}{2}, \mathbf{u} \sin \frac{\theta}{2}\right) q(w,v)=(cos2θ,usin2θ)
// 方法1
double sin_theta =
sqrt(quaternion.x() * quaternion.x() + quaternion.y() * quaternion.y() + quaternion.z() * quaternion.z());
double cos_theta = quaternion.w();
double rotation_angle_rad1 = ((cos_theta < 0.0) ? atan2(-sin_theta, -cos_theta) : atan2(sin_theta, cos_theta));
std::cout << " rotation_angle_rad :" << 2 * rotation_angle_rad1 << std::endl;
std::cout << " rotation_angle_degree :" << 2 * rotation_angle_rad1 * 180.0 / M_PI << std::endl;
// 方法2
double rotation_angle_rad2 = 2 * acos(quaternion.w());
std::cout << " rotation_angle_rad2 :" << rotation_angle_rad2 << std::endl;
Eigen::Vector3d rotation_axis = quaternion.vec() / sin(rotation_angle_rad2 / 2);
std::cout << "roatation_axis :" << rotation_axis.transpose() << endl;
// 方法3
Eigen::AngleAxisd rotation_vector(quaternion);
std::cout << "rotation_vector.angle() : " << rotation_vector.angle() << endl;
std::cout << "rotation_vector.axis() : " << rotation_vector.axis().transpose() << endl;