amoBBS 阿莫电子论坛

 找回密码
 注册
搜索
bottom↓
楼主: seanwood

捷联惯导算法心得

  [复制链接]
发表于 2013-2-10 18:49:33 | 显示全部楼层
好帖子,精辟
发表于 2013-2-10 19:44:53 | 显示全部楼层
谢谢分享~
发表于 2013-2-12 19:41:28 | 显示全部楼层
mark
发表于 2013-2-16 02:19:15 | 显示全部楼层
捷联式惯性导航原理
发表于 2013-2-16 08:50:42 | 显示全部楼层
加上星光就可以搞弹道导弹了
发表于 2013-2-16 17:31:43 | 显示全部楼层
本帖最后由 jzjjzj 于 2013-2-16 17:33 编辑

终于看懂了它们之间的关系了
发表于 2013-2-17 19:24:27 | 显示全部楼层
mark
发表于 2013-2-19 11:14:39 | 显示全部楼层
先顶一下 慢慢研究~~
发表于 2013-2-19 12:49:45 | 显示全部楼层
收藏 捷联惯性导航系统
 楼主| 发表于 2013-2-20 10:15:59 | 显示全部楼层
四元数是有三角函数运算,但是只在解出欧拉角的时候才用。每个积分周期是用不到的。

举个例子,比如为了积分精确,积分周期用1ms一次,但对外部电机的控制周期比如是10ms。那结算姿态只需要10ms一次。
欧拉角微分方程需要每1ms周期计算6个三角函数来积分。
四元数则只需要每10ms计算3个反三角函数,这运算量差距就很明显了。
发表于 2013-2-20 10:17:06 | 显示全部楼层
学习中ing
发表于 2013-2-20 12:58:18 | 显示全部楼层
学习学习
发表于 2013-2-21 14:53:50 | 显示全部楼层
看了几遍了,没看一边似乎又多懂一点,还在继续啃
发表于 2013-2-21 21:05:57 | 显示全部楼层
百年好贴
发表于 2013-2-22 13:37:02 | 显示全部楼层
楼主总结的非常好。
发表于 2013-2-22 13:48:10 | 显示全部楼层
牛人 谢谢 mark
发表于 2013-2-22 14:58:20 | 显示全部楼层
学习!
发表于 2013-2-25 09:54:09 | 显示全部楼层
不错,好贴,学习了
发表于 2013-3-1 23:08:42 | 显示全部楼层
大哥,四轴飞行的时候应该是不停运动的啊,比如上升或者下降,那么加速度计测出来的值不就不准了么?
发表于 2013-3-4 12:29:42 | 显示全部楼层
学习了 感谢楼主
发表于 2013-3-7 11:12:44 | 显示全部楼层
感谢  楼主
发表于 2013-3-8 17:28:56 | 显示全部楼层
先收藏了,太深奥了
发表于 2013-3-15 09:33:08 | 显示全部楼层
LZ说用加速度计算出欧拉角来初始化四元数,然而加速度计只能计算俯仰角和滚动角,不能计算偏航角。因此怎样计算初始四元数;另外,LZ在程序里初始化四元数是q0 = 1, q1 = 0, q2 = 0, q3 = 0,是否默认初始机体坐标与地理坐标重合,如果不重合是否会对以后的计算造成误差。程序里也没有看见用加速度计初始化四元数啊
发表于 2013-3-15 17:02:35 | 显示全部楼层
技术牛人,为人也如此无私,赞一个
发表于 2013-3-16 17:05:29 | 显示全部楼层
有点迷糊,
发表于 2013-3-18 16:52:34 | 显示全部楼层
seanwood 发表于 2013-2-20 10:15
四元数是有三角函数运算,但是只在解出欧拉角的时候才用。每个积分周期是用不到的。

举个例子,比如为了积 ...

楼主的意思是每个积分周期都要初始化四元数,那eIntxyz还有用吗?那上一时刻算出的时刻在下一时刻也用不上啊
 楼主| 发表于 2013-3-20 13:27:04 | 显示全部楼层
