1、機器人的位姿描述和坐標變換機器人的位姿I的位姿YXZYiXiZiYwXwZw2-1、基本概念、基本概念1) 自由度自由度(Degree of Freedom, DOF)：指一個點或一個物體運動的方式，或一個動態系統的變化方式。每個自由度可表示一個獨立的變量，而利用所有的自由度，就可完全規定所研究的一個物體或一個系統的位置和姿態。也指描述物體運動所需的獨立坐標數，3維空間需要6個自由度。2) 操作臂操作臂(Manipulator)：具有和人手臂(Arm)相似的功能、可在空間抓放物體或進行其它操作的機電裝置。-ArmArm3) 末端執行器末端執行器(End-Effector)：位于機器人腕部的末端

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

3、照機器人基座的坐標系，即機器人末端位姿的參考坐標系。8) 坐標變換坐標變換(Coordinate Transformation)： 將一個點的坐標描述從一個坐標系轉換到另一個坐標系下描述的過程。手腕機座手臂YwXwZw9) 位姿位姿(Position&Pose)：機器人末端執行器在指定坐標系中的位置和姿態。10) 工作空間工作空間(Working Space)：機器人在執行任務時，其腕軸交點能在空間活動的范圍。由連桿尺寸和構形決定。11) 負載負載(Load)：作用于末端執行器上的質量和力矩。12) 額定負載額定負載(Rated Load)：機器人在規定的性能范圍內，末端機械接口處能夠

4、承受的最大負載量(包括末端執行器在內)。13) 分辨率分辨率(Resolution)：機器人每個關節能夠實現的最小移動距離或最小轉動角度。14) 位姿精度位姿精度(Pose Accuracy)：指令設定位姿與實際到達位姿的一致程度。15) 軌跡精度軌跡精度(Path Accuracy)：機器人機械接口中心跟指令軌跡的一致程度.16) 點位控制點位控制(Point to Point Control，PTP)：控制機器人從一個位姿轉到另 一個位姿，其路徑不限。17) 連續軌跡控制連續軌跡控制(Continuous Path Control，CP)：機械接口在指定的軌跡上，按照編程規定的位姿和速度移

5、動。它適于對兩個以上的運動環節進行控制。18) 協調控制協調控制(Coordinated Control)：協調多個手臂或多臺機器人同時進行某種作業的控制。19) 伺服系統伺服系統(Servo System)：控制機器人的位姿和速度等，使其跟隨目標值變化的控制系統。20) 離線編程離線編程(Off-line Programming)：機器人作業方式的信息記憶過程與作業對象不發生直接關系的編程方式。21) 在線編程在線編程(On-line Programming)：通過人的示教來完成操作信息的記憶 過程的編程方式。22) 人工智能人工智能(Artificial Intelligence，AI)：

6、機器人能執行一些類似人類智力活動的能力。如推理、規劃、圖像識別、理解和學習等。23) 模式識別模式識別(Pattern Recognition)：通過類似人類感覺器官的傳感器所檢測的信息來分析、描述和區分各個物體特征的方法。24) 機器人語言機器人語言(Robot Language)：機器人系統中的計算機編程語言，主要有VAL、VAL2、LAMA、RAIL等。25) 觸覺觸覺(Tactile Sense)：機器人與物體之間接觸時所得到的感覺信息。26) 壓覺壓覺(Sense of Contact Force)：機器人與物體某個表面接觸時，沿法線方向受到的力的信息感覺。27) 視覺視覺(Visu

7、al Sense)：機器人對光等外界信息的感覺。利用這種感覺可以 識別物體的輪廓、方位、背景等環境狀態。28) 接近覺接近覺(Proximity Sense)：機器人能感受到與物體接近程度的能力。29) 滑覺滑覺(Slip Sense)：機器人能感受到其末端執行器與被夾持物之間滑移程度的能力。力、力矩超聲視覺2-2、機器人機構分類與機構簡圖、機器人機構分類與機構簡圖1) 機器人機構的基本組成機器人機構的基本組成連桿Link關節Joint2) 機構圖形符號機構圖形符號 移動關節移動關節 轉動關節轉動關節 球關節球關節 圓柱關節圓柱關節 末端執行器末端執行器 機座機座 連桿連桿關節關節=運動副運動

