多旋翼PID控制器笔记

高度控制

垂向速度环

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

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

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

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

比力 f \boldsymbol{f} f(Specific force)：作用在单位质量物体上的非引力之外力。加速度计的测量值即为比力。
a i = f + μ \boldsymbol{a}_i=\boldsymbol{f}+\boldsymbol{\mu } ai​=f+μ
其中， a i \boldsymbol{a}_i ai​为惯性系下绝对加速度， μ \boldsymbol{\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 = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] M_O^C = \begin{bmatrix} \cos\psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} MOC​=⎣⎡​cosψ−sinψ0​sinψcosψ0​001​⎦⎤​

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

最后需要根据允许最大倾角对加速度指令的输出进行限幅，允许最大水平加速度可由允许最大倾角得出。
A C C m a x = g tan ⁡ ( T i l t A n g l e m a x ) ACC_{max}=\frac{g}{\tan(TiltAngle_{max})} ACCmax​=tan(TiltAnglemax​)g​

水平加速度到姿态的映射

多旋翼水平方向的加速度由机身倾斜产生，滚转角产生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顺序，因此需要先求俯仰角，然后求解滚转角。
θ = tan ⁡ − 1 − A C C x g \theta=\tan^{-1}\frac{-ACC_x}{g} θ=tan−1g−ACCx​​
ϕ = tan ⁡ − 1 A C C y g / cos ⁡ θ \phi=\tan^{-1}\frac{ACC_y}{g/\cos\theta} ϕ=tan−1g/cosθACCy​​

姿态环

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

欧拉运动学方程

姿态环得出的是欧拉角变化率指令，方便起见，需要转换到机体坐标系下进行角速度控制。根据欧拉运动学方程，机体角速度与欧拉角变化率与关系可以表示为
[ p q r ] = [ 1 0 − sin ⁡ θ 0 cos ⁡ ϕ sin ⁡ ϕ cos ⁡ θ 0 − sin ⁡ ϕ cos ⁡ ϕ cos ⁡ θ ] [ ϕ ˙ θ ˙ ψ ˙ ] \begin{bmatrix} p\\ q\\ r \end{bmatrix} =\begin{bmatrix} 1 & 0 & -\sin\theta\\ 0 & \cos\phi & \sin\phi\cos\theta\\ 0 & -\sin\phi & \cos\phi\cos\theta \end{bmatrix} \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix} ⎣⎡​pqr​⎦⎤​=⎣⎡​100​0cosϕ−sinϕ​−sinθsinϕcosθcosϕcosθ​⎦⎤​⎣⎡​ϕ˙​θ˙ψ˙​​⎦⎤​

角速度环

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

欧拉动力学方程

刚体绕定点的转动过程可由欧拉动力学方程描述：
M b = I ω ˙ b + ω b × ( I ω b ) M^b=I\dot{\omega}^b+\omega^b\times(I\omega^b) Mb=Iω˙b+ωb×(Iωb)
其中， ω b \omega^b ωb为机体系相对惯性系转动的角速度，可由陀螺仪测量出。 I I I为刚体惯量矩阵，可表示为
I = [ I x x − I x y − I x z − I y x I y y − I y z − I z x − I z y I z z ] I=\begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz}\\ -I_{yx} & I_{yy} & -I_{yz}\\ -I_{zx} & -I_{zy} & I_{zz} \end{bmatrix} I=⎣⎡​Ixx​−Iyx​−Izx​​−Ixy​Iyy​−Izy​​−Ixz​−Iyz​Izz​​⎦⎤​
对于结构上关于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​}。
对应欧拉动力学方程可表示为
[ L M N ] = [ I x x ω ˙ x b + ( I z z − I y y ) ω y b ω z b I y y ω ˙ y b + ( I x x − I z z ) ω z b ω x b I z z ω ˙ z b + ( I y y − I x x ) ω x b ω y b ] \begin{bmatrix} L\\ M\\ N \end{bmatrix}=\begin{bmatrix} I_{xx}\dot{\omega}^b_x+(I_{zz}-I_{yy})\omega^b_y\omega^b_z\\ I_{yy}\dot{\omega}^b_y+(I_{xx}-I_{zz})\omega^b_z\omega^b_x\\ I_{zz}\dot{\omega}^b_z+(I_{yy}-I_{xx})\omega^b_x\omega^b_y \end{bmatrix} ⎣⎡​LMN​⎦⎤​=⎣⎡​Ixx​ω˙xb​+(Izz​−Iyy​)ωyb​ωzb​Iyy​ω˙yb​+(Ixx​−Izz​)ωzb​ωxb​Izz​ω˙zb​+(Iyy​−Ixx​)ωxb​ωyb​​⎦⎤​
参考 刚体质量分布与牛顿-欧拉方程
由于目前暂无法方便测量角加速度，因此不加角加速度环。由角速度环得出角加速度指令，经过欧拉动力学方程，得出力矩指令，力矩指令除以最大拉力，进入控制分配后与力臂参数运算，即可得出所需无量纲油门指令。