多旋翼PID控制器笔记

高度控制

垂向速度环

垂向速度指令在NED坐标系下给出，采用PI+前馈控制，输出垂向加速度指令。速度指令和前馈加速度可由参考模型给出。对I项，需要进行限幅和抗饱和。根据设计要求对最终加速度输出进行限幅。

为了加快响应速度，可以加入D项，或者引入加速度环。

加速度指令到油门指令的映射

垂向速度环给出了NED坐标系下z轴加速度指令（可认为是惯性加速度），多旋翼拉力方向一般在机体系下z轴方向，加速度计输出值为比力在机体系下的分量坐标，为了方便，首先需要将加速度指令转换到机体系z轴所需的加速度指令，即考虑到机体的倾斜，维持竖直方向加速度所需的机体z轴的加速度。
这里，仅考虑了机体倾斜的影响，忽略了水平加速度指令的影响，即垂向与水平进行了解耦。也可以按照转换矩阵对全局加速度指令进行理论上的转换。
得到机体系下的期望加速度后，根据多旋翼的质量，可得出期望拉力，再通过动力系统（电机螺旋桨）油门输入与拉力的映射关系（可通过电机螺旋桨拉力测试得出），可以得到无量纲拉力指令（0-1油门）。
由期望加速度得到的油门只是在多旋翼悬停状态的油门增量，而维持多旋翼悬停的“基础油门”，是对抗自身重力所需的。因此，想要得出最终的油门指令，需要将加速度指令转换为比力指令。

比力$\mathbf{f}$(Specific force)：作用在单位质量物体上的非引力之外力。加速度计的测量值即为比力。
$${\mathbf{a}}_i=\mathbf{f}+\mathbf{\mu }$$
其中，${\mathbf{a}}_i$为惯性系下绝对加速度，$\mathbf{\mu }$为万有引力加速度，在NED坐标系下可近似表示为 { 0   0   g } T \left \{0\: 0\:g\right \}^T { 00g}T。

比力指令可以直接映射到油门指令，简单粗暴。

垂向加速度环

因为加速度计可以测量出机体加速度，为了得到更好的控制效果，可以增加垂向加速度环。垂向加速度环采用PI+前馈控制，输出无量纲多旋翼总体拉力油门指令。
比力误差可对应油门增量，油门增量经过积分可得到总油门，这是增量控制。加速度环的P项，其实相当于速度环中D项，也可以加快响应速度。前馈的作用是加速度指令的直接映射，加快响应速度。
如果油门与拉力的映射能准确测量，比例和前馈项也可去掉，算是增量动态逆原理。但目前油门和拉力的映射仅仅是在静态情况测量，动态或复杂气流环境误差可能较大。（by 鞠总）

水平控制

水平速度环

水平速度的控制在机头水平坐标系（C系）下进行，NED坐标系下的速度指令可以通过偏航角进行转换。

由NED系到C系的转换矩阵
$$M_O^C=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

水平速度环同样采用PI+前馈控制，输出垂向加速度指令。前馈指令可由参考模型给出。

最后需要根据允许最大倾角对加速度指令的输出进行限幅，允许最大水平加速度可由允许最大倾角得出。
$$AC{C}_{max}=\genfrac{}{}{0ex}{}{}{\tan (TiltAngl{e}_{max})}$$