開始學習《Robotics，Vision and Control》，今天是學習的第一篇章，因爲項目需要，
需要在Matlab上做一個仿真實驗，由於是串聯機器人系統，不涉及到移動機器人系統，因此
本系列文章主要是學習和介紹書中相關章節。
位姿的表示
主要講述了怎麼去描述2D點和3D點的位姿，重點是相對位姿的表達。
1）一個點可以用一個座標向量來描述，此座標向量表示了此點相對於參考座標系的位移
2）由一系列點組成的剛體可以由一個單獨的座標系來描述，並且它的成員點可以用相對於此座標系
的位移來描述
3）一個目標座標系的位置和姿態稱爲它的位姿
4）一個座標系相對於另一個座標系的相對位姿可以用ξ來表示
5）一個點相對於一個座標系的座標向量可以通過點乘一個相對位姿轉換到另一個座標系下
在2D空間表達位姿
在2D空間中一般使用平移量和一個旋轉量來表示點的位姿，R∼(x,y,θ)，如下圖所示：
將{B}中的點P在{A}中表示：
其中相對位姿的表達：
在Matlab中變換矩陣的計算：
其中1，2分別表示平移量x,y。30*pi/180表示的是旋轉量，表示旋轉了30°。
我們可以在matlab中把這種旋轉變換繪製出來，有兩種方式，一種是靜態的，只顯示變換之後
的結果，用trplot2;另一種是動態的顯示，它可以動態地顯示出變換的過程，用tranimate函數（只適用於三維）：
其中T1表示要繪製的變換矩陣，注意這裏的變換都是相對於世界座標系的變換後的結果，可選項
‘frame’，‘1’，表示此結果座標系我們用{1}來標識它，後面的該座標系的顏色。
注意：座標變換的先後順序是不能更換的，不同的變換順序會得到完全不一樣的結果
Matlab中點的繪製方式：
此外我們還需要注意幾個Matlab函數，因爲一般的座標都做過齊次處理：
有時候我們僅僅只需要前兩個座標值，而不需要齊次項，這時候我們可以用h2e函數和homtrans函數
描述三維位姿
三維座標系是二維座標系的擴展，多了一個z來表達，三維空間中的點自然就用(x,y,z)來表示，也可以表示成
向量t(x,y,z)，如圖所示：
在三維空間中也從兩個方面來考慮：平移和旋轉。
1、三維空間中的姿態
三維空間中的變換順序和二維的一樣是不能交換順序的，變換順序將影響最終結果。數學上有幾種方式來描述
旋轉：正交旋轉矩陣、歐拉角、旋轉軸和單位四元素。
正交旋轉矩陣
關於x,y,z軸旋轉一個角度的正交旋轉矩陣分別爲：
![]()
在Matlab中很方便地可以利用一些函數計算旋轉矩陣：rotx(a)、roty(a)、rotz(a)
同樣地，我們可以用trplot(R)和tranimate（R）來靜態地或動態地顯示變換結果。
**從以上可以看出，正交矩陣有9個元素，但它們不是獨立的，每個列向量的模是1和三個
列向量相互正交各有3個約束，因此正交矩陣只有3個獨立的變量**
歐拉角
根據旋轉的順序不同歐拉角有兩種：一種是有繞重複軸旋轉的歐拉角，如XYX,XZX等6種，
另一種是每次旋轉都繞不同軸的卡爾達納，如XYZ,XZY等6種。
每一種形式都應用於特定的領域，在航空和機械領域用得比較多的就是ZYZ形式，在Matlab機器人工具箱中也是用的這種形式的歐拉角
在matlab中可以用相應的函數很方便地計算：
上述兩種方式完全等價
有時候我們知道變換矩陣了，我們需要知道對應這個矩陣的歐拉角，可以用tr2eul()函數
注意歐拉角中的Y方向旋轉的角度不能是0或負值，或kpi,會奇異
另一種被廣泛使用的歐拉角是XYZ（roll-pitch-yaw）
在matlab中相應的函數是：rpy2r(x,y,z)以及tr2rpy(R)
這種XYZ形式的歐拉角在Y旋轉角度爲（2k+1）pi/2的情況下會奇異
旋轉軸
兩個特定姿態的座標系是通過空間中某個軸的旋轉而聯繫在一起的。
R就是世界座標系繞着軸V旋轉theta角得到的。其中theta和V分別是R的特徵值和對應的特徵向量。
在matlab中可以利用eig函數來求一個矩陣的特徵值和對應的特徵向量的：
其中V的列向量就是lambda中對應的列特徵值所對應的特徵向量，對於每一個正交旋轉矩陣來說，總是有一個特
徵值爲1，其所對應的特徵向量就是對應的旋轉軸
在Matlab中可以用angvec2r()函數來得到旋轉軸對應的變換矩陣
單位四元素
四元素一直以來都備受爭議，但它卻在機器人中得到了很廣泛地應用。四元素是複數的擴展，是一個標量加一個矢量組成：
Matlab中可以直接用Quaternion()來將矩陣轉換爲四元素：
所有和四元素有關的操作都在Quaternion這個類中，其有很多的成員函數，如q.R將四元素轉換爲矩陣形式，
q.plot將其繪製出來。
將平移和旋轉組合起來
前面討論的都是隻考慮旋轉，而沒有考慮平移，接下來將結合兩者一起來討論。
有兩種非常實用的表示方式：四元素和4x4的齊次變換矩陣
在Matlab中，用transl()和trotx()等函數來進行計算：
其中transl只有旋轉，trotx只有旋轉，相應的troty(),和trotz()
我們可以使用t2r(T)將T中旋轉部分提取出來，使用transl(T)'將平移部分提取出來：
此次只是一些基礎知識，二維和三維中位姿的描述方式，但卻是後面一切學習的根基。
