《机器人运动学》课件-第三章数学基础

机器人及其控制

数学基础

基本问题如何描述机器人与环境之间的位置关系？数学基础数学基础3.1位置与姿态的描述3.2坐标变换3.3齐次坐标与齐次变换3.4齐次矩阵的运算3.5变换方程3.6旋转变换通式3.7欧拉角位置的表示3.1位置和姿态的描述在直角坐标系A中,空间任意一点p的位置(Position)可用3×1列向量(位置矢量)表示:姿态的表示3.1位置和姿态的描述空间物体B的方位(Orientation)可由某个固接于此物体的坐标系{B}的三个单位主矢量[xB,yB,zB]相对于参考坐标系A的方向余弦组成的3×3矩阵描述.x{A}yzxyz{B}{B}{A}xx表示刚体{B}

相对于{A}的方位旋转矩阵的性质3.1位置和姿态的描述注意：为单位矢量3×3旋转矩阵有9个元素，6个约束条件，3个独立变量称为旋转矩阵，上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。是正交矩阵，且满足旋转矩阵的逆等于其转置矩阵例2.1

绕一个轴的旋转3.1位置和姿态的描述

例2.1

绕一个轴的旋转如何表示绕一般轴的旋转？3.1位置和姿态的描述

为了完全描述刚体B在空间的位姿(位置和姿态)、通常将物体B与某一坐标系{B}相固接。相对参考系{A}，坐标系{B}的原点位置和坐标轴方位，分别由位置矢量(PositionVector)和旋转矩阵(RotationMatrix)描述。这样，刚体的位姿（位置和姿态）可由坐标系{B}来描述，即位姿的表示3.1位置和姿态的描述例：手爪坐标系3.1位置和姿态的描述z-轴:接近矢量(approachingobjectdirection)y-轴:方位矢量(alongtheorientationofthelineconnectingtwofingers)

x-轴:法向矢量n=o

a手爪的方位:手爪的位姿:平移坐标变换3.2坐标变换在机器人学的许多问题中，涉及到以不同坐标系表示同一量。下面讨论从一个坐标系的描述到另个坐标系的描述之间的变换关系。旋转坐标变换3.2坐标变换同一点p在两个坐标系{A}和{B}中的描述具有以下变换关系：复合坐标变换3.2坐标变换例

已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。求位置矢量ApB0和旋转矩阵。假设点p在坐标系{B}的描述为Bp=[3,7,0]T，求它在坐标系{A}中的描述Ap。解：3.2坐标变换解：3.2坐标变换例

已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。求位置矢量ApB0和旋转矩阵。假设点p在坐标系{B}的描述为Bp=[3,7,0]T，求它在坐标系{A}中的描述Ap。解：3.2坐标变换例

已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。求位置矢量ApB0和旋转矩阵。假设点p在坐标系{B}的描述为Bp=[3,7,0]T，求它在坐标系{A}中的描述Ap。所谓齐次坐标就是将一个原本是n

维的向量用一个n+1维向量来表示。三维空间的直角坐标的齐次坐标表示其中s为比例因子，有

则(x，y，z，s)T称为直角坐标点(px,py,pz)T的齐次坐标。齐次坐标(HomogeneousCoordinates)3.3齐次坐标与齐次变换当s≠0当s=03.3齐次坐标与齐次变换齐次坐标(HomogeneousCoordinates)简记为齐次坐标表示坐标变换3.3齐次坐标与齐次变换解：3.3齐次坐标与齐次变换例2.3已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。假设点p在坐标系{B}的描述为Bp=[3,7,0]T，用齐次变换方法求它在坐标系{A}中的描述Ap。例2.3已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。假设点p在坐标系{B}的描述为Bp=[3,7,0]T，用齐次变换方法求它在坐标系{A}中的描述Ap。解：3.3齐次坐标与齐次变换齐次变换矩阵也代表坐标平移与坐标旋转的复合其中，I3×3是3×3阶单位矩阵，等式右端第一个矩阵称为平移变换矩阵，常用Trans(ApBo)来表示；第二个矩阵标为旋转变换矩阵,常用Rot(k,

)来表示.将其分解成两个矩阵相乘的形式之后就可以看出这一点。3.3齐次坐标与齐次变换空间中的某点用矢量ai+bj+ck描述，该点也可表示为：对已知矢量u=[x,y,z,1]T进行平移变换所得的矢量v

