第二章 位姿描述和齐次变换(2010-09)

数学基础机械手作为执行机构是用来保证复杂空间运动的综合刚体，而且它自身也往往需要在机械加工或装配等过程中作为统一体进行运动。因此，我们需要一种用以描述单一刚体位移、速度和加速度以及动力学问题的有效而又方便的数学方法---矩阵法数学描述是以四阶方阵变换三维空间点的齐次坐标为基础的，能够将运动、变换和映射与矩阵运算联系起来补充：向量的点积和叉积矩阵的乘法zzyyxxbabababakbabajbabaibababbbaaakjibaxyyxzxxzyzzyzyxzyx)()()(1.方向角与方向余弦=AOB(0)为向量，的夹角，记作=方向角的余弦称为其方向余弦.方向余弦OAOABBabBOAOab),(bacosrx222zyxxoyzxoyzxyzxrrrr)cos,cos,(cos2.向量在轴u上的投影等于向量的模乘以轴与向量的夹角的余弦：uABBuABBuABB)cos(PrABABju向量补充212212212,232221232221332211,coszzyyxxdbbbaaababababaBA已知：a=(a1,a2,a3),b=(b1,b2,b3)空间任意两直线的公法线长度公式给定一直线过p点，具有方向矢量m，另一直线过点q，具有方向矢量n，则：)cos()(nmnmacrnmqpnma位置描述(position)---点在坐标系的位置一旦建立了一个坐标系，我们就能够用某个3×1位置矢量来确定该空间内任一点的位置。对于直角坐标系{A}，空间任一点p的位置可用3×1的列矢量AP表示。其中，px、py、pz入是点p在坐标系{A}中的三个坐标分量。Ap的上标A代表参考坐标系{A}。我们称Ap为位置矢量，见图2．1。方位描述(orientation)物体的方位可由某个固接于此物体的坐标系描述为了规定空间某刚体B的方位，设置一直角坐标系{B}与此刚体固接。用坐标系{B}的三个单位主矢量xB、yB、zB相对于参考坐标系{A}方向余弦组成的3×3矩阵来表示刚体B相对于坐标系{A}的方位。称为姿态矩阵/旋转矩阵。式中，上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。共有9个元素，但只有3个是独立的。由于的三个列矢量AxB、AyB、和AzB都是单位矢量，且双双相互垂直，因而它的9个元素满足6个约束条件(正交条件)。坐标系轴上的投影在坐标系的单位基矢量示了中的三个列矢量分别表姿态矩阵AkjiBRBBBAB,,坐标系轴上的投影在坐标系的单位基矢量示了中的三个行矢量分别表姿态矩阵BkjiARAAAAB,,位姿描述要完全描述刚体B在空间的位姿(位置和姿态)，通常将物体B与某一坐标系{B}相固接。{B}的坐标原点一般选在物体B的特征点上，如质心等。相对参考系{A}，坐标系{B}的原点位置和坐标轴的方位，分别由位置矢量B和旋转矩阵描述。这样，刚体B的位姿可由坐标系{B}来描述，即有(2.9)Y(orientation)x(normal)z(approach)aonRAB手抓坐标系Y(orientation)x(normal)z(approach)aonRAB平移坐标变换(2.10)前面讨论的是在一个坐标系中位姿的描述，在大量的机器人问题中，涉及到用不同的坐标系来描述同一个刚体的位置及姿态问题，这就涉及到从一个坐标系的描述到另一个坐标系的描述之间的变换关系，这种变换关系包括：平移变换和旋转变换旋转矩阵设固定参考坐标系直角坐标为ΣOxyz，动坐标系为ΣO´uvw，研究旋转变换情况。xyzwvuPo(O')图2-3①初始位置时，动静坐标系重合，O、O´重合，如图。各轴对应重合，设P点是动坐标系ΣO´uvw中的一点，且固定不变。则P点在ΣO´uvw中可表示为：wwuvuuuvwkPjPiPP、、为坐标系ΣO´uvw的单位矢量，则P点在Σoxyz中可表示为：uivjwkzzyyxxxyziPiPiPPuvwPxyzP②当动坐标系ΣO´uvw绕O点回转时，求P点在固定坐标系Σoxyz中的位置yzxo(O')uvwPPwPvPu图2-4已知：P点在ΣO´uvw中是不变的仍然成立，由于ΣO´uvw回转，则：wwuvuuuvwkPjPiPP)(PwwvvuuxxuvwxkPjPiPiiP)(PwwvvuuyyuvwykPjPiPjjP)(PwwvvuuzzuvwzkPjPiPjjP用矩阵表示为:wvwzvzzwvyywxvxxzyxPPPkkjkikkjjjijkijiiiPPPy（2-7）uvwxyzwzvzzwvyywxvxxPRpkkjkikkjjjijkijiii:Ry则旋转矩阵为：定义反过来：xyzuvwPRP1RRRdet*1T1RRRdet因此是正交矩阵，的行列式，由于为的伴随矩阵，为RRRR旋转矩阵的几何意义参考坐标系的姿态矩阵坐标系对可以作为固连于刚体的ABRAB)1PPAPPBRAB的坐标中的同一个空间点成坐标系变换点的坐标中的。它使坐标系可以作为坐标变换矩阵AB)2系中的投影之间的关系矢量在同一坐标示具有转动关系的两个可以作为算子。用来表RAB)3三个基本旋转矩阵),(xR即动坐标系求的旋转矩阵，也就是求出坐标系中各轴单位矢量在固定坐标系中各轴的投影分量，很容易得到在重合时，有：角，轴转动绕，XOvwOvwO'wvkji,,Oxyz),(xR100010001R由图2-5可知，在y轴上的投影为，在z轴上的投影为,在y轴上的投影为，在z轴上的投影为，所以有：vjcosyjsinzksinyjwkcoszkvjwkwzvzzwvyywxvxxkkjkikkjjjijkijiiiy)R(x,xyzouvwU'V'W'O'图2-5cossin0sincos0001uxii方向余弦阵同理：cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R（comsin0sincos0001)R(x,三个基本旋转矩阵:xyzouvwU'W'O'xyzouvwU'W'O'v'绕坐标轴转动的旋转矩阵式中，s表示sin，c表示cos。以后将一律采用此约定。旋转矩阵---举例[例1]已知转动坐标系OUVW中的两点aUVW＝(4，3，2)T和bUVW＝(6，2，4)T，若OUVW系统绕OZ轴转动了60。，试求参考坐标系中的相应点axyz和bxyz。[解]uvwZxyzuvwZxyzbRbaRa0060,60,旋转矩阵---举例[例2]已知参考坐标系OXYZ中的两点aXYZ＝(4，3，2)T和bXYZ＝(6，2，4)T，若OUVW系统绕OZ轴转动了60。，试求转动坐标系中的相应点aUVW和bUVW。[解]xyzTZxyzxyzTZuvwbRbaRa0060,60,合成旋转矩阵:例1：在动坐标中有一固定点，相对固定参考坐标系做如下运动：①R（x,90°）；②R(z,90°)；③R(y,90°)。求点在固定参考坐标系下的位置。TuvwPo321'OxyzuvwPo'Oxyz解1：用画图的简单方法解2：用分步计算的方法①R（x,90°）②R（z,90°）③R（y,90°）2313210101-00001'P21323110000101-0''P312213001-010100'''P（2-14）（2-15）（2-16）上述计算方法非常繁琐，可以通过一系列计算得到上述结果。将式（2-14）（2-15）（2-16）联写为如下形式：wvuzyxPPPRPPP33R3x3为二者之间的关系矩阵，我们令：),(),(),RR33xRzRy（定义1：当动坐标系绕固定坐标系各坐标轴顺序有限次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。注意：旋转矩阵间不可以交换uvwO'Oxyz旋转次序对变换结果的影响合成旋转矩阵为了表示绕OXYZ坐标系各轴的一连串有限转动，可把基本旋转矩阵连乘起来。由于矩阵乘法不可交换，故完成转动的次序是重要的。例如，先绕OX轴转α角，然后绕OZ袖转θ角，再绕OY转ψ角；表示这种转动的旋转矩阵为如果转动的次序变化为，先绕OY转ψ角绕OX轴转α角，然后绕OZ袖转θ角，再绕OX轴转α角；表示这种转动的旋转矩阵为除绕OXYZ参考系的坐标轴转动外，OUVW坐标系也可以绕它自己的坐标轴转动。这时，合成旋转矩阵可按下述简单规则求得：1.两坐标系最初重合，因此旋转矩阵是一个3×3单位矩阵I3。2．如果OUVW坐标系绕OXYZ坐标系的一坐标轴转动，则可对上述旋转矩阵左乘相应的基本旋转矩阵。3．如果OUVW坐标系绕自己的一坐标铀转动，则可对上述旋转矩阵右乘相应的基本旋转矩阵合成旋转矩阵规则先绕OY轴转ψ角，然后绕OW袖转θ角，再绕OU转α角；表示这种转动的旋转矩阵为位姿坐标变换/一般变换(2.13)位姿坐标变换---示例例2．1已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。求位置矢量APB0和旋转矩阵。假设点p在坐标系{B}的描述为BP＝[5．9，0]T，求它在坐标系{A}中的描述AP。RAB齐次坐标一般来说，n维空间的齐次坐标表示是一个（n+1）维空间实体。有一个特定的投影附加于n维空间，也可以把它看作一个附加于每个矢量的特定坐标—比例系数。kcjbiavzyxTwwzyxV式中i,j,k为x,y,z轴上的单位矢量，a=,b=,c=，w为比例系数wxwywz显然，齐次坐标表达并不是唯一的，随w值的不同而不同。在计算机图学中，w作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取w=1。列矩阵[例]：kjiV543可以表示为：V=[3451]T或V=[68102]T或V=[-12-16-20-4]T齐次坐标与三维直角坐标的区别V点在ΣOXYZ坐标系中表示是唯一的（x、y、z）而在齐次坐标中表示可以是多值的。不同的表示方法代表的V点在空间位置上不变。xyzzzxV图2-2o几个特定意义的齐次坐标：[0,0,0,n]T—坐标原点矢量的齐次坐标，n为任意非零比例系数[1000]T—指向无穷远处的OX轴[0100]T—指向无穷远处的OY轴[0010]T—指向无穷远处的OZ轴这样，利用齐次坐标不仅可以规定点的位置，还可以用来规定矢量的方向。第四个元素非零时，代表点的位置；第四个元素为零时，代表方向。平面的齐次坐标平面齐次坐标由行矩阵P=[abcd]来表示当点v=[xyzw]T处于平面P内时，矩阵乘积PV=O，或记为0dwczbyaxwzyxdcbaPV如果定义一个常数m=，则有：222cbamdmcwzmbwymawx)()(kmcjmbimakwzjwyiwx=可以把矢量解释为某个平面的外法线，此平面沿着法线方向与坐标原点的距离为。)(kmcjmbima