多旋翼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}=\frac{g}{\tan (TiltAngl{e}_{max})}$$

水平加速度到姿态的映射

多旋翼水平方向的加速度由机身倾斜产生，滚转角产生y方向的加速度，俯仰角产生x负方向的加速度。
假设多旋翼竖直方向处于悬停状态，则 { − A C C x   − A C C y    g } T \left \{-ACC_x\: -ACC_y\:\:g\right \}^T {−ACCx​−ACCy​g}T矢量与机体系z轴指向相同，求解机体系z轴的欧拉角，即可得到相应期望滚转、俯仰角。示意图如下：

欧拉角的产生顺序，一般采用z-y-x顺序，因此需要先求俯仰角，然后求解滚转角。
$$\theta ={\tan }^{-1}\frac{-AC{C}_x}{g}$$
$$\varphi ={\tan }^{-1}\frac{AC{C}_y}{g/\cos \theta }$$

姿态环

由期望水平加速度求出期望欧拉角后，姿态环采用P控制，输出欧拉角变化率指令。

欧拉运动学方程

姿态环得出的是欧拉角变化率指令，方便起见，需要转换到机体坐标系下进行角速度控制。根据欧拉运动学方程，机体角速度与欧拉角变化率与关系可以表示为
$$\left[\begin{array}{c}p\\ q\\ r\end{array}\right]=\left[\begin{array}{ccc}1& 0& -\sin \theta \\ 0& \cos \varphi & \sin \varphi \cos \theta \\ 0& -\sin \varphi & \cos \varphi \cos \theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

角速度环

角速度环采用PID控制，输出无量纲力矩指令。

欧拉动力学方程

刚体绕定点的转动过程可由欧拉动力学方程描述：
$$M^b=I{\dot{\omega }}^b+{\omega }^b\times (I{\omega }^b)$$
其中，${\omega }^b$为机体系相对惯性系转动的角速度，可由陀螺仪测量出。$I$为刚体惯量矩阵，可表示为
$$I=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{yx}& I_{yy}& -I_{yz}\\ -I_{zx}& -I_{zy}& I_{zz}\end{array}\right]$$
对于结构上关于x-z、y-z平面对称的多旋翼，惯量矩阵可以简化为对角阵，即 I = diag { I x x   I y y   I z z } I=\textup{diag}\{I_{xx}\:I_{yy}\:I_{zz}\} I=diag{Ixx​Iyy​Izz​}。
对应欧拉动力学方程可表示为
$$\left[\begin{array}{c}L\\ M\\ N\end{array}\right]=\left[\begin{array}{c}{I}_{xx}{\dot{\omega }}_x^b+(I_{zz}-I_{yy}){\omega }_y^b{\omega }_z^b\\ I_{yy}{\dot{\omega }}_y^b+(I_{xx}-I_{zz}){\omega }_z^b{\omega }_x^b\\ I_{zz}{\dot{\omega }}_z^b+(I_{yy}-I_{xx}){\omega }_x^b{\omega }_y^b\end{array}\right]$$
参考 刚体质量分布与牛顿-欧拉方程
由于目前暂无法方便测量角加速度，因此不加角加速度环。由角速度环得出角加速度指令，经过欧拉动力学方程，得出力矩指令，力矩指令除以最大拉力，进入控制分配后与力臂参数运算，即可得出所需无量纲油门指令。