機(jī)器人的位姿描述與坐標(biāo)變換0912

1、戰(zhàn) 強(qiáng)北京航空航天大學(xué)機(jī)器人研究所機(jī)器人的位姿連桿I的位姿YXZYiXiZiYwXwZw2-1、基本概念、基本概念1) 自由度自由度(Degree of Freedom, DOF)：指一個(gè)點(diǎn)或一個(gè)物體運(yùn)動(dòng)的方式，或一個(gè)動(dòng)態(tài)系統(tǒng)的變化方式。每個(gè)自由度可表示一個(gè)獨(dú)立的變量，而利用所有的自由度，就可完全規(guī)定所研究的一個(gè)物體或一個(gè)系統(tǒng)的位置和姿態(tài)。也指描述物體運(yùn)動(dòng)所需的獨(dú)立坐標(biāo)數(shù)，3維空間需要6個(gè)自由度。2) 操作臂操作臂(Manipulator)：具有和人手臂(Arm)相似的功能、可在空間抓放物體或進(jìn)行其它操作的機(jī)電裝置。-ArmArm3) 末端執(zhí)行器末端執(zhí)行器(End-Effector)：位于機(jī)器

2、人腕部的末端，直接執(zhí)行工作要求的裝置。如靈巧手、夾持器。-Hand/GripperHand/Gripper4) 手腕手腕(Wrist)：位于執(zhí)行器與手臂之間，具有支撐和調(diào)整末端執(zhí)行器姿態(tài)功能的機(jī)構(gòu)。操作臂的組成部分之一。5)手臂手臂(Arm)：位于基座和手腕之間，由操作手的動(dòng)力關(guān)節(jié)和連桿等組成的組件。能支撐手腕和末端執(zhí)行器，并具有調(diào)整末端執(zhí)行器位置的功能。操作臂的組成部分。Outdated！6) 世界坐標(biāo)系世界坐標(biāo)系(World Coordinate System)：參照地球的直角坐標(biāo)系。7) 機(jī)座坐標(biāo)系、基坐標(biāo)系機(jī)座坐標(biāo)系、基坐標(biāo)系(Base reference coordinate sys

3、tem)：參照機(jī)器人基座的坐標(biāo)系，即機(jī)器人末端位姿的參考坐標(biāo)系。8) 坐標(biāo)變換坐標(biāo)變換(Coordinate Transformation)： 將一個(gè)點(diǎn)的坐標(biāo)描述從一個(gè)坐標(biāo)系轉(zhuǎn)換到另一個(gè)坐標(biāo)系下描述的過(guò)程。手腕機(jī)座手臂YwXwZw9) 位姿位姿(Position&Pose)：機(jī)器人末端執(zhí)行器在指定坐標(biāo)系中的位置和姿態(tài)。10) 工作空間工作空間(Working Space)：機(jī)器人在執(zhí)行任務(wù)時(shí)，其腕軸交點(diǎn)能在空間活動(dòng)的范圍。由連桿尺寸和構(gòu)形決定。11) 負(fù)載負(fù)載(Load)：作用于末端執(zhí)行器上的質(zhì)量和力矩。12) 額定負(fù)載額定負(fù)載(Rated Load)：機(jī)器人在規(guī)定的性能范圍內(nèi)，末端機(jī)

4、械接口處能夠承受的最大負(fù)載量(包括末端執(zhí)行器在內(nèi))。13) 分辨率分辨率(Resolution)：機(jī)器人每個(gè)關(guān)節(jié)能夠?qū)崿F(xiàn)的最小移動(dòng)距離或最小轉(zhuǎn)動(dòng)角度。14) 位姿精度位姿精度(Pose Accuracy)：指令設(shè)定位姿與實(shí)際到達(dá)位姿的一致程度。15) 軌跡精度軌跡精度(Path Accuracy)：機(jī)器人機(jī)械接口中心跟指令軌跡的一致程度.16) 點(diǎn)位控制點(diǎn)位控制(Point to Point Control，PTP)：控制機(jī)器人從一個(gè)位姿轉(zhuǎn)到另 一個(gè)位姿，其路徑不限。17) 連續(xù)軌跡控制連續(xù)軌跡控制(Continuous Path Control，CP)：機(jī)械接口在指定的軌跡上，按照編程規(guī)定的

