四轴飞行器姿态控制算法

姿态解算

姿态解算

(attitude algorithm),

是指把陀螺仪，加速度计

,

罗盘等的数据融

合在一起，得出飞行器的空中姿态，飞行器从陀螺仪器的 三轴角速度通

过四元数法得到俯仰，航偏，滚转角，这是快速解算，结合三轴地磁和

三周加速度得到漂移补偿和深度解算。

姿态的数学模型坐标系

姿态解算需要 解决的是四轴飞行器和地球的相对姿态问题。

地理坐标系

是固定 不变的，正北，正东，正上构成了坐标系的

X

，

Y

，

Z

轴用坐标

系

R

表示，飞行器上固定一个坐标系用

r

表示，那么我们就可以适用欧

拉角，四元数等来描 述

r

和

R

的角 位置关系。

姿态的数学表示

姿态有多种数学表示 方式，常见的是四元数，欧拉角，矩阵和轴角。在

四轴飞行器中使用到了四元数和欧拉角

,

姿态解算的核心在于旋转。

姿态

解算中使用四元数来保存飞行器的姿态，

包括旋转和方位。

在获得四元

数之后，会将其转化为欧拉角，然后输入到姿态控制算法中。 姿态控制

算法的输入参数必须要是欧拉角。

AD

值是指

MPU6050

的陀螺仪和加

< br>速度值，

3

个维度的陀螺仪值和

3

个维度的加速度值，

每个值为

16< /p>

位精

度。

AD

值 必须先转化为四元数，然后通过四元数转化为欧拉角。在四

轴上控制流程如下图：

下面是用四元数表示飞行姿态的数学公式 ，从

MPU6050

中采集的数据

经过 下面的公式计算就可以转换成欧拉角，

传给姿态

PID

控制器中进行

姿态控制

.

PID

控制算法

先简单说明下四轴飞行器是如何飞行的

,

四轴飞行器的螺旋 桨与空气发

生相对运动，

产生了向上的升力，

< br>当升力大于四轴的重力时四轴就可以

起飞了。

四轴飞行器 飞行过程中如何保持水平：我们先假设一种理想状

况：

四个电机 的转速是完全相同的是不是我们控制四轴飞行器的四个电

机保持同样的转速，当转速超过 一个临界点时（升力刚好抵消重力）四

轴就可以平稳的飞起来了呢？答案是否定的，由于 四个电机转向相同，

四轴会发生旋转。

我们控制四轴电机

1

和电机

3

同向，< /p>

电机

2

电机

4< /p>

反

向，刚好抵消反扭矩，巧妙的实现了平衡

,

但是实际上由于电机和螺旋