机器人的位姿描述和坐标变换

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例：。和等效转角的等效