玩转四旋翼无人机（四元数）

四元数介绍 Quaternions and spatial rotation

单位四元数，也就是规范化四元数，提供一种方便的数学表示来表达多维空间中的方向和物体的旋转。相对于欧拉角来说，四元数更简单，避免了万向节锁。对比与旋转矩阵，四元数的数值稳定，更加高校。四元数在计算机图形，计算机视觉，机器人，导航和分子动力学，飞行器动力学等广泛应用。

四元数在16.oct.1843被发现，wiliiam Rowan Hamiton花费数年的时间使用一个3D数表示3D旋转，后来失败，于是发现4维才能工作。

这个图给我们关于四元数的直观的理解，这个图给人一种错误的提示就是，+1，-1代表180度，i代表90度。实际上想要它正常工作，那么i表示180度，i^2表示360度，+1，-1表示同一种变换。由Axis-Angle 转四元数的公式可知，当旋转角为180度的时候，旋转向量为[1,0,0],q=i.也就是旋转180度。
-1表示旋转了360度，也就是没有变化，按说应该是1.事实上一个四元数中所有项都取反，得到一个不同的四元数，但是旋转是相同的。
Unit quaternions, also known as versors, provide a convenient mathematical notation for representing orientations and rotations of objects in three dimensions. Compared to Euler angles they are simpler to compose and avoid the problem of gimbal lock. Compared to rotation matrices they are more numerically stable and may be more efficient. Quaternions have applications in computer graphics, computer vision, robotics, navigation, molecular dynamics, flight dynamics, orbital mechanics of satellites and crystallographic texture analysis.

基本特点

• 不可互换
• 分布率

• 不满足结合律

• not commutative

• associative
• distributive over sum

product of two quaternions

$$\mathbf{q}_1\otimes\mathbf{q}_2=[\mathbf{q}_1]{_L} \mathbf{q}_2$$

and

$$\mathbf{q}_1\otimes\mathbf{q}_2=[\mathbf{q}_2]{_R} \mathbf{q}_1$$

使用四元数表示3D旋转矩阵

一个单独的四元数无法包括平移，如果我们想要rotate, reflect and scale围绕一个点而不是原点，我们需要单独处理平移.
- For Reflection & scaling: $P_{out}=q\otimes P_{in}\otimes q$
- For Rotation & scaling: $P_{out}=q\otimes P_{in}\otimes q*$

对于大部分应用只涉及旋转，在该前提之下四元数变为具备单位幅值。

欧拉角

我们使用欧拉角来表示3维旋转，分解旋转维3各独立的角。 当我们考虑3D旋转的时候，欧拉角是最最然也是直觉上容易理解的方式。但是使用欧拉角会遇到很多问题。因此介绍欧拉角的目的就是获取直观的理解，然后转到四元数或者旋转矩阵。这里使用右手坐标系。有一些顺序是等价的。通过结合2各平面的旋转可以构造任意3D旋转。因此总共12种可能排列xyz, yzx, zxy or reversing the order zyx, xzy, yxz and 按照先后旋转的顺序 xyx, xzx, yxy, yzy, zxz and zyz。

线性旋转

我们不能在3维中得到线性旋转，这里介绍为什么要引入四元数。为了研究欧拉旋转的非线性根源，我们尝试把角显示在线性的3D网格中，把旋转映射其中。
在欧拉角和3D旋转的其他表示之间的相互转换时，我们遇到奇异点。首先考虑这样一个问题，如何表示2D的球面。
这里只涉及到经度和纬度。在数学中有多重奇异点，这些情形中我们这里考虑
- 在一种表示中，很多被映射成为一个点到另外一种表示中。
- 在一种表示中，在靠近奇异点的无穷小的变换在另外一种表示中引起巨大的变化。
-

类似的，欧拉角转换到四元数的时候，围绕0,1,0轴的旋转（一维空间）在欧拉角中为一个二维平面
在红色的平面内，对角线围绕轴0,1,0旋转，角度=heading+bank。

even if we do apply the angles in our chosen order it does not necessarily mean that each point on the diagram represents a unique rotation, it may be that the same rotation is represented at several places on the diagram, this happens at the singularities attitude= ±π/2.

conversions

euler angle to quaternion

(cos(ψ/2) + i * sin(ψ/2)) * (cos(θ/2) + j * sin(θ/2)) * (cos(φ/2) + k * sin(φ/2))

first method:

w = c1 c2 c3 - s1 s2 s3
x = s1 s2 c3 +c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3

where:

c1 = cos(heading / 2)
c2 = cos(attitude / 2)
c3 = cos(bank / 2)
s1 = sin(heading / 2)
s2 = sin(attitude / 2)
s3 = sin(bank / 2)
An alternative form is:

w = Math.sqrt(1.0 + C1 * C2 + C1*C3 - S1 * S2 * S3 + C2*C3) / 2
x = (C2 * S3 + C1 * S3 + S1 * S2 * C3) / (4.0 * w)
y = (S1 * C2 + S1 * C3 + C1 * S2 * S3) / (4.0 * w)
z = (-S1 * S3 + C1 * S2 * C3 + S2) /(4.0 * w)
where:

C1 = cos(heading)
C2 = cos(attitude)
C3 = cos(bank)
S1 = sin(heading)
S2 = sin(attitude)
S3 = sin(bank)

Conversion Quaternion to Euler

Conversion Quaternion to Euler
Equations

heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
attitude = asin(2*qx*qy + 2*qz*qw)
bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)

except when qx*qy + qz*qw = 0.5 (north pole)
which gives:
heading = 2 * atan2(x,w)
bank = 0
and when qx*qy + qz*qw = -0.5 (south pole)
which gives:
heading = -2 * atan2(x,w)
bank = 0

这里
Heading = rotation about y axis
Attitude = rotation about z axis
Bank = rotation about x axis

	airplane	telescope	symbol	angular velocity
applied first	heading	azimuth	($\theta$)	yaw
applied second	attitude	elevation	($\phi$)	pitch
applied last	bank	tilt	($\psi$)	roll

q = cos(a/2) + i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2))

quaternion to axis angle:

angle = 2 * acos(qw)
x = qx / sqrt(1-qw*qw)
y = qy / sqrt(1-qw*qw)

Conversion Euler to Axis-Angle

angle = 2 * acos(c1c2c3 - s1s2s3)
x = s1 s2 c3 +c1 c2 s3
y = s1 c2 c3 + c1 s2 s3
z = c1 s2 c3 - s1 c2 s3

to normalise divide x,y and z by:

sqrt(x2 + y2 + z2) = sqrt((s1 s2 c3 +c1 c2 s3)2+(s1 c2 c3 + c1 s2 s3)2+(c1 s2 c3 - s1 c2 s3)2)

where:

c1 = cos(heading / 2)
c2 = cos(attitude / 2)
c3 = cos(bank / 2)
s1 = sin(heading / 2)
s2 = sin(attitude / 2)
s3 = sin(bank / 2)

Euler to matrix (RPY -> matrix)

R=⎡⎣⎢ch∗casa−sh∗c