Tags: SLAM
一、旋转与旋转矩阵
1.2D旋转
如下图所示，在坐标系O-xy中有一点P，已知坐标为(x,y)，向量OP与X轴的夹角为β。将点P绕坐标原点O旋转了α角到P’。这里规定与右手系旋转方向相同的为旋转正方向，对于2D情况而言即逆时针旋转为旋转正方向。但在摄影测量中一般规定的是顺时针旋转方向为正方向。因此在应用时需要注意正方向的问题。正方向不同最终会导致的结果是旋转矩阵中sinα的符号是相反的。设P’点的坐标为(x’,y’)。点P、P’在x、y轴上的垂足分别为A、B、C、D。试用P点坐标表示P’点坐标。
首先由于旋转，所以OP=OP’，设其长度为r。有如下公式。
r=x2+y2‾‾‾‾‾‾‾√
而且，在三角形OPA中，有
cosβ=xrsinβ=yr
也即
x=rcosβy=rsinβ
基于上面的已知条件，在三角形OP’C中，x’可以如下表示
x′=r⋅cos(α+β)=r⋅(cosαcosβ−sinαsinβ)=cosαcosβ⋅r−sinαsinβ⋅r=cosα⋅x−sinα⋅y
同理y’如下
y′=r⋅sin(α+β)=r⋅(sinαcosβ+cosαsinβ)=sinαcosβ⋅r+cosαsinβ⋅r=sinα⋅x+cosα⋅y
因此将上面的式子写成矩阵相乘的形式如下
(x′y′)=(cosαsinα−sinαcosα)(xy)
这个公式便是二维平面内旋转前后的坐标关系。
2.3D旋转
在推得上面2D旋转的公式之后，3D旋转公式就相对简单了。例如某个点绕Z轴旋转α角，也就是说旋转后的Z坐标是不变的，变化的只是X、Y坐标。因此甚至可以基于2D公式，不用推导的写出下面这个式子，因为为了满足相等关系，必须补充系数矩阵元素。
⎛⎝⎜⎜⎜x′y′z⎞⎠⎟⎟⎟=⎛⎝⎜⎜cosαsinα0−sinαcosα0001⎞⎠⎟⎟⎛⎝⎜⎜xyz⎞⎠⎟⎟
而这个式子中的系数矩阵可以记为如下形式
RZ(α)=⎛⎝⎜⎜cosαsinα0−sinαcosα0001⎞⎠⎟⎟
便把R称为旋转矩阵。同时依据绕哪个轴旋转其坐标不变这个性质可以从形式上很快写出绕X、Y轴的旋转矩阵。但这里千万要注意并不是简单的把sin、cos挪个位置而已。因为涉及到旋转后的横、纵轴问题。例如以Y轴为转轴进行旋转，那么横轴应该是原坐标系的Z轴，纵轴应该是原坐标系的X轴。这一点一定要注意，否则会出错。也就是说最后的旋转矩阵中sin会相差一个负号，进而导致后续运算的出错。如下图。
为了简单记住到底旋转后把谁当作横轴谁当作纵轴，总结了一个简单的小方法。即以哪个轴旋转，就用手握住这个轴，然后逆时针转动，最先碰到的那个轴作为横轴，剩下的那个作为纵轴，如下图。
据此，可得到绕各轴旋转的旋转矩阵
RX(α)=⎛⎝⎜⎜1000cosαsinα0−sinαcosα⎞⎠⎟⎟RY(α)=⎛⎝⎜⎜cosα0−sinα010sinα0cosα⎞⎠⎟⎟RZ(α)=⎛⎝⎜⎜cosαsinα0−sinαcosα0001⎞⎠⎟⎟
二、欧拉角
欧拉角用于描述某个物体在坐标系中的姿态。假设一个物体的前方（朝向我们的方向）为X轴，右侧为Y轴，上方为Z轴。那么可以定义翻滚、俯仰和偏航如下。
翻滚，Roll，绕X轴
俯仰，Pitch，绕Y轴
偏航，Yaw，绕Z轴
这样对于任何一个姿态，我们可以利用Roll-Pitch-Yaw三个角依次旋转从而还原和表述物体的姿态了，形象而且直观。
但是这里有个很重要的问题，即绕谁旋转。每次是绕固定轴旋转还是绕旋转之后的轴旋转的，这会导致不一样的结果。也就是说 描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。
第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转γ，然后绕{A}的Y轴旋转β，最后绕{A}的Z轴旋转α，就能旋转到当前姿态。在整个过程中，坐标系{A}都没有发生变动。
另一种姿态描述方式是绕自身坐标轴旋转：假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转α，然后绕Y轴旋转β，最后绕X轴旋转γ，就能旋转到当前姿态。
虽然两种旋转方式不同，但可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转(γ,β,α)和绕自身坐标轴Z-Y-X旋转(α,β,γ)的最终结果一样，只是描述的方法有差别而已。
R=RZ(α)RY(β)RX(γ)=⎛⎝⎜⎜cosαcosβsinαcosβ−sinβcosαsinβsinγ−sinαcosγcosαcosγ+sinαsinβsinγcosβsinγcosαsinβcosγ+sinαsinγsinαsinβcosγ−sinγcosαcosβcosγ⎞⎠⎟⎟
三、四元数
一个四元数的定义如下
q=q0+q1i+q2j+q3k
一个四元数拥有一个实部和三个虚部，三个虚部之间满足如下关系。
⎧⎩⎨⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
有时四元数也定义成(w,x,y,z)的形式，q0对应w，x、y、z依次对应q1、q2、q3。四元数的定义有了，那么如何用四元数表示旋转呢？如某次旋转是绕某一向量K=(Kx,Ky,Kz)进行了角度为θ的旋转，那么利用四元数就可以表示这个旋转：
x=Kxsinθ2y=Kysinθ2z=Kzsinθ2w=cosθ2
而且满足条件
x2+y2+z2+w2=1
四、相互转换
1.四元数转旋转矩阵
设四元数为：
q=q0+q1i+q2j+q3k
则其对应的旋转矩阵R为：
R=⎛⎝⎜⎜⎜1−2(q22+q23)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q21+q23)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q21+q22)⎞⎠⎟⎟⎟
2.旋转矩阵转四元数
已知旋转矩阵如下
R=⎛⎝⎜⎜r11r21r31r12r22r32r13r23r33⎞⎠⎟⎟
其对应的四元数为
⎧⎩⎨⎪⎪⎪⎪⎪⎪q0=tr(R)+1√2q1=r23−r324q0q2=r31−r134q0q3=r12−r214q0
其中tr(R)表示R矩阵的迹，也即矩阵R的主对角线（从左上方至右下方的对角线）上各个元素的总和。
3.欧拉角转旋转矩阵
某次旋转绕固定坐标轴X-Y-Z旋转(γ,β,α)或者说绕自身坐标轴Z-Y-X旋转(α,β,γ)，对应的旋转矩阵如下
R=RZ(α)RY(β)RX(γ)=⎛⎝⎜⎜cosαcosβsinαcosβ−sinβcosαsinβsinγ−sinαcosγcosαcosγ+sinαsinβsinγcosβsinγcosαsinβcosγ+sinαsinγsinαsinβcosγ−sinγcosαcosβcosγ⎞⎠⎟⎟
4.旋转矩阵转欧拉角
有旋转矩阵
R=⎛⎝⎜⎜r11r21r31r12r22r32r13r23r33⎞⎠⎟⎟
可求出各轴的旋转角为
θZ=atan2(r21,r11)θY=atan2(−r31,r231+r233‾‾‾‾‾‾‾‾√)θX=atan2(r32,r33)
atan2为C++中函数，atan2(y,x)的做法：当 x 的绝对值比 y 的绝对值大时使用 atan(y/x)；反之使用 atan(x/y)。这样就保证了数值稳定性。需要注意的是，旋转顺序必须要是Z、Y、X。
5.欧拉角转四元素
将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ角）转换为四元数：
q=⎛⎝⎜⎜⎜⎜q0q1q2q3⎞⎠⎟⎟⎟⎟=⎛⎝⎜⎜⎜⎜⎜⎜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)⎞⎠⎟⎟⎟⎟⎟⎟
6.四元数转欧拉角
由四元数q=(q0,q1,q2,q3)或q=(w,x,y,z)到欧拉角的转换为
α=arctan(2(q0q1+q2q3)1−2(q21+q22))β=arcsin(2(q0q2−q1q3))γ=arctan(2(q0q3+q1q2)1−2(q22+q23))
由于arctan和arcsin的取值范围在−π/2 - π/2之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数。
α=atan2(2(q0q1+q2q3),1−2(q21+q22))β=arcsin(2(q0q2−q1q3))γ=atan2(2(q0q3+q1q2),1−2(q22+q23))
最后这个网站(https://quaternions.online/) 可以动态展示四元数与欧拉角的对应关系和变化情况，便于理解。
五、参考资料
