安卓坐标系转换之一：从手机坐标系到地球坐标系

在安卓手机中，加速度计、陀螺仪、磁场计、重力计等结果都是参照手机本身的坐标系来说的。
安卓官方文档中对手机坐标系作了定义，如图1：

图1 手机坐标系

很多情况下我们需要消除手机的姿态对测试数据的影响，把测量数据转换到地球坐标系中，这时就要利用到坐标变换了。图2是地球坐标系：

图2 地球坐标系

为了区分与手机坐标系的区别，这里把地球坐标系加一个下标w，在此坐标系中：

• $z_{w}$ 垂直与地面向上
• $y_{w}$与$z_{w}$垂直，指向北。
• $x_{w}$垂直与$z_{w}$与$y_{w}$确定的平面，指向东。

这里可以参考官方文档的描述。

另外，这里要说一下地磁场。
地磁的南极在地理北极附近，但并不是完全重合，这里不再与正北做区分，下同。
磁感线是由磁北极指向磁南极的，所以在北半球，磁场的方向因地理位置而异，但都是斜向下的，所以可以分解为一个竖直向下的分量和一个水平分量，水平的分量指向北。如图3。

图3 地磁场示意

安卓里给出了坐标变换的方法，即getRotationMatrix()方法。

下面我们解读一下代码，看是如何完成坐标转换的。

public static boolean getRotationMatrix(float[] R, float[] I,
            float[] gravity, float[] geomagnetic) {
        // TODO: move this to native code for efficiency
        float Ax = gravity[0];
        float Ay = gravity[1];
        float Az = gravity[2];// 重力沿【手机坐标系】三个轴的分量。但合力一定是竖直【向下】的，也就是地球坐标系的Z的反方向。（即地球坐标系的-Z方向）
        final float normsqA = (Ax * Ax + Ay * Ay + Az * Az);//求重力合力
        final float g = 9.81f;
        final float freeFallGravitySquared = 0.01f * g * g;
        if (normsqA < freeFallGravitySquared) {
            // gravity less than 10% of normal value
            return false;//重力太小。
        }
        final float Ex = geomagnetic[0];
        final float Ey = geomagnetic[1];
        final float Ez = geomagnetic[2];//当前磁场的大小，沿【手机坐标系】三个轴的分量。虽然磁场的水平分量是指北的，但总分量并不指北。
        float Hx = Ey * Az - Ez * Ay;
        float Hy = Ez * Ax - Ex * Az;
        float Hz = Ex * Ay - Ey * Ax;//磁力与重力的叉乘。得到垂直于【地球坐标系】-Z与磁场方向所构成的平面的向量，这个向量是指西的，即【地球坐标系】向量-X的方向。
        final float normH = (float) Math.sqrt(Hx * Hx + Hy * Hy + Hz * Hz);
        if (normH < 0.1f) {
            // device is close to free fall (or in space?), or close to
            // magnetic north pole. Typical values are  > 100.
            return false;//失重，在太空，或者手机在地磁的北极（站在地磁北极向下的重力为0）。
        }
        final float invH = 1.0f / normH;
        Hx *= invH;
        Hy *= invH;
        Hz *= invH;//单位化【地球坐标系】-X
        final float invA = 1.0f / (float) Math.sqrt(Ax * Ax + Ay * Ay + Az * Az);
        Ax *= invA;
        Ay *= invA;
        Az *= invA;//单位化【地球坐标系】-Z
        final float Mx = Ay * Hz - Az * Hy;
        final float My = Az * Hx - Ax * Hz;
        final float Mz = Ax * Hy - Ay * Hx;//【地球坐标系】单位化-z与-x的叉乘，得到【地球坐标系】单位的Y，这个Y才是真正的指北。不要与磁场方向搞混。从而得到了【地球坐标系】的一组基。注意，这里的向量只是方向与【地球坐标系】的坐标轴方向相同，但表示还是在【手机坐标系】里。
        if (R != null) {
            if (R.length == 9) {
                R[0] = Hx;     R[1] = Hy;     R[2] = Hz;//【地球坐标系】-X方向，指西
                R[3] = Mx;     R[4] = My;     R[5] = Mz;//【地球坐标系】Y方向，指北
                R[6] = Ax;     R[7] = Ay;     R[8] = Az;//【地球坐标系】-Z方向，向下
                } else if (R.length == 16) {//4x4的是坐标平移的情况，我们在此不予考虑。
                R[0]  = Hx;    R[1]  = Hy;    R[2]  = Hz;   R[3]  = 0;
                R[4]  = Mx;    R[5]  = My;    R[6]  = Mz;   R[7]  = 0;
                R[8]  = Ax;    R[9]  = Ay;    R[10] = Az;   R[11] = 0;
                R[12] = 0;     R[13] = 0;     R[14] = 0;    R[15] = 1;
            }
        }
        if (I != null) {
            // compute the inclination matrix by projecting the geomagnetic
            // vector onto the Z (gravity) and X (horizontal component
            // of geomagnetic vector) axes.
            final float invE = 1.0f / (float) Math.sqrt(Ex * Ex + Ey * Ey + Ez * Ez);
            final float c = (Ex * Mx + Ey * My + Ez * Mz) * invE;//【地球坐标系】Y与磁场方向夹角余弦
            final float s = (Ex * Ax + Ey * Ay + Ez * Az) * invE;//【地球坐标系】-Z与磁场方向夹角余弦
            if (I.length == 9) {//由以上两个公式，求出了当地磁场方向分别与竖直、水平两个分量的夹角。
                I[0] = 1;     I[1] = 0;     I[2] = 0;
                I[3] = 0;     I[4] = c;     I[5] = s;
                I[6] = 0;     I[7] = -s;     I[8] = c;
            } else if (I.length == 16) {
                I[0] = 1;     I[1] = 0;     I[2] = 0;
                I[4] = 0;     I[5] = c;     I[6] = s;
                I[8] = 0;     I[9] = -s;     I[10] = c;
                I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0;
                I[15] = 1;
            }
        }
        return true;
    }

