我想用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 库。我需要一些关于如何去做的提示，我必须用数学术语来遵循哪些步骤。
问问题
339 次
3 回答
1
给定可以从欧拉角计算的原点向量O=(X, Y, Z)和旋转矩阵（注意，有很多变体），具有相对坐标的点的绝对坐标由下式给出Rp=(x, y, z)
P = R p + O.
带有第二帧
P = R'p'+ O',
给出从第一帧局部坐标到第二帧的方程
p' = R'*(P - O') = R'*(R p + O - O')
其中*表示转置（这也是旋转矩阵的逆矩阵）。
于 2017-07-17T15:47:21.153 回答
0
我来到了这里。求点 Pfa 的变换，即相对于框架 A，相对于框架B。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)
我使用Tait-Bryan ZYX角作为旋转矩阵，欧拉角 - 维基百科。 这是我的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()
于 2017-07-27T21:00:18.310 回答
0
这些点是机器人位置的笛卡尔坐标。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 相机坐标到世界坐标（改变基础？），但我不明白我必须做什么。目前我正在 Excel 表格中进行一些计算，试图弄清楚该怎么做。另外我不得不说这个位置是相对于 Frame 而言的，而 Frame 又具有相对于 World 坐标系的坐标。在这种情况下，此 Frame 的值为：
Fa= [X 571.16217, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
现在，如果我有第二个 Frame Fb：
Fb= [X 0, Y -1168.71704, Z 372.404694000000, A -179.72329, B -0.2066, C 0.8562]
我知道我对 Fb 的观点 P 应该是：
Pfb =[X -1106.036,Y -822.9583, Z 1039.342,A -165.2141, B -3.169379,C -178.6721]
我知道这个结果是因为我使用了一个自动执行此计算的程序，但我不知道它是如何做到的。
于 2017-07-17T14:38:02.340 回答