毕卡求解法中的角增量

lologame

看了下用毕卡求解法，更新姿态四元数，其他都明白了，但是有个地方没有明白，就是所谓的角增量与角速度。

陀螺仪读出来的数据不是直接就是角速度了吗，为什么书上说陀螺仪出来的是角增量呢，

按照我的理解

角增量  约=  角速度 * 采样时间T，  

这是什么意思，希望有大神能够指点一二~、、

---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

忽略那个问题，顺便分享一下将IMUupdate的四元数更新微分方程改为一、二、三、四阶毕卡求解法的代码：

直接将四元数更新方程，进行替代就可以了

注：
1.IMUupdate函数的四元数姿态更新方程 实质上就是毕卡一阶近似算法
2.下面的q0_last,  q1_last,  q2_last,  q3_last,  是四元数缓冲，因为我发现IMUupdate里面貌似忽略了这个问题，
比如最开始q0，进行更新了，然后开始更新q1，但是更新q1是用的更新后的q0，但是根据方程，更新四元数的时候,都是用的更新前的四元数

一阶近似算法
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;

二阶近似算法
delta2 = (gx*gx + gy*gy + gz*gz)*T*T;

q0 = q0_last*(1-delta2/8) + (-q1_last*gx - q2_last*gy - q3_last*gz)*halfT;
q1 = q1_last*(1-delta2/8) + (q0_last*gx + q2_last*gz - q3_last*gy)*halfT;
q2 = q2_last*(1-delta2/8) + (q0_last*gy - q1_last*gz + q3_last*gx)*halfT;
q3 = q3_last*(1-delta2/8) + (q0_last*gz + q1_last*gy - q2_last*gx)*halfT;

三阶近似算法
delta2 = (gx*gx + gy*gy + gz*gz)*T*T;

q0 = q0_last*(1-delta2/8) + (-q1_last*gx - q2_last*gy - q3_last*gz)*T*(0.5 - delta2/48);
q1 = q1_last*(1-delta2/8) + (q0_last*gx + q2_last*gz - q3_last*gy)*T*(0.5 - delta2/48);
q2 = q2_last*(1-delta2/8) + (q0_last*gy - q1_last*gz + q3_last*gx)*T*(0.5 - delta2/48);
q3 = q3_last*(1-delta2/8) + (q0_last*gz + q1_last*gy - q2_last*gx)*T*(0.5 - delta2/48);

四阶近似算法
delta2 = (gx*gx + gy*gy + gz*gz)*T*T;

q0 = q0_last*(1 - delta2/8 + delta2*delta2/384) + (-q1_last*gx - q2_last*gy - q3_last*gz)*T*(0.5 - delta2/48);
q1 = q1_last*(1 - delta2/8 + delta2*delta2/384) + (q0_last*gx + q2_last*gz - q3_last*gy)*T*(0.5 - delta2/48);
q2 = q2_last*(1 - delta2/8 + delta2*delta2/384) + (q0_last*gy - q1_last*gz + q3_last*gx)*T*(0.5 - delta2/48);
q3 = q3_last*(1 - delta2/8 + delta2*delta2/384) + (q0_last*gz + q1_last*gy - q2_last*gx)*T*(0.5 - delta2/48);

附上理论公式的图片

kmani

你把速度对时间的积分看成位移的变化，这样好理解些。
我还不会这些，希望希望楼主多多帮助。

lologame

kmani 发表于 2013-5-30 09:24
你把速度对时间的积分看成位移的变化，这样好理解些。
我还不会这些，希望希望楼主多多帮助。 ...

速度对时间的积分就是位移的变化量，确实是这样，我也是这样理解的。

但是书上说 陀螺仪 直接输出的就是角增量，令我难以理解，

毕卡求解法，是已知采样周期的角增量，然后直接进行四元数更新

书上的意思是，直接取陀螺仪的输出（因为已经输出角增量了），然后带入方程，就OK了。

但是我们陀螺仪实际输出的是角速度，要得到角增量说白了，还是要积分，采样周期T小的时候，积分结果近似看为    角速度*采样周期T

由角速度得到角增量后，才能带入毕卡求解方程更新四元数、

直接输出角增量 另一个让人不能理解的地方是，如果就假设它输出的是角增量，那么这到底是哪段时间的角增量，这根本没办法确定、、、

kmani

这方面，我不太在行，搞不定~~~
难道是有的陀螺仪确实可以直接输出角增量？

lologame

kmani 发表于 2013-5-30 09:41
这方面，我不太在行，搞不定~~~
难道是有的陀螺仪确实可以直接输出角增量？ ...

作者05年写的，那个时候没有直接输出角速度的陀螺仪呀、、、？
呵呵

majianjia1991

MEMS陀螺仪，音叉陀螺仪，激光陀螺仪输出的是角速度。
机械陀螺仪，输出的是角增量

nnnkey

激光的一般是增量型

lologame

nnnkey 发表于 2013-5-30 09:48
激光的一般是增量型

也就是mems陀螺仪，我们常用的微机电是输出的角速度、
而其它类型的陀螺仪输出的是角增量吗？哦，这样的话就好理解了~~

不过还真心奇怪，输出角增量的时候，如何知道这是哪段时间和哪段时间的角增量~~呵呵，难道是两次采集之间的~~？

cchwhk

我是这样理解的：halfT就是姿态解算周期的一半，即对应公式9.2.48的二分之一。陀螺仪传感器输出的是角速度，在解算的时候乘以了halfT，即为角增量。