【轉】位姿變換

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)二、歐拉角與旋轉矩陣歐拉角是描述剛體旋轉最直觀的一種形式，它每每只用於可視化的人機交互，而沒法直接參與運算。 
對於三維空間座標系，咱們一般這樣規定： ⎧⎩⎨繞 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]有了歐拉角與旋轉矩陣之間的轉換關係，下面舉一個簡單的例子驗證一下： 
如上圖所示，有位於立方體頂點上的三個座標系O1O一、O2O二、O3O3，以及位於立方體中心的點PP，立方體的邊長分別爲六、八、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 版權聲明：本文爲博主原創文章，轉載請附上博文連接！