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

jmp2002

原帖由 峰回路转 于 2012-1-31 17:42 发表

受教了。。谢谢。
我搞这个完全处于兴趣使然，无奈上学太少很多东西搞不太明白，只好多多请教学习。就当是万里长征了，呵呵
从从开始，首先获得初始状态的四元，先从acc获得初始的欧拉角，测试了一下用Roll= ata ...

单位是弧度，你换算成角度就行了：1.57*180/3.14159=90(度)

luopeixuan

原帖由 votreami 于 2011-6-23 10:49 发表
一点也不懂

Me Too!

dongfang

原帖由 (涉嫌广告已屏蔽) 于 2012-1-30 23:04 发表

最近研究XPLANE、APM SIM还有MAVLINK，得把空速管什么的数据都弄出来才好仿真。
xplane可是个好东西啊，可惜只有一架PT60。。。找不到其他模型了，不知道你用什么模型模拟的？
APM功能很强大，正在研究，不过是C ...

我也使用PT60模拟的。APM成品度太高了，看了就感觉不是个人业余工作能达到的。
等会儿我晒几张测试报告吧，我觉得只要xplan是可靠的，它模拟出来的数据就有参考性，即使每架飞机都是不一样的。

峰回路转

原帖由 jmp2002 于 2012-1-31 21:25 发表

单位是弧度，你换算成角度就行了：1.57*180/3.14159=90(度)

呵呵。的确如此。谢谢

dongfang

XPLAN测试报告：http://bbs.5imx.com/bbs/viewthread.php?tid=573424

峰回路转

刚才测试了我移植到stm32下的四元，欧拉角互转代码，我理解为，首先将三个已知的欧拉角（例如：roll=2，pitch=3，raw=4）转换为四元数，得到一个四元数（w,x,y,z），然后不做任何处理。直接在转换会欧拉角，那么得到的欧拉角仍然应该为roll=2，pitch=3，raw=4。
按照这个思路测试，结果却不是这么回事？是我理解的不对么？
另外：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;
}

与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;
}
里面的：ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
与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);
里的 .5f 是0.5*采样间隔吗？如果是采样间个的话那我上面的现象就是正常了。。。

[ 本帖最后由 峰回路转 于 2012-2-1 02:48 编辑 ]

峰回路转

原帖由 dongfang 于 2012-1-31 23:44 发表
XPLAN测试报告：http://bbs.5imx.com/bbs/viewthread.php?tid=573424

恩。建立理论基础的利器啊。。。。我有套apm，xplan下的模拟一直没有试过，主要是用了好几天也没下 载完xplan软件，在官网上下的那个测试版有时间限制，等有时间了好好向您学习

[ 本帖最后由 峰回路转 于 2012-2-1 02:51 编辑 ]

heuyck

原帖由 峰回路转 于 2012-2-1 02:47 发表
刚才测试了我移植到stm32下的四元，欧拉角互转代码，我理解为，首先将三个已知的欧拉角（例如：roll=2，pitch=3，raw=4）转换为四元数，得到一个四元数（w,x,y,z），然后不做任何处理。直接在转换会欧拉角，那么得到 ...

这里的0.5并非采样间隔，想想就知道了，如果是采样间隔，起码应该是0.5*T的形式。这里就是简单的欧拉角/2。
为什么要/2我不清楚，欧拉角和四元数互转推算过程可以去找惯导书，推算过程我看不懂也不想懂，反正最后计算公式就是欧拉角/2的。

公式里都是以弧度做单位，欧拉角有它自己的取值范围，roll和yaw是-π到π，pitch是-π/2到π/2。所以那个欧拉角为2 3 4的例子是无效的，转成四元数再转回欧拉角，欧拉角就回到有效范围里，当然和初始值不相等了。
建议你把输入的欧拉角弄成角度，然后在公式里再转成弧度计算，最后计算得到的欧拉角弧度再转回角度，这样角度值容易看一些。。。。

[ 本帖最后由 heuyck 于 2012-2-1 09:29 编辑 ]

