我想用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
也許，這不是表示基礎或框架的合適數學方法。但在許多工業機器人中，這是正確的。實際上，它是從庫卡機器人字面上獲取的。 –