F570四轴飞行器学习笔记

四轴飞行器学习笔记

1.前言

最近迷上了四轴飞行器，想系统地学一学无人机相关的知识。起因很简单：我做嵌入式开发，最近项目里涉及到姿态传感器和PID控制，正好四轴飞行器不仅是这些知识的绝佳实践载体，又可以作为个人玩具耍耍~~作为一个爱折腾的技术宅，如果能亲手写一套控制逻辑，看着自己组装的无人机稳稳地悬停在空中，那种成就感十足。

一开始我是奔着DIY去的。觉得只有自己从零搭一台，才能真正理解底层原理。于是开始疯狂看资料：

• 飞行原理：升力、姿态控制、四旋翼动力学

• PID控制或LQR控制

• 飞控选型：F4、F7、H7芯片的区别，BetaFlight、iNav、ArduPilot固件对比

• 硬件搭配：机架、电机、电调、电池、螺旋桨等......

了解相关知识后，我决定选择使用无刷电机，因为其性能会更好，而且不容易损坏。于是我列了无人机组装的BOM清单，然后改了又改，最后开始选型电机，研究KV值；选型电调，研究电调的各种开源固件协议...就这样我折腾了大半个月，代码一行没写，在硬件兼容性和排查短路上就花费了太多太多的时间。我买了好几款电调，好几款电机，期间出现各种意外：电调跟电机的KV值不匹配，烧坏了我的电调；电机测试时可能是因为焊接问题莫名冒烟损坏；电调的固定方式困难，一不小心就短路了等情况，最终积累了不少损坏的电机电调，制作成本也开始日益增加。

本以为是技术大佬的进阶之路，结果却变成了"踩坑之旅"，还是小看了这个基础飞行器搭建。我开始反思：我的初衷是学习研究飞控算法和各种高阶应用，实在是没必要死磕在这些重复造轮子的硬件琐事上。对于像我这种"算法学习"的人来说，我需要的只是一个基础的平台！ 于是我开始研究开源的飞控，试图直接买被验证过的硬件搭配去组装成品，毕竟网上那么多公开可用的案例。我对着各类飞控研究了几天，然后问题又来了：著名的ArduPilot、Pixhawk等开源平台，它们的兼容性很强大，支持多种飞行器模型、多类的传感器，但随着而来的是，他们的代码量非常巨大，对我这种想从简单方案入门的人并不太友好。

在入门的各种问题困扰下，我开始在网上找成品开源的四轴飞行器，我的诉求就是找到一个简单的平台，方便入门，支持二次开发的产品。这便于我研究和修改飞控相关的算法，可以自己写一套控制逻辑。最后综合考虑下来，我选择了轮趣的F570开源无刷四轴飞行器。它简直是我的理想之选，完美地满足我的需求：

• 使用耐用的无刷电机，性能足以满足各种算法验证

• 碳纤维机架，硬度足够了

• 它飞行器内部还是用的四合一电调，不再需要复杂凌乱的接线了。不像我之前买的那种单路的电调，4个电机要4个电调，供电和接线搞的令人头疼。

• 它配备了桨叶保护套，在我调试使用期间，安全系数是拉满了。

• 软件方面，有完整的开源资料，代码量也不像其他开源飞控那么多那么复杂，入门友好。

另外，这款无人机还有个特点是采用了系留式的方案，电池是外置的，飞机标配的电池容量很大，调试期间基本不用担心续航的问题。同时还有光流块、雷达模块可以选配拓展，他们的资料包里也提供了这两个传感器的定点算法方案，值得参考学习一下~

更贴心的是，轮趣科技在 B 站上还发布了【F570 无刷四轴飞行器完整开源教程】视频，包含22条视频学习内容，从课程简介、硬件组成到飞控板与电调版原理图讲解，基于 STM32 与 FreeRTOS，还覆盖了 PID/LQR 两种核心算法，零基础也能跟着从核心原理学到功能复现，大家可以直接去 B 站参考学习：https://www.bilibili.com/video/BV1GvrwBaE56/?vd_source=2e33f81092ad6fb91403e4d96ee63d2f#reply286319827793

轮趣科技的F570无人机

基于轮趣的飞行器硬件，解决了我的"造轮子"难题后，就可以开始我的愉快学习之路了！

2.飞行器的基本硬件和软件系统组成

虽然购买了成品的飞行器不再重复造轮子，但是飞行器的基本硬件还是要了解的，还有整个软件框架是如何实现，这个也是飞行器开发必须了解的内容。这里我总结了如下的要点分享给大家：

