偶拉角转旋转矩阵 python 欧拉角转旋转矩阵

转载

云端筑梦师 2024-08-11 16:19:12

文章标签 偶拉角转旋转矩阵 python 四元数欧拉角坐标系旋转矩阵 文章分类 Python 后端开发

引言

最近在做无人机的室内定位技术攻关，涉及到了坐标系的一些转换，其实在以前做小四轴的时候有应用到用四元素来解算欧拉角进行四轴姿态的控制，那时候还不太懂只是套公式应用了一下。现在心血来潮整理一下几种三维旋转表示（欧拉角，四元数，旋转矩阵，轴角）以及他们之间的相互转换方法，并且加入了自己的一些推导。

一、欧拉角、旋转矩阵、轴角、四元数相关定义了解

1、欧拉角(Euler Angle)

将旋转分解为三个分离的转角，常用在飞行器上，但因为万向锁问题（Gimbal Lock) 而同样具有奇异性。

在经典力学里，时常用zxz顺规来设定欧拉角；照着第二个转动轴的轴名，简称为x顺规。另外，还有别种欧拉角组。合法的欧拉角组中，唯一的限制是，任何两个连续的旋转，必须绕着不同的转动轴旋转。因此，一共有12种顺规，分别是：6种绕三条轴的旋转(也叫Tait-Bryan Angle，XYZ,XZY,YXZ,YZX,ZXY,ZYX)，另外6种只绕两条轴的旋转(也叫Proper Euler Angle，XYX,YXY,XZX,ZXZ,YZY,ZYZ)。例如，zyz顺规，第二个转动轴是y轴，时常用在量子力学、核子物理学、粒子物理学。另外，还有一种顺规，xyz顺规，是用在航空航天工程学。因为这个欧拉角理解还是比较简单的更多请参考维基百科等。

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_坐标系

关于万向锁问题这里有一个很形象的动图：

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_欧拉角_02

2、旋转矩阵（Rotation matrix）

首先这部分知识要用到线性代数的知识，这里矩阵其实是方程组的系数矩阵，我们先来看一个简单的例子：

假设空间中的任意一点

$A(x{_{0}},y{_{0}},z{_{0}})$

绕Z轴旋转了

$\alpha$

度，那么求旋转后的坐标，这里我直接给出自己的推导:假设旋转之后的点为

$B(x{_{1}},y{_{1}},z{_{1}})$

，A点到Z轴的距离为L，A点到z轴的距离的直线与XZ平面的夹角为β，那么我们可以得出以下几条式子：

$L=\sqrt{x_{0}^{^{2}}+y_{0}^{^{2}}}$

$\beta =\arctan \frac{y_{0}}{x_{0}}$

$\\x{_{1}}=L\times cos(\alpha +\beta )\\ y{_{1}}=L\times sin(\alpha +\beta )\\z_{1}=z_{0}$

按照三角函数的倍角公式展开代入L和

$\alpha$

的表达式化简之后有

$\\ x{_{1}}=x_{0}\cos \alpha -y_{0}\sin \alpha \\ y{_{1}}=x_{0}\sin \alpha +y_{0}\cos \alpha\\z_{1}=z_{0}$

写出矩阵形式有

$\begin{bmatrix} x_{1}} \\ y_{1} \\z_{1} \end{bmatrix}=\begin{bmatrix} \cos \alpha & -\sin \alpha & 0\\ \sin \alpha & \cos \alpha & 0\\ 0 & 0 & 1 \end{bmatrix}\times \begin{bmatrix} x_{0}\\y_{0} \\ z_{0} \end{bmatrix}$

尽管图示中仅仅表示的是旋转一个锐角

$\alpha$

的情形，但是我们推导中使用的是三角函数的基本定义来计算坐标的，因此当旋转的角度是任意角度（例如大于180度，导致A’点进入到第四象限）结论仍然是成立的。

同理可得其他两个轴的旋转矩阵如下

绕x轴旋转

$\alpha$

的矩阵：

$\begin{bmatrix} 1 & 0 & 0\\0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha\end{bmatrix}$

绕y轴旋转

