捷联系统的四元数法姿态算法

捷联系统的四元数法姿态算法
算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。

具体算法：
1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A
2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b
3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B
4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A
5.重复2～4部，即可连续更新姿态
6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态
核心算法1，欧拉角转四元数
void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
float fCosHRoll = cos(ea.fRoll * .5f);
float fSinHRoll = sin(ea.fRoll * .5f);
float fCosHPitch = cos(ea.fPitch * .5f);
float fSinHPitch = sin(ea.fPitch * .5f);
float fCosHYaw = cos(ea.fYaw * .5f);
float fSinHYaw = sin(ea.fYaw * .5f);
w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}
核心算法2，四元数转欧拉角
EulerAngle Quaternion::ToEulerAngle() const
{
EulerAngle ea;
ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));
return ea;
}
核心算法3，四元数乘法
Quaternion Quaternion::Multiply(const Quaternion &b)
{
Quaternion c;
c.w=w*b.w -x*b.x -y*b.y -z*b.z;
c.x=w*b.x +x*b.w +y*b.z -z*b.y;
c.y=w*b.y -x*b.z +y*b.w +z*b.x;
c.z=w*b.z +x*b.y -y*b.x +z*b.w;
c.Normalize();
return c;
}
次要的规范化算法：
void Quaternion::Normalize(){
float s=getS();
w/=s;
x/=s;
y/=s;
z/=s;
}
float Quaternion::getS(){
return sqrt(w*w+x*x+y*y+z*z);
}
我的loop函数，算法的集成部分：
Quaternion nowQ;
void loop() {
int intx, inty,intz;
float pitch,roll,yaw;
gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw); EulerAngle dt;
dt.fRoll=roll;
dt.fPitch=pitch;
dt.fYaw=-yaw;
Quaternion dQ;
dQ.FromEulerAngle(dt);
nowQ=nowQ.Multiply(dQ);
count++;
if (count>1000){
EulerAngle nowG=nowQ.ToEulerAngle();
Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
Serial.print(",");
Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
Serial.print(",");
Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
Serial.print(",");
Serial.print(nowQ.getS(),11);//偏航
Serial.println();
count=0;
}
}
四元数与欧拉角之间的转换
在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。

本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系：
图1 3D Cartesian coordinate System (from wikipedia)
定义分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle 表示，分别为Yaw、Pitch、Roll。

图2 Tait-Bryan angles (from wikipedia)
一、四元数的定义
通过旋转轴和绕该轴旋转的角度可以构造一个四元数：
其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。

二、欧拉角到四元数的转换
三、四元数到欧拉角的转换
arctan和arcsin的结果是，这并不能覆盖所有朝向(对于角
的取值范围已经满足)，因此需要用atan2来代替arctan。

四、在其他坐标系下使用
在其他坐标系下，需根据坐标轴的定义，调整一下以上公式。

如在Direct3D中，笛卡尔坐标系的X轴变为Z轴，Y轴变为X轴，Z轴变为Y轴（无需考虑方向）。