我什么时候说过每个周期都要初始化啊?
初始化,就是把 加速度计、电子罗盘的三轴结合算出的欧拉角,转换成四元数。
每个更新周期,是用陀螺仪的角速度去对四元数进行积分。
要用到欧拉角的时候,再使用 四元数->欧拉角,来结算出欧拉角。
示例代码中只有更新周期,没有初始化和欧拉角结算,不要想直接拿来用,帖子整个看一遍。
发表于 2013-3-20 15:11:36 来自手机 | 显示全部楼层
太棒了,楼主写的太好了,简洁,明确。终于懂得了。谢谢楼主。
 楼主| 发表于 2013-3-21 09:56:31 | 显示全部楼层
本帖最后由 seanwood 于 2013-3-21 10:00 编辑

有人问加电子罗盘的融合是怎么回事,我再献丑一回,开课讲解喽,来鼓掌来顶
我嚓,顶楼不能编辑的,只好放在楼中间了。这也好,给肯挨页翻帖子的朋友一个惊喜。
  1. //=====================================================================================================
  2. // AHRS.c
  3. // S.O.H. Madgwick
  4. // 25th August 2010
  5. //=====================================================================================================
  6. // Description:
  7. //
  8. // Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
  9. // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
  10. // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
  11. // axis only.
  12. //
  13. // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
  14. //
  15. // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
  16. // orientation.  See my report for an overview of the use of quaternions in this application.
  17. //
  18. // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
  19. // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
  20. // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
  21. //
  22. //=====================================================================================================

  23. //----------------------------------------------------------------------------------------------------
  24. // Header files

  25. #include "AHRS.h"
  26. #include <math.h>

  27. //----------------------------------------------------------------------------------------------------
  28. // Definitions

  29. #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  30. #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
  31. #define halfT 0.5f                // half the sample period

  32. //---------------------------------------------------------------------------------------------------
  33. // Variable definitions

  34. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  35. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

  36. //====================================================================================================
  37. // Function
  38. //====================================================================================================

  39. void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  40.         float norm;
  41.         float hx, hy, hz, bx, bz;
  42.         float vx, vy, vz, wx, wy, wz;
  43.         float ex, ey, ez;

  44.         // auxiliary variables to reduce number of repeated operations
  45.         float q0q0 = q0*q0;
  46.         float q0q1 = q0*q1;
  47.         float q0q2 = q0*q2;
  48.         float q0q3 = q0*q3;
  49.         float q1q1 = q1*q1;
  50.         float q1q2 = q1*q2;
  51.         float q1q3 = q1*q3;
  52.         float q2q2 = q2*q2;   
  53.         float q2q3 = q2*q3;
  54.         float q3q3 = q3*q3;         
  55.         
  56.         // normalise the measurements
  57.         norm = sqrt(ax*ax + ay*ay + az*az);      
  58.         ax = ax / norm;
  59.         ay = ay / norm;
  60.         az = az / norm;
  61.         norm = sqrt(mx*mx + my*my + mz*mz);         
  62.         mx = mx / norm;
  63.         my = my / norm;
  64.         mz = mz / norm;         
  65.         
  66. 从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz(测量值)
  67.         // compute reference direction of flux
  68.         hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  69.         hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  70.         hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         

  71. 计算地理坐标系下的磁场矢量bxyz(参考值)。
  72. 因为地理地磁水平夹角,我们已知是0度(抛去磁偏角的因素,固定向北),所以by=0,bx=某值
  73. 但地理参考地磁矢量在垂直面上也有分量bz,地球上每个地方都是不一样的。
  74. 我们无法得知,也就无法用来融合(有更适合做垂直方向修正融合的加速度计),所以直接从测量值hz上复制过来,bz=hz。
  75. 磁场水平分量,参考值和测量值的大小应该是一致的(bx*bx) + (by*by)) = ((hx*hx) + (hy*hy))。
  76. 因为by=0,所以就简化成(bx*bx)  = ((hx*hx) + (hy*hy))。可算出bx。
  77.         bx = sqrt((hx*hx) + (hy*hy));
  78.         bz = hz;        
  79.    
  80.         // estimated direction of gravity and flux (v and w)
  81.         vx = 2*(q1q3 - q0q2);
  82.         vy = 2*(q0q1 + q2q3);
  83.         vz = q0q0 - q1q1 - q2q2 + q3q3;

  84. 我们把地理坐标系上的磁场矢量bxyz,转到机体上来wxyz。
  85. 因为by=0,所以所有涉及到by的部分都被省略了。
  86. 类似上面重力vxyz的推算,因为重力g的gz=1,gx=gy=0,所以上面涉及到gxgy的部分也被省略了
  87. 你可以看看两个公式:wxyz的公式,把bx换成gx(0),把bz换成gz(1),就变成了vxyz的公式了(其中q0q0+q1q1+q2q2+q3q3=1)。
  88.         wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  89.         wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  90.         wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  91.       
  92. 现在把加速度的测量矢量和参考矢量做叉积,把磁场的测量矢量和参考矢量也做叉积。都拿来来修正陀螺。
  93.         // error is sum of cross product between reference direction of fields and direction measured by sensors
  94.         ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  95.         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  96.         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
  97.         
  98.         // integral error scaled integral gain
  99.         exInt = exInt + ex*Ki;
  100.         eyInt = eyInt + ey*Ki;
  101.         ezInt = ezInt + ez*Ki;
  102.         
  103.         // adjusted gyroscope measurements
  104.         gx = gx + Kp*ex + exInt;
  105.         gy = gy + Kp*ey + eyInt;
  106.         gz = gz + Kp*ez + ezInt;
  107.         
  108.         // integrate quaternion rate and normalise
  109.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  110.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  111.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  112.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  113.         
  114.         // normalise quaternion
  115.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  116.         q0 = q0 / norm;
  117.         q1 = q1 / norm;
  118.         q2 = q2 / norm;
  119.         q3 = q3 / norm;
  120. }

  121. //====================================================================================================
  122. // END OF CODE
  123. //====================================================================================================
