请教四轴AHRS算法的问题

13902532910

我虽然死套别人写的程序，出了结果，但想把其中每一步搞懂，因为发现旋转再静止后，2至3秒，数据才稳定下来。
希望懂的人指点下，谢谢!
根据别人写的程序如下:

13902532910

先说下我对程序的理解

我觉得上面是用上一次四元数决定的物体的姿态，用来算各轴上加速度值和磁场强度值。
下面是我查的资料，四元数转换为旋转为旋转矩阵

先看轴上加速度值vx,vy,vz，由上式可算出vx,vy,vz。
即：[0,0,1]*上面矩阵，因为只有竖直向下有加速度，所以是[0,0,1]。
**********************************************************************************
问题1：但是我不明白，为什么程序里面VZ为什么不是1-2(q1^2+q2^2)。
*************************************************************************************
对于对各轴上磁场强度值wx,wy,wz，程序上好像采取的是将mx,my,mz按照四元数转为参考坐标中各轴上的磁场强度值hx,hy,hz，由于磁场方向可能不是水平，但是hy上应该为0,所以后面用bx,bz来补偿，将Y轴上搞到0。上面的矩阵是将参考坐标系转换为物体自己的坐标系，由于这步是将物体坐标系转换为参考坐标系，所以上面的矩阵应该修改，不过好像只要将矩阵由右乘改为左乘就可以了。
即：上面矩阵*[mx,my,mz]T(T表示转置，将行变成列)
算出bx,bz后，然后跟加速度一样算出wx,wy,wz。
即:[bx,0,bz]*上面矩阵

13902532910

***************************************************************************
问题2：这一步真的好难理解
听人说好像是加速度矢量在物体坐标系里表示为[ax,ay,az](测量)和[vx,vy,vz](四元数),两者误差可以用两个矢量的叉积表示，还是搞不懂，为什么叉积就可以表示误差。
对于exInt=exInt+ex*Ki这个式子，听人说是累计误差，防止零漂神马的，可是Ki的值为什么取0.005呢，又是怎么起到作用的呢?
gx=gx+Kp*ex+exInt这个式子也搞不懂，这是如何起到修正作用的呢？Kp的值取2有什么作用呢？
*********************************************************************************

13902532910

对于这步，我是这么理解的。
下面是我查的资料，四元数只要单位化，就可以具有下面的意义。

此时的四元数可以看成参考坐标系经过旧四元数旋转一次后，再经过新的陀螺仪旋转一次后，形成新的四元数。
则新的四元数等于旧四元数乘上陀螺仪旋转对应的四元数。
首先需将陀螺仪旋转转换成四元数，由于采样很快，alpha的值趋近于0，根据正弦余弦极限情况可知，故q0近似为1,q1近似为a/2*cos(Bx),q2近似为a/2*cos(By),q3近似为a/2*cos(Bz)。又因为a*cos(Bx)，可看成角度a在X轴上的投影角度，即等于gx*T,则q1=gx*halfT。同理可推q2和q3。
*****************************************************************************************
问题3：程序此处为什么直接q0=,q1=,q2=,q3=，这样做不是新的q0值会影响q1,新的q0和q1
会影响q2吗？为什么不搞个temp1=,temp2=,temp3=temp4=,然后再q0=temp1,q1=temp2,q2=temp3,q3=temp4？
*******************************************************************************************

13902532910

下面是我查的资料，四元数如何得出欧拉角，符合情况

登云钓月

楼上威武 不明觉厉

正点原子

帮顶....

13902532910

问题1解决了，原来q0*q0+q1*q1+q2*q2+q3*q3=1,所以q0q0 - q1q1 - q2q2 + q3q3=1-2(q1*q1-q2*q2)
问题3也解决了，的确原程序有问题，改成temp代替后效果更好了。
就剩问题2了。。。。
求大神帮忙看下问题2，谢谢！

13902532910

在MATLAB中发现从物体坐标系到世界坐标系，变换成世界坐标系到物体坐标系，的确右乘改成左乘就可以了。

蜕变

楼主这是全姿态的姿态结算吗？就是加上磁罗盘后在模块与俯仰和翻滚角的时候，航向角也能比较准确的指示相对于北的方向吗？

wzmin

LZ你好！请问你是用什么芯片做的？ 得到的欧拉角准吗？

yuzeyuan1

搞了一下午，发现几个神奇的问题，楼主贴的错别公式就不说了，好像有两处都是笔误吧。另外，楼主找到了http://blog.csdn.net/yaokang522/article/details/46789075
类似这个网页上所讲的四元数与欧拉角之间的转换，它最后说

然而，在惯导的书里写着，横滚角是arctan(-T31/T33)，也就是arctan( (2*(q1q3+q0q2)) / (1-2*(q1*q1+q2*q2)) )。俯仰角是arcsin(T32)，也就是arcsin( 2*(q2*q3-q0*q1) )。航向角是arctan( T12/T22 )，也就是arctan( (2*(q1q2+q0q3)) / (1-2*(q1*q1+q3*q3)) )。书是《惯性导航》秦永元，250页之后写的很清楚。很明显，和上面图中式（7）所对应的不一样！！！
不管是书上还是网址上，w、常数项、q0应该是互相对应的，同理x、X轴、q1应该是互相对应的，等等，这没错吧？
这不是很矛盾吗？楼上那些说能搞出来的。。请问你们这里看明白了吗？怎么越过去的？我卡住了。

yuzeyuan1

