機(jī)器人學(xué)導(dǎo)論 課件 第二章-2.1節(jié)-位姿描述與變換

第2章機(jī)器人運(yùn)動學(xué)2.1節(jié)位姿描述與齊次變換第1-15周，星期二，16:40-18:15，（五）103機(jī)器人技術(shù)基礎(chǔ)22.1.1位姿描述2.1.2坐標(biāo)變換2.1.3其他姿態(tài)描述三角度姿態(tài)法等效軸-角法四元數(shù)

法本節(jié)目錄3大寫斜體加粗：矩陣R、T小寫斜體加粗：矢量p、默認(rèn)矢量為列向量；小寫斜體不加粗：標(biāo)量a，p左上標(biāo)：變量所在坐標(biāo)系

符號約定4

符號約定5如何描述機(jī)器人某構(gòu)件，例如末端手爪的空間狀態(tài)建立一個世界坐標(biāo)系{A}；在末端手爪某處，例如兩手指尖端中點(diǎn)建立一個坐標(biāo)系{B}，其原點(diǎn)為P；坐標(biāo)系{B}與手爪固聯(lián)，隨手爪運(yùn)動；坐標(biāo)系{B}相對于坐標(biāo)系{A}的描述，就唯一確定了手爪的空間狀態(tài)——位姿P位姿描述6空間中某點(diǎn)P在坐標(biāo)系{A}中的描述

位置描述7

結(jié)論：矢量與某坐標(biāo)系各坐標(biāo)軸單位向量的點(diǎn)積，就得到矢量在該坐標(biāo)系中的表達(dá)姿態(tài)描述8

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

姿態(tài)描述92025/6/1

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

姿態(tài)描述12將坐標(biāo)系{A}的各坐標(biāo)軸在{B}中的表達(dá)組成一個矩陣：{A}系的3個坐標(biāo)軸相對{B}系的坐標(biāo)就是其在{B}系三個坐標(biāo)軸上的投影。{A}、{B}兩坐標(biāo)系各軸夾角的大小與坐標(biāo)軸向量在哪個坐標(biāo)系表達(dá)無關(guān)姿態(tài)描述13結(jié)論：姿態(tài)描述14

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

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

注意：繞某一軸旋轉(zhuǎn)，規(guī)定按照右手定則，逆時針為正坐標(biāo)變換24旋轉(zhuǎn)變換——實(shí)例解：

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

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

坐標(biāo)變換坐標(biāo)變換的一般情況26坐標(biāo)變換的一般情況

解：首先建立一個中間坐標(biāo)系{C}，它與{A}姿態(tài)相同，與{B}原點(diǎn)重合

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

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

齊次變換：舉例29一般坐標(biāo)變換——實(shí)例

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

30

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

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

逆變換：舉例32

復(fù)合變換：連續(xù)旋轉(zhuǎn)解：

如何求{C}系相對于{A}系的姿態(tài)？

則作為將{B}系映射到{A}系中的旋轉(zhuǎn)矩陣，這樣可將參考坐標(biāo)系從{B}變到{A}，結(jié)果可變成了{(lán)C}系（相對于{A}系）的姿態(tài)。連續(xù)旋轉(zhuǎn)可通過矩陣相乘得到，即滿足旋轉(zhuǎn)矩陣的合成法則復(fù)合變換：連續(xù)旋轉(zhuǎn)33

復(fù)合變換：連續(xù)齊次變換解：

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

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

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

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

變換方程的實(shí)際用途根據(jù)右圖中的變換路徑，可得：式中，，為用戶給定的變換，由機(jī)器人正運(yùn)動學(xué)模型得到

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

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

三角度姿態(tài)法43旋轉(zhuǎn)變換一般不滿足交換律，也即：再次考察旋轉(zhuǎn)變換矩陣R

注意：旋轉(zhuǎn)不滿足交換律，那么必須要使用三個有順序的參數(shù)才能準(zhǔn)確描述姿態(tài)采用3個獨(dú)立的姿態(tài)角來描述3個姿態(tài)角的任意組合有33=27種形式，但是為了保持3個姿態(tài)角的獨(dú)立性，需要保證兩個連續(xù)旋轉(zhuǎn)軸的軸線不能平行，因此3姿態(tài)角存在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個定軸的旋轉(zhuǎn)）歐拉角（繞3個動軸的旋轉(zhuǎn)）三角度姿態(tài)法4412×2=24種三角度姿態(tài)法45X-Y-Z固定角

