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

水水5

DaedraMech 發(fā)表于 2025-7-28 14:23
) U  b4 k  `6 L$ t! T" V; T8 q  u1 g一般對算法這邊來說最直接的就是拿到變換矩陣了，不知道為什么還要舍近求遠獲得歐拉角，不過樓主需要的話 ...

. i  c' Z7 z# K0 f感謝層主，膜拜
2 F6 b7 R+ V7 K5 j9 ]
9 K0 a$ O. u% q, p; L& R/ b我一定把它付諸實踐

我又搜索了一些其他的方法，再次請樓主給于指導(dǎo)。一些方法貼入后面的回復(fù)。

水水5

方法一 本文轉(zhuǎn)載自簡書，作者是梁間。表示感謝
數(shù)學(xué)模型已知兩個坐標系在各方向上尺度縮放比例一致，兩個坐標系的轉(zhuǎn)換關(guān)系可以用7個參數(shù)來表示，3個旋轉(zhuǎn)參數(shù)，3個平移參數(shù)，1個比例參數(shù)。已知三點在A、B兩個坐標系中的坐標，那么這7個參數(shù)可以唯一確定。
坐標轉(zhuǎn)換的數(shù)學(xué)模型為：

: M+ c- f: L  g6 v) [其中，λ是比例參數(shù)，R是旋轉(zhuǎn)矩陣，Δ是平移向量，A、B分別是兩個坐標系中的坐標。
& p" r6 J/ I. [% \& s" \0 g8 X/ R# y比例參數(shù)λ最容易計算
+ n' b; q! H; |" _# A" `' K0 C
4 a( S  ^: _# C+ \其中 為兩點在A坐標系中的距離。
旋轉(zhuǎn)矩陣R是一個3x3的正交矩陣，有3個自由度。可利用反對稱矩陣S來構(gòu)造旋轉(zhuǎn)矩陣R：
. |  O- Q* h; j/ u
那么

其中I是單位矩陣，這里R只有a、b、c三個變量，解出a、b、c即可確定旋轉(zhuǎn)矩陣R。
把兩點帶入(1)式并相減消去ΔX、ΔY、ΔZ
2 o+ Y+ j2 C) m, h5 R9 T
9 E" D  D7 H* m' I這里為點在A坐標系X軸向坐標值，為已知量。我們簡化一下寫法設(shè)定：

$ g* l4 z9 `! m* e" C! c, [" C這樣(3)式可寫為
* \+ m7 r- @' N8 ]7 D
把(2)式帶入(4)

帶入S

展開

整理可得

( l1 }6 U; W0 F  }; W& y& K3 r(5)式只有兩個獨立方程，解不出a、b、c三個未知量。帶入點得到和(5)式類似的方程組，兩個方程組聯(lián)立，取3個獨立方程

) E4 D. N+ h" I7 V6 y
9 _0 x; D: g0 z% U) x可結(jié)出

8 S" s& i# d& E+ H把a、b、c帶入(2)式可到旋轉(zhuǎn)矩陣R，把任意一點坐標帶入(1)式可得Δ。
! W6 `$ Y4 Q! V* p  o/ g" [


+ S: m2 ~3 o4 f作者：梁間
* \' e+ [3 L# M. F# L鏈接：https://www.jianshu.com/p/58cf5655f9a9
來源：簡書
6 l- w6 c! e, h著作權(quán)歸作者所有。商業(yè)轉(zhuǎn)載請聯(lián)系作者獲得授權(quán)，非商業(yè)轉(zhuǎn)載請注明出處。
/ g! Q! y2 H8 ^" n! r% b
3 X! _5 z* m. C1 o$ U. q

水水5

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

通過實際的坐標點例子，來直觀解釋通過SVD求對應(yīng)的坐標系關(guān)系。

已知四個點在坐標系A(chǔ)中的坐標為：

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

可得矩陣A為

% S, Z/ J/ m! I( d6 y5 p- T2 G- ?
5 o0 R; A) w6 W矩陣A

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

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

可得矩陣B為

矩陣B步驟一：求兩個數(shù)據(jù)集的質(zhì)心

根據(jù)上述公式，可得質(zhì)心為