5、位姿和速度移動(dòng)。它適于對(duì)兩個(gè)以上的運(yùn)動(dòng)環(huán)節(jié)進(jìn)行控制。18) 協(xié)調(diào)控制協(xié)調(diào)控制(Coordinated Control)：協(xié)調(diào)多個(gè)手臂或多臺(tái)機(jī)器人同時(shí)進(jìn)行某種作業(yè)的控制。19) 伺服系統(tǒng)伺服系統(tǒng)(Servo System)：控制機(jī)器人的位姿和速度等，使其跟隨目標(biāo)值變化的控制系統(tǒng)。20) 離線(xiàn)編程離線(xiàn)編程(Off-line Programming)：機(jī)器人作業(yè)方式的信息記憶過(guò)程與作業(yè)對(duì)象不發(fā)生直接關(guān)系的編程方式。21) 在線(xiàn)編程在線(xiàn)編程(On-line Programming)：通過(guò)人的示教來(lái)完成操作信息的記憶 過(guò)程的編程方式。22) 人工智能人工智能(Artificial Intelligenc

6、e，AI)：機(jī)器人能執(zhí)行一些類(lèi)似人類(lèi)智力活動(dòng)的能力。如推理、規(guī)劃、圖像識(shí)別、理解和學(xué)習(xí)等。23) 模式識(shí)別模式識(shí)別(Pattern Recognition)：通過(guò)類(lèi)似人類(lèi)感覺(jué)器官的傳感器所檢測(cè)的信息來(lái)分析、描述和區(qū)分各個(gè)物體特征的方法。24) 機(jī)器人語(yǔ)言機(jī)器人語(yǔ)言(Robot Language)：機(jī)器人系統(tǒng)中的計(jì)算機(jī)編程語(yǔ)言，主要有VAL、VAL2、LAMA、RAIL等。25) 觸覺(jué)觸覺(jué)(Tactile Sense)：機(jī)器人與物體之間接觸時(shí)所得到的感覺(jué)信息。26) 壓覺(jué)壓覺(jué)(Sense of Contact Force)：機(jī)器人與物體某個(gè)表面接觸時(shí)，沿法線(xiàn)方向受到的力的信息感覺(jué)。27) 視覺(jué)視

7、覺(jué)(Visual Sense)：機(jī)器人對(duì)光等外界信息的感覺(jué)。利用這種感覺(jué)可以 識(shí)別物體的輪廓、方位、背景等環(huán)境狀態(tài)。28) 接近覺(jué)接近覺(jué)(Proximity Sense)：機(jī)器人能感受到與物體接近程度的能力。29) 滑覺(jué)滑覺(jué)(Slip Sense)：機(jī)器人能感受到其末端執(zhí)行器與被夾持物之間滑移程度的能力。力、力矩超聲視覺(jué)2-2、機(jī)器人機(jī)構(gòu)分類(lèi)與機(jī)構(gòu)簡(jiǎn)圖、機(jī)器人機(jī)構(gòu)分類(lèi)與機(jī)構(gòu)簡(jiǎn)圖1) 機(jī)器人機(jī)構(gòu)的基本組成機(jī)器人機(jī)構(gòu)的基本組成連桿Link關(guān)節(jié)Joint2) 機(jī)構(gòu)圖形符號(hào)機(jī)構(gòu)圖形符號(hào) 移動(dòng)關(guān)節(jié)移動(dòng)關(guān)節(jié) 轉(zhuǎn)動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)關(guān)節(jié) 球關(guān)節(jié)球關(guān)節(jié) 圓柱關(guān)節(jié)圓柱關(guān)節(jié) 末端執(zhí)行器末端執(zhí)行器 機(jī)座機(jī)座 連桿連桿關(guān)節(jié)關(guān)節(jié)

