序號	內容
1	【數理知識】自由度 degree of freedom 及自由度的計算方法
2	【數理知識】剛體 rigid body 及剛體的運動
3	【數理知識】剛體基本運動，平動，轉動
4	【數理知識】向量數乘，內積，外積，matlab代碼實現
5	【數理知識】最小二乘法，從線性回歸出發，數值舉例并用最小二乘法求解回歸模型
6	【數理知識】最小二乘法，一般線性情況，矩陣化表示過程，最佳參數的求解公式過程
7	【數理知識】協方差，隨機變量的的協方差，隨機變量分別是單個數字和向量時的協方差
8	【數理知識】奇異值分解，從數據的線性變換角度來理解
9	【數理知識】旋轉矩陣的推導過程，基于向量的旋轉來實現，同時解決歐式變換的非線性局限
10	【數理知識】三維空間旋轉矩陣的歐拉角表示法，四元數表示法，兩者之間的轉換，Matlab 代碼實現
11	【數理知識】已知 N>=3 個點在前后時刻的坐標，求剛體平移矩陣，旋轉矩陣，且這 N>=3 點間距離始終不變代表一個剛體

之前我們已經討論過旋轉矩陣。需要再次強調的是，旋轉的順序很重要，并且會影響最終的結果。先旋轉 $X$ 軸，再旋轉 $Y$ 軸，最后旋轉 $Z$ 軸得到的結果與先旋轉 $Z$ 軸，再旋轉 $Y$ 軸，最后旋轉 $X$ 軸得到的結果是不同的。這種順序的差異導致了不同的方向和空間方向的變化。這也是為什么在實際應用中，我們需要明確指定旋轉順序，以確保我們得到正確和一致的結果。

這次基于三維空間，討論下旋轉矩陣的兩種表示方法，分別是歐拉角表示法，四元數表示法，以及二者之間的轉換關系如何。

在三維空間中，旋轉矩陣 $R$ 的維度為 $3\times 3$，其是一個正交矩陣，行列式為 $1$。

1. 歐拉角（Euler Angles）表示法

歐拉角通常由三個角度組成

• 滾轉角（roll），常用符號為 $?$
• 俯仰角（pitch），常用符號為 $\theta$
• 偏航角（yaw），常用符號為 $\psi$

這三個角度分別描述了繞 $X,Y,Z$ 軸旋轉的角度。

繞 $X$ 軸旋轉 $?$ 角度的旋轉矩陣為

$$R_x(?)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (?)& ?\sin (?)\\ 0& \sin (?)& \cos (?)\end{array}\right]$$

繞 $Y$ 軸旋轉 $\theta$ 角度的旋轉矩陣為

$$R_y(\theta )=\left[\begin{array}{ccc}\cos (\theta )& 0& \sin (\theta )\\ 0& 1& 0\\ ?\sin (\theta )& 0& \cos (\theta )\end{array}\right]$$

繞 $Z$ 軸旋轉 $\psi$ 角度的旋轉矩陣為

$$R_z(\psi )=\left[\begin{array}{ccc}\cos (\psi )& ?\sin (\psi )& 0\\ \sin (\psi )& \cos (\psi )& 0\\ 0& 0& 1\end{array}\right]$$

例如，對于一個分別依次繞固定軸 $XYZ$ 的歐拉角表示，其旋轉矩陣為

$$\begin{array}{rl}R& =R_z(?)R_y(\theta )R_x(\psi )\\ & =\left[\begin{array}{ccc}\cos (\theta )\cos (\psi )& \sin (?)\sin (\theta )\cos (\psi )?\cos (?)\sin (\psi )& \cos (?)\sin (\theta )\cos (\psi )+\sin (?)\sin (\psi )\\ \cos (\theta )\sin (\psi )& \sin (?)\sin (\theta )\sin (\psi )+\cos (?)\cos (\psi )& \cos (?)\sin (\theta )\sin (\psi )?\sin (?)\cos (\psi )\\ ?\sin (\theta )& \sin (?)\cos (\theta )& \cos (?)\cos (\theta )\end{array}\right]\end{array}$$

