点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达![]()
重磅干货，第一时间送达
问题的提出来源于一个实际场景，已知机器人坐标系与放在机器人上的相机坐标转换关系，当相机移动一段位移及旋转以后，求该旋转和位移在机器人坐标系中的表示
如下图，假设机器人坐标系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 npimport mathfrom numpy.linalg import invalpha = -90 * np.pi/180beta = 0gamma = -90 * np.pi/180cos_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 = 0beta = -45 * np.pi/180gamma = 0cos_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)
版权声明：本文为CSDN博主「himlen」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/v1379481/article/details/126143955
编辑：古月居
声明：部分内容来源于网络，仅供读者学习、交流之目的。文章版权归原作者所有。如有不妥，请联系删除。
下载1：OpenCV-Contrib扩展模块中文版教程 在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。 下载2：Python视觉实战项目52讲 在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。 下载3：OpenCV实战项目20讲 在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。 交流群
欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~
