如何解决为什么四元数产生错误的角度?
我在这里为AHRS / MARG / IMU写了一些代码。 目的是将加速度计(重力)和磁力计(地球磁场)矢量结合起来以产生四元数,该四元数表示身体框架如何从全局框架旋转,我正在关注本文https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4570372/。这是通过将Qacc乘以Qmag(等式36),将Qacc乘以俯仰和滚动(等式25)以及将Qmag乘以偏航(等式35)来实现的。问题是我分别获得了Qacc(+-80以上的俯仰角除外)和Qmag的正确角度,但是当我将Qacc乘以Qmag时,俯仰角,横滚角和偏航角都变得不正确了。在我看来,这一定是因为我的四元数乘法,但是我仔细检查了代码,乘法似乎是正确的... 这是我的短代码:
void UpdateAccMagQuaternion(){
float ax = 0,ay = 0,az = 0,mx = 0,my = 0,mz = 0;
float recipnorm,cosAngle,sinAngle;
float XZprojection = 0;
float sensorBuffer[3] = {0};
float axis[3] = {0,0};
LSM303DLHC_AccReadXYZ(sensorBuffer); //acceleration vector
ax = sensorBuffer[0];
ay = sensorBuffer[1];
az = sensorBuffer[2];
//normalize gravity acceleration vector
recipnorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipnorm;
ay *= recipnorm;
az *= recipnorm;
//Qacc
if(az < 0) {
Qacc.w = -ay/sqrt(2*(1-az));
Qacc.x = sqrt((1-az)/2);
Qacc.y = 0;
Qacc.z = ax/(sqrt(2*(1-az)));
}
else {
Qacc.w = sqrt((1+az)/2);
Qacc.x = -ay/sqrt(2*(1+az));
Qacc.y = ax/(sqrt(2*(1+az)));
Qacc.z = 0;
}
LSM303DLHC_MagReadXYZ(sensorBuffer);
mx = (sensorBuffer[0]); //Magnetometer offset and scale calibration
my = (sensorBuffer[1]);
mz = (sensorBuffer[2]);
// normalize earth magnetic field vector
recipnorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipnorm;
my *= recipnorm;
mz *= recipnorm;
//Qmag
XZprojection = mx*mx + my*my;
if (mx < 0){
Qmag.w = my/(sqrt(2)*sqrt(XZprojection-mx*sqrt(XZprojection)));
Qmag.x = 0;
Qmag.y = 0;
Qmag.z = sqrt(XZprojection-mx*sqrt(XZprojection))/sqrt(2*XZprojection);
}
else {
Qmag.w = sqrt(XZprojection+mx*sqrt(XZprojection))/sqrt(2*XZprojection);
Qmag.x = 0;
Qmag.y = 0;
Qmag.z = my/(sqrt(2)*sqrt(XZprojection+mx*sqrt(XZprojection)));
}
//Total Q = Qacc X Qmag
Q.w = (Qacc.w*Qmag.w - Qacc.x*Qmag.x - Qacc.y*Qmag.y - Qacc.z*Qmag.z);
Q.x = (Qacc.w*Qmag.x + Qacc.x*Qmag.w + Qacc.y*Qmag.z - Qacc.z*Qmag.y);
Q.y = (Qacc.w*Qmag.y - Qacc.x*Qmag.z + Qacc.y*Qmag.w + Qacc.z*Qmag.x);
Q.z = (Qacc.w*Qmag.z + Qacc.x*Qmag.y - Qacc.y*Qmag.x + Qacc.z*Qmag.w);
}
这是我的将四元数转换为欧拉角的代码:
roll = atan2(2.0 * (Q.z * Q.y + Q.w * Q.x),1.0 - 2.0 * (Q.x * Q.x + Q.y * Q.y))*57.2958;
pitch = asin(2.0 * (Q.y * Q.w - Q.z * Q.x))*57.2958;
yaw = atan2(2.0 * (Q.z * Q.w + Q.x * Q.y),- 1.0 + 2.0 * (Q.w * Q.w + Q.x * Q.x))*57.2958;
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。