機器人學導論 課件 第二章-2.1節-位姿描述與變換

第2章機器人運動學2.1節位姿描述與齊次變換第1-15周，星期二，16:40-18:15，（五）103機器人技術基礎22.1.1位姿描述2.1.2坐標變換2.1.3其他姿態描述三角度姿態法等效軸-角法四元數

法本節目錄3大寫斜體加粗：矩陣R、T小寫斜體加粗：矢量p、默認矢量為列向量；小寫斜體不加粗：標量a，p左上標：變量所在坐標系

符號約定4

符號約定5如何描述機器人某構件，例如末端手爪的空間狀態建立一個世界坐標系{A}；在末端手爪某處，例如兩手指尖端中點建立一個坐標系{B}，其原點為P；坐標系{B}與手爪固聯，隨手爪運動；坐標系{B}相對于坐標系{A}的描述，就唯一確定了手爪的空間狀態——位姿P位姿描述6空間中某點P在坐標系{A}中的描述

位置描述7

結論：矢量與某坐標系各坐標軸單位向量的點積，就得到矢量在該坐標系中的表達姿態描述8

因為上述向量均為單位向量，所以：

姿態描述92025/6/1

姿態描述10將坐標系{B}的各坐標軸在{A}中的表達組成一個矩陣：矩陣中各元素均是{A}、{B}兩個坐標系各坐標軸之間夾角的余弦又稱為方向余弦矩陣（directioncosinematrix）。姿態描述11

姿態描述12將坐標系{A}的各坐標軸在{B}中的表達組成一個矩陣：{A}系的3個坐標軸相對{B}系的坐標就是其在{B}系三個坐標軸上的投影。{A}、{B}兩坐標系各軸夾角的大小與坐標軸向量在哪個坐標系表達無關姿態描述13結論：姿態描述14

在工業機器人領域，為了形象地描述機器人（俗稱機械臂、操作臂等，manipulator）的姿態，姿態矩陣一般寫成如下形式：a為接近矢量（approachvector），表示手爪接近物體的方向o為方位矢量（orientationvector），表示手爪中的一個手指指向另一個手指的方向n為法向矢量（normalvector）姿態描述15位置和姿態合稱位姿圖中代表手爪位姿的坐標系{B}，可表示為：其中：表示姿態表示位置工業機器人的位姿描述姿態描述16位姿圖的說明矢量箭頭從一個坐標原點指向另一個坐標系原點矢量指明它表示的是箭頭處坐標系相對于箭尾坐標系的相對關系例如：{B}相對于{A}機器人學中，機器人末端的位姿（矩陣）通常也稱作機器人的位形（configuration）。姿態描述172.1.1位姿描述2.1.2坐標變換2.1.3其他姿態描述三角度姿態法等效軸-角法四元數

法本節目錄18坐標變換把一個矢量在{B}坐標系中的表達轉換到{A}坐標系中；矢量本身沒有變化，但是在不同坐標系中的值不同；坐標變換的本質所在，即描述的是坐標系之間的變換而不是對象本身。坐標變換（Transformation,映射Mapping）坐標變換19平移變換坐標系{B}相對于{A}僅有平移：已知矢量P在{B}中的表達：則矢量P在{A}中的表達：只有{A}、{B}姿態相同時，上式才成立20旋轉變換坐標系{B}相對于{A}僅有旋轉：由姿態矩陣的定義和性質：可知：坐標變換21旋轉變換已知矢量p在{B}中的表達：待求解矢量p在{A}中的表達：也即，點P在{A}坐標系各軸上的投影可利用{A}的各坐標軸在{B}中的表達與的點積來計算，即：只有在同一個坐標系中表達的兩個矢量才能執行運算。點積結果是標量，與該矢量在哪個坐標系表達無關！BpApBp坐標變換22旋轉變換由于{B}相對于{A}的旋轉矩陣所以：訣竅：坐標變換23旋轉變換——實例解：可得：

注意：繞某一軸旋轉，規定按照右手定則，逆時針為正坐標變換24旋轉變換——實例解：

