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

dongfang

找到了58楼代码的出处了，代码还在更新中，现在已经支持9dof了，

作者的主页在：
http://code.google.com/p/imumargalgorithm30042010sohm/
http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms

c代码在此：http://www.x-io.co.uk/res/sw/madgwick_algorithm_c.zip

姿态解算这么高深的问题，有看得懂的代码，感觉开源真是太了不起了。

峰回路转

原帖由 dongfang 于 2012-1-28 10:51 发表


你好，Quaternion dQ，也就是a=a*b中的那个b，是自上一个姿态计算后，发生旋转(增量delta)的角度，可以从角速度读数乘以增量时间得到dt（这里的dt不是时间，是增量角度），再转换成四元数得到。

你的那段带AC ...

呵呵。那我就明白了。关于四元的公式用这个就可以了吧：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;
}
这个函数输入gyrox y z输出wxyz也就是q0 q1 q2 q3 对四元的概念还是有些模糊，看来得好好看看角度，弧度，欧拉，四元之间的关系。

峰回路转

原帖由 dongfang 于 2012-1-28 12:37 发表
找到了58楼代码的出处了，代码还在更新中，现在已经支持9dof了，

作者的主页在：
http://code.google.com/p/imumargalgorithm30042010sohm/
http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms ...

这个代码楼主有没有测试过，他融合了三轴加速度和三轴磁场，期待详解。。。。

dongfang

奇怪，我刚看到了h.e.u.y.c.k的回复，刚想说两句，突然不见了，是被我自己删了么？

tedeum

顶起 楼主是偶像

xaut

做一个46656000行的查表法如何？:em15:

heuyck

原帖由 dongfang 于 2012-1-28 22:34 发表
奇怪，我刚看到了h.e.u.y.c.k的回复，刚想说两句，突然不见了，是被我自己删了么？

不想发帖了，免得有人jjww。最近讲啥都有人跟。
你看过回帖了解就好了

oppop

原帖由 dongfang 于 2012-1-28 10:52 发表

不是很明白线加速度对角速度的影响指的是什么，我的代码没有集成acc是事实。

由于MEMS陀螺的固有特性，每个坐标上的角速度输出值会受到线加速度的影响，越便宜的陀螺仪这种影响越明显，即，Linear Acceleration Effect on Bias。
换句话说，低端的陀螺仪对陀螺本身的振动非常敏感。高端的陀螺仪贵的原因一方面是固有噪声低和零漂稳定性低，另一方面则是对加速度的敏感度低。因为高端的陀螺仪每个轴向上都会做两对相同的结构来构成差分的形式，来消除振动带来的影响。这样做的话，成本自然就高了。
一般的、起步比较晚的MEMS陀螺仪生产厂商，一般是不会提Linear Acceleration Effect on Bias这个指标的，因为他们这个指标都很差。但是在实际运用中，振动是不可避免的，振动带来的误差往往占很大比例。这就是为什么有些飞控的内部的IMU要加减振垫子的原因。

dongfang

原帖由 oppop 于 2012-1-29 18:45 发表


由于MEMS陀螺的固有特性，每个坐标上的角速度输出值会受到线加速度的影响，越便宜的陀螺仪这种影响越明显，即，Linear Acceleration Effect on Bias。
换句话说，低端的陀螺仪对陀螺本身的振动非常敏感。高端的 ...

首次听说，不知道实际影响有多大，好像感觉不出来么。

dongfang

原帖由 (涉嫌广告已屏蔽) 于 2012-1-29 09:22 发表

不想发帖了，免得有人jjww。最近讲啥都有人跟。
你看过回帖了解就好了

唉，人怕出名猪怕胖，就讨论些技术算了，别扯那些是非了。算法上荒废很久了，已经遇到了AVR的运算极限，STM32也没搞定（本身就是新学，IIC没搞定就已经损失了一个测试板）。

最近各种分心的事很多，加上在研究四轴，感觉精力不济。
你要是有时间的话，我建议你的飞控搞一个飞行测试模式，就是依次测试：
测试SETUP: 测试在100米左右安全微风空域进行
TEST 1: 测试油门到达指定速度SPEED的理想PID参数，参数能和SPEED成函数关系最好。我在xplan的测试中已经发现I*I积分是和SPEED直接关系的。
TEST 2: 测试各SPEED下保持平衡飞行的ROLL的理想PID参数。
TEST 3: 测试各SPEED下保持高度、平衡飞行的PITCH的理想PID参数。
TEST 4: 测试各SPEED下保持高度、平衡飞行，仅使用方向舵转弯，各个转弯力度飞行的方向舵的理想PID参数。
TEST 5: 测试各SPEED下保持高度、平衡飞行，组合使用方向舵和副翼转弯，各个转弯力度飞行的方向舵、副翼、升降舵的理想PID参数。
测试错误控制：一旦飞机失控，高度降低20米以上，立刻使用FAILSAFE参数，救回。

我在xplan上完成过一部分测试，也发现了一些有趣的规律，只是程序写的很烂，每次测试都是直接修改了前一次的测试代码，所以没留下来什么能共享的。

峰回路转

呵呵。各位都是高手，最近假期较长，想好好研究下，平台用的是fy20的平衡仪，上面有stm32f103brt+adxl335+ly530+lpr530a。需要的东西都全了。。把原来的mcu拆了保存好。万一以后想玩平衡仪了还可以装回去，装上一片新的。加装jtag（swd）接口。这个本身串口是引出的。加装hp03气压传感器。stm32没有eeprom，装了一片24c02.试过吧kk_c移植到这个平台上，测试可以工作。但是加速度计等都没用上，这次乘着长假玩玩。。。

[ 本帖最后由 峰回路转 于 2012-1-29 23:03 编辑 ]

