位姿描述和齐次变换

位姿描述和齐次变换第1页，共69页，2023年，2月20日，星期三

1.方向角与方向余弦=AOB(0)为向量，的夹角，记作=方向角的余弦称为其方向余弦.

方向余弦2.向量在轴u上的投影等于向量的模乘以轴与向量的夹角的余弦：第2页，共69页，2023年，2月20日，星期三向量补充已知：a=(a1,a2,a3),b=(b1,b2,b3)第3页，共69页，2023年，2月20日，星期三空间任意两直线的公法线长度公式给定一直线过p点，具有方向矢量m，另一直线过点q，具有方向矢量n，则：第4页，共69页，2023年，2月20日，星期三位置描述(position)---点在坐标系的位置一旦建立了一个坐标系，我们就能够用某个3×1位置矢量来确定该空间内任一点的位置。对于直角坐标系{A}，空间任一点p的位置可用3×1的列矢量AP表示。其中，px、py、pz入是点p在坐标系{A}中的三个坐标分量。Ap的上标A代表参考坐标系{A}。我们称Ap为位置矢量，见图2．1。

第5页，共69页，2023年，2月20日，星期三方位描述(orientation)物体的方位可由某个固接于此物体的坐标系描述为了规定空间某刚体B的方位，设置一直角坐标系{B}与此刚体固接。用坐标系{B}的三个单位主矢量xB、yB、zB相对于参考坐标系{A}方向余弦组成的3×3矩阵来表示刚体B相对于坐标系{A}的方位。称为姿态矩阵/旋转矩阵。式中，上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。共有9个元素，但只有3个是独立的。由于的三个列矢量AxB、AyB、和AzB都是单位矢量，且双双相互垂直，因而它的9个元素满足6个约束条件(正交条件)。

第6页，共69页，2023年，2月20日，星期三位姿描述要完全描述刚体B在空间的位姿(位置和姿态)，通常将物体B与某一坐标系{B}相固接。{B}的坐标原点一般选在物体B的特征点上，如质心等。相对参考系{A}，坐标系{B}的原点位置和坐标轴的方位，分别由位置矢量B和旋转矩阵描述。这样，刚体B的位姿可由坐标系{B}来描述，即有

(2.9)Y(orientation)

x(normal)

z(approach)

第7页，共69页，2023年，2月20日，星期三手抓坐标系Y(orientation)

x(normal)

z(approach)

第8页，共69页，2023年，2月20日，星期三平移坐标变换

(2.10)前面讨论的是在一个坐标系中位姿的描述，在大量的机器人问题中，涉及到用不同的坐标系来描述同一个刚体的位置及姿态问题，这就涉及到从一个坐标系的描述到另一个坐标系的描述之间的变换关系，这种变换关系包括：平移变换和旋转变换

第9页，共69页，2023年，2月20日，星期三

旋转矩阵

设固定参考坐标系直角坐标为ΣOxyz，动坐标系为ΣO´uvw，研究旋转变换情况。①

初始位置时，动静坐标系重合，O、O´重合，如图。各轴对应重合，设P点是动坐标系ΣO´uvw中的一点，且固定不变。则P点在ΣO´uvw中可表示为：

、、为坐标系ΣO´uvw的单位矢量，则P点在Σoxyz中可表示为：第10页，共69页，2023年，2月20日，星期三当动坐标系ΣO´uvw绕O点回转时，求P点在固定坐标系Σoxyz中的位置已知：P点在ΣO´uvw中是不变的仍然成立，由于ΣO´uvw回转，则：用矩阵表示为:（2-7）第11页，共69页，2023年，2月20日，星期三反过来：第12页，共69页，2023年，2月20日，星期三旋转矩阵的几何意义第13页，共69页，2023年，2月20日，星期三三个基本旋转矩阵即动坐标系求的旋转矩阵，也就是求出坐标系中各轴单位矢量在固定坐标系中各轴的投影分量，很容易得到在重合时，有：第14页，共69页，2023年，2月20日，星期三由图2-5可知，在y轴上的投影为，在z轴上的投影为,在y轴上的投影为，在z轴上的投影为，所以有：方向余弦阵第15页，共69页，2023年，2月20日，星期三同理：三个基本旋转矩阵:

第16页，共69页，2023年，2月20日，星期三绕坐标轴转动的旋转矩阵式中，s表示sin，c表示cos。以后将一律采用此约定。