我以前从来没有把传感器转180度以上，让它倒过来，所以用着也能行，pitch  roll  yaw也试过都基本是对的，但是今天偶然把传感器倒过来试试，发现转动俯仰角时，横滚角竟然会从0到-180之间变化，感觉它其实是想直接跳变到-180的，但由于q0至q4都需要渐变，所以画出图来就成了横滚角从0快速渐变到-180。。。仔细又找了一遍，只有我上一层上说的地方有问题呀。哪位懂这个，帮忙解释一下呗

xuyan021

问题2是用PI控制法来对陀螺仪的结果进行修正。

xiaoloveyao

楼主可否放出全部源码？ 包括9轴数据部分。

ffme

关于问题2，叉乘。先来看叉乘定义，|向量a×向量b|=|a||b|sinθ，若向量a和b均为单位向量，，即|a|=1，|b|=1，那么，|向量a×向量b|=|a||b|sinθ=sinθ。当θ在值很小的时候，有sinθ≈θ，即高等数学上的等价无穷小。所以两单位向量的叉积可以表示它们之间在空间里方向上的误差。这也是程序反复使用单位化、归一化的原因。

睡不着的诗人

问一下AHRS中的参数是什么，从mpu6050中直接读到的原始数据吗

清风莞尔

13902532910 发表于 2015-4-16 00:44
问题1解决了，原来q0*q0+q1*q1+q2*q2+q3*q3=1,所以q0q0 - q1q1 - q2q2 + q3 ...

后面式子括号里应该是“+”号吧，这样就对了

chuck__chee

yuzeyuan1 发表于 2015-11-24 17:52
我以前从来没有把传感器转180度以上，让它倒过来，所以用着也能行，pitch  roll  yaw ...

朋友 我现在也遇到这个问题了 不知道当时你怎么解决的~ 望赐教

yuzeyuan1

hello，这个是我最开始遇到的问题了，当时我的代码中没在意转一圈后跳变的问题，所以后面的滤波等等处理都是直接过滤这个跳变角度的，只要改一下不让它跳变就没问题了。
由于使用tan和tan2，所以角度本身就是+-180和+-90跳变的。
        roll(m)=atan2(2*q0*q1+2*q2*q3,1-2*q1*q1-2*q2*q2)/pi*180;
        pitch(m)=asin(2*q0*q2-2*q1*q3)/pi*180;
        yaw1(m)=atan2(2*q0*q3+2*q1*q2,1-2*q2*q2-2*q3*q3)/pi*180;
        azimuth1(m)=yaw1(m);
        if((azimuth1(m)-azimuth1(m-1)<345)&&(azimuth1(m)-azimuth1(m-1)>-345))
            luopan(m)=azimuth1(m)+sumquan*360;
        end
        if(azimuth1(m)-azimuth1(m-1)>345&&azimuth1(m-1))
            sumquan=sumquan-1;luopan(m)=azimuth1(m)+sumquan*360;
        end
        if(azimuth1(m)-azimuth1(m-1)<-345)
            sumquan=sumquan+1;luopan(m)=azimuth1(m)+sumquan*360;
        end
随便写一下matlab代码，大概意思就是，当检测到是180跳到了-180，sumquan就加1，当检测到是-180跳到了180，sumquan就减1，最后的值存在数组luopan里，它就是一个上可以正无穷，下可到负无穷的数了，不再受+-180的限制。代码你自己想一上靠谱的就行，我这个随便写的，没出过问题就没变过，反正测试用。

yuzeyuan1

hello，这个是我最开始遇到的问题了，当时我的代码中没在意转一圈后跳变的问题，所以后面的滤波等等处理都是直接过滤这个跳变角度的，只要改一下不让它跳变就没问题了。
由于使用tan和tan2，所以角度本身就是+-180和+-90跳变的。
        roll(m)=atan2(2*q0*q1+2*q2*q3,1-2*q1*q1-2*q2*q2)/pi*180;
        pitch(m)=asin(2*q0*q2-2*q1*q3)/pi*180;
        yaw1(m)=atan2(2*q0*q3+2*q1*q2,1-2*q2*q2-2*q3*q3)/pi*180;
        azimuth1(m)=yaw1(m);
        if((azimuth1(m)-azimuth1(m-1)<345)&&(azimuth1(m)-azimuth1(m-1)>-345))
            luopan(m)=azimuth1(m)+sumquan*360;
        end
        if(azimuth1(m)-azimuth1(m-1)>345&&azimuth1(m-1))
            sumquan=sumquan-1;luopan(m)=azimuth1(m)+sumquan*360;
        end
        if(azimuth1(m)-azimuth1(m-1)<-345)
            sumquan=sumquan+1;luopan(m)=azimuth1(m)+sumquan*360;
        end
随便写一下matlab代码，大概意思就是，当检测到是180跳到了-180，sumquan就加1，当检测到是-180跳到了180，sumquan就减1，最后的值存在数组luopan里，它就是一个上可以正无穷，下可到负无穷的数了，不再受+-180的限制。代码你自己想一上靠谱的就行，我这个随便写的，没出过问题就没变过，反正测试用。

yuzeyuan1

chuck__chee 发表于 2016-7-28 00:32
朋友 我现在也遇到这个问题了 不知道当时你怎么解决的~ 望赐教

我回复错了，看楼上

Lynnllh

4楼实际上是用欧拉法解四元数微分方程

曾洪涌

大哥能否跟我说下该算法求解三个角度时，磁力计直接得到数据要如何处理才能带入这个公式