当前位置：首页 > news >正文

舰船摇摆下的坐标变换技术

news 2025/8/26 13:04:25

为了根据目标的经纬高（BLH）和转台自身的经纬高（BLH）以及本舰的姿态（横摇、纵摇、艏摇）求解转台的指令角（方位角和高低角），并通过C++程序实现，我们需要完成以下步骤：

将目标和转台的BLH坐标转换为地心地固坐标系（ECEF）坐标。
计算目标相对于转台的ECEF向量。
将该向量转换为东北地坐标系（NED）中的向量。
使用本舰姿态将NED向量转换到舰船坐标系。
在舰船坐标系中计算方位角和高低角。

以下是完整的C++代码实现，包括必要的数学计算和坐标变换。

#include <iostream>
#include <cmath>
#include <vector>// 定义WGS84椭球参数
const double WGS84_A = 6378137.0; // 半长轴
const double WGS84_F = 1.0 / 298.257223563; // 扁率
const double WGS84_E2 = 2 * WGS84_F - WGS84_F * WGS84_F; // 第一偏心率平方// 结构体存储BLH坐标
struct BLH {double lat; // 纬度，弧度double lon; // 经度，弧度double height; // 高度，米
};// 结构体存储姿态角（横摇、纵摇、艏摇），弧度
struct Attitude {double roll; // 横摇 φdouble pitch; // 纵摇 θdouble yaw; // 艏摇 ψ
};// 将BLH转换为ECEF坐标
std::vector<double> blhToEcef(const BLH& blh) {double sinLat = sin(blh.lat);double cosLat = cos(blh.lat);double sinLon = sin(blh.lon);double cosLon = cos(blh.lon);double N = WGS84_A / sqrt(1 - WGS84_E2 * sinLat * sinLat);double x = (N + blh.height) * cosLat * cosLon;double y = (N + blh.height) * cosLat * sinLon;double z = (N * (1 - WGS84_E2) + blh.height) * sinLat;return {x, y, z};
}// 计算从ECEF到NED的旋转矩阵
std::vector<std::vector<double>> ecefToNedMatrix(const BLH& blh) {double sinLat = sin(blh.lat);double cosLat = cos(blh.lat);double sinLon = sin(blh.lon);double cosLon = cos(blh.lon);std::vector<std::vector<double>> R = {{-sinLat * cosLon, -sinLat * sinLon, cosLat},{-sinLon, cosLon, 0},{-cosLat * cosLon, -cosLat * sinLon, -sinLat}};return R;
}// 计算从NED到舰船坐标系的旋转矩阵（基于姿态）
std::vector<std::vector<double>> nedToBodyMatrix(const Attitude& att) {double sinRoll = sin(att.roll);double cosRoll = cos(att.roll);double sinPitch = sin(att.pitch);double cosPitch = cos(att.pitch);double sinYaw = sin(att.yaw);double cosYaw = cos(att.yaw);// 旋转矩阵：R_body_ned = R_x(roll) * R_y(pitch) * R_z(yaw)std::vector<std::vector<double>> R = {{cosPitch * cosYaw, cosPitch * sinYaw, -sinPitch},{sinRoll * sinPitch * cosYaw - cosRoll * sinYaw, sinRoll * sinPitch * sinYaw + cosRoll * cosYaw, sinRoll * cosPitch},{cosRoll * sinPitch * cosYaw + sinRoll * sinYaw, cosRoll * sinPitch * sinYaw - sinRoll * cosYaw, cosRoll * cosPitch}};return R;
}// 矩阵乘以向量
std::vector<double> matrixMultiply(const std::vector<std::vector<double>>& M, const std::vector<double>& v) {std::vector<double> result(3, 0.0);for (int i = 0; i < 3; i++) {for (int j = 0; j < 3; j++) {result[i] += M[i][j] * v[j];}}return result;
}// 计算指令角：方位角和高低角（弧度）
void computePointingAngles(const BLH& targetBLH, const BLH& platformBLH, const Attitude& platformAtt, double& az, double& el) {// 1. 将目标和平台BLH转换为ECEFstd::vector<double> targetECEF = blhToEcef(targetBLH);std::vector<double> platformECEF = blhToEcef(platformBLH);// 2. 计算相对ECEF向量std::vector<double> deltaECEF = {targetECEF[0] - platformECEF[0],targetECEF[1] - platformECEF[1],targetECEF[2] - platformECEF[2]};// 3. 计算ECEF到NED的旋转矩阵std::vector<std::vector<double>> R_ecef2ned = ecefToNedMatrix(platformBLH);// 将相对ECEF向量转换到NED坐标系std::vector<double> deltaNED = matrixMultiply(R_ecef2ned, deltaECEF);// 4. 计算NED到舰船坐标系的旋转矩阵std::vector<std::vector<double>> R_ned2body = nedToBodyMatrix(platformAtt);// 将NED向量转换到舰船坐标系std::vector<double> bodyVec = matrixMultiply(R_ned2body, deltaNED);double x_b = bodyVec[0];double y_b = bodyVec[1];double z_b = bodyVec[2];// 5. 计算方位角和高低角az = atan2(y_b, x_b); // 方位角：atan2(y_b, x_b)double r = sqrt(x_b * x_b + y_b * y_b);el = atan2(-z_b, r); // 高低角：因为舰船坐标系z向下，所以用-z_b
}int main() {// 示例数据BLH targetBLH;targetBLH.lat = 0.5; // 弧度targetBLH.lon = 0.5; // 弧度targetBLH.height = 1000.0; // 米BLH platformBLH;platformBLH.lat = 0.5; // 弧度platformBLH.lon = 0.5; // 弧度platformBLH.height = 0.0; // 米Attitude platformAtt;platformAtt.roll = 0.1; // 弧度platformAtt.pitch = 0.05; // 弧度platformAtt.yaw = 1.0; // 弧度double az, el;computePointingAngles(targetBLH, platformBLH, platformAtt, az, el);// 输出结果（弧度）std::cout << "Azimuth (rad): " << az << std::endl;std::cout << "Elevation (rad): " << el << std::endl;// 转换为度数输出std::cout << "Azimuth (deg): " << az * 180.0 / M_PI << std::endl;std::cout << "Elevation (deg): " << el * 180.0 / M_PI << std::endl;return 0;
}