四元数和欧拉角

CHAO_^

已于 2022-08-30 22:17:33 修改

阅读量1.2w

点赞数 5

分类专栏： SLAM 文章标签：算法

于 2015-01-10 18:29:18 首次发布

本文链接：https://blog.csdn.net/jinzhichaoshuiping/article/details/42584887

版权

SLAM 专栏收录该内容

17 篇文章 0 订阅

订阅专栏

四元数

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

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），姿态矩阵可以通过四元数和欧拉角算出。

欧拉角

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