第17页，共69页，2023年，2月20日，星期三旋转矩阵---举例[例1]已知转动坐标系OUVW中的两点aUVW＝(4，3，2)T和bUVW＝(6，2，4)T，若OUVW系统绕OZ轴转动了60。，试求参考坐标系中的相应点axyz和bxyz。[解]第18页，共69页，2023年，2月20日，星期三旋转矩阵---举例[例2]已知参考坐标系OXYZ中的两点aXYZ＝(4，3，2)T和bXYZ＝(6，2，4)T，若OUVW系统绕OZ轴转动了60。，试求转动坐标系中的相应点aUVW和bUVW。[解]第19页，共69页，2023年，2月20日，星期三

合成旋转矩阵:例1：在动坐标中有一固定点，相对固定参考坐标系做如下运动：①R（x,90°）；②R(z,90°)；③R(y,90°)。求点在固定参考坐标系下的位置。解1：用画图的简单方法第20页，共69页，2023年，2月20日，星期三解2：用分步计算的方法①R（x,90°）②R（z,90°）③R（y,90°）（2-14）（2-15）（2-16）第21页，共69页，2023年，2月20日，星期三

上述计算方法非常繁琐，可以通过一系列计算得到上述结果。将式（2-14）（2-15）（2-16）联写为如下形式：R3x3为二者之间的关系矩阵，我们令：定义1：当动坐标系绕固定坐标系各坐标轴顺序有限次转动时，其合成旋转矩阵为各基本旋转矩阵依旋转顺序左乘。注意：旋转矩阵间不可以交换第22页，共69页，2023年，2月20日，星期三旋转次序对变换结果的影响第23页，共69页，2023年，2月20日，星期三合成旋转矩阵为了表示绕OXYZ坐标系各轴的一连串有限转动，可把基本旋转矩阵连乘起来。由于矩阵乘法不可交换，故完成转动的次序是重要的。例如，先绕OX轴转α角，然后绕OZ袖转θ角，再绕OY转ψ角；表示这种转动的旋转矩阵为

如果转动的次序变化为，先绕OY转ψ角绕OX轴转α角，然后绕OZ袖转θ角，再绕OX轴转α角；表示这种转动的旋转矩阵为

第24页，共69页，2023年，2月20日，星期三除绕OXYZ参考系的坐标轴转动外，OUVW坐标系也可以绕它自己的坐标轴转动。这时，合成旋转矩阵可按下述简单规则求得：1.两坐标系最初重合，因此旋转矩阵是一个3×3单位矩阵I3。2．如果OUVW坐标系绕OXYZ坐标系的一坐标轴转动，则可对上述旋转矩阵左乘相应的基本旋转矩阵。3．如果OUVW坐标系绕自己的一坐标铀转动，则可对上述旋转矩阵右乘相应的基本旋转矩阵合成旋转矩阵规则先绕OY轴转ψ角，然后绕OW袖转θ角，再绕OU转α角；表示这种转动的旋转矩阵为第25页，共69页，2023年，2月20日，星期三位姿坐标变换/一般变换(2.13)第26页，共69页，2023年，2月20日，星期三位姿坐标变换---示例例2．1已知坐标系{B}的初始位姿与{A}重合，首先{B}相对于坐标系{A}的zA轴转30°，再沿{A}的xA轴移动12单位，并沿{A}的yA轴移动6单位。求位置矢量APB0和旋转矩阵。假设点p在坐标系{B}的描述为BP＝[5．9，0]T，求它在坐标系{A}中的描述AP。

第27页，共69页，2023年，2月20日，星期三齐次坐标

一般来说，n维空间的齐次坐标表示是一个（n+1）维空间实体。有一个特定的投影附加于n维空间，也可以把它看作一个附加于每个矢量的特定坐标—比例系数。

式中i,j,k为x,y,z轴上的单位矢量，a=,b=,c=，w为比例系数

显然，齐次坐标表达并不是唯一的，随w值的不同而不同。在计算机图学中，w作为通用比例因子，它可取任意正值，但在机器人的运动分析中，总是取w=1。列矩阵第28页，共69页，2023年，2月20日，星期三[例]：可以表示为：

V=[3451]T

或V=[68102]T

或V=[-12-16-20-4]T

第29页，共69页，2023年，2月20日，星期三

