四元数和欧拉角

四元数

四元数在姿态解算中用来表示旋转。四元数的乘积可表示三维空间上任意的伸缩旋转变换。

Quaternions do not suffer from gimbal lock. With a threeangle(roll, pitch, yaw) system, there  are always certain orientations in which there is no simple change to the three values to  represent a simple local rotation.  You often see this rotation having "pitched up" 90 degree  when you are trying to specify a local yaw for right .

翻译：在三角度坐标系中（俯仰角，横滚角，航向角（这三个角也通常称为欧拉角）），总有一定的方向,不能通过简单的改变三个值来表示一个简单的旋转，当你想置顶一个偏航角时，你经常看到该角度上仰了90度。

a+bi称为二维复数，四元数属于四维复数:w+xi+yj+zk.

i,j,k满足如下性质

i2  =  j2  =  k2  =  -1

i  *  j  =  k  =  -j  *  i

j  *  k  =  i  =  -k  *  j

k  *  i  =  j  =  -i  *  k

为了方便表示，q=w+xi+yj+zk 可以分为一个纯量和一个标量。表示为（S，V）。

已知一个三维空间的伸缩旋转变换的转轴方向，旋转角度和伸缩比例，可以求出相应的四元数。设转轴方向为A（Xa，Ya，Za），伸缩比例为T2那么将有如下公式。

x  =  s  *  X a

y  =  s  *  X b

z  =  s  *  X c

w  =  cos(θ/ 2)

s  =  sin(θ/ 2)

最终得到q=T(cos(θ/ 2)+sin(θ/ 2)(Xa*i+Ya*j+Za*k))

特别地，用单位化的四元数来描述旋转。当转轴方向矢量（Xa2+Ya2+Za2=1），此时的四元数是一个单位四元数。可以表示为Q=（cos(θ/ 2)；sin(θ/ 2)Xa，+sin(θ/ 2)Ya，sin(θ/ 2)Za）

其中，转轴方向矢量也可以用旋转轴在x,y,z方向上的分量，cos(x),cos(y),cos(z).

四元数乘法的意义在于可以将两个旋转合并。例如：

Q=Q1*Q2 表示Q的是先做Q2的旋转，再做Q1的旋转的结果，而多个四元数的旋转也是可以合并的，当有多次旋转操作时，使用四元数可以获得更高的计算效率。

已知四元数Q=q0+q1i+q2j+q3k

可以得到旋转矩阵

怎么样由加速度计测出的加速度和陀螺仪测出的角速度得出四元数

以下代码来自IMU官方库

<span style="font-size:14px;">#define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计#define Ki 0.005f // 积分增益支配率的陀螺仪偏见的衔接#define halfT 0.5f // 采样周期的一半 //---------------------------------------------------------------------------------------------------// 变量定义 float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 //====================================================================================================// Function//==================================================================================================== void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {float norm;float vx, vy, vz;float ex, ey, ez;         // 测量正常化norm = sqrt(ax*ax + ay*ay + az*az);       ax = ax / norm;ay = ay / norm;az = az / norm;      // 估计方向的重力vx = 2*(q1*q3 - q0*q2);vy = 2*(q0*q1 + q2*q3);vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;    //这是把四元数换算成姿态矩阵中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系正好是这三个元素。所以这里的vx，vy,  vz，就是当前的欧拉角（四元数）的机体参照坐标系上，换算出来的重力单位向量。// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和ex = (ay*vz - az*vy);ey = (az*vx - ax*vz);ez = (ax*vy - ay*vx);// 积分误差比例积分增益exInt = exInt + ex*Ki;eyInt = eyInt + ey*Ki;ezInt = ezInt + ez*Ki;// 调整后的陀螺仪测量gx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;// 整合四元数率和正常化q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  // 正常化四元norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;}</span>

得出四元数后，可以通过四元数转欧拉角，得到当前的欧拉角。

 

姿态矩阵（旋转矩阵） 

姿态矩阵是一个3x3的矩阵，姿态矩阵通常用来确定旋转体中某一点的位置。例如已知旋转体某一点相对坐标为(X0,Y0,Z0),那么以该坐标的转置去乘以旋转矩阵，便可以得到该点旋转后的坐标（X1，Y1，Z1），姿态矩阵可以通过四元数和欧拉角算出。

 

欧拉角

构件在三维空间中的有限运动，可以次用三个相对转角（每次的旋转轴是被转动坐标系的某一坐标轴）表示，即进动角，章动角，自旋角，也可称为俯仰角，横滚角，航向角。用欧拉角确定的姿态矩阵是三次坐标转换矩阵的乘积。

可得姿态矩阵

欧拉角转四元数

第一次绕Z轴转动 ，转动角度为，四元数表示为

第二次绕Y轴转动，转动角度为，四元数表示为

第三次绕X轴转动，转动角为，四元数表示为

三次转动的合成是这三个四元数的乘积，得到该旋转的四元数为：

在姿态解算时需要最先初始化欧拉角，只通过加速度计便可得到欧拉角，初始化欧拉角是为了得到初始化的四元数。

 

 

四元数转欧拉角

  Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;  // pitch

  Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

  Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 57.3; // yaw