为：平移的齐次坐标3.3齐次坐标与齐次变换旋转的齐次坐标3.3齐次坐标与齐次变换例2.4

已知点u=7i+3j+2k，将u绕z轴旋转90°得到点v，再将点v绕y轴旋转90°得到点w，求点v、w的坐标。解：3.3齐次坐标与齐次变换如果把上述两变换组合在一起3.3齐次坐标与齐次变换例2.4

已知点u=7i+3j+2k，将u绕z轴旋转90°得到点v，再将点v绕y轴旋转90°得到点w，求点v、w的坐标。解：若改变旋转次序，首先使u

绕y

轴旋转90°，再绕z

轴旋转90°，会使u变换至与w不同的位置w1。3.3齐次坐标与齐次变换例2.5

已知点u=7i+3j+2k，将u绕z轴旋转90°得到点v，再将点v绕y轴旋转90°得到点w，最后进行平移变换4i-3j+7k，求最终的坐标。解：把上述三变换组合在一起3.3齐次坐标与齐次变换则3.3齐次坐标与齐次变换例2.5

已知点u=7i+3j+2k，将u绕z轴旋转90°得到点v，再将点v绕y轴旋转90°得到点w，最后进行平移变换4i-3j+7k，求最终的坐标。解：1.坐标系的描述描述｛B｝相对于参考系｛A｝的位姿2.坐标映射表示同一点P在两个坐标系｛A｝和｛B｝中描述之间的映射关系

3.运动算子T表示在同一坐标系中，点P运动前后的算子关系。3.3齐次坐标与齐次变换齐次变换矩阵T具有以下不同的物理解释：Ap2=T

Ap13.4齐次矩阵的运算

给定坐标系{A}，{B}和{C}，若已知{B}相对{A}的描述为，{C}相对{B}的描述为，则复合变换：齐次矩阵相乘

3.4齐次矩阵的运算齐次矩阵相乘

变换矩阵相乘不满足交换率3.4齐次矩阵的运算3.4齐次矩阵的运算齐次矩阵相乘

变换矩阵的左乘和右乘的运动解释是不同的：变换顺序“从右向左”，指明运动是相对固定坐标系而言的；变换顺序“从左向右”，指明运动是相对运动坐标系而言的。物理意义：坐标系的映射变换坐标系的运动例:点(7,3,2)在固定坐标系中按以下序列运动1.Rot(z,90)2.Rot(y,90)3.Trans(4,-3,7)3.4齐次矩阵的运算Trans(4,-3,7)Rot(y,90)Rot(z,90)3.4齐次矩阵的运算3.4齐次矩阵的运算

从坐标系{B}相对{A}的描述，求得坐标系{A}相对{B}的描述，是齐次变换求逆问题。对于给定的，求解，等价于给定和计算和。3.4齐次矩阵的运算齐次矩阵求逆对于给定的，求解，等价于给定和计算和。3.4齐次矩阵的运算齐次矩阵求逆3.4齐次矩阵的运算齐次矩阵求逆已知表示{B}相对于{A}绕其z轴转30度，再沿x轴移动4,沿y轴移动3。求.3.4齐次矩阵的运算齐次变换方程3.5变换方程给定变换:确定

绕任意轴/角的转动前面讨论了旋转矩阵的三种特殊情况，即绕x,y和z轴的旋转矩阵，现在讨论绕过原点的任意轴k旋转θ角的变换矩阵。3.6旋转变换通式给出一个变换矩阵C，它绕任意向量k旋转，我们把k当作C坐标系的z轴单位向量。

nxoxax0nyoyay0C=nzoz

az00001

k=axi+ayj+azk

绕k旋转就相等于绕C坐标系的z轴旋转。

Rot（k，θ）=Rot（Cz，θ）（2.23）

kTzzyyxx00图2.16一般性旋转变换C如果我们给一个坐标系T，它在参考坐标系里被描述，它在C坐标系里用X描述，这样

T=CX其中X描述T相对C的位姿，求X，我们得到

