机器人的位姿描述与坐标变换.pdf

机器人学机器人学战 强北京航空航天大学机器人研究所第二章第二章 机器人的位姿描述与坐标变换机器人的位姿描述与坐标变换机器人的位姿连杆I的位姿YXZYiXiZiYwXwZw2-1、基本概念、基本概念1) 自由度自由度(Degree of Freedom, DOF)：：指一个点或一个物体运动的方式，或一个动态系统的变化方式每个自由度可表示一个独立的变量，而利用所有的自由度，就可完全规定所研究的一个物体或一个系统的位置和姿态也指描述物体运动所需的独立坐标数，3维空间需要6个自由度2) 操作臂操作臂(Manipulator)：：具有和人手臂(Arm)相似的功能、可在空间抓放物体或进行其它操作的机电装置ArmArm3) 末端执行器末端执行器(End-Effector)：：位于机器人腕部的末端，直接执行工作要求的装置如灵巧手、夹持器Hand/GripperHand/Gripper4) 手腕手腕(Wrist)：：位于执行器与手臂之间，具有支撑和调整末端执行器姿态功能的机构操作臂的组成部分之一5)手臂手臂(Arm)：：位于基座和手腕之间，由操作手的动力关节和连杆等组成的组件能支撑手腕和末端执行器，并具有调整末端执行器位置的功能。

操作臂的组成部分Outdated！6) 世界坐标系世界坐标系(World Coordinate System)：：参照地球的直角坐标系7) 机座坐标系、基坐标系机座坐标系、基坐标系(Base reference coordinate system)：：参照机器人基座的坐标系，即机器人末端位姿的参考坐标系8) 坐标变换坐标变换(Coordinate Transformation)：：将一个点的坐标描述从一个坐标系转换到另一个坐标系下描述的过程手腕机座手臂YwXwZw9) 位姿位姿(Position&Pose)：：机器人末端执行器在指定坐标系中的位置和姿态10) 工作空间工作空间(Working Space)：：机器人在执行任务时，其腕轴交点能在空间活动的范围由连杆尺寸和构形决定11) 负载负载(Load)：：作用于末端执行器上的质量和力矩12) 额定负载额定负载(Rated Load)：：机器人在规定的性能范围内，末端机械接口处能够承受的最大负载量(包括末端执行器在内)13) 分辨率分辨率(Resolution)：：机器人每个关节能够实现的最小移动距离或最小转动角度14) 位姿精度位姿精度(Pose Accuracy)：：指令设定位姿与实际到达位姿的一致程度。

15) 轨迹精度轨迹精度(Path Accuracy)：：机器人机械接口中心跟指令轨迹的一致程度.16) 点位控制点位控制(Point to Point Control，，PTP)：：控制机器人从一个位姿转到另一个位姿，其路径不限17) 连续轨迹控制连续轨迹控制(Continuous Path Control，，CP)：：机械接口在指定的轨迹上，按照编程规定的位姿和速度移动它适于对两个以上的运动环节进行控制18) 协调控制协调控制(Coordinated Control)：：协调多个手臂或多台机器人同时进行某种作业的控制19) 伺服系统伺服系统(Servo System)：：控制机器人的位姿和速度等，使其跟随目标值变化的控制系统20) 离线编程离线编程(Off-line Programming)：：机器人作业方式的信息记忆过程与作业对象不发生直接关系的编程方式21) 编程编程(On-line Programming)：：通过人的示教来完成操作信息的记忆 过程的编程方式22) 人工智能人工智能(Artificial Intelligence，，AI)：：机器人能执行一些类似人类智力活动的能力。

