齐次变换

null第二章 齐次坐标变换 ChapterⅡ Homogeneous Transformation 第二章 齐次坐标变换 ChapterⅡ Homogeneous Transformation 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.1 引言 (Introduction) 机器人操作涉及到各物体之间的关系和各物体与机械手之间的关系。这一章将给出描述这些关系必须的表达方法。类似这种表示方法在计算机图形学中已经解决。在计算机图形学和计算机视觉中，物体之间的关系是用齐次坐标变换来描述的。在本课程我们将采用齐次坐标变换来描述机械手各关节坐标之间、各物体之间以及各物体与机械手之间的关系。 本章首先介绍向量和平面的表示方法，然后引出向量和平面的坐标变换，这些变换基本上是由平移和旋转组成，因此可以用坐标系来描述各种物体和机械手的空间位置和姿态。稍后还要介绍逆变换，逆变换是运动学求解的基础。2.2 点向量和平面的描述（Notation of point vectors and planes）2.2 点向量和平面的描述（Notation of point vectors and planes） 2.2.1 点向量（Point vectors） 点向量描述空间的一个点在某个坐标系的空间位置。同一个点在不同坐标系的描述及位置向量的值也不同。如图2.1中，点p在E坐标系上表示为 Ev，在H坐标系上表示为 Hu，且v ≠ u。一个点向量可表示为 v = ai + bj + ck 通常用一个（n + 1）维列矩阵表示，即除 x、y、z 三个方向上的分量外，再加一个比例因子 w ，即 v = [ x y z w ]T 其中 a = x/w, b = y/w, c = z/w。 改变比例因子 w，则分量 a、b、c 的数值相应改变，但描述的还是同一个点向量。如 v = 3i + 4j + 5k 可表示为 v = [ 3 4 5 1 ]T = [ 6 8 10 2 ]T = [ -3 -4 -5 -1]T 在向量中增加一个比例因子 w 是为了方便坐标变换中的矩阵运算。null 已知两个向量 a = ax i + ay j + az k b = bx i + by j + bz k （2.1） 向量的点积是标量。用“ · ”来定义向量点积，即 a · b = ax bx + ay by + az bz （2.2 ） 向量的叉积是一个垂直于由叉积的两个向量构成的平面的向量。用“×”表示叉积，即 a × b = ( ay bz ¯ az by ) i + ( az bx ¯ ax bz ) j + ( ax by ¯ ay by ) k （ 2.3） 可用行列式表示为 i j k a × b = ax ay az （2.4） bx by bz 2.2.2 平面（Planes）2.2.2 平面（Planes） 平面可用一个行矩阵表示，即 p = [ a b c d ] （2.5） 它表示了平面p的法线方向，且距坐标原点的 距离为－d / m，其中 m = （2.6） 如图2.2所示，如果将 x－y 平面沿z 轴正 方向平移一个单位距离，构成平面 p，则 p = [ 0 0 1 -1] 即 a = 0, b = 0, c = 1, d = -1, m = = 1 平面p上任一点v为 v = [ x y 1 1 ]T，它与平面p的点乘为零，即 p • v = 0 平面p上方任一点v，如 v = [ 0 0 2 1 ]T，它与平面p的点乘为一个正数，即 p • v = 1 平面p下方任一点v，如 v = [ 0 0 0 1 ]T，它与平面p的点乘为一个负数，即 p • v = -1 注意：平面 [ 0 0 0 0 ] 无定义。null H空间的变换是由4×4矩阵来完成的，它可以表示平移、旋转、扩展和透视等各种变换。如已知点u（在平面p上）,它的变换v（在平面q上）用矩阵积表示为 v = H u （2.7） 其中H为4×4 变换矩阵，u和v为4×1的点列向量，相应的平面p到q的变换是 q = p H-1 （2.8） 其中H-1为H的逆阵，p和q为1×4 的平面行向量。 经变换后的平面向量q与点向量v的点乘为 q · v = p H-1 · H u ＝ p · u （ 2.9） 与变换前平面p与点u的点乘相等，证明了变换的等效性。2.3 变换(Transformation)2.4 平移变换（Translation transformation）2.4 平移变换（Translation transformation） 用向量 h ＝ a i + b j + c k 进行平移，其相应的H变换矩阵是 1 0 0 a 0 1 0 b H = Trans ( a b c ) = 0 0 1 c (2.10)          0 0 0 1 因此对向量 u = [ x y z w ]T，经H变换为向量v可表示为 x + aw x / w + a y + bw y / w + b v = z + cw = z / w + c (2.11) w 1 可见，平移实际上是对已知向量 u = [ x y z w ]T 与平移向量 h = [ a b c 1 ]T 相加。 null【例2.1】对点向量 u = [ 2 3 2 1 ]T 进行平移，平移向量为 h = [ 4 -3 7 1 ]T，则平移后的向量为 v = [ 6 0 9 1 ]T，或 1 0 0 4 2 6 0 1 0 ―3 3 0 v = H ∙ u = 0 0 1 7 2 = 9 0 0 0 1 1 1 点向量的平移过程如图2.3所示。 对平面的平移则用 H－1 进行变换，如对平面 p = [ 1 0 0 -2 ] 进行 H 变换为平面q，则根据变 换原理有 1 0 0 -4 0 1 0 3 q ＝ p H－1 ＝[ 1 0 0 -2 ] 0 0 1 -7 0 0 0 1 ＝[ 1 0 0 -6 ] 平面 p ＝ [ 1 0 0 -2 ] 是 y－z 平面沿 x 正方向移动2个单位形成的平面（图2.3），点u = [ 2 3 2 1 ]T 是平面 p上的一个点，它们的点乘 p ∙ u = 0。经 H 变换后的平面 q＝[ 1 0 0 -6 ]是 y－z 平面沿 x 正方向移动6个单位形成的平面，点v = [6 0 9 1]T 是平面 q上一个点，平面 q 与点 v 的点乘也应是零，即 q ∙ v ＝0，说明变换前后的结果不变，证明 H 变换是正确的。 2.5 旋转变换（Rotation transformation）2.5 旋转变换（Rotation transformation） 如图2.4所示，绕 x, y, z 轴旋转一个θ角 的相应变换是 1 0 0 0 0 cosθ - sinθ 0 Rot ( x, θ) = 0 sinθ cosθ 0 （2.12） 0 0 0 1      cosθ 0 sinθ 0 0 1 0 0 Rot ( y, θ) = - sinθ 0 cosθ 0 (2.13) 0 0 0 1   cosθ - sinθ 0 0 sinθ cosθ 0 0 Rot ( z, θ) = 0 0 1 0 (2.14) 0 0 0 1注意：θ角旋转的正方向遵循右手螺旋法则（如图2.4所示）null【例2.2】点 u = 7i + 3j + 2k，它绕z轴旋转90°为v， 经式（2.14）变换得到（ sinθ=1，cosθ=0） 0 -1 0 0 7 -3 1 0 0 0 3 7 v = Rot ( z, 90°) = 0 0 1 0 2 ＝ 2 0 0 0 1 1 1 起始点u和终点v如图2.5所示。如将v点再绕y轴 旋转90°得到w。用式（2.13）变换得到 0 0 1 0 -3 2 0 1 0 0 7 7 w = Rot ( y, 90°) = -1 0 0 0 2 ＝ 3 0 0 0 1 1 1   结果如图2.6所示。如果将上述两次旋转结合起来， 写成一个表达式得到 w = Rot ( y, 90°) v ＝ Rot ( y, 90°) Rot ( z, 90°) u 用两个变换矩阵 Rot ( y, 90°) 、 Rot ( z, 90°) 和起始 点u代入上式计算的结果与前面分两次计算的结果相同。null为此，先将点u绕z轴旋转90°，然后再绕y轴旋转90°，我们得到 0 0 1 0 0 -1 0 0 7 2 0 1 0 0 1 0 0 0 3 7 w ＝ Rot ( y, 90°) Rot ( z, 90°) u = -1 0 0 0 0 0 1 0 2 ＝ 3 0 0 0 1 0 0 0 1 1 1 如果按着逆序旋转，首先绕y轴旋转90°，然后再绕z轴旋转90°，其结果为   0 -1 0 0 0 0 1 0 7 -3 1 0 0 0 0 1 0 0 3 2 w = Rot ( z, 90°) Rot ( y, 90°) u = 0 1 0 0 -1 0 0 0 2 = -7 0 0 0 1 0 0 0 1 1 1 逆序旋转的结果如图2.7所示。显然，变换的顺序不同，其结果也不同 。这从 矩阵相乘是不可交换的（AB≠BA）也可以得到证明。 null如对经过两次旋转变换得到的点向量w再进行一次平移（平移向量为 h ＝ [ 4 -3 7 1]T ）， 则可得到如图2.8所示的点向量n。变换过程如下 1 0 0 4 2 6 0 1 0 -3 7 4 n = Trans (4, －3, 7) w = 0 0 1 7 3 = 10 0 0 0 1 1 1 2.6 坐标系 (Coordinate frames) 2.6 坐标系 (Coordinate frames) 齐次变换矩阵H由四个列向量组成，它的前三个列向量称为方向向量，由式 （2.12）到式（2.14）的旋转变换（分别绕 x、y、z 轴旋转θ角）确定，第四个列向 量称为平移向量，它的平移分量（沿 x、y、z 轴的平移量）由式（2.10）第四列的前 三个元素确定。如　 0 0 1 4 1 0 0 -3 H＝Trans ( 4, -3, 7 ) Rot ( y, 90°) Rot ( z, 90°) = 0 1 0 7 （2.15） 0 0 0 1 坐标系的原点，即零向量 [ 0 0 0 1 ] T 的 H 变换是 [ 4 -3 7 1 ] T，相当于将原点按平移 向量的各个分量进行平移的结果（ 如图 2.9 所 示）。如果对 x、y、z 轴的单位向量进行 H变 换，分别得到 [ 4 -2 7 1 ] T 、[ 4 -3 8 1 ] T 和 [ 5 -3 7 1 ] T。这四个向量在图2.9中标出，并 形成了一个新坐标系。null 这个新坐标系的 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 相对变换（Relative transformation）2.7 相对变换（Relative transformation） 我们刚刚描述的旋转和平移都是相对于一个固定的坐标系而进行的。这样，在 已给的例子里 0 0 1 4 1 0 0 -3 Trans ( 4, -3, 7 ) Rot ( y, 90°) Rot ( z, 90°) = 0 1 0 7 （2.16） 0 0 0 1 坐标系首先绕参考坐标系 z 轴旋转90°，然后绕 y 轴旋转 90°，最后平移 4i－3j+7k， 如图2.9所示。如果以相反次序从左到右来进行这些操作：首先对坐标平移4i―3j+7k，然 后将它绕当前坐标系的 y 轴旋转 90°，此时当前坐标系的 y 轴与参考坐标系的 y 轴是相同 的。然后再绕着新坐标系（当前的）坐标系的 z 轴旋转90°，所得结果与前面的方法相同 （见图2.11）。 null null 一般的情况下，如果我们用一个旋转和/或平移变换矩阵右乘一个坐标系的变换，那么产生的平移和/或旋转是相对于前一个变换的坐标系（当前坐标系）的轴来说的。如果我们用一个描述平移和/或旋转的变换矩阵左乘一个坐标系的变换，那么产生的平移和/或旋转是相对于基坐标系来说的。 【例2.3】给一个坐标系C和一个变换T，T为绕 z 轴旋转90°，并在 x 轴方向上平移10个单位，当变换是相对于基坐标系产生时，我们用 T 左乘 C 得到新的位置 x 为 0 -1 0 10 1 0 0 20 0 0 1 0 1 0 0 0 0 0 -1 10 1 0 0 20 x = T C = 0 0 1 0 0 1 0 0 ＝ 0 1 0 0 （2.17） 0 0 0 1 0 0 0 1 0 0 0 1 当变换是相对于当前坐标系 C 轴产生时，我们用 T 右乘 C 得到新的位置 y 为 1 0 0 20 0 -1 0 10 0 -1 0 30 0 0 -1 10 1 0 0 0 0 0 -1 10 y = C T = 0 1 0 0 0 0 1 0 ＝ 1 0 0 0 （2.18） 0 0 0 1 0 0 0 1 0 0 0 1 结果如图2.12所示。null 2.8 物体的描述（Object representation）2.8 物体的描述（Object representation） 变换可用来描述物体的位置与方向（方位）。如图2.13所示的楔形物体用六个角点 来描述，这六个角点是相对于物体所在的参考坐标系的。如果把物体绕 z 轴旋转 90°， 然后绕 y 轴旋转 90°，接着沿 x 方向平移4个单位，我们可以描述这个变换为 0 0 1 4 1 0 0 0 Trans ( 4, 0, 0 ) Rot ( y, 90°) Rot ( z, 90°) = 0 1 0 0 0 0 0 1 这个变换表示了对参考坐标系的旋转和平移操作，变换后物体的六个角点为 4 4 6 6 4 4 0 0 1 4 1 -1 -1 1 1 -1 1 -1 -1 1 1 -1 1 0 0 0 0 0 0 0 4 4 0 0 0 0 4 4 = 0 1 0 0 0 0 0 2 2 0 1 1 1 1 1 1 0 0 0 1 1 1 1 1 1 1 变换后该物体在坐标上的方位如图2.13所示。null 从图2.13可以看出，由于楔形物体的角点与它所在的坐标系有固定的关系，因此没有必要对所有的角点进行变换，只要对物体所在的坐标系进行变换，就可得到变换后的各个角点在基坐标中的位置，将这些角点用直线连接起来就可得到楔形物体的边缘，它与逐点变换的结果完全相同（见图2.14）。2.9 逆变换（Inverse transformation)2.9 逆变换（Inverse transformation) 所谓逆变换就是将被变换的坐标系返回到原来的坐标系，在数学上就是求变换矩阵的逆。 下面我们写出变换矩阵的一般表达形式 nx ox ax px ny oy ay py T = nz oz az pz （2.19） 0 0 0 1 式中 n, o, a 是旋转变换列向量，p 是平移向量，其逆是 nx ny nz - p.n ox oy oz - p.o T-1 = ax ay az - p.a （2.20） 0 0 0 1 式中的 “ . ” 表示向量的点积。这个结果很容易用式2.19右乘式2.20是单位矩阵来证明。 2.10 一般性旋转变换（General rotation transformation）2.10 一般性旋转变换（General rotation transformation） 前面我们介绍的旋转变换都是绕 x，y，z 轴旋转的旋转变换，这些变换都有一个简单的几何解释。例如：在绕 z 轴旋转的情况下，表示 z 轴保持恒定，x 轴和 y 轴将如图2.15所示那样变化。 null 如图2.16所示，给出一个变换矩阵 C，它绕 任意向量 k 旋转，我们把 k当作 C坐标系的 z 轴单 位向量。 nx ox ax 0 ny oy ay 0 C = nz oz az 0 （2.21） 0 0 0 1   k = ax i + ay j + az k （2.22） 绕 k 旋转就相等于绕 C 坐标系的 z 轴旋转。 Rot（ k，θ）= Rot（Cz，θ） （2.23） 如果我们给一个坐标系T，它在参考坐标 系里被描述，它在C坐标系里用X描述，这样 T = C X （2.24） 其中X描述T相对C的位姿，求X，我们得到 X = C-1 T （2.25）nullT 绕 k 旋转就等于Ｘ绕Ｃ坐标系的 z 轴旋转 Rot（ k, θ）Ｔ ＝ C Rot（ z, θ）X （2.26） Rot（ k, θ）Ｔ ＝ C Ro t（ z, θ）C-1 T （2.27） 这样　　　　 Rot（ k, θ）＝ C Rot（ z, θ）C-1　 　　　　　　　 （2.28）　　　　　　　 展开式（2.28），我们发现 C Rot（ z, θ）C-1 仅是 k 的函数。 用C-1右乘 Rot（ z, θ） ，我们得到 　　　　　　　　 cosθ -sinθ 0 　0　　nx ny nz 0 　　　　　　　　 sinθ cosθ 0 0　　ox oy oz 0 Rot（ z, θ）C-1 ＝　 0 0 　 1 0　　ax ay az 0 　　　　　　　　 0 　　0 　 0 1　　 0 0 0 1 nx cosθ－ox sinθ ny cosθ―oy sinθ nz cosθ―oz sinθ 0 nx cosθ + ox sinθ ny cosθ + oy sinθ nz cosθ+ oz sinθ 0 = ax ay az 0 （2.29） 0 0 0 1 再用C左乘 nx ox ax 0 ny oy ay 0 C = nz oz az 0 （2.30） 0 0 0 1null得到 C Rot（ z, θ）C-1 = nxnx cosθ― nxox sinθ+ nxox sinθ+ oxox cosθ+ ax ax nynx cosθ― nyox sinθ+ nxoy sinθ+ oyox cosθ+ ay ax nznx cosθ― nzox sinθ+ nxoz sinθ+ oz ox cosθ+ az ax 0 nxny cosθ― nxoy sinθ+ nyox sinθ+ oyox cosθ+ ax ay nyny cosθ― nyoy sinθ+ nyoy sinθ+ oyoy cosθ+ ay ay nzny cosθ― nzoy sinθ+ nyoz sinθ+ oyoz cosθ+ az ay 0     nxnz cosθ― nxoz sinθ+ nzox sinθ+ ozox cosθ+ ax az 0 nynz cosθ― nyoz sinθ+ nzoy sinθ+ ozoy cosθ+ ay az 0 nznz cosθ― nzoz sinθ+ nzoz sinθ+ ozoz cosθ+ az az 0 （2.31） 0 1null应用下列关系进行简化： C 坐标系任意的行或列与其他行或列的点积为零，因为这些向量是正交的； C 坐标系任意的行或列与其自身的点积为I ，因为它们是单位量； z 向量是 x 和 y 向量的叉积：a = n × o，它有下列分量 ax = ny oz ― nz oy ay = nz ox ― nx o z az = nx oy ― ny ox 正矢 Versθ=（1―cosθ），简写成 Versθ，且 kx = ax ，ky = ay ，kz = az 。由此可得到简化式为 Rot ( k, θ) = kx kx Versθ+ cosθ ky kx Versθ―kz sinθ kz kx Versθ + kysinθ 0 kx kyVersθ+ kz sinθ ky ky Versθ+ cosθ kz kyVersθ― kzxsinθ 0 kx kzVersθ―kysinθ ky kz Versθ + kxsinθ kz kzVersθ+ cosθ 0 （2.32） 0                                    0                                  0 1 上式是一般性的旋转变换的重要结论。从这个结论可以得出每一个基本旋转变换。例如： Rot ( x, θ)就是Rot ( k, θ)当 kx= 1，ky= 0, kz= 0 的情况，将这些值代入式（2.32）得到 1 0 0 0 0 cosθ -sinθ 0 Rot ( x, θ) = 0 sinθ cosθ 0 （2.33） 0 0 0 1 这个结果与以前一样。2.11 等价旋转角与旋转轴（Equivalent angle and axis of rotation） 2.11 等价旋转角与旋转轴（Equivalent angle and axis of rotation） 任给一个旋转变换，从（2.32）方程得到一个轴，绕这个轴旋转的等价旋转角可由 如下方法得到。已知一个旋转变换 R   nx ox ax 0 ny oy ay 0 R = nz oz az 0 （2.34） 0 0 0 1 令 R 和式 （2.32）的 Rot ( k, θ) 相等，并将对角线各项相加得到 nx + oy + az +1 = k2x Versθ+ cosθ+ k2yVersθ+ cosθ + k2z Versθ+ 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 = 2 kx sinθ （2.38） ax ― nz = 2 ky sinθ （2.39） ny ― ox = 2 kz sinθ （2.40） 把式（2.38）到式（2.40）两边平方并相加有 （oz ― ay）2 +（ ax ― nz）2 +（ ny ― ox ）2 = 4 sin2θ （2.41）null我们得到了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的各分量为