一个例子搞明白机器人坐标系变换

himlen

已于 2022-08-15 12:00:24 修改

阅读量7.1k

点赞数 6

文章标签：矩阵

于 2022-08-04 10:11:04 首次发布

本文链接：https://blog.csdn.net/v1379481/article/details/126143955

版权

问题的提出来源于一个实际场景，已知机器人坐标系与放在机器人上的相机坐标转换关系，当相机移动一段位移及旋转以后，求该旋转和位移在机器人坐标系中的表示

如下图，假设机器人坐标系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，顺序不能错，否则就不能用上面的旋转矩阵）即

$^{R}T_{C} = \begin{bmatrix} 0 & 0 & 1 & 0\\ -1 & 0 & 0 & 0\\ 0 & -1 & 0 & 1\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$^{R}T_{C}$ 指C refer to R，即C以R为参考系

类似地，

$^{C}T_{C'} = \begin{bmatrix} 0.707& 0 & -0.707 & 0\\ 0 & 1 & 0 & 0\\ 0.707& 0 & 0.707 & -2\\ 0& 0 & 0 & 1 \end{bmatrix}$

为了求 $^{R}T_{R'}$ ，可以先求 $^{C}T_{R'}$ ，然后通过 $^{R}T_{R'} = \ ^{R}T_{C} \ * \ ^{C}T_{R'} = \ ^{R}T_{C} \ * \ ^{C}T_{C'}\ * \ ^{C}T_{R}$ 求出

其中， $^{C}T_{R}$ 为 $^{R}T_{C}$ 的逆矩阵，而 $^{C}T_{R'} = \ ^{C}T_{C'}\ * \ ^{C}T_{R}$ 也很好理解，因为我们已知如何从C坐标转到R坐标 $^{C}T_{R}$ ，因此在C'坐标做一次相同的内旋（右乘）就可以得到R'相对C的坐标系，由此最后推出目标转换 $^{R}T_{R'}$

相应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[2][1], R_T_Rh[2][2])
euler_y = math.atan2(-R_T_Rh[2][0], math.sqrt(R_T_Rh[2][1]* R_T_Rh[2][1]+ R_T_Rh[2][2]* R_T_Rh[2][2]\
))
euler_z = math.atan2(R_T_Rh[1][0], R_T_Rh[0][0])

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)