捷联系统的四元数法姿态算法.docx

捷联系统的四元数法姿态算法算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。具体算法：1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B4. A=AB，做四元数乘法，即可得到当前姿态对应的新的四元数A5.重复24部，即可连续更新姿态6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态。这里严重感谢/heath/archive/2009/12/13/103127.html 的作者，他的代码解决了我最重要的问题: Tait-Bryan angle定义的欧拉角和四元数的双向转换。但他是个游戏开发者，没做姿态更新，我补充了姿态更新的代码。另外补充一句，我使用的是ardunio做的开发，也就是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 (count1000) 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