8、=運(yùn)動(dòng)副運(yùn)動(dòng)副3) 機(jī)器人按機(jī)構(gòu)形式分類(lèi)與簡(jiǎn)圖機(jī)器人按機(jī)構(gòu)形式分類(lèi)與簡(jiǎn)圖 串聯(lián)機(jī)器人并聯(lián)機(jī)器人優(yōu)點(diǎn)：工作空間大、速度快缺點(diǎn)：系統(tǒng)的剛性較弱、定位精度較差優(yōu)點(diǎn)：系統(tǒng)的剛度大、定位精度高缺點(diǎn)：工作空間小、運(yùn)動(dòng)速度低串聯(lián)機(jī)器人的種類(lèi)：A、直角坐標(biāo)型機(jī)器人、直角坐標(biāo)型機(jī)器人圓柱坐標(biāo)機(jī)器人圓柱坐標(biāo)機(jī)器人),(RZFPzRXZY),(ZYXFP zR球坐標(biāo)機(jī)器人球坐標(biāo)機(jī)器人),(RFPRSCARA機(jī)器人機(jī)器人),( FP關(guān)節(jié)型關(guān)節(jié)型機(jī)器人（通用）機(jī)器人（通用）并聯(lián)機(jī)器人示例：2-3 剛體位姿的數(shù)學(xué)描述剛體位姿的數(shù)學(xué)描述000zyxPoo)cos()cos()cos()cos()cos()cos()cos(

9、)cos()cos(33ZZZYZXYZYYYXXZXYXXZYXROOOOOOOO剛體位置：剛體位置：剛體姿態(tài)：剛體姿態(tài)：?jiǎn)挝恢魇噶浚?￥假設(shè)機(jī)器人的連桿和關(guān)節(jié)都是假設(shè)機(jī)器人的連桿和關(guān)節(jié)都是剛體剛體￥ ￥ OXYZZYXObnt11RRROOTOOOO9個(gè)元素，只有3個(gè)獨(dú)立，滿(mǎn)足6個(gè)約束條件:0.1.XZZYYXZZYYXXOOOOOOOOOOOOOOOOOOOOOOOO, PROOOOO剛體的位置和姿態(tài)：R是單位正交陣姿態(tài)矩陣R的特點(diǎn)：)cos()cos()cos()cos()cos()cos()cos()cos()cos(33ZZZYZXYZYYYXXZXYXXZYXROOOOOOOOj

10、ZjXjYiZiXiYOiOj例：某剛體j在參考系i中的 位置 姿態(tài)?jiOOR106?jiooP 2-4 坐標(biāo)變換（坐標(biāo)變換（點(diǎn)的映射點(diǎn)的映射）1、坐標(biāo)平移、坐標(biāo)平移（坐標(biāo)系方位相同）（坐標(biāo)系方位相同）iOjjiPPPjZjXjYPOjiPiZiXiYOiOj沿著不同軸向的組合平移：zyxzyxPOji000000POOOPOjjiiY1X1Z1Y2X2Z2Y3X3Z3三坐標(biāo)的直角坐標(biāo)機(jī)器人適用的機(jī)器人類(lèi)型舉例（有平移關(guān)節(jié)）iZiXiYOijZjXjYPOj15例: TjP765已知 求 P點(diǎn)在i坐標(biāo)系中的坐標(biāo)。 TTTOjijiPPP72150150765解答：2、坐標(biāo)旋轉(zhuǎn)、坐標(biāo)旋轉(zhuǎn)（坐標(biāo)

11、系原點(diǎn)相同）（坐標(biāo)系原點(diǎn)相同）ZiXiYiZjXjYjP坐標(biāo)系j由坐標(biāo)系i旋轉(zhuǎn)而成TiiiizyxPTjjjjzyxP求點(diǎn)P在i坐標(biāo)系的坐標(biāo)：已知點(diǎn)P在j坐標(biāo)系的坐標(biāo)：ZiZjXiXjYiYjPjxjyjzcos(,)ijijyxY Xixiyizcos(,)cos(,)ijijjijyxY XyY Ycos(,)cos(,)cos(,)ijijjijjijyxY XyY YzY Z( ,)ijY X關(guān)于？),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(jijjijjijijijjijjijijijjijjijiiZZzYZyXZxz

12、ZYzYYyXYxyZXzYXyXXxxPZiZjXiXjYiYjPjxjyjzixiyizjjjjijijijijijijijijiizyxZZYZXZZYYYXYZXYXXXP),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(Rji 姿態(tài)矢量矩姿態(tài)矢量矩陣陣OXYZZYXObntPj)cos()cos()cos()cos()cos()cos()cos()cos()cos(ZZZYZXYZYYYXXZXYXXROOPRPjjii坐標(biāo)系j相對(duì)于i的方位TijijjiRRR1旋轉(zhuǎn)矩陣的性質(zhì)：旋轉(zhuǎn)矩陣 繞一個(gè)坐標(biāo)軸旋轉(zhuǎn)的轉(zhuǎn)動(dòng)矩陣?yán)@一個(gè)坐標(biāo)軸

