Amelia飞控（二）：算法设计（上）

2017-07-25

这是Amelia的算法概述。本文主要内容：

姿态解算算法
数字滤波器的设计
控制算法

姿态解算

Amelia的姿态解算使用了加速度和角速度一阶互补滤波和FIR滤波器，互补滤波结果通过截止频率为80Hz的FIR低通滤波器滤除噪声。关于FIR滤波器的设计请参见后续文章。

互补滤波

以下是Amelia所使用的互补滤波代码。

angle_x    = (1.0    - K1) * angleAx    + K1 * (angle_x + gyroGy * DT);
angle_y    = (1.0    - K1) * angleAy    + K1 * (angle_y + gyroGx * DT);

angleAx angleAy是通过加速度计算得到的机体倾角，计算方法如下：

angleAx = atan2(ax, az) * 57.29577951;

这里只给出了X轴倾角的计算方法。重力加速度在Z方向和X方向的分量的arctan值即为X方向上机身水平面与水平方向的夹角，乘57.29577951是通过弧度换算角度即180/π。

gyro_m是当前角速度。DT为程序运行一个周期的时间。angle + gyro_m DT是计算上一次机体角度加上通过角速度计算出程序运行一个周期机身所转过的角度，即为使用角速度估计得到的当前角度，其实这里的DT不用准确值，因为我们并不需要知道角度的准确值，而是需要一个能够表征机身倾角的数值。最后angle的计算值为通过加速度计算的机身倾角与通过角速度计算的机身倾角进行加权求和，也叫互补。为什么需要进行互补呢？由于加速度计测量的瞬时值有很大噪声，但是通过加速度向量进行角度计算没有累积误差。而通过角速度计算倾角的特性正好相反，angle + gyro_m DT会因为累积误差而逐渐偏移正确值。所以需要对两者进行互补。

FIR滤波器

FIR滤波器全称是有限冲击响应滤波器，可以设计为高通、低通、带通、带阻。FIR滤波器运算形式为输入信号序列与已知序列进行卷积。Amelia的FIR滤波器代码如下：

float FIR_Filter_Y(float input)
{
    float    output;

    temp_y = y[0];

    y[0] =    input;    //递推序列
    y[9] =    y[8];
    y[8] =    y[7];
    y[7] =    y[6];
    y[6] =    y[5];
    y[5] =    y[4];
    y[4] =    y[3];
    y[3] =    y[2];
    y[2] =    y[1];
    y[1] =    temp_y;

    output    = y[0] * 0.006749454325693 + y[1] * 0.025063820521276 + y[2] * 0.081909996095874    //卷积
        + y[3] * 0.162947247253628 + y[4] * 0.223329481803529 + y[5] * 0.223329481803529
        + y[6] * 0.162947247253628 + y[7] * 0.081909996095874 + y[8] *    0.025063820521276    
        + y[9] * 0.006749454325693;
    return    output;
}

滤波器序列可由MATLAB进行设计，具体关于FIR滤波器介绍以及MATLAB设计方法请参见后续文章。

Amelia并没有使用目前主流的四元数姿态解算 + 卡尔曼滤波算法。主要是考虑到该方案在姿态解算的效果上与互补滤波 + FIR滤波器相比并没有显著优势，同时互补滤波 + FIR滤波器更加简洁易懂，对初学者更为友好。你可以在Amelia工程没有使用的部分代码中发现四元数姿态解算 + 卡尔曼滤波，这是Amelia的实验代码，你可在此基础上自由发挥。

控制算法

Amelia使用双环PID算法进行控制。外环为角度控制环，通过PI控制器输出角度期望值。内环为角速度控制环，接收外环输出，再通过PID控制器控制飞行器各方向角速度从而控制飞行器姿态。过于PID算法的更多介绍请参见后续文章。

最后

写这个系列文章的目的是为了方便大家了解Amelia，同时为想自己制作飞控的朋友提供参考。
Amelia的代码已经上传GitHub，Clone整个Project到本地，用Keil uVision 5打开USER文件夹下的Amelia.uvprojx文件即可开始编辑和编译。其他设计资料请访问百度网盘密码: wt38。希望大家能够顺利地制作出自己的四旋翼飞行器，欢迎大家提出宝贵的意见和建议或对Amelia的代码和硬件设计进行改进。

声明：Amelia飞控的代码、PCB文件以及本文基于CC知识共享协议 知识共享署名-相同方式共享 4.0 国际许可协议 发布。

本作品采用知识共享署名-相同方式共享 4.0 国际许可协议进行许可。