8、副3) 機器人按機構形式分類與簡圖機器人按機構形式分類與簡圖 串聯機器人并聯機器人優點：工作空間大、速度快缺點：系統的剛性較弱、定位精度較差優點：系統的剛度大、定位精度高缺點：工作空間小、運動速度低串聯機器人的種類：A、直角坐標型機器人、直角坐標型機器人圓柱坐標機器人圓柱坐標機器人),(RZFPzRXZY),(ZYXFP zR球坐標機器人球坐標機器人),(RFPRSCARA機器人機器人),( FP關節型關節型機器人（通用）機器人（通用）并聯機器人示例：2-3 剛體位姿的數學描述剛體位姿的數學描述000zyxPoo)cos()cos()cos()cos()cos()cos()cos()cos()

9、cos(33ZZZYZXYZYYYXXZXYXXZYXROOOOOOOO剛體位置：剛體位置：剛體姿態：剛體姿態：單位主矢量￥ ￥假設機器人的連桿和關節都是￥假設機器人的連桿和關節都是剛體剛體￥ ￥ OXYZZYXObnt11RRROOTOOOO9個元素，只有3個獨立，滿足6個約束條件:0.1.XZZYYXZZYYXXOOOOOOOOOOOOOOOOOOOOOOOO, PROOOOO剛體的位置和姿態：R是單位正交陣姿態矩陣R的特點：)cos()cos()cos()cos()cos()cos()cos()cos()cos(33ZZZYZXYZYYYXXZXYXXZYXROOOOOOOOjZjXjY

10、iZiXiYOiOj例：某剛體j在參考系i中的 位置 姿態?jiOOR106?jiooP 2-4 坐標變換（點的映射）坐標變換（點的映射）1、坐標平移（坐標系方位相同）、坐標平移（坐標系方位相同）iOjjiPPPjZjXjYPOjiPiZiXiYOiOj沿著不同軸向的組合平移：zyxzyxPOji000000POOOPOjjiiY1X1Z1Y2X2Z2Y3X3Z3三坐標的直角坐標機器人適用的機器人類型舉例（有平移關節）iZiXiYOijZjXjYPOj15例: TjP765已知 求 P點在i坐標系中的坐標。 TTTOjijiPPP72150150765解答：2、坐標旋轉、坐標旋轉（坐標系原點相

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

12、YyXYxyZXzYXyXXxxPZiZjXiXjYiYjPjxjyjzixiyizjjjjijijijijijijijijiizyxZZYZXZZYYYXYZXYXXXP),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(Rji 姿態矢量矩姿態矢量矩陣陣OXYZZYXObntPj)cos()cos()cos()cos()cos()cos()cos()cos()cos(ZZZYZXYZYYYXXZXYXXROOPRPjjii坐標系j相對于i的方位TijijjiRRR1旋轉矩陣的性質：旋轉矩陣 繞一個坐標軸旋轉的轉動矩陣繞一個坐標軸旋轉的轉

13、動矩陣jZiZiXjYiYjXjZiZiXjYiYjXjZiZiXjYiYjX1）RX2）RY3）RZ1000cossin0sincos),(ijiZRcossin0sincos0001),(ijiXRcos0sin010sin0cos),(ijiYRjZiZiXjYiYjXjZiZiXjYiYjXjZiZiXjYiYjX轉動矩陣的特點：轉動矩陣的特點：(1) 主對角線上有一個元素為1，其余均為轉角的余弦/正弦；(2) 繞軸轉動的次序與元素1所在的行、列號對應；(3) 元素1所在的行、列，其它元素均為0；(4) 從元素1所在行起，自上而下，先出現的正弦為負，后出現的為正，反之依然。1000co

14、ssin0sincos),(ijiZRcossin0sincos0001),(ijiXRcos0sin010sin0cos),(ijiYR 繞多個坐標軸旋轉的轉動矩陣繞多個坐標軸旋轉的轉動矩陣1）、繞固定坐標系旋轉),(iXR),(iZR( , )?jiR cossin0sincoscoscossinsinsincossincoscossin0sincos00011000cossin0sincos),(Rji),(iiiZYX坐標系),(mmmZYX坐標系),(jjjZYX坐標系iZiXjYiYjXmZmXmYjZ),(),(),(XRZRRji2）、繞運動坐標系旋轉),(iZR),(1YR)

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