heuyck

CbE = Cγ·Cθ·Cψ =
[cosψcosθ                                        sinψ cos θ                                        -sinθ]
[cosψsinθsinγ - sinψcosγ                sinψsinθsinγ + cosψcosγ                cosθsinγ ]
[cosψsinθcosγ + sinψsinγ                sinψsinθcosγ - cosψsinγ                cosθcosγ]
这是姿态矩阵，也就是余弦矩阵，也可以看成是欧拉角转余弦矩阵的方程，其中γ是横滚roll，θ是俯仰pitch，ψ是航向yaw。

        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));
这个里面的几个wxyz是把四元数转成了余弦矩阵的几个元素，然后用余弦矩阵里对应的关系用倒三角函数求出r p y。


欧拉角、四元数、余弦矩阵，这三种姿态的表达方式都可以互相转化，如果想了解多一点，还是去看书吧。比如邓正隆的《惯性技术》。

[ 本帖最后由 heuyck 于 2012-2-1 09:14 编辑 ]

峰回路转

公式里都是以弧度做单位，欧拉角有它自己的取值范围，roll和yaw是-π到π，pitch是-π/2到π/2。
看到这句话茅塞顿开啊。。。。。。。测试了一下果然如此，也就是说roll和yaw取值范围正负180度，pitch取值在正负90度（度格式）
至于那个。5f我也不多想了，根据公式直接改成      
     float fCosHRoll = cos(ea_fRoll * 0.5);
        float fSinHRoll = sin(ea_fRoll * 0.5);
        float fCosHPitch = cos(ea_fPitch * 0.5);
        float fSinHPitch = sin(ea_fPitch * 0.5);
        float fCosHYaw = cos(ea_fYaw * 0.5);
        float fSinHYaw = sin(ea_fYaw * 0.5);
这样来回转换的数值就正确了。。。
ea_fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.57, 1.57));就是对pitch做了一个限制。取值范围就限制在正负π/2(弧度)

[ 本帖最后由 峰回路转 于 2012-2-1 17:15 编辑 ]

峰回路转

乘胜追击，测试了四元的乘法，Quaternion_Multiply(w,x,y,z,b_w,b_x,b_y,b_z )                        //{
        //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;
        Quaternion_Normalize(c_w,c_x,c_y,c_z);
        //return c;
}

发现输出严重错乱。研究中。。

峰回路转

终于调通了。代码如下
ea_EulerAngle[0]=0;//(atan2(ax,az));           //根据acc算出欧拉（弧度）
        ea_EulerAngle[1]=0;//(-atan2(ay,az));
        ea_EulerAngle[2]=0;
        Quaternion_FromEulerAngle( ea_EulerAngle[0],ea_EulerAngle[1],ea_EulerAngle[2] ); //欧拉转四元 初始值
        ea_Quaternion[0]=w;
        ea_Quaternion[1]=x;
        ea_Quaternion[2]=y;                          //放入初始化数组
        ea_Quaternion[3]=z;

          Roll=ea_EulerAngle[0]*57.29578;                         //将弧度转换为角度，便于观察
          Pitch=ea_EulerAngle[1]*57.29578;

         delay_ms(21);

        while(1)
        {test=!test;
          GyroRead();                                           //读取gyro
          new_EulerAngle[0]=(gx/rad*f);          //读取到的为度/s   /pi/180
          new_EulerAngle[1]=(gy/rad*f);
          new_EulerAngle[2]=(gz/rad*f);

          Quaternion_FromEulerAngle( new_EulerAngle[0],new_EulerAngle[1],new_EulerAngle[2] ); //欧拉转四元 更新
          new_Quaternion[0]=w;
          new_Quaternion[1]=x;                  //得到新的四元
          new_Quaternion[2]=y;
          new_Quaternion[3]=z;

          Quaternion_Multiply(ea_Quaternion[0],ea_Quaternion[1],ea_Quaternion[2],ea_Quaternion[3],new_Quaternion[0],new_Quaternion[1],new_Quaternion[2],new_Quaternion[3] )        ;//四元乘法，和原始姿态融合
          ea_Quaternion[0]=c_w;
          ea_Quaternion[1]=c_x;                  //得到新的四元
          ea_Quaternion[2]=c_y;
          ea_Quaternion[3]=c_z;

          Quaternion_ToEulerAngle(c_w,c_x,c_y,c_z);        //四元转欧拉(弧度)

          Roll=ea_fRoll*57.29578;                                                //弧度转化为度
          Pitch=ea_fPitch*57.29578;
          Yaw=ea_fYaw*57.29578;
         
      LED1=!LED1;
          delay_ms(21);
        }