齐次坐标与三维直角坐标的区别V点在ΣOXYZ坐标系中表示是唯一的（x、y、z）而在齐次坐标中表示可以是多值的。不同的表示方法代表的V点在空间位置上不变。第30页，共69页，2023年，2月20日，星期三

几个特定意义的齐次坐标：[0,0,0,n]T—坐标原点矢量的齐次坐标，n为任意非零比例系数[1000]T—指向无穷远处的OX轴[0100]T—指向无穷远处的OY轴[0010]T—指向无穷远处的OZ轴

这样，利用齐次坐标不仅可以规定点的位置，还可以用来规定矢量的方向。第四个元素非零时，代表点的位置；第四个元素为零时，代表方向。第31页，共69页，2023年，2月20日，星期三平面的齐次坐标平面齐次坐标由行矩阵P=[abcd]来表示当点v=[xyzw]T处于平面P内时，矩阵乘积PV=O，或记为

如果定义一个常数m=，则有：=可以把矢量解释为某个平面的外法线，此平面沿着法线方向与坐标原点的距离为。第32页，共69页，2023年，2月20日，星期三因此一个平行于x、y轴，且在z轴上的坐标为单位距离的平面P可以表示为：或

有：PV=例如：点V=[102011]T必定处于此平面内，而点V=[0021]T处于P平面的上方点V=[0001]T处于P平面下方。因为：

与点矢相仿，平面也没有意义第33页，共69页，2023年，2月20日，星期三齐次变换

其中，4×1的列矢量表示三维空间的点，称为点的齐次坐标，

齐次变换矩阵是4×4的方阵,综合地表示了平移变换和旋转变换。

第34页，共69页，2023年，2月20日，星期三T反映了∑O´在∑O中的位置和姿态，即表示了该坐标系原点和各坐标轴单位矢量在固定坐标系中的位置和姿态。该矩阵可以由4个子矩阵组成，写成如下形式：为姿态矩阵，表示动坐标系∑O´在固定参考坐标系∑O中的姿态，即表示∑O´各坐标轴单位矢量在∑O各轴上的投影为位置矢量矩阵，代表动坐标系∑O´坐标原点在固定参考坐标系∑O中的位置为透视变换矩阵，在视觉中进行图像计算，一般置为0为比例系数第35页，共69页，2023年，2月20日，星期三平移齐次坐标变换

空间某点由矢量ai+bj+ck描述。其中，i,j,k为轴x,y,z上的单位矢量。此点可用平移齐次变换表示为

例2．3作为例子，让我们考虑矢量2i+3j+2k被矢量4i-3j+7k平移变换得到的新的点矢量原坐标系中的表示平移后形成的新坐标系新坐标系中的表示第36页，共69页，2023年，2月20日，星期三相对变换

举例说明：例1：动坐标系∑0′起始位置与固定参考坐标系∑0重合,动坐标系∑0′做如下运动：①R(Z,90º)②R（y,90º）③Trans(4，-3,7)，求合成矩阵解1：用画图的方法：第37页，共69页，2023年，2月20日，星期三解2：用计算的方法

以上均以固定坐标系多轴为变换基准，因此矩阵左乘。如果我们做如下变换，也可以得到相同的结果：例2：①先平移Trans(4,-3,7)；②绕当前轴转动90º；③绕当前轴转动90º；求合成旋转矩阵。（2-20）第38页，共69页，2023年，2月20日，星期三解1：用画图的方法解2：用计算的方法（2-21）第39页，共69页，2023年，2月20日，星期三式（2-20）和式（2-21）无论在形式上，还是在结果上都是一致的。因此我们有如下的结论：动坐标系在固定坐标系中的齐次变换有2种情况：定义1：如果所有的变换都是相对于固定坐标系中各坐标轴旋转或平移，则依次左乘，称为绝对变换。定义2：如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移，则齐次变换为依次右乘，称为相对变换。结果均为为动坐标系在固定坐标中的位姿（位置+姿态）。相对于固定坐标系，

也就是说，动坐标系绕自身坐标轴做齐次变换，要达到绕固定坐标系相等的结果，就应该用相反的顺序。

第40页，共69页，2023年，2月20日，星期三旋转齐次坐标变换