又已知點P在{B}系中的表達：求：注意：映射變換不改變向量本身，只是在不同坐標系描述向量，或者說求向量在不同坐標系中的坐標坐標變換25繞各坐標軸的旋轉矩陣

繞z軸有夾角θ：繞x軸有夾角θ：繞y軸有夾角θ：

坐標變換坐標變換的一般情況26坐標變換的一般情況

解：首先建立一個中間坐標系{C}，它與{A}姿態相同，與{B}原點重合

顯然：于是：因為{C}與{A}僅存在平移關系，所以：

齊次變換矩陣27一般坐標變換的表達一般情況下的坐標變換，可由下式計算：為了使表達更簡潔，引入齊次變換矩陣：原3×1坐標向量增加一行，變成4×1的齊次坐標向量齊次變換矩陣的性質28齊次變換矩陣的性質

齊次變換：舉例29一般坐標變換——實例

解：{B}相對于{A}的齊次變換矩陣為：其中：最后，可得：

30

逆變換解：其中旋轉矩陣部分，根據單位正交矩陣的性質直接寫出：利用一般變換的映射公式：顯然：由此，可得：

逆變換31解：于是最后，可得

逆變換：舉例32

復合變換：連續旋轉解：

如何求{C}系相對于{A}系的姿態？

則作為將{B}系映射到{A}系中的旋轉矩陣，這樣可將參考坐標系從{B}變到{A}，結果可變成了{C}系（相對于{A}系）的姿態。連續旋轉可通過矩陣相乘得到，即滿足旋轉矩陣的合成法則復合變換：連續旋轉33

復合變換：連續齊次變換解：

3.聯立上述兩式，得4.由此，可得注意：展開得變換方程（1）34

利用齊次變換的遞推特性，求不直接關聯兩坐標系的關系，或未知變換。根據第1個變換路徑，可得：從第2個變換路徑，也可得：前面兩式可構造一個變換方程：據此，可求得：變換方程（2）352025/6/1如右圖，注意{D}鄰近的兩坐標系與{D}的相對關系與前例相反。利用齊次變換的遞推特性，求不直接關聯兩坐標系的關系，或未知變換。

從第2個變換路徑，也可得：根據前面兩式，可求解鏈路中的其他變換，例如：變換方程（3）36

變換方程的實際用途根據右圖中的變換路徑，可得：變換方程（4）37

變換方程的實際用途根據右圖中的變換路徑，可得：式中，，為用戶給定的變換，由機器人正運動學模型得到

38【例】如下圖所示，一輪式移動機器人上搭載機械手在房間內進行拾取木塊的作業，天花板上安放一攝像頭用作機器人的視覺反饋系統。各坐標系如圖所示，其中，{W}為參考坐標系，{B}和{T}分別為附著在輪式移動機器人和機械手末端上的物體坐標系，{C}為攝像頭坐標系，{S}為附著在木塊上的物體坐標系。通過視覺傳感器測量得到通過關節角度測量裝置標定得到預先已知求：木塊相對機械手的位形變換方程（5）39自由矢量與線矢量的變換物理效果與作用點無關的矢量——自由矢量（freevector）線速度、力（偶）矩等物理效果與作用點有關的矢量——線矢量（linevector）角速度、力等兩坐標系間的自由矢量變換，僅涉及到旋轉線速度力（偶）矩兩坐標系間的線矢量變換，需要考慮坐標系原點偏移的影響本章目錄402025/6/12.1.1位姿描述2.1.2映射與算子2.1.3其他姿態描述三角度姿態法等效軸-角法四元數

法三角度姿態法41R有9個元素，是否一定需要9個變量才能唯一確定旋轉矩陣？再次考察旋轉變換矩陣R由于R是單位正交矩陣，所以存在6個約束條件：3個列向量是單位向量：3個正交條件：因此，R中只有3個獨立變量，也即用3個參數即可表示姿態。三角度姿態法42線性代數中的凱萊公式指出，對于任何一個正交陣R存在一個反對稱矩陣，滿足：再次考察旋轉變換矩陣R其中：這再次說明，可用3個參數表示姿態。