16、scossinsincoscoscos1000cossin0sincoscos0sin010sin0cos1000cossin0sincos),(Rji證明： ),(iiiZYX坐標系),(111ZYX坐標系),(222ZYX坐標系),(jjjZYX坐標系jiiiiijjjPZRYRZRPYRZRPZRPRPPYRPRPPZRPRP),(),(),(),(),(),() 3),()2),() 121211112122112221）繞運動坐標系旋轉),(iZR),(1YR),(2ZRiX1XjX2XiYjY1Y2（Y )(1ZZijZ2Z?jiR 2）、繞固定坐標系旋轉),(iiiZYX坐標系)

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

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

19、標系B初始位姿與初始位姿與A重合重合,首先首先B相對于坐標系相對于坐標系A的的Z軸轉軸轉30度度,再沿再沿A的的X軸移動軸移動10個單位個單位,并沿并沿A的的Y軸移動軸移動5個單位個單位.假設點假設點P在在 坐標系坐標系B的描述為的描述為PB=3,7,0T,求它在坐標系求它在坐標系A中的中的描述描述PA.0562.12098. 90500010073100030cos30sin030sin30cos)5 ,()10,()30,(ooooAAAOBABAAYPXPPZRPRPPBBAZAXAYOiPOjBZBXBYzyxPxa yb zc 2-5 齊次坐標與齊次變換齊次坐標與齊次變換1、齊次坐標

20、、齊次坐標cbaP0齊次坐標直角坐標1）點的齊次坐標：TTTPPzyxP2864,1432非零的比例因子2）坐標軸方向的齊次坐標：Tcba0T0001X軸：a,b,c稱為方向數Y軸：Z軸：T0010T0100TT1000,0000坐標原點無意義點AZAXAYOijXiZiXiYPOjiOiPjYjZOj2、齊次變換、齊次變換441000PRTOjjijii141iP點P在i坐標系中的齊次坐標：點P在j坐標系中的齊次坐標：141jP旋轉矩陣與平移向量構成的齊次變換矩陣：11110001iOjijjijOjijijjiPPRPPPRPTiP齊次變換矩陣Tji表示了坐標系j相對于坐標系i的位姿的含義

21、：1000401030011100TjiiXiYiZjXjYjZPOjijOiOjXjYjZPjOijjiiTPP134jOip 旋轉的齊次變換旋轉的齊次變換 平移加旋轉變換平移加旋轉變換10000),(RKRji1000100001000),()(33PRRPIKRPTransTjijOijijiOjOiji 平移的齊次變換平移的齊次變換1000)(3*3PIPTransOjiOji1000100001000),()(33PRRPIKRPTransTjijOijijiOjOijiiZiXiYOiZjXjYjPjPOjiPPOjicZcXcYiZiXiYOiZjXjYjPjPOjiPPOjic

22、ZcXcY例例2: 已知坐標系已知坐標系B初始位姿與初始位姿與A重合重合,首先首先B相對于坐標系相對于坐標系A的的Z軸軸轉轉30度度,再沿再沿A的的X軸移動軸移動10個單位個單位,并沿并沿A的的Y軸移動軸移動5個單位個單位.假設假設點點P在在 坐標系坐標系B的描述為的描述為PB=3,7,0T,求它在坐標系求它在坐標系A中的描述中的描述PA.10562.12098. 91073100001000030cos30sin0030sin30cos10510010000100001)(),(BBPZRYXTransTPPBAA10562.12098. 910731051001000030cos30sin

23、0030sin30cosBTPPBAA解法1:解法2:練習題1：已知坐標系A初始位姿與B重合,首先A相對于坐標系B的Z軸轉30度,再沿B的X軸移動10個單位, 再相對于A的Y軸轉60度，并沿A的Z軸移動5個單位. 假設點P在坐標系A的描述為 =12,0,4T,求它在坐標系B中的描述 ；APBP1892. 5897. 6946.21)15,()60,()30,()10,(21ZTransYRZRXTransTTPooB解法1:2-6 機器人姿態的其他表示方法機器人姿態的其他表示方法)cos()Ycos()cos()cos()cos()cos()cos()cos()cos(33ZZZZXYZYYY

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

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

26、),(2tan0,90),(2tan0,9022122212rrArrAif通常的選擇22311121cos0,tan2(,),Arrrif if if2111tan2()Arr3233tan2(,)Arrccscssccssccssscssscsccsssccc333231232221131211rrrrrrrrr2、ZYX歐拉角：（繞動坐標軸Z-Y-X旋轉）/BBBXBYBZBBAccscssccssccssssssscsccsssccccssccssccsscXRYRZRRBBBzyxBA000010010010000),(),(),(),(ccscssccssccssscssscscc

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

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

29、矢量K轉 角的旋轉矩陣zzzyyyxxxzzzyyyxxxjjiikonkonkonaonaonaonRR3)、iXiYiZjXjYjZOK iZ iX iY jX jY繞K轉 角j相對于i的Z軸轉 角 jZ旋轉變換的尺寸鏈圖：iijjRRRKRRjjjiiiji),(1),(),(RZRRKRRjjiijiTjjiijiRZRRKRR),(),(zyxzyxzyxzzzyyyxxxkkkooonnnkonkonkonKR1000cossin0sincos),( 利用旋轉矩陣的正交性質:onanaaoonaaoonn01假設：)cos1 (cossinverscscverskkskverskk

30、skverskkskverskkcverskkskverskkskverskkskverskkcverskk),K(Rzzxzyyzxxyzyyzyxyxzzxyxx整理得：旋轉變換通式0, 1zyxkkk討論：討論：（1）（2）（3）1000cossin0sincos),(KRcossin0sincos0001),(KRcos0sin010sin0cos),(KR0, 1zxykkk0, 1xyzkkk例：坐標系B原來與A重合，將坐標系B繞過原點O的軸線kjiKA313131轉動o120，求旋轉矩陣)120,(oAKR解答：31zyxkkk1）2）23120,23120sin,21120co

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

32、非對角線元素成對相減得：sin2sin2sin2zxyyzxxyzkonknakaocverskkskverskkskverskkskverskkcverskkskverskkskverskkskverskkcverskkzzxzyyzxxyzyyzyxyxzzxyxxzzzyyyxxxaonaonaonsin2sin2sin2zxyyzxxyzkonknakao將上式兩邊平方相加得：) 1()()()(tan)()()(21sinsin4)()()(2222222222zyxxyzxyzxyzxyzxyzxyzaononnaaoonnaaoonnaao求得轉角sin2,sin2,sin2xyzzxyyzxonknakaok求得轉軸注意注意：1）多值性：。一般取（是合理解。都有任意（的值不唯一。例如對于和o1800)360,(),),(),onKKKKK2）存在病態情況：殊的解法。，轉軸不確定，需要特分母都很小，或和度時，由于上式的分子或的值接近當0sin1800) 1()()()(tan222zyxxyzxyzaononnaao例：。和等效轉角的等效