第41页，共69页，2023年，2月20日，星期三合成齐次变换除绕OXYZ参考系的坐标轴转动外，OUVW坐标系也可以绕它自己的坐标轴转动。这时，合成旋转矩阵可按下述简单规则求得：1.两坐标系最初重合，因此旋转矩阵是一个4×4单位矩阵I4。2．如果OUVW坐标系绕（或沿）OXYZ坐标系的一坐标轴转动（或平移），则左乘相应的齐次变换矩阵--绝对变换。3．如果OUVW坐标系绕（或沿）自己的一坐标铀转动（或平移），则可右乘相应的齐次变换矩阵—相对变换第42页，共69页，2023年，2月20日，星期三齐次变换矩阵T

的意义：机器人用到相对变换的时候比较多例如机械手抓一个杯子，如右图所示，手爪需要转动一个角度才抓的牢，相对于固定坐标系表达太麻烦，可以直接根据手爪的坐标系表示但也要知道在∑O中的位姿，就用右乘的概念。

oH第43页，共69页，2023年，2月20日，星期三齐次变换矩阵的几何意义第44页，共69页，2023年，2月20日，星期三齐次变换矩阵的逆阵第45页，共69页，2023年，2月20日，星期三齐次变换矩阵的逆阵第46页，共69页，2023年，2月20日，星期三齐次变换矩阵举例例：动坐标系绕参考坐标系的z轴旋转30度，并分别沿x,y的正向平移3个和4个单位，求齐次变换矩阵及其逆阵。第47页，共69页，2023年，2月20日，星期三通用齐次变换

动坐标系绕过P=[PX,PY,PZ]点而分量为kx,ky,kz的任意单位矢量k转动ψ角时的变换矩阵

研究这种转动的好处是，对于某种角运动，可以用动坐标系绕某轴k的一次运动代替绕参考坐标系（固定坐标系）或(和)动坐标系坐标系的坐标轴的数次运动动。

第48页，共69页，2023年，2月20日，星期三通用齐次变换第49页，共69页，2023年，2月20日，星期三通用齐次变换第50页，共69页，2023年，2月20日，星期三通用齐次变换---例题第51页，共69页，2023年，2月20日，星期三等效转角和转轴R=Rot(f，θ)=

把上式两边的对角线项分别相加，并化简得

所以把式(2．47)中的非对角线项成对相减可得第52页，共69页，2023年，2月20日，星期三等效转角和转轴对上式各行平方后相加得

所以把旋转规定为绕矢量f的正向旋转，使得o＜θ＜180°。这时，式(2．50)中的符号取正号，转角θ被惟一地确定为

而矢量f的各分量可由式(2．49)求得第53页，共69页，2023年，2月20日，星期三等效转角和转轴---例题第54页，共69页，2023年，2月20日，星期三变换方程初步

基坐标系目标系工具系工作站系第55页，共69页，2023年，2月20日，星期三例题：试求立方体中心在机座坐标系∑0中的位置该手爪从上方把物体抓起，同时手爪的开合方向与物体的Y轴同向，那么，求手爪相对于∑0的姿态是什么？

在机器人工作台上加装一电视摄像机，摄像机可见到固联着6DOF关节机器人的机座坐标系原点，它也可以见到被操作物体（立方体）的中心，如果在物体中心建一局部坐标系，则摄像机所见到的这个物体可由齐次变换矩阵T1来表示，如果摄像机所见到的机座坐标系为矩阵T2表示。xyz第56页，共69页，2023年，2月20日，星期三解1：因此物体位于机座坐标系的（11，10，1）T处，它的X，Y，Z轴分别与机座坐标系的-Y，X，Z轴平行。第57页，共69页，2023年，2月20日，星期三解2：第58页，共69页，2023年，2月20日，星期三第59页，共69页，2023年，2月20日，星期三绕固定轴x-y-z旋转

第60页，共69页，2023年，2月20日，星期三z-y-x欧拉角

第61页，共69页，2023年，2月20日，星期三Z-Y-Z欧拉角

第62页，共69页，2023年，2月20日，星期三3种最常见的欧拉角类型步1步2步3类型1绕OZ轴转φ角绕当前OU'

轴转θ角绕当前OW″轴转ψ角类型2绕OZ轴转φ角绕当前OV'轴转θ角绕当前OW″轴转ψ角类型3绕OX轴转φ角绕OY轴转θ角绕OZ轴转ψ角φφφu′v′w′①x(u)y(v)z(w)oθu