RTKLIB——坐标系相互转换（ecef2pos，pos2ecef，ecef2enu，enu2ecef）

本文代码选自RTKLIB_2.4.2版本，文中所有代码均在rtkcmn.c源文件中，宏定义在头文件中。

文章目录

宏定义与工具函数

`宏定义`

#define PI 3.141592653589793
#define RE_WGS84    6378137.0           /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84    (1.0/298.257223563) /* earth flattening (WGS84) */

上述定义分别对应：圆周率PI，WGS84椭球长半轴，WGS84椭球扁率。

`工具`函数

dot函数函数

参数声明：

向量a,向量b，维数n

意义：

求向量a和向量b的点积，将结果存储到c中作为返回值返回，其中n为维数。

/* inner product ---------------------------------------------------------------
 * inner product of vectors
 * args   : double *a,*b     I   vector a,b (n x 1)
 *          int    n         I   size of vector a,b
 * return : a'*b
 *-----------------------------------------------------------------------------*/
extern double dot(const double *a, const double *b, int n)
{
    
    
    double c=0.0;
    
    while (--n>=0) c+=a[n]*b[n];
    return c;
}

matmul矩阵乘法函数

参数声明：

tr：转置标志，n：左矩阵行，k：左矩阵列或右矩阵的行，m：右矩阵的列

A：左矩阵，B：右矩阵，C：原矩阵/结果矩阵

alpha：乘法结果缩放因子，beta：原矩阵缩放因子

意义：

求得两矩阵的积，并且对其结果进行缩放。更详细的介绍请移步:RTKLIB——matmul(矩阵乘法函数)

extern void matmul(const char *tr, int n, int k, int m, double alpha,
				   const double *A, const double *B, double beta, double *C)
{
    
    
	double d;
	int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4);

	for (i = 0; i < n; i++)
	{
    
    
		for (j = 0; j < k; j++)
		{
    
    
			d = 0.0;
			switch (f)
			{
    
    
			case 1:
				for (x = 0; x < m; x++)
				{
    
    
					d += A[i + x * n] * B[x + j * m];
				}
				break;
			case 2:
				for (x = 0; x < m; x++)
				{
    
    
					d += A[i + x * n] * B[j + x * k];
				}
				break;
			case 3:
				for (x = 0; x < m; x++)
				{
    
    
					d += A[x + i * m] * B[x + j * m];
				}
				break;
			case 4:
				for (x = 0; x < m; x++)
				{
    
    
					d += A[x + i * m] * B[j + x * k];
				}
				break;
			}
			if (beta == 0.0)
				C[i + j * n] = alpha * d;
			else
				C[i + j * n] = alpha * d + beta * C[i + j * n];
		}
	}
}

地固坐标系（xyz）→ 地理坐标系（BLH）

基础公式原理

经度直接求解：
$\frac{y}{x}$
在代码计算中，RTKLIB使用了atan2函数，将数值范围归化到-90度～90之间。因为分母不能为0，因此在计算前还需要判断坐标的合理性，于是上述公式转换成如下形式：
$\begin{cases} \begin{cases} \arctan\frac{y}{x} & x > 0 \\ \arctan\frac{y}{x}+\pi & x < 0 , y \ge 0 \\ \arctan\frac{y}{x}-\pi & x < 0 , y < 0 \\ \frac{\pi}{2} & x = 0 , y > 0 \\ -\frac{\pi}{2} & x = 0 , y < 0 \\ \end{cases} & x≠0,y≠0\\ 0 & x = 0 , y = 0 \\ \end{cases} \\$

使用atan2计算时，该函数会自动根据x,y的正负进行判断，因此写程序时只需要对x=0,y=0的情况进行额外判断即可。

纬度与大地高迭代求解：
$r=x^2+y^2 \\ sinB = \frac{z}{\sqrt{r^2+z^2}} \\ v = \frac{a}{\sqrt{1-e^2\sin^2B}} \\ z = rsinB + e^2vsinB \\$

