|
原帖由 峰回路转 于 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); |
|