我想用python制作一个程序，它将采用点坐标（XYZ-ABC），例如： POINT = X 100，Y 200，Z 120， A -90，B 0，CO 相对于基准： B = X 0，Y 200，Z 0，A 0，B 0，C 0 并找出相同点相对于另一点的坐标基础： A = X 100，Y 200，Z 0，A 0，B 0，C 0。我发现了很多关于3D转换的信息，但我不知道从哪里开始。我也有transformation.py库。我需要一些关于如何去做这件事的提示，我必须用数学术语来说明哪些步骤。针对2种不同的基础改变点表示
这些点是机器人位置的笛卡尔坐标。 XYZ（平移）和ABC（Rz，Ry，Rx旋转）欧拉角，相对于基础或框架。我需要（我认为）找到这个位置的单位矢量矩阵。这是我迄今所做的：
C(b)C(a) S(c)S(b)C(a)-C(c)S(a) C(c)S(b)C(a)+S(c)S(a) x
C(b)S(a) C(c)C(a)+S(c)S(b)S(a) C(c)S(b)S(a)-S(c)C(a) y
-S(b) S(c)C(b) C(c)C(b) z
0 0 0 1
//For example point P= [X -534.884033,Y -825.747070, Z 1037.32373,
A -165.214142,B -3.16937923,C -178.672119]
我已经阅读了这个问题太[3D Camera coordinates to world coordinates (change of basis?)，但我不明白我必须做的。目前，我正在Excel工作表中进行一些计算，试图弄清楚该怎么做。 另外，我不得不说这个位置是关于一个框架，而这个框架又相对于世界坐标系统有坐标。在这种情况下，该框架的值是：
Fa= [X 571.16217, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
现在，如果我有第二个框FB：
Fb= [X 0, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
我知道，我的点P相对于Fb的应该是：
Pfb =[X -1106.036,Y -822.9583, Z 1039.342,A -165.2141, B -3.169379,C -178.6721]
我知道这个结果，因为我使用了一个程序来做这个计算自动地，但我不知道它是如何做到的。
鉴于原矢量O=(X, Y, Z)和旋转矩阵R，你可以从Euler angles计算（注意，有很多变体），由
P = R p + O.
给出相对坐标p=(x, y, z)点的绝对坐标与第二帧
P = R'p'+ O',
给予从在第一帧本地坐标的方程组到第二
p' = R'*(P - O') = R'*(R p + O - O')
其中*表示转置（它也是旋转矩阵的逆）。
我已经到了。 查找相对于帧B的点Pfa的变换，即相对于帧A的变换。Pfb ?? 这个例子在Kuka工业机器人中将一个位置或一个点从一个框架转换到另一个框架是有用的。此外，对于基础或框架的任何类型的仿射变换，我们只需要考虑齐次变换矩阵的旋转次序。
A = Rz
B = Ry
C = Rx
Fa_mat --> Homogeneous transformation matrix(HTM) of Frame A, relative to World CS(coordinate system).
Fb_mat --> HTM of Frame B, relative to World CS.
Pfa_mat --> HTM of point A in Frame A.
Pfb_mat --> HTM of point B in Frame B.
Pwa_mat --> HTM of point A in World CS.
Pwb_mat --> HTM of point B in World CS.
If Pwa == Pwb then:
Pwa = Fa_mat · Pfa_mat
Pwb = Fb_mat · Pfb_mat
Fa_mat · Pfa_mat = Fb_mat · Pfb_mat
Pfb_mat = Pwa · Fb_mat' (Fb_mat' is the inverse)
我用泰特 - 布赖恩ZYX角度的旋转矩阵，euler angles - Wikipedia. 这是我的Python代码：
# -*- coding: utf-8 -*-
"""
Created on Tue Jul 18 08:54:16 2017
@author: xabier fernandez
"""
import math
import numpy as np
def point_rotation(point_mat):
decpl = 7
sy = math.sqrt(math.pow(point_mat[0,0],2) + math.pow(point_mat[1,0],2))
singularity = sy < 1e-6
if not singularity :
A = math.atan2(point_mat[1,0], point_mat[0,0])
B = math.atan2(-point_mat[2,0], sy)
C = math.atan2(point_mat[2,1] , point_mat[2,2])
else :
A = 0
B = math.atan2(-point_mat[2,0], sy)
C = math.atan2(-point_mat[1,2], point_mat[1,1])
A = round(math.degrees(A),decpl)
B = round(math.degrees(B),decpl)
C = round(math.degrees(C),decpl)
return np.array([A,B,C])
def point_translation(point_mat):
decpl = 5
X = round(point_mat[0,3],decpl)
Y = round(point_mat[1,3],decpl)
Z = round(point_mat[2,3],decpl)
return np.array([X,Y,Z])
def point_to_mat(posX,posY,posZ,degA,degB,degC):
t=np.zeros((4,4))
radA=math.radians(degA)
radB=math.radians(degB)
radC=math.radians(degC)
cos_a=math.cos(radA)
sin_a=math.sin(radA)
cos_b=math.cos(radB)
sin_b=math.sin(radB)
cos_c=math.cos(radC)
sin_c=math.sin(radC)
t[0,0] = cos_a*cos_b
t[0,1] = -sin_a*cos_c + cos_a*sin_b*sin_c
t[0,2] = sin_a*sin_c + cos_a*sin_b*cos_c
t[1,0] = sin_a*cos_b
t[1,1] = cos_a*cos_c + sin_a*sin_b*sin_c
t[1,2] = -cos_a*sin_c + sin_a*sin_b*cos_c
t[2,0] = -sin_b
t[2,1] = cos_b*sin_c
t[2,2] = cos_b*cos_c
t[0,3] = posX
t[1,3] = posY
t[2,3] = posZ
t[3,0] = 0
t[3,1] = 0
t[3,2] = 0
t[3,3] = 1
return t
def test1():
"""
-----------------------------------
Rotational matrix 'zyx'
-----------------------------------
Fa--> Frame A relative to world c.s
Fb--> Frame B relative to world c.s
-----------------------------------
Pwa--> Point A in world c.s
Pwb--> Point B in world c.s
-----------------------------------
Pfa--> Point in frame A c.s
Pfb--> Point in frame B c.s
-----------------------------------
Pwa == Pwb
Pw = Fa x Pfa
Pw = Fb x Pfb
Pfb = Fb' x Pw
-----------------------------------
"""
frameA_mat = point_to_mat(571.162170,-1168.71704,372.404694,-179.723297,-0.206600,0.856200)
frameB_mat = point_to_mat(1493.90100, 209.460, 735.007, 179.572, -0.0880000, 0.130000)
Pfa_mat = point_to_mat(-534.884033, -825.747070,1037.32373, -165.214142, -3.16937923, -178.672119)
inverse_frameB_mat = np.linalg.inv(frameB_mat)
#--------------------------------------------------------------------------
#Point A in World coordinate system
Pwa_mat = np.dot(frameA_mat,Pfa_mat)
Pwa_Trans = point_translation(Pwa_mat)
Pwa_Rot = point_rotation(Pwa_mat)
print('\n')
print('Point A in World C.S.: ')
print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pwa_Trans[0],Pwa_Trans[1],Pwa_Trans[2]))
print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pwa_Rot[0],Pwa_Rot[1],Pwa_Rot[2]))
print('\n')
#--------------------------------------------------------------------------
#Point A affine transformation
#Point A in Frame B coordinate system
Pfb_mat= np.dot(inverse_frameB_mat,Pwa_mat)
Pfb_Trans = point_translation(Pfb_mat)
Pfb_Rot = point_rotation(Pfb_mat)
print('Point A in Frame B C.S.: ')
print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pfb_Trans[0],Pfb_Trans[1],Pfb_Trans[2]))
print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pfb_Rot[0],Pfb_Rot[1],Pfb_Rot[2]))
#--------------------------------------------------------------------------
#Point B in World coordinate system
Pwb_mat = np.dot(frameB_mat,Pfb_mat)
Pwb_Trans = point_translation(Pwb_mat)
Pwb_Rot = point_rotation(Pwb_mat)
print('\n')
print('Point B in World C.S.: ')
print(('Translation--> X = {0} , Y = {1} , Z = {2} ').format(Pwb_Trans[0],Pwb_Trans[1],Pwb_Trans[2]))
print(('Rotation(Euler angles)--> : A = {0} , B = {1} , C = {2} ').format(Pwb_Rot[0],Pwb_Rot[1],Pwb_Rot[2]))
print('\n')
if __name__ == "__main__":
test1()
恐怕你的信息就没有意义了 - 什么是' XYZABC'？它们看起来并不像标准基础的任何标准方式。 – meowgoesthedog
也许，这不是表示基础或框架的合适数学方法。但在许多工业机器人中，这是正确的。实际上，它是从库卡机器人字面上获取的。 –