四旋翼无人机位置环PID期望姿态输出公式推导

令$\theta =pitch,\gamma =roll,\psi =yaw$，有
$$\left[\begin{array}{c}{F}_x\\ F_y\\ F_z\end{array}\right]={\mathbf{C}}_b^n\left[\begin{array}{c}0\\ 0\\ f\end{array}\right]=\left[\begin{array}{c}\sin \gamma \cos \psi -\cos \gamma \sin \psi \sin \theta \\ -\sin \gamma \sin \psi -\cos \gamma \cos \psi \sin \theta \\ \cos \gamma \cos \theta \end{array}\right]f$$
其中$f$为无人机$b$系下的总拉力，$F_x,F_y,F_z$为东北天坐标系下对应的期望力，欧拉角旋转顺序为Z-X-Y，航向角北偏东为正，范围0~360°。

期望拉力(高度环)

观察上式可得出，期望拉力
$$f_d=\frac{F_z}{\cos \gamma \cos \theta }$$

期望俯仰角

对公式(1)进行稍微变换，$F_x\sin \psi +F_{y}cos\psi$，得
$$\begin{array}{rl}{F}_x\sin \psi +F_{y}cos\psi & =f(\sin \gamma \cos \psi \sin \psi -\cos \gamma \sin \psi \sin \theta \sin \psi -\sin \gamma \sin \psi \cos \psi -\cos \gamma \cos \psi \sin \theta \cos \psi )\\ & =-(\cos \gamma \sin \theta )f\\ & =-(\cos \gamma \sin \theta )\frac{F_z}{\cos \gamma \cos \theta }\\ & =-\tan \theta F_z\end{array}$$

期望俯仰角${\theta }_d$为
$${\theta }_d=\arctan (-\frac{F_x\sin \psi +F_{y}cos\psi }{F_z})$$

期望横滚角

对公式(1)进行稍微变换，$F_x\cos \psi -F_{y}sin\psi$，得
$$\begin{array}{rl}{F}_x\cos \psi -F_{y}sin\psi & =f(\sin \gamma \cos \psi \cos \psi -\cos \gamma \sin \psi \sin \theta \cos \psi +\sin \gamma \sin \psi \sin \psi +\cos \gamma \cos \psi \sin \theta \sin \psi )\\ & =(\sin \gamma )f\\ & =\sin \gamma \frac{F_z}{\cos \gamma \cos \theta }\\ & =\tan \gamma \frac{F_z}{\cos \theta }\end{array}$$
期望横滚角${\gamma }_d$为
$${\gamma }_d=\arctan (\cos \theta \frac{F_x\cos \psi -F_{y}sin\psi }{F_z})$$