齐次坐标变换.ppt

第二章齐次坐标变换Chapter HomogeneousTransformation 2 1引言2 2点向量和平面的描述2 3变换2 4平移变换2 5旋转变换2 6坐标系2 7相对变换2 8物体的描述2 9逆变换2 10一般性旋转变换2 11等价旋转角与旋转轴2 12扩展与缩小2 13透视变换2 14变换方程2 15小结 2 1引言 Introduction 机器人操作涉及到各物体之间的关系和各物体与机械手之间的关系 这一章将给出描述这些关系必须的表达方法 类似这种表示方法在计算机图形学中已经解决 在计算机图形学和计算机视觉中 物体之间的关系是用齐次坐标变换来描述的 在本课程我们将采用齐次坐标变换来描述机械手各关节坐标之间 各物体之间以及各物体与机械手之间的关系 本章首先介绍向量和平面的表示方法 然后引出向量和平面的坐标变换 这些变换基本上是由平移和旋转组成 因此可以用坐标系来描述各种物体和机械手的空间位置和姿态 稍后还要介绍逆变换 逆变换是运动学求解的基础 2 2点向量和平面的描述 Notationofpointvectorsandplanes 2 2 1点向量 Pointvectors 点向量描述空间的一个点在某个坐标系的空间位置 同一个点在不同坐标系的描述及位置向量的值也不同 如图2 1中 点p在E坐标系上表示为Ev 在H坐标系上表示为Hu 且v u 一个点向量可表示为v ai bj ck通常用一个 n 1 维列矩阵表示 即除x y z三个方向上的分量外 再加一个比例因子w 即v xyzw T其中a x w b y w c z w 改变比例因子w 则分量a b c的数值相应改变 但描述的还是同一个点向量 如v 3i 4j 5k可表示为v 3451 T 68102 T 3 4 5 1 T在向量中增加一个比例因子w是为了方便坐标变换中的矩阵运算 已知两个向量a axi ayj azkb bxi byj bzk 2 1 向量的点积是标量 用 来定义向量点积 即a b axbx ayby azbz 2 2 向量的叉积是一个垂直于由叉积的两个向量构成的平面的向量 用 表示叉积 即a b aybz azby i azbx axbz j axby ayby k 2 3 可用行列式表示为ijka b axayaz 2 4 bxbybz 2 2 2平面 Planes 平面可用一个行矩阵表示 即p abcd 2 5 它表示了平面p的法线方向 且距坐标原点的距离为 d m 其中m 2 6 如图2 2所示 如果将x y平面沿z轴正方向平移一个单位距离 构成平面p 则p 001 1 即a 0 b 0 c 1 d 1 m 1平面p上任一点v为v xy11 T 它与平面p的点乘为零 即p v 0平面p上方任一点v 如v 0021 T 它与平面p的点乘为一个正数 即p v 1平面p下方任一点v 如v 0001 T 它与平面p的点乘为一个负数 即p v 1注意 平面 0000 无定义 H空间的变换是由4 4矩阵来完成的 它可以表示平移 旋转 扩展和透视等各种变换 如已知点u 在平面p上 它的变换v 在平面q上 用矩阵积表示为v Hu 2 7 其中H为4 4变换矩阵 u和v为4 1的点列向量 相应的平面p到q的变换是q pH 1 2 8 其中H 1为H的逆阵 p和q为1 4的平面行向量 经变换后的平面向量q与点向量v的点乘为q v pH 1 Hu p u 2 9 与变换前平面p与点u的点乘相等 证明了变换的等效性 2 3变换 Transformation 2 4平移变换 Translationtransformation 用向量h ai bj ck进行平移 其相应的H变换矩阵是100a010bH Trans abc 001c 2 10 0001因此对向量u xyzw T 经H变换为向量v可表示为x awx w ay bwy w bv z cw z w c 2 11 w1可见 平移实际上是对已知向量u xyzw T与平移向量h abc1 T相加 例2 1 对点向量u 2321 T进行平移 平移向量为h 4 371 T 则平移后的向量为v 6091 T 或100426010 330v H u 00172 9000111点向量的平移过程如图2 3所示 对平面的平移则用H 1进行变换 如对平面p 100 2 进行H变换为平面q 则根据变换原理有100 40103q pH 1 100 2 001 70001 100 6 平面p 100 2 是y z平面沿x正方向移动2个单位形成的平面 图2 3 点u 2321 T是平面p上的一个点 它们的点乘p u 0 经H变换后的平面q 100 6 是y z平面沿x正方向移动6个单位形成的平面 点v 6091 T是平面q上一个点 平面q与点v的点乘也应是零 即q v 0 说明变换前后的结果不变 证明H变换是正确的 2 5旋转变换 Rotationtransformation 如图2 4所示 绕x y z轴旋转一个 角的相应变换是10000cos sin 0Rot x 0sin cos 0 2 12 0001cos 0sin 00100Rot y sin 0cos 0 2 13 0001cos sin 00sin cos 00Rot z 0010 2 14 0001 注意 角旋转的正方向遵循右手螺旋法则 如图2 4所示 例2 2 点u 7i 3j 2k 它绕z轴旋转90 为v 经式 2 14 变换得到 sin 1 cos 0 0 1007 3100037v Rot z 90 00102 2000111起始点u和终点v如图2 5所示 如将v点再绕y轴旋转90 得到w 用式 2 13 变换得到0010 32010077w Rot y 90 10002 3000111结果如图2 6所示 如果将上述两次旋转结合起来 写成一个表达式得到w Rot y 90 v Rot y 90 Rot z 90 u用两个变换矩阵Rot y 90 Rot z 90 和起始点u代入上式计算的结果与前面分两次计算的结果相同 为此 先将点u绕z轴旋转90 然后再绕y轴旋转90 我们得到00100 100720100100037w Rot y 90 Rot z 90 u 100000102 30001000111如果按着逆序旋转 首先绕y轴旋转90 然后再绕z轴旋转90 其结果为0 10000107 31000010032w Rot z 90 Rot y 90 u 0100 10002 70001000111逆序旋转的结果如图2 7所示 显然 变换的顺序不同 其结果也不同 这从矩阵相乘是不可交换的 AB BA 也可以得到证明 如对经过两次旋转变换得到的点向量w再进行一次平移 平移向量为h 4 371 T 则可得到如图2 8所示的点向量n 变换过程如下100426010 374n Trans 4 3 7 w 00173 10000111 2 6坐标系 Coordinateframes 齐次变换矩阵H由四个列向量组成 它的前三个列向量称为方向向量 由式 2 12 到式 2 14 的旋转变换 分别绕x y z轴旋转 角 确定 第四个列向量称为平移向量 它的平移分量 沿x y z轴的平移量 由式 2 10 第四列的前三个元素确定 如0014100 3H Trans 4 3 7 Rot y 90 Rot z 90 0107 2 15 0001坐标系的原点 即零向量 0001 T的H变换是 4 371 T 相当于将原点按平移向量的各个分量进行平移的结果 如图2 9所示 如果对x y z轴的单位向量进行H变换 分别得到 4 271 T 4 381 T和 5 371 T 这四个向量在图2 9中标出 并形成了一个新坐标系 这个新坐标系的x y z轴的方向分别是 0 1 0 0 T 0 0 1 0 T和 1 0 0 0 T 它是由单位向量的H变换减去这个坐标原点的向量得到的 这些方向向量相应于变换矩阵的前三列 见式 2 15 可见 H变换矩阵描述了一个坐标系绕原参考坐标系旋转和对参考坐标系平移的三个轴的方向和原点的位置 见图2 9 如图2 10所示 当对一个向量n进行式 2 15 给出的H变换时 原向量n可以被认为是在新坐标系描述的那个向量u 即被变换了的向量u就是相对于参考坐标系描述的同一个向量n 2 7相对变换 Relativetransformation 我们刚刚描述的旋转和平移都是相对于一个固定的坐标系而进行的 这样 在已给的例子里0014100 3Trans 4 3 7 Rot y 90 Rot z 90 0107 2 16 0001坐标系首先绕参考坐标系z轴旋转90 然后绕y轴旋转90 最后平移4i 3j 7k 如图2 9所示 如果以相反次序从左到右来进行这些操作 首先对坐标平移4i 3j 7k 然后将它绕当前坐标系的y轴旋转90 此时当前坐标系的y轴与参考坐标系的y轴是相同的 然后再绕着新坐标系 当前的 坐标系的z轴旋转90 所得结果与前面的方法相同 见图2 11 一般的情况下 如果我们用一个旋转和 或平移变换矩阵右乘一个坐标系的变换 那么产生的平移和 或旋转是相对于前一个变换的坐标系 当前坐标系 的轴来说的 如果我们用一个描述平移和 或旋转的变换矩阵左乘一个坐标系的变换 那么产生的平移和 或旋转是相对于基坐标系来说的 例2 3 给一个坐标系C和一个变换T T为绕z轴旋转90 并在x轴方向上平移10个单位 当变换是相对于基坐标系产生时 我们用T左乘C得到新的位置x为0 1010100200010100000 11010020 x TC 00100100 0100 2 17 000100010001当变换是相对于当前坐标系C轴产生时 我们用T右乘C得到新的位置y为100200 10100 103000 110100000 110y CT 01000010 1000 2 18 000100010001结果如图2 12所示 2 8物体的描述 Objectrepresentation 变换可用来描述物体的位置与方向 方位 如图2 13所示的楔形物体用六个角点来描述 这六个角点是相对于物体所在的参考坐标系的 如果把物体绕z轴旋转90 然后绕y轴旋转90 接着沿x方向平移4个单位 我们可以描述这个变换为00141000Trans 4 0 0 Rot y 90 Rot z 90 01000001这个变换表示了对参考坐标系的旋转和平移操作 变换后物体的六个角点为44664400141 1 111 11 1 111 11000000044000044 01000002201111110001111111变换后该物体在坐标上的方位如图2 13所示 从图2 13可以看出 由于楔形物体的角点与它所在的坐标系有固定的关系 因此没有必要对所有的角点进行变换 只要对物体所在的坐标系进行变换 就可得到变换后的各个角点在基坐标中的位置 将这些角点用直线连接起来就可得到楔形物体的边缘 它与逐点变换的结果完全相同 见图2 14 2 9逆变换 Inversetransformation 所谓逆变换就是将被变换的坐标系返回到原来的坐标系 在数学上就是求变换矩阵的逆 下面我们写出变换矩阵的一般表达形式nxoxaxpxnyoyaypyT nzozazpz 2 19 0001式中n o a是旋转变换列向量 p是平移向量 其逆是nxnynz p noxoyoz p oT 1 axayaz p a 2 20 0001式中的 表示向量的点积 这个结果很容易用式2 19右乘式2 20是单位矩阵来证明 2 10一般性旋转变换 Generalrotationtransformation 前面我们介绍的旋转变换都是绕x y z轴旋转的旋转变换 这些变换都有一个简单的几何解释 例如 在绕z轴旋转的情况下 表示z轴保持恒定 x轴和y轴将如图2 15所示那样变化 如图2 16所示 给出一个变换矩阵C 它绕任意向量k旋转 我们把k当作C坐标系的z轴单位向量 nxoxax0nyoyay0C nzozaz0 2 21 0001k axi ayj azk 2 22 绕k旋转就相等于绕C坐标系的z轴旋转 Rot k Rot Cz 2 23 如果我们给一个坐标系T 它在参考坐标系里被描述 它在C坐标系里用X描述 这样T CX 2 24 其中X描述T相对C的位姿 求X 我们得到X C 1T 2 25 T绕k旋转就等于 绕 坐标系的z轴旋转Rot k CRot z X 2 26 Rot k CRot z C 1T 2 27 这样Rot k CRot z C 1 2 28 展开式 2 28 我们发现CRot z C 1仅是k的函数 用C 1右乘Rot z 我们得到cos sin 00nxnynz0sin cos 00oxoyoz0Rot z C 1 0010axayaz000010001nxcos oxsin nycos oysin nzcos ozsin 0nxcos oxsin nycos oysin nzcos ozsin 0 axayaz0 2 29 0001再用C左乘nxoxax0nyoyay0C nzozaz0 2 30 0001 得到CRot z C 1 nxnxcos nxoxsin nxoxsin oxoxcos axaxnynxcos nyoxsin nxoysin oyoxcos ayaxnznxcos nzoxsin nxozsin ozoxcos azax0nxnycos nxoysin nyoxsin oyoxcos axaynynycos nyoysin nyoysin oyoycos ayaynznycos nzoysin nyozsin oyozcos azay0nxnzcos nxozsin nzoxsin ozoxcos axaz0nynzcos nyozsin nzoysin ozoycos ayaz0nznzcos nzozsin nzozsin ozozcos azaz0 2 31 01 应用下列关系进行简化 C坐标系任意的行或列与其他行或列的点积为零 因为这些向量是正交的 C坐标系任意的行或列与其自身的点积为I 因为它们是单位量 z向量是x和y向量的叉积 a n o 它有下列分量ax nyoz nzoyay nzox nxozaz nxoy nyox正矢Vers 1 cos 简写成Vers 且kx ax ky ay kz az 由此可得到简化式为Rot k kxkxVers cos kykxVers kzsin kzkxVers kysin 0kxkyVers kzsin kykyVers cos kzkyVers kzxsin 0kxkzVers kysin kykzVers kxsin kzkzVers cos 0 2 32 0001上式是一般性的旋转变换的重要结论 从这个结论可以得出每一个基本旋转变换 例如 Rot x 就是Rot k 当kx 1 ky 0 kz 0的情况 将这些值代入式 2 32 得到10000cos sin 0Rot x 0sin cos 0 2 33 0001这个结果与以前一样 2 11等价旋转角与旋转轴 Equivalentangleandaxisofrotation 任给一个旋转变换 从 2 32 方程得到一个轴 绕这个轴旋转的等价旋转角可由如下方法得到 已知一个旋转变换Rnxoxax0nyoyay0R nzozaz0 2 34 0001令R和式 2 32 的Rot k 相等 并将对角线各项相加得到nx oy az 1 k2xVers cos k2yVers cos k2zVers cos 1 2 35 nx oy az k2x k2y k2z Vers 3cos 1 2cos 2 36 由此可得到旋转角的余弦是cos 1 2 nx oy az 1 2 37 对非对角线项相减 我们得到oz ay 2kxsin 2 38 ax nz 2kysin 2 39 ny ox 2kzsin 2 40 把式 2 38 到式 2 40 两边平方并相加有 oz ay 2 ax nz 2 ny ox 2 4sin2 2 41 我们得到了sin 的表达式sin 1 2 oz ay 2 ax nz 2 ny ox 2 2 42 规定这个旋转是绕k正方向旋转 当0 180 时 在上式中取十号是合理的 这个旋转角 被唯一定义为tan oz ay 2 ax nz 2 ny ox 2 nx oy az 1 2 43 k的各分量为kx oz ay 2sin 2 44 ky ax nz 2sin 2 45 kz ny ox 2sin 2 46 注意 当旋转角 较小或接近180 时 上述三个式子的分子和分母都很小 所计算的k值是不精确的 为此可继续根据式 2 32 和式 2 33 对应元素以及它们的代数和相等的关系来求出k的各个分量 2 12扩展与缩小 Stretchingandscaling 一个变换Ta0000b00T 00c0 2 47 0001将沿着x轴以a因子 沿着y轴以b因子 沿着z轴c因子均匀扩展着各种物体 假定在一个物体上任意一个点xi yj zk 它的变换是axa000 xby0b00ycz 00c0z 2 48 100011这个正好表示出所说的扩展 这样 一个正方体可以由这个变换变成长方体 变换ss0000s00s 00s0 2 49 0001将以s为比例因子来扩展或缩小任一物体 2 13透视变换 Perspectivetransformation 假设由一个简单透镜把一个物体形成的像如图2 17所示 透镜的轴沿着y的方向 焦距为f 物体上的一个点x y z成象为x y z y 表示象距 它随着物距y而变化 如果在通过y 而垂直于y的平面 照相机的底片 上画出各个点 那么就形成了一个透视像 射线穿过透镜中心不偏转 则z y z y 2 50 x y x y 2 51 根据平行透镜的轴的射线通过焦点 我们可以写出z f z y f 2 52 x f x y f 2 53 x y 和z 是负数 而f是正数 用式 2 50 和式 2 52 消去y 得z f z z y z f 2 54 求出x x 1 y f 2 55 y y 1 y f 2 56 z z 1 y f 2 57 齐次变换p能导出同样结果 变换p是10000100p 0010 2 58 0 1 f01任何一点xi yj zk变换为x1000 xy0100yz 0010z 2 59 1 y f0 1 f011用比例因子1 y f除得到的象点x y z 有 x 1 y f i y 1 y f j z 1 y f k 2 60 这个结果与前面利用透视原理的结果完全相同 在p变换的第二列最底一元素为 1 f 则导出一个沿着y轴的一透视变换 如果 1 f是第三列最底一项 那就是沿z轴的透视变换 2 14变换方程 Transformequations 研究一下图2 18描述的一个物体与机械手情况 机械手用变换Z相对于基坐标系被定位 机械手的端点用变换ZT6来描述 而末端执行器用变换