编程飞行

不懂:em22: 帮顶

峰回路转

关于欧拉角，gyro输出值积分得到的就是欧拉角么？另外计算时输入的gyro的单位是mv/度/s ? acc的是mv/g
也就是说用gyro积分计算角度偏移量 gyrox_dt=gyrox*dt
用acc计算当前欧拉角 Roll= atan2(ax,az);
                Pitch= atan2(-ay,az);

[ 本帖最后由 峰回路转 于 2012-1-30 03:45 编辑 ]

dongfang

原帖由 峰回路转 于 2012-1-30 02:35 发表
关于欧拉角，gyro输出值积分得到的就是欧拉角么？另外计算时输入的gyro的单位是mv/度/s ? acc的是mv/g
也就是说用gyro积分计算角度偏移量 gyrox_dt=gyrox*dt
用acc计算当前欧拉角 Roll= atan2(ax,az);
                Pitch=  ...

gyro输出值*间隔时间=欧拉角。gyro的单位要查芯片手册，数字芯片不用mv。

heuyck

原帖由 dongfang 于 2012-1-29 21:34 发表

唉，人怕出名猪怕胖，就讨论些技术算了，别扯那些是非了。算法上荒废很久了，已经遇到了AVR的运算极限，STM32也没搞定（本身就是新学，IIC没搞定就已经损失了一个测试板）。

最近各种分心的事很多，加上在研究四 ...

最近研究XPLANE、APM SIM还有MAVLINK，得把空速管什么的数据都弄出来才好仿真。
xplane可是个好东西啊，可惜只有一架PT60。。。找不到其他模型了，不知道你用什么模型模拟的？
APM功能很强大，正在研究，不过是C++写的，看得我一头雾水。最近又有新版APM2出来，好像只卖200刀，也可以研究一下。
MAVLINK也是个好东西，支持MAVLINK的GCS一大堆，而且有免费库可以用，还是C代码的。

heuyck

原帖由 峰回路转 于 2012-1-30 02:35 发表
关于欧拉角，gyro输出值积分得到的就是欧拉角么？另外计算时输入的gyro的单位是mv/度/s ? acc的是mv/g
也就是说用gyro积分计算角度偏移量 gyrox_dt=gyrox*dt
用acc计算当前欧拉角 Roll= atan2(ax,az);
                Pitch=  ...

那个积分不是平时意义上的机体欧拉角，那个是在dt微分时间内的转动欧拉角，其实也不能算是欧拉角，因为欧拉角有严格的转动顺序。只是这里dt微分下，转动顺序的区别很小了。
东方拿它来转成增量四元数，然后用四元数转动的方式去更新原有四元数，得到了新的四元数。

在你贴的那个算法里，gxyz就是就是输入的三轴陀螺的角速率，带四元数q0123更新方程就是一阶龙库算法，增量法一样的。
一阶就是指考虑dt时间内，gxyz的角速率是一直不变的，二三四阶就是再细分这dt时间内的三维转动，我用一阶觉得已经够用了。
那个算法里的halfT就是dt/2，单位是秒，比如我的计算频率为50hz，dt即为20ms，那么这个算法里的halfT就是0.01秒。
gxyz的单位应该是弧度/秒，陀螺的mv转成弧度/秒的比例，可在芯片手册中查得。
这个算法中用acc校准gyro用的是向量叉积的办法，我也是用叉积的。
计算机体坐标系下，四元数的重力和加速度计的重力的向量叉积，算出来的向量正好是机体坐标系中三维xyz的夹角的cos值，也就是叉积。
在误差90度以内，机体坐标系下xyz误差夹角和叉积成正比例关系，正好用来加到陀螺上做PI误差校准。
加速度计的重力向量要砍掉向心加速度，另外与标准重力做个比较，超过一定范围不对四元数做校准。

另外acc按你的算法应该是平面夹角，与机体欧拉角不同，我是这么算欧拉角，用来初始化四元数的，xy可能跟你不一样。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);

heuyck

acc计算欧拉角，四元数重力向量，这几个结合余弦矩阵（转置矩阵）和欧拉角的三次转动方程推算过程，会比较清楚。

oppop

原帖由 dongfang 于 2012-1-29 20:58 发表

首次听说，不知道实际影响有多大，好像感觉不出来么。

这个是振动所产生的加速度的计算公式。振动起来，所产生的加速度是很恐怖的，自己可以算算。
1khz、1um的振动可以产生39.5g的加速度。然后拿这个加速度去乘以Linear Acceleration Effect on Bias可以得到由于振动所产生额外的角速度偏移量，越是低端的陀螺仪，这个值越大。
Acceleration = 4 * p² * ƒ² * displacement

surebo

强贴留名，收藏起来

峰回路转

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

那个积分不是平时意义上的机体欧拉角，那个是在dt微分时间内的转动欧拉角，其实也不能算是欧拉角，因为欧拉角有严格的转动顺序。只是这里dt微分下，转动顺序的区别很小了。
东方拿它来转成增量四元数，然后用四元 ...

受教了。。谢谢。
我搞这个完全处于兴趣使然，无奈上学太少很多东西搞不太明白，只好多多请教学习。就当是万里长征了，呵呵
从从开始，首先获得初始状态的四元，先从acc获得初始的欧拉角，测试了一下用Roll= atan2(ax,az); Pitch= atan2(-ay,az);与您的公式imu.euler.x = atan2(imu.accel.y, imu.accel.z); imu.euler.y = -asin(imu.accel.x / ACCEL_1G);结果是相同的，
当x轴旋转90度时，acc_x=1g acc_y=0g acc_z=0g 此时用上面的公式计算出的欧拉角为1.57，这个值是什么单位或者是什么意义呢？

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