Java实现发射坐标系O-xyz转WGS-84坐标系

<think>我们面对的问题是将发射坐标系（O-xyz）转换为WGS-84坐标系（经纬度和高度）。根据引用[1]，这个过程分为两步： 1. 将发射坐标系（O-xyz）转换为地心坐标系（ECEF，Oe-XeYeZe）。 2. 将地心坐标系（ECEF）转换为WGS-84坐标系（经度、纬度、高度）。 ### 步骤1: 发射坐标系转地心坐标系（ECEF） 发射坐标系是一个局部直角坐标系，通常以发射点作为原点。转换到地心坐标系需要知道发射点在地心坐标系中的坐标（即发射点的经纬度和高度，以及发射点的方位角（通常为北偏东角度））。 转换过程通常包括： - 平移：将原点从发射点移动到地心。 - 旋转：将坐标轴从发射坐标系的方向（通常x轴指向发射方向，y轴指向水平垂直方向，z轴垂直向下）旋转到地心坐标系的方向（ECEF的方向）。 具体转换公式如下： 设发射点在地心坐标系中的坐标为$(X_0, Y_0, Z_0)$，发射方位角为$\alpha$（从北向东偏的角度，即方位角），发射点的地理纬度为$\phi$，经度为$\lambda$。 则发射坐标系中的点$(x, y, z)$转换到地心坐标系$(X, Y, Z)$的公式为： $$ \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} X_0 \\ Y_0 \\ Z_0 \end{bmatrix} + R \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} $$ 其中，$R$是旋转矩阵，由三个旋转组成： 1. 绕z轴旋转$(\alpha + \frac{\pi}{2})$（因为发射坐标系的x轴指向发射方向，而北方向是y轴的负方向，所以需要先旋转90度将y轴转到北方向，再加上方位角α）。 2. 绕x轴旋转$(\frac{\pi}{2} - \phi)$（将z轴从垂直向下转为指向地心，因为地理纬度φ是从赤道量起的，而旋转角度是从z轴与地心坐标系Ze轴的夹角考虑）。 更常见的做法是使用方向余弦矩阵，由发射坐标系到地心坐标系的转换矩阵可以由发射点的经度、纬度和方位角确定。 旋转矩阵$R$可以表示为： $$ R = R_z(\lambda) \cdot R_y\left(-\phi - \frac{\pi}{2}\right) \cdot R_x(\alpha) $$ 但更直接的是使用当地东北天（ENU）坐标系作为中介。发射坐标系通常与东北天坐标系有固定关系：发射方向通常与北方向有一个方位角α的夹角，因此发射坐标系与东北天坐标系之间的旋转关系为绕垂直轴（天方向）旋转α角。 而东北天坐标系到地心坐标系的转换矩阵为： $$ R_{ENU}^{ECEF} = \begin{bmatrix} -\sin\lambda & -\sin\phi \cos\lambda & \cos\phi \cos\lambda \\ \cos\lambda & -\sin\phi \sin\lambda & \cos\phi \sin\lambda \\ 0 & \cos\phi & \sin\phi \end{bmatrix} $$ 但是，发射坐标系通常定义：x轴沿发射方向（在水平面内与北方向成α角），y轴在水平面内垂直于发射方向（向右），z轴垂直向下。而东北天坐标系：x轴指向东，y轴指向北，z轴指向天。 因此，发射坐标系与东北天坐标系的关系为： - 发射坐标系的x轴：在东北天坐标系中为（cosα, sinα, 0）方向（因为发射方向在水平面内，由北向东偏α角，所以北方向的分量为cosα，东方向的分量为sinα）。 - 发射坐标系的y轴：在东北天坐标系中为（-sinα, cosα, 0）方向（与x轴垂直，且向右）。 - 发射坐标系的z轴：与东北天坐标系的z轴方向相反，即（0,0,-1）。 因此，从发射坐标系到东北天坐标系的旋转矩阵为： $$ R_{launch}^{ENU} = \begin{bmatrix} \sin\alpha & -\cos\alpha & 0 \\ \cos\alpha & \sin\alpha & 0 \\ 0 & 0 & -1 \end{bmatrix} $$ 注意：这个矩阵的构造需要验证。实际上，我们通常将发射坐标系中的一个向量转换到东北天坐标系，通过这个旋转矩阵。 然后，将东北天坐标系转换到地心坐标系： $$ R_{ENU}^{ECEF} = \begin{bmatrix} -\sin\lambda & -\sin\phi \cos\lambda & \cos\phi \cos\lambda \\ \cos\lambda & -\sin\phi \sin\lambda & \cos\phi \sin\lambda \\ 0 & \cos\phi & \sin\phi \end{bmatrix} $$ 因此，从发射坐标系到地心坐标系的旋转矩阵为： $$ R = R_{ENU}^{ECEF} \cdot R_{launch}^{ENU} $$ 所以，总转换公式为： $$ \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \begin{bmatrix} X_0 \\ Y_0 \\ Z_0 \end{bmatrix} + R_{ENU}^{ECEF} \cdot R_{launch}^{ENU} \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} $$ ### 步骤2: 地心坐标系（ECEF）转WGS-84坐标系 将地心坐标$(X, Y, Z)$转换为经度$\lambda$、纬度$\phi$和高度$h$。 经度$\lambda$的计算公式： $$ \lambda = \arctan2(Y, X) $$ 纬度$\phi$和高度$h$需要通过迭代计算，公式如下： 首先，计算大地纬度$\phi$的初始值： $$ \phi_0 = \arctan2\left( Z, \sqrt{X^2+Y^2} \cdot (1-e^2) \right) $$ 其中，$e^2$是椭球偏心率平方（WGS-84椭球的$e^2=6.69437999014 \times 10^{-3}$）。 然后，使用迭代计算： $$ N = \frac{a}{\sqrt{1-e^2 \sin^2 \phi}} $$ $$ h = \frac{\sqrt{X^2+Y^2}}{\cos \phi} - N $$ $$ \phi = \arctan\left( \frac{Z}{\sqrt{X^2+Y^2}} \cdot \frac{1}{1 - e^2 \frac{N}{N+h}} \right) $$ 迭代直到$\phi$和$h$收敛。 或者使用闭合公式（近似）： $$ \phi = \arctan2\left( Z + e'^2 b \sin^3 \theta, \sqrt{X^2+Y^2} - e^2 a \cos^3 \theta \right) $$ 其中，$\theta = \arctan2(Z \cdot a, \sqrt{X^2+Y^2} \cdot b)$，$a$是长半轴，$b$是短半轴，$e'^2 = \frac{a^2-b^2}{b^2}$。 但通常为了精度，我们使用迭代方法。 ### Java代码实现 下面我们给出Java代码实现的框架。由于转换涉及复杂的数学运算，我们将使用迭代方法计算纬度和高度。 注意：我们需要知道发射点的WGS-84坐标（经度、纬度、高度）和发射方位角α，以及发射坐标系的原点（通常为(0,0,0)在发射点）。 步骤： 1. 将发射点从WGS-84转换为ECEF坐标（这一步需要，因为我们在转换公式中需要发射点的ECEF坐标$(X_0, Y_0, Z_0)$）。 2. 根据发射点的经度、纬度和方位角，计算旋转矩阵$R$（由发射坐标系到ECEF坐标系的旋转矩阵）。 3. 将发射坐标系中的点$(x, y, z)$通过旋转矩阵$R$和平移向量$(X_0, Y_0, Z_0)$转换到ECEF坐标。 4. 将得到的ECEF坐标转换为WGS-84坐标。 #### 辅助函数：将WGS-84转换为ECEF 首先，实现一个函数将WGS-84坐标（经度λ，纬度φ，高度h）转换为ECEF坐标： ```java // WGS-84椭球参数 private static final double a = 6378137.0; // 长半轴 private static final double f = 1.0 / 298.257223563; // 扁率 private static final double b = a * (1 - f); // 短半轴 private static final double e2 = 1 - (b * b) / (a * a); // 第一偏心率平方 public static double[] wgs84ToEcef(double longitude, double latitude, double height) { double lonRad = Math.toRadians(longitude); double latRad = Math.toRadians(latitude); double N = a / Math.sqrt(1 - e2 * Math.sin(latRad) * Math.sin(latRad)); double x = (N + height) * Math.cos(latRad) * Math.cos(lonRad); double y = (N + height) * Math.cos(latRad) * Math.sin(lonRad); double z = (N * (1 - e2) + height) * Math.sin(latRad); return new double[]{x, y, z}; } ``` #### 辅助函数：将ECEF转换为WGS-84 接下来，实现ECEF到WGS-84的转换（迭代法）： ```java public static double[] ecefToWgs84(double x, double y, double z) { double longitude = Math.atan2(y, x); double p = Math.sqrt(x * x + y * y); double latitude = Math.atan2(z, p * (1 - e2)); // 初始纬度 double height = 0; double prevLat = 0; double N = 0; // 迭代计算纬度 do { prevLat = latitude; N = a / Math.sqrt(1 - e2 * Math.sin(prevLat) * Math.sin(prevLat)); latitude = Math.atan2(z + e2 * N * Math.sin(prevLat), p); height = p / Math.cos(latitude) - N; } while (Math.abs(latitude - prevLat) > 1e-9); // 转换为度 longitude = Math.toDegrees(longitude); latitude = Math.toDegrees(latitude); return new double[]{longitude, latitude, height}; } ``` #### 主转换函数：发射坐标系到WGS-84 现在，我们实现从发射坐标系（O-xyz）到WGS-84的转换。需要输入： - 发射点的WGS-84坐标（经度、纬度、高度） - 发射方位角（从北向东偏的角度，单位：度） - 发射坐标系中的点（x, y, z） 步骤： 1. 将发射点从WGS-84转换为ECEF坐标（得到$(X_0, Y_0, Z_0)$）。 2. 计算旋转矩阵$R$（由发射坐标系到ECEF的旋转矩阵）。 3. 将发射坐标系中的点$(x, y, z)$通过旋转和平移转换到ECEF坐标。 4. 将ECEF坐标转换为WGS-84坐标。 旋转矩阵$R$的构造： - 首先，构造从发射坐标系到东北天（ENU）坐标系的旋转矩阵$R_{launch}^{ENU}$。 - 然后，构造从东北天（ENU）坐标系到ECEF坐标系的旋转矩阵$R_{ENU}^{ECEF}$。 - 最后，$R = R_{ENU}^{ECEF} \cdot R_{launch}^{ENU}$。 注意：旋转矩阵是正交矩阵，因此是3x3矩阵。 ```java public static double[] launchToWgs84(double launchX, double launchY, double launchZ, double originLon, double originLat, double originHeight, double azimuth) { // Step 1: 将发射点转换为ECEF double[] originEcef = wgs84ToEcef(originLon, originLat, originHeight); double X0 = originEcef[0]; double Y0 = originEcef[1]; double Z0 = originEcef[2]; // Step 2: 计算旋转矩阵 // 将方位角转换为弧度 double alpha = Math.toRadians(azimuth); double lambda = Math.toRadians(originLon); // 发射点经度（弧度） double phi = Math.toRadians(originLat); // 发射点纬度（弧度） // 构造发射坐标系到ENU坐标系的旋转矩阵 // 注意：发射坐标系：x轴沿发射方向（方位角α），y轴在水平面内垂直于发射方向（向右），z轴向下。 // ENU坐标系：x东，y北，z天。 // 发射坐标系的x轴在ENU坐标系中为：东方向分量：sin(α), 北方向分量：cos(α) -> 所以ENU坐标系的x分量（东）为sin(α)*x - cos(α)*y? // 实际上，发射坐标系中的向量v_launch = [x, y, z]^T，在ENU坐标系中： // v_enu_x = sin(α) * x - cos(α) * y (因为y轴在发射坐标系中向右，在ENU中相当于与x轴垂直，且方向为（-cosα, sinα）) // v_enu_y = cos(α) * x + sin(α) * y // v_enu_z = -z (因为发射坐标系的z向下，而ENU的z向上) // 因此，旋转矩阵R_launch_to_enu为： // [ sin(α) -cos(α) 0 ] // [ cos(α) sin(α) 0 ] // [ 0 0 -1 ] double sinA = Math.sin(alpha); double cosA = Math.cos(alpha); double[][] R_launch_to_enu = { {sinA, -cosA, 0}, {cosA, sinA, 0}, {0, 0, -1} }; // 构造ENU到ECEF的旋转矩阵 // 公式： // [ -sin(lambda) -sin(phi)*cos(lambda) cos(phi)*cos(lambda) ] // [ cos(lambda) -sin(phi)*sin(lambda) cos(phi)*sin(lambda) ] // [ 0 cos(phi) sin(phi) ] double sinLambda = Math.sin(lambda); double cosLambda = Math.cos(lambda); double sinPhi = Math.sin(phi); double cosPhi = Math.cos(phi); double[][] R_enu_to_ecef = { {-sinLambda, -sinPhi*cosLambda, cosPhi*cosLambda}, {cosLambda, -sinPhi*sinLambda, cosPhi*sinLambda}, {0, cosPhi, sinPhi} }; // 计算总的旋转矩阵：R = R_enu_to