$\alpha$

的矩阵：

$\begin{bmatrix} \cos \alpha & 0 & \sin \alpha \\0 & 1 & 0\\ -\sin \alpha & 0 & \cos \alpha\end{bmatrix}$

3、轴角

轴角用一个以单位矢量定义的旋转角，再加上一个标量定义的旋转角来表示旋转。通常的表示[x,y,z,theta]，前面三个表示轴，最后一个表示角度。表示非常直观，也很紧凑。轴角最大的一个局限就是不能进行简单的插值，此外，轴角形式的旋转不能直接施于点或矢量，所以转换为矩阵或者四元素。

4、四元数(Quaternion)

欧拉旋转是有万向节死锁(Gimbal Lock)的问题的，而四元数(Quaternion)这种数学工具可以避免这个情况。

首先推荐一篇文章《Understanding Quaternions》

英文原版：http://www.3dgep.com/understanding-quaternions/

中文翻译：https://www.qiujiawei.com/understanding-quaternions/

关于理解四元数的文章很多：https://zhuanlan.zhihu.com/p/27471300

看完上面文章之后应该已经对四元数有了一个比较形象的概念以及四元数的运算法则（四元数和矩阵一样，不满足乘法交换率，也就是说，A*B不等于B*A），这里再做一些补充，也算是我自己的一些理解和看法吧。

四元素感觉上就是轴角的进化，也是使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度，即(x,y,z,w)，其中

$\\w = \cos \frac{\theta}{2} \\x = x * \sin \frac{\theta}{2} \\y = y * \sin \frac{\theta}{2} \\z = z * \sin \frac{\theta}{2}$

其实四元数类似于复数，只不过它的虚部有三个维度且两两互相正交，上面有一篇文章里面提到'如何利用低维信息去理解高维信息'这个特别形象，这里不多展开介绍了。这里举个例子怎样用四元数表示一个旋转：

假设空间中存在一个点

$A(x_{0}},y_{0},z_{0})$

然后我们指定它绕轴

$n(\omega x,\omega y,\omega z)$

旋转角度

$\theta$

，求其旋转之后的点B第一步：先把A点用纯四元数表示，即：

$A(x_{0}},y_{0},z_{0})=\left [ 0, x_{0}},y_{0},z_{0}\right ]=\left [ 0,\nu \right ]$

第二步：用四元数p表示旋转，即：

$p=\left [ \cos \frac{\theta }{2} , n \sin \frac{\theta }{2}\right ]$

，这条公式和复数的旋转变换公式一样，只不过这里的n是对应三轴的，相关证明这里不展开了，相关推导和展开在《复变函数》《高等数学》以及上面介绍的文章中都有讲到。第三步：这里用我们线性代数的知识可得B的四元数表示方式是：

$B=p A p^{-1}$

最后验算可以得到B的四元数仍然是一个纯四元数，其虚部三个分量表示旋转后的坐标。

$\\pA=0\times \cos\frac{\theta }{2}-x_{0}n_{x} \sin \frac{\theta }{2}-y_{0}n_{_{y}} \sin \frac{\theta }{2}-z_{0}n_{z} \sin \frac{\theta }{2} \\+(0\times n_{x}\sin \frac{\theta }{2}+x_{0}\cos \frac{\theta }{2}+y_{0} n_{z}\sin\frac{\theta }{2} - z_{0}n_{y}\sin \frac{\theta }{2})i \\+(0\times n_{y}\sin \frac{\theta }{2}-x_{0}n_{z}\sin\frac{\theta }{2}+y_{0} \cos \frac{\theta }{2} + z_{0}n_{x}\sin\frac{\theta }{2})j \\+(0\times n_{z}\sin \frac{\theta }{2}+x_{0}n_{y}\sin\frac{\theta }{2}-y_{0} n_{x}\sin\frac{\theta }{2} + z_{0}\cos \frac{\theta }{2})k$

$p^{-1}=\frac{p^{*}}{{\left \| p \right \|}^{2}}$

式子比较长这里就不展开了，我们直接得到最后的结果B=

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_四元数_26