硬件部分：

• 机架，这个无需过多介绍。选型就是往强度大、质量轻这两个方向选型，碳纤维机架非常合适。

• 无刷电机需搭配电调使用，常规方案为1个电机对应1个电调（共4个），而轮趣F570飞行器采用四合一电调方案，仅需1个电调板即可驱动4个电机；如果是有刷电机，则需要基本的有刷电机驱动电路。

• 4个桨叶（一对正桨、一对反桨）

• 飞控板：里面跑飞控算法程序，是飞行器的大脑

• 驱动电路：负责电机的控制。有刷电机或无刷电机均需要，通常无刷电机的驱动电路会更复杂，也称作电调。

• 测距传感器：可以是气压计，也可以是激光测距。我个人比较推荐激光测距，它比气压计会稳定的多，不需要做太复杂的数据处理。

• 光流模块：要实现飞行器定点悬停功能，大部分方案都是使用这个传感器提供位置信息，不然飞行器只会乱飞。

  此外，轮趣F570还支持选配雷达模块，这个虽然不是必须的硬件，不过有雷达还可以实现避障、跟随功能。此外，轮趣还提供了雷达定位算法，仅用雷达就可以实现飞行器的定点功能，非常强大。

  软件部分： 这个部分用图片展示，见下图。软件部分每个人设计的都不太一样，这个见仁见智。下面这个是轮趣科技提供的实现，可以作为参考。

  

  3.飞行器的基本控制原理

  在我们对飞行器进行控制之前，需要对其进行简易的坐标系建立和运动分析，如下图所示。（部分内容摘抄自轮趣科技的F570飞行器资料）

  

如上图所示，无人机的电机1、3桨叶为逆时针旋转，而2、4桨叶为顺时针旋转，因此当4个电机转速一致且升力之和等于机体自重时，无人机可以保持悬停，当升力同时增加或减少时，无人机可上升或下降。

当 4 个桨叶升力之和等于无人机自重时，机体处于受力平衡的悬停基础状态；在此基础上，通过调整不同电机的转速差改变升力分布，即可实现各类姿态运动，具体描述如下：

• 电机1、3增速（减速）且2、4减速（增速），由于转矩差的作用，无人机可实现偏航运动；

• 电机1、2增速（减速）且3、4减速（增速），无人机可实现俯仰运动；

• 电机1、4增速（减速）且2、3减速（增速），无人机可实现横滚运动。

• 将以上的基本动作进行组合，无人机可实现各种复杂运动。

通过上述的分析，我们可以使用数学建模的方式，对无人机进行动力学建模，进而使用LQR控制使其平衡。也可以使用单纯的电机运动分析，对飞行器做简单的PID控制，最终把PID计算的结果输入到电机使其平衡。轮趣科技赠送的资料里还贴心地提供了这两种算法，有详细的文档说明和STM32控制代码。

4.飞行器的控制算法设计

了解飞行器的基本控制原理后，就可以基于这个原理来设计控制器了，以下章节会简单介绍PID控制器和LQR控制器的实现方法：

PID控制器

在PID控制器中，不需要对四轴进行建模分析，只需要知道四轴的翻滚、旋转、上升与下降如何控制即可，这些控制内容在上面章节有提到。

四轴飞行器一般会采用串级PID控制，其控制结构如下：外环控制器为角度环，输入为目标角度和实际角度，输出为角速度的目标值。外环特点是响应较慢，更侧重于整体的系统稳定，抑制累计误差等。内环控制器为角速度环，输入为外环的输出值和实际的角速度，输出为电机的油门值，其特点是响应快速，增加系统的抗干扰能力。整体的系统模型如下图所示：

其中串级的PID公式在C语言中可以表示如下：

//roll-外环(角度)
PID_Update(&rollPID,0,attitude.roll);
//pitch-外环(角度)
PID_Update(&pitchPID,0,attitude.pitch);
//yaw-外环(角度)
PID_Update(&yawPID,0,attitude.yaw);
//rollrate-内环(角速度)
PID_Update(&rollratePID,rollPID.output,imu_ori.gyro.x); //rollPID.output是Roll角度环的输出
//pitchrate-内环(角速度)
PID_Update(&pitchratePID,pitchPID.output,imu_ori.gyro.y);//pitchPID.output是Pitch角度环的输出
//yawrate-内环(角速度)
PID_Update(&yawratePID,yawPID.output,imu_ori.gyro.z);//yawPID.output是yaw角度环的输出

