无人机姿态解算_互补滤波（1）

一、 基础知识

1. 坐标系：遵循右手定则

1.1 大地坐标系（地球坐标系）：北（x轴）东（y轴）地（z轴），地就是向下指向地
1.2 机体坐标系： 前（x轴）右（y轴）下（z轴）
1.3 IMU坐标系：该坐标系就是IMU本身的坐标系，一般无人机的IMU都是在飞控的中间，因此，IMU坐标系和机体坐标系可以看做有相同的原点，只需要简单的改变一下坐标轴的正负号就能和机体坐标系一致了，并不需要复杂的旋转。

2.姿态: 横滚（roll）俯仰(pitch) 偏航(yaw)

如果所示：
横滚(roll): 绕x轴旋转的动作叫横滚，向右横滚为正方向，向左横滚为负方向；
俯仰（pitch）:绕y轴旋转的动作叫做俯仰，向上（抬头）为正方向，向下为负方向；
偏航（yaw）: 绕z轴旋转的动作叫做偏航，向右偏航为正方向，向下偏航为负方向。

3. 旋转矩阵

一般定义旋转矩阵为

然后欧拉角和四元数都可以表示旋转矩阵，欧拉角和四元数可以互换。然后一般在无人机上，都是用四元数来表示欧拉角的。

4. 欧拉角：横滚角（roll angle）俯仰角(pitch angle) 偏航角(yaw angle)

3.1 姿态角也叫欧拉角。
横滚角(roll angle): 绕x轴横滚的角度，向右横滚为正方向，向左横滚为负方向；
俯仰角（pitch angle）:绕y轴俯仰的角度，向上（抬头）为正方向，向下为负方向；
偏航角（yaw angle）: 绕z轴偏航的角度，向右（顺时针）偏航为正方向，向左（逆时针）偏航为负方向。

3.2 欧拉角表示旋转，旋转顺序是Z-Y-X。
下面的矩阵是机体系到地球系的旋转矩阵。

如果将一个机体系转到地球系就是用上面的矩阵。

下面是地球系到机体系的旋转矩阵。

下面的意思就是将地球系vg通过地球系到机体系的旋转矩阵旋转至机体系。

5. 四元数

4.1 四元数：q0,q1,q2,q3，软件中默认初值分别是0，1，1，1。
四元数的更新（即q0,q1,q2,q3怎么来）：根据陀螺仪的数据来更新四元数
△t ： 是读取陀螺仪的时间，也是调用姿态解算的时间。
wx,wy,wz：分别是陀螺仪测量到的值，单位是弧度每秒，rad/s. 如果用的是度每秒，那么最终的姿态是：你稍微动一下，姿态就变化很大。

4.2 四元数表示的矩阵

6. 用四元数表示欧拉角

欧拉角是地球系下的表示；传感器的数值是在机体系上表示的，因此，需要将机体系转换到地球系。所以要用到下面的旋转矩阵。

对比红框的内容就可以用四元数表示欧拉角了：

二、Mahony 互补滤波算法

开源代码位置：https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
源文件在 《四、代码 》 Algorithm/madgwick_algorithm_c的压缩包中。

传入的数据：校准且滤波后的九轴数据。其中，陀螺仪的单位要从度转换成弧度，不然的话，稍微动一动IMU，姿态就变换很大。

	gyro.x = gyro.x * DEG2RAD;	/* 度转弧度 */
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;

三、 PID控制：Mahony互补滤波中只用到了P和I。

代码中的下面三个参数是需要根据实际来进行修改的。

这里我只用到了比例调节。下面是根据横滚角输出的PID图，可以看到姿态到达目标姿态还是很快的。

四、代码

gitee 搜索 stm32_minidrone

如果错误，请指正。