寒假有幾項學習計劃，其中有一些是為了一些任務而學，最主要的任務是我要在2021_v4的基礎上編寫2022_v1的大援程式碼，為此順便學習一下機器人學的知識（下學期也有這方面的老黃的課程），看看能不能在結構和演算法上對前一版本上有所突破。
本系列參考資料：
- 《Robotics, Vision and Control》
- B站公開課：臺灣交通大學機器人學公開課
- 老黃的《機器人技術基礎》課程講解及PPT
- N多有關機器人學的大佬的部落格
講道理機器人學之所以是機器人學，果然還是有點小難的，我看著這些資料確實有點吃力，看了很久，才把第一章給拿下，希望後面順利。順便練習了一下公式該怎麼敲（如何打公式確實值得自己單獨再學一學。
0128附加：發現公式還挺好打的，本文好多手打公式哦~
00 概述
概述部分我們主要要了解的是我們怎麼描述一個物體在空間中的姿態，以及如何變換不同的視角對同一物體進行描述。
有一定基礎的可以跳過這個部分。
00-1 點與物體的描述
機器人與計算機視覺中的一個最基本的要求是要表示物體在環境中的位置和方向，這個物體，包括我們將要研究的物件：機器人、攝像機、工件、障礙物和路徑。所以對於物體的位置和方向的描述就成為了機器人學的入門知識，也是各種機器人教材、課程的第一課。（hh也是老黃正式上課的第一講）
現在來想一個問題，對於空間中的一個點（不管是上述哪種物體的抽象），我們該怎麼來描述呢？
這個問題很熟悉，我們要採用座標的形式來描述一個點：即可以用一個座標向量來表示一個點，代表點在參考座標系中的位移。
那麼我們如何描述一個物體的位置或者姿態呢？
這麼問不是很具體，我們可以換一種問法，即，我們如何表示物體上的一個點呢？
這要取決於我們的研究問題，我們通常認為物體是剛性的，構成它的點相對於物體座標系要保持固定的相對位置，所以很自然的，我們可以對物體建立物體座標系，用物體座標系的位置和方向來描述物體的位置和方向。就比如這裡的{B}，就是物體座標系，而{A}是參考座標系。
00-2 位姿
可以看到，在B座標系下研究B物體的點更為方便，而B和A之間存在一個向量P的關係，這個向量P就代表了B的位姿。
位姿：物體在座標系中的位置和方向。圖形上表示為一組座標軸，如座標系{B}。
我們如果考慮點在這兩個座標系視角的轉換，就可以從座標系的變換入手分析：
對{A}施加一個變換，旋轉+平移，就能夠將A變換為B，
那麼對於座標系B描述的一個點P的座標，該怎麼用座標系A來描述？(由於難以手打，所以拍照了)
公式的意思是：P在A下的座標==B相對於A的位姿 · P在B下的座標。
00-3 相對位姿
相對於一個參考座標系A的某個座標系B的相對位姿用上面的ξ（ksi）表示，描述了座標系{A}經過平移和旋轉轉化為{B}的動作。若沒有上標A，表示相對於世界座標系O。
相對位姿的重要特點是可以複合/組合，並通過運算子進行代數計算。
點座標的換算關係上面已經提到過(這裡再提是因為會打這個公式)了：
下面是位姿的合成關係
所以綜合兩者，p點從C向A座標系轉換如下：
ξ到底是什麼呢？
那麼在後面的0102部分，我們將介紹這些 ξ 的具體表達。
00-4 抽象一些的代數規則
這裡總結一些ξ的代數規則：
-
基本規律
\[\xi{}\oplus{}0=\xi\\ \xi{}\ominus{}0=\xi\\ \xi{}\ominus{}\xi=0\\ \ominus{}\xi{}\oplus{}\xi=0 \] -
逆運算的性質
\[\ominus{}^X\xi_Y = {}^Y\xi_X \] -
不可交換性：
\[\xi_1{}\oplus{}\xi_2\neq{}\xi_2{}\oplus{}\xi_1 \]這一點在後面也有體現，即旋轉的不可交換性。
00-5 小總結
- 一個點可以用座標向量來表示，它代表該點在參考座標系中的位移；
- 一個剛體可以由其上的一組點代表，方便起見，我們常對物體建立物體座標系；
- 一個物體在座標系的位置和方向稱為其位姿；
- 相對位姿表示一個座標系相對另一個座標系的位姿，ξ；
- 一個點可以用不同座標系中的不同座標向量來描述，向量（座標）之間通過座標系的相對位姿進行轉換，運算子是 · ；
- 用相對位姿寫成的代數表示式是可以用線性代數等知識進行運算的。
01 二維空間位姿描述
01-1 概念
對於上圖中的兩個座標系，我們如何描述他們之間的關係、或是如何形容兩者之間的轉換呢？
座標系{A}和{B}的相對位姿可等價表示為兩個座標系間的平移和旋轉的向量疊加。
所以我們可以分別從旋轉和平移來討論其數學表達，最後再疊加起來。
對於上圖2.6這個例子，A和B的變換，我們可以先使得B旋轉為與A平行的V，再平移。
01-1-1 旋轉
就是旋轉矩陣：B座標系變換到V，順時針旋轉θ：
變換公式為：
點在V下的座標==BtoV的旋轉矩陣×點在B下的座標.
其實很好推導。不記得了可以再推導一遍。
因為這個旋轉矩陣行列式為1，說明向量在變換前後長度不變。
此外該矩陣標準正交（每列都是單位向量且相互正交），而正交矩陣有一個特殊的性質：
\[R^{-1} = R^T \]據此我們可以將上面的公式整理為：
\[\left[\begin{matrix} ^Bx \\ ^By \end{matrix}\right] = {}(^VR_B)^{-1} \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] = (^VR_B)^T \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] = (^BR_V) \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] \]此外由於旋轉矩陣的特殊性質，我們還可以得出：
\[R(-\theta) = R(\theta)^T \]
01-1-2 平移
平移相對簡單，即進行簡單的向量加法即可實現。
下面公式即是對V進行平移(x,y)的動作後得到A的動作公式：
注意這裡的(x,y)是易錯點！大家一定要理解這裡有什麼不同，意思很簡單，但容易在形式上犯迷糊！
令t = (x, y)，這個平移量 t 指的是從A原點到V原點的平移量，因為座標表示正好和原點平移反了一下。
附一個例子：
01-1-3 疊加
疊加只需要我們將上面的Vx、Vy替換為Bx、By即可。
我們可以把最終結果簡記為：
到這裡可以很容易看出在這個具體情況下00部分提出的相對位姿ξ是誰。
p～代表座標向量齊次形式，這在CV領域應用廣泛。
\[\tilde{P} = \lambda\tilde{P} \]
01-2 matlab實現
01-2-1 座標系變換、複合以及不可交換性
% 先用SE2建立一個齊次變換，書上的se2已經更新了
% 9.10版本工具箱中用se2函式，10.4版本為SE2函式
T1 = SE2(1,2,30*pi/180)
% 引數分別是X、Y平移距離、THETA是旋轉角度(弧度制)，如果最後註明'deg'則為角度制
% 輸出為：
T1 =
0.8660 -0.5000 1
0.5000 0.8660 2
0 0 1
% 建立一個5×5的座標格子，我們在這個圖裡繪製一下座標系
axis([0 5 0 5]);
% 我們指定這個座標系標籤為1，顏色為藍色
trplot2(T1, 'frame', '1', 'color', 'b')
% 下面我們用例子來展示一下複合運算的不可交換性
%%建立T2變換及其座標系
T2 = SE2(2,1,0);
% 輸出為：
T2 =
1 0 2
0 1 1
0 0 1
trplot2(T2,'frame','2','color','r')
% 看一看T1T2複合的效果
T3 = T1*T2;
% 輸出為：
T3 =
0.8660 -0.5000 2.232
0.5000 0.8660 3.866
0 0 1
trplot2(T3,'frame','3', 'color','g');
% 看一看T2T1複合效果：
T4 = T2*T1;
% 輸出為：
T4 =
0.8660 -0.5000 3
0.5000 0.8660 3
0 0 1
trplot2(T4, 'frame', '4', 'color', 'c');
總的輸出影像為：
可以看到4和3是不同的。
01-2-2 座標變換
P = [3 ; 2]
plot_point(P, '*')
P1 = double(inv(T1)) * [P; 1]
% 相對於{1}座標系的座標;
% 根據上面概念中的公式推導而來(取一個逆即可)
% 輸出為：
P1 = 3×1
1.7321
-1.0000
1.0000
e2h(P)
% 輸出為：
ans = 3×1
3
2
1
h2e( double(inv(T1)) * e2h(P) )
% 輸出為：
ans = 2×1
1.7321
-1.0000
%下面寫法與上面等效
homtrans( double(inv(T1)), P)
% 輸出為：
ans = 2×1
1.7321
-1.0000
% 相同的點對於座標系{2}有：
P2 = homtrans( double(inv(T2)), P)
% 輸出為：
P2 = 2×1
1
1
% 此處書中程式碼落後版本，必須加上double，否則不對應
-
inv() 矩陣求逆
-
e2h()將歐幾里得座標點轉換為齊次形式
畢竟歐幾里得是Euclid
-
h2e()將齊次形式轉換為歐幾里得座標點
02 三維空間位姿描述
02-1 姿態描述概念及方法
三維情況是二維的延伸和升級，三維旋轉複雜得多。
不得不提一下尤拉旋轉公式：
任何兩個獨立的正交座標系都可以通過一系列(不超過3次)相對於座標軸的旋轉得到，其中連續兩次的旋轉不得繞同一軸線。
此外，旋轉順序也決定旋轉結果，這一點可以從00部分的位姿代數ξ不可交換性可以看出。
02-1-1 正交旋轉矩陣
概念
我們考慮使用與二維空間類似的ξ來描述三維旋轉：
事實上三維的標準正交旋轉矩陣為：與二維有些類似
matlab實現
% Rx(θ)-> rotx()
R = rotx(90)%注意是弧度制
trplot(R1)
%tranimate(R1)%旋轉的動畫展示
%輸出
R1 = 3×3
1 0 0
0 0 -1
0 1 0
%效果是繞x軸逆時針(以下圖視角)旋轉了90°
%rotx()此MATLAB函式建立一個3×3矩陣，用於旋轉3×1向量或x軸周圍向量的3×N矩陣，單位為ang度。
%⭐⭐一個值得注意的地方：
%動畫展示必須要是tranimate(R1)
q = Quaternion(A)
%旋轉座標系繪製
>>q.plot()
%錯誤繪製方式
>>plot(q)
%旋轉動畫表示
>>q.animate()
%錯誤動畫表示
>>tranimate(q)
02-1-2 三角度表示法
三角度表示法是一個統稱，其中還有很多小的描述方式，比如尤拉式(XYX \ XZX \ YXY \ YZY \ ZXZ \ ZYZ)、卡爾丹式(XYZ \ XZY \ YZX \ YXZ \ ZXY \ ZYX)等，但都滿足尤拉旋轉公式，這些序列統稱為尤拉角。
公式
以上面的ZYZ序列為例：
matlab實現
R = rotz(0.1/pi*180)*roty(0.2/pi*180)*rotz(0.3/pi*180);
%輸出：
R = 3×3
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
% 下面這種方式與上面等價，更為簡便
R1 = eul2r(0.1/pi*180, 0.2/pi*180, 0.3/pi*180);
% tr2eul的逆運算是eul2r,獲得給定旋轉矩陣的尤拉角
gama = tr2eul(R)
%輸出：
gama = 1×3
0.1000 0.2000 0.3000
gama1 = tr2eul(R1)
%輸出：
gama1 = 1×3
0.1000 0.2000 0.3000
可以看到上面我們的尤拉角選取都是正數，我們可以繼續探討一些特殊情形：
% θ為負時進行逆運算
R = eul2r(0.1/pi*180, -0.2/pi*180, 0.3/pi*180)
%輸出：
R = 3×3
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
%逆運算：
tr2eul(R)
ans = 1×3
-3.0416 0.2000 -2.8416
可見逆運算結果與正運算輸入的引數並不相同，但是前者對應的旋轉矩陣仍與後者相同。不同的尤拉角對應同一旋轉矩陣，說明：旋轉矩陣到尤拉角的對映不唯一。工具箱返回的θ始終為正。
R = eul2r(0.1/pi*180, 0, 0.3/pi*180)
% 輸出：
R = 3×3
0.9211 -0.3894 0
0.3894 0.9211 0
0 0 1.0000
tr2eul(R)
% 輸出：
ans = 1×3
0 0 0.4000
eul2r(ans)
這與原值完全不同，這是因為θ=0時，我們計算的ZYZ函式實際上成為了：
此時我們取逆只能得到兩角度的和，而無法具體得知各自為多少。有點像線性代數裡三維<->二維作變換時的不可逆情況。
另一種廣泛應用的是翻滾-俯仰-偏航角（卡爾丹角）：
也稱導航角；這種表示方法允許正負值出現，不會產生多解，但是也存在奇異點(θ=90°)。在工具箱中預設為xyz的順序。
X：前進方向
Y：右手方向
Z：垂直向下（看具體怎麼定義，不唯一）
R = rpy2r(0.1/pi*180,0.2/pi*180,0.3/pi*180)
gamma = tr2rpy(R)
%輸出
R = 3×3
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
gamma = 1×3
0.1000 0.2000 0.3000
02-1-3 奇異點及萬向節鎖
三角度表示法存在先天不足，因為它存在難以反變換的奇異點。當任意兩連續軸共線的時候就會發生。
對於 ZYZ 形式的尤拉角，它發生在 θ=kπ,k∈Z 時，對於用橫滾-俯仰-偏航角的情況，會發生在θ=±(2k+1)π/2時。雖然都存在奇異點，但我們可以想辦法讓奇異點不在航行體正常執行時出現。
我感覺這個知識點有一點難，這個可以等到回頭我再積澱積澱再補充。
02-1-4 雙向量表示法
對於機械臂末端的末端執行器，我們一般會單獨固聯一個座標系{E}；末端軸線為座標系的 z 軸，並被稱為接近向量，記為
根據右手定則，我們至少需要兩個確定且正交的向量才能完全確定全部三個單位向量。
所以我們引入姿態向量，它位於末端執行器的兩個手指之間，與a正交/垂直。
所以兩向量法定義的旋轉矩陣如下：
當然根據線性代數的知識可知，任意兩非平行向量就可以定義一個座標系。只不過視我們的需求，我們通常需要將向量調整稱為互相垂直的形態。
matlab實現
a = [1 0 0]';%接近向量
o = [0 1 0]';%姿態向量
oa2r(o, a)
% 輸出：
ans = 3×3
0 0 1
0 1 0
-1 0 0
02-1-5 繞特定向量旋轉
對於空間中兩個任意姿態的座標系，總可以在空間中找到某個軸，使其中一個座標繞該軸旋轉一個角度就能與另一個座標系姿態重合。
原文這裡翻譯為任意向量，我覺得特定向量更為合適。
% 拿以前導航角的例子說明：
R = rpy2r(0.1,0.2,0.3)
% 輸出：
R = 3×3
1.0000 -0.0052 0.0035
0.0052 1.0000 -0.0017
-0.0035 0.0017 1.0000
% 這個變換確定的旋轉角度θ和圍繞的特定向量如下
[theta,v] = tr2angvec(R)
%輸出：
theta = 0.0065
v = 1×3
0.2660 0.5354 0.8016
% eig()可以求特徵向量和特徵值
[v,lamda] = eig(R)
% 輸出：
v = 3×3 complex
-0.6816 + 0.0000i -0.6816 + 0.0000i 0.2660 + 0.0000i
0.1045 + 0.5880i 0.1045 - 0.5880i 0.5354 + 0.0000i
0.1564 - 0.3927i 0.1564 + 0.3927i 0.8016 + 0.0000i
lamda = 3×3 complex
1.0000 + 0.0065i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 1.0000 - 0.0065i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i
怎麼通過eig()求出的特徵向量、特徵值確定我們要找的特定向量呢？
我們回憶一下正交旋轉矩陣的性質，即一定有一個特徵值是實數1，還有一對共軛復特徵值：
特徵值特徵向量的定義為：
當λ==1時，對應的特徵向量時不隨旋轉而改變的，這就是我們要找的圍繞向量軸。如上面的例子，應該是第三列對應的向量。
補充一點：
如果有需求根據這個圍繞向量和旋轉角度計算旋轉矩陣。理論根據是羅德里格斯公式。
R = angvec2r(pi/2, [1,0,0]) % 輸出： R = 3×3 1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000[公式](羅德里格斯公式推導 - Jing_Rui - 部落格園 (cnblogs.com))如下：S(v)為反對稱矩陣
另一種描述是：K為v的單位向量
R=I + (1 - cosΘ)K2 + sinΘ K
最後我們再探討一點，即這種方法雖然引入了四個引數(三引數的向量軸和單引數的角度)，但實際上可以轉化為3個引數：因為單位特徵向量可以通過開根號確定第三個值。
02-1-6 單位四元數描述
四元數表示法是對前面三角度表示法的優化，因其格式優雅、功能強大、計算簡單而被廣泛應用於機器人、計算機視覺、計算機圖形學以及航空航天慣性導航領域。
四元數的基本性質：
單位四元數可以描述座標系的旋轉，與02-1-5有一些類似，座標系繞某單位向量n旋轉θ角，用單位四元數可以表示為：
matlab實現：
-
基本實現：
% 四元數在matlab中通過Quaternion類來實現 q=UnitQuaternion(rpy2tr(0.1 ,0.2,0.3)) %版本原因，需要將Quaternion改為UnitQuaternion，否則會報錯 %輸出為： q = 0.99999 < 0.00086809, 0.0017476, 0.0026165 > % 為了描述座標系的旋轉，我們使用單位四元數進行描述： q.norm %輸出為： ans = 1 %這說明q是一個單位四元數 -
過載一些標準方法、函式：
四元數之間的乘積問題，可以表示為一個矩陣-向量積，如下：
據書可知，這種方法的計算複雜度相對較低。
q = q * q;
%輸出為：
q =
0.99998 < 0.0017362, 0.0034952, 0.0052329 >
-
關於取逆
q.inv() %輸出為： ans = 0.99998 < -0.0017362, -0.0034952, -0.0052329 > % 可以嘗試一下q乘以它的逆： q*q.inv()%或者q/q %輸出為： q = 1 < 0, 0, 0 > %這個單位四元數表示無效旋轉 -
向正交旋轉矩陣轉換
q.R %輸出為： ans = 3×3 1 0 0 0 1 0 0 0 1 -
繪製四元數所指的方向：
q.plot()
可見我們這個旋轉確實無效，呈現的還是原來的座標系。
-
有一個錯誤或者說困惑：
書上說可以將一個三元向量傳入四元數的建構函式中，產生一個單位四元數，現在應該是被刪除了吧。這裡留有疑惑⭐。
p = UnitQuaternion([1 2 3]) %報錯 %而輸入 p = UnitQuaternion([1 2 3 5]) %則輸出 p = 0.16013 < 0.32026, 0.48038, 0.80064 > -
使用過載的乘法，四元數可用於旋轉一個三元向量
p*[1 0 0]' %注意有轉置符號’ %輸出為： ans = 3×1 -0.7436 0.5641 0.3590
02-2 平移與旋轉組合
前面描述的最實用的姿態描述方法是四元數和齊次變換矩陣。
下面我們進行旋轉和平移的疊加：
02-2-1 四元數+平移
P在A下的座標==表示AtoB旋轉的單位四元數·P在B下座標+平移向量。
02-2-2 齊次矩陣+平移
這個與二維類似，直接寫公式。
02-2-3 matlab實現
T = transl(1, 0, 0) * trotx(90) *transl(0, 1, 0)
% transl實現平移動作
% 輸出：
T = 4×4
1 0 0 1
0 0 -1 0
0 1 0 1
0 0 0 1
% 繪製相應座標系
trplot(T)
% 要提取矩陣T的旋轉矩陣，可用t2r()函式
t2r(T)
%要提取平移向量，可用transl()函式
transl(T)
03 簡單總結 | Review
通過本部分的學習，我們需要總結出幾個巨集觀的印象，以便於後續的深入學習：
-
我們認識了很多描述位姿的數學物件，各有優劣，我們需要視情況選擇不同的描述方式。
後續主要使用齊次變換的方式。
-
座標系常見且方便。
-
機器人工具箱中位姿ξ的具體描述方法(表2.1)以及旋轉描述法的轉換(圖2.15)：備忘：