复制代码
发表于 2013-3-22 08:38:23 | 显示全部楼层
seanwood 发表于 2013-3-21 09:56
有人问加电子罗盘的融合是怎么回事,我再献丑一回,开课讲解喽,来鼓掌来顶
我嚓,顶楼不能编 ...

请问楼主,磁阻传感器测量值mxyz乘以旋转矩阵转换到地理坐标系变成hxyz,该矢量的方向应该也是东北天,如果说地磁水平分量正北,那么为什么不用bx=hx,而是用bx*bx=hx*hy,是考虑到转换矩阵中四元数的误差?
发表于 2013-3-22 09:51:30 | 显示全部楼层
楼主的帖子解析得很明白,让我弄懂了很多东西

但还有一个疑问,单用加速度计和地磁传感器,在静态的情况下,能否准确计算出完整的欧拉角?

yaw需要通过地磁传感器来计算,但地磁本身对应磁平面的映射,也需要pitch和roll

我看了一些文献,提到单用加速度计和地磁传感器,当pitch和roll过大的时候(大于40度)的时候,地磁的计算就不准确了?
 楼主| 发表于 2013-3-22 10:04:02 | 显示全部楼层
黑眼豆豆 发表于 2013-3-22 08:38
请问楼主,磁阻传感器测量值mxyz乘以旋转矩阵转换到地理坐标系变成hxyz,该矢量的方向应该也是东北天,如 ...

你为啥要用电子罗盘呢?不就是为了修正四元数被陀螺积分过程中产生的误差嘛。hxyz和bxyz本来应该是完全相等,但是hxyz被四元数转到地理坐标系的时候因为四元数有误差,所以他们的误差,就是四元数的误差。
 楼主| 发表于 2013-3-22 10:08:00 | 显示全部楼层
STM32_Study 发表于 2013-3-22 09:51
楼主的帖子解析得很明白,让我弄懂了很多东西

但还有一个疑问,单用加速度计和地磁传感器,在静态的情况下 ...

如roll/pitch不是水平,地磁当然不能只靠地磁的xy来算。
但是谁让你先算地磁啊?先用加速度计算出roll和pitch,然后再用这个roll/pitch去和地磁三轴组合算出yaw,不就没误差了吗?
发表于 2013-3-22 10:13:30 | 显示全部楼层
搞下来研究研究
发表于 2013-3-22 11:59:41 | 显示全部楼层
太牛了,后悔没学好数学啊
发表于 2013-3-22 12:47:05 | 显示全部楼层
强贴留名!
发表于 2013-3-22 14:33:45 | 显示全部楼层
seanwood 发表于 2013-3-22 10:04
你为啥要用电子罗盘呢?不就是为了修正四元数被陀螺积分过程中产生的误差嘛。hxyz和bxyz本来应该是完全相 ...