二、旋转表示方式之间的转换

1、欧拉角 ----> 旋转矩阵

欧拉角构造旋转矩阵就直接把三个Elemental Rotation Matrix （上面我们推到过了三个旋转矩阵）乘在一起就好了，即：

这里以ＹＸＺ顺序旋转为例举个例子：

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_偶拉角转旋转矩阵 python_27

其余11种旋转也是如此,这里不做展开

２、旋转矩阵----> 欧拉角

最简单的方式是由四元数q解出旋转角θ和旋转轴n，但那样要计算一个arccosarccos函数，代价较大。（这在《视觉ALAM十四讲》一书中有谈到）。这里直接给出公式吧

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_欧拉角_28

假设矩阵为

$R=\left \{ m_{ij},i,j\in \left [ 1,2,3 \right ] \right \}$

则：

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_坐标系_30

其实这样表达是有问题的，相关说明在上面文章中有讲到这里不展开了

３、旋转矩阵----> 四元数

偶拉角转旋转矩阵 python 欧拉角转旋转矩阵_坐标系_31

其实这样表达也有问题，这里也不展开了，因为这种方式在做工程的时候比较少用到，上面文章也有提到

４、欧拉角---->四元数（重点）

上面我们已经讲到了空间中的旋转用四元数表示以及旋转矩阵推到，欧拉角构造四元数跟欧拉角构造旋转矩阵一样，就是把三个基础旋转Elemental Rotation组合在一起。那么用于旋转的四元数

的表达式是：这里我按照XYZ 的顺序举个例子

$q\left ( \alpha ,\beta ,\gamma \right )=q_{x}(\alpha )q_{y}(\beta )q_{z}(\gamma )=\begin{bmatrix} \sin \frac{\alpha }{2}\\ 0 \\ 0\\\cos \frac{\alpha }{2} \end{bmatrix}\begin{bmatrix} 0\\ \sin \frac{\beta }{2} \\ 0 \\\cos \frac{\beta }{2} \end{bmatrix}\begin{bmatrix} 0\\ 0 \\ \sin \frac{\gamma }{2} \\ \cos \frac{\gamma }{2} \end{bmatrix} \\=\begin{bmatrix} \sin \frac{\beta }{2}\sin \frac{\gamma }{2} \cos \frac{\alpha }{2}+\cos \frac{\beta }{2}\cos \frac{\gamma }{2}\sin \frac{\alpha }{2}\\\sin \frac{\beta }{2}\cos \frac{\gamma }{2}\cos \frac{\alpha }{2}+\cos \frac{\beta }{2}\sin \frac{\gamma }{2}\sin \frac{\alpha }{2} \\\cos \frac{\beta }{2}\sin \frac{\gamma }{2}\cos \frac{\alpha }{2}-\sin \frac{\beta }{2}\cos \frac{\gamma }{2}\sin \frac{\alpha }{2} \\ \cos \frac{\beta }{2}\cos \frac{\gamma }{2}\cos \frac{\alpha }{2}-\sin \frac{\beta }{2}\sin \frac{\gamma }{2}\sin \frac{\alpha }{2}\end{bmatrix}$

这里要注意的是，这个不是矩阵运算而是四元数的乘法运算

即给定一个欧拉旋转（分别绕x轴、y轴和z轴旋转X、Y、Z度），则对应的四元数为：q = (x, y, z, w)其中：

x = sin(Y/2)sin(Z/2)cos(X/2)+cos(Y/2)cos(Z/2)sin(X/2) y = sin(Y/2)cos(Z/2)cos(X/2)+cos(Y/2)sin(Z/2)sin(X/2) z = cos(Y/2)sin(Z/2)cos(X/2)-sin(Y/2)cos(Z/2)sin(X/2) w = cos(Y/2)cos(Z/2)cos(X/2)-sin(Y/2)sin(Z/2)sin(X/2)

参考文章：

【1】https://zhuanlan.zhihu.com/p/45404840

【2】

【3】https://zhuanlan.zhihu.com/p/27471300

【4】https://www.zhihu.com/question/23005815

【5】

【6】

本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。