這個矩陣代表了首先繞 $X$ 軸旋轉 $?$ 角，然后繞 $Y$ 軸旋轉 $\theta$ 角，再然后繞 $Z$ 軸旋轉 $\psi$ 角的總的旋轉效果。

更多關于歐拉角的推導和細節可參考文章：第3章-數理知識基礎 -＞ 坐標轉換和【數理知識】旋轉矩陣的推導過程，基于向量的旋轉來實現，同時解決歐式變換的非線性局限。

% 給定歐拉角 phi theta psi
phi   = deg2rad(10);  % 示例：10度，記得轉換為弧度
theta = deg2rad(22);  % 示例：22度
psi   = deg2rad(35);  % 示例：35度R_x = [ 1  0         00  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)  0sin(psi)  cos(psi)  00         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.40180.5318    0.8440    0.0694-0.3746    0.1610    0.9131

---

% 給定點坐標
point_1 = [ 102235];
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. 四元數（Quaternion）表示法

四元數是由 $1$ 個實數加上 $3$ 個復數組合而成，通常可以表示為

$$q=q_w+q_x\text{i}+q_y\text{j}+q_z\text{k}$$

其中 $q_w,q_x,q_y,q_z$ 都是實數，$\text{i,?j,?k}$ 是四元數的基元，滿足如下所示的乘法關系

• ${\text{i}}^2={\text{j}}^2={\text{k}}^2=\text{i}\text{j}\text{k}=?1$
• ${\text{i}}^0={\text{j}}^0={\text{k}}^0=1$

四元數還可看作由一個標量和一個向量組成，其中 $q_w$ 是四元數的標量部分，$q_x,q_y,q_z$ 構成四元數的向量部分。

---

假設有兩個四元數分別為 $q_1=(q_{w1},[q_{x1},q_{y1},q_{z1}])$，$q_2=(q_{w2},[q_{x2},q_{y2},q_{z2}])$，同時令 $v_1=[q_{x1},q_{y1},q_{z1}]$，$v_2=[q_{x2},q_{y2},q_{z2}]$，則有如下運算法則

• 四元數的和：$q_1+q_2=(q_{w1}+q_{w2},(v_1+v_2))$
• 四元數乘法：$q_1{q}_2=q_{w1}{q}_{w2}?v_1?v_2+q_{w1}{v}_2+q_{w2}{v}_1+v_1\times v_2=(q_{w1}{q}_{w2}?v_1?v_2,(q_{w1}{v}_2+q_{w2}{v}_1+v_1\times v_2))$
• 四元數的模：$\Vert q_1\Vert =\sqrt{q_{w1}^2+q_{x1}^2+q_{y1}^2+q_{z1}^2}$
• 單位四元數：$\Vert q_1\Vert =1$
• 四元數的共軛：$q_1^{?}=(q_{w1},?v_1)$
• 四元數的逆：$q_1^{?1}=\frac{q_1^{?}}{\Vert q_1\Vert }$

---

四元數是一個擴展的復數系統，常用于表示三維空間中的旋轉。

一個單位四元數（長度為 $1$）可以表示 3D 空間中的旋轉。將一個點旋轉到另一個位置可以通過四元數乘法來完成。

繞 $X$ 軸旋轉 $?$ 角度的四元數為

$$q_{?}=(\cos (\frac{?}{2}),\sin (\frac{?}{2}),0,0)$$

繞 $Y$ 軸旋轉 $\theta$ 角度的四元數為

$$q_{\theta }=(\cos (\frac{\theta }{2}),0,\sin (\frac{\theta }{2}),0,0)$$

繞 $Z$ 軸旋轉 $\psi$ 角度的四元數為

$$q_{\psi }=(\cos (\frac{\psi }{2}),0,0,\sin (\frac{\psi }{2}))$$

