已知面角，如何求歐拉角，機器人坐標變換怎么求？

水水5

DaedraMech 發表于 2025-7-28 14:23
* W; N$ x8 T# E4 R一般對算法這邊來說最直接的就是拿到變換矩陣了，不知道為什么還要舍近求遠獲得歐拉角，不過樓主需要的話 ...

" q+ T# E  Y: o% |- O7 |" ^% L感謝層主，膜拜
% G, b& C% h9 a
4 d# `" V* k. x( |( [我一定把它付諸實踐
  r. y- B6 l& p
我又搜索了一些其他的方法，再次請樓主給于指導。一些方法貼入后面的回復。

水水5

方法一 本文轉載自簡書，作者是梁間。表示感謝
  T& X+ J6 @: P: |6 k) A數學模型已知兩個坐標系在各方向上尺度縮放比例一致，兩個坐標系的轉換關系可以用7個參數來表示，3個旋轉參數，3個平移參數，1個比例參數。已知三點在A、B兩個坐標系中的坐標，那么這7個參數可以唯一確定。
坐標轉換的數學模型為：
, D6 ^9 H4 W7 T2 H, \: C
其中，λ是比例參數，R是旋轉矩陣，Δ是平移向量，A、B分別是兩個坐標系中的坐標。
比例參數λ最容易計算

其中 為兩點在A坐標系中的距離。
% k' w9 C/ x% h5 V( @2 O旋轉矩陣R是一個3x3的正交矩陣，有3個自由度。可利用反對稱矩陣S來構造旋轉矩陣R：
- ^1 G; R9 N9 `4 z; G4 q
那么
5 T6 z: W8 q5 {1 c8 j* P
其中I是單位矩陣，這里R只有a、b、c三個變量，解出a、b、c即可確定旋轉矩陣R。
把兩點帶入(1)式并相減消去ΔX、ΔY、ΔZ
; K$ f5 e' D9 v. e; n# d. [
這里為點在A坐標系X軸向坐標值，為已知量。我們簡化一下寫法設定：
. h4 L9 o4 ^, Y3 b
這樣(3)式可寫為
4 J% |/ `' X  X% S
7 h+ A4 P; [" q3 N3 _( {( ^6 s. h  {把(2)式帶入(4)
+ x# E3 ^) A4 C) [" T! H$ e
: M' v$ }2 Y) J! G$ M帶入S
$ g2 x- x8 p+ @+ z' `
展開

0 q. ], S2 V( h5 e+ D7 B整理可得

0 ], ?/ z) f7 D- T7 p. I(5)式只有兩個獨立方程，解不出a、b、c三個未知量。帶入點得到和(5)式類似的方程組，兩個方程組聯立，取3個獨立方程
6 I) k2 v% j, {7 w+ u) F; t
* u1 A; p9 }" {- `
可結出

: }* D7 m3 s% W  \, K把a、b、c帶入(2)式可到旋轉矩陣R，把任意一點坐標帶入(1)式可得Δ。
9 @4 j3 |  n1 J1 t


作者：梁間
) L7 D; f* q, e+ [% i. Q% [鏈接：https://www.jianshu.com/p/58cf5655f9a9
% k! N& }2 H0 j0 }4 Q3 A來源：簡書
6 p# N2 x1 k+ M5 u9 `" H- p9 \, A著作權歸作者所有。商業轉載請聯系作者獲得授權，非商業轉載請注明出處。
+ c1 T; C* ~4 [5 b1 H; t
1 J0 {$ f* }8 n' s3 |9 z

水水5

方法二，這里提到了SVD算法

通過實際的坐標點例子，來直觀解釋通過SVD求對應的坐標系關系。

已知四個點在坐標系A中的坐標為：

（0， 0， 0）; （1，0，0）; （1，1，0）; （0，1，0）

可得矩陣A為

9 `+ l; O, j- v2 |6 H: @0 E
- _9 R) ^" i7 `! L8 ]  t! c矩陣A

這四個點在坐標系B中的坐標為：

（2，2， 2）; （3，2，2）; （3，3，2）; （2，3，2）

可得矩陣B為

矩陣B步驟一：求兩個數據集的質心

根據上述公式，可得質心為

步驟二：將兩個數據集的質心移動至同一個點，即只存在于一個旋轉的轉換關系。