其中， $e$ 表示WGS84椭球体的偏心率； $v$ 表示地球表面上的纬度对应的曲率半径； $B$ 表示计算得到的大地纬度， $h$ 表示计算得到的大地高。不难看出，大地维度是关于自身的函数，求解大地高需要 $v$ ，而 $v$ 随着维度而变化。因此需要进行迭代计算，在计算初，需要初始化：
$\\ z=y$
于是，当 $z$ 收敛后，可以对大地维度进行求解。
$\arctan\frac{z}{\sqrt{r^2}} \\ h = \sqrt{r^2+z^2}-v$
最终使用atan2求解大地维度，比asin求解更加稳定，并且从运算角度上，求解atan2中运算的过程比asin要少一步。代码中还将超限的值进行了归化，避免出现NAN无法计算，于是大地维度最终为：
$\begin{cases} \arctan\frac{z}{\sqrt{r^2}} & r^2 \ge 10^{-12} \\ \begin{cases} \frac{\pi}{2} & z > 0 \\ -\frac{\pi}{2} & z < 0 \end{cases}&r^2<10^-12\\ \end{cases}\\$

`ecef2pos`代码

参数声明：

r：测站的xyz

pos：用于存储计算得到的BLH

调用函数：

dot：求向量a和向量b的点积，将结果存储到c中作为返回值返回，其中n为维数。

void ecef2posq(const double *r, double *pos)
{
    
    
	double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp;
	for (z = r[2], zk = 0.0; fabs(z - zk) >= 1E-4;)
	{
    
    
		zk = z;
		sinp = z / sqrt(r2 + z * z);
		v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
		z = r[2] + v * e2 * sinp;
	}
	pos[0] = r2 > 1E-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0);
	pos[1] = r2 > 1E-12 ? atan2(r[1], r[0]) : 0.0;
	pos[2] = sqrt(r2 + z * z) - v;
}

上述代码中，a?b:c表示三目操作符，如果a为真，则执行b，反之执行c

地理坐标系（BLH）→ 地固坐标系（xyz）

基础原理

通过BLH反求XYZ不需要迭代，直接带入公式求解即可：
$x=(v+h)cosBcosL \\ y=(v+h)cosBsinL \\ z=(v⋅(1−e^2)+h)sinB$
其中：
$=\frac{a}{\sqrt{1 - e^2 * sin^2B}} \\$

`pos2ecef`函数

参数声明：

pos：测站的BLH

r：用于存储计算得到的xyz

/* transform geodetic to ecef position -----------------------------------------
 * transform geodetic position to ecef position
 * args   : double *pos      I   geodetic position {lat,lon,h} (rad,m)
 *          double *r        O   ecef position {x,y,z} (m)
 * return : none
 * notes  : WGS84, ellipsoidal height
 *-----------------------------------------------------------------------------*/
extern void pos2ecef(const double *pos, double *r)
{
    
    
    double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
    double e2 = FE_WGS84 * (2.0 - FE_WGS84), v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);

    r[0] = (v + pos[2]) * cosp * cosl;
    r[1] = (v + pos[2]) * cosp * sinl;
    r[2] = (v * (1.0 - e2) + pos[2]) * sinp;
}

地固坐标系（xyz）→ 站心坐标系（ENU）

基础原理

$\begin{bmatrix} E \\ N \\ U \end{bmatrix} = \begin{bmatrix} -sinL & cosL & 0\\-sinB cosL & -sinB sinL & cosB\\cosB cosL & cosB sinL & sinB \end{bmatrix} \begin{bmatrix} Xs-Xr \\ Ys-Yr \\ Zs-Zr \end{bmatrix}$

其中，L表示参考点对大地经度，B表示参考点的大地维度。

当我们要求一个测站的ENU时，参考点应该为其本身，因此求出来的E应该为0。

