|
![](static/image/common/ico_lz.png)
楼主 |
发表于 2011-11-6 18:31:39
|
显示全部楼层
开别人源!
以下均为原作者的话,如有版权问题请与我联系!120538967
原作者:5iMX dongfang
资料来源:http://bbs.5imx.com/bbs/viewthread.php?tid=504301&highlight=%CB%C4%D4%AA%CA%FD
ourdev_692510DX36ZT.doc(文件大小:258K) (原文件名:捷联系统的四元数法姿态算法.doc)
算法名:捷联系统的四元数法姿态算法
算法输入:物体的初始姿态,3轴陀螺仪不同时刻的Yaw,Pitch,Roll的角速度;
算法输出:物体的当前姿态。
具体算法:(有颜色的是李国乐加的)
1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A(四元数初值)
2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度,乘以上次计算以来的间隔时间,得到上一时刻以来(Yaw,Pitch,Roll)的变化量,命名为欧拉角b(角速度对时间积分的角度变化量得到欧拉角)
3. b是Tait-Bryan angle定义的欧拉角,将其转为四元数B(欧拉角->四元数)
4. A=A×B,做四元数乘法,即可得到当前姿态对应的新的四元数A(更新四元数)
5.重复2~4部,即可连续更新姿态(循环计算)
6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a,就可以以直观的形式查看当前姿态(四元数->欧拉角)
这里严重感谢http://www.cppblog.com/heath/archive/2009/12/13/103127.html 的作者,他的代码解决了我最重要的问题: Tait-Bryan angle定义的欧拉角和四元数的双向转换。但他是个游戏开发者,没做姿态更新,我补充了姿态更新的代码。
另外补充一句,我使用的是arduino做的开发,也就是12M的8位AVR单片机,目前,每秒在获取数据400个的速度下,如此大工作量的算法依然能跑。3D旋转N圈,大约10秒钟,各轴漂移在5度左右。
\核心算法1,欧拉角转四元数
void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
float fCosHRoll = cos(ea.fRoll * .5f);
float fSinHRoll = sin(ea.fRoll * .5f);
float fCosHPitch = cos(ea.fPitch * .5f);
float fSinHPitch = sin(ea.fPitch * .5f);
float fCosHYaw = cos(ea.fYaw * .5f);
float fSinHYaw = sin(ea.fYaw * .5f);
w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}
核心算法2,四元数转欧拉角
EulerAngle Quaternion::ToEulerAngle() const
{
EulerAngle ea;
ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));
return ea;
}
核心算法3,四元数乘法
Quaternion Quaternion::Multiply(const Quaternion &b)
{
Quaternion c;
c.w=w*b.w -x*b.x -y*b.y -z*b.z;
c.x=w*b.x +x*b.w +y*b.z -z*b.y;
c.y=w*b.y -x*b.z +y*b.w +z*b.x;
c.z=w*b.z +x*b.y -y*b.x +z*b.w;
c.Normalize();
return c;
}
次要的规范化算法:
void Quaternion::Normalize(){
float s=getS();
w/=s;
x/=s;
y/=s;
z/=s;
}
float Quaternion::getS(){
return sqrt(w*w+x*x+y*y+z*z);
}
我的loop函数,算法的集成部分:
Quaternion nowQ;
void loop() {
int intx, inty,intz;
float pitch,roll,yaw;
gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw);
EulerAngle dt;
dt.fRoll=roll;
dt.fPitch=pitch;
dt.fYaw=-yaw;
Quaternion dQ;
dQ.FromEulerAngle(dt);
nowQ=nowQ.Multiply(dQ);
count++;
if (count>1000){
EulerAngle nowG=nowQ.ToEulerAngle();
Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
Serial.print(",");
Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
Serial.print(",");
Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
Serial.print(",");
Serial.print(nowQ.getS(),11);//偏航
Serial.println();
count=0;
}
} |
|