一、问题来源
至于为什么要做这个仿真，其实我是想通过Unity来测试一下PID算法是如何控制四旋翼飞行器的控制姿态的。在此之前，我也想过直接做实物，然后去实现PID算法，但是实现硬件平台要涉及很多方面的知识。比如，电路设计、信号处理、结构设计、无线通讯、芯片测试、PCB板设计等等，考虑到物质成本和时间成本，以及我在模拟仿真世界摸爬滚打这么多年，我还是用模拟仿真实现吧。
其实，一开始我是想做硬件的。每一步的计划我都列出来了，其中包含要学什么知识、买什么硬件、成本预算多少和要做出什么效果等等。我实现的两个基本任务是：一是硬件IIC通讯，二是MPU6050数据解算及其与PC通讯。在此期间，我也遇到挺多困难。我印象最深刻的是在硬件IIC通讯的测试过程中，通讯过程完全按照时序图编程，但是数据不传输，于是我去找别人做过的案例，经过细心分析代码，其中的某个重启信号需要重新开启才可以进行通讯。其他还有很多是因为细节和测试问题上花费大量的时间。逐渐地，我意识到，我不能再这样进行下去了，否则明年都搞不明白PID算法怎么应用在四旋翼飞行器上。
于是，自然而然我就先想到模拟仿真，这样想法就出来了，这是很自然的一个过程。
二、模型搭建
1、三维模型
模型的搭建是关键的步骤。参考相关书籍《DIY四轴飞行器》-黄和悦、《四旋翼飞行器快速上手》-陈志旺等等。首先我们要明白的是电机通过驱动螺旋桨产生升力，同时产生反扭矩作用在机架上，反扭矩转向与螺旋桨转向相反。中间的部分是机架、控制电路和电源的整合结构，设计的时候注意质心尽量在最中间。
2、传感器模型
四旋翼飞行器的主要用到的传感器是陀螺仪、加速度计、磁强计、气压计。陀螺仪用来检测飞行器的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）对应的角速度大小，初始坐标系定好后，角速度与时间相乘就可以获得以上三个角度。加速度计用来检测机架坐标系的三个坐标轴的加速度，主要用来检测飞行器的受力状况。磁强计是相当于指南针，让飞行器分清南北，从而让飞行器知道机体坐标系与大地坐标系的偏差，有利于偏航角的调整。气压计则是用来测量飞行器与地面的高度，使得飞行器在指定的空中高度悬浮。
在Unity中，角速度可以直接获取，相当于有了陀螺仪传感器；相对于地面的高度可以获取，相当于有了气压计传感器；地球坐标系可以直接获取，相当于有了磁强计；但是加速度获取不了，需要编程实现，获得加速度有两种办法：一是通过求合力，然后比上质量获得加速度；二是对速度求导。我比较推荐第一种方法。
三、运动学分析
飞行器在空中的运动有：垂直起落、悬停、俯仰运动、横滚运动、偏航运动，这些运动都是通过调节四个电机的转速实现。注意以下的分析都是在没有干扰且质心在中心的情况下进行的。
1、垂直起落和悬停
四旋翼飞行器的四个螺旋桨的旋转方向如图，这是为了相互抵消反扭矩而设计的。当飞行器要垂直上升或者下落时，只需要控制四个电机的油门同时上升或者下降即可。实现悬停则要求四个螺旋桨产生的升力等于自身的重力。
2、俯仰Pitch运动（前后运动）
在悬停的油门下，1,2转速减少，3,4转速增大，即可向前运动，反之，则向后运动。
3、横滚Roll运动（左右运动）
在悬停的油门下，2,3转速增大，1,4转速减少，则向右运动，反之，则向左运动。
4、偏航Yaw运动（左旋右旋运动）
在悬停的油门下，增大1,3转速，降低2,4转速，由于1,3产生的反扭矩大于2,4产生的反扭矩，飞行器会逆时针旋转，即左旋运动；反之，则右旋运动。
四、欧拉角与四元数
这里做一下欧拉角和四元数的简单介绍，以及他们之间的相互转换。
1、欧拉角
这有两个坐标系，一个是地球坐标系，另一个是自身坐标系。首先机体绕着地球坐标系的z轴旋转ψ角，然后绕着自身坐标系的y轴旋转θ角，在绕着自身坐标系的x轴旋转 φ角。详细可参考：https://www.zhihu.com/question/47736315
2、四元数
可以简单地理解为将平面复数的虚部再次拆分为三个部分，四元数可以表达更高维的空间变换。
五、卡尔曼滤波
我们通过传感器测得的信号，总会掺杂噪声，如果不进行处理，会使得系统的控制变得不稳定，或者出现较小波动就过度响应，导致系统振荡抖动明显。因此，我们要滤波，即从混合的信号中提取有用的信号。
详细可参考：
【1】李永乐老师关于卡尔曼滤波的原理讲解
https://www.douyin.com/video/6943106434418707743?previous_page=video_detail%E2%80%8B
【2】https://mp.weixin.qq.com/s/pedsiTnePumAY4CWZT4XGQ
六、PID算法
1、系统框图
2、公式
注意输入的可以是角度，速度等，输出量就可以直接作为控制量的升力大小。
详细可参考：https://blog.csdn.net/mjy520123/article/details/116565745
七、PID算法代码
public class PID
{
private float setPoint; //设置点
private float measuredPoint; //测量点
private float integral;//积分器
private float previous_error; //上一次的误差
private float now_error; //本次误差
private float Kp;
private float Ki;
private float Kd;
private float minLimit; //输出量的下限
private float maxLimit; //输出量的上限
//构造方法，用来初始化对象
public PID(float Kp, float Ki, float Kd, float minLimit, float maxLimit)
{
//将初始化的数据公开到对象上，以便各个方法使用
this.Kp = Kp;
this.Ki = Ki;
this.Kd = Kd;
this.minLimit = minLimit;
this.maxLimit = maxLimit;
}
//获取更新数据
public void RenewCalculData(float setPoint, float measuredPoint)
{
this.setPoint = setPoint;
this.measuredPoint = measuredPoint;
}
//PID算法核心计算
public float Calcul()
{
now_error = setPoint - measuredPoint; //计算得到偏差
integral += now_error;
float Output = Kp * now_error + Ki * integral + Kd * (now_error - previous_error);//位置式PID控制器
Output = Mathf.Clamp(Output, this.minLimit, this.maxLimit);
previous_error = now_error;
return Output;
}
}
八、效果展示
九、改进
1、增加传感器模拟代码
2、增加卡尔曼滤波算法
3、使用串联PID算法实现更好的控制
项目资料：在本博客的资源里，名叫:Rotor_Aircraft