不好意思,楼主不要怪我不求甚解啊!既然旋转矩阵有误差那么hxy就不一定是水平的,那么求参考地磁水平分量就不见得是(bx*bx)  = ((hx*hx) + (hy*hy))?
发表于 2013-3-22 19:02:19 | 显示全部楼层
LZ讲得不错
虽然有些还没弄懂但还是支持一下
发表于 2013-3-23 22:01:03 | 显示全部楼层
seanwood 发表于 2013-3-21 09:56
有人问加电子罗盘的融合是怎么回事,我再献丑一回,开课讲解喽,来鼓掌来顶
我嚓,顶楼不能编 ...

LZ这P值和I值必须通过实际测试得到吗??
不太会找P值,I值  
我把P值放到2或3的时候,静置陀螺仪,显示出来的姿态定位有明显的偏差,而且会不停的晃动
如果再调大P值  就晃动的更厉害了
除非把两个值都往小了调  姿态能像点样子   但这样就是只有陀螺仪的效果了  妥妥的错了
求问lz我这是什么问题   是磁阻传感器没调整好吗?
发表于 2013-3-26 20:54:33 | 显示全部楼层
开始看不懂,现在刚有点入门了,确实是刚体运动通俗易懂的讲解,楼主牛叉
发表于 2013-3-27 01:52:55 | 显示全部楼层
楼主的这个精神非常的值得大家学习。。这篇文章绝对是精品中的精品。
发表于 2013-3-27 22:11:45 | 显示全部楼层
的确呀!白莲好贴
发表于 2013-3-28 00:38:06 | 显示全部楼层
真心膜拜楼主的理解能力和讲解能力。启蒙贴啊
发表于 2013-3-29 11:34:52 | 显示全部楼层
一直在学习 还未能超越 谢谢
发表于 2013-3-29 13:12:09 | 显示全部楼层
问下下面这个函数这两段怎么理解
// 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
       
        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

其他的都懂了,就这个还没想通


我搞了个FPV专用飞控 全传感器+osd+GPS
最近再搞个433遥控,可以回发数据
以前在模型论坛混,玩技术的不多,不深,现在改这里吧
说实话这里四轴发展太慢了,现在都GPS自主飞行了,坛子里好像没人搞,去搞微型四轴

有兴趣联系我吧,我的资料程序PCB全开源


试飞的视频在下面
hXttp://v.youku.com/v_show/id_XNTMyOTk5NjM2.hXtm




void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;         
       
        // 测量正常化
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;      
       
        // 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
       
        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);
       
        // 积分误差比例积分增益
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;
       
        // 调整后的陀螺仪测量
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;
       
        // 整合四元数率和正常化
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
       
        // 正常化四元
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
}
发表于 2013-3-29 23:57:54 | 显示全部楼层
这段代码没有懂??

从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz(测量值)
        // compute reference direction of flux
        hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
        hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
        hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
发表于 2013-3-30 00:59:29 | 显示全部楼层
xukkkkkk 发表于 2013-3-29 13:12
问下下面这个函数这两段怎么理解
// 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);

我的理解

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
发表于 2013-3-30 07:15:12 | 显示全部楼层
真是好文章,超级喜欢这个帖子,感谢楼主~!!!!
发表于 2013-4-1 21:48:10 | 显示全部楼层
这几天由仔细看了看相关的资料,有点入门了
再次来谢谢LZ
发表于 2013-4-3 11:26:20 | 显示全部楼层
mark 一下正在学习中
发表于 2013-4-3 14:37:36 | 显示全部楼层
先标记了 回去慢慢看
发表于 2013-4-3 20:34:55 | 显示全部楼层
很不错的东西,学习了、、、、
发表于 2013-4-3 20:39:53 | 显示全部楼层
xukkkkkk 发表于 2013-3-29 13:12
问下下面这个函数这两段怎么理解
// 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);

