浅谈四轴飞行器控制—四元数姿态控制

四元数被广泛应用于四轴飞行器的姿态解算与姿态控制中，跟欧拉角相比具有许多优点，如计算量小，且不会有万像锁的问题 。

---

四元数可以表示如下：

$$q=\begin{bmatrix} w & x & y & z \end{bmatrix}=\cos(\theta)+\hat u\sin(\theta)$$
其中$\hat u=\begin{bmatrix}u_x&u_y&u_z\end{bmatrix}$表示旋转轴，$\theta$表示绕旋转轴逆时针旋转的角度。对应的，四元数的各个参数可以表示如下：
$$\begin{align} w&=\cos(\frac{\theta}{2})\\ x&=u_x \sin(\frac{\theta}{2})\\ y&=u_y \sin(\frac{\theta}{2})\\ z&=u_z \sin(\frac{\theta}{2})\\\\ \end{align}$$
设当前姿态四元数为$q_c$，目标姿态四元数为$q_t$，从当前姿态旋转到目标姿态对应的姿态差四元数为$q_d$. 根据四元数的乘法，我们先做当前四元数的旋转，再做姿态差旋转即可得到目标姿态四元数，即：
$$\begin{align} (q_d q_c)\hat v(q_d q_c)^*&=q_t\hat v q_t^*\\ \Leftrightarrow q_d q_c&=q_t\\ \Leftrightarrow q_d q_c q_c^{-1}&=q_t q_c^{-1}\\ \Leftrightarrow q_d&=q_t q_c^{-1}\\ \overset{|q_c|^2=1}{\Longleftrightarrow}q_d&=q_t q_c^* \end{align}$$
得到姿态差$q_d=\begin{bmatrix}w_d&x_d&y_d&z_d\end{bmatrix}$后，根据四元数所表示的旋转意义，可以知道$x_d$, $y_d$, $z_d$分别表示各旋转轴分量与转角$\sin(\frac{\theta}{2})$的乘积. 所以我们可以近似用$x_d$, $y_d$, $z_d$来表示绕各个轴旋转的姿态误差，作为PID的误差输入进行控制.