三角度姿態法43旋轉變換一般不滿足交換律，也即：再次考察旋轉變換矩陣R

注意：旋轉不滿足交換律，那么必須要使用三個有順序的參數才能準確描述姿態采用3個獨立的姿態角來描述3個姿態角的任意組合有33=27種形式，但是為了保持3個姿態角的獨立性，需要保證兩個連續旋轉軸的軸線不能平行，因此3姿態角存在12中形式。3*2*2=12X-Y-Z、X-Z-Y、Y-X-Z、Y-Z-X、Z-X-Y、Z-Y-X、Z-Y-Z、Z-X-Z、Y-Z-Y、Y-X-Y、X-Y-X、X-Z-XRPY（繞3個定軸的旋轉）歐拉角（繞3個動軸的旋轉）三角度姿態法4412×2=24種三角度姿態法45X-Y-Z固定角

繞固定坐標系三個軸的三次轉動，得到的三個轉角（

,

,

）稱為X-Y-Z固定角。在描述運動物體時，例如：飛機，它們又被稱為橫滾角（Roll）、俯仰角（Pitch）和偏航角（Yaw）——R-P-Y角

ZYaw三角度姿態法46X-Y-Z固定角

三角度姿態法47X-Y-Z固定角復合變換：計算，得：注意：繞固定坐標軸的連續變換，按變換順序“左乘”得到最終變換矩陣三角度姿態法48X-Y-Z固定角已知旋轉矩陣R，求對應的X-Y-Z固定角（

,

,

）在實現機器人連續運動控制的姿態插補時，經常需要根據已知旋轉矩陣求解姿態角已知：根據：三角度姿態法492025/6/1X-Y-Z固定角Atan2(y,x)—“四象限反正切函數”，內置于大多數編程語言，可根據x、y的符號給出不同的角度值，例如：Atan2(-2.0,-2.0)=-135°Atan2(2.0,2.0)=45°三角度姿態法50關于

角的說明計算中，通常取：－90°≤

≤90°若

＝±90°，則cos

＝0，此時，

和

的值無法計算規定：或51歐拉角是瑞士數學家歐拉（Euler，1707-1783）提出的一種采用繞動坐標系3個坐標軸的轉角組合描述剛體姿態的方法。A.Z-Y-X歐拉角B.Z-Y-Z（Z-X-Z）歐拉角A.Z-Y-X歐拉角將{B}繞其z軸旋轉角度

繞{B}的新y軸旋轉角度

繞{B}的新x軸旋轉角度

B.Z-Y-Z歐拉角將{B}繞其z軸旋轉角度

繞{B}的新y軸旋轉角度

繞{B}的新z軸旋轉角度

進動角章動角自旋角三角度姿態法三角度姿態法52Z-Y-X歐拉角坐標系{B}相對{A}的姿態的另一種表示法，是假想相對運動坐標系軸連續轉動，并利用旋轉映射得出旋轉矩陣。首先，假設初始{B}與{A}重合。

將{B}繞其z軸旋轉角度

繞{B}的新y軸旋轉角度

繞{B}的新x軸旋轉角度

三角度姿態法53Z-Y-X歐拉角

也即：注意：繞運動坐標軸的連續變換，按變換順序“右乘”得到最終變換矩陣三角度姿態法54Z-Y-X歐拉角Z-Y-X歐拉角定義的位姿矩陣為：上述結果與繞固定軸X-Y-Z旋轉得到的位姿矩陣相等！三角度姿態法55X-Y-Z固定角與Z-Y-X歐拉角Z-Y-X歐拉角X-Y-Z固定角結論：坐標系{B}相對于坐標系{A}的姿態可以假想繞三個坐標軸依次旋轉得到繞固定坐標系三個軸的連續旋轉與繞運動軸以相反順序旋轉的結果相同沿固定坐標系的固定角連續變換，按旋轉順序連續“左乘”沿運動坐標系的歐拉角連續變換，按旋轉順序連續“右乘”三角度姿態法56常用的Z-Y-Z歐拉角變換與機器人末端工具姿態描述常采用Z-Y-Z歐拉角描述，這樣可與腕部三個垂直正交旋轉關節的轉角直接對應用Z-Y-Z歐拉角描述的旋轉矩陣：若已知旋轉矩陣：則，三個歐拉角為：顯然，此時

