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

发新话题 发布投票 发布商品 发布悬赏 发布活动 发布辩论 发布视频 窗体顶端捷联系统的四元数法姿态算法算法输入：物体的初始姿态，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，就可以以直观的形式查看当前姿态核心算法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;次要的规范化算法：voidQuaternion: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;floatpitch,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.print(nowG.fYaw/3.1415926535*180,11);/偏航 Serial.print(,); Serial.print(nowQ.getS(),11);/偏航 Serial.println(); count=0;四元数与欧拉角之间的转换 在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系： 图1 3D Cartesian coordinate System (from wikipedia) 定义分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。 图2 Tait-Bryan angles (from wikipedia) 一、四元数的定义 通过旋转轴和绕该轴旋转的角度可以构造一个四元数： 其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。 二、欧拉角到四元数的转换 三、四元数到欧拉角的转换 arctan和arcsin的结果是，这