總旋轉的四元數是這三個四元數的乘積。四元數乘法不是通常的標量乘法，它有特定的乘法規則。

給定一個四元數 $q$（模長為 1，有關系 $\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}=1$），假設采用的旋轉順序為 $XYZ$，其對應的旋轉矩陣 $R$ 可以表示為

$$\begin{array}{rl}R& =\left[\begin{array}{ccc}1?2(q_y^2+q_z^2)& 2(q_x{q}_y?q_w{q}_z)& 2(q_x{q}_z+q_w{q}_y)\\ 2(q_x{q}_y+q_w{q}_z)& 1?2(q_x^2+q_z^2)& 2(q_y{q}_z?q_w{q}_x)\\ 2(q_x{q}_z?q_w{q}_y)& 2(q_y{q}_z+q_w{q}_x)& 1?2(q_x^2+q_y^2)\end{array}\right]\end{array}$$

單位四元數在描述 3D 旋轉時有一些優勢，其不受歐拉角中的 “萬向鎖” 問題的影響。

% 給定四元數
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.40170.5318    0.8440    0.0694-0.3746    0.1609    0.9131

---

3. 四元數轉歐拉角

從四元數到歐拉角的轉換并不是唯一的，因為對于某些旋轉，存在多種歐拉角表示。但是，對于大多數實際應用，可以從一個特定的四元數計算一個特定的歐拉角集。

給定四元數 $q=(q_w,q_x,q_y,q_z)$，若想將它轉換為 $XYZ$ 順序的歐拉角 $(?,\theta ,\psi )$。以下是從四元數到歐拉角的轉換方法

$$\begin{array}{rl}?& =\text{atan2}(2(q_w{q}_x+q_y{q}_z),1?2(q_x^2+q_y^2))\\ \theta & =\arcsin (2(q_w{q}_y?q_x{q}_z))\\ \psi & =\text{atan2}(2(q_w{q}_z+q_x{q}_y),1?2(q_y^2+q_z^2))\end{array}$$

其中 $\text{atan2}()$ 不是 $\arctan ()$。

舉例說明，因為若使用 $\arctan (y/x)$，其返回值在 $[?\pi /2,\pi /2]$ 之間，因為它不能區分 $x$ 的正負。而 $\text{atan2}(y,x)$，其返回值在 $[?\pi ,\pi ]$ 之間，可以區分 $x$ 的正負，因此更為實用，尤其是在計算歐拉角時。更重要的是，$\text{atan2}(y,x)$ 能夠處理 $x=0$ 的情況，這在計算角度或歐拉角時非常有用。

注意，由于使用 $\arcsin ()$，當 $\theta$ 接近 $\pm 90°$ 時，可能會出現數值不穩定。這是因為在這些極端情況下，航向和滾動變得不可區分，這就是所謂的萬向鎖問題。

% 給定四元數
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.9953theta_deg =21.9990psi_deg =34.9983

---

4. 歐拉角轉四元數

給定三個歐拉角 $?,\theta ,\psi$，相應的四元數為

$$\begin{array}{rl}{q}_w& =\cos (\frac{?}{2})\cos (\frac{\theta }{2})\cos (\frac{\psi }{2})+\sin (\frac{?}{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ q_x& =\sin (\frac{?}{2})\cos (\frac{\theta }{2})\cos (\frac{\psi }{2})?\cos (\frac{?}{2})\sin (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ q_y& =\cos (\frac{?}{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2})+\sin (\frac{?}{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2})\\ q_z& =\cos (\frac{?}{2})\cos (\frac{\theta }{2})\sin (\frac{\psi }{2})?\sin (\frac{?}{2})\sin (\frac{\theta }{2})\cos (\frac{\psi }{2})\end{array}$$

得到的四元數是 $(q_w,q_x,q_y,q_z)$。

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

---

Ref

1. 旋轉矩陣 - Wikipedia
2. 干貨整理：歐拉角、旋轉矩陣、四元數合輯