步驟二：將兩個數(shù)據(jù)集的質(zhì)心移動至同一個點，即只存在于一個旋轉(zhuǎn)的轉(zhuǎn)換關(guān)系。

對應(yīng)坐標系中的點同時減去質(zhì)心，計算后的矩陣A和B分別為

" p; n, Q; L5 L
計算后的矩陣A

, E1 \, E) m7 W! N, E計算后的矩陣B

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

步驟三：通過SVD算法計算旋轉(zhuǎn)和平移關(guān)系。

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

上述公式中，顏色相同的框內(nèi)數(shù)據(jù)進行點乘，構(gòu)成3X3的矩陣a1,a2,a3,a4。

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

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

步驟四：計算旋轉(zhuǎn)和平移關(guān)系

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

根據(jù)公式

得平移矩陣為

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

DaedraMech

% O, i0 k2 G9 s1 `5 {2 k
0 W0 t: ^8 Q7 z; w9 a. M( w沒想到水水大俠對此問題如此孜孜以求，鉆研精神和信息獲取能力令小弟佩服，我比較功利，方法能解決問題就行。

1 ?: W  i5 ^1 g我把之前回復(fù)中的齊次變換矩陣改一下符號，樓主可以對比下和文章中的表達方式是不是一回事：
$ i: E, g& m& F; A
+ {/ P4 j' W4 u6 W/ r, q
平移、旋轉(zhuǎn)、縮放這類坐標變換我們統(tǒng)稱為線性變換。平移和縮放非常簡單，沒什么好說的，重點就是如何求得旋轉(zhuǎn)矩陣R，上述兩種方法就是通過純代數(shù)的方法求解R。
' ^( L1 Q) v! V" K0 v你用什么方法和選擇的工具有關(guān)，上述兩種方法多用在圖像處理和機器學(xué)習(xí)中，算法工程師手里的工具是集成編程環(huán)境，是代碼，采用代數(shù)方法自然是比較直接的。但要知道矩陣可是聯(lián)系代數(shù)和幾何的橋梁啊，而我們則掌握了強大的幾何工具SW，利用【旋轉(zhuǎn)矩陣可以表示坐標系】這一幾何意義我們可以直接得到R，完全不需要上面繁瑣的運算。
! G  P; H0 E, @9 o4 a  L- }6 G

水水5

DaedraMech 發(fā)表于 2025-7-29 20:37
沒想到水水大俠對此問題如此孜孜以求，鉆研精神和信息獲取能力令小弟佩服，我比較功利，方法能解決問題就行 ...

8 I7 _8 ]$ `% B! P( J$ _5 B$ r感謝大俠的持續(xù)教誨。大俠如果有，從變換矩陣導(dǎo)出歐拉角，的計算公式，恭請再給明示下。

經(jīng)過又一個周末的摸索和學(xué)習(xí)，我已經(jīng)學(xué)到了旋轉(zhuǎn)矩陣的測量方法，并且通過計算一個點的坐標在兩個坐標系中的坐標值進行了驗證。
: y1 O6 ]% `/ h+ ~/ \
4 b+ |4 Y. I6 r3 A這是第一步，下一步是求歐拉角。

& X( ^! F4 d2 D0 N( a& j我最終的目的是：
“在機器人示教工具坐標系的時候，不需要再繁瑣的用6點法搞一個小時示教，而是我作為機械工程師，可以直接從三維中計算出，通過計算，直接可以提供XYZABC的數(shù)值”

但是這里面有個難題，我不知道機器人工具坐標系參數(shù)他是采用的哪種形式的歐拉角
0 Z( @( t% b( A$ S下圖是我的作業(yè)

; [# m1 H. y, u9 q, {

DaedraMech

! V  @( \* h" [歐拉角其實也不難求出，就是麻煩點。
% M5 K0 ~( G2 x+ p( u7 d
( j  v* K8 b, @* G/ e假設(shè)你手上的示教器里面歐拉角就是按照常規(guī)的Roll-Pitch-Yaw順序（ZYX順序）定義的，那我們不妨把單獨繞Z、Y、X軸的旋轉(zhuǎn)矩陣從左到右依次乘起來，看看最終的旋轉(zhuǎn)矩陣里各個元素的表達式都是啥。
! s6 F+ f8 X0 h$ S3 g
從上面的結(jié)果可以看出，通過對矩陣中特定元素之間的運算，我們是能夠分離出繞特定坐標軸的轉(zhuǎn)角的，即：
% F; M+ \! R; Y5 w  x
6 S% i7 \5 n% ^如果不知道示教器中歐拉角是如何定義的（并且上述順序錯誤的話），就需要再交換單軸旋轉(zhuǎn)矩陣的乘法順序，獲得新的表達式再次實驗。（也可以直接在示教器上輸入角度看機械臂到底先繞哪個軸旋轉(zhuǎn)）

另外可以想象到，空間中一定存在某個軸，能夠讓坐標系一步到位，直接一次旋轉(zhuǎn)到最終位置，而這就要用到四元數(shù)。所以如果示教器還支持四元數(shù)輸入的話，也可以將旋轉(zhuǎn)矩陣轉(zhuǎn)化為四元數(shù)：
: u* w# N" k5 W  ?3 H
; J3 ~6 |! o: w2 l. L四元數(shù)中xyz表示了轉(zhuǎn)軸的方向，w表示了繞這個軸的轉(zhuǎn)角，其值為轉(zhuǎn)角的半角余弦值。

$ Q5 E; n  q# i& E$ i
$ \( ]: r+ }9 P1 Q2 c4 }% l: i0 T

水水5

0 b  e7 d% ]+ q+ p( g8 v. ?

DaedraMech 發(fā)表于 2025-8-6 20:03
1 F0 F. N: Y0 M8 e9 z! Q歐拉角其實也不難求出，就是麻煩點。
0 I! J* y5 y. Q: [: P* g( T
% c% E! i, {/ f* V" b: E假設(shè)你手上的示教器里面歐拉角就是按照常規(guī)的Roll-Pitch-Yaw順序（ZY ...

1 i' ^: T. V/ A2 T6 A感謝大俠的熱情回復(fù)。匯報下進展：

  y( r1 ?# `8 P& E首先證明了旋轉(zhuǎn)矩陣是對的，第一步已經(jīng)完成，
