电子罗盘-航向角计算

坐标变换

• 手机初始状态accelerometer 与 magnetometer 读数为
  (假定初始状态为水平放置,如上图所示)
  $$G_1 = \begin{bmatrix} a_{x1} \\ a_{y1} \\ a_{z1} \\ \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} \tag{1}$$
  $$M_1 = \begin{bmatrix} m_{x1} \\ m_{y1} \\ m_{z1} \end{bmatrix} = \begin{bmatrix} cos(\delta) \\ 0 \\ sin(\delta) \end{bmatrix} \tag{2}$$
• 依上图坐标系,手机绕$x,y,z$轴旋转的变换矩阵为
  $$C_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \\ \end{bmatrix} \tag{3}$$
  $$C_y(\phi) = \begin{bmatrix} cos(\phi) & 0 & sin(\phi) \\ 0 & 1 & 0 \\ -sin(\phi) & 0 & cos(\phi) \\ \end{bmatrix} \tag{4}$$
  $$C_z(\psi) = \begin{bmatrix} cos(\psi) & -sin(\psi) & 0 \\ sin(\psi) & cos(\psi) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \tag{5}$$

姿态角计算

• 下列计算均假设acclerometer读数基本稳定,即手机近似静止,可对加速度数据做低通滤波处理.
• 旋转过后accelerometer与magnetometer读数为(基于机体坐标系,旋转次序 yaw->pitch ->roll)
  $$G_2 = \begin{bmatrix} a_{x2} \\ a_{y2} \\ a_{z2} \\ \end{bmatrix} =C_y(\phi) C_x(\theta) C_z(\psi)G_1 = C_y(\phi) C_x(\theta) C_z(\psi) \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} \tag{6}$$
  $$M_2 = \begin{bmatrix} m_{x2} \\ m_{y2} \\ m_{z2} \\ \end{bmatrix} =C_y(\phi) C_x(\theta) C_z(\psi)M_1 = C_y(\phi) C_x(\theta) C_z(\psi) \begin{bmatrix} cos(\delta) \\ 0 \\ sin(\delta) \end{bmatrix} \tag{7}$$

• 计算翻滚角$\phi$
  由公式(6)可得,
  
  

  G2=⎡⎣⎢ax2ay2az2⎤⎦⎥=Cy(ϕ)Cx(θ)Cy(ψ)G1=Cy(ϕ)Cx(θ)Cy(ψ)⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢cos(ϕ)0−sin(ϕ)010sin(ϕ)0cos(ϕ)⎤⎦⎥⎡⎣⎢1000cos(θ)sin(θ)0−sin(θ)cos(θ)⎤⎦⎥⎡⎣⎢cos(ψ)sin(ψ)0−sin(ψ)cos(ψ)0001⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢cos(ϕ)cos(ψ)+sin(ϕ)sin(θ)sin(ψ)cos(θ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)sin(ϕ)sin(θ)cos(ψ)−cos(ϕ)sin(ψ)cos(θ)cos(ψ)sin(ϕ)sin(ψ)+cos(ϕ)sin(θ)cos(ψ)sin(ϕ)cos(θ)−sin(θ)cos(ϕ)cos(θ)⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢g⋅sin(ϕ)cos(θ)−g⋅sin(θ)g⋅cos(ϕ)cos(θ)⎤⎦⎥(365)(366)(367)(368)(8) (8) (365) G 2 = [ a x 2 a y 2 a z 2 ] = C y ( ϕ ) C x ( θ ) C y ( ψ ) G 1 = C y ( ϕ ) C x ( θ ) C y ( ψ ) [ 0 0 g ] (366) = [ c o s ( ϕ ) 0 s i n ( ϕ ) 0 1 0 − s i n ( ϕ ) 0 c o s ( ϕ ) ] [ 1 0 0 0 c o s ( θ ) − s i n ( θ ) 0 s i n ( θ ) c o s ( θ ) ] [ c o s ( ψ ) − s i n ( ψ ) 0 s i n ( ψ ) c o s ( ψ ) 0 0 0 1 ] [ 0 0 g ] (367) = [ c o s ( ϕ ) c o s ( ψ ) + s i n ( ϕ ) s i n ( θ ) s i n ( ψ ) s i n ( ϕ ) s i n ( θ ) c o s ( ψ ) − c o s ( ϕ ) s i n ( ψ ) s i n ( ϕ ) c o s ( θ ) c o s ( θ ) s i n ( ψ ) c o s ( θ ) c o s ( ψ ) − s i n ( θ ) c o s ( ϕ ) s i n ( θ ) s i n ( ψ ) − s i n ( ϕ ) c o s ( ψ ) s i n ( ϕ ) s i n ( ψ ) + c o s ( ϕ ) s i n ( θ ) c o s ( ψ ) c o s ( ϕ ) c o s ( θ ) ] [ 0 0 g ] (368) = [ g ⋅ s i n ( ϕ ) c o s ( θ ) − g ⋅ s i n ( θ ) g ⋅ c o s ( ϕ ) c o s ( θ ) ]
  $$\begin{align} G_2 &= \begin{bmatrix} a_{x2} \\ a_{y2} \\ a_{z2} \\ \end{bmatrix} = C_y(\phi) C_x(\theta) C_y(\psi)G_1 = C_y(\phi) C_x(\theta) C_y(\psi) \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} \\ &=\begin{bmatrix} cos(\phi) & 0 & sin(\phi) \\ 0 & 1 & 0 \\ -sin(\phi) & 0 & cos(\phi) \\ \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \\ \end{bmatrix} \begin{bmatrix} cos(\psi) & -sin(\psi) & 0 \\ sin(\psi) & cos(\psi) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} \\ &= \begin{bmatrix} cos(\phi)cos(\psi)+sin(\phi)sin(\theta)sin(\psi) & sin(\phi)sin(\theta)cos(\psi)-cos(\phi)sin(\psi) & sin(\phi)cos(\theta) \\ cos(\theta)sin(\psi) & cos(\theta)cos(\psi) & -sin(\theta)\\ cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) & sin(\phi)sin(\psi)+cos(\phi)sin(\theta)cos(\psi) & cos(\phi)cos(\theta) \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix} \\ &= \begin{bmatrix} g\cdot sin(\phi)cos(\theta) \\ -g\cdot sin(\theta) \\ g\cdot cos(\phi)cos(\theta) \end{bmatrix} \\ \end{align} \tag{8}$$
  
  由公式(8)可得,
  $$\frac{a_{x2}}{a_{z2}}=tan(\phi)$$
  则
  $$\phi= tan(\frac{a_{x2}}{a_{z2}}) \tag{9}$$

