四元数理论
四元数理论：
俯仰角：Pitch
航向角：Yaw
滚动角：Roll
姿态获得过程：
1、 首先有初始姿态A（四元数）
2、 获得一段时间陀螺仪的积分b（gx，gy，gz）
3、 将b转为四元数B
4、 更新姿态：A’=A×B（乘完后要规范化，以防止超出范围）
5、 不断重复2到4步即可不断得到最新的姿态A
6、 将最新的A转换回欧拉角，则可以用于体坐标和惯性坐标之间的转换
设计的程序：
#region -----------------------------------------------结构体定义（欧拉角，四元素，加速度和陀螺仪）
struct EulerAngle
{
public double Roll, Pitch, Yaw;
}
struct Quaternion
{
public double w, x, y, z;
}
struct acc
{
public int x, y, z;
}
struct gyro
{
public int x, y, z;
}
#endregion
#region ---------------------------------------------------------------------------欧拉角转四元数
Quaternion FromEulerAngle(EulerAngle ea)
{
Quaternion lq;
double fCosHRoll = Math.Cos(ea.Roll * .5f);
double fSinHRoll = Math.Sin(ea.Roll * .5f);
double fCosHPitch = Math.Cos(ea.Pitch * .5f);
double fSinHPitch = Math.Sin(ea.Pitch * .5);
double fCosHYaw = Math.Cos(ea.Yaw * .5);
double fSinHYaw = Math.Sin(ea.Yaw * .5);
lq.w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
lq.x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
lq.y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
lq.z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
return lq;
}
#endregion
#region --------------------------------------------------------------------------------四元数转欧拉角
EulerAngle ToEulerAngle(Quaternion bBQ)
{
EulerAngle ea;
ea.Roll = (double)Math.Atan2(2 * (bBQ.w * bBQ.z + bBQ.x * bBQ.y), 1 - 2 * (bBQ.z * bBQ.z + bBQ.x * bBQ.x));
ea.Pitch = (double)Math.Asin(Math.Min(1.0f, Math.Max(2 * (bBQ.w * bBQ.x - bBQ.y * bBQ.z), -1.0f)));
ea.Yaw = (double)Math.Atan2(2 * (bBQ.w * bBQ.y + bBQ.z * bBQ.x), 1 - 2 * (bBQ.x * bBQ.x + bBQ.y * bBQ.y));
return ea;
}
#endregion
#region ---------------------------------------------------------------------------------四元数乘法运算
Quaternion Multiply(Quaternion lq)
{
Quaternion c;
c.w = BQ.w * lq.w - BQ.x * lq.x - BQ.y * lq.y - BQ.z * lq.z;
c.x = BQ.w * lq.x + BQ.x * lq.w + BQ.y * lq.z - BQ.z * lq.y;
c.y = BQ.w * lq.y - BQ.x * lq.z + BQ.y * lq.w + BQ.z * lq.x;
c.z = BQ.w * lq.z + BQ.x * lq.y - BQ.y * lq.x + BQ.z * lq.w;
c = Normalize(c);
return c;
}
#endregion
#region -------------------------------------------------------------------------------四元数归一化
Quaternion Normalize(Quaternion e)
{
double s = (double)Math.Sqrt(e.w * e.w + e.x * e.x + e.y * e.y + e.z * e.z);
e.w /= s;
e.x /= s;
e.y /= s;
e.z /= s;
return e;
}
#endregion
#region ------------------------------------------将惯性坐标转到体坐标
acc Eu
ler_acc(acc myacc,EulerAngle ea)
{
acc result;
double A = Math.Sin(ea.Roll);
double a = Math.Cos(ea.Roll);
double B = Math.Sin(ea.Pitch);
double b = Math.Cos(ea.Pitch);
double C = Math.Sin(ea.Yaw);
double c = Math.Cos(ea.Yaw);
myacc.x = 0;
myacc.y = 64;
myacc.z = 0;
result.x = -(int)((a * c - A * B * C) * myacc.x - A * b * myacc.y + (a * C + A * B * c) * myacc.z);
result.y = (int)((A * c - a * B * C) * myacc.x + a * b * myacc.y + (A * C - a * B * c) * myacc.z);
result.z = -(int)( C * b * myacc.x + B * myacc.y + b * c * myacc.z);
return result;
}
#endregion
#region ------------------------------------------将体坐标转到惯性坐标
acc Euler_acc2(acc myacc, EulerAngle ea)
{
acc result;
/*
double P = Math.Cos(ea.Pitch);
double p = Math.Sin(ea.Pitch);
double Y = Math.Cos(ea.Yaw);
double y = -Math.Sin(ea.Yaw);
double R = Math.Cos(ea.Roll);
double r = Math.Sin(ea.Roll);
//myacc.x = 0;
//myacc.y = 64;
//myacc.z = 0;
result.z = (int)( P * Y * myacc.x + p * Y * myacc.y - y * myacc.z);
result.y = (int)( (P * y * r - p * R)*myacc.x + (p * y * r + P * R)*myacc.y + Y*r*myacc.z);
result.x = (int)( (P*y*R+p*r)*myacc.x + (p*y*R-P*r)*myacc.y + Y*R*myacc.z);
*/
double Y = Math.Cos(ea.Pitch);
double y = -Math.Sin(ea.Pitch);
double R = Math.Cos(ea.Yaw);
double r = -Math.Sin(ea.Yaw);
double P = Math.Cos(ea.Roll);
double p = -Math.Sin(ea.Roll);
//myacc.x = 0;
//myacc.y = 64;
//myacc.z = 0;
result.x = (int)((P * R - p * y * r) * myacc.x + (p * R + P * y * r) * myacc.y - (Y * r) * myacc.z);
result.y = (int)((-Y * p) * myacc.x + (Y * P) * myacc.y + y * myacc.z);
result.z = (int)((P * r + p * y * R) * myacc.x + (p * r - P * y * R) * myacc.y + Y * R * myacc.z);
return result;
}
#endregion
#region -----------------------------------------------------------------根据重力加速度修正陀螺仪
int cya, cxa, cza;
void acc_to_gy()
{
if ((Math.Abs(xa - xa0) < interal_a) & (Math.Abs(ya - ya0) < interal_a) & (Math.Abs(za - za0) < interal_a) & (Math.Abs(xa - xa0) < interal_a))
{
ca0++;
//************判断是否只有重力加速度（15次连续判断有效） //可以选择20次判断有效，取第10次的值，这样会稳定一点
if (ca0 == 10)
{
cya = Math.Min(64, Math.Max(-64, ya));
cxa = Math.Min(64, Math.Max(-64, xa));
cza = Math.Min(64, Math.Max(-64, za));
}
if (ca0 == 20)
{
//*********************************************根据重力加速度而推算的欧拉角
Pitch = -180d * Math.Asin((double)cza / 64d) / Math.PI;
Roll = 180d * Math.As
in( Math.Min(1, Math.Max(-1, (double)cxa / 64d / Math.Cos(Pitch * Math.PI / 180d) ))) / Math.PI;
//从π扩到2π
if (cya < 0)
Roll = 180d - Roll;
ca0 = 0;
EulerAngle lea;
lea.Roll = Roll*Math.PI/180d;
lea.Pitch = Pitch * Math.PI / 180d;
lea.Yaw = nowG.Yaw;
//**********************************将上面的欧拉角转为对应的4元数
Quaternion lq = FromEulerAngle(lea);
lea = ToEulerAngle(lq);
textBox2.BeginInvoke(new System.EventHandler(display2), lea.Pitch * 180 / Math.PI);
textBox4.BeginInvoke(new System.EventHandler(display4), lea.Yaw * 180 / Math.PI);
textBox6.BeginInvoke(new System.EventHandler(display6), lea.Roll * 180 / Math.PI);
//*********************************************下面对比两组4元数的值
//BQ = revise(BQ, lq);
textBox12.BeginInvoke(new System.EventHandler(display12), BQ.w);
textBox13.BeginInvoke(new System.EventHandler(display13), BQ.x);
textBox14.BeginInvoke(new System.EventHandler(display14), BQ.y);
textBox15.BeginInvoke(new System.EventHandler(display15), BQ.z);
textBox16.BeginInvoke(new System.EventHandler(display16), lq.w);
textBox17.BeginInvoke(new System.EventHandler(display17), lq.x);
textBox18.BeginInvoke(new System.EventHandler(display18), lq.y);
textBox19.BeginInvoke(new System.EventHandler(display19), lq.z);
}
}
else
{
ca0 = 0;
}
xa0=xa;
ya0=ya;
za0=za;
}
#endregion