13、旋轉(zhuǎn)的轉(zhuǎn)動(dòng)矩陣jZiZiXjYiYjXjZiZiXjYiYjXjZiZiXjYiYjX1）RX2）RY3）RZ1000cossin0sincos),(ijiZRcossin0sincos0001),(ijiXRcos0sin010sin0cos),(ijiYRjZiZiXjYiYjXjZiZiXjYiYjXjZiZiXjYiYjX轉(zhuǎn)動(dòng)矩陣的特點(diǎn)：轉(zhuǎn)動(dòng)矩陣的特點(diǎn)：(1) 主對(duì)角線(xiàn)上有一個(gè)元素為1，其余均為轉(zhuǎn)角的余弦/正弦；(2) 繞軸轉(zhuǎn)動(dòng)的次序與元素1所在的行、列號(hào)對(duì)應(yīng)；(3) 元素1所在的行、列，其它元素均為0；(4) 從元素1所在行起，自上而下，先出現(xiàn)的正弦為負(fù)，后出現(xiàn)的為正，反之依然。10

14、00cossin0sincos),(ijiZRcossin0sincos0001),(ijiXRcos0sin010sin0cos),(ijiYR 繞多個(gè)坐標(biāo)軸旋轉(zhuǎn)的轉(zhuǎn)動(dòng)矩陣?yán)@多個(gè)坐標(biāo)軸旋轉(zhuǎn)的轉(zhuǎn)動(dòng)矩陣1）、繞固定坐標(biāo)系旋轉(zhuǎn)),(iXR),(iZR( , )?jiR cossin0sincoscoscossinsinsincossincoscossin0sincos00011000cossin0sincos),(Rji),(iiiZYX坐標(biāo)系),(mmmZYX坐標(biāo)系),(jjjZYX坐標(biāo)系iZiXjYiYjXmZmXmYjZ),(),(),(XRZRRji2）、繞運(yùn)動(dòng)坐標(biāo)系旋轉(zhuǎn)),(iZR),(

15、1YR),(2ZR),(),(),(),(ZRYRZRRjiZYZ歐拉角),(iiiZYX坐標(biāo)系),(111ZYX坐標(biāo)系),(222ZYX坐標(biāo)系),(jjjZYX坐標(biāo)系iX1XjX2XiYjY1Y2（Y )(1ZZijZ2Z注意注意：多個(gè)旋轉(zhuǎn)矩陣連乘時(shí)，次序不同則含義不同。1）繞新的動(dòng)坐標(biāo)軸依次轉(zhuǎn)動(dòng)時(shí)，每個(gè)旋轉(zhuǎn)矩陣要從左往右左往右乘，即旋轉(zhuǎn)矩陣的相乘順序與轉(zhuǎn)動(dòng)次序相同；2）繞舊的固定坐標(biāo)軸依次轉(zhuǎn)動(dòng)時(shí)，每個(gè)旋轉(zhuǎn)矩陣要從右往右往左左乘，即旋轉(zhuǎn)矩陣的相乘順序與轉(zhuǎn)動(dòng)次序相反。cossinsinsinsinsinsincoscossincossinsincoscoscossinsincoscossins

16、incoscossinsincoscoscos1000cossin0sincoscos0sin010sin0cos1000cossin0sincos),(Rji證明： ),(iiiZYX坐標(biāo)系),(111ZYX坐標(biāo)系),(222ZYX坐標(biāo)系),(jjjZYX坐標(biāo)系jiiiiijjjPZRYRZRPYRZRPZRPRPPYRPRPPZRPRP),(),(),(),(),(),() 3),()2),() 121211112122112221）繞運(yùn)動(dòng)坐標(biāo)系旋轉(zhuǎn)),(iZR),(1YR),(2ZRiX1XjX2XiYjY1Y2（Y )(1ZZijZ2Z?jiR 2）、繞固定坐標(biāo)系旋轉(zhuǎn)),(iiiZYX

17、坐標(biāo)系),(mmmZYX坐標(biāo)系),(jjjZYX坐標(biāo)系iZiXjYiYjXmZmXmYjZ),(iZ),(iXjiimimmiijijjmmPZRXRPXRPRPPZRPRP),(),(),()2),() 1證明與討論證明與討論:?jiR 適用的機(jī)器人類(lèi)型舉例（有旋轉(zhuǎn)關(guān)節(jié)）Y6X6Z6Y0X0Z0Y7X7Z7例例1: 已知坐標(biāo)系已知坐標(biāo)系B初始位姿與初始位姿與A重合重合,首先首先B相對(duì)于坐標(biāo)系相對(duì)于坐標(biāo)系A(chǔ)的的Z軸轉(zhuǎn)軸轉(zhuǎn)30度度, 假設(shè)點(diǎn)假設(shè)點(diǎn)P在在 坐標(biāo)系坐標(biāo)系B的描述為的描述為PB=3,7,0T,求它在坐標(biāo)求它在坐標(biāo)系系A(chǔ)中的描述中的描述PA.3、坐標(biāo)變換綜合（平移、坐標(biāo)變換綜合（平移+旋

