姿态解算
作者：互联网
为了能够控制四轴飞行器的飞行，我们需要建立坐标系对四轴飞行器的姿态进行描述。主流的姿态解算算法包括欧拉角以及四元数。接下来详细介绍如何对四轴飞行器的姿态以及姿态变化进行描述。
坐标系的建立
在建立三维坐标系时，无非有两种模式，一种是以地面或者说地球为参考系，另一种则是以自己为参考系。这两种参考系也引申出了四轴飞行器的两种飞行模式，有头模式和无头模式。
当我们以四轴飞行器自身为参考系，如果推动遥控器控制飞行器向前飞行，飞行器总是会向着机头方向前进，如果我们在飞行过程中改变了机头的朝向（操纵了遥控器改变了航向角），飞行器就会依照机头的变化向着机头方向飞行。有头模式有利于航拍等情况，常用于玩具航模，大多旋翼飞行器。
当我们以地球作为参考系，不论飞行器的机头方向怎么变化，飞行器始终认定起飞时的机头方向为前方。无头模式下飞行器可以通过磁力计对飞行器相对地球磁场的角度进行测量，从而计算出机头在磁场中的方向。无头模式常用在四轴或者六轴飞行器中（这些飞行器确实分不出来哪里是机头，采用无头模式也很合理）。
我们的四轴飞行器采用的是无头模式，使用的参考坐标系是当地的水平坐标系，称为地理坐标系；其自身的相对坐标系称为载体坐标系。我们这里使用常见的NED坐标系作为地理坐标系，如下图所示。
飞行姿态与电机布局
四轴飞行器一共有两组，四个电机呈十字状排列。每组电机（对角的电机为一组）的轴距相等，且两组电机的几何中心重合，使得四轴飞行器在产生向上升力时能够保证力矩的平衡，飞行器不会朝向任何一个方向倾斜。当两组电机转动方向相反时，可以保证飞行器在竖直方向上的反扭距保持平衡，保证了四轴飞行器航向的稳定。四轴飞行器各个旋翼对机身施加的反扭距与旋翼的旋转方向相反，所以当处于对角的两个电机旋转方向相同，不在同一对角的任意两个电机旋转方向相反时，可以平衡旋翼对机身的反扭距。
因为四轴飞行器结构的对称性，衍生出两种飞行姿态，分别是+型和x型 ，如下图所示。x型的机头方向位于两个电机之间，而+型的机头方向位于某个电机之上。相对而言，+型更加简单，因为能够明确头尾，飞行控制做起来也比较直接，而x型更灵活，适合飞特技，因为X型的3轴姿态需要4个电机都调整，即调整的力量更大，可以更加快速。
动力学模型
我们采用欧拉角对四轴飞行器的姿态进行描述，不论采用+型还是x型，我们选取飞机的机头方向作为x轴方向，以竖直向上的方向做为z轴建立右手系，将四轴飞行器沿x轴转动对应的变化角称为翻滚角Roll（记作ϕ），沿y轴转动对应发生变化的角为俯仰角Pitch（记作θ），沿z轴转动发生变化所对应的角度称为偏航角Yaw（记作ψ）。
俯仰角θ则为载体绕Y轴旋转的角度，指向水平面以下为正，指向水平面以上为负，角度范围从−90°至90°；翻滚角ϕ为载体绕XX轴旋转的角度，坐标Y指向水平面以上为正，指向水平面以下为负，角度范围从−180°至180°；而航向角ψ为机体绕ZZ轴旋转的角度，俯视图逆时针为正，顺时针为负。
图 三个角度的旋转示意？
运动的描述，都是在理想情况下进行的，我们假设我们获取到的数据都是正确的且飞行器状态没有受到外界干扰，在实际完成时，需要滤波算法与pid反馈控制协同完成。
垂直运动
垂直运动是最为直接的一种移动了，包括了垂直上升和垂直下降。在不考虑外界干扰的前提下，只需要保证四个电机的转速相同，同时增大至升力大于重力即为上升，同时减小至升力小于重力即为下降，如下图 a 所示。
翻滚
翻滚包括了四轴飞行器俯仰角，横滚角或者偏航角单一的变化导致的飞行器绕任一坐标轴的转动。如下图b所示，只需要保证以翻滚方向为对称轴对应的电机保持转速变化相等，同时翻滚方向同侧的电机的变化相反即可使飞行器发生翻滚运动。图中电机1和电机2以相同幅度减小转速，电机3和电机4对应的增大转速。
水平旋转
水平旋转需要保持四轴飞行器的高度不变，即保持飞行器总体升力不变，但是需要飞行器产生一个力使机体发生旋转。如图c所示，对角线上的两组电机，一组增大转速，一组减小转速可使飞行器产生水平方向的“转动力”。图中电机1和电机3转速增大，电机2和电机4减小相同的转速，使飞行器发生水平旋转。
水平运动
四轴飞行器无法直接进行水平运动，需要使机体产生一个倾角，使垂直方向的升力分量与重力相等，水平方向的升力使飞行器加速前进。如图d所示，电机先采用与翻滚运动相同的策略使机体产生倾角，随后调整电机转速至垂直运动的模式，使飞行器加速前进，到达指定位置后反向翻滚回到稳定的姿态。
欧拉角
欧拉角的定义有很多，我们选择了其中比较常用的一种。在飞行姿态描述部分已经介绍过欧拉角，这里只简单给出俯仰角，偏航角以及横滚角的定义。选取飞机的机头方向作为x轴方向，以竖直向上的方向作为z轴建立右手系，将四轴飞行器沿x轴转动对应的变化角称为翻滚角Roll（记作γ），沿y轴转动对应发生变化的角为俯仰角Pitch（记作θ），沿z轴转动发生变化所对应的角度称为偏航角Yaw（记作ψ）。
空间中的一个基本旋转可以是绕着空间中的一个固定的坐标轴旋转，也可以是一次绕着初始时与地球坐标系对齐的，正在旋转的轴的旋转。考虑到动态旋转轴的情况可以由多次静态旋转轴旋转得到，我们这里只讨论绕静态旋转轴的情况，这种旋转可以被分解为绕着3个坐标轴的3次旋转。因为3维空间只有三个自由度，而我们的分解也存在三个相互独立的参量，所以这种分解是合理的。
俯仰角，横滚角以及偏航角分别对应了3维空间的三个自由度，所有的在三维空间中的一次空间变换都可以用一组欧拉角进行表示（注意这里是一次，欧拉角存在万向锁的问题，后面会详细叙述）。
欧拉角的代数实现
我们需要意识到，空间中的同一次旋转可能存在多种欧拉角表示的方式，所以存在着多种欧拉角的定义：首先选择第一次旋转的转轴，有3种可能，然后分别选择不同于第一次旋转轴的轴进行旋转，有两种可能，最后选择除第二次旋转的转轴进行旋转（可以和第一次选择一样的旋转轴），一共存在12种定义方式。我们这里选择“北东地”的方式，即“Z-Y-X”的方式进行旋转。
第一个旋转角度：偏航角（绕z轴旋转ψ度）
C n 1 = [ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ] C_{n}^{1}=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1\end{array}\right] Cn1=⎣⎡cosψ−sinψ0sinψcosψ0001⎦⎤
第二个旋转角度：俯仰角（绕y轴旋转θ度）
C 1 2 = [ cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ ] C_{1}^{2}=\left[\begin{array}{ccc}\cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta\end{array}\right] C12=⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤
第三个旋转角度：横滚角（绕x轴旋转γ度）
C 2 b = [ 1 0 0 0 cos φ sin γ 0 − sin γ cos γ ] C_{2}^{b}=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \varphi & \sin \gamma \\ 0 & -\sin \gamma & \cos \gamma\end{array}\right] C2b=⎣⎡1000cosφ−sinγ0sinγcosγ⎦⎤
那么空间中的一次旋转可以记作
C n b = C 2 b C 1 2 C n 1 = [ 1 0 0 0 cos γ sin γ 0 − sin γ cos γ ] [ cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ ] [ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ] C_{n}^{b}=C_{2}^{b} C_{1}^{2} C_{n}^{1}=\left[\begin{array}{ccc}1 & 0 & 0 \\ 0 & \cos \gamma & \sin \gamma \\ 0 & -\sin \gamma & \cos \gamma\end{array}\right]\left[\begin{array}{ccc}\cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta\end{array}\right]\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1\end{array}\right] Cnb=C2bC12Cn1=⎣⎡1000cosγ−sinγ0sinγcosγ⎦⎤⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤⎣⎡cosψ−sinψ0sinψcosψ0001⎦⎤
合并，易得
C b n = [ cos θ cos ψ ( − cos γ sin ψ + sin γ sin θ cos ψ ) ( sin γ sin ψ + cos γ sin θ cos ψ ) cos θ sin ψ ( cos γ cos ψ + sin γ sin θ sin ψ ) ( − sin γ cos ψ + cos γ sin θ sin ψ ) − sin θ sin γ cos θ cos γ cos θ ] C_{b}^{n}=\left[\begin{array}{ccc}\cos \theta \cos \psi & \left(\begin{array}{c}-\cos \gamma \sin \psi \\ +\sin \gamma \sin \theta \cos \psi\end{array}\right) & \left(\begin{array}{c}\sin \gamma \sin \psi \\ +\cos \gamma \sin \theta \cos \psi\end{array}\right) \\ \cos \theta \sin \psi & \left(\begin{array}{c}\cos \gamma \cos \psi \\ +\sin \gamma \sin \theta \sin \psi\end{array}\right) & \left(\begin{array}{c}-\sin \gamma \cos \psi \\ +\cos \gamma \sin \theta \sin \psi\end{array}\right) \\ -\sin \quad \theta & \sin \gamma \cos \theta \quad \cos \gamma \cos \theta\end{array}\right] Cbn=⎣⎢⎢⎢⎢⎡cosθcosψcosθsinψ−sinθ(−cosγsinψ+sinγsinθcosψ)(cosγcosψ+sinγsinθsinψ)sinγcosθcosγcosθ(sinγsinψ+cosγsinθcosψ)(−sinγcosψ+cosγsinθsinψ)⎦⎥⎥⎥⎥⎤
记作，
C b n = [ C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 ] C_{b}^{n}=\left[\begin{array}{lll}C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33}\end{array}\right] Cbn=⎣⎡C11C21C31C12C22C32C13C23C33⎦⎤
所以，三个姿态角的计算公式为：
横滚角： γ = a tan 2 ( C 32 , C 33 ) \gamma=a \tan 2\left(C_{32}, C_{33}\right) γ=atan2(C32,C33)
俯仰角： θ = − a sin ( C 31 ) \theta=-a \sin \left(C_{31}\right) θ=−asin(C31)
偏航角： ψ = a tan 2 ( C 12 , C 22 ) \psi=a \tan 2\left(C_{12}, C_{22}\right) ψ=atan2(C12,C22)
欧拉角的代码实现
代码实现如下
/*欧拉角实现代码*/
/*计算roll pitch yaw 欧拉角*/
state[0] = -asinf(rMat[2][0]) * RAD2DEG; //pitch
state[1]= atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG; //roll
state[2]= atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG; //yaw
欧拉角表示姿态的缺陷
欧拉角在表示飞行器姿态时有着直观的优点，我们可以很容易的理解物体在空间中的旋转，但是欧拉角表示飞行器姿态并非完美无缺，他存在着不可避免的奇异性的问题。
下图展示了欧拉角表示姿态的最大问题，在ZYX坐标系中，先以Yaw旋转90度，再以Pitch旋转90度，此时，空间中的第三个“自由度”Roll会和初始的Yaw完全重合，即，本次旋转失去了一个自由度。在实际应用中，欧拉角也不能用于在变动幅度过大时的四轴飞行器，同时，考虑到三角函数运算的复杂性，我们最终没有选择欧拉角对飞行器姿态进行表示。
四元数
欧拉角可以较为简单的描述旋转，但是具有奇异性，这是不可避免的，就好像使用经纬度表示地球上的方位，但是总是存在两个极点使经度失去意义。三维空间存在着三个自由度，想要使用一个三维向量来描述他，同时避免奇异性是不可能的。
在二维空间中，我们使用复数来描述旋转，例如，一个复向量a+bi乘上i相当于把这个向量旋转90度。类似的，在三维空间中，我们使用类似复数的方式——四元数来描述空间中物体的姿态变化。前人已经证明了，使用四元数表示物体在三维空间中的旋转，即是紧凑的，也是没有奇异性的。但是可能没有那么直观。
我们使用如下方式表达四元数：
q ⃗ = q 0 + q 1 i + q 2 j + q 3 k \vec{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k q =q0+q1i+q2j+q3k
其中，i，j，k为四元数的三个三个虚部，他们符合以下关系：
{ i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j \left\{\begin{array}{l}i^{2}=j^{2}=k^{2}=-1 \\ i j=k, j i=-k \\ j k=i, k j=-i \\ k i=j, i k=-j\end{array}\right. ⎩⎪⎪⎨⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
我们可以用以下形式表达四元数，用一个标量和一个向量来表达四元数
q ⃗ = [ s , v ⃗ ] , s = q 0 ∈ R , v ⃗ = [ q 1 , q 2 , q 3 ] T ∈ R 3 \vec{q}=[s, \vec{v}], \quad s=q_{0} \in \mathbb{R}, \vec{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3} q =[s,v ],s=q0∈R,v =[q1,q2,q3]T∈R3
在这里，s称为四元数的实部，v称为四元数的虚部。虚部的三个向量被用来表示三维空间中的一个空间点。
利用四元数表示旋转
定理一
就如同在复数中我们使用模长为1的复数表示二维空间中的纯旋转，即没有长度缩放的旋转，我们使用模长为1的四元数表示三维空间中的纯旋转，下面给出证明：
不妨假设存在一个四元数 q ⃗ = [ a , v ⃗ ] , a = q 0 ∈ R , v ⃗ = [ q 1 , q 2 , q 3 ] T ∈ R 3 \vec{q}=[a, \vec{v}], \quad a=q_{0} \in \mathbb{R}, \vec{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3} q =[a,v ],a=q0∈R,v =[q1,q2,q3]T∈R3，满足等式 q ⃗ 2 = − 1 \vec{q}^{2}=-1 q 2=−1，展开后不难得到如下等式：
( a + v ⃗ ) 2 = − 1 (a+\vec{v})^{2}=-1 (a+v )2=−1
( a 2 + v ⃗ 2 ) + ( 2 a v ⃗ ) = − 1 \left(a^{2}+\vec{v}^{2}\right)+(2 a \vec{v})=-1 (a2+v 2)+(2av )=−1
其中，-1是一个纯实数，该方程在定义域内的解只有两种可能。
情况一 v ⃗ = 0 \vec{v}={0} v =0，易得$ a^{2}=-1 ， 又 因 为 ，又因为 ，又因为a$为实数，无解
情况二 a = 0 a=0 a=0，解得 v ⃗ 2 = − 1 \vec{v}^{2}=-1 v 2=−1，即 ∣ v ⃗ ∣ = 1 |\vec{v}|=1 ∣v ∣=1
我们发现四元数 q ⃗ \vec{q} q 是一个实部为0，虚部模长为1的一个单位四元数，我们将四元数的虚部单独置入三维空间，表示该空间中的一个向量 u ⃗ \vec{u} u ，而其实部所对应的坐标轴（我们取其方向向量记作 R e ⃗ \vec{Re} Re ）和该虚空间内的所有向量垂直，且 R e ⃗ \vec{Re} Re 与 u ⃗ \vec{u} u 垂直， u ⃗ \vec{u} u 为一个模长为1的虚向量，那么我们可以将 u ⃗ \vec{u} u 与 R e ⃗ \vec{Re} Re 共同构成的平面看作是一个等价的复数平面，其中 R e ⃗ \vec{Re} Re 对应复数的实轴 u ⃗ \vec{u} u 对应复数的虚轴。
因为决定复数性质的是 i 2 = 1 i^{2} = 1 i2=1，而 u ⃗ 2 = − 1 \vec{u}^2=-1 u 2=−1满足该式子，在只有该条件下时，可以认为该平面等价于复数平面。可以将等价理解为：复数满足的性质该平面上的数也满足。所以我们可以直接给出该种四元数对应的欧拉公式：
e u θ = cos θ + u ⃗ sin θ e^{u \theta}=\cos \theta+\vec{u} \sin \theta euθ=cosθ+u sinθ
其中 u ⃗ \vec{u} u 是一个单位纯四元数。
参考复平面的性质，我们可以认为该四元数可以表示空间内任一包含过向量 u ⃗ \vec{u} u 的平面的旋转，这些平面的集合等价于3维空间，所以模长为1的四元数可以表示空间内的任一旋转。
定理二
假设一个空间三维点 p ⃗ = [ x , y , z ] ∈ R 3 \vec{p}=[x, y, z] \in \mathbb{R}^{3} p =[x,y,z]∈R3，以及由轴角 n ⃗ , θ \vec{n}, \theta n ,θ指定的旋转。三维点 p ⃗ \vec{p} p 经过旋转变成 p ⃗ ′ \vec{p}^{\prime} p ′。我们使用 p ⃗ x = [ 0 , x , y , z ] = [ 0 , v ⃗ ] \vec{p}_{x}=[0, x, y, z]=[0, \vec{v}] p x=[0,x,y,z]=[0,v ]描述空间三维点，其中的 v ⃗ \vec{v} v 表示空间中的三个轴对应。我们用模长为1的四元数表示一次旋转,那么旋转后的 p ⃗ ′ \vec{p}^{\prime} p ′即可表示为如下乘式（注意使用的是四元数乘法） q ⃗ = [ cos θ 2 , u ⃗ sin θ 2 ] = e u ⃗ θ 2 \vec{q}=\left[\cos \frac{\theta}{2}, \vec{u} \sin \frac{\theta}{2}\right]=e^{\vec{u} \frac{\theta}{2}} q =[cos2θ,u sin2θ]=eu 2θ：
p ⃗ ′ = q ⃗ p ⃗ x q ⃗ − 1 \vec{p}^{\prime}=\vec{q} \vec{p}_{x} \vec{q}^{-1} p ′=q p xq −1
接下来给出证明：
因为 p ⃗ x \vec{p}_{x} p x是一个纯四元数，实部为0，我们直接将其虚部放入三维虚空间。不妨假设转轴方向上的单位向量为 u ⃗ \vec{u} u ， v ⃗ \vec{v} v 与 u ⃗ \vec{u} u 构成一个平面，在该平面上，将 v ⃗ \vec{v} v 分解到 u ⃗ \vec{u} u 及与 u ⃗ \vec{u} u 垂直的方向上，有 v ⃗ = v ⃗ 1 + v ⃗ 2 \vec{v}=\vec{v}_{1}+\vec{v}_{2} v =v 1+v 2，如图所示。
不难发现， v ⃗ 2 \vec{v}_{2} v 2不会随着转轴的转动发生变化，我们只需要计算 v ⃗ 1 \vec{v}_{1} v 1的改变量即可。过 v ⃗ 1 \vec{v}_{1} v 1做关于 v ⃗ 2 \vec{v}_{2} v 2的垂面，如下图所示。
上图中，转轴 u ⃗ \vec{u} u 从原点方向垂直纸面向上，y方向上的转轴大小恰好为 ∣ u ⃗ v ⃗ 1 ∣ |\vec{u}\vec{v}_{1}| ∣u v 1∣（注意这里 u ⃗ \vec{u} u 与 v ⃗ 1 \vec{v}_{1} v 1都是四元数），可以通过以下乘式得到：
u ⃗ v ⃗ 1 = ( u ⃗ ) ( v ⃗ 1 ) = ( − u ⃗ ⋅ v ⃗ 1 ) + ( u ⃗ × v ⃗ 1 ) \vec{u}\vec{v}_{1}=(\vec{u})\left(\vec{v}_{1}\right)=\left(-\vec{u} \cdot \vec{v}_{1}\right)+\left(\vec{u} \times \vec{v}_{1}\right) u v 1=(u )(v 1)=(−u ⋅v 1)+(u ×v 1)
通过旋转角度 θ \theta θ计算出 v ⃗ 1 ′ \vec{v}_{1}^{\prime} v 1′的大小如下:
v ⃗ 1 ′ = ( cos θ + sin θ u ⃗ ) v ⃗ 1 = e u ⃗ θ v ⃗ 1 \vec{v}_{1}^{\prime}=(\cos \theta+\sin \theta \vec{u}) \vec{v}_{1}=e^{\vec{u} \theta} \vec{v}_{1} v 1′=(cosθ+sinθu )v 1=eu θv 1
带入前式可以得到 p ⃗ ′ \vec{p}^{\prime} p ′的虚部 v ⃗ ′ \vec{v}^{\prime} v ′:
v ⃗ ′ = e u θ v ⃗ 1 + v ⃗ 2 \vec{v}^{\prime}=e^{u \theta} \vec{v}_{1}+\vec{v}_{2} v ′=euθv 1+v 2
又因为纯四元数的性质有如下结论:
P = e θ 2 u , P − 1 = P ∗ = e − θ 2 u P=e^{\frac{\theta}{2} u} ,P^{-1}=P^{*}=e^{-\frac{\theta}{2} u} P=e2θu,P−1=P∗=e−2θu
其中 P P P为 p p p对应的三维空间内的矩阵。
要证 p ⃗ ′ = q ⃗ p ⃗ x q ⃗ − 1 \vec{p}^{\prime}=\vec{q} \vec{p}_{x} \vec{q}^{-1} p ′=q p xq −1，只需证 v ′ = p p v 1 + p p ∗ v 2 v^{\prime}=p p v_{1}+p p^{*} v_{2} v′=ppv1+pp∗v2，即证
（1） P ∗ v 2 = v 2 P ∗ P^{*} v_{2}=v_{2} P^{*} P∗v2=v2P∗
（2） P v 1 = v 1 P ∗ Pv_{1}=v_{1} P^{*} Pv1=v1P∗
因为 P P P与 v 2 v_{2} v2均在实轴 R e ⃗ \vec{Re} Re 与转轴方向的单位向量 u ⃗ \vec{u} u 组成的等价复平面内，他们之间满足乘法交换律，（1）式得证。
如图所示，不妨假设平面上存在一个向量 q q q使等式（2）成立。在复平面中，有向量 q = r e μ θ q=r e^{\mu \theta} q=reμθ成立（这里的 θ \theta θ与旋转角不同），那么，要证等式（2），即证 r e μ θ v 1 = v 1 r e − μ θ re^{\mu\theta}v_{1}=v_{1}re^{-\mu\theta} reμθv1=v1re−μθ，即证
v 1 = e μ θ v 1 e μ θ v_{1}=e^{\mu \theta} v_{1} e^{\mu \theta} v1=eμθv1eμθ
又，根据欧拉公式，展开等式右边，易得
v 1 ⃗ = e μ θ v 1 e μ θ = ( cos θ + u ⃗ sin θ ) v 1 ⃗ ( cos θ + u ⃗ sin θ ) = cos 2 θ v 1 ⃗ + sin θ cos θ v 1 ⃗ u ⃗ + cos θ sin θ u ⃗ v 1 ⃗ + sin 2 θ u ⃗ v 1 ⃗ u ⃗ = cos 2 θ v 1 ⃗ + sin 2 θ v 1 ⃗ = v 1 ⃗ \begin{aligned}\vec{v_{1}}&= e^{\mu \theta} v_{1} e^{\mu \theta}=(\cos \theta +\vec{u}\sin \theta )\vec{v_{1}}(\cos \theta +\vec{u}\sin \theta )\\&=\cos ^2 \theta \vec{v_{1}} + \sin \theta \cos \theta \vec{v_{1}} \vec{u} + \cos \theta \sin \theta \vec{u} \vec{v_{1}} +\sin ^2 \theta \vec{u} \vec{v_{1}}\vec{u} \\&= \cos ^2 \theta\vec{v_{1}}+\sin ^2 \theta\vec{v_{1}}\\&=\vec{v_{1}}\end{aligned} v1 =eμθv1eμθ=(cosθ+u sinθ)v1 (cosθ+u sinθ)=cos2θv1 +sinθcosθv1 u +cosθsinθu v1 +sin2θu v1 u =cos2θv1 +sin2θv1 =v1
又因为 p ∈ q p \in q p∈q，所以等式（2）成立。
综上所述，我们可以使用 p ⃗ ′ = q ⃗ p ⃗ q ⃗ − 1 \vec{p}^{\prime}=\vec{q} \vec{p} \vec{q}^{-1} p ′=q p q −1描述一次在空间中的旋转。
从四元数到旋转矩阵
在实际应用中，四元数乘法的运算开销依旧比较大，相较而言，矩阵的计算开销较小，现存的矩阵计算优化技术也比较成熟，我们将四元数转化为旋转矩阵进行运算。有如下两个方案。
方案一：将四元数 q ⃗ \vec{q} q 转化为轴角 n ⃗ , θ \vec{n}, \theta n ,θ,再根据罗德里格斯公式 R ⃗ = cos θ I ⃗ + ( 1 − cos θ ) n ⃗ n ⃗ T + sin θ n ⃗ ∧ \vec{R}=\cos \theta \vec{I}+(1-\cos \theta) \vec{n} \vec{n}^{T}+\sin \theta \vec{n}^{\wedge} R =cosθI +(1−cosθ)n n T+sinθn ∧转化为旋转矩阵。这是非常直观的方案，但是需要计算一个arccos函数，代价较大。
方案二：对于四元数 q ⃗ = q 0 + q 1 i + q 2 j + q 3 k \vec{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k q =q0+q1i+q2j+q3k，对应的旋转矩阵可以用如下公式计算：
R ⃗ = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 3 − 2 q 0 q 2 2 q 1 q 2 − 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 3 + 2 q 0 q 2 2 q 2 q 3 − 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] \vec{R}=\left[\begin{array}{ccc}1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \\ 2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \\ 2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2}\end{array}\right] R =⎣⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤
反之，由旋转矩阵到四元数的转换如下。假设矩阵为 R ⃗ = { m i j } , i , j ∈ [ 1 , 2 , 3 ] \vec{R}=\left\{m_{i j}\right\}, i, j \in[1,2,3] R ={mij},i,j∈[1,2,3]，其对应的四元数 q ⃗ \vec{q} q 由下式给出：
q 0 = t r ( R ) + 1 2 , q 1 = m 23 − m 32 4 q 0 , q 2 = m 31 − m 13 4 q 0 , q 3 = m 12 − m 21 4 q 0 q_{0}=\frac{\sqrt{tr(R)+1}}{2}, q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}, q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}, q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}} q0=2tr(R)+1 ,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21
由于硬件平台的算力有限，我们采用第二种方案，下面给出代码实现：
/*计算旋转矩阵*/
void imuComputeRotationMatrix(void)
{
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
不难发现，当 q 0 q_{0} q0接近于0时，其余三个分量会非常大难以计算，导致解的不稳定。这时我们考虑使用别的计算方案。
标签：cos,飞行器,旋转,vec,theta,解算,sin,姿态 来源： https://blog.csdn.net/xj_lance/article/details/116259833