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

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

文章目录

• • 宏定义与工具函数
  • • `宏定义`
    • `工具`函数
  • 地固坐标系（xyz）→ 地理坐标系（BLH）
  • • 基础公式原理
    • `ecef2pos`代码
  • 地理坐标系（BLH）→ 地固坐标系（xyz）
  • • 基础原理
    • `pos2ecef`函数
  • 地固坐标系（xyz）→ 站心坐标系（ENU）
  • • 基础原理
    • `ecef2enu`代码
  • 站心坐标系（ENU）→ 地固坐标系（xyz）
  • • 基础原理：
    • `enu2ecef`函数

宏定义与工具函数

宏定义

#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;elseC[i + j * n] = alpha * d + beta * C[i + j * n];}}
}

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

基础公式原理

经度直接求解：
t a n L = y x tanL = \frac{y}{x} tanL=xy​
在代码计算中，RTKLIB使用了atan2函数，将数值范围归化到-90度～90之间。因为分母不能为0，因此在计算前还需要判断坐标的合理性，于是上述公式转换成如下形式：
L = { { arctan ⁡ y x x > 0 arctan ⁡ y x + π x < 0 , y ≥ 0 arctan ⁡ y x − π x < 0 , y < 0 π 2 x = 0 , y > 0 − π 2 x = 0 , y < 0 x ≠ 0 , y ≠ 0 0 x = 0 , y = 0 L = \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} \\ L=⎩ ⎨ ⎧​⎩ ⎨ ⎧​arctanxy​arctanxy​+πarctanxy​−π2π​−2π​​x>0x<0,y≥0x<0,y<0x=0,y>0x=0,y<0​0​x=0,y=0x=0,y=0​

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

纬度与大地高迭代求解：
r = x 2 + y 2 s i n B = z r 2 + z 2 v = a 1 − e 2 sin ⁡ 2 B z = r s i n B + e 2 v s i n B 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 \\ r=x2+y2sinB=r2+z2 ​z​v=1−e2sin2B ​a​z=rsinB+e2vsinB

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

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 ) c o s B c o s L y = ( v + h ) c o s B s i n L z = ( v ⋅ ( 1 − e 2 ) + h ) s i n B x=(v+h)cosBcosL \\ y=(v+h)cosBsinL \\ z=(v⋅(1−e^2)+h)sinB x=(v+h)cosBcosLy=(v+h)cosBsinLz=(v⋅(1−e2)+h)sinB
其中：
v = a 1 − e 2 ∗ s i n 2 B v =\frac{a}{\sqrt{1 - e^2 * sin^2B}} \\ v=1−e2∗sin2B ​a​

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）

基础原理

[ E N U ] = [ − s i n L c o s L 0 − s i n B c o s L − s i n B s i n L c o s B c o s B c o s L c o s B s i n L s i n B ] [ X s − X r Y s − Y r Z s − Z r ] \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} ​ENU​ ​= ​−sinL−sinBcosLcosBcosL​cosL−sinBsinLcosBsinL​0cosBsinB​ ​ ​Xs−XrYs−YrZs−Zr​ ​

其中，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坐标。

[ X s − X r Y s − Y r Z s − Z r ] = [ − s i n L c o s L 0 − s i n B c o s L − s i n B s i n L c o s B c o s B c o s L c o s B s i n L s i n B ] T [ E N U ] \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} ​Xs−XrYs−YrZs−Zr​ ​= ​−sinL−sinBcosLcosBcosL​cosL−sinBsinLcosBsinL​0cosBsinB​ ​T ​ENU​ ​

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);
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！