机器人的数学基础齐次变换矩阵及其运算

LOGO机器人学基础——齐次变换矩阵及其运算上海电机学院机械学院齐次变换矩阵及其运算0001xxxxyyyyzzzznoapnoapFnoap由于各种原因，变换矩阵应写成方型形式，3*3或4*4均可.为保证所表示的矩阵为方阵，如果在同一矩阵中既表示姿态又表示位置，那么可在矩阵中加入比例因子使之成为4*4矩阵。上海电机学院机械学院变换可定义为空间的一个运动。已知一直角坐标系中的某点坐标，那么该点在另一直角坐标系中的坐标可通过齐次坐标变换来求得。变换可分为如下形式：纯平移纯旋转平移与旋转的结合上海电机学院机械学院1.平移的齐次变换空间某一点在直角坐标系中的平移,由A(x,y,z)平移至A′(x′,y′,z′),即zzzyyyxxx'''110001000100011'''zyxzyxzyxa′=Trans(Δx,Δy,Δz)a平移算子上海电机学院机械学院①算子左乘:表示点的平移是相对固定坐标系进行的坐标变换。②算子右乘:表示点的平移是相对动坐标系进行的坐标变换。③该公式亦适用于坐标系的平移变换、物体的平移变换,如机器人手部的平移变换。1000100010001),,(Transzyxzyx上海电机学院机械学院例动坐标系{A}相对于固定坐标系的X0、Y0、Z0轴作(-1,2,2)平移后到{A’}；动坐标系{A}相对于自身坐标系(即动系)的X、Y、Z轴分别作(-1,2,2)平移后到{A’’}。已知A,写出坐标系{A’}、{A’’}1000110010011010A1000310030010010'A1000110020011010''A上海电机学院机械学院2．旋转的齐次变换点在空间直角坐标系中的旋转如图所示。A(x,y,z)绕Z轴旋转θ角后至A’(x’,y’,z’),则A与A’之间的关系为：zzyxyyxx'cossin'sincos'11000010000cossin00sincos1'''zyxzyx记为:a′=Rot(z,θ)a旋转算子上海电机学院机械学院1000010000cossin00sincos),(zRot同理，绕x轴、Y轴旋转算子内容为：绕Z轴旋转算子内容为：10000cossin00sincos00001),(xRot10000cos0sin00100sin0cos),(yRot上海电机学院机械学院如图所示单操作手臂，并且手腕也具有一个旋转自由度。已知手部的起始位姿矩阵为G1.若手臂绕Z0轴旋转90°，则手臂到达G2；若手臂不动，仅手部绕手腕Z1轴转90°，则手部到达G3.写出手部坐标系G2、G3表达式。上海电机学院机械学院上海电机学院机械学院3．复合齐次变换复合变换是由固定参考坐标系或当前运动坐标系的一系列沿轴平移和绕轴旋转变换所组成的。任何变换都可以分解为按一定顺序的一组平移和旋转变换。相对于固定坐标系相对于动坐标系算子左乘算子右乘上海电机学院机械学院已知坐标系中点U的位置矢量，将此点绕Z轴旋转90°，再绕Y轴旋转90°，如图所示，求旋转变换后所得的点W。Tu1237Rot(,90)Rot(,90)YZWU700100100010010003100000102000100011上海电机学院机械学院平移变换和旋转变换可以组合在一个齐次变换中。上例中点U若还要作4i-3j+7k的平移，则只要左乘上平移变换算子即可得到最后的列阵表达式。uzRotyRotTransE)90,()90,()7,3,4(上海电机学院机械学院上海电机学院机械学院齐次变换矩阵的数学意义：ABT（1）同一点在不同坐标系{B}和{A}中的变换；（2）描述坐标系{B}相对于坐标系{A}的位置和方位；（3）点的运动算子。pTpBABA1000401030011100TAB1000100010001),,(Transzyxzyx1000010000cossin00sincos),(zRot上海电机学院机械学院4．变换矩阵相乘对于给定的坐标系｛A｝、｛B｝、｛C｝，已知｛B｝相对｛A｝的描述为，｛C｝相对｛B｝的描述为，则pTpCBCBpTTpTpCBCABBABA。从而定义复合变换表示｛C｝相对于｛A｝的描述，是两变换矩阵的乘积。注意：变换矩阵相乘不满足“交换律”，变换矩阵的左乘和右乘的运动解释不同。TTTBCABACTTTBCABACTABTBC上海电机学院机械学院101000CBBCBAABBCABACpRpRTTT复合变换可解释为：TACTBCTABTBCTAC(1)和分别代表同一坐标系{C}相对于{A}和{B}的描述。则表示坐标系{C}从映射为的变换。(2)坐标系{C}相对于{A}的描述是这样得到的：最初{C}与{A}重合，首先相对于{A}作运动，到达{B},然后相对{B}作运动，到达最终位置{C}。TACTABTBC上海电机学院机械学院5.变换矩阵求逆如果知道坐标系｛B｝相对于｛A｝的描述。希望得到｛A｝相对于｛B｝的描述，这是个齐次变换求逆问题。•对4*4矩阵直接求逆；•利用齐次变换矩阵的特点，简化矩阵求逆运算。求逆问题可以描述为：已知，求解。TABTBA100BAABABpRT100ABBABApRT利用旋转矩阵正交性TABABR1R利用复合变换公式(2.13),求出在{B}中描述。0BAp上海电机学院机械学院100BATABTABBApRRT0)(000ABBABABABppRp000BATABBABAABpRpRp上海电机学院机械学院下面我们写出变换矩阵的一般表达形式nxoxaxpxnyoyaypyT=nzozazpz0001式中n,o,a是旋转变换列向量，p是平移向量，其逆是nxnynz-p.noxoyoz-p.oT-1=axayaz-p.a0001式中的“.”表示向量的点积。上海电机学院机械学院计算T矩阵的逆矩阵。0.500.86630.86605201050001T10.50.8660(30.520.86650)001(302051)0.8660.50(30.86620.550)0001T0.50.86603.2300150.8660.501.5980001-0.5上海电机学院机械学院6变换方程为了描述机器人的操作，必须建立机器人本身各连杆之间，机器人与周围环境之间的运算关系。为此要规定各种坐标系来描述机器人与环境的相对位姿关系。{B}代表基座坐标系；{W}代表腕部坐标系；{T}代表工具坐标系；{S}代表工作站坐标系；{G}代表目标坐标系；它们之间的位姿关系用相应的齐次变换来描述。TBS描述工作站坐标系相对于基座的位姿；TSG描述目标坐标系相对于{S}的位姿；TBW描述腕部{W}相对于基座{B}的位姿；………………上海电机学院机械学院TGT对物体进行操作时，工具坐标系{T}相对于目标坐标系{G}的位姿直接影响操作效果。它是机器人控制和规划的目标。实际上，它与其他变换之间的关系类似于空间尺寸链，TGT则是封闭环。如图所示，工具坐标系{T}相对于基座坐标系{B}的描述可用两种变换矩阵的乘积来表示：TTTWTBWBTTTTTGTSGBSBT令上面两式相等，则得变换方程TTTTTGTSGBSWTBW上海电机学院机械学院变换方程中的任一变换矩阵都可用其余的变换矩阵来表示。例如，为了对目标物进行有效操作，工具坐标系{T}相对于目标坐标系{G}的位姿是预先规定的，需要改变以达到这一目的，即通常规定，求。TGTTBW1TTTTTWTGTSGBSBW根据变换方程，可以立即求出TGTTBW上海电机学院机械学院kjikzyxkkk旋转变换通式令是过原点的单位矢量，求绕k旋转θ角的旋转矩阵R(k,θ)。问题描述：令),(kRotRAB即R(k,θ)表示坐标系{B}相对于参考系{A}的方位。坐标系{B}由坐标系{A}绕轴旋转角得到。kk{A}上海电机学院机械学院kxAyAzAxByBzBAAyAxAzBBxByBzAB旋转变换通式xxxABAByyyzzznokRRnoknok再定义两坐标系{A’}和{B’}，分别与{A}和{B}固接，但要求(1){A’}和{B’}的z轴与k重合。(2)旋转之前{A’}和{B’}重合，{A}和{B}也重合。上海电机学院机械学院AAAABBABBRRRRcossin0sincos0001ABR又因为所以可以得到：坐标系{B}绕k轴相对于{A}旋转θ角相当于：坐标系{B’}相对于{A’}的z轴旋转θ角，保持其他关系不变。则TBBBBRR''kxAyAzAxByBzBAAyAxAzBBxByBzAB坐标系{A}经过如下变换到坐标系{B}:上海电机学院机械学院1cossin0sincos0001cossin0sincos0001xxxxxxAAABBABByyyyyyzzzzzzxxxxyzyyyxyzzzzxyznoknokRRRRnoknoknoknoknoknnnnokooonokkkk把上式右端三矩阵相乘，并运用旋转矩阵的正交性质：onknkkoonkkoonn01上海电机学院机械学院1c1c1c1c1c1c1c1c1cxxyxzzxyABxyzyyzyxxzyyzxzzkkckkkskkksRkkkskkckkkskkkskkkskkc①该式为一般旋转齐次变换通式，概括了绕X、Y、Z轴进行旋转变换的情况。反之，当给出某个旋转齐次变换矩阵，则可求得k及转角θ。②变换算子公式不仅适用于点的旋转，也适用于矢量、坐标系、物体的旋转。③左乘是相对固定坐标系的变换；右乘是相对动坐标系的变换。当kx=1，ky=kz=0时…………当ky=1，kx=kz=0时…………当kz=1，kx=ky=0时…………上海电机学院机械学院反之，若给出某个旋转齐次矩阵则可根据求出其等效矢量k及等效转角θ),(Rotk上海电机学院机械学院等效转轴和等效转角给定旋转矩阵，求对应的等效旋转轴和等效转角ABRkxxxyyyzzznoaRnoanoa设，令1c1c1c1c1c1c1c1c1cxxxyyyzzzxxyxzzxyxyzyyzyxxzyyzxzznoaRnoanoakkckkkskkkskkkskkckkkskkkskkkskkc上海电机学院机械