角不能等于0°或180°。若出現此情況，則取

＝0°三角度姿態法57三角度位姿描述中的奇異點（SingularPoint）問題無論采用歐拉角還是固定角表示位姿，當中間軸轉角等于±90°或0°、180°時，總會出現無法求解的情況例如：Z-Y-Z歐拉角，

＝0°或180°Z-Y-X歐拉角，

＝±90°奇異發生在第一次轉動軸線與最后一次轉動軸線共線的位置第一次和最后一次旋轉的轉軸重合（即中間軸的轉角

=±90°）時，導致繞第一、第三軸的轉角無法計算，歐拉角描述的姿態發生奇異；對應的位形或位姿，稱為奇異位形（singularconfiguration）58三角度位姿描述中的奇異點問題從實際物理意義上來說，此種情況意味著第1、3軸重合，導致繞第1、3軸的轉角無法計算，稱為奇異現象對應的位姿點（由姿態角元素構成的點），稱為奇異點Z-Y-X歐拉角，

＝±90°三角度姿態法59位姿奇異現象的具體案例飛行器中的萬向節死鎖（Gimballock）問題陀螺儀：X軸控制偏航（右圖中的藍色圖示），Y軸控制俯仰（右圖中的紅色圖示），Z軸控制橫滾（右圖中的綠色圖示）。俯仰角

=±

/2時發生奇異發生萬向節鎖死時，俯仰角和航偏角沒影響，橫滾角度受影響。工程上一般在發生奇異時，人為設定橫滾角

=0。/hanjuefu5827/article/details/80659343?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task三角度姿態法60位姿奇異現象的具體案例機槍轉塔跟蹤過頂飛機目標飛機過頂飛行；轉塔方位跟蹤速度趨向于無窮大。當接近方位角為90

的位置，它的工作性能越來越不理想。為跟蹤飛過飛機頭頂的目標，槍手需要操控機槍以非常快的速度繞方位角轉動。如果目標直接飛過槍手頭頂，對方位角的跟蹤速度趨向于無窮大。三角度姿態法等效軸-角法612025/6/1能否用一次旋轉變換描述{B}相對于{A}的姿態？歐拉旋轉定理：在三維空間里，剛體的任意旋轉等價于一個繞著某固定軸的旋轉（簡化描述）假設{B}與{A}初始狀態重合，將{B}繞過原點的任意單位向量

按右手定則旋轉θ角，可到達{B}的實際姿態此旋轉記為：右手定則等效軸-角法62

等效軸-角法63等效軸-角旋轉矩陣表達式的推導

64等效軸角旋轉矩陣表達式的推導于是，根據旋轉變換方程可得：

由旋轉矩陣的正交性，可得：于是：根據假設：等效軸-角法652025/6/1等效軸-角旋轉矩陣表達式的推導可得將上式右端相乘，并利用可得等效軸-角法662025/6/1根據旋轉矩陣求解等效軸-角若已知旋轉矩陣R：可求解等效軸-角：注意：θ不能等于0°或180°，否則出現奇異現象，無法確定轉軸等效軸-角法反對稱矩陣67羅德里格斯（Rodrigues）公式等效軸-角法歐拉定理：任一旋轉矩陣R總可以等效為繞某一固定軸的旋轉運動。68羅德里格斯（Rodrigues）公式等效軸-角法四元數的定義692025/6/1

以超復數表達的四元數以四元向量表達的四元數威廉·若宛·哈密頓（WilliamRowanHamilton1805-1865）愛爾蘭數學家，他提出了四元數。四元數虛數單