2-位姿描述和齐次变换-熊有伦机器人技术基础

2.位姿描述和齐次变换ENTER位姿描述与齐次变换刚体位姿的描述1坐标变换2齐次坐标系和齐次变换3齐次变换矩阵的运算4变换方程52.1刚体位姿的描述坐标描述：指坐标系的位置和方位（姿态）位置的描述方位的描述旋转矩阵的正交性坐标系位姿的描述2.1.1位置的描述xAyzpPppAP{}AAZAXAYAO在直角坐标系A中,空间任意一点p的位置(Position)可用3x1列向量(位置矢量)表示:2.1.2方位的描述ABRnoa{}AAZAXAY()ABOO{}BBZBXBYnoa111213212223313233AAABBBXYZrrrrrrrrr空间物体B的方位(Orientation)可由某个固接于此物体的坐标系{B}的三个单位主矢量[xB,yB,zB]相对于参考坐标系A的方向余弦组成的3x3矩阵描述.ˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆBABABABABABABABABAXXYXZXXYYYZYXZYZZZ2.1.4旋转矩阵的意义若坐标系B可由坐标系A,通过绕A的某一坐标轴获得,则绕x,y,z三轴的旋转矩阵分别为10000),(00100),(00001),(cssczcsscycsscxRRR旋转矩阵的几何意义:1)可以表示固定于刚体上的坐标系{B}对参考坐标系的姿态矩阵.2)可作为坐标变换矩阵.它使得坐标系{B}中的点的坐标变换成{A}中点的坐标.3)可作为算子,将{B}中的矢量或物体变换到{A}中.RABRABRABpBpA2.1.3旋转矩阵R的正交性1ABBTBAARRRBTAAAAABTBBBBABTAXXYZYZR上述矩阵称为旋转矩阵,它是正交的ˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆBABABAABBABABABABABAXXYXZXXYYYZYXZYZZZR{}AAZAXAY()ABOO{}BBZBXBYnoa2.1.5坐标系位姿的描述{}{,}AABBORGBRPBO{}BBZBXBYnoaABORGP{}AAZAXAYAO刚体位姿(即位置和姿态),用刚体的方位矩阵和方位参考坐标的原点位置矢量表示2.2坐标变换坐标变换（映射mappings）：将在一个坐标系下的描述改变为在另一个坐标系下的描述。平移映射旋转映射一般映射一般映射的齐次坐标表达方法2.2.1平移映射AP{}AAZAXAYAOBP{}BBZBXBYBOABORGPABABORGPPP注意：只有在两坐标系平行的时候，才能将两坐标系的位置描述直接相加。坐标系{A}和{B}具有相同的方位,但原点不重合.则点P在两个坐标系中的位置矢量满足下式2.2.2旋转映射{}AAZAXAY()ABOO{}BBZBXBYBPAP,.ABBxAABByAABBzApXPpYPpZPAABBPPRBTAAAAABTBBBBABTAXXYZYZR坐标系{A}和{B}有相同的原点但方位不同,则点P的在两个坐标系中的位置矢量有如下关系:2.2.3一般映射BO{}BBZBXBYABORGP{}AAZAXAYAOAPBPAABABBORGPRPP一般情况原点既不重合,方位也不同。这时有:2.3平移与旋转的齐次变换矩阵平移旋转绕Z轴旋转绕X轴旋转绕Y轴旋转绕多个轴的旋转复合运动：多次平移与旋转齐次坐标解释[0,0,0,n]T—坐标原点矢量的齐次坐标，n为任意非零比例系数[1000]T—指向无穷远处的OX轴[0100]T—指向无穷远处的OY轴[0010]T—指向无穷远处的OZ轴这样，利用齐次坐标不仅可以规定点的位置，还可以用来规定矢量的方向。第四个元素非零时，代表点的位置；第四个元素为零时，代表方向。空间某点的直角坐标和齐次坐标分别为：1xxxyypypzzz和设:P＝（x,y,z)T;U＝P0=(x0,y0,z0)T则:V＝P＋P0＝（x+x0,y+y0,z+z0)T可用齐次坐标表示为：1100010001000111000000321zyxzyxzzyyxxvvv平移齐次变换矩阵:2.3.1平移ZVUPYX0000000100010(,,)00110001xytransxyzzIp02.3.2旋转P=R(Z,θ)P1；变换矩阵R(Z,θ)ｚiｙiｘiｏiｚjｙjｘjｏjθ绕z轴旋转θ角若空间有一点p，则其在坐标系{i}和坐标系{j}中的坐标分量之间就有以下关系：jijjijjizzyxyyxxcossinsincos若补齐所缺的有些项，再作适当变形，则有：jjjijjjijjjizyxzzyxyzyxx1000cossin0sincosjijjijjizzyxyyxxcossinsincos将上式写成矩阵的形式，则有：jjjiiizyxzyx1000cossin0sincosjjjijjjijjjizyxzzyxyzyxx1000cossin0sincos同理节目录章目录cs00sc00Rot(,)00100001Zc0s00100Rot(,)s0c00001Y10000cs0Rot(,)0sc00001X2.3.3一般映射的齐次坐标表达方法AABABBORGPRPP1、将1加到3维坐标的第4维，原坐标向量变成4×1的齐次坐标向量；2、将[0001]加到[RP]的最下面一行，构成4×4的齐次矩阵。1100011ABAABABBORGBPPRPPTAABBPTPP点在{A}和{B}中的位置矢量分别增广为:TBBBBTAAAAzyxzyx1,1PP0011101AABBORBABGABRPPPTP2.3.3一般映射的齐次坐标表达方法03300000100010001AAAAABBBBBRPIPRTAABABBORGPRPP例：如图所示，固连于连杆的坐标系{B}位于OB点，xb=2，yb=1，zb=0。在XOY平面内，坐标系{B}相对固定坐标系{A}的z轴有一个300的偏转，试写出表示连杆位姿的坐标系{B}的4×4矩阵表达式。cossin[]0001-sin02cos010010dnoaP手部的位姿表示0001TnoaPXXXXYYYYZZZZnoaPnoaPnoaPZB轴：接近矢量aYB轴：姿态矢量oXB轴：法向矢量n例：如图所示，手部抓握物体Q，物体是边长为2个单位的正立方体，写出表达该手部位姿的矩阵式。0001010110010011TnoaP变换方程为了描述机器人的操作，必须建立机器人本身各连杆之间、机器人与环境的运动关系，因此需要规定各种坐标系来描述机器人与环境的相对位姿关系。{}{}{}{}BTGS表示基座，表示工具，表示目标，表示工作站。{}{}{}{}{}{}BSSGGTTSBTGSTGT表示相对于的位姿表示相对于的位姿工具框相对于目标框的位姿直接影响操作效果，是机器人控制规划的目标BBSGTSGTTTTT变换方程•原始坐标系OAXAYAZA绕ＸＡ旋转α后得到坐标系O’X’Y’Z’；它再绕Y’旋转β后成O”X”Y”Z”；又绕Z”旋转γ成为OBXBYBZB连续绕运动坐标系旋转→连续右乘2.3.4算子左右乘规则(,)BPRotzP(,)PRotyP(,)(,)(,)(,)(,)(,)AAABAPRotxPRotxRotyPRotxRotyRotzP2.3.4算子左右乘规则用滚\仰\偏转(RPY)表示运动姿态偏转Yaw:绕X轴转.俯仰Pitch:绕Y轴转,横滚Roll:绕Z轴转,zxoyα（回转）（俯仰）（偏转）βγzxyoαβγγγββαα(,,)(,)(,)(,)RPYRotzRotyRotx连续绕固定坐标系旋转→连续左乘AAABABxyz的初始方位与参考系相重合，首先将绕转角，再绕转角，最后再绕转角1001100101020102;0012001200010001AAAA若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算子右乘。【例4】已知坐标系中点U的位置矢量u=[7321]T，将此点绕Z轴旋转90°，再绕Y轴旋转90°，求旋转变换后所得的点W。节目录章目录001010=(Y,90)(Z,90)=010100100001ototTRR平移变换和旋转变换可以组合在一个齐次变换中，称为复合变换。HH。H。节目录章目录点W若还要作4i – 3j + 7k的平移至E点，则只要左乘上平移变换算子，即可得到最后E点的列阵表达:Trans(4,3,7)Rot(,90)Rot(,90)YZEHUU7100400100103100030017010020001000117600141003340107210000111本节完动画2.3.5复合运动目标物齐次矩阵表示1）旋转前2）旋转后节目录章目录本节完楔块绕Z轴旋转-90°再绕X轴方向平移41）旋转前2）旋转后11111111002200220000221111111111Q44664466111111110000221111111111Q楔块绕Z轴旋转-90°再绕X轴方向平移4算子H=Trans(4,0,0)Rot(Z,-90◦)Q’=HQ==Trans(4,0,0)Rot(Z,-90◦)Q2.4齐次坐标的逆变换{B}相对于{A}:;{A}相对于{B}:;两者互为逆矩阵.求逆的办法:1.直接求2.简化方法BO{}BBZBXBYnoaABORGP{}AAZAXAYAO1000101BBATATABAAABBBABRpRRpTT0000000{B}()-=-ppRpp0pRpRpABBABABBABABBAATAAABBB原点在坐标系中的描述：BATABT1ABT2.4齐次坐标的逆变换一般,若1000zzzzyyyyxxxxpaonpaonpaonT10001npopTapxyzxyzxyznnnoooaaaBO{}BBZBXBYnoaABORGP{}AAZAXAYAO,,,pnoaTTTTxyzxyzxyzxyzpppnnnoooaaa则{}{}3043ABBATBAZXYT已知表示相对于绕