基于四元数解算陀螺仪姿态角算法的实现.pdf

《基于四元数解算陀螺仪姿态角算法的实现.pdf》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、基于四元数解算陀螺仪姿态角算法的实现1112关越魏，何波贤，于仁清，丁广威（1.92724部队，山东青岛266108；2.92106部队，山东青岛266108）摘要：采用四元数将三轴陀螺仪以及三轴加速度计的数据进行互补融合，讨论了四元数解算的设计过程，给出了相应步骤的详细代码。实验证明，该算法有效抑制陀螺仪的误差发散，并提高了系统的测量精度。关键词：四元数；姿态解算；叉乘；微处理器DOI:10.16184/j.cnki.comprg.2015.09.0131引言3.1四元数初始化基于陀螺仪的姿态角测量目前大量应用于汽车、移动电子设利用Atmega8单片机在采样周期

2、内连续采集MPU6050传备、无人机等领域，尤其是手机上的应用，比如GPS导航、感感器的10组数据，然后求和取平均值，作为四元数的输入，应控制、体感互动、拍照稳定等功能的实现。目前对陀螺仪的姿四元数的初始化主要是对误差积分以及全局四元数进行赋值，态角解算方法有3种，欧拉角法由于存在“奇点”现象而不能应代码如下：用于全姿态跟踪，并难以在工程实践中广泛应用；且方向余弦volatilefloatexInt,eyInt,ezInt;//误差积分法方程的计算量大，工作效率低；而四元数方法比较难理解，volatilefloatq0,q1,q2,q3;//全局四元数q0=1.0

3、f,q1=0.0f,q2=0.0f,q3=0.0f;//初始化四但是计算量简单并能够实时解算姿态角，因此得到广泛的应用。//元数基于四元数的姿态解算方法能够有效结合陀螺以及加速exInt=0.0,eyInt=0.0,ezInt=0.0;//误差积分度计的误差特性，将运动场以及重力加速度两个互不相干的3.2修正角速度误差物理矢量进行互补融合。主要利用陀螺仪测量的角速度作为三轴重力加速度计由于噪声比较大，而且在飞行过程中，四元数的更新，以重力加速度作为四元数的观测，通过8位受机体振动影响比三轴陀螺仪明显，短时间内的可靠性不高。微处理器实时解算姿态角。陀螺仪噪声小，但是

4、由于积分是离散的，长时间的积分会出2硬件结构现漂移的情况，因此需要将用重力加速度计求得的姿态来矫MPU-6050是集成三轴陀螺、三轴加速度计、温度传感器正陀螺仪积分姿态的漂移。为一体的组合传感器，其中主控采用AVR单片机，传感器数根据余弦矩阵和欧拉角的定义以及地理坐标系的重力向据主要通过I2C接口形式传输到单片机上进行实时处理，系统量，设经旋转至机体坐标系上的重力加速度为：结构框图如图1所示。（3）图1硬件结构框图简化有：3基于四元数的姿态解算四元数基本思路为：一个坐标系到另一个坐标系的变换然后把机体测得的加速度值ab=（abxabyabz）作归一化处理。可以通过

5、绕一个定义在参考坐标系中的矢量μ的单次转动来加速度计测量的重力向量为ab=（abxabyabz）；经陀螺积分实现，其中矢量μ既代表了旋转轴的长度，同时也代表旋转后的姿态来推算出的重力向量是vxvyvz；它们之间的误差向角，所以它的表达式为：量就是陀螺积分后的姿态和加速度计测量的姿态之间的误差。并且根据向量间的误差，可以用向量积(也叫外积、叉乘)来（1）表示，exeyez就是两个重力向量的叉积。这个叉积向量仍旧并且四元数的旋转矩阵为：是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系上，而且叉积的大小与陀螺积分误差成正比，正好用来纠正（2）陀螺。由于陀螺是对机体直

6、接积分，所以对陀螺的纠正量会四元数的解算姿态角的流程如图2所示。作者简介：关越魏（1986-），男，助理讲师，研究方向：计算机仿真；何波贤，硕士，研究方向：计算机仿真。图2四元数解算流程收稿日期：2015-01-192015.0932直接体现在对机体坐标系的纠正，所以有：程为：（4）其中：x，y，z是机体坐标系上的单位向量，经简化得：（6）其中ωxωyωz为三轴输出的实际角速度。这部分代码为：这部分代码为：#definehalfT0.5f//采样周期的一半//测量正常化//四元数更新norm=sqrt(ax*ax+ay*ay+az*az);q0=q0+(-q1*g

7、x-q2*gy-q3*gz)*halfT;ax=ax/norm;q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;ay=ay/norm;q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;az=az/norm;q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;//估计方向的重力vx=2*(q1*q3-q0*q2);3.5规范化处理vy=2*(q0*q1+q2*q3);使用8位微处理器实时计算四元数的更新，存在误差等vz=q0*q0-q1*q1-q2*q2+q3*q3;因素，会导致在计算过程中会逐渐失去规范化特性，因此每//经

8、叉乘积并求