X=C-1TT绕k旋转就等于Ｘ绕Ｃ坐标系的z轴旋转Rot（k,θ）Ｔ＝CRot（z,θ）XRot（k,θ）Ｔ＝CRo

t（z,θ）C-1T

Rot（k,

θ）＝CRot（z,θ）C-1kTzzyyxxx00图2.16一般性旋转变换C3.6旋转变换通式绕任意轴/角的转动绕任意轴/角的转动表示坐标系B相对参考系A方位

Rot（k,

θ）＝CRot（z,θ）C-13.6旋转变换通式{B}{A}k{A}k{A’}ZZ’Zk{A’}Z’Z’{B’}{B’}{B}k

3.6旋转变换通式展开式（2.28），我们发现CRot（z,θ）C-1仅是k的函数。用C-1右乘Rot（z,θ），我们得到

cosθ-sinθ00nxnynz0sinθcosθ00oxoyoz0Rot（z,θ）C-1＝0010axayaz00001

0001

nxcosθ－oxsinθnycosθ―oysinθnzcosθ―ozsinθ0nxcosθ+oxsinθnycosθ+oysinθnzcosθ+ozsinθ0=ax

ayaz0（2.29）0001再用C左乘

nxoxax0nyoyay0C=nzozaz0（2.30）0001计算3.6旋转变换通式得到CRot（z,θ）C-1=nxnxcosθ―nxoxsinθ+nxoxsinθ+oxoxcosθ+axaxnynxcosθ―nyoxsinθ+nxoysinθ+oyoxcosθ+ayaxnznxcosθ―nzoxsinθ+nxozsinθ+ozoxcosθ+azax

0nxnycosθ―nxoysinθ+nyoxsinθ+oyoxcosθ+axaynynycosθ―nyoysinθ+nyoysinθ+oyoycosθ+ayaynznycosθ―nzoysinθ+nyozsinθ+oyozcosθ+azay0

nxnzcosθ―nxozsinθ+nzoxsinθ+ozoxcosθ+axaz0nynzcosθ―nyozsinθ+nzoysinθ+ozoycosθ+ayaz0nznzcosθ―nzozsinθ+nzozsinθ+ozozcosθ+azaz0013.6旋转变换通式应用下列关系进行简化：C坐标系任意的行或列与其他行或列的点积为零，因为这些向量是正交的；C坐标系任意的行或列与其自身的点积为I，因为它们是单位量；z向量是x和y向量的叉积：a=n×o，它有下列分量ax=nyoz―nzoyay=nzox―nxozaz=nxoy―nyox可得到简化式为

Rot(k,θ)=kxkxVersθ+cosθkykxVersθ―kzsinθkzkxVersθ+kysinθ0kxkyVersθ+kzsinθkykyVersθ+cosθkzkyVersθ―kzxsinθ0kxkzVersθ―kysinθkykzVersθ+kxsinθkzkzVersθ+cosθ00

0

01其中Versθ=（1―cosθ），上式是一般性的旋转变换公式。3.6旋转变换通式

给出任一旋转变换，求进行等效旋转θ角的转轴k

旋转矩阵

等效转轴与等效转角3.6旋转变换通式若已知旋转变换：令怎么计算？将对角线各项相加得到nx+oy+az+1=k2xVersθ+cosθ+k2yVersθ+cosθ+k2zVersθ+cosθ+1

nx+oy+az=(k2x+k2y+k2z)Versθ+3cosθ=1+2cosθ由此可得到旋转角的余弦是

cosθ=1/2（nx+oy+az―1）对非对角线项相减，我们得到oz―ay=2kxsinθax―nz=2kysinθny―ox=2kzsinθ把以上三式两边平方并相加有

（oz―ay）2+（ax―nz）2+（ny―ox）2=4sin2θ旋转矩阵

等效转轴与等效转角3.6旋转变换通式sinθ的表达式sinθ=±1/2√（oz―ay）2+（ax―nz）2+（ny―ox）2

规定：绕k正方向旋转，当0≤θ≤180°时，在上式中取正。这个旋转角θ被唯一定义为tanθ=√（oz―ay）2+（ax―nz）2+（ny―ox）2/（nx+oy+az―1）k的各分量为