[发明专利]基于三轴陀螺仪和加速度计值使用四元数解算姿态的方法

权利要求书

1.基于三轴陀螺仪和加速度计值使用四元数解算姿态的方法，其特征在于：包括以下步骤，

步骤1、用欧拉角描述一次平面旋转，坐标系的变换关系，设坐标系绕旋转α角后得到坐标系，在内的投影由于旋转绕进行，所以Z坐标未变，即有，

；

转换成矩阵形式表示为：

；

整理后即为：；

旋转阵；

所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转需要转三次，

；；

上述运算得到了一个表示旋转的方向余弦矩阵；

步骤2、再通过套用欧拉角微分方程得到欧拉角结算姿态，

；

上式中的左侧，是本次更新后的欧拉角，对应飞行器的roll、pitch、yaw，右侧是上个周期测算出来的角度，三个角速度由直接安装在飞行器的三轴陀螺仪再这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，因此再此求解这个微分方程就能解算

出当前的欧拉角；

步骤3、四元数组法求解，在平面（x，y）中的旋转可以用复数来表示，同理在三维中的旋转也可以用单位四元数来描述，首先定义一个四元数，

；

用矢量表示三维空间中的旋转轴，w是标量，表示旋转角度，那么就是绕轴旋转w度，所以一个四元数可以表示一个完整的旋转，只有单位四元数才可以表示旋转，用欧拉角描述的方向余弦矩阵用四元数描述则为：

。

2.根据权利要求1所述的基于三轴陀螺仪和加速度计值使用四元数解算姿态的方法，其特征在于：所述步骤3中软件解算中，首先把加速度计采集到的值（三维向量）转化为单位向量，即向量处以模，传入参数是陀螺仪x、y、z的值和加速度计x、y、z值，下面把四元数换算成方向余弦中的第三行的三个元素，vx,vy,vz 其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量；axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量；axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量，它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差，向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积，这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺，由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。