文档上说R就是旋转矩阵：

R is the identity matrix when the device is aligned with the world’s coordinate system, that is, when the device’s X axis points toward East, the Y axis points to the North Pole and the device is facing the sky.

那为什么R是旋转矩阵？

设旋转矩阵为C，那么有【手机坐标系】里的一点$V_{d}=\{x_{d},y_{d},z_{d}\}^ \mathrm{ T }$,其对应在【地球坐标系】的点表示为$V_{w}=\{x_{w},y_{w},z_{w}\}^ \mathrm{ T }$,那么有：

$V_{w}=C·V_{d}$
可以代入 $V_{d}\{H_{x},H_{y},H_{z}\}^ \mathrm{ T }$,即地球坐标系的X轴的单位向量，对应的应该是 $V_{w}=\{1,0,0\}^ \mathrm{ T }$.同理，代入下面两行，得到：


$$\begin{align*}\\ C· \begin{bmatrix} H_{x}& M_{x} &A_{x} \\ H_{y}&M_{y} &A_{y} \\ H_{z}&M_{z} &A_{z}\\ \end{bmatrix} &= \begin{bmatrix} 1& 0 &0 \\ 0&1 &0 \\ 0&0 &1\\ \end{bmatrix} \\ C·D&=E\\ C&=D^{-1} \end{align*}$$

注：这里的Z和X求出来其实是负方向的。为了方便计算。在实际转换中求出地球坐标系对应的坐标后，z轴与X轴坐标需要加负号。

又D为正交矩阵，$D^{-1}=D^{T}$,所以有

$$\begin{align*} C&=D^{-1} \\ &=D^{T} =\begin{bmatrix} H_{x}& M_{x} &A_{x} \\ H_{y}&M_{y} &A_{y} \\ H_{z}&M_{z} &A_{z}\\ \end{bmatrix}^{T}\\ &=\begin{bmatrix} H_{x}& H_{y} &H_{z} \\ M_{x}&M_{y} &M_{z} \\ A_{x}&A_{y} &A_{z}\\ \end{bmatrix}=R \end{align*}$$

即R就是旋转矩阵。