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

水水5

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

感謝層主，膜拜
9 j6 R) t% ?5 u$ @
% a& S7 n- h* ^$ p  [) G' t我一定把它付諸實踐
4 n% r& k, q4 M1 ]; U. c) j$ _
0 R4 L' x; N7 _0 N4 P+ F6 S2 a我又搜索了一些其他的方法，再次請樓主給于指導。一些方法貼入后面的回復。

水水5

7 {4 F) L, C1 }5 O! q% e' F8 m
方法一 本文轉載自簡書，作者是梁間。表示感謝
6 g& W' E0 ]) }數學模型已知兩個坐標系在各方向上尺度縮放比例一致，兩個坐標系的轉換關系可以用7個參數來表示，3個旋轉參數，3個平移參數，1個比例參數。已知三點在A、B兩個坐標系中的坐標，那么這7個參數可以唯一確定。
坐標轉換的數學模型為：
( V+ V0 l5 X* u8 G6 S. q1 L
其中，λ是比例參數，R是旋轉矩陣，Δ是平移向量，A、B分別是兩個坐標系中的坐標。
. B! M! m# D7 b# q& g& p比例參數λ最容易計算

" _' o$ q; q( Y其中 為兩點在A坐標系中的距離。
旋轉矩陣R是一個3x3的正交矩陣，有3個自由度。可利用反對稱矩陣S來構造旋轉矩陣R：

- n& ~% ?: Q. m7 H7 I3 I那么

) O+ D7 P; d1 S+ d, l, ^" H4 ~其中I是單位矩陣，這里R只有a、b、c三個變量，解出a、b、c即可確定旋轉矩陣R。
# D2 y# |. z. P, U7 [: p把兩點帶入(1)式并相減消去ΔX、ΔY、ΔZ

這里為點在A坐標系X軸向坐標值，為已知量。我們簡化一下寫法設定：
8 Q9 {+ M; k- Q: R- y
這樣(3)式可寫為

' ]% _3 b" ^7 K# E: S" }3 P3 s把(2)式帶入(4)

帶入S
) q' x' j% N& }+ C* }$ O# o
展開

整理可得
5 E9 l( T- z0 s6 G0 {2 ]
- G! `+ r& ?/ I(5)式只有兩個獨立方程，解不出a、b、c三個未知量。帶入點得到和(5)式類似的方程組，兩個方程組聯立，取3個獨立方程
5 j3 H; k3 N5 m+ t3 a5 q7 n
4 J; I  _( j$ [, I9 O
可結出
0 o, z3 y1 R7 E) X2 R. M- W
: L0 @, m9 o8 n! ?  p2 d* G把a、b、c帶入(2)式可到旋轉矩陣R，把任意一點坐標帶入(1)式可得Δ。
) \3 v, {6 x, |: x0 L


作者：梁間
, s* E6 F0 z6 }. v& N0 U鏈接：https://www.jianshu.com/p/58cf5655f9a9
來源：簡書
著作權歸作者所有。商業轉載請聯系作者獲得授權，非商業轉載請注明出處。
  t5 q! c) e# b! n& c  z1 M
2 n6 F& w! ^2 W; K

水水5

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

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

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

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

可得矩陣A為

! V: J! R! R$ N7 @0 P矩陣A

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

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

可得矩陣B為

1 }0 ?! m* S5 T
' e; d- ~& B. |9 e3 S矩陣B步驟一：求兩個數據集的質心

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

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

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

- x4 m6 d/ e) f$ K2 p0 C6 q
計算后的矩陣A

計算后的矩陣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。

根據公式

得平移矩陣為

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

4 [* r5 m; ^; ~. W' k

DaedraMech

$ n6 K% m7 y+ b8 L7 U7 N& a3 Z沒想到水水大俠對此問題如此孜孜以求，鉆研精神和信息獲取能力令小弟佩服，我比較功利，方法能解決問題就行。

3 U- p9 t( y1 ~$ ?% [" E我把之前回復中的齊次變換矩陣改一下符號，樓主可以對比下和文章中的表達方式是不是一回事：


平移、旋轉、縮放這類坐標變換我們統稱為線性變換。平移和縮放非常簡單，沒什么好說的，重點就是如何求得旋轉矩陣R，上述兩種方法就是通過純代數的方法求解R。
2 j- f7 f. w+ Y" `: e3 b你用什么方法和選擇的工具有關，上述兩種方法多用在圖像處理和機器學習中，算法工程師手里的工具是集成編程環境，是代碼，采用代數方法自然是比較直接的。但要知道矩陣可是聯系代數和幾何的橋梁啊，而我們則掌握了強大的幾何工具SW，利用【旋轉矩陣可以表示坐標系】這一幾何意義我們可以直接得到R，完全不需要上面繁瑣的運算。
2 Z' [0 V) X7 {7 h: ]