实际初始化是没有计算角度
出现的问题：得到的roll pitch yaw值转换为角度后数值缩小了10倍，例如：90度，算出来的是9.0度，仔细检查了adc和弧度-度转换没发现问题在哪。还有就是ea_fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0 , 1.0));中的clamp函数，应该是clamp（x,min,max）if x<min x=min;if x>max x=max. 但是一旦加加上CLAMP ，pitch的输出始终为零。只好写成 ea_fPitch = asin(2 * (w * x - y * z));
目前静态漂移在每分钟18度左右。

[ 本帖最后由 峰回路转 于 2012-2-2 02:05 编辑 ]

静静的回忆

没人研究哪来的FPV给你玩

峰回路转

原帖由 (涉嫌广告已屏蔽) 于 2012-2-2 12:45 发表

asin不用CLAMP限幅，四元数每次都会规范化一下，而且欧拉角的定义就是这个范围，所以此处算出的pitch是绝对不会出现问题的。
静态漂移跟陀螺零偏有关系，与算法无关。校准过的陀螺，yaw轴没有加计校正，大概漂移在 ...

谢谢，把弧度转换的系数统一用#define  rad 180/PI 输出值就正常了。以前有的用的是180/PI 有的用的是57.29578，难道#define  rad 180/PI 得到的rad=572.9578？
下面改研究一下陀螺的零偏和加速度的融合了。呵呵。。。

竹蜻蜓的天空

:em26:

vv.vv

:em26: :em26:

峰回路转

ea_rolll=(atan2(ay,az));           //根据acc算出欧拉（弧度）  
ea_pitch=(-atan2(ax,az));
这样算的确有问题。p和r会互相影响
应该用 h_euyck 的公式：
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);      //accel_1g = 1 ？
但是这样当pitch翻转超过90度时该怎么办呢？

计算速度融合采用了 输出=acc*0.7+gyro*0.3 的方法。抑制漂移很有效果，问题还是当pitch超过90度时似乎是溢出还是怎么的，输出就不正常了。、
是不是欧拉角限制问题啊？

[ 本帖最后由 峰回路转 于 2012-2-3 12:59 编辑 ]

heuyck

accel_1g是重力加速度单位，是9.81xxxx
无论yaw-pitch-roll顺序定义的欧拉角pitch范围，还是arcsin函数计算结果的范围，都是正负90度。。。。何来超过90度。。。
比如roll和yaw不变为0度，你想象中的pitch慢慢变大，最后超过90度比如91度的过程，欧拉角的实际描述过程如下：
roll和yaw维持0度，pitch慢慢变大；在90度时进入奇点，欧拉角陷入万向锁问题，roll和yaw无法计算和分辨；越过90度到达91度时，pitch减小到89度，roll变成-180度，yaw变成180度。

heuyck

所谓奇点或者万向锁的问题，我解释一下，你可以拿本书比划着想象。
按yaw-pitch-roll顺序定义的欧拉角，如果顺序中间的pitch是90度，那么先yaw多少度和后roll多少度是等价的，根本分不清楚。从公式上来看，roll和yaw也无法从arctan中计算出来，arctan的输入有很小的波动，roll和yaw就乱变。
按其他顺序定义的欧拉角同样有这个问题。

[ 本帖最后由 heuyck 于 2012-2-3 13:21 编辑 ]

fqrmmra

帮顶:em26: 搞技术的都是牛人