三维刚体运动 Xk+1 Xk Xk yk+1 yk +Ts 2k+1 Zk k x= cosy coso ry=sin8 sin y cosφ-cos0sim中 r=cosesin ycoso+sinesin XZ rx=cosysin中 R =sinesin wsin+cosecos ==cosesinwsino-sinecos rx =-sin y ry=sin 0 cosu r=cose cosw三维刚体运动 k k k k k z y x k k k z x z y z z yx yy yz xx xy xz k k k z y x t t t z y x r r r r r r r r r z y x R + T           =           +                     =           + + + 1 1 1                              cos cos sin cos sin cos sin sin sin cos sin sin sin cos cos cos sin cos sin cos sin sin sin sin cos cos sin cos cos = = = − = − = + = = + = − = z z z y z x yz yy yx xz xy xx r r r r r r r r r           = zx zy z z yx yy yz xx xy xz r r r r r r r r r R