如推理、规划、图像识别、理解和学习等23) 模式识别模式识别(Pattern Recognition)：：通过类似人类感觉器官的传感器所检测的信息来分析、描述和区分各个物体特征的方法24) 机器人语言机器人语言(Robot Language)：：机器人系统中的计算机编程语言，主要有VAL、VAL2、LAMA、RAIL等25) 触觉触觉(Tactile Sense)：：机器人与物体之间接触时所得到的感觉信息26) 压觉压觉(Sense of Contact Force)：：机器人与物体某个表面接触时，沿法线方向受到的力的信息感觉27) 视觉视觉(Visual Sense)：：机器人对光等外界信息的感觉利用这种感觉可以 识别物体的轮廓、方位、背景等环境状态28) 接近觉接近觉(Proximity Sense)：：机器人能感受到与物体接近程度的能力29) 滑觉滑觉(Slip Sense)：：机器人能感受到其末端执行器与被夹持物之间滑移程度的能力力、力矩超声视觉2-2、机器人机构分类与图形符号、机器人机构分类与图形符号1) 机器人机构的基本组成机器人机构的基本组成连杆Link关节Joint2) 机构图形符号机构图形符号移动关节移动关节转动关节转动关节球关节球关节圆柱关节圆柱关节末端执行器末端执行器机座机座连杆连杆关节关节==运动副运动副3) 机器人按机构形式分类与简图机器人按机构形式分类与简图串联机器人并联机器人优点：工作空间大、速度快缺点：系统的刚性较弱、定位精度较差优点：系统的刚度大、定位精度高缺点：工作空间小、运动速度低串联机器人的种类：A、直角坐标型机器人、直角坐标型机器人B B B B、、、 圆柱坐标机器人、 圆柱坐标机器人),,(RZFPθ=zRθXZY),,(ZYXFP =zRθC C C C、、、 球坐标机器人、 球坐标机器人),,(RFPφθ=RθφD D D D、、、、SCARA机器人机器人),,(γφθ= FPθφγE E E E、、、关节型机器人（通用）、关节型机器人（通用）并联机器人示例：2-3 刚体位姿的数学描述刚体位姿的数学描述⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=000'zyxPoo⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡∠∠∠∠∠∠∠∠∠==×)'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos(][33''''ZZZZZXYZYYYXXZXYXXZYXROOOOOOOO位置矢量姿态矢量位置矢量姿态矢量单位主矢量￥ ￥￥ ￥假设机器人的连杆和关节都是刚体假设机器人的连杆和关节都是刚体￥ ￥￥ ￥OXYZZ'Y'X'O'bntYXZYiXiZiYwXwZw1''1'==−RRROOTOOOO},{} '{''PROOOOO=刚体的位置和姿态：姿态矩阵R的特点：9个元素，只有3个独立，满足6个约束条件:☺0...1...''''''''''''======XZZYYXZZYYXXOOOOOOOOOOOOOOOOOOOOOOOO☺R是单位正交阵⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡∠∠∠∠∠∠∠∠∠=)Z' Zcos()Z' Zcos()Z'Xcos()Y' Zcos()Y'Ycos()Y'Xcos()X' Zcos()X'Ycos()X'Xcos(R'OO2-4 坐标变换（坐标变换（点的映射点的映射））1、坐标平移、坐标平移（坐标系方位相同）（坐标系方位相同）PPPOjiji+=jZjXjY•POjiPiZiXiYOiOj沿着不同轴向的组合平移：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡Δ∑Δ∑Δ∑=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡Δ∑+⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡Δ∑+⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡Δ∑=zyxzyxPOji000000POOOPOjjii+=Y1X1Z1Y2X2Z2Y3X3Z3三坐标的直角坐标机器人适用的机器人类型举例（有平移关节）iZiXiYOijZjXjY•POj15例: []TjP765−=已知求 P点在i坐标系中的坐标。

[][][]TTTOjijiPPP72150150765−=+−=+=解答：10.302、坐标旋转、坐标旋转（坐标系原点相同）（坐标系原点相同）ZiXiYiZjXjYjP坐标系j由坐标系i旋转而成TiiiizyxP][=TjjjjzyxP][=求点P在i坐标系的坐标：已知点P在j坐标系的坐标：ZiZjXiXjYiYjPjxjyjz⎪⎩⎪⎨⎧∠+∠+∠=∠+∠+∠=∠+∠+∠==),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(jijjijjijijijjijjijijijjijjijiiZZzYZyXZxzZYzYYyXYxyZXzYXyXXxxPixiyiz⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡∠∠∠∠∠∠∠∠∠=jjjjijijijijijijijijiizyxZZYZXZZYYYXYZXYXXXP),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(Rji►姿态矢量矩阵►姿态矢量矩阵OXYZZ'Y'X'O'bntPj⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡∠∠∠∠∠∠∠∠∠=)'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos('ZZZZZXYZYYYXXZXYXXROOPRPjjii=坐标系j相对于i的方位TijijjiRRR==−1旋转矩阵的性质：旋转矩阵►绕一个坐标轴旋转的转动矩阵►绕一个坐标轴旋转的转动矩阵⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=θθθθθcossin0sincos0001),(ijiXR⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=θθθθθcos0sin010sin0cos),(ijiYRjZiZiXjYiYθθjXjZiZiXjYiYθθjX⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=1000cossin0sincos),(θθθθθijiZRjZiZiXjYiYθθjX转动矩阵的特点：转动矩阵的特点：(1) 主对角线上有一个元素为1，其余均为转角的余弦/正弦；(2) 绕轴转动的次序与元素1所在的行、列号对应；(3) 元素1所在的行、列，其它元素均为0；(4) 从元素1所在行起，自上而下，先出现的正弦为负，后出现的为正，反之依然。

⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=1000cossin0sincos),(θθθθθijiZR⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=θθθθθcossin0sincos0001),(ijiXR⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=θθθθθcos0sin010sin0cos),(ijiYR►绕多个坐标轴旋转的转动矩阵►绕多个坐标轴旋转的转动矩阵1）、绕固定坐标系旋转),(αiXR),(θiZR),(),(),(αθθαXRZRRji=),,(iiiZYX坐标系⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=αααθαθθαθαθθααααθθθθθαcossin0sincoscoscossinsinsincossincoscossin0sincos00011000cossin0sincos),(Rji),,(mmmZYX坐标系),,(jjjZYX坐标系iZiXjYiYθαjXαθmZmXmYθjZ2）、绕运动坐标系旋转),(ϕiZR),(1θYR),(2φZR),(),(),(),,(φθϕφθϕZRYRZRRji=ZYZ欧拉角),,(iiiZYX坐标系),,(111ZYX坐标系),,(222ZYX坐标系),,(jjjZYX坐标系iX1XjX2XiYjY1Y2Y)(1ZZijZ2Zϕϕθθθφφφϕφθ注意注意：：多个旋转矩阵连乘时，次序不同则含义不同。

1）绕新的动坐标轴依次转动时，每个旋转矩阵要从左往右左往右乘，即旋转矩阵的相乘顺序与转动次序相同；2）绕旧的固定坐标轴依次转动时，每个旋转矩阵要从右往左右往左乘，即旋转矩阵的相乘顺序与转动次序相反⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+−+−−−=θφθφθθϕφϕφθϕφϕφθϕθϕφϕφθϕφϕφθϕcossinsinsinsinsinsincoscossincossinsincoscoscossinsincoscossinsincoscossinsincoscoscos⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=1000cossin0sincoscos0sin010sin0cos1000cossin0sincos),,(φφφφθθθθϕϕϕϕφθϕRji证明：iX1XjX2XiYjY1Y2Y)(1ZZijZ2Zϕϕθθθφφφϕφθ),,(iiiZYX坐标系),,(111ZYX坐标系),,(222ZYX坐标系),,(jjjZYX坐标系jiiiiijjjPZRYRZRPYRZRPZRPRPPYRPRPPZRPRP),(),(),(),(),(),()3),()2),() 12121111212211222φγϕ=γϕ=ϕ=⋅=γ=⋅=φ=⋅=1）绕运动坐标系旋转),(ϕiZR),(1θYR),(2φZR2）、绕固定坐标系旋转),,(iiiZYX坐标系),,(mmmZYX坐标系),,(jjjZYX坐标系iZiXjYiYθαjXαθmZmXmYθjZ),(θiZ),(αiXjiimimmiijijjmmPZRXRPXRPRPPZRPRP),(),(),()2),() 1θα=α=⋅=θ=⋅=适用的机器人类型举例（有旋转关节）Y6X6Z6Y0X0Z0Y7X7Z73、坐标变换综合（平移、坐标变换综合（平移+旋转）旋转）PRPPOjijjii+=旋转部分平移部分推导(中间坐标系C)：iZiXiYOiZjXjYjPjPOjiPPOjicZcXcYI（旋转）: c与j 原点重合，c与i姿态相同jjijjccRPRPP==II（平移）: c与i 原点重合PRPPPPOjijjiOcici+=+=问题：是否可以先平移后旋转？iZiXiYOiZjXjYjPjPOjiPPOjicZcXcY推导(中间坐标系C)：I（平移）: c与i原点重合，c与j姿态相同jOijOccPPPPPjj+≠+=II（旋转）: c与i 姿态相同jjiOijjiOccijOcjicciiRPPRPPRPPRRPPjjj+=+=+==)(未知例例1: 已知坐标系已知坐标系B初始位姿与初始位姿与A重合重合,首先首先B相对于坐标系相对于坐标系A的的Z轴转轴转30度度,再沿再沿A的的X轴移动轴移动10个单位个单位,并沿并沿A的的Y轴移动轴移动5个单位个单位.假设点假设点P在 坐标系在 坐标系B的描述为的描述为PB={3,7,0}T,求它在坐标系求它在坐标系A中的描述中的描述PA.⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=++=+=0562.12098. 90500010073100030cos30sin030sin30cos)5 ,()10,()30,(ooooAAAOBABAAYPXPPZRPRPPBBAZAXAYOiPOjBZBXBY⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=ωzyxPωxa =ωyb =ωzc =1、齐次坐标、齐次坐标⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=cbaP2-5 齐次坐标与齐次变换齐次坐标与齐次变换0≠ω齐次坐标直角坐标1）点的齐次坐标：[][][]TTTPPzyxP2864,1432===ω非零的比例因子2）坐标轴方向的齐次坐标：[]Tcba0[]T0001X轴：a,b,c称为方向数Y轴：Z轴：[]T0010[]T0100[][]TT1000,0000坐标原点无意义点jXiZiXiYPOjiOiPjYjZOj2、齐次变换、齐次变换441000×⎥⎦⎤⎢⎣⎡=PRTOjjijii141×⎥⎦⎤⎢⎣⎡iP141×⎥⎦⎤⎢⎣⎡jP旋转矩阵与平移向量构成的齐次变换矩阵：点P在i坐标系中的齐次坐标：点P在j坐标系中的齐次坐标：⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡+=⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡11110001iOjijjijOjijijjiPPRPPPRPTiP齐次变换矩阵Tji的含义：表示了坐标系j相对于坐标系i的位姿⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡−=1000401030011100TjiiXiYiZjXjYjZPOjijOiOjXjYjZPjOijjiiTPP=►旋转的齐次变换►旋转的齐次变换►平移加旋转变换►平移加旋转变换⎥⎦⎤⎢⎣⎡=10000),(RKRjiθ⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡==×1000100001000),()(33PRRPIKRPTransTjijOijijiOjOijiθ►平移的齐次变换►平移的齐次变换⎥⎦⎤⎢⎣⎡=1000)(3*3PIPTransOjiOji例例2: 已知坐标系已知坐标系B初始位姿与初始位姿与A重合重合,首先首先B相对于坐标系相对于坐标系A的的Z轴转轴转30度度,再沿再沿A的的X轴移动轴移动10个单位个单位,并沿并沿A的的Y轴移动轴移动5个单位个单位.假设点假设点P在 坐标系在 坐标系B的描述为的描述为PB={3,7,0}T,求它在坐标系求它在坐标系A中的描述中的描述PA.⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡−⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡===10562.12098. 91073100001000030cos30sin0030sin30cos10510010000100001)(),(BBPZRYXTransTPPBAA⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡−===10562.12098. 910731051001000030cos30sin0030sin30cosBTPPBAA解法1:解法2:练习题1：已知坐标系A初始位姿与B重合,首先A相对于坐标系B的Z轴转30度,再沿B的X轴移动10个单位, 再相对于A的Y轴转60度，并沿A的Z轴移动5个单位. 假设点P在坐标系A的描述为=[12,0,4]T,求它在坐标系B中的描述；APBP⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡−==⋅=1892. 5897. 6946.21)15,()60,()30,()10,(21ZTransYRZRXTransTTPooB解法1:11.62-6 机器人姿态的其他表示方法机器人姿态的其他表示方法⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡∠∠∠∠∠∠∠∠∠==×)'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos(][33''''ZZZZZXYZYYYXXZXYXXZYXROOOOOOOO前面：用前面：用3*3的旋转矩阵表示姿态的旋转矩阵表示姿态单位主矢量?旋转矩阵有9个元素，6个约束条件，3个独立元素；?计算编程时需要输入9个元素，不方便；?一般采用3个元素来表示。

1、RPY角：（绕固定坐标轴X-Y-Z旋转）RPY角是描述船舶在海中航行时姿态的一种方法XYZαγβR（Z， ）：RollαR（Y， ）：PitchβR（X， ）：Yawγαγβ（1）B的方位描述如下：{A}和{B} 重合，首先，将{B}绕XA 转 角，再绕YA转角，最后绕ZA转角αβγ//////AAAZBYBXBBA→→→=),(),(),(),,(γβααβγAAAxyzBAXRYRZRR=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=γγγγββββααααcssccssccssc000010010010000⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−++−=γβγββγαγβαγαγβαβαγαγβαγαγβαβαccscssccssccssscssscsccsssccc⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=333231232221131211rrrrrrrrr⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−++−=γβγββγαγβαγαγβαβαγαγβαγαγβαβαccscssccssccssscssscsccsssccc⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡333231232221131211rrrrrrrrr（2）如果已知机器人的姿态矩阵，如何求RPY角？221211cosrr +=β),(2tan),(2tan),,(2tan, 0cos3332112122121131rrArrArrrA=+=+−=≠γαββ,90,sin,0cos±=βββ⇒=βsuchasrangeofif if ⎪⎪⎩⎪⎪⎨⎧⎩⎨⎧−==−=⎩⎨⎧===),(2tan0,90),(2tan0,9022122212rrArrAγαβγαβifif通常的选择通常选择oo9090≤β≤−2、ZYX欧拉角：（绕动坐标轴Z-Y-X旋转）γβα//////BBBXBYBZBBA→→→=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−++−=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−==γβγββγαγβαγαγβαβαγαγβαγαγβαβαγγγγββββααααγβαγβαccscssccssccssssssscsccsssccccssccssccsscXRYRZRRBBBzyxBA000010010010000),(),(),(),,(iX1XjX2XiYjY1Y2Y)(1ZZijZ2Zαγ⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−−++−=γβγββγαγβαγαγβαβαγαγβαγαγβαβαccscssccssccssscssscsccsssccc),(),(),(),,(γβααβγAAAxyzBAXRYRZRR=3、ZYZ欧拉角γβα//////BBBZBYBZBBA→→→=),(),(),(),,(γβαγβαBBBzyzBAZRYRZRR=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−+−+−−−=333231232221131211rrrrrrrrrcsscsssccscsscccssccssccsscccβγβγββαγαγβαγαγβαβαγαγβαγαγβα⎪⎩⎪⎨⎧−=+==≠),(2tan),(2tan),(2tan, 0sin3132332322311323rrArrrArrAifγβαβ⎪⎪⎩⎪⎪⎨⎧⎩⎨⎧−===⎩⎨⎧−====),(2tan0,180),(2tan0, 0, 0sin11121112rrArrAifγαβγαββ已知姿态矩阵求欧拉角•上述描述姿态的方法称为角度设定法，共有24种，其中12种RPY法和12种欧拉法，并且是对偶的，实际上只有12种不同的旋转矩阵。

•确定旋转时是绕固定轴还是动轴非常重要▲X Y Z的排列:1266/33331213=+=⋅+⋅=PCCCEULERRPYZXZXYZ2-7 旋转变换通式旋转变换通式1、旋转矩阵通式旋转矩阵通式：θiXiYiZjXjYjZOKθkkjkikKzyx++=旋转矩阵?),(=θKR1)、RKRji=θ),(坐标系j 相对于i的姿态2)、定义两个坐标系i’和j’，i与i’固连，j与j’固连；i’和j’的Z轴与矢量K重合，旋转前，i与j重合，i’与j’重合绕通过原点原点的任意单位矢量K转 角的旋转矩阵⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡==zzzyyyxxxzzzyyyxxxjjiikonkonkonaonaonaonRR''3)、iXiYiZjXjYjZOKθ' iZ' iX' iY' jX' jY绕K转 角θj’相对于i’的Z轴转 角θ' jZ旋转变换的尺寸链图：ii’jj’RRRKRRjjjiiiji''''),(=θ=1''),(),(−θ=θ=RZRRKRRjjiijiTjjiijiRZRRKRR''),(),(θ=θ=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θθθ−θ⋅⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=θzyxzyxzyxzzzyyyxxxkkkooonnnkonkonkonKR1000cossin0sincos),(★ 利用旋转矩阵的正交性质:onanaaoonaaoonn×==⋅=⋅=⋅=⋅=⋅=⋅01假设：)cos1 (cossinθ−=θθ=θθ=θverscs⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θ+θθ+θθ−θθ−θθ+θθ+θθ+θθ−θθ+θ=θcverskkskverskkskverskkskverskkcverskkskverskkskverskkskverskkcverskk),K(Rzzxzyyzxxyzyyzyxyxzzxyxx整理得：旋转变换通式0, 1===zyxkkk讨论：讨论：（1）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θθθ−θ=θcossin0sincos0001),(KR（2）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θθ−θθ=θcos0sin010sin0cos),(KR（3）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θθθ−θ=θ1000cossin0sincos),(KR0, 1===zxykkk0, 1===xyzkkk例：坐标系B原来与A重合，将坐标系B绕过原点O的轴线kjiKA313131++=转动o120=θ，求旋转矩阵)120,(oAKR解答：31===zyxkkk1）2）23120,23120sin,21120cos==−=ooovers3）带入旋转通式得：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=010001100)120,(oAKRo7 .54)31cos(a=2、等效转轴与等效转角、等效转轴与等效转角转轴和转角转轴和转角旋转矩阵旋转矩阵12？⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡zzzyyyxxxaonaonaon),(θKR=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡θ+θθ+θθ−θθ−θθ+θθ+θθ+θθ−θθ+θcverskkskverskkskverskkskverskkcverskkskverskkskverskkskverskkcverskkzzxzyyzxxyzyyzyxyxzzxyxx⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡zzzyyyxxxaonaonaon1）将方程两边矩阵的主对角线元素分别相加，则θ+=θ+θ++=++ccverskkkaonzyxzyx213)(222) 1(21cos−++=θzyxaon2）将方程两边矩阵的非对角线元素成对相减得：θ=−θ=−θ=−sin2sin2sin2zxyyzxxyzkonknakao将上式两边平方相加得：) 1()()()(tan)()()(21sinsin4)()()(2222222222−++−+−+−±=θ−+−+−±=θθ=−+−+−zyxxyzxyzxyzxyzxyzxyzaononnaaoonnaaoonnaao求得转角θ−=θ−=θ−=sin2,sin2,sin2xyzzxyyzxonknakaok求得转轴★注意注意：1）多值性：。

一般取（是合理解都有任意（的值不唯一例如对于和o1800)360,(),),(),≤θ≤×+θ≡θθ−−θθonKKKKK2）存在病态情况：殊的解法转轴不确定，需要特分母都很小，或和度时，由于上式的分子或的值接近当0sin1800≈θθ) 1()()()(tan222−++−+−+−±=θzyxxyzxyzaononnaao例：和等效转角的等效转轴求复合旋转矩阵θ=K)90,()90,(ZRYRRBA解：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−⋅⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−=010001100100001010001010100RBA3tan,23sin,21cos−=θ=θ−=θ利用前面的公式可求得：kjiKkkkzyx31313131++====)90,()90,()120,(ZRYRKR=任何一组绕过原点的轴线的复合转动任何一组绕过原点的轴线的复合转动总等效于绕过原点的某一矢量的转动总等效于绕过原点的某一矢量的转动3、齐次变换通式、齐次变换通式*讨论矢量讨论矢量K不通过原点的情况不通过原点的情况kkjkikKzyx++=假设单位矢量通过点kpjpipPzyx++=求绕矢量K转角的齐次变换矩阵θ1)、定义两个坐标系i’和j’，坐标原点在P点，i与i’固连，j与j’固连；i’和j’的坐标轴分别与i和j的坐标轴平行，旋转前，i与i’重合，j与j’重合。

iXiYiZjXjYjZOKθP' iZ' iX' iY' jX' jY' jZ2）旋转变换的尺寸链图：ii’jj’TTTTjjjiiiji''''=⎥⎦⎤⎢⎣⎡==××10PI)P(TransT3133' iiTjiTii'Tji''Tjj'⎥⎦⎤⎢⎣⎡−=−==××−10)(31331''PIPTransTTjjjj⎥⎦⎤⎢⎣⎡θ=θ=××100),(),(3113''KRKRotTji⎥⎦⎤⎢⎣⎡+θ−θ=−θ=×10),(),()(),()(31PPKRKRPTransKRotPTransTji例：坐标系B原来与A重合，将坐标系B绕过点P的矢量kjiKA313131++=转动o120=θ解答：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=010001100)120,(KR1）[]TAP321=，求旋转矩阵该矢量经过点2）带入旋转通式得：⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡−=θ=1000101010012100),(KTTBA反之，如果求P点，其值不唯一2-8、自由矢量的变换、自由矢量的变换前面讨论的是位置矢量的变换，如旋转、平移等；如何处理速度矢量速度矢量、力矢量力矢量的变换问题？矢量分类：矢量分类：1、自由矢量自由矢量：由维数、大小、方向三要素规定，如速度矢量、纯力矩矢量；2、线矢量：线矢量：由维数、大小、方向、作用线四要素规定，如力矢量。

对于自由矢量对于自由矢量：在不同坐标系的描述只与旋转矩阵有关，与坐标原点的位置无关，对于速度矢量对于力矩矢量BBAAVRV⋅=BBAAmRm⋅=2-9、总结、总结1、变换矩阵T的物理含义1）、坐标系的描述坐标系的描述：如坐标系B相对于A的位姿；2）、同一点在不同坐标系同一点在不同坐标系A和和B间的影射关系间的影射关系；3）、运动算子：运动算子：表示同一坐标系中点运动前后的算子关系，如平移算子、旋转算子⎥⎦⎤⎢⎣⎡=10PRTBoABABA2、变换矩阵T的相乘★矩阵相乘的顺序一般不可换，特殊可换的情况为变换都是同参考系下的平移或绕同一坐标轴的旋转3、变换矩阵求逆例：两坐标系A和B，B先绕A的Z轴转30度，再沿X轴移动4个单位，沿Y轴移动3个单位，已知，求[]TAP3 , 2 , 1=BP解：ABAAABBBBAAPTPTPPTP⋅=⋅=⋅=−1;1）⎥⎦⎤⎢⎣⎡⋅−=⋅=−−==⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡=10)(1010PRRTPRPRPRRPRTPRTBoATBATBAABBoATBABoAABAoBTBAABAoBABABBoABABA2）)0 , 3, 4()30,()]30,()0 , 3 , 4([11−−−===−−TransZRotZRotTransTTBAAB。