搜索
bottom↓
回复: 4

看到一个无法理解的姿态解算算法,请教一下大家

[复制链接]
(32484347)

出0入0汤圆

发表于 2020-4-1 09:34:04 | 显示全部楼层 |阅读模式
            看到一个姿态解算算法无法理解,贴上来大伙给分析分析。

        for ( int i = 0 ; i < 3 ; i++)
        {
                deltaGyroAngle[i] = (gyro[i]) * looptime;
        }

        GEstG[2] = GEstG[2] - (deltaGyroAngle[0])  * GEstG[0];
        GEstG[0] = (deltaGyroAngle[0]) * GEstG[2]  + GEstG[0];


        GEstG[1] =  GEstG[1] + (deltaGyroAngle[1]) * GEstG[2];
        GEstG[2] = -(deltaGyroAngle[1]) * GEstG[1] + GEstG[2];


        GEstG[0] = GEstG[0] - (deltaGyroAngle[2])  * GEstG[1];
        GEstG[1] = (deltaGyroAngle[2]) * GEstG[0]  + GEstG[1];

       其中 :  gyro[i] 为各个轴向的角速度 , looptime 为单个计算周期实际时间间隔 。 GEstG[i] 为姿态角(单位为g)

       疑惑 :(1) 这个是什么算法,欧拉角?  那是不是计算顺序固定不能更改?
               (2) (deltaGyroAngle[0])  * GEstG[0]   这个角度直接乘以加速度,得到的结果还是加速度?     

       数学没学好啊,一脸懵逼
              
(32472785)

出0入0汤圆

发表于 2020-4-1 12:46:46 | 显示全部楼层
完整代码在这里,一起看看
https://github.com/silver13/Bold ... ilverware/src/imu.c
(32471898)

出0入0汤圆

发表于 2020-4-1 13:01:33 | 显示全部楼层
好像是罗德里格旋转公式,加小角度优化掉三角函数
(32470493)

出0入0汤圆

 楼主| 发表于 2020-4-1 13:24:58 | 显示全部楼层
qwe2231695 发表于 2020-4-1 13:01
好像是罗德里格旋转公式,加小角度优化掉三角函数

  是的, 就是这个飞控。  
  这个姿态解算算法,真心看不太懂
(32466909)

出0入0汤圆

发表于 2020-4-1 14:24:42 | 显示全部楼层
本帖最后由 lcw_swust 于 2020-4-1 14:26 编辑

我也没看懂,不过我的姿态解算是用的向量旋转公式.
推导在这帖子的32楼,不知道和楼主这个是否一样.
https://www.amobbs.com/thread-5614575-1-1.html
姿态处理
采用二维角度旋转公式
x'=x*cosθ-y*sinθ
y'=x*sinθ+y*cosθ
应用到三维,且由于角度很小时sinX=0,cosX=1,对公式加以简化,得到:
        GAX+=(Gz*GAY-Gy*GAZ)/SINDIV;
        GAY+=(Gx*GAZ-Gz*GAX)/SINDIV;
        GAZ+=(Gy*GAX-Gx*GAY)/SINDIV;
  (Gx\Gy\Gz为陀螺仪数据,GAX\GAY\GAZ为姿态向量,和加速度差不多,可由atan2得到姿态角.)
(SINDIV这个系数需要实际测试一下)
然后与加速度向量进行互补滤波融合。
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子论坛 ( 公安交互式论坛备案:44190002001997 粤ICP备09047143号 )

GMT+8, 2021-4-12 08:59

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

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