先mark一下，争取两周之内写完
由于快要毕业了，所以一直都很忙，并没有时间写完，所以拖到现在，不好意思。现在开始介绍四旋翼定高以及调试过程。
首先介绍下四旋翼的模型
图中为 四旋翼的升力,并且满足
，
。所以
其中，为螺旋桨升力系数，
为螺旋桨转速，则又动力学方程知
其中X即为z轴位移距离，则推出四旋翼的模型
显然，系统输出z轴加速度与系统的输入u成正比关系，因此采用PID线性控制器来作为z轴方向上的高度控制器。
假设在没有任何外部观测传感器的情况下，仅有惯性器件估计的位置和速度书算法如下图所示。
上述传递函数为：
特征方程跟为：。因此这种纯惯性通道是不稳定的，而引起系统发散的根本原因是系统无阻尼，所以，要在惯性通道中引入观测量使得系统具有阻尼。所以博主采用经典回路反馈法设计四轴位置估计算法。
高度信息的观测器件一般有：超声波传感器、气压计。
定高算法实现
博主第一次定高用的传感器为超声波传感器，总体感觉超声波传感器做好极值滤波（突然大范围跳动）后，定高效果相当好，只是定高的范围有限制，博主超声波定高适用范围为5cm-250cm。
第二次定高采用的传感器为MS5611气压计，效果比较好，高度范围也提升了不少，但是对气流很敏感，所以需要把气压计密封好，不要让螺旋桨的气流对其造成错误影响。
所以超声波定高导航算法采用 超声波融合惯导的算法。总体算法框图如下：
其中，分别为速度和加速度的反馈系数，所以上述的特征多项式为：
由典型二级系统的特征多项式的标准形式：，可以得到
从而选择合适的系数。
控制器由之前推导的四轴模型而选择采用PID 线性控制器作为z轴方向上的高度控制器。选取高度-速度-加速度三级PID控制器方案用于z轴高速控制器，高度控制系统框图如下，
以上即为，四旋翼定高。
四轴定高调试现象（博主调试用的高度-速度串级PID 控制，没有用加速度）
1.只有外环高度控制器时
只有比例控制时，由四轴模型可以等效为另一个熟悉系统：光滑平面上用力F 推小滑块来控制距离。
所以现象为：高度控制不稳定，频繁上下加减速（最理想是在给定悬停点上下等幅振荡）
比例积分控制时，有了积分来消除静差，四轴定高现象为衰减振荡。
一般不加微分。
2.只有内环速度控制器时
只有比例控制时,那么四轴定高控制目标就是让四轴的z轴速度为0，不会控制四轴的高度
所以现象为：1.四轴开启定高时，若z轴速度为0，则稳定不动，定高高度准确无误差
2.四轴开启定高是，若z轴速度<0，那么控制器输出>0，四轴向上飞（做一个加速度减小的减速运动），直至四轴z轴速度为0，但是最终稳定高度会比期望高度高，既高度有误差
3.四轴开启定高时，若z轴速度>0，那么控制器输出<0，四轴向下飞（做一个加速度减小的减速运动）， 直至四轴z轴速度为0，但是最终稳定高度会比期望高度低，既高度有误差
加入积分控制，既可得到与期望位置的误差，从而消除了高度误差，可以稳定准确定高
9103
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
