机器人学_第3章_齐次变换

null第三章 齐次坐标变换 第三章 齐次坐标变换 3.1 引言 3.2 点向量 3.3 平移变换 3.4 旋转变换 3.5 坐标系 3.6 相对变换 3.7 逆变换 3.8 一般性旋转变换 3.9 等价旋转角与旋转轴3.10 变换方程 3.11 小结3.1 引言 (Introduction) 机器人操作涉及到各物体之间的关系和各物体与机械手之间的关系。这一章将给出描述这些关系必须的表达方法。在这里我们将采用齐次坐标变换来描述机械手各关节坐标之间、各物体之间以及各物体与机械手之间的关系。 本章首先介绍向量的表示方法，然后引出向量的坐标变换，这些变换基本上是由平移和旋转组成，因此可以用坐标系来描述各种物体和机械手的空间位置和姿态。稍后还要介绍逆变换，逆变换是运动学求解的基础。 3.1 引言 (Introduction)3.2 点向量的描述（Notation of point vectors ） 点向量描述空间的一个点在某个坐标系的空间位置。同一个点在不同坐标系的描述及位置向量的值也不同。如图3.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 是为了方便坐标变换中的矩阵运算。3.2 点向量的描述（Notation of point vectors ）null已知两个向量 a = ax i + ay j + az k b = bx i + by j + bz k （3.1） 向量的点积是标量。用“ · ”来定义向量点积，即 a · b = ax bx + ay by + az bz （3.2 ） 向量的叉积是一个垂直于由叉积的两个向量构成的平面的向量。用“×”表示叉积，即 a × b = ( ay bz ¯ az by ) i + ( az bx ¯ ax bz ) j + ( ax by ¯ ay bx ) k （ 3.3） 可用行列式表示为 i j k a × b = ax ay az （3.4） bx by bz 3.3 平移变换（Translation transformation）3.3 平移变换（Translation transformation）{A}平移到{B} 点P在B中的描述为 把 称为B相对于A的平移矢量求使得null可见，平移实际上是对已知向量 u = [ x y z w ]T 与平移向量 h = [ a b c 1 ]T 相加null【例3.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 点向量的平移过程如图3.3所示。 3.4 旋转变换（Rotation transformation）参考坐标系Oxyz是三维 空间中的固定坐标系，Ouvw是由Oxyz经过旋转得到。 点p在Ouvw坐标系中固定不变。点p在Oxyz 系和Ouvw系 中的坐标分别表示为： 由矢量分量的定义有： Pu，pv ， pw分别表示P沿Ou,Ov,Ow轴的 分量，或P在各轴的投影，iu,，jv,，kw代表 单位向量 3.4 旋转变换（Rotation transformation）null将上式写成矩阵形式：如果Ouvw系绕Oxyz系的z轴转过角，此时，kw=kz , z坐标不会发生变化表示成齐次坐标的形式有：null 如图3.4所示，绕 x, y, z 轴旋转一个θ角 的相应变换是 1 0 0 0 0 cosθ - sinθ 0 Rot ( x, θ) = 0 sinθ cosθ 0 （3.12） 0 0 0 1      cosθ 0 sinθ 0 0 1 0 0 Rot ( y, θ) = - sinθ 0 cosθ 0 (3.13) 0 0 0 1   cosθ - sinθ 0 0 sinθ cosθ 0 0 Rot ( z, θ) = 0 0 1 0 (3.14) 0 0 0 1 注意：θ角旋转的正方向遵循右手螺旋法则（如图3.4所示）null【例3.2】点 u = 7i + 3j + 2k，它绕z轴旋转90°为v， 经式（3.14）变换得到（ sinθ=1，cosθ=0） 0 -1 0 0 7 -3 1 0 0 0 3 7 v = Rot ( z, 90°) u = 0 0 1 0 2 = 2 0 0 0 1 1 1 起始点u和终点v如图3.5所示。如将v点再绕y轴 旋转90°得到w。用式（3.13）变换得到 0 0 1 0 -3 2 0 1 0 0 7 7 w = Rot ( y, 90°)v = -1 0 0 0 2 = 3 0 0 0 1 1 1   结果如图3.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 逆序旋转的结果如图3.7所示。显然，变换的顺序不同，其结果也不同 。这从 矩阵相乘是不可交换的（AB≠BA）也可以得到证明。 null如对经过两次旋转变换得到的点向量w再进行一次平移（平移向量为 h ＝ [ 4 -3 7 1]T ）， 则可得到如图3.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 3.5 坐标系 (Coordinate frames) 齐次变换矩阵H由四个列向量组成，它的前三个列向量称为方向向量，由式 （3.12）到式（3.14）的旋转变换（分别绕 x、y、z 轴旋转θ角）确定，第四个列向 量称为平移向量，它的平移分量（沿 x、y、z 轴的平移量）由式（3.10）第四列的前 三个元素确定。如　 0 0 1 4 1 0 0 -3 H＝Trans ( 4, -3, 7 ) Rot ( y, 90°) Rot ( z, 90°) = 0 1 0 7 （3.15） 0 0 0 1 坐标系的原点，即零向量 [ 0 0 0 1 ] T 的 H 变换是 [ 4 -3 7 1 ] T，相当于将原点按平移 向量的各个分量进行平移的结果（ 如图 3.9 所 示）。如果对 x、y、z 轴的单位向量进行 H变 换，分别得到 [ 4 -2 7 1 ] T 、[ 4 -3 8 1 ] T 和 [ 5 -3 7 1 ] T。这四个向量在图3.9中标出，并 形成了一个新坐标系。 3.5 坐标系 (Coordinate frames) 0zyxzyx0Trans ( 4, -3, 7 )Rot ( z, 90°)Rot ( y, 90°)图3.9 坐标原点与单位向量的H 变换 null 这个新坐标系的 x、y、z 轴的方向分别是 [ 0，1，0，0 ] T、[ 0，0，1，0 ] T 和 [ 1，0，0，0 ] T，它是由单位向量的H变换减去这个坐标原点的向量得到的。这些方向向量相应于变换矩阵的前三列（见式（3.15））。可见，H变换矩阵描述了一个坐标系绕原参考坐标系旋转和对参考坐标系平移的三个轴的方向和原点的位置（见图3.9）。如图3.10所示，当对一个向量 n 进行式（3.15）给出的 H 变换时，原向量 n 可以被认为是在新坐标系描述的那个向量 u ，即被变换了的向量 u 就是相对于参考坐标系描述的同一个向量 n 。00zzyyxxu ( 7, 3, 2, 1 )•n ( 6, 4, 10, 1 )图3.10 向量的 H 变换3.6 相对变换（Relative transformation）3.6 相对变换（Relative transformation） 我们刚刚描述的旋转和平移都是相对于一个固定的坐标系而进行的。这样，在 已给的例子里 0 0 1 4 1 0 0 -3 Trans ( 4, -3, 7 ) Rot ( y, 90°) Rot ( z, 90°) = 0 1 0 7 （3.16） 0 0 0 1 坐标系首先绕参考坐标系 z 轴旋转90°，然后绕 y 轴旋转 90°，最后平移 4i－3j+7k， 如图3.9所示。如果以相反次序从左到右来进行这些操作：首先对坐标平移4i―3j+7k，然 后将它绕当前坐标系的 y 轴旋转 90°，此时当前坐标系的 y 轴与参考坐标系的 y 轴是相同 的。然后再绕着新坐标系（当前的）坐标系的 z 轴旋转90°，所得结果与前面的方法相同 。 null相对固定坐标系运动null相对活动坐标系运动null 一般的情况下，如果我们用一个旋转和/或平移变换矩阵右乘一个坐标系的变换，那么产生的平移和/或旋转是相对于前一个变换的坐标系（当前坐标系）的轴来说的。如果我们用一个描述平移和/或旋转的变换矩阵左乘一个坐标系的变换，那么产生的平移和/或旋转是相对于基坐标系(固定坐标系)来说的。3.7逆变换（Inverse transformation) 3.7逆变换（Inverse transformation) 所谓逆变换就是将被变换的坐标系返回到原来的坐标系，在数学上就是求变换矩阵的逆。 下面我们写出变换矩阵的一般表达形式 nx ox ax px ny oy ay py T = nz oz az pz （3.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 （3.20） 0 0 0 1 式中的 “ . ” 表示向量的点积。这个结果很容易用式3.19右乘式3.20是单位矩阵来证明。 3.8一般性旋转变换（General rotation transformation）3.8一般性旋转变换（General rotation transformation） 前面我们介绍的旋转变换都是绕 x，y，z 轴旋转的旋转变换，这些变换都有一个简单的几何解释。例如：在绕 z 轴旋转的情况下，表示 z 轴保持恒定，x 轴和 y 轴将如图3.15所示那样变化。 null 如图3.16所示，给出一个变换矩阵 C，它绕 任意向量 k 旋转，我们把 k当作 C坐标系的 z 轴单 位向量。 nx ox ax 0 ny oy ay 0 C = nz oz az 0 （3.21） 0 0 0 1   k = ax i + ay j + az k （3.22） 绕 k 旋转就相等于绕 C 坐标系的 z 轴旋转。 Rot（ k，θ）= Rot（Cz，θ） （3.23） 如果我们给一个坐标系T，它在参考坐标 系里被描述，它在C坐标系里用X描述，这样 T = C X （3.24） 其中X描述T相对C的位姿，求X，我们得到 X = C-1 T （3.25）CnullT 绕 k 旋转就等于Ｘ绕Ｃ坐标系的 z 轴旋转 Rot（ k, θ）Ｔ ＝ C Rot（ z, θ）X （3.26） Rot（ k, θ）Ｔ ＝ C Ro t（ z, θ）C-1 T （3.27） 这样　　　　 Rot（ k, θ）＝ C Rot（ z, θ）C-1　 　　　　　　　 （3.28）　　　　　　　 展开式（3.28），我们发现 C Rot（ z, θ）C-1 仅是 k 的函数。 用C-1右乘 Rot（ z, θ） ，我们得到 　　　　　　　　 cosθ -sinθ 0 　0　　nx ny nz 0 　Rot（ z, θ）C-1 ＝　　sinθ cosθ 0 0　　ox oy oz 0 　 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 （3.29） 0 0 0 1 再用C左乘 nx ox ax 0 C = ny oy ay 0 nz oz az 0 （3.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 （3.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θ― kxsinθ 0 kx kzVersθ―kysinθ ky kz Versθ + kxsinθ kz kzVersθ+ cosθ 0 （3.32） 0                                    0                                  0 1 上式是一般性的旋转变换的重要结论。从这个结论可以得出每一个基本旋转变换。例如： Rot ( x, θ)就是Rot ( k, θ)当 kx= 1，ky= 0, kz= 0 的情况，将这些值代入式（3.32）得到 1 0 0 0 0 cosθ -sinθ 0 Rot ( x, θ) = 0 sinθ cosθ 0 （3.33） 0 0 0 1 这个结果与以前一样。3.9 等价旋转角与旋转轴（Equivalent angle and axis of rotation） 3.9 等价旋转角与旋转轴（Equivalent angle and axis of rotation） 任给一个旋转变换，从（3.32）方程得到一个轴，绕这个轴旋转的等价旋转角可由 如下方法得到。已知一个旋转变换 R   nx ox ax 0 ny oy ay 0 R = nz oz az 0 （3.34） 0 0 0 1 令 R 和式 （3.32）的 Rot ( k, θ) 相等，并将对角线各项相加得到 nx + oy + az +1 = k2x Versθ+ cosθ+ k2yVersθ+ cosθ + k2z Versθ+ cosθ+1 （3.35） nx + oy + az = ( k2x + k2y + k2z ) Versθ+ 3cosθ = 1 + 2cosθ （3.36） 由此可得到旋转角的余弦是 cosθ = 1/2（nx + oy + az―1） （3.37） 对非对角线项相减，我们得到 oz ― ay = 2 kx sinθ （3.38） ax ― nz = 2 ky sinθ （3.39） ny ― ox = 2 kz sinθ （3.40） 把式（3.38）到式（3.40）两边平方并相加有 （oz ― ay）2 +（ ax ― nz）2 +（ ny ― ox ）2 = 4 sin2θ （3.41） null我们得到了sinθ的表达式 sinθ = ±1/2√（oz ―ay）2 +（ ax ―nz）2 +（ ny ―ox ）2 （3.42） 规定这个旋转是绕 k 正方向旋转，当 0≤θ≤180°时，在上式中取十号是合理的。 这个旋转角θ被唯一定义为 tanθ =√（oz ―ay）2 +（ ax ―nz）2 +（ ny ―ox ）2 /（nx + oy + az―1 ） （3.43） k的各分量为 kx =（oz―ay）/ 2 sinθ （3.44） ky =（ax―nz）/ 2 sinθ （3.45） kz =（ny―ox）/ 2 sinθ （3.46） 3.10变换方程（Transform equations） 3.10变换方程（Transform equations） 研究一下图3.18描述的一个物体与机械手情 况，机械手用变换 Z 相对于基坐标系被定位。 机械手的端点用变换 ZT6 来描述，而末端执行器 用变换 T6E 来描述。物体用变换 B 相对于基坐 标系被定位。最后，机械手末端抓手用变换 BG 相对于物体被定位。末端抓手位置的描述有两种 方式，一种是相对于物体的描述，一种是相对于 机械手的描述。由于两种方式描述的是同一个 点，我们可以把这个描述等同起来，得到 Z ZT6 T6E = B BG （3.61） 这个方程可以用有向变换图来表示（见图 3.19）。图的每一段弧表示一个变换。从它的定 义的坐标系向外指向。 用 Z-1左乘和用E-1右乘方程（3.61），得到 T6 = Z-1 B G E-1 （3.62）null 从有向变换图上我们可以直接得到上述结果，从T6弧线的尾部开始，沿着图形顺时针依次列出各个变换，直到T6弧的箭头为止。在逆变换时，我们从T6弧的箭头开始，按逆时针方向依次列出各个变换，直到T6弧的起始点为止，则可得到T6的逆 T6-1 = E G-1 B-1 Z （3.63） 对上式求逆得到与式（3.62）完全相同的结果。 作为进一步的例子，假设一个物体 B 的位置不知道，但机械手移动，使得末端抓手正好定位在物体上面。然后用 G-1 右乘式（3.61）求出 B 。或者在有向变换图中从 B 的尾部沿着逆时针方向到达弧 B 的箭头，直接得到同样结果。 B = Z T6 E G-1 （3.64） 同样，我们可以用有向变换图求出变换的连接组。例如 Z T6 = B G E-1 （3.65） 用有向变换图简化了变换方程的求解，可以直接写出变换结果。为了避免画圆，我们用图3.20所示的形式表示这个变换图，其中虚线表示那两个节点是被连在一起的，中间各垂线段表示相对坐标系。 3.11 小结（Summary） 3.11 小结（Summary） 齐次变换可以用来描述空间坐标系的位置与方向。如果坐标系被固定在物体或机械手连杆上，那么该物体或机械手的位置与方向同样很容易被描述。 物体A相对于物体B的齐次变换可以求其逆，来获得物体B相对于物体A的描述。 变换可以表示为旋转变换和/或平移变换的乘积。如果变换是从左到右，那么旋转和/或平移是相对于当前的坐标系。如果变换是从右到左，那么旋转和/或平移是相对于参考坐标系进行。 齐次变换用正交分量来描述坐标系，即用角度的正弦和余弦。这种描述可与旋转联系起来。在一般性旋转的情况下，旋转是绕任意向量旋转θ角。