18、轉(zhuǎn)）旋轉(zhuǎn)）PRPPOjijjii旋轉(zhuǎn)部分平移部分推導(dǎo)(中間坐標(biāo)系C)：iZiXiYOiZjXjYjPjPOjiPPOjicZcXcYI（旋轉(zhuǎn)）: c與j 原點(diǎn)重合，c與i姿態(tài)相同jjijjccRPRPPII（平移）: c與i 原點(diǎn)重合PRPPPPOjijjiOcici問(wèn)題：是否可以先平移后旋轉(zhuǎn)？iZiXiYOiZjXjYjPjPOjiPPOjicZcXcY推導(dǎo)(中間坐標(biāo)系C)：I（平移）: c與i原點(diǎn)重合，c與j姿態(tài)相同jOijOccPPPPPjjII（旋轉(zhuǎn)）: c與i 姿態(tài)相同jjiOijjiOccijOcjicciiRPPRPPRPPRRPPjjj)(未知jOccjPPP例例1: 已知坐標(biāo)

19、系已知坐標(biāo)系B初始位姿與初始位姿與A重合重合,首先首先B相對(duì)于坐標(biāo)系相對(duì)于坐標(biāo)系A(chǔ)的的Z軸轉(zhuǎn)軸轉(zhuǎn)30度度,再沿再沿A的的X軸移動(dòng)軸移動(dòng)10個(gè)單位個(gè)單位,并沿并沿A的的Y軸移動(dòng)軸移動(dòng)5個(gè)單位個(gè)單位.假設(shè)點(diǎn)假設(shè)點(diǎn)P在在 坐標(biāo)系坐標(biāo)系B的描述為的描述為PB=3,7,0T,求它在坐標(biāo)系求它在坐標(biāo)系A(chǔ)中的中的描述描述PA.0562.12098. 90500010073100030cos30sin030sin30cos)5 ,()10,()30,(ooooAAAOBABAAYPXPPZRPRPPBBAZAXAYOiPOjBZBXBYzyxPxa yb zc 2-5 齊次坐標(biāo)與齊次變換齊次坐標(biāo)與齊次變換1、

20、齊次坐標(biāo)、齊次坐標(biāo)cbaP0齊次坐標(biāo)直角坐標(biāo)1）點(diǎn)的齊次坐標(biāo)：TTTPPzyxP2864,1432非零的比例因子2）坐標(biāo)軸方向的齊次坐標(biāo)：Tcba0T0001X軸：a,b,c稱(chēng)為方向數(shù)Y軸：Z軸：T0010T0100TT1000,0000坐標(biāo)原點(diǎn)無(wú)意義點(diǎn)AZAXAYOijXiZiXiYPOjiOiPjYjZOj2、齊次變換、齊次變換441000PRTOjjijii141iP點(diǎn)P在i坐標(biāo)系中的齊次坐標(biāo)：點(diǎn)P在j坐標(biāo)系中的齊次坐標(biāo)：141jP旋轉(zhuǎn)矩陣與平移向量構(gòu)成的齊次變換矩陣：11110001iOjijjijOjijijjiPPRPPPRPTiP齊次變換矩陣Tji表示了坐標(biāo)系j相對(duì)于坐標(biāo)系i的位

21、姿的含義：1000401030011100TjiiXiYiZjXjYjZPOjijOiOjXjYjZPjOijjiiTPP134jOip 旋轉(zhuǎn)的齊次變換旋轉(zhuǎn)的齊次變換 平移加旋轉(zhuǎn)變換平移加旋轉(zhuǎn)變換10000),(RKRji1000100001000),()(33PRRPIKRPTransTjijOijijiOjOiji 平移的齊次變換平移的齊次變換1000)(3*3PIPTransOjiOji1000100001000),()(33PRRPIKRPTransTjijOijijiOjOijiiZiXiYOiZjXjYjPjPOjiPPOjicZcXcYiZiXiYOiZjXjYjPjPOjiPP

22、OjicZcXcY例例2: 已知坐標(biāo)系已知坐標(biāo)系B初始位姿與初始位姿與A重合重合,首先首先B相對(duì)于坐標(biāo)系相對(duì)于坐標(biāo)系A(chǔ)的的Z軸軸轉(zhuǎn)轉(zhuǎn)30度度,再沿再沿A的的X軸移動(dòng)軸移動(dòng)10個(gè)單位個(gè)單位,并沿并沿A的的Y軸移動(dòng)軸移動(dòng)5個(gè)單位個(gè)單位.假設(shè)假設(shè)點(diǎn)點(diǎn)P在在 坐標(biāo)系坐標(biāo)系B的描述為的描述為PB=3,7,0T,求它在坐標(biāo)系求它在坐標(biāo)系A(chǔ)中的描述中的描述PA.10562.12098. 91073100001000030cos30sin0030sin30cos10510010000100001)(),(BBPZRYXTransTPPBAA10562.12098. 910731051001000030cos3

23、0sin0030sin30cosBTPPBAA解法1:解法2:練習(xí)題1：已知坐標(biāo)系A(chǔ)初始位姿與B重合,首先A相對(duì)于坐標(biāo)系B的Z軸轉(zhuǎn)30度,再沿B的X軸移動(dòng)10個(gè)單位, 再相對(duì)于A的Y軸轉(zhuǎn)60度，并沿A的Z軸移動(dòng)5個(gè)單位. 假設(shè)點(diǎn)P在坐標(biāo)系A(chǔ)的描述為 =12,0,4T,求它在坐標(biāo)系B中的描述 ；APBP1892. 5897. 6946.21)15,()60,()30,()10,(21ZTransYRZRXTransTTPooB解法1:2-6 機(jī)器人姿態(tài)的其他表示方法機(jī)器人姿態(tài)的其他表示方法)cos()Ycos()cos()cos()cos()cos()cos()cos()cos(33ZZZZXY

24、ZYYYXXZXYXXZYXROOOOOOOO前面：用前面：用3*3的旋轉(zhuǎn)矩陣表示剛體的姿態(tài)的旋轉(zhuǎn)矩陣表示剛體的姿態(tài)單位主矢量旋轉(zhuǎn)矩陣有9個(gè)元素，6個(gè)約束條件，3個(gè)獨(dú)立元素個(gè)獨(dú)立元素；計(jì)算編程時(shí)需要輸入9個(gè)元素，不方便；一般采用3個(gè)元素來(lái)表示。1、RPY角：（繞固定坐標(biāo)軸X-Y-Z旋轉(zhuǎn)）RPY角是描述船舶在海中航行時(shí)姿態(tài)的一種方法。XYZaR（Z， ）：RollR（Y， ）：PitchR（X， ）：Yaw翻滾翻滾偏航偏航俯仰俯仰（1）船舶上建立的坐標(biāo)系B相對(duì)于參考系A(chǔ)的方位描述如下： A和B 重合，首先，將B繞XA 轉(zhuǎn) 角，再繞YA轉(zhuǎn) 角，最后繞ZA轉(zhuǎn) 角。 /AAAZBYBXBBA),(),

