一 旋轉矩陣
旋轉矩陣（右手坐标系）
繞X軸旋轉
繞Y軸旋轉
繞Z軸旋轉
繞某個向量v繞XYZ旋轉x,y,z進行旋轉。v’ = Rz*Ry*Rx*v;
Matlab代碼如下
syms sinx cosx siny cosy sinz cosz;
Rx = [1 0 0;0 cosx -sinx;0 sinx cosx];
Ry = [cosy 0 siny;0 1 0;-siny 0 cosy];
Rz = [cosz -sinz 0;sinz cosz 0;0 0 1];
R = Rz*Ry*Rx
輸出：
R =
[ cosy*cosz, cosz*sinx*siny - cosx*sinz, sinx*sinz + cosx*cosz*siny]
[ cosy*sinz, cosx*cosz + sinx*siny*sinz, cosx*siny*sinz - cosz*sinx]
[ -siny, cosy*sinx, cosx*cosy ]
二 旋轉矩陣含義
以繞Y軸進行旋轉為例，繞Y軸旋轉對應的矩陣是
[ cosy, 0, siny]
Ry= [ 0, 1, 0 ]
[ -siny, 0, cosy]
從-Y軸方向看繞Y的旋轉：
經過旋轉後，Z軸的方向向量變成了(sin(ry),0,cos(ry))，X軸的方向向量變成了(cos(ry),0,-sin(ry))，而Y的方向向量仍然是(0,1,0)。可以發現，旋轉後的新的三個坐标軸向量即為對應旋轉矩陣的三個列向量。
使用旋轉矩陣對一個向量進行旋轉
[ cosy, 0, siny] [x] [x’]
Ry*v= [ 0, 1, 0 ] *[y] = [y’]
[ -siny, 0, cosy] [z] [z’]
旋轉的實質就是相當于在新的坐标系下，保持該點在新的坐标系下的坐标仍然為(x,y,z)，在新的坐标系的坐标為(x,y,z)，那麼在原先的坐标系下的點的坐标即為Ry*v。
三 左手坐标系向右手坐标系轉換
可以看出，從左手坐标系到右手坐标系轉換，隻需将z值取反即可。
以Qr為右手坐标系的一點，Ql為對應的左手坐标系的點，轉換公式如下
轉換矩陣Sz如下
現在進行旋轉矩陣轉換的推導
假如ML為如下矩陣，
[ a00, a01, a02]
ML = [ a10, a11, a12]
[ a20, a21, a22]
則對應的MR為
[ a00, a01,-a02]
MR = [ a10, a11,-a12]
[ -a20,-a21, a22]
即紅色标出的成員被取反。
四 四元數到旋轉角的轉化
四元數q=[w x y z]，w用來表示旋轉量，(x,y,z)則是旋轉軸。
四元數相乘：
q1 * q2 =
(w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2) i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2) j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2) k
将四元數應用于一個向量使之旋轉，将這個向量v也用四元數表示，v=[0 vx vy vz]，旋轉的計算式為：
同時也可以将四元數的旋轉用矩陣形式表示，對應的旋轉矩陣
同時，使用一組歐拉角，也能計算出對應的旋轉矩陣
R = Rz*Ry*Rx=
[ cosy*cosz, cosz*sinx*siny - cosx*sinz, sinx*sinz + cosx*cosz*siny]
[ cosy*sinz, cosx*cosz + sinx*siny*sinz, cosx*siny*sinz - cosz*sinx]
[ -siny, cosy*sinx, cosx*cosy ]
分别為繞X、Y、Z軸旋轉的角度，也就是對應上式R中的x、y、z。得出如下的轉換公式：
rx= atan2(Mq[2][1],Mq[2][2]),
ry = arcsin(Mq[2][0]);
rz= atan2(Mq[1][0],Mq[0][0]
也就是如下的計算式
五 三角函數
1 asin函數
它的結果範圍是-PI/2到PI/2，使用matlab進行繪圖可得
x=[-1:0.05:1];
y = asin(x);
plot(x,y);
2 acos函數
它的取值範圍是0到PI
x=[-1:0.05:1];
y = acos(x);
plot(x,y);
3 atan2函數
它的取值範圍是-PI/2到PI/2
y=[-10000:1:10000];
x = ones(1,length(y));
rad = atan2(y,x);
plot(y,rad);
結果圖如下：