【转】位姿变换

https://blog.csdn.net/lixujie666/article/details/82153503

一、位姿变换1、旋转矩阵与变换矩阵旋转矩阵是描述刚体旋转最常见的一种形式，而变换矩阵通常是指旋转矩阵与平移向量组成的齐次变换矩阵。对于三维空间的位姿变换，有旋转矩阵： R=[r1r2r3]=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥R=[r1r2r3]=[r11r12r13r21r22r23r31r32r33]平移向量： t=[t1t2t3]Tt=[t1t2t3]T则旋转矩阵RR与平移向量tt组成的齐次变换矩阵为： T=[R0Tt1]=⎡⎣⎢⎢⎢⎢r11r21r310r12r22r320r13r23r330t1t2t31⎤⎦⎥⎥⎥⎥T=[Rt0T1]=[r11r12r13t1r21r22r23t2r31r32r33t30001]旋转矩阵RR是一种单位正交阵，它具有单位正交阵所有的性质： {r1T⋅r2=r1T⋅r3=r2T⋅r3=0∥r1∥=∥r2∥=∥r3∥=det(R)=1{r1T⋅r2=r1T⋅r3=r2T⋅r3=0‖r1‖=‖r2‖=‖r3‖=det(R)=1重要的是，旋转矩阵的逆等于旋转矩阵的转置，即R−1=RTR−1=RT，从而有RR−1=RRT=IRR−1=RRT=I。齐次变换矩阵TT的逆为： T−1=[R−10T−R−1t1]=[RT0T−RTt1]T−1=[R−1−R−1t0T1]=[RT−RTt0T1]假设我们由坐标系A经过变换矩阵ABT=[R0Tt1]TBAT=[Rt0T1]得到了坐标系B，那么坐标系B下的点BPPBP可在坐标系A中表示为： AP=ABTBP=[R0Tt1][BP1]=RBP+tPAP=TBAT⁡PBP=[Rt0T1][PBP1]=RPBP+t同样，坐标系A下的点APPAP可在坐标系B中表示为： BP=BATAP=ABT−1AP=[RT0T−RTt1][AP1]=RTAP−RTt=RT(AP−t)PBP=TABT⁡PAP=TBAT−1⁡PAP=[RT−RTt0T1][PAP1]=RTPAP−RTt=RT(PAP−t)2、欧拉角与旋转矩阵欧拉角是描述刚体旋转最直观的一种形式，它往往只用于可视化的人机交互，而无法直接参与运算。 
对于三维空间坐标系，我们通常这样规定： ⎧⎩⎨绕 X 轴旋转的角度称为横滚角，Roll绕 Y 轴旋转的角度称为俯仰角，Pitch绕 Z 轴旋转的角度称为偏航角，Yaw{绕 X 轴旋转的角度称为横滚角，Roll绕 Y 轴旋转的角度称为俯仰角，Pitch绕 Z 轴旋转的角度称为偏航角，Yaw所有的旋转，沿着旋转轴方向顺时针旋转的角度为正，逆时针旋转的角度为负。对于平移，沿着该轴正方向平移的距离为正，负方向平移的距离为负。
设刚体绕着ZZ轴旋转γγ角度，那么刚体上所有点的zz坐标值不变，xx与yy的坐标值分别变为： x′=ρcos(θ+γ)=ρ(cosθ⋅cosγ−sinθ⋅sinγ)=x⋅cosγ−y⋅sinγy′=ρsin(θ+γ)=ρ(cosθ⋅sinγ+sinθ⋅cosγ)=x⋅sinγ+y⋅cosγx′=ρcos(θ+γ)=ρ(cosθ⋅cosγ−sinθ⋅sinγ)=x⋅cosγ−y⋅sinγy′=ρsin(θ+γ)=ρ(cosθ⋅sinγ+sinθ⋅cosγ)=x⋅sinγ+y⋅cosγ写成矩阵的形式： ⎡⎣⎢x′y′z′⎤⎦⎥=⎡⎣⎢cosγsinγ0−sinγcosγ0001⎤⎦⎥⎡⎣⎢xyz⎤⎦⎥=RZ(γ)⎡⎣⎢xyz⎤⎦⎥[x′y′z′]=[cosγ−sinγ0sinγcosγ0001][xyz]=RZ(γ)[xyz]从而，我们总结出欧拉角与旋转矩阵的关系： ⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Roll：RX(α)=rotx(α)=⎡⎣⎢1000cosαsinα0−sinαcosα⎤⎦⎥Pitch：RY(β)=roty(β)=⎡⎣⎢cosβ0−sinβ010sinβ0cosβ⎤⎦⎥Yaw：RZ(γ)=rotz(γ)=⎡⎣⎢cosγsinγ0−sinγcosγ0001⎤⎦⎥{Roll：RX(α)=rotx(α)=[1000cosα−sinα0sinαcosα]Pitch：RY(β)=roty(β)=[cosβ0sinβ010−sinβ0cosβ]Yaw：RZ(γ)=rotz(γ)=[cosγ−sinγ0sinγcosγ0001]有了欧拉角与旋转矩阵之间的转换关系，下面举一个简单的例子验证一下： 
如上图所示，有位于立方体顶点上的三个坐标系O1O1、O2O2、O3O3，以及位于立方体中心的点PP，立方体的边长分别为6、8、10。下面先进行三个坐标系之间的变换：
O1→O2O1→O2：
相对变换：O1O1先沿着坐标轴O1X1O1X1的正方向平移10个单位到达O2O2，然后绕着当前位置的O1Z1O1Z1轴顺时针旋转90°，此时新得到的O1O1与O2O2坐标系完全重合，即完成了O1O1到O2O2的坐标系变换。以上过程可以在MATLAB中用变换矩阵表述为： 12T=transl(10,0,0)*trotz(pi/2)=⎡⎣⎢⎢⎢10000100001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥T21T=transl(10,0,0)*trotz(pi/2)=[10010010000100001][0−100100000100001]=[0−1010100000100001]绝对变换：O1O1先绕着坐标轴O1Z1O1Z1顺时针旋转90°，然后沿着原来O1X1O1X1轴的正方向平移10个单位到达O2O2，此时新得到的O1O1与O2O2坐标系完全重合，即完成了O1O1到O2O2的坐标系变换。以上过程可以在MATLAB中用变换矩阵表述为：12T′=transl(10,0,0)*trotz(pi/2)=⎡⎣⎢⎢⎢10000100001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥=12TT21T′=transl(10,0,0)*trotz(pi/2)=[10010010000100001][0−100100000100001]=[0−1010100000100001]=T21T你可能会问，两次不同的操作为什么表达式是一样的？原因是这里牵涉到相对变换与绝对变换的区别： ⎧⎩⎨相对变换：每一步都以新得的坐标系为参考，每一步得到的变换矩阵依次右乘；绝对变换：始终以最初的坐标系为参考，通常是先旋转再平移，每一步得到的变换矩阵依次左乘；相对变换与绝对变换不可同时使用！{相对变换：每一步都以新得的坐标系为参考，每一步得到的变换矩阵依次右乘；绝对变换：始终以最初的坐标系为参考，通常是先旋转再平移，每一步得到的变换矩阵依次左乘；相对变换与绝对变换不可同时使用！当然，O1→O2O1→O2的相对变换还可以有很多种操作顺序，比如： 12T′′=trotz(pi/2)*transl(0,-10,0)=⎡⎣⎢⎢⎢0100−100000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢1000010000100−1001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥=12TT21T″=trotz(pi/2)*transl(0,-10,0)=[0−100100000100001][1000010−1000100001]=[0−1010100000100001]=T21T等等，这里不再一一列举。
O2→O1O2→O1：
相对变换： 21T=transl(0,10,0)*trotz(-pi/2)=⎡⎣⎢⎢⎢10000100001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥=trotz(-pi/2)*transl(-10,0,0)=⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢100001000010−10001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥T12T=transl(0,10,0)*trotz(-pi/2)=[10000101000100001][0100−100000100001]=trotz(-pi/2)*transl(-10,0,0)=[0100−100000100001][100−10010000100001]=[0100−1001000100001]绝对变换： 21T′=transl(0,10,0)*trotz(-pi/2)=⎡⎣⎢⎢⎢10000100001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−100100000100001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥=21TT12T′=transl(0,10,0)*trotz(-pi/2)=[10000101000100001][0100−100000100001]=[0100−1001000100001]=T12T相信这些表达式的意义都很容易理解，接下来验证一下12TT21T与21TT12T是不是互逆： 12T21T=⎡⎣⎢⎢⎢0100−1000001010001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢1000010000100001⎤⎦⎥⎥⎥=IT21TT12T=[0−1010100000100001][0100−1001000100001]=[1000010000100001]=I显然，O1→O2O1→O2与O2→O1O2→O1互为逆变换，12TT21T与21TT12T互逆。其实，由O1O1到O2O2的变换矩阵就是对坐标系O2O2在坐标系O1O1中位姿的一种描述，即12TT21T描述了O2O2在O1O1中的位置和姿态。同理，21TT12T描述了O1O1在O2O2中的位置和姿态。那么，我们已经知道了点PP在O1O1坐标系下的坐标为1P=(5,4,3)TP1P=(5,4,3)T，坐标系O1O1在坐标系O2O2下的位姿为21TT12T，顺理成章地，我们就可以求得点PP在O2O2坐标系下的坐标为： 2P=21T1P=⎡⎣⎢⎢⎢0−1001000001001001⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢5431⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥P2P=T12T⁡P1P=[0100−1001000100001][5431]=[4531]很显然，结果跟事实是一致的。
O2↔O3O2↔O3： 23T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥32T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥23T32T=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥2=⎡⎣⎢⎢⎢1000010000100001⎤⎦⎥⎥⎥=I3P=32T2P=⎡⎣⎢⎢⎢−1000010000−108061⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥T32T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=[−1008010000−160001]T23T=transl(8,0,6)*troty(pi)=troty(pi)*transl(-8,0,-6)=[−1008010000−160001]T32TT23T=[−1008010000−160001]2=[1000010000100001]=IP3P=T23T⁡P2P=[−1008010000−160001][4531]=[4531]O1→O3O1→O3： 13T=transl(10,8,6)*trotz(pi/2)*troty(pi)=trotz(-pi/2)*trotx(pi)*transl(-8,-10,-6)=12T23T=⎡⎣⎢⎢⎢0−100−100000−1010861⎤⎦⎥⎥⎥3P=31T1P=13T−11P=⎡⎣⎢⎢⎢0−100−100000−1081061⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢5431⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢4531⎤⎦⎥⎥⎥--------------------- 作者：嵙杰 来源：CSDN 原文：https://blog.csdn.net/lixujie666/article/details/82153503 版权声明：本文为博主原创文章，转载请附上博文链接！