25、(),(),(AAAxyzBAXRYRZRRcssccssccssc000010010010000ccscssccssccssscssscsccsssccc333231232221131211rrrrrrrrrccscssccssccssscssscsccsssccc333231232221131211rrrrrrrrr（2）如果已知機(jī)器人的姿態(tài)矩陣，如何求RPY角？221121cosrr 222221121()()cosc cs crr 9090221121cosrrtan2( , )arctanyAy xx雙變量反正切函數(shù)tan2( 2, 2)135 ,tan2(2,2)45AA cos0

26、90, ),(2tan0,90),(2tan0,9022122212rrArrAif通常的選擇22311121cos0,tan2(,),Arrrif if if2111tan2()Arr3233tan2(,)Arrccscssccssccssscssscsccsssccc333231232221131211rrrrrrrrr2、ZYX歐拉角：（繞動(dòng)坐標(biāo)軸Z-Y-X旋轉(zhuǎn)）/BBBXBYBZBBAccscssccssccssssssscsccsssccccssccssccsscXRYRZRRBBBzyxBA000010010010000),(),(),(),(ccscssccssccssscsss

27、csccsssccc),(),(),(),(AAAxyzBAXRYRZRRiX1XjX2XiYjY1Y2（Y )(1ZZijZ2Z坐標(biāo)系B相對(duì)于參考系A(chǔ)的方位描述如下：RPY角3、ZYZ歐拉角/BBBZBYBZBBA),(),(),(),(BBBzyzBAZRYRZRR333231232221131211rrrrrrrrrcsscsssccscsscccssccssccssccc),(2tan),(2tan),(2tan, 0sin3132332322311323rrArrrArrAif),(2tan0,180),(2tan0, 0, 0sin11121112rrArrAif已知姿態(tài)矩陣求歐拉

28、角坐標(biāo)系B相對(duì)于參考系A(chǔ)的方位描述如下：上述描述姿態(tài)的方法稱(chēng)為角度設(shè)定法，共有24種，其中12種RPY法和12種歐拉法，并且是對(duì)偶的，實(shí)際上只有12種不同的旋轉(zhuǎn)矩陣。確定旋轉(zhuǎn)時(shí)是繞固定軸還是動(dòng)軸非常重要。X Y Z的排列:1266/33331213PCCCEULERRPYZXZXYZ2-7 旋轉(zhuǎn)變換通式旋轉(zhuǎn)變換通式1、旋轉(zhuǎn)矩陣通式旋轉(zhuǎn)矩陣通式： iXiYiZjXjYjZOKkkjkikKzyx旋轉(zhuǎn)矩陣?),(KR1)、RKRji),(坐標(biāo)系j 相對(duì)于 i的姿態(tài)2)、定義兩個(gè)坐標(biāo)系i和j，i與i固連， j與j 固連； i和j 的Z軸與矢量K重合，旋轉(zhuǎn)前，i與j重合，i與j 重合。繞通過(guò)原點(diǎn)原點(diǎn)的

