AHRS 姿态结算 yaw不准确

979653421@qq.co

是这样的，本人最近在移植ahrs的姿态结算，程序是网上的，不过很奇怪结算出来的姿态pitch，roll 是准确的，但是yaw不准确，
具体表现为，假如我轻轻转动几度，它算出来的的yaw角可能要100多度了

979653421@qq.co

原来是自己上传数据时多乘了10

流浪者转世

楼主能分享一下程序吗？？？

979653421@qq.co

回复【3楼】流浪者转世:
---------------------------------
#define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer
#define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases
#define halfT 0.5f // 采样周期的一半

//---------------------------------------------------------------------------------------------------

float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计

方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差

//====================================================================================================
// 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;

// 辅助变量，以减少重复操作数
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;          

// 测量正常化
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;         

// 计算参考磁通方向
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;        

//估计方向的重力和磁通（V和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);  

// 错误是跨产品的总和之间的参考方向的领域和方向测量传感器
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);

// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

// 整合四元数率和正常化
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;  

// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}




/*输入参数为加速度xyz原始数据，磁力计xyz原始数据，陀螺仪数据必须转换为弧度制*/

zb120302020

楼主、、同样的问题，同求解决方案……

979653421@qq.co

回复【5楼】zb120302020:
---------------------------------
是不是用匿名上位机，它的roll，pitch是X100
yaw是X10，然后再传到上位机，你可能是yaw也X100了，改为10就正常了

yuzeyuan1

回复【6楼】979653421@qq.com:
---------------------------------
楼主，我加了你的QQ好友，有些问题想请教。
第一个：/*输入参数为加速度xyz原始数据，磁力计xyz原始数据，陀螺仪数据必须转换为弧度制*/  我先做了如下处理
         axg=axg/8192+0.01065; ayg=ayg/8192+0.03039; azg=azg/8192+0.086081;    %将加速计的原始数据除以分辨率8192，可得到真正的加速度，再减去零偏，零偏要求X Y轴归0、Z轴归1
         wx=wx/16.4+1.92557;   wy=wy/16.4-0.332004;   wz=wz/16.4-0.396221; %将陀螺仪的原始数据除以分辨率16.4，可得到真正的角速度，再减去零偏，零偏要求X Y Z轴归0
         hx=hx+320;         hy=hy+315;           hz=hz+55; %加上八字校准法得到的偏移量
原始数据应该这么处理再用吗？就是取个零偏，这里面的/8192和/16.4是MPU6050数据手册里写的，反正你贴出来的代码里有测量数据单位化的处理，这里除不除都一样。
第二个问题：这个系统有要求加速计、陀螺仪、罗盘的三轴要按什么方向放置吗？我的X轴代表横滚角roll、Y轴代表俯仰角pitch、Z轴代表航向角yaw，求出来的结果roll和pitch进行了/pi*180的处理后也只有正负一点几那么大，根本不是我的正确俯仰角和横滚角。我的问题出在哪里？

yuzeyuan1

回复【6楼】979653421@qq.com:
---------------------------------
哦，刚少写了一句，wx=wx/180*pi;wy=wy/180*pi;wz=wz/180*pi;  %转化为弧度制
这句话我在我的代码里写了。还有哪里有问题？为啥只有一点几度的俯仰和横滚，而且形状也完全不对。

yuzeyuan1

回复【6楼】979653421@qq.com:
---------------------------------
第二个问题里一点几的问题好了，我不小心把赋值写到循环里了，现在感觉还是正方向没弄对，两个传感器的正方向是根据什么来确定的？

混蛋潘

回复【4楼】979653421@qq.com:
---------------------------------
求楼主知道，也在做四轴算法

混蛋潘

回复【2楼】979653421@qq.com:
---------------------------------
求楼主知道，也在做四轴算法