四元数和欧拉角

四元数和欧拉角

四元数

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

Quaternions do not suffer from gimbal lock. With a three-angle(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官方库

#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;

}

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



姿态矩阵（旋转矩阵）

姿态矩阵是一个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