这位仁兄的视频的确不错,怎么联系你呢?
发表于 2013-4-5 22:52:57 | 显示全部楼层
很好的帖子啊
发表于 2013-4-6 00:52:38 | 显示全部楼层
很详细,收藏了。
发表于 2013-4-6 11:31:07 | 显示全部楼层
学习,mark
发表于 2013-4-8 12:57:08 来自手机 | 显示全部楼层
好帖!慢慢研究
发表于 2013-4-8 15:49:19 | 显示全部楼层
mark~学习中》》》
发表于 2013-4-8 20:48:16 | 显示全部楼层
楼主,搞了多就到现在这个水平,我数学不好,搞了1年
发表于 2013-4-9 08:58:49 | 显示全部楼层
john800422 发表于 2013-3-30 00:59
我的理解

你的截图是那本书上或者资料上的
发表于 2013-4-9 11:35:20 | 显示全部楼层
xukkkkkk 发表于 2013-4-9 08:58
你的截图是那本书上或者资料上的

自己想的
自己寫的
发表于 2013-4-9 20:21:26 | 显示全部楼层
mark 卡尔曼算法
发表于 2013-4-9 22:10:34 | 显示全部楼层
多谢楼主分享
发表于 2013-4-10 10:45:52 | 显示全部楼层
学习了。。。
发表于 2013-4-14 22:08:52 | 显示全部楼层
john800422 发表于 2013-3-30 00:59
我的理解

老兄,你的文章在那里找的,可以传上来看看不?
发表于 2013-4-14 22:53:32 | 显示全部楼层
luozhongchao123 发表于 2013-4-14 22:08
老兄,你的文章在那里找的,可以传上来看看不?

自己想的
自己寫的
发表于 2013-4-19 09:33:57 | 显示全部楼层
楼主辛苦了,感谢楼主,正需要这个,虽然有点看不懂,慢慢看!
发表于 2013-4-21 11:50:36 | 显示全部楼层
我也想了解一下国外的四轴!楼主可否告诉我以上代码的出处?
发表于 2013-4-25 17:05:06 | 显示全部楼层
mark,thanks.
发表于 2013-4-25 22:41:20 | 显示全部楼层
真心谢谢楼主 很清晰的知识脉络 谢谢分享
发表于 2013-4-25 22:56:18 | 显示全部楼层
学习学习
发表于 2013-4-27 08:54:40 | 显示全部楼层
标记,学习,LZ辛苦了
发表于 2013-4-27 21:38:56 | 显示全部楼层
mark
发表于 2013-4-27 21:52:50 | 显示全部楼层
好贴子,留着看,谢谢楼主
发表于 2013-4-28 03:27:59 | 显示全部楼层
发表于 2013-4-28 12:00:15 | 显示全部楼层
请教前辈一个问题,   前辈帖子里写更新四元数的方法是用一阶龙库法,  但是我看其他有些讲四元数的文章里又提到四元数乘法来更新,这两者有什么区别呢?
发表于 2013-4-28 16:56:21 | 显示全部楼层
陀螺倒是没用过,过去一直用磁通门+石英加速度计做电子罗盘的计算,目的是将倾斜了的三个磁通门器件算回大地坐标系,由于这些传感器都是单轴的,安装时有正交度误差以及磁坐标轴与重力坐标轴平行度问题,带来的误差也不小,这个又怎么去解决呢?
发表于 2013-4-28 20:09:40 | 显示全部楼层
MARK上先
发表于 2013-4-29 17:37:18 | 显示全部楼层
顶一下楼主,慢慢来看~
发表于 2013-4-29 18:08:01 | 显示全部楼层
牛人,mark~
发表于 2013-4-30 19:28:19 | 显示全部楼层
这个帖子据说是精华,留个名先。
发表于 2013-4-30 22:48:50 | 显示全部楼层
好  铁    !!! 楼主辛苦了!! 好人一生平安!!!
发表于 2013-4-30 23:07:21 来自手机 | 显示全部楼层
总结得非常好马克先
发表于 2013-4-30 23:23:27 | 显示全部楼层
xukkkkkk 发表于 2013-3-29 13:12
问下下面这个函数这两段怎么理解
// 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);

