MiniFly微型四轴学习开发日志（三）——关于姿态解算的相关算法

最新推荐文章于 2022-10-31 22:46:28 发布

铠袖一触1

最新推荐文章于 2022-10-31 22:46:28 发布

阅读量1.2k

点赞数 4

分类专栏：无人机文章标签： stm32 嵌入式

本文链接：https://blog.youkuaiyun.com/watchingdog/article/details/116528532

版权

无人机专栏收录该内容

4 篇文章

订阅专栏

之前两篇日志当中提到了关于角度-角速度串级PID与定高Z轴PID配合对四轴进行姿态矫正的大概算法。但是对于测量而来的角速度、三个姿态解算值以及去重力加速度之后的Z轴加速度，却并没有详细说明其如何得来，只是说它们从MPU9250当中的陀螺仪以及加速度计结合算法得出，今天就来简单介绍MPU9250当中的算法。

欧拉角与四元数

介绍姿态解算的算法之前，先要了解到相关的知识点才好理解后面的代码，首先要学习到欧拉角和四元数。

如上图，设定 xyz- 轴为参考系的参考轴。称 xy- 平面与 XY- 平面的相交为交点线，用英文字母（ N ）代表。 zxz 顺规的欧拉角可以静态地这样定义： α 是 x- 轴与交点线的夹角， β 是 z- 轴与 Z- 轴的夹角， γ 是交点线与 X- 轴的夹角。对于夹角的顺序、标记，夹角两个轴的定义，并没有指定，因此只要适当定义，就是我们需要的三个姿态解算值roll/pitch/yaw。

而关于如何得到欧拉角，就要涉及到另一个知识点：四元数。

四元数可以理解为一个三维向量和一个实数的组合，如下图：

它和普通向量类似，也可以单位化、求模。我个人的理解是，这个四维向量相当于一个三维向量的一个w角的旋转动作，也可以表示一个姿态。

四元数可以转换成四元数矩阵，如下图：

而若要通过四元数求得欧拉角则需要以下转换：

显然，若要求得欧拉角，我们就需要四元数矩阵当中的R21、R11、以及第三行的三个元素。

互补滤波算法的姿态解算

有了以上知识点，我们就能大略了解姿态解算的算法。

首先，四轴用的MPU9250当中，包含有陀螺仪和加速度计，它们的原始测量值分别为角速度以及实际的3轴加速度共六个数值。所以一开始代码如下：

void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)/*数据融合 互补滤波*/
{
  float normalise;
  float ex, ey, ez;
  float q0s, q1s, q2s, q3s; /*四元数的平方*/
  static float R11,R21; /*矩阵(1,1),(2,1)项*/
  static float vecxZ, vecyZ, veczZ; /*机体坐标系下的 Z 方向向量*/
  float halfT =0.5f * dt;
  Axis3f tempacc =acc;
  gyro.x = gyro.x * DEG2RAD; /* 度转弧度 */
  gyro.y = gyro.y * DEG2RAD;
  gyro.z = gyro.z * DEG2RAD;

入口参数当中，acc、gyro为加速度计与陀螺仪测量的原始数据的结构体，state为要输出的各个姿态值的结构体指针。

一开始，用tempacc记录好加速度的测量原始数据，即三轴加速度。并将陀螺仪的原始数据转为弧度制。

接下来需要将上次的四元数进行处理得到机体坐标下的重力向量，将acc单位化得到实测出来的机体坐标下的重力向量，如下：

/*单位化加速计测量值*/
normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
acc.x *= normalise;
acc.y *= normalise;
acc.z *= normalise;
/*机体坐标系下的 Z 方向向量*/
vecxZ = 2 * (q1 * q3 - q0 * q2); /*矩阵(3,1)项*/
vecyZ = 2 * (q0 * q1 + q2 * q3); /*矩阵(3,2)项*/
veczZ = q0s - q1s - q2s + q3s; /*矩阵(3,3)项*/

单位化acc后得到的就是机体坐标下的重力向量，而在机体坐标之下，重力向量即四元数矩阵的第三行。前者是实际测出来的重力向量，后者是根据陀螺仪数据推算而得。于是只要将二者进行叉乘求误差ex、ey、ez，就能用于修正陀螺仪数据，得到修正后的gyro结构体，如下：

/*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
ex = (acc.y * veczZ - acc.z * vecyZ);
ey = (acc.z * vecxZ - acc.x * veczZ);
ez = (acc.x * vecyZ - acc.y * vecxZ);
/*误差累计，与积分常数相乘*/
exInt += Ki * ex * dt ; 
eyInt += Ki * ey * dt ;
ezInt += Ki * ez * dt ;
/*用叉积误差来做 PI 修正陀螺零偏，即抵消陀螺读数中的偏移量*/
gyro.x += Kp * ex + exInt;
gyro.y += Kp * ey + eyInt;
gyro.z += Kp * ez + ezInt;

修正完毕之后，用一阶毕卡算法更新四元数，用到的数据由当前的四元数，修正后的gyro数据以及测量周期，如下：

/* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
float q0Last = q0;
float q1Last = q1;
float q2Last = q2;
float q3Last = q3;
q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;

最后通过更新后的四元数，用四元数与欧拉角的对应关系就可以算出roll/pitch/yaw，并且更新过后的四元数对应的四元数矩阵的第三行即当前的机体坐标下重力向量，与加速度计的三轴加速度点乘再减去重力加速度即可得到去除重力后的Z轴加速度，代码如下：

R11 = q0s + q1s - q2s - q3s; /*矩阵(1,1)项*/
R21 = 2 * (q1 * q2 + q0 * q3); /*矩阵(2,1)项*/
/*机体坐标系下的 Z 方向向量*/
vecxZ = 2 * (q1 * q3 - q0 * q2); /*矩阵(3,1)项*/
vecyZ = 2 * (q0 * q1 + q2 * q3); /*矩阵(3,2)项*/
veczZ = q0s - q1s - q2s + q3s; /*矩阵(3,3)项*/
state->attitude.pitch = -asinf(vecxZ) * RAD2DEG;
state->attitude.roll = atan2f(vecyZ, veczZ) * RAD2DEG;
state->attitude.yaw = atan2f(R21, R11) * RAD2DEG;
/*Z 轴加速度(去除重力加速度)*/
state->acc.z= tempacc.x* vecxZ + tempacc.y * vecyZ + tempacc.z * veczZ - baseZacc;

由此，要输出的就是修正后的gyro数据（角速度），求出来的三个姿态解算值，以及去除重力后的Z轴加速度，我大概画了一个算法流程草图，如下：