問題的提出來源于一個實際場景，已知機器人坐標系與放在機器人上的相機坐標轉換關系，當相機移動一段位移及旋轉以后，求該旋轉和位移在機器人坐標系中的表示

如下圖，假設機器人坐標系R (x朝前，y朝左，z朝上), 相機坐標系C (處于R坐標系上方1米，x朝右，y朝下，z朝前), 相機之后沿C的z軸負方向移動2m，并繞C的y軸方向旋轉-45度得到新坐標系C'

求C'旋轉與R坐標系一致后相對于坐標系R的新坐標系R'（即目標坐標系R'應該表示為沿R的x軸負方向移動2m，并繞R的z軸轉45度，轉換到平面坐標（x,y,theta）就是（-2，0，45））

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

繞某個坐標系的x軸的旋轉theta角時變換矩陣可以表示為

繞某個坐標系的y軸的旋轉theta角時變換矩陣可以表示為 

繞某個坐標系的z軸的旋轉theta角時變換矩陣可以表示為

假設我們先繞某個參考坐標系的Y軸旋轉90°，再繞x軸轉180°，最后平移（1.5，1，1.5），得到的變換矩陣為

順帶的，如果要變換參考坐標系上的某個點p（0,1,0），可以通過下面計算的到

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

內旋（每次繞自身旋轉之后的軸轉）對應的是右乘變換矩陣

在機器人坐標轉換中，常用的旋轉順序為 X-Y-Z，先繞X，然后是Y，最后是Z，對應的旋轉矩陣為

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

有了這些鋪墊，回到我們最開頭的問題。

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

相應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°），符合預期

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)