姿态和位置，四旋翼的控制流程



姿态和位置计算：EKF

位置控制：PID
姿态控制：姿态环是直接P控制，姿态率是PID控制。

主要是滤波算法和姿态算法还有PID算法。 滤波算法主要是将获取到的陀螺仪和加速度计的数据进行去噪声及融合，得出正确的角度数据。主要采用互补滤波或者高大上的卡尔曼滤波。
姿态算法是将获得的滤波后的传感器数据计算得出飞行器自身坐标系与地理空间坐标系的偏差，即欧拉角。 一般采用四元数算法。
PID算法就是用来控制四个电机的转速来纠正欧拉角，从而使机身保持平稳。

无人机或者飞行器的控制器叫做飞控，了解这种控制方法不但是其中的算法还包括控制对象，控制器等等。飞控作为一个体系来理解更容易入手。
<img src="https://i-blog.csdnimg.cn/blog_migrate/f63d214dd88fdd8ac074ee2dcc4b7019.png" data-rawwidth="300" data-rawheight="73" class="content_image" width="300">传感器构成控制回路的闭环；遥控器给过来的信号可以做为控制目标，这时只涉及到油门控制也就是油门曲线和姿态之间的控制问题。但如果将遥控器或者地面站已经设定好的路径，功能中蕴含的姿态，如定高，特殊油门，自动起降等等，飞控需要自己完全接管姿态控制；输出是为了控制电机转速——针对四旋翼的控制原理就是成对的电机调速： 传感器构成控制回路的闭环；遥控器给过来的信号可以做为控制目标，这时只涉及到油门控制也就是油门曲线和姿态之间的控制问题。但如果将遥控器或者地面站已经设定好的路径，功能中蕴含的姿态，如定高，特殊油门，自动起降等等，飞控需要自己完全接管姿态控制；输出是为了控制电机转速——针对四旋翼的控制原理就是成对的电机调速：
<img src="https://i-blog.csdnimg.cn/blog_migrate/fdce56aa82a41216d864fc472f800fb5.png" data-rawwidth="300" data-rawheight="286" class="content_image" width="300">
飞控当中常用的算法暂时还是PID，智能PID如模糊PID，也有采用LQR，变结构控制器等等。控制系统结构上面一般采用双闭环的形式，分出姿态换和位置换进行控制。
姿态算法提供控制所需要的闭环回路，姿态的获取有几种途径：陀螺仪积分，加速度计+强磁计，陀螺仪与加速度计强磁计的数据融合（滤波）。
姿态的标示方式有欧拉角（也叫姿态角），四元数，指数形式，矩阵形式等等。欧拉角毕竟是非全局，非唯一的。四元数是全局标示姿态，但不唯一。
至于原因，我把自己总结的公式贴在下面：
<img src="https://i-blog.csdnimg.cn/blog_migrate/876632af27aaca274acb758c906c9499.png" data-rawwidth="392" data-rawheight="282" class="content_image" width="392"> <img src="https://i-blog.csdnimg.cn/blog_migrate/0bbfab848e56fc9afc394dfaa328add6.png" data-rawwidth="556" data-rawheight="266" class="origin_image zh-lightbox-thumb" width="556" data-original="https://pic2.zhimg.com/6f58e9ab90d71f50244ce64df0b73191_r.png">四元数也好，欧拉角也好都有比较固定的工程计算方法。 四元数也好，欧拉角也好都有比较固定的工程计算方法。

一个旋翼类无人机系统的算法主要有两类：姿态检测算法、姿态控制算法。

姿态控制、被控对象（即四旋翼无人机）、姿态检测三个部分构成一个闭环控制系统。被控对象的模型是由其物理系统决定，我们设计无人机的算法就是设计姿态控制算法、姿态检测算法。

1:姿态检测算法：姿态的三个自由度可以用欧拉角表示，也可以用四元数表示。姿态检测算法的作用就是将加速度计、陀螺仪等传感器的测量值解算成姿态，进而作为系统的反馈量。常用的姿态检测算法有卡尔曼滤波、互补滤波等。

2:姿态控制算法：控制飞行器姿态的三个自由度，以给定姿态与姿态检测算法得出的姿态偏差作为输入，被控对象模型的输入量作为输出（例如姿态增量），从而达到控制飞行器姿态的作用。最常用的就是PID控制及其各种PID扩展（分段、模糊等）、高端点的有自适应控制。当然，姿态控制算法里面又常用角速度、角度双闭环控制，所以常常有PD外环+PID内环等等，这些细节就不说了。

这俩算法搞清楚，再指导无刷电机的控制方法，基本就能搞清飞控的组成了。云台控制也是同理。

四旋翼的控制流程：控制中心单片机通过IMU陀螺仪加速度计（MPU6050等等）获取四旋翼的角度（俯仰、横滚和偏航）的相对基准角度变化、然后滤波（卡尔曼滤波等等）处理获得方向余弦矩阵和四元数得到欧拉角、使用PID控制或者PI,PD控制（P比例I积分D微分）将系统反馈值和期望值进行比较、并根据偏差不断修复、直至达到期望的预定值。P的作用是加快系统达到预期的速度；I的作用是消除净差；D有阻尼的作用、就是阻止系统突变。
通过PID自动控制算法处理、输出期望的PWM波给四个电调、控制四个无刷电机的转速、从而得到一个期望的力控制四旋翼的前后左右上下飞行。

自稳(stabilise)模式下：
没有position and velocity estimator，只用根据imu得到的偏角对马达转速进行调整已达到自稳。偏转补偿角度控制器为pi，角度加速度控制器为pid。

定点(loiter)模式下：
ekf根据gps，imu，sonar（如果够低），optical flow sensor（如果有）的数据计算出现在的位置。然后可以得到现在位置与desired position的差值，位置控制器有一个pid控制偏转角，然后再到角加速度的pid控制。

. 三角函数直接解算欧拉角+互补滤波+单级PID版本
效果：简单暴力，但是补滤波效果差，单级PID响应慢，打舵跟随效应差。
2. 三角函数直接解算欧拉角+卡尔曼滤波+单级PID版本
效果：卡尔曼滤波噪声偏大，滞后略微严重，单级PID难操作，打舵响应慢，跟随效应差。不过比较适合初学四轴的人，难度比四元数加串级PID版本低，易于理解。
3. 四元数姿态解算+互补滤波（德国开源四轴）+串级PID版本
效果：四元数难理解，基于PI控制的互补滤波不适合非专业人员，PID参数较单级PID参数难调。打舵响应极佳，稳定程度高，易于操作，是目前四轴的主流算法