問(wèn)題的提出來(lái)源于一個(gè)實(shí)際場(chǎng)景，已知機(jī)器人坐標(biāo)系與放在機(jī)器人上的相機(jī)坐標(biāo)轉(zhuǎn)換關(guān)系，當(dāng)相機(jī)移動(dòng)一段位移及旋轉(zhuǎn)以后，求該旋轉(zhuǎn)和位移在機(jī)器人坐標(biāo)系中的表示

如下圖，假設(shè)機(jī)器人坐標(biāo)系R (x朝前，y朝左，z朝上), 相機(jī)坐標(biāo)系C (處于R坐標(biāo)系上方1米，x朝右，y朝下，z朝前), 相機(jī)之后沿C的z軸負(fù)方向移動(dòng)2m，并繞C的y軸方向旋轉(zhuǎn)-45度得到新坐標(biāo)系C'

求C'旋轉(zhuǎn)與R坐標(biāo)系一致后相對(duì)于坐標(biāo)系R的新坐標(biāo)系R'（即目標(biāo)坐標(biāo)系R'應(yīng)該表示為沿R的x軸負(fù)方向移動(dòng)2m，并繞R的z軸轉(zhuǎn)45度，轉(zhuǎn)換到平面坐標(biāo)（x,y,theta）就是（-2，0，45））

為此需要先弄清變換矩陣(transformation matrix)的工作原理

繞某個(gè)坐標(biāo)系的x軸的旋轉(zhuǎn)theta角時(shí)變換矩陣可以表示為

繞某個(gè)坐標(biāo)系的y軸的旋轉(zhuǎn)theta角時(shí)變換矩陣可以表示為 

繞某個(gè)坐標(biāo)系的z軸的旋轉(zhuǎn)theta角時(shí)變換矩陣可以表示為

假設(shè)我們先繞某個(gè)參考坐標(biāo)系的Y軸旋轉(zhuǎn)90°，再繞x軸轉(zhuǎn)180°，最后平移（1.5，1，1.5），得到的變換矩陣為

順帶的，如果要變換參考坐標(biāo)系上的某個(gè)點(diǎn)p（0,1,0），可以通過(guò)下面計(jì)算的到

需要注意的是這里用的是外旋（每次繞固定軸轉(zhuǎn)，如下圖），因此是左乘變換矩陣

內(nèi)旋（每次繞自身旋轉(zhuǎn)之后的軸轉(zhuǎn)）對(duì)應(yīng)的是右乘變換矩陣

在機(jī)器人坐標(biāo)轉(zhuǎn)換中，常用的旋轉(zhuǎn)順序?yàn)?X-Y-Z，先繞X，然后是Y，最后是Z，對(duì)應(yīng)的旋轉(zhuǎn)矩陣為

R=Rz(γ)∗Ry(β)∗Rx(α)

有了這些鋪墊，回到我們最開(kāi)頭的問(wèn)題。

已知從R變換到C，通過(guò)外旋方式需要先繞R的x軸轉(zhuǎn)-90°（右手法則），然后再繞R的z軸轉(zhuǎn)-90°，最后朝R的z軸平移1.0 （注意必須是先x，后y，最后z，順序不能錯(cuò)，否則就不能用上面的旋轉(zhuǎn)矩陣）即

相應(yīng)python代碼為

import numpy as np

import math

from numpy.linalg import inv

alpha = -90 * np.pi/180

beta = 0

gamma = -90 * np.pi/180

cos_a = math.cos(alpha)

sin_a = math.sin(alpha)

cos_b = math.cos(beta)

sin_b = math.sin(beta)

cos_g = math.cos(gamma)

sin_g = math.sin(gamma)

R_T_C = np.array([[cos_g*cos_b, -sin_g*cos_a + cos_g*sin_b*sin_a, sin_g*sin_a+cos_g*sin_b*cos_a, 0],

                                 [sin_g*cos_b, cos_g*cos_a + sin_g*sin_b*sin_a, -cos_g*sin_a+sin_g*s\

in_b*cos_a, 0],

                                 [-sin_b, cos_b*sin_a, cos_b*cos_a,1],

                                 [0, 0, 0, 1]])

C_T_R = inv(R_T_C)

alpha = 0

beta = -45 * np.pi/180

gamma = 0

cos_a = math.cos(alpha)

sin_a = math.sin(alpha)

cos_b = math.cos(beta)

sin_b = math.sin(beta)

cos_g = math.cos(gamma)

sin_g = math.sin(gamma)

C_T_Ch = np.array([[cos_g*cos_b, -sin_g*cos_a + cos_g*sin_b*sin_a, sin_g*sin_a+cos_g*sin_b*cos_a, 0]\

,

                                 [sin_g*cos_b, cos_g*cos_a + sin_g*sin_b*sin_a, -cos_g*sin_a+sin_g*s\

in_b*cos_a, 0],

                                 [-sin_b, cos_b*sin_a, cos_b*cos_a,-2],

                                 [0, 0, 0, 1]])

R_T_Rh = np.dot(R_T_C, np.dot(C_T_Ch, C_T_R))

euler_x = math.atan2(R_T_Rh, R_T_Rh)

euler_y = math.atan2(-R_T_Rh, math.sqrt(R_T_Rh* R_T_Rh+ R_T_Rh* R_T_Rh\

))

euler_z = math.atan2(R_T_Rh, R_T_Rh)

print("R_T_C")

print(R_T_C)

print("C_T_Ch")

print(C_T_Ch)

print("R_T_Rh")

print(R_T_Rh)

print("to euler")

print("euler_x ",euler_x, "degree ", euler_x*180/3.14)

print("euler_y ",euler_y, 'degree ', euler_y*180/3.14)

print("euler_z ",euler_z, 'degree ', euler_z*180/3.14)

最終輸出為（-2，0，45°），符合預(yù)期

R_T_C

[[ 6.12323400e-17  6.12323400e-17  1.00000000e+00  0.00000000e+00]

 [-1.00000000e+00  3.74939946e-33  6.12323400e-17  0.00000000e+00]

 [-0.00000000e+00 -1.00000000e+00  6.12323400e-17  1.00000000e+00]

 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

C_T_Ch

[[ 0.70710678 -0.         -0.70710678  0.        ]

 [ 0.          1.         -0.          0.        ]

 [ 0.70710678  0.          0.70710678 -2.        ]

 [ 0.          0.          0.          1.        ]]

R_T_Rh

[[ 7.07106781e-01 -7.07106781e-01 -1.79345371e-17 -2.00000000e+00]

 [ 7.07106781e-01  7.07106781e-01  4.32978028e-17 -1.65762483e-16]

 [-1.79345371e-17 -4.32978028e-17  1.00000000e+00 -2.22044605e-16]

 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

to euler

('euler_x ', -4.3297802811774664e-17, 'degree ', -2.4820396516303945e-15)

('euler_y ', 1.7934537145592993e-17, 'degree ', 1.0280944860531014e-15)

('euler_z ', 0.7853981633974483, 'degree ', 45.022824653356906)