四元数被广泛应用于四轴飞行器的姿态解算与姿态控制中,跟欧拉角相比具有许多优点,如计算量小,且不会有万像锁的问题 。
四元数可以表示如下:
q=[wxyz]=cos(θ)+u^sin(θ)
其中 u^=[uxuyuz] 表示旋转轴, θ 表示绕旋转轴逆时针旋转的角度。对应的,四元数的各个参数可以表示如下:
wxyz=cos(θ2)=uxsin(θ2)=uysin(θ2)=uzsin(θ2)
设当前姿态四元数为 qc ,目标姿态四元数为 qt ,从当前姿态旋转到目标姿态对应的姿态差四元数为 qd . 根据四元数的乘法,我们先做当前四元数的旋转,再做姿态差旋转即可得到目标姿态四元数,即:
(qdqc)v^(qdqc)∗⇔qdqc⇔qdqcq−1c⇔qd⟺|qc|2=1qd=qtv^q∗t=qt=qtq−1c=qtq−1c=qtq∗c
得到姿态差 qd=[wdxdydzd] 后,根据四元数所表示的旋转意义,可以知道 xd , yd , zd 分别表示各旋转轴分量与转角 sin(θ2) 的乘积. 所以我们可以近似用 xd , yd , zd 来表示绕各个轴旋转的姿态误差,作为PID的误差输入进行控制.