3、机器人的位姿描述与坐标变换

《机器人学》战强北京航空航天大学机器人研究所第三章机器人的位姿描述与坐标变换第三章机器人的位姿描述与坐标变换机器人的位姿连杆I的位姿YXZYiXiZiYwXwZw3-1刚体位姿的数学描述000'zyxPoo)'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos(][33''''ZZZYZXYZYYYXXZXYXXZYXROOOOOOOO刚体位置：刚体姿态：单位主矢量￥￥假设机器人的连杆和关节都是刚体￥￥OXYZZ'Y'X'O'bnt1''1'RRROOTOOOO9个元素，只有3个独立，满足6个约束条件:☺☺0...1...''''''''''''XZZYYXZZYYXXOOOOOOOOOOOOOOOOOOOOOOOO},{}'{''PROOOOO刚体的位置和姿态：R是单位正交阵姿态矩阵R的特点：)'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos()'cos(][33''''ZZZYZXYZYYYXXZXYXXZYXROOOOOOOOjZjXjYiZiXiYOiOj例：某刚体j在参考系i中的位置姿态?jiOOR106?jiooP3-2坐标变换（点的映射）1、坐标平移（坐标系方位相同）iOjjiPPPjZjXjYPOjiPiZiXiYOiOj沿着不同轴向的组合平移：zyxzyxPOji000000POOOPOjjii已知点P在j坐标系的坐标，平移j至i，求点P在i坐标系的坐标。Y1X1Z1Y2X2Z2Y3X3Z3三坐标的直角坐标机器人适用的机器人类型举例（有平移关节）YXZiZiXiYOijZjXjYPOj15例:TjP765已知求P点在i坐标系中的坐标。TTTOjijiPPP72150150765解答：2、坐标旋转（坐标系原点相同）ZiXiYiZjXjYjP坐标系j由坐标系i旋转而成TiiiizyxP][TjjjjzyxP][求点P在i坐标系的坐标：已知点P在j坐标系的坐标：ZiZjXiXjYiYjPjxjyjzcos(,)ijijyxYXixiyizcos(,)cos(,)ijijjijyxYXyYYcos(,)cos(,)cos(,)ijijjijjijyxYXyYYzYZ(,)ijYX关于？☺),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(),cos(jijjijjijijijjijjijijijjijjijiiZZzYZyXZxzZYzYYyXYxyZXzYXyXXxxPZiZjXiXjYiYjPjxjyjzixiyiz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('ZZZYZXYZYYYXXZXYXXROOPRPjjii坐标系j相对于i的方位TijijjiRRR1旋转矩阵的性质：旋转矩阵►绕一个坐标轴旋转的转动矩阵jZiZiXjYiYqqjXjZiZiXjYiYqqjXjZiZiXjYiYqqjX1）RX2）RY3）RZqqqqqcossin0sincos0001),(ijiXRjZiZiXjYiYqqjXcos(,)cos(,)cos(,)cos(,)cos(,)cos(,)cos(,)cos(,)cos(,)ijijijjiijijijijijijXXXYXZRYXYYYZZXZYZZ1000cossin0sincos),(qqqqqijiZRqqqqqcos0sin010sin0cos),(ijiYRjZiZiXjYiYqqjXjZiZiXjYiYqqjX转动矩阵的特点：(1)主对角线上有一个元素为1，其余均为转角的余弦/正弦；(2)绕坐标轴转动的次序与元素1所在的行、列号对应；(3)元素1所在的行、列，其它元素均为0；(4)从元素1所在行起，自上而下，先出现的正弦为负，后出现的为正，反之依然。1000cossin0sincos),(qqqqqijiZRqqqqqcossin0sincos0001),(ijiXRqqqqqcos0sin010sin0cos),(ijiYR►绕多个坐标轴旋转的转动矩阵1）、绕固定坐标系旋转),(iXR),(qiZR(,)?jiRqqqqqqqqqqqqcossin0sincoscoscossinsinsincossincoscossin0sincos00011000cossin0sincos),(Rji),,(iiiZYX坐标系),,(mmmZYX坐标系),,(jjjZYX坐标系iZiXjYiYqjXqmZmXmYqjZ),(),(),(qqXRZRRji2）、绕运动坐标系旋转),(iZR),(1qYR),(2ZR),(),(),(),,(qqZRYRZRRjiZYZ欧拉角),,(iiiZYX坐标系),,(111ZYX坐标系),,(222ZYX坐标系),,(jjjZYX坐标系iX1XjX2XiYjY1Y2（Y))(1ZZijZ2Zqqq注意：多个旋转矩阵连乘时，次序不同则含义不同。1）绕新的动坐标轴依次转动时，每个旋转矩阵要从左往右乘，即旋转矩阵的相乘顺序与转动次序相同；2）绕旧的固定坐标轴依次转动时，每个旋转矩阵要从右往左乘，即旋转矩阵的相乘顺序与转动次序相反。qqqqqqqqqcossinsinsinsinsinsincoscossincossinsincoscoscossinsincoscossinsincoscossinsincoscoscos1000cossin0sincoscos0sin010sin0cos1000cossin0sincos),,(qqqqqRji证明：),,(iiiZYX坐标系),,(111ZYX坐标系),,(222ZYX坐标系),,(jjjZYX坐标系jiiiiijjjPZRYRZRPYRZRPZRPRPPYRPRPPZRPRP),(),(),(),(),(),()3),()2),()12121111212211222qqq1）绕运动坐标系旋转),(iZR),(1qYR),(2ZRiX1XjX2XiYjY1Y2（Y))(1ZZijZ2Zqqq?jiR2）、绕固定坐标系旋转),,(iiiZYX坐标系),,(mmmZYX坐标系),,(jjjZYX坐标系iZiXjYiYqjXqmZmXmYqjZ),(qiZ),(iXjiimimmiijijjmmPZRXRPXRPRPPZRPRP),(),(),()2),()1qq证明与讨论:?jiR适用的机器人类型举例（有旋转关节）例1:已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z轴转30度,假设点P在坐标系B的描述为PB={3,7,0}T,求它在坐标系A中的描述PA.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)(未知jOccjPPP例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3-3齐次坐标与齐次变换1、齐次坐标cbaP0齐次坐标直角坐标1）点的齐次坐标：TTTPPzyxP2864,1432非零的比例因子2）坐标轴方向的齐次坐标：Tcba0T0001X轴：a,b,c称为方向数Y轴：Z轴：T0010T0100TT1000,0000坐标原点无意义点AZAXAYOijXiZiXiYPOjiOiPjYjZOj2、齐次变换441000PRTOjjijii141iP点P在i坐标系中的齐次坐标：点P在j坐标系中的齐次坐标：141jP旋转矩阵与平移向量构成的齐次变换矩阵：11100011jOjjOjijjjiiijiiPPPRPRPPTiP齐次变换矩阵Tji表示了坐标系j相对于坐标系i的位姿的含义：1000401030011100TjiiXiYiZjXjYjZPOjijOiOjXjYjZPjOijjiiTPP134jOip例：写出齐次转换矩阵jiT（2）纯旋转的齐次变换（3）平移加旋转变换10000),(RKRjiq1000100001000),()(33PRRPIKRPTransTjijOijijiOjOijiq（1）纯平移的齐次变换1000)(3*3PIPTransOjiOji*几种特例：1000100001000),()(33PRRPIKRPTransTjijOijijiOjOijiqiZiXiYOiZjXjYjPjPOjiPPOjicZcXcYiZiXiYOiZjXjYjPjPOjiPPOjicZcXcY同样的公式不同的含义{i}-{c}-{j}{i}-{c}-{j}静系动系例2:已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z轴转30度,再沿A的X轴移动10个单位,并沿A的Y轴移动5个单位.假设点P在坐标系B的描述为PB={3,7,0}T,求它在坐标系A中的描述PA.