基于四元数法的捷联式惯性导航系统的姿态解算（完善中）

     从开始学习姿态解算到现在大概有一周的时间了，我把学习的心得分享给大家 ，同时也是做一个学习总结

和给大家提供一个学习的思路。刚开始学习没大有什么思路，先从网上买了一本《惯性导航》秦永元  ,如图:

，了解陀螺仪和加速度 计的原理。重点看了第九章 捷联式系统 。下面做一个姿态控制的整体分析:

(引用自amoBBS论坛的feng.matrix)

框图说明：加速度计acc 和gyro进行融合，通过卡尔曼滤波得到真实pitch 和roll，再结合磁阻传感器得到方位角（yaw）。

这里需要强调一下,加速计和陀螺仪仪积分得到的角度并不是真实pitch ,roll .图中给出的卡尔曼滤波包括真实pitch ,poll角的计算.

理清思路，为了让四轴能飞行，首先必须能够自己感觉自己的姿态，其中陀螺仪担任重要的角色！

    下面说明一下陀螺仪的输出角速度到 欧拉角（维基百科）的过程，这一部分可以参考《惯性导航》的288页，由于一般的

传感器精度有限度，数据处理过程简化为如下图过程 ：

流程说明：系统一启动，首先获取初始四元数（维基百科）是为了找到一个参考坐标系，因为只有确定的参考坐标系才能在计算

出相对参考坐标系转动的角度，及载体的航向角、俯仰角、横滚角。其中Q=Q1*Q2

表示Q的是先做Q2的旋转，再做Q1的旋转的结果，而多个四元数的旋转也是可以合并的，流程图的核心算法如下：

//核心算法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));
//#define CLAMP(x , min , max) ((x) > (max) ? (max) : ((x) < (min) ? (min) : 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;
  }
}

 代码来自http://bbs.5imx.com/bbs/forum.php?mod=viewthread&tid=504301&page=1,本人已经测试可以使用仅供大家参考。程序以弧度

为角度单位。下面是大部分数据的融合的基本过程，陀螺仪存在积分漂移，隔三差五的问问加速度计和磁传感器，我角度飘了多少了。关于数据融合，

也有好多方法，这里采用一种最为形象，贴切的，简单易懂的，[互补滤波]。如图：

通过上图可以看出,陀螺仪在融合后的角度基础上积分,然后与加速度计测量的角度融合,然后又提供给下一次积分,看起来更像一个反馈系统。

另外一种融合方法:全姿态融合，3维的姿态融合方法：

图中的符号解释：Bg是加速度计测得的值,扩充为四元数,Bg=[0,Bgx,Bgy,Bgz],上标B代表是在体坐标系(Body Frame)下的测量值
（1）Bh是磁传感器计测得的值,扩充为四元数,Bh=[0,Bhx,Bhy,Bhz]
（2）Eh是参考坐标系下的固有磁场,地磁场,上标E代表是在参考坐标系(earth Frame)下的值
（3）Eg是参考坐标系下的固有加速度,重力加速度
（4）p,q,r是陀螺仪测得的角速度
（5）[a,b,c,d]是姿态四元数Q

假设现在已经得到正确的姿态四元数Q,那么可以利用四元数旋转将参考坐标系和体坐标系下的向量互相转换,将Eh和Eg转换到体坐标

系下(BEh=Q×Eh×Q* , BEg=Q×Eg×Q*,BEh、BEg是参考坐标系的Eh、Eg，由Q旋转到体坐标系下)在理想情况下,那么Bg=BEg,Bh=BEh,也

就是 { Q×Eh×Q* - Bh = 0} , { Q×Eg×Q* - Bg = 0}这两个方程成立,联立这两个方程就可以解得姿态四元数Q,这个方程组无解,因此我

们只能找到最优解,找最优解的最小方差问题有许多方法可以采用,如[梯度下降][高斯牛顿],图中是用的高斯牛顿法,由于高斯牛顿法含有矩阵求

逆的计算,计算量超大,处理器不是DSP或FPGA的话就算了,关于梯度下降]和[高斯牛顿],可以在google,wiki百科搜索。

      下面给出陀螺仪积分求姿态的精确计算方法，加速度计和磁传感器经过高斯牛顿迭代得到姿态误差速率,陀螺仪直接通过四元数微分方程得到姿

态四元数速率,两个加起来积分得到姿态四元数。过程如下图：

代码来自http://bbs.5imx.com/bbs/forum.php?mod=viewthread&tid=504301&page=1

数据融合参考http://www.amobbs.com/thread-4769138-1-1.html。