开源！我的捷联系统的四元数法姿态算法和程序

dongfang

最近研究飞控的时候，感受到，网上大量他人共享开放的信息给我提供了很多便利。（尽管也混杂了大量无法使用的信息）

捷联系统的四元数法姿态算法的难点不是资料找不到，而是资料大多数没有标明欧拉角的定义（拜科学家意见不统一所赐，欧拉角有12种定义方法），而不同定义的欧拉角，双向转换四元数的公式都是不一样的。而我最终找到了一套适合飞控的算法。

为了不让这些开放信息到了我这里就终结了，也为了后人不要重复无谓的重复劳动，我把我做的东西也开源出来。


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

算法输入：物体的初始姿态，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，就可以以直观的形式查看当前姿态


这里严重感谢http://www.cppblog.com/heath/archive/2009/12/13/103127.html 的作者，他的代码解决了我最重要的问题: Tait-Bryan angle定义的欧拉角和四元数的双向转换。但他是个游戏开发者，没做姿态更新，我补充了姿态更新的代码。


另外补充一句，我使用的是ardunio做的开发，也就是12M的8位AVR单片机，目前，每秒在获取数据400个的速度下，如此大工作量的算法依然能跑。3D旋转N圈，大约10秒钟，各轴漂移在5度左右。

[ 本帖最后由 dongfang 于 2011-6-23 11:01 编辑 ]

dongfang

核心算法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;
}

dongfang

核心算法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;
}

dongfang

核心算法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;
}

dongfang

次要的规范化算法：
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);
}

dongfang

我的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;
  }
}

dongfang

我的excel版本的学习笔记 捷联式惯性导航算法研究222.xlsx (24.57 KB, 下载次数: 1458)

2011-6-23 10:42 上传

点击文件名下载附件

dongfang

论坛不让上传rar文件，库函数的源码就不传了。看得懂上面代码的人，100%也能自己做了。

算法的有效性，可以参考excel文件。

votreami

一点也不懂

MAGIC-JW

:em26: 用得上真是宝  

高数线性偶都丢完了 泪奔

dongfang

原帖由 MAGIC-JW 于 2011-6-23 11:21 发表
:em26: 用得上真是宝  

高数线性偶都丢完了 泪奔

me too....实际上，我后来才去网上翻了一下线性代数。这里面的公式推导，我们不用关心，那是理科、数学家的事情。
我们做工科的，找到能用的就行了。

datiao

学习！

dstdx72

:em26: :em24: 热烈欢迎，狂顶....
http://www.cppblog.com/heath/archive/2009/12/13/103127.html
这个我也看过，哈哈，相当不错
四元数法前几天就试过了，其精度跟积分频率和陀螺输出准确率关系很大
比如陀螺的角速率输出误差（含测量误差）较大，加上积分误差，那么当转动90度时可能算出来才81度
理论上讲，积分频率越高，积分误差越小
所以估计得性能很好的mcu才能达到要求
没想到楼主的8位机也能达到这个精度？每秒姿态计算多少次？
大约10秒钟，各轴漂移在5度左右，你用的什么陀螺？性能这么好
我用的lpr550+lpy550，是用了过采样算法把AD分辨率提高了1000倍，才在静态下做到了每分钟小于5度的静态漂移的

leanang

谢谢 dongfang 的无私.

oppop

好东西

hifiafrica

我所见过的自驾仪，都是非常依靠GPS的信号的，我最早玩的AP50，就是三个村田陀螺增稳阻尼，就能飞航线，而且还是三角翼。

不要试图去提高你姿态系统的性能，这个估计很难，要试图把GPS的信号引入你的姿态系统，让GPS的航向啊，位置点漂移啊，对你的姿态起到反馈作用。

没有什么飞机能只变化姿态，而不变化航迹的。GPS只要检测到航迹变化，就意味着你的姿态发生变化了。

firedog2011

搞FPV搞到这么复杂还有乐趣可言吗？唉

栋栋

原帖由 firedog2011 于 2011-6-23 15:51 发表
搞FPV搞到这么复杂还有乐趣可言吗？唉

各有所好，对科学的钻研精神的确令人敬佩。应该是我们的好榜样对吧！

aband

楼主没有买《惯性导航》？你说的这些东西在这本书上都有 还有详细的推导

fujianshu

厉害啊