**这是因为：**站心坐标系通常以站点所在地的经线为站心坐标系的东向轴，以经线的垂线方向（即纬线）为站心坐标系系的北向轴，而天向轴则垂直于本地坐标系的东北平面向上。因此，东向分量与站点所在的经线重合，因此为0。

而当我们需要求卫星的高度角时，那么参考点应该为测站，xyz则为卫星地固坐标系下的三维向量，这样就可以求出卫星以测站为原点的站心坐标，进而求出其方位角、仰角。

`ecef2enu`代码

参数声明：

pos：参考点经纬度坐标（弧度）

r：待求点与参考点三维坐标差

e：用于存储计算得到的ENU

调用函数：

xyz2enu：用于求旋转矩阵

matmul：用于进行矩阵运算

意义：

给定参考点经纬度和待求点与参考点的坐标差，返回待求点ENU

/* ecef to local coordinate transfromation matrix ------------------------------
 * compute ecef to local coordinate transfromation matrix
 * args   : double *pos      I   geodetic position {lat,lon} (rad)
 *          double *E        O   ecef to local coord transformation matrix (3x3)
 * return : none
 * notes  : matirix stored by column-major order (fortran convention)
 *-----------------------------------------------------------------------------*/
extern void xyz2enu(const double *pos, double *E)
{
    
    
	double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);

	E[0] = -sinl;			E[3] = cosl;			E[6] = 0.0;
	E[1] = -sinp * cosl;	E[4] = -sinp * sinl;	E[7] = cosp;
	E[2] = cosp * cosl;		E[5] = cosp * sinl;		E[8] = sinp;
}
/* transform ecef vector to local tangental coordinate -------------------------
 * transform ecef vector to local tangental coordinate
 * args   : double *pos      I   geodetic position {lat,lon} (rad)
 *          double *r        I   vector in ecef coordinate {x,y,z} 	s-r
 *          double *e        O   vector in local tangental coordinate {e,n,u}
 * return : none
 *-----------------------------------------------------------------------------*/
extern void ecef2enu(const double *pos, const double *r, double *e)
{
    
    
	double E[9];

	xyz2enu(pos, E);
	matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
}

站心坐标系（ENU）→ 地固坐标系（xyz）

基础原理：

RTKLIB中xyz与enu相互转化通过矩阵转置即可完成，不过需要注意：求得的结果是待求点与参考点的坐标差，还需要将结果加上参考点的xyz，才是待求点的xyz坐标。

$\begin{bmatrix} Xs-Xr \\ Ys-Yr \\ Zs-Zr \end{bmatrix} = \begin{bmatrix} -sinL & cosL & 0\\-sinB cosL & -sinB sinL & cosB\\cosB cosL & cosB sinL & sinB \end{bmatrix}^T \begin{bmatrix} E \\ N \\ U \end{bmatrix}$

`enu2ecef`函数

参数声明：

pos：参考点经纬度坐标（弧度）

e：待求点的ENU

r：用于存储计算得到的xyz坐标差

调用函数：

xyz2enu：用于求旋转矩阵

matmul：用于进行矩阵运算(TN表示旋转矩阵E需要转置，后者不需要转置)

意义：

给定参考点经纬度和待求点与参考点的坐标差，返回待求点ENU

/* transform local vector to ecef coordinate -----------------------------------
 * transform local tangental coordinate vector to ecef
 * args   : double *pos      I   geodetic position {lat,lon} (rad)
 *          double *e        I   vector in local tangental coordinate {e,n,u}
 *          double *r        O   vector in ecef coordinate {x,y,z}
 * return : none
 *-----------------------------------------------------------------------------*/
extern void enu2xzy(const double *pos, const double *e, double *r)
{
    
    
    double E[9];
    
    xyz2enu(pos,E);
    matmul("TN",3,1,3,1.0,E,e,0.0,r);
}