• 计算俯仰角$\theta$

  • 方法一:由公式(8)可得,
    $$a_{y2} =-g\cdot sin(\theta)$$
    则
    $$\theta = -arcsin(\frac{a_{y2}}{g})$$ ,此时$g=sqrt(a_{x2}^2+a_{y2}^2+a_{z2}^2)$.
  • 方法二:已求得$\phi$条件下,同由公式(8)得,
    $$\frac{a_{y2}}{a_{x2}sin(\phi)+a_{z2}cos(\phi)}=-tan(\theta)$$
    则
    $$\theta= -arctan(\frac{a_{y2}}{a_{x2}sin(\phi)+a_{z2}cos(\phi)}) \tag{10}$$
  • 方法一与方法二求得结果一致,本文采用方法一(android getOrientation()方法用的方法二)
• 计算航向角$\psi$
  由公式(7)可得,
  $$\begin{align} M_2 &= \begin{bmatrix} m_{x2} \\ m_{y2} \\ m_{z2} \\ \end{bmatrix} = C_y(\phi)C_x(\theta) C_z(\psi)M_1 = C_y(\phi)C_x(\theta) C_z(\psi) \begin{bmatrix} cos(\delta) \\ 0 \\ sin(\delta) \end{bmatrix} \\ &= \begin{bmatrix} cos(\phi)cos(\psi)+sin(\phi)sin(\theta)sin(\psi) & sin(\phi)sin(\theta)cos(\psi)-cos(\phi)sin(\psi) & sin(\phi)cos(\theta) \\ cos(\theta)sin(\psi) & cos(\theta)cos(\psi) & -sin(\theta)\\ cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) & sin(\phi)sin(\psi)+cos(\phi)sin(\theta)cos(\psi) & cos(\phi)cos(\theta) \end{bmatrix} \begin{bmatrix} cos(\delta) \\ 0 \\ sin(\delta) \end{bmatrix} \\ &= \begin{bmatrix} cos(\phi)cos(\psi)cos(\delta)+sin(\phi)sin(\theta)sin(\psi)cos(\delta)+sin(\phi)cos(\theta)sin(\delta)\\ cos(\theta)sin(\psi)cos(\delta)-sin(\theta)sin(\delta)\\ cos(\phi)sin(\theta)sin(\psi)cos(\delta)-sin(\phi)cos(\psi)cos(\delta)+cos(\phi)cos(\theta)sin(\delta) \end{bmatrix} \\ \end{align} \tag{11}$$
  已知$\theta$及$\phi$条件下,由上式可得:
  $$m_{x2}sin(\phi)+m_{z2}cos(\phi)=sin(\theta)sin(\psi)cos(\delta)+cos(\theta)sin(\delta)$$
  $$[m_{x2}sin(\phi)+m_{z2}cos(\phi)]sin(\theta)+m_{y2}cos(\theta)=\ sin(\psi)cos(\delta)$$
  $$m_{x2}cos(\phi)-m_{z2}sin(\phi)=cos(\psi)cos(\delta)$$
  则:
  $$\psi=arctan(\frac{m_{x2}sin(\phi)sin(\theta)+m_{z2}cos(\phi)sin(\theta)+m_{y2}cos(\theta)}{m_{x2}cos(\phi)-m_{z2}sin(\phi)})$$

Android 应用

• 依上述公式求取航向角为与x轴夹角,通过android api getOrientation()方法获取方位角为与y轴夹角.需做角度转换

  float yaw = (float) Math.toDegrees(ECompass.getYaw(accVals,magVals));
                //getOrientation获取方位角为与y轴夹角,本文计算方法求取方位角为与x轴夹角,角度需转换
                yaw -= 90;
                if(yaw<-180)
                    yaw += 360;

public class ECompass {

    private static float yaw;
    private static float pitch;
    private static float roll;
    //accVal,magVal为android phone 获取传感器原始数据
    public static float getYaw(float[] accVals, float[] magVals) {
        roll = (float)Math.atan2(accVals[0],accVals[2]);
        pitch = -(float)Math.atan(accVals[1]/(accVals[0]*Math.sin(roll)+accVals[2]*Math.cos(roll)));
        yaw = (float)Math.atan2(magVals[0]*Math.sin(roll)*Math.sin(pitch)+magVals[2]*Math.cos(roll)*Math.sin(pitch)+magVals[1]*Math.cos(pitch),
               magVals[0]*Math.cos(roll)-magVals[2]*Math.sin(roll));
        return yaw;
    }
}