29、任意單位矢量K轉(zhuǎn) 角的旋轉(zhuǎn)矩陣zzzyyyxxxzzzyyyxxxjjiikonkonkonaonaonaonRR3)、iXiYiZjXjYjZOK iZ iX iY jX jY繞K轉(zhuǎn) 角j相對(duì)于i的Z軸轉(zhuǎn) 角 jZ旋轉(zhuǎn)變換的尺寸鏈圖：iijjRRRKRRjjjiiiji),(1),(),(RZRRKRRjjiijiTjjiijiRZRRKRR),(),(zyxzyxzyxzzzyyyxxxkkkooonnnkonkonkonKR1000cossin0sincos),( 利用旋轉(zhuǎn)矩陣的正交性質(zhì):onanaaoonaaoonn01假設(shè)：)cos1 (cossinverscscverskkskve

30、rskkskverskkskverskkcverskkskverskkskverskkskverskkcverskk),K(Rzzxzyyzxxyzyyzyxyxzzxyxx整理得：旋轉(zhuǎn)變換通式0, 1zyxkkk討論：討論：（1）（2）（3）1000cossin0sincos),(KRcossin0sincos0001),(KRcos0sin010sin0cos),(KR0, 1zxykkk0, 1xyzkkk例：坐標(biāo)系B原來(lái)與A重合，將坐標(biāo)系B繞過(guò)原點(diǎn)O的軸線(xiàn)kjiKA313131轉(zhuǎn)動(dòng)o120，求旋轉(zhuǎn)矩陣)120,(oAKR解答：31zyxkkk1）2）23120,23120sin,211

31、20cosooovers3）帶入旋轉(zhuǎn)通式得：010001100)120,(oAKRo7 .54)31cos(a2、等效轉(zhuǎn)軸與等效轉(zhuǎn)角、等效轉(zhuǎn)軸與等效轉(zhuǎn)角轉(zhuǎn)軸和轉(zhuǎn)角轉(zhuǎn)軸和轉(zhuǎn)角旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣12？zzzyyyxxxaonaonaon),(KRcverskkskverskkskverskkskverskkcverskkskverskkskverskkskverskkcverskkzzxzyyzxxyzyyzyxyxzzxyxxzzzyyyxxxaonaonaon1）將方程兩邊矩陣的主對(duì)角線(xiàn)元素分別相加，則ccverskkkaonzyxzyx213)(222) 1(21coszyxaon2）將方程兩

32、邊矩陣的非對(duì)角線(xiàn)元素成對(duì)相減得：sin2sin2sin2zxyyzxxyzkonknakaocverskkskverskkskverskkskverskkcverskkskverskkskverskkskverskkcverskkzzxzyyzxxyzyyzyxyxzzxyxxzzzyyyxxxaonaonaonsin2sin2sin2zxyyzxxyzkonknakao將上式兩邊平方相加得：) 1()()()(tan)()()(21sinsin4)()()(2222222222zyxxyzxyzxyzxyzxyzxyzaononnaaoonnaaoonnaao求得轉(zhuǎn)角sin2,sin2,si

33、n2xyzzxyyzxonknakaok求得轉(zhuǎn)軸注意注意：1）多值性：。一般?。ㄊ呛侠斫?。都有任意（的值不唯一。例如對(duì)于和o1800)360,(),),(),onKKKKK2）存在病態(tài)情況：殊的解法。，轉(zhuǎn)軸不確定，需要特分母都很小，或和度時(shí)，由于上式的分子或的值接近當(dāng)0sin1800) 1()()()(tan222zyxxyzxyzaononnaao例：。和等效轉(zhuǎn)角的等效轉(zhuǎn)軸求復(fù)合旋轉(zhuǎn)矩陣K)90,()90,(ZRYRRBA解：010001100100001010001010100RBA3tan,23sin,21cos利用前面的公式可求得：kjiKkkkzyx31313131)90,()90,()120,(ZRYRKR任何一組繞過(guò)原點(diǎn)的軸線(xiàn)的復(fù)合轉(zhuǎn)動(dòng)任何一組繞過(guò)原點(diǎn)的軸線(xiàn)的復(fù)合轉(zhuǎn)動(dòng)總等效于繞過(guò)原點(diǎn)的某一矢量的轉(zhuǎn)動(dòng)總等效于繞過(guò)原點(diǎn)的某一矢量的轉(zhuǎn)動(dòng)3、齊次變換通式、齊次變換通式