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

发新话题 发布投票 发布商品 发布悬赏 发布活动 发布辩论 发布视频 捷联系统的四元数法姿态算法算法输入：物体的初始姿态，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，就可以以直观的形式查看当前姿态 核心算法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 lo