使⽤MPU6050硬件DMP解算姿态是⾮常简单的，下⾯介绍由三轴陀螺仪和加速度计的值来使⽤四元数软件解算姿态的⽅法。
我们先来看看如何⽤欧拉⾓描述⼀次平⾯旋转(坐标变换)： 设坐标系绕旋转α⾓后得到坐标系，在空间中有⼀个⽮量在坐标系中的投影为，在内的投影为由于旋转绕进⾏，所以Z坐标未变，即有。
我们约会吧 非诚勿扰转换成矩阵形式表⽰为：
三二三事变四川大学整理⼀下：
llvm兰州石化研究院所以从旋转到可以写成
上⾯仅仅是绕⼀根轴的旋转，如果三维空间中的欧拉⾓旋转要转三次:
上⾯得到了⼀个表⽰旋转的⽅向余弦矩阵。
不过要想⽤欧拉⾓解算姿态，其实我们套⽤欧拉⾓微分⽅程就⾏了：
上式中左侧,，是本次更新后的欧拉⾓，对应row,pit,yaw。右侧，是上个周期测算出来的⾓度，，，三个⾓速度由直接安装在四轴飞⾏器的三轴陀螺仪在这个周期转动的⾓度，单位
为弧度，计算间隔时T陀螺⾓速度，⽐如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分⽅程就能解算出当前的欧拉⾓。
前⾯介绍了什么是欧拉⾓，⽽且欧拉⾓微分⽅程解算姿态关系简单明了，概念直观容易理解，那么我们为什么不⽤欧拉⾓来表⽰旋转⽽要引⼊四元数呢？
⼀⽅⾯是因为欧拉⾓微分⽅程中包含了⼤量的三⾓运算，这给实时解算带来了⼀定的困难。⽽且当俯仰⾓为90度时⽅程式会出现神奇的“GimbalLock”。所以欧拉⾓⽅法只适⽤于
⽔平姿态变化不⼤的情况，⽽不适⽤于全姿态飞⾏器的姿态确定。
四元数法只求解四个未知量的线性微分⽅程组，计算量⼩，易于操作，是⽐较实⽤的⼯程⽅法。
我们知道在平⾯(x,y)中的旋转可以⽤复数来表⽰，同样的三维中的旋转可以⽤单位四元数来描述。我们来定义⼀个四元数：
我们可以把它写成，其中,。那么是⽮量，表⽰三维空间中的旋转轴。w是标量，表⽰旋转⾓度。那么就是绕轴旋转w度，所以⼀个四元数可以表⽰⼀个完整的旋转。只有单位四元
数才可以表⽰旋转，⾄于为什么，因为这就是四元数表⽰旋转的约束条件。
⽽刚才⽤欧拉⾓描述的⽅向余弦矩阵⽤四元数描述则为：
所以在软件解算中，我们要⾸先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模，传⼊参数是陀螺仪x,y,z值和加速度计x,y,z值： <code>void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm;
下⾯把四元数换算成⽅向余弦中的第三⾏的三个元素。刚好vx,vy,vz 其实就是上⼀次的欧拉⾓（四元数）的机体坐标参考系换算出来的重⼒的单位向量。
<code>// estimated direction of gravity vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
axyz是机体坐标参照系上，加速度计测出来的重⼒向量，也就是实际测出来的重⼒向量。
axyz是测量得到的重⼒向量，vxyz是陀螺积分后的姿态来推算出的重⼒向量，它们都是机体坐标参照系上的重⼒向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以⽤向量叉积（也叫向量外积、叉乘）来表⽰，exyz就是两个重⼒向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，⽽陀螺积分误差也是在机体坐标系，⽽且叉积的⼤⼩与陀螺积分误差成正⽐，正好拿来纠正陀螺。（你可以⾃⼰拿东西想象⼀下）由
于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
<code>// integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki;
⽤叉积误差来做PI修正陀螺零偏
<code>// integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; 四元数微分⽅程，其中T为测量周期，为陀螺仪⾓速度，以下都是已知量，这⾥使⽤了⼀阶龙哥库塔求解四元数微分⽅程：
我最美丽的时候<code>// integrate quaternion rate and normalise q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 +
(q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
硫化氢气体最后根据四元数⽅向余弦阵和欧拉⾓的转换关系，把四元数转换成欧拉⾓：
所以有：
Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议