繞固定坐標(biāo)系三個軸的三次轉(zhuǎn)動，得到的三個轉(zhuǎn)角（

,

,

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

ZYaw三角度姿態(tài)法46X-Y-Z固定角

三角度姿態(tài)法47X-Y-Z固定角復(fù)合變換：計算，得：注意：繞固定坐標(biāo)軸的連續(xù)變換，按變換順序“左乘”得到最終變換矩陣三角度姿態(tài)法48X-Y-Z固定角已知旋轉(zhuǎn)矩陣R，求對應(yīng)的X-Y-Z固定角（

,

,

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

角的說明計算中，通常?。海?0°≤

≤90°若

＝±90°，則cos

＝0，此時，

和

的值無法計算規(guī)定：或51歐拉角是瑞士數(shù)學(xué)家歐拉（Euler，1707-1783）提出的一種采用繞動坐標(biāo)系3個坐標(biāo)軸的轉(zhuǎn)角組合描述剛體姿態(tài)的方法。A.Z-Y-X歐拉角B.Z-Y-Z（Z-X-Z）歐拉角A.Z-Y-X歐拉角將{B}繞其z軸旋轉(zhuǎn)角度

繞{B}的新y軸旋轉(zhuǎn)角度

繞{B}的新x軸旋轉(zhuǎn)角度

B.Z-Y-Z歐拉角將{B}繞其z軸旋轉(zhuǎn)角度

繞{B}的新y軸旋轉(zhuǎn)角度

繞{B}的新z軸旋轉(zhuǎn)角度

進(jìn)動角章動角自旋角三角度姿態(tài)法三角度姿態(tài)法52Z-Y-X歐拉角坐標(biāo)系{B}相對{A}的姿態(tài)的另一種表示法，是假想相對運(yùn)動坐標(biāo)系軸連續(xù)轉(zhuǎn)動，并利用旋轉(zhuǎn)映射得出旋轉(zhuǎn)矩陣。首先，假設(shè)初始{B}與{A}重合。

將{B}繞其z軸旋轉(zhuǎn)角度

繞{B}的新y軸旋轉(zhuǎn)角度

繞{B}的新x軸旋轉(zhuǎn)角度

三角度姿態(tài)法53Z-Y-X歐拉角

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

角不能等于0°或180°。若出現(xiàn)此情況，則取

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

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

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

=±90°）時，導(dǎo)致繞第一、第三軸的轉(zhuǎn)角無法計算，歐拉角描述的姿態(tài)發(fā)生奇異；對應(yīng)的位形或位姿，稱為奇異位形（singularconfiguration）58三角度位姿描述中的奇異點(diǎn)問題從實(shí)際物理意義上來說，此種情況意味著第1、3軸重合，導(dǎo)致繞第1、3軸的轉(zhuǎn)角無法計算，稱為奇異現(xiàn)象對應(yīng)的位姿點(diǎn)（由姿態(tài)角元素構(gòu)成的點(diǎn)），稱為奇異點(diǎn)Z-Y-X歐拉角，

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

=±

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

=0。/hanjuefu5827/article/details/80659343?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task三角度姿態(tài)法60位姿奇異現(xiàn)象的具體案例機(jī)槍轉(zhuǎn)塔跟蹤過頂飛機(jī)目標(biāo)飛機(jī)過頂飛行；轉(zhuǎn)塔方位跟蹤速度趨向于無窮大。當(dāng)接近方位角為90

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

按右手定則旋轉(zhuǎn)θ角，可到達(dá){B}的實(shí)際姿態(tài)此旋轉(zhuǎn)記為：右手定則等效軸-角法62

等效軸-角法63等效軸-角旋轉(zhuǎn)矩陣表達(dá)式的推導(dǎo)

64等效軸角旋轉(zhuǎn)矩陣表達(dá)式的推導(dǎo)于是，根據(jù)旋轉(zhuǎn)變換方程可得：

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

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