第二步，我找了那些計算歐拉角和四元數(shù)的公式，公式應(yīng)該是沒錯的。雖然歐拉角的解不唯一，但是四元數(shù)是唯一解。經(jīng)過比對，還是和示教器中的數(shù)值有一定的差距。

4 \- G( g! |6 _" h+ t! C經(jīng)過分析，機械安裝有一定的誤差，手爪又這么長。所以至少證明了理論計算是正確的。但是實際應(yīng)用，還是要修正。免不了還是要示教一下。
) I7 R) p" Z8 N; Z5 P第三步，通過咨詢得到 歐拉角的旋轉(zhuǎn)方式。還未進一步計算，等下次有機會了再實驗。

這是FANUC對坐標系歐拉角的描述，市面上絕大多數(shù)機器人，都是遵循固定坐標系XYZ這樣的一個旋轉(zhuǎn)順序。

) r& V5 X6 v  \& v) Z- o# y上士聞道，勤而行之；中士聞道；若存若亡；下士聞道，大笑之。不笑不足以為道。
感謝各位的技術(shù)支持，感謝論壇。

水水5

DaedraMech 發(fā)表于 2025-8-6 20:03
+ W$ q' r, L2 J( _4 j" P歐拉角其實也不難求出，就是麻煩點。

假設(shè)你手上的示教器里面歐拉角就是按照常規(guī)的Roll-Pitch-Yaw順序（ZY ...

大俠有沒有 mathcad 教程 源文件？

您給出的公式的求解，還是有點一知半解。數(shù)學(xué)沒學(xué)好，尷尬
# g8 V. j: q  D9 w

ckc521

歐拉角旋轉(zhuǎn)很簡單的,不管是什么順規(guī)的旋轉(zhuǎn),都可以用基本旋轉(zhuǎn)矩陣推導(dǎo)出來,但是歐拉角的萬向鎖問題很煩人,雖然有算法可以解決.

qqqabcw

提供一種思路：在一個統(tǒng)一的坐標系下，執(zhí)行器、工件目標平面都提取一個平面法向向量，把他們弄成平行的（調(diào)整機器人的角度），最后就是平移。不難就是有點燒腦