姿态解算比较
最近在研究姿态解算部分,之前实验室的四轴控制不涉及直接控制姿态角部分。心血来潮,研究过阿莫里面的四轴姿态算法,有个最流行的算法,做简单融合。如下所示://=====================================================================================================
// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' .Incorporates the magnetic distortion
// compensation algorithms from my filter which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.See my report for an overview of the use of quaternions in this application.
//
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
//
//=====================================================================================================
//----------------------------------------------------------------------------------------------------
// Header files
#include "AHRS.h"
#include <math.h>
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.5f // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
//====================================================================================================
// Function
//====================================================================================================
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// 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);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
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;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
本人用实际的飞行数据亲测了下,在matlab下做仿真。效果表明这个算法的误差还是有点大的,最大达到了10度了,而且好像调Kp、Ki对结果影响不大。希望大牛指导下这个是什么情况?这个算法只能做到这个效果吗?
融合用的什么方法? s15200380596 发表于 2014-3-11 20:28
融合用的什么方法?
就是上面这段代码~ 这个代码实际上很多人在用,匿名用的也是这个,不过没有用到对YAW的处理。只有俯仰和横滚部分,最终效果可以看到融合效果还是很好地。 longwu537 发表于 2014-3-11 22:18
这个代码实际上很多人在用,匿名用的也是这个,不过没有用到对YAW的处理。只有俯仰和横滚部分,最终效果可 ...
完整的代码是有对yaw的处理的吧,我这个主要是因为暂时没有地磁计的数据,所以省去了这个部分。总的来说,这个算法简单实用,能飞。但是效果肯定不好,我用ekf的算法,要求误差最大不能超过2度的。 还是什么算法? #define halfT 0.5f // half the sample period
有與取樣週期吻合嗎?
自己用過,啟動馬達有震動時,誤差至少小於 5 度,
沒有像你說的誤差這麼大,應該是你移植有問題。 john800422 发表于 2014-3-12 01:32
#define halfT 0.5f // half the sample period
有與取樣週期吻合嗎?
halfT就是采样周期的一半是吧~另外实际飞行的时候不仅仅是电机的影响,加速度计对一般震动和机械噪声特别敏感,所以这个数据也会有影响到后面的解算。我这个加计数据是以及处理过的了,另外也不是说一般用的时候就误差达到10度以上,图上面看到,我这个是在做大角度的连续飞行,是用来做实验,验证算法的,一般飞行不会这样。所以结果是小角度正常飞行是可以的。 hz770495569 发表于 2014-3-12 08:59
halfT就是采样周期的一半是吧~另外实际飞行的时候不仅仅是电机的影响,加速度计对一般震动和机械噪声特别 ...
helfT 是取樣週期一半沒錯,
若是實際飛行就另當別論了,
就算上述算法誤差為零,平衡沒調好也有可能出現誤差大的情況。
但"加速度會影響到解算結果"和"小角度正常飛行"是確實的,
因為上面算法並非全姿態,從 ex ey ez 可看出來 ( 不考慮磁力計 ),
當向量 a = ( 0 0 1 )^T 且 向量 v = ( 0 0 -1 )^T 時,誤差 e 依舊為 0,若有錯歡迎糾正。
若是要驗證解算算法,利用實際飛行會讓變因增加很多,前述的平衡效果就是其一,
最好是有一旋轉裝置可以準確快速旋轉到要求角度,此時"實際旋轉角度"與"解算姿態角度"比較,
才能直接反映出算法的好壞,至少就我認知是如此。 john800422 发表于 2014-3-12 13:16
helfT 是取樣週期一半沒錯,
若是實際飛行就另當別論了,
嗯,受教了。谢谢!halfT这个我没有多考虑,我的四轴运算频率是400hz。另外我这个取得数据就是实际四轴飞行的数据,飞行测试时,有意的在roll、pitch方向来回波动。还有,图中的真实的角度数据是飞机上搭载的IMU设备测到的。这个IMU价值人民币好几万块,因此性能是ok的,基本认为实际的角度就是这样的。 john800422 发表于 2014-3-12 13:16
helfT 是取樣週期一半沒錯,
若是實際飛行就另當別論了,
还有,我发现这个算法有点滞后,是跟我halfT有关吗? 有人跟我要这个算法的代码,现在放在这边。有问题大家请指教{:lol:} hz770495569 发表于 2014-3-12 15:08
还有,我发现这个算法有点滞后,是跟我halfT有关吗?
halfT 由取樣頻率決定,
若取樣頻率 400Hz 的話,取樣週期為 0.0025s,所以 halfT 為 0.00125s,
有滯后的問題可以試著增加取樣及解算的頻率。 john800422 发表于 2014-3-12 18:01
halfT 由取樣頻率決定,
若取樣頻率 400Hz 的話,取樣週期為 0.0025s,所以 halfT 為 0.00125s,
有滯后 ...
好的,谢谢~ lxl_lw 发表于 2014-3-12 00:30
还是什么算法?
好像是Mahony滤波算法~ 技术好帖子啊~!!!顶起~!!! john800422 发表于 2014-3-12 01:32
#define halfT 0.5f // half the sample period
有與取樣週期吻合嗎?
大哥,我采用IMUupdate这个算法已经得出很符合要求的Pitch和Roll,请问应该用什么方法得到准确的Yaw呢?
我试过用初始的Yaw+陀螺仪积分来算,但是会飘。用磁力计算有倾角时的Yaw动态时效果也很不好。
这个问题困扰我好久,还望楼主大哥解答
silence2455 发表于 2014-3-29 23:35
大哥,我采用IMUupdate这个算法已经得出很符合要求的Pitch和Roll,请问应该用什么方法得到准确的Yaw呢?
...
你好能否交流一下,我最近也在弄IMUupdate但问题多多,能否看一下你的代码 onev 发表于 2014-3-30 19:42
你好能否交流一下,我最近也在弄IMUupdate但问题多多,能否看一下你的代码 ...
行,给个邮箱我发给你 silence2455 发表于 2014-3-31 09:24
行,给个邮箱我发给你
onev2012@163.com谢谢啊 我也做过KP,KI数据调整实验,觉得不错,角度稳定在0.01度以内。看到你使用MATLAB仿真,如果你蓝色的是解算出来的角度,那么很好奇你红色的实际角度是如何得到的呢??摄像头捕捉吗? 007 发表于 2014-4-6 15:35
我也做过KP,KI数据调整实验,觉得不错,角度稳定在0.01度以内。看到你使用MATLAB仿真,如果你蓝色的是解算 ...
红色是高性能的IMU设备~把它所得的角度认为是实际的角度,检验算法可靠性的~ hz770495569 发表于 2014-4-6 15:38
红色是高性能的IMU设备~把它所得的角度认为是实际的角度,检验算法可靠性的~ ...
求教此高性能IMU,我也想做楼主的类似实验,需要检测真实角度。想做提高解算姿态速度的算法,却苦于不知道真实姿态 007 发表于 2014-4-6 18:02
求教此高性能IMU,我也想做楼主的类似实验,需要检测真实角度。想做提高解算姿态速度的算法,却苦于不知 ...
这个是实验室买的设备~xsens的~具体我不清楚,没用过~ hz770495569 发表于 2014-4-6 18:21
这个是实验室买的设备~xsens的~具体我不清楚,没用过~
好的,谢谢! hz770495569 发表于 2014-4-6 18:21
这个是实验室买的设备~xsens的~具体我不清楚,没用过~
请问上次你说的实验室的xsens设备,现在了解型号了吗?我对这种设备很感兴趣,如果不是很贵也想买一个,真羡慕你啊 mark{:smile:} 007 发表于 2014-4-10 14:21
请问上次你说的实验室的xsens设备,现在了解型号了吗?我对这种设备很感兴趣,如果不是很贵也想买一个, ...
XSENSE公司的MTI-G~不便宜,好几万呢~ hz770495569 发表于 2014-4-11 13:10
XSENSE公司的MTI-G~不便宜,好几万呢~
多希望成为你的同学·· 007 发表于 2014-4-11 13:25
多希望成为你的同学··
{:sweat:} {:sweat:} {:sweat:} 007 发表于 2014-4-11 13:25
多希望成为你的同学··
几万的东西就让你把持不住了 mazhenyu 发表于 2014-4-11 22:15
几万的东西就让你把持不住了
我应该做个怎样的表情··难道四轴长路漫漫,到处都是花钱的地方吗 007 发表于 2014-4-12 13:56
我应该做个怎样的表情··难道四轴长路漫漫,到处都是花钱的地方吗
我说的是教研室值钱的东西多得是 john800422 发表于 2014-3-12 13:16
helfT 是取樣週期一半沒錯,
若是實際飛行就另當別論了,
你好 请问那个四元素四个系数更新的Halft具体指的是啥?就是陀螺仪的采样频率吗?比如说我设置成8分频(就是往SMPLRT_DIV这个寄存器写0x07)那么Halft就是1.0/250吗?谢谢。。{:smile:} 牛人啊!学习学习 silence2455 发表于 2014-3-29 23:35
大哥,我采用IMUupdate这个算法已经得出很符合要求的Pitch和Roll,请问应该用什么方法得到准确的Yaw呢?
...
同志啊,这个问题也困扰了我很久,不知道你有没有解决?如果解决了的话,可以给我分享一下经验吗? 残忆视觉 发表于 2014-9-17 20:29
同志啊,这个问题也困扰了我很久,不知道你有没有解决?如果解决了的话,可以给我分享一下经验吗? ...
采用AHRS这个算法吧,融合陀螺仪,加计,磁力计的,效果很好,注意磁力计的校准,这个很重要。。。 好东西,感谢楼主共享 EKF算法是什么- - 扩展卡尔曼? 能解算姿态? hz770495569 发表于 2014-3-12 15:00
嗯,受教了。谢谢!halfT这个我没有多考虑,我的四轴运算频率是400hz。另外我这个取得数据就是实际四轴飞 ...
用了几万块的imu,还弄什么算法。随便滤个波就能用了吧 求大神帮忙看看,我的AHRS调整到真实位置时为什么会分段?前段相应很快,后面调整很慢
http://www.amobbs.com/thread-5601221-1-1.html 学习学习 hz770495569 发表于 2014-3-11 23:29
完整的代码是有对yaw的处理的吧,我这个主要是因为暂时没有地磁计的数据,所以省去了这个部分。总的来说 ...
这位仁兄能否求一下你的EKF版本的算法,最近也在弄卡尔曼,onev2012@163.com 楼主我想问下你的Matlab仿真是怎么实现的?所用的数据是采集回来的数据? 不懂帮顶{:victory:} bygreencn 发表于 2014-12-17 09:49
楼主我想问下你的Matlab仿真是怎么实现的?所用的数据是采集回来的数据?
数据是采集回来的飞行数据,然后matlab编程~ 看来matlab得好好学习一下,不然,用仿真调参会有很大帮助。 来学习学习哦, 这个算法感觉yaw的漂移一定要通过磁力计进行修正。另外陀螺仪的Z值校准也很重要,消除bias 默默七 发表于 2014-12-24 17:33
这个算法感觉yaw的漂移一定要通过磁力计进行修正。另外陀螺仪的Z值校准也很重要,消除bias ...
地磁一直没来的及搞过,陀螺仪校正是刚开始取零点均值吗?还有,经常听到椭圆拟合是什么? onev 发表于 2014-11-8 09:51
这位仁兄能否求一下你的EKF版本的算法,最近也在弄卡尔曼,
公司专利,商业保密,不好意思~ 本帖最后由 my12doom 于 2015-1-3 22:57 编辑
楼主你的数据能否给我一套?以及,用于对比的实际的角度数据是如何测量的?
另外觉得角度差是不是陀螺性能问题? my12doom 发表于 2015-1-3 22:52
楼主你的数据能否给我一套?以及,用于对比的实际的角度数据是如何测量的?
另外觉得角度差是不是陀螺性能 ...
代码和数据在12楼,已共享! hz770495569 发表于 2015-1-4 11:41
代码和数据在12楼,已共享!
数据看到了多谢,参考角度是怎样测的?是类似量角器的测量吗? my12doom 发表于 2015-1-7 19:13
数据看到了多谢,参考角度是怎样测的?是类似量角器的测量吗?
角度是飞机上搭载的IMU传感器测到的,这个因为用的是商业IMU,几万块一个,所以认为它的精度高,接近实际角度~ hz770495569 发表于 2015-1-7 20:16
角度是飞机上搭载的IMU传感器测到的,这个因为用的是商业IMU,几万块一个,所以认为它的精度高,接近实际 ...
你们用的陀螺仪和加速计是哪个?这个算法修改Kp Ki只会影响比较长期的数据(>5秒),短期数据主要还是靠陀螺仪性能和校准 my12doom 发表于 2015-1-8 13:44
你们用的陀螺仪和加速计是哪个?这个算法修改Kp Ki只会影响比较长期的数据(>5秒),短期数据主要还是靠 ...
陀螺仪用的单轴的,加速度计用的ADXL345~
页:
[1]