三轴加速度计解算姿态（四元数）

当传感器载体静止时，加速度计只会输出重力加速度，可以凭此来计算载体的俯仰角和滚转角。

假设导航坐标系为东北天，载体坐标系为右前上。

初始载体坐标系和导航坐标系重合，对应的四元数为q=[1,0,0,0]，使用此四元数表示载体在导航坐标系下的旋转。

先将四元数转为旋转矩阵 C = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2 w y + 2 x z 2 x y + 2 w z 1 − 2 x 2 − 2 z 2 2 y z − 2 w x 2 x z − 2 w y 2 w x + 2 y z 1 − 2 x 2 − 2 y 2 ] C=\begin{bmatrix} 1-2y^{2}-2z^{2} & 2xy-2wz & 2wy+2xz \\ 2xy+2wz & 1-2x^{2}-2z^{2} & 2yz-2wx \\ 2xz-2wy & 2wx+2yz & 1-2x^{2}-2y^{2} \end{bmatrix} C= ​1−2y2−2z22xy+2wz2xz−2wy​2xy−2wz1−2x2−2z22wx+2yz​2wy+2xz2yz−2wx1−2x2−2y2​ ​ 可以使用此旋转矩阵对载体在导航坐标系下进行旋转，旋转后的载体坐标系和导航坐标系，对于一相同向量有如下关系，推导参考3维旋转矩阵推导与助记。

[ x ′ y ′ z ′ ] = C − 1 × [ x y z ] \begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \end{bmatrix} =C^{-1}\times \begin{bmatrix} x \\ y \\ z \end{bmatrix} ​x′y′z′​ ​=C−1× ​xyz​ ​

可以使用此公式将导航坐标系中的向量映射到载体坐标系。 C − 1 C^{-1} C−1为旋转矩阵的逆矩阵，同样也是转置矩阵 C T C^{T} CT。

加速度计测量的重力向量为

[ a x b a y b a z b ] \begin{bmatrix} a_{xb}\\ a_{yb}\\ a_{zb} \end{bmatrix} ​axb​ayb​azb​​ ​

需要将其转换到导航坐标系下，根据前面的旋转坐标系的公式可以得到

[ a x b a y b a z b ] = C − 1 × [ a x n a y n a z n ] \begin{bmatrix} a_{xb} \\ a_{yb} \\ a_{zb} \end{bmatrix} =C^{-1}\times \begin{bmatrix} a_{xn} \\ a_{yn} \\ a_{zn} \end{bmatrix} ​axb​ayb​azb​​ ​=C−1× ​axn​ayn​azn​​ ​

则

[ a x n a y n a z n ] = C × [ a x b a y b a z b ] \begin{bmatrix} a_{xn} \\ a_{yn} \\ a_{zn} \end{bmatrix} =C\times \begin{bmatrix} a_{xb} \\ a_{yb} \\ a_{zb} \end{bmatrix} ​axn​ayn​azn​​ ​=C× ​axb​ayb​azb​​ ​

其中 ( a x b , a y b , a z b ) (a_{xb},a_{yb},a_{zb}) (axb​,ayb​,azb​)为加速度计在载体坐标系下测量的加速度向量， ( a x n , a y n , a z n ) (a_{xn},a_{yn},a_{zn}) (axn​,ayn​,azn​)为转换到导航坐标系下的加速度向量。

现在，需要将测量的加速度向量在导航坐标系下的映射旋转到标准重力的方向即

[ 0 0 g ] \begin{bmatrix} 0\\ 0\\ g \end{bmatrix} ​00g​ ​

将这个旋转应用到载体上即为载体当前的姿态。

设 v 1 → = [ a x n a y n a z n ] \overrightarrow{v_{1}} =\begin{bmatrix} ax_{n} \\ ay_{n} \\ az_{n} \end{bmatrix} v1​ ​= ​axn​ayn​azn​​ ​， v 2 → = [ 0 0 g ] \overrightarrow{v_{2}}=\begin{bmatrix} 0\\ 0\\ g \end{bmatrix} v2​ ​= ​00g​ ​， v 1 → \overrightarrow{v_{1}} v1​ ​和 v 2 → \overrightarrow{v_{2}} v2​ ​之间的夹角为 θ \theta θ

则旋转轴 v → = v 1 → × v 2 → \overrightarrow{v}=\overrightarrow{v_{1}} \times \overrightarrow{v_{2}} v =v1​ ​×v2​ ​，然后绕此轴旋转 θ \theta θ。

根据 v 1 → ⋅ v 2 → = ∣ v 1 → ∣ ∣ v 2 → ∣ ⋅ cos ⁡ ( θ ) \overrightarrow{v_{1}} \cdot \overrightarrow{v_{2}}=\left | \overrightarrow{v_{1}} \right | \left | \overrightarrow{v_{2}} \right | \cdot \cos(\theta) v1​ ​⋅v2​ ​= ​v1​ ​ ​ ​v2​ ​ ​⋅cos(θ)，将 v 1 → \overrightarrow{v_{1}} v1​ ​和 v 2 → \overrightarrow{v_{2}} v2​ ​归一化，则 θ = arccos ⁡ ( v 1 → ⋅ v 2 → ) \theta=\arccos(\overrightarrow{v_{1}} \cdot \overrightarrow{v_{2}} ) θ=arccos(v1​ ​⋅v2​ ​)

根据四元数公式 q ′ = [ cos ⁡ ( θ 2 ) v → × sin ⁡ ( θ 2 ) ] q^{'}=\begin{bmatrix} \cos(\frac{\theta}{2} ) & \overrightarrow{v} \times \sin(\frac{\theta}{2}) \end{bmatrix} q′=[cos(2θ​)​v ×sin(2θ​)​] 可以得到此旋转对应的四元数，其中 v → \overrightarrow{v} v 需要归一化处理。

将初始四元数 q q q左乘 q ′ q^{'} q′就能得到载体当前姿态对应的四元数。

以下是使用Eigen3的C++示例

加速度计解算姿态在载体静止时可以得到较准确的俯仰角和滚转角，但是因为重力在水平方向上没有分量，所以无法解算偏航角。而且当物体做非匀速运动时，额外的加速度会导致姿态不正确。