已知面角，如何求欧拉角，机器人坐标变换怎么求？

水水5 · 发表于 4 天前

DaedraMech 发表于 2025-7-28 14:23
2 W" b5 G! N9 x* W: o: D3 T+ @一般对算法这边来说最直接的就是拿到变换矩阵了，不知道为什么还要舍近求远获得欧拉角，不过楼主需要的话 ...

感谢层主，膜拜

我一定把它付诸实践

我又搜索了一些其他的方法，再次请楼主给于指导。一些方法贴入后面的回复。

水水5 · 发表于 4 天前

方法一本文转载自简书，作者是梁间。表示感谢
7 j) @ I( m6 c& {3 _数学模型已知两个坐标系在各方向上尺度缩放比例一致，两个坐标系的转换关系可以用7个参数来表示，3个旋转参数，3个平移参数，1个比例参数。已知三点在A、B两个坐标系中的坐标，那么这7个参数可以唯一确定。
坐标转换的数学模型为：
$\begin{bmatrix} X \\ Y \\ Z \end{bmatrix}_A = λ \left( \begin{bmatrix} ΔX \\ ΔY \\ ΔZ \end{bmatrix} + R\begin{bmatrix} X \\ Y \\ Z \end{bmatrix}_B \right) ...(1)$
其中，λ是比例参数，R是旋转矩阵，Δ是平移向量，A、B分别是两个坐标系中的坐标。
比例参数λ最容易计算
$λ = \frac{|P_1P_2|_A}{|P_1P_2|_B} = \frac{|P_2P_3|_A}{|P_2P_3|_B} = \frac{|P_3P_1|_A}{|P_3P_1|_B}$
其中

为

两点在A坐标系中的距离。
旋转矩阵R是一个3x3的正交矩阵，有3个自由度。可利用反对称矩阵S来构造旋转矩阵R：
$S= \begin{bmatrix} 0&-c&-b \\ c&0&-a \\ b&a&0 \end{bmatrix}$
那么
$R=\frac{I+S}{I−S} ...(2)$
其中I是单位矩阵，这里R只有a、b、c三个变量，解出a、b、c即可确定旋转矩阵R。
把

两点带入(1)式并相减消去ΔX、ΔY、ΔZ
$\begin{bmatrix} P_{1AX}-P_{2AX} \\ P_{1AY}-P_{2AY} \\P_{1AZ}-P_{2AZ} \end{bmatrix} = λR\begin{bmatrix} P_{1BX}-P_{2BX} \\ P_{1BY}-P_{2BY} \\P_{1BZ}-P_{2BZ} \end{bmatrix} ...(3)$
这里 $P_{1AX}$ 为

点在A坐标系X轴向坐标值，为已知量。我们简化一下写法设定：
$X_{A12} = P_{1AX}-P_{2AX}$
这样(3)式可写为
$\begin{bmatrix} X_{A12}\\ Y_{A12} \\ Z_{A12} \end{bmatrix} = λR\begin{bmatrix} X_{B12}\\ Y_{B12} \\ Z_{B12} \end{bmatrix} ...(4)$
把(2)式带入(4)
$(I-S)\begin{bmatrix} X_{A12}\\ Y_{A12} \\ Z_{A12} \end{bmatrix} = λ(I+S)\begin{bmatrix} X_{B12}\\ Y_{B12} \\ Z_{B12} \end{bmatrix}$
带入S
$\begin{bmatrix} 1&c&b \\ -c&1&a \\ -b&-a&1\end{bmatrix}\begin{bmatrix} X_{A12}\\ Y_{A12} \\ Z_{A12} \end{bmatrix} = λ\begin{bmatrix} 1&-c&-b \\ c&1&-a \\ b&a&1 \end{bmatrix}\begin{bmatrix} X_{B12}\\ Y_{B12} \\ Z_{B12} \end{bmatrix}$
展开
$\begin{bmatrix} X_{A12}+cY_{A12}+bZ_{A12} \\ -cX_{A12}+Y_{A12}+aZ_{A12} \\ -bX_{A12}-aY_{A12}+Z_{A12}\end{bmatrix} = λ\begin{bmatrix} X_{B12}-cY_{B12}-bZ_{B12}\\ cX_{B12}+Y_{B12}-aZ_{B12} \\ bX_{B12}+aY_{B12}+Z_{B12} \end{bmatrix}$
整理可得
$\begin{bmatrix} X_{A12} -λX_{B12} \\ Y_{A12} - λY_{B12} \\ Z_{A12} - λ Z_{B12} \end{bmatrix} = \begin{bmatrix}-c(λY_{B12}+Y_{A12})-bλ(Z_{B12}+bZ_{A12})\\ c(λX_{B12}+X_{A12})-a(λZ_{B12} +Z_{A12} )\\ b(λX_{B12}+X_{A12})+a(λY_{B12}+Y_{A12} )\end{bmatrix}...(5)$
(5)式只有两个独立方程，解不出a、b、c三个未知量。带入

点得到和(5)式类似的方程组，两个方程组联立，取3个独立方程
$\begin{bmatrix} X_{A12} -λX_{B12} \\ Y_{A12} - λY_{B12} \\ Z_{A13} - λ Z_{B13} \end{bmatrix} = \begin{bmatrix} -b( λZ_{B12}+Z_{A12}) -c(λY_{B12}+Y_{A12}) \\ -a(λZ_{B12} +Z_{A12} )+c(λX_{B12}+X_{A12})\\ a(λY_{B13}+Y_{A13} )+b(λX_{B13 }+X_{A13})\end{bmatrix}$
$= \begin{bmatrix} 0 & -b(λZ_{B12}+Z_{A12}) &-c(λY_{B12}+Y_{A12}) \\ -a(λZ_{B12} +Z_{A12} )& 0 & c(λX_{B12}+X_{A12})\\ a(λY_{B13}+Y_{A13} )&b(λX_{B13 }+X_{A13})&0\end{bmatrix} \begin{bmatrix} a\\b \\c \end{bmatrix}$
可结出
$\begin{bmatrix} a\\ b \\c \end{bmatrix} = \begin{bmatrix} 0 & -(λZ_{B12}+Z_{A12}) &-(λY_{B12}+Y_{A12}) \\ -(λZ_{B12} +Z_{A12} )& 0 & (λX_{B12}+X_{A12})\\ (λY_{B13}+Y_{A13} )&(λX_{B13 }+X_{A13})&0\end{bmatrix} ^{-1 }\begin{bmatrix} X_{A12} -λX_{B12} \\ Y_{A12} - λY_{B12} \\ Z_{A13} - λ Z_{B13} \end{bmatrix}$
把a、b、c带入(2)式可到旋转矩阵R，把任意一点坐标带入(1)式可得Δ。

作者：梁间
链接：https://www.jianshu.com/p/58cf5655f9a9
来源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

水水5 · 发表于 4 天前

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

通过实际的坐标点例子，来直观解释通过SVD求对应的坐标系关系。

已知四个点在坐标系A中的坐标为：

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

可得矩阵A为

矩阵A

这四个点在坐标系B中的坐标为：

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

可得矩阵B为

矩阵B步骤一：求两个数据集的质心

根据上述公式，可得质心为

步骤二：将两个数据集的质心移动至同一个点，即只存在于一个旋转的转换关系。

对应坐标系中的点同时减去质心，计算后的矩阵A和B分别为

计算后的矩阵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。

根据公式

得平移矩阵为

总结：如果在实际项目中，需要获取多台设备间的关系，如机器人相对于产品间的关系，或者机床相对于产品的关系，则该方法较为实用。注意：在实际的选择参考点时，不要在一条线上选点。如上述选的四个点，要求不能共线。要不然会减少有效数据。

DaedraMech · 发表于 4 天前

本帖最后由 DaedraMech 于 2025-7-29 20:42 编辑

没想到水水大侠对此问题如此孜孜以求，钻研精神和信息获取能力令小弟佩服，我比较功利，方法能解决问题就行。

我把之前回复中的齐次变换矩阵改一下符号，楼主可以对比下和文章中的表达方式是不是一回事：

平移、旋转、缩放这类坐标变换我们统称为线性变换。平移和缩放非常简单，没什么好说的，重点就是如何求得旋转矩阵R，上述两种方法就是通过纯代数的方法求解R。
你用什么方法和选择的工具有关，上述两种方法多用在图像处理和机器学习中，算法工程师手里的工具是集成编程环境，是代码，采用代数方法自然是比较直接的。但要知道矩阵可是联系代数和几何的桥梁啊，而我们则掌握了强大的几何工具SW，利用【旋转矩阵可以表示坐标系】这一几何意义我们可以直接得到R，完全不需要上面繁琐的运算。

		自动登录	找回密码
密码			注册会员

已知面角，如何求欧拉角，机器人坐标变换怎么求？

本帖子中包含更多资源