您好,请问下 您的算法也是用四元素么? 还是用MK的 那种方式?
看了您的视频, 佩服。
我是学生,最近入门四轴, 参考前人的资料在微型四轴能飞,P但ID响应不行,老是荡。现在不知道是数据融合的问题还是
PID的控制太过于简略。想把算法调稳定了再做大四轴。
能否参考下您的 算法。 万分感谢!!
发表于 2013-5-1 13:11:22 | 显示全部楼层
好久没有看到到这样的帖子了,非常高兴,先拿下来看看,要是以后有什么值得分享的东西我也会像这样也大家讨论。
发表于 2013-5-1 14:57:36 | 显示全部楼层
好帖子,顶!
发表于 2013-5-1 17:13:18 | 显示全部楼层
楼主是高手,向楼主学习
发表于 2013-5-1 17:40:07 | 显示全部楼层
公式最左边的上面带点的三个是本次更新后的角度,不带点的是上个更新周期算出来的角度。

这句话里好像有点表达错误,但是我也不确定,带点应该是本次更新后 角度的增量
发表于 2013-5-2 09:09:19 | 显示全部楼层
mark................
发表于 2013-5-3 19:14:34 | 显示全部楼层
这个算法心得写得很好,谢谢楼主的分享。
发表于 2013-5-3 21:07:08 | 显示全部楼层
标记一下
发表于 2013-5-4 08:40:01 | 显示全部楼层
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

这个函数的参数
gx gy gz, 是陀螺仪直接读出来的数据,还是转换成了实际角速度数据?
ax ay az,  是加速度计直接读出来3个轴的数据吗?

初次接触四轴,希望大神点拨~~
发表于 2013-5-5 19:06:56 | 显示全部楼层
这个帖子,真心不错,谢谢楼主!
发表于 2013-5-6 15:23:33 | 显示全部楼层
本帖最后由 小笨蛋 于 2013-5-6 16:23 编辑

恩,写的真好!我认认真真的打印出来看了几篇,并试图推导。
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);
我简单的算了一下,发现ex ey ez好像就是sin(角度偏差),在角度偏差比较小的情况下就应该等于(角度偏差)。、、、、请问这一句对不对?
然后乘以Kp来修正陀螺仪偏差。

还有我见过另一种算法是把欧拉角转换成四元数,在利用四元数相乘来更新四元数。不知道和你这种方式本质上是不是一样?我试图证明、、、未果。

我用matlab做四元数旋转的实验,结果单旋转Pitch轴的话会出现奇怪的现象,我单片机里面也出现相同的现象,不知道为什么。
  1. % clc;
  2. % clear all;
  3. % close all;

  4. q=[1,0,0,0];
  5. euler3=[0,02*pi/180,0];
  6. a=[];
  7. euan=[];

  8. for i=1:1:360
  9.     p=euler2quat(euler3);
  10.     q=quatmultiply(p,q);
  11.     quatnormalize(q);
  12.     a=[a;q];
  13.     e=quat2euler(q);
  14.     euan=[euan;e];
  15. end

  16. hold;
  17. plot(a(:,1),'r','LineWidth',2);
  18. plot(a(:,2),'g','LineWidth',2);
  19. plot(a(:,3),'b','LineWidth',2);
  20. plot(a(:,4),'c','LineWidth',2);
  21. plot(euan(:,1),'m','LineWidth',2);
  22. plot(euan(:,2),'y','LineWidth',2);
  23. plot(euan(:,3),'k','LineWidth',2);
复制代码

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
发表于 2013-5-6 21:11:21 | 显示全部楼层

很牛叉的样子,学习了
发表于 2013-5-6 21:31:27 | 显示全部楼层
不错的说。。。
发表于 2013-5-6 22:48:19 | 显示全部楼层
lologame 发表于 2013-5-4 08:40
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

这个函数的参数

陀螺的部分是實際的角速度(rad/sec)
发表于 2013-5-7 08:11:55 | 显示全部楼层
g921002 发表于 2013-5-6 22:48
陀螺的部分是實際的角速度(rad/sec)

嗯,我已成功验证,谢谢你的热心回复
发表于 2013-5-8 09:20:11 | 显示全部楼层
mark 四轴 算法
发表于 2013-5-8 15:00:08 | 显示全部楼层
收藏,以后慢慢研究
友情提示:标题不合格、重复发帖,将会被封锁ID。详情请参考:论坛通告:封锁ID、获得注册邀请码、恢复被封ID、投诉必读
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|阿莫电子论坛(原ourAVR/ourDEV) ( 粤ICP备09047143号 )

GMT+8, 2017-11-24 11:39

阿莫电子论坛, 原"中国电子开发网"

© 2004-2016 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表