有了串级PID控制器后，还需要对PID控制器输出整合到电机油门，完成最终的控制。在四轴飞行器的平衡控制中，一共需要3个串级的PID控制环，分别是Pitch方向的角速度、角度的串级、Roll方向的角速度、角度的串级、Yaw方向的角速度、角度的串级。我们假设3个串级PID的输出结果为 pitchrate_output、rollrate_output、yawrate_output，假设4个电机的控制量为m1、m2、m3、m4，参照章节-飞行器的基本控制原理中的控制描述，可以列出以下输出公式：

m1 = -rollrate_output - pitchrate_output + yawrate_output + basethro //basethro表示基础油门值
m2 = +rollrate_output - pitchrate_output - yawrate_output + basethro
m3 = +rollrate_output + pitchrate_output + yawrate_output + basethro
m4 = -rollrate_output + pitchrate_output - yawrate_output + basethro

除了上述的串级PID公式和输出，我们还需要加上一个基础油门值，让飞行器的推力和自身重力平衡。所以最终公式里还需要加上这个basethro

LQR控制器

说明：本章部分内容摘抄自轮趣科技的F570飞行器资料。

LQR控制有别于PID，它需要对飞行器进行数学建模。我们以本文章节3-飞行器的基本控制原理里面的图示为例，对相关的符号作以下规定：

以表示世界坐标系，表示机体坐标系，定义世界坐标系的基向量为，刚体坐标系的基向量为，欧拉角。采用欧拉角表示法，则有从世界坐标系到机体坐标系的旋转矩阵：

其中牛顿定律和欧拉方程为我们提供了6自由度刚体的移动和转动动力学方程如下：

合外力 由重力和4个旋翼产生的推力叠加而成：

则有

其中，。

合力矩 由推力力矩和****4个电机给机身的反扭矩叠加而成：

其中

解得

又因为欧拉角角速度与刚体坐标系下的角速度有如下的关系：

当无人机处于平衡状态的时候，可以认为

则式变为

联立式，则可得线性化后的模型：

得到线性化的模型后，我们将该模型看成6个子系统，有3个关于位置的子系统，有3个关于姿态的子系统，由于在我们的产品中没有水平位置的传感器信息，因此这里只考虑高度子系统和3个关于姿态的子系统。则有以下4个状态空间表达式：

a) 升降运动子系统：

b) 横滚运动子系统：

c) 俯仰运动子系统：

d) 偏航运动子系统：

在获取到上述4个子系统后，就可以来设计LQR的飞行器系统控制器了。主要注意的是这里整个系统实际输入是4个电机的升力，其中桨叶和电机转速会有以下的关系：

通过上述两个式子，系统的实际输入可以表示为

即

综上分析，最终的C语言实现代码如下：

//状态反馈系数 
static const float k11 = 0.6276f; 
static const float k12 = 0.4117f; 
static const float k21 = 0.0521f; 
static const float k22 = 0.0214f; 
static const float k31 = 0.0521f; 
static const float k32 = 0.0214f; 
static const float k41 = 0.0105f; 
static const float k42 = 0.0045f; 
... 
// 姿态控制器（包含了高度控制） 
static void Attitude_Controller(const ATTITUDE_DATA_t* attitude, const IMU_DATA_t* imudata, const float Height, const float Height_dot, float motorf[4][1]) 
{ 
    float u[4][1] = {0}; // 系统的输入量 
    float T[4][4] = 
                    {{0.25f, -3.9284f, -3.9284f,  50.2828f}, 
                     {0.25f,  3.9284f, -3.9284f, -50.2828f}, 
                     {0.25f,  3.9284f,  3.9284f,  50.2828f}, 
                     {0.25f, -3.9284f,  3.9284f, -50.2828f}}; 
... 
    // LQR控制器 
u[0][0] = -(k11 * (Height - FlyControl_height) + k12 * Height_dot) + height_to_weight(Height) + FlyControl_unuseHeight; 
u[1][0] = -(k21 * (attitude -> roll  + FlyControl_roll  - roll_ref ) + k22 * imudata -> gyro.x); 
u[2][0] = -(k31 * (attitude -> pitch + FlyControl_pitch - pitch_ref) + k32 * imudata -> gyro.y); 
u[3][0] = -(k41 * (attitude -> yaw - FlyControl_yaw ) + k42 * (imudata -> gyro.z + FlyControl_gyroz)); 

    // 系统的输入转换为4个电机的升力 
    mul(4, 4, 4, 1, (const float (*)[4])T, (const float (*)[1])u, motorf); 
} 
...