對應坐標系中的點同時減去質心，計算后的矩陣A和B分別為

+ `* z) U9 s9 p& w# P! K# V計算后的矩陣A
* Y7 \- |/ u4 @, a: M
- x* t  {' I8 `8 O- L計算后的矩陣B

備注：此處的矩陣A和矩陣B一樣，因為舉得例子較為特殊，只存在平移關系。計算過程通用。

步驟三：通過SVD算法計算旋轉和平移關系。

定義一個3X3的矩陣，將矩陣A的每一行數據與矩陣B進行點乘，會產生四組3X3的矩陣，將這四組數據求和，得到最終的3X3的矩陣，就是我們需要用SVD算法來進行奇異值分解的矩陣H。

上述公式中，顏色相同的框內數據進行點乘，構成3X3的矩陣a1,a2,a3,a4。

矩陣H = a1 + a2 + a3 + a4。計算結果如下

通過SVD算法分解該矩陣，這里直接通過MATLAB接口調用，具體原理在前面的章節中已描述。

步驟四：計算旋轉和平移關系

根據上述求出的u1和v1，可求得旋轉矩陣R為將該旋轉矩陣轉為歐拉角則Rx = 0, Ry = 0, Rz = 0。

根據公式

得平移矩陣為

總結：如果在實際項目中，需要獲取多臺設備間的關系，如機器人相對于產品間的關系，或者機床相對于產品的關系，則該方法較為實用。注意：在實際的選擇參考點時，不要在一條線上選點。如上述選的四個點，要求不能共線。要不然會減少有效數據。

% ?8 ~! d# N7 n

DaedraMech

; H+ {; |% t0 E6 s; _
/ T) ~5 W- T) e7 G$ N& d沒想到水水大俠對此問題如此孜孜以求，鉆研精神和信息獲取能力令小弟佩服，我比較功利，方法能解決問題就行。

我把之前回復中的齊次變換矩陣改一下符號，樓主可以對比下和文章中的表達方式是不是一回事：
7 j; h- f- ?0 {: N5 Z/ y1 N

: }+ \6 ?' U/ T平移、旋轉、縮放這類坐標變換我們統稱為線性變換。平移和縮放非常簡單，沒什么好說的，重點就是如何求得旋轉矩陣R，上述兩種方法就是通過純代數的方法求解R。
$ h* M7 l6 k  M$ ^你用什么方法和選擇的工具有關，上述兩種方法多用在圖像處理和機器學習中，算法工程師手里的工具是集成編程環境，是代碼，采用代數方法自然是比較直接的。但要知道矩陣可是聯系代數和幾何的橋梁啊，而我們則掌握了強大的幾何工具SW，利用【旋轉矩陣可以表示坐標系】這一幾何意義我們可以直接得到R，完全不需要上面繁瑣的運算。
/ v8 V. R, ]8 ^3 R+ t

水水5

# I8 d3 l. N% o

DaedraMech 發表于 2025-7-29 20:37
+ v3 E+ B5 s2 Y& {2 s& C沒想到水水大俠對此問題如此孜孜以求，鉆研精神和信息獲取能力令小弟佩服，我比較功利，方法能解決問題就行 ...

9 }( y% u7 b& D# C: ]感謝大俠的持續教誨。大俠如果有，從變換矩陣導出歐拉角，的計算公式，恭請再給明示下。
/ F; y2 ]3 e+ U
經過又一個周末的摸索和學習，我已經學到了旋轉矩陣的測量方法，并且通過計算一個點的坐標在兩個坐標系中的坐標值進行了驗證。
6 _& B4 T  ?- }0 ~+ R: D: b  n2 X- A
這是第一步，下一步是求歐拉角。
; F! a% f: |& \$ R
( o6 c, }+ \/ o我最終的目的是：
: {; g. J+ k8 i4 `“在機器人示教工具坐標系的時候，不需要再繁瑣的用6點法搞一個小時示教，而是我作為機械工程師，可以直接從三維中計算出，通過計算，直接可以提供XYZABC的數值”
# m4 r% |& n) p: y0 M( e
但是這里面有個難題，我不知道機器人工具坐標系參數他是采用的哪種形式的歐拉角
下圖是我的作業
2 z# V9 J* h" R# u1 C; m. [$ O