/*********版权所有（C）2020，武汉高德红外股份有限公司*************************************** * 文件名称： Arith_ZHRPY.h * 文件标识： 有源定位模块标准化 * 内容摘要： 光电伺服坐标系 <-> 目标大地坐标系，基于北-天-东 轴序 * 其它说明： * 当前版本：2.0 * 创建作者：11531yj * 创建日期：2023.10.26 * 包含关系： *****************************************************************************************/ #ifndef Arith_ZHRPY_h__ #define Arith_ZHRPY_h__ #include #include namespace sRPYCacl { /******************************************************** 坐标系示意图: * y天 x北（载机前进方向/机头方向） ▕ ▕ ╱ ▕ ╱ ▕-——————————z东 * x指北为0°顺时针转为正方向（0-360°） * y指天，水平为0° 车头向上为正（0-90°） * z指东，水平为0° 车体右倾向下为正（-180-180°）/ ********************************************************** *系统符号定义 **********************************************************/ #define ANGLE(x) (x / 3.141592653589793 * 180) #define RADIAN(x) (x / 180 * 3.141592653589793) #define ABS(x) ((x) < 0 ? -(x) : (x)) //取x的绝对值 #define MIN(a, b) ((a) < (b) ? (a) : (b)) //取a、b两者中的最小值 #define MAX(a, b) ((a) > (b) ? (a) : (b)) //取a、b两者中的最大值 #define PI 3.141592653589793 #define AzimuthMax 360 #define PitchMax 80 #define PitchMin -5 #define Standardzation 1 //1/0 1 标定误差模式 / 0 修正后定位模式 #define RPYMode 1 // 1 载机姿态角参与计算 / 0 载荷姿态角参与计算 #define ROWS 5 //读数据的行数 #define COLS 9 //读数据的列数 typedef double DOUBLE64; typedef int SINT32; const double a = 6378137; //地球长轴 const double f = 1 / 298.257222101; //地球椭偏率 const double b = 6356752.3141; //地球短轴 const double ee = 0.0066943800229; //地球第一偏心率 const double epep = 0.00673949677548; //地球第二偏心率 const int FrameRate = 20; //帧频HZ 729帧频50HZ、5kg帧频20HZ /********************************************************** *数据结构定义 **********************************************************/ //光电伺服坐标系到光电基准坐标系的安装旋角误差 typedef struct tgAngle3D { DOUBLE64 dHAngle; //航向夹角 DOUBLE64 dVAngle; //俯仰夹角 DOUBLE64 dSAngle; //横滚夹角 }Angle3D; //光电基准到载体惯导直角坐标系位置偏差 typedef struct tgBiasXYZ { DOUBLE64 deltaX; //X坐标的偏差 DOUBLE64 deltaY; //Y坐标的偏差 DOUBLE64 deltaZ; //Z坐标的偏差 }BiasXYZ; //RPY姿态角 typedef struct tgEulerRPY { DOUBLE64 Yaw; //方位角 DOUBLE64 Pitch; //俯仰角 DOUBLE64 Roll; //横滚角 }EulerRPY; //定义空间直角坐标系 typedef struct tgPointXYZ { DOUBLE64 X; DOUBLE64 Y; DOUBLE64 Z; }PointXYZ; //定义大地坐标系 typedef struct tgPointBLH { DOUBLE64 B; //纬度 DOUBLE64 L; //经度 DOUBLE64 H; //高度 }PointBLH; //定义极坐标系 typedef struct tgPointPole { DOUBLE64 Pitch; //俯仰角 DOUBLE64 Azimuth; //方位角 DOUBLE64 distance; //极径 DOUBLE64 fRoll; //横滚 }Pole; //目标结构体 typedef struct stTrgOutput { unsigned char nObjID; //目标编号 double tLon; //目标经度 double tLat; //目标纬度 double tH; //目标高度 double tX; //地心地固，X double tY; //地心地固，Y double tZ; //地心地固，Z double tSpeedX; //目标X方向速度 double tSpeedY; //目标Y方向速度 double tSpeedZ; //目标Z方向速度 double Speed; //目标的合速度 }stTrgOutput; //载机结构体 typedef struct stTrackPlatInfo { unsigned char nPlaneID; //载机编号 double fLon; //载机经度 double fLat; //载机纬度 double fWGS_H; //载机高度 double fPlatAz; //载机航向角 double fPlatPt; //载机俯仰角 double fPlatRoll; //载机横滚角 double fServoAz; //伺服方位角 double fServoPt; //伺服俯仰角 double fRoll; //伺服横滚角 double x; //CGCS直角坐标x double y; //CGCS直角坐标y double z; //CGCS直角坐标z double Distance; //激光测距值 double tarLon; //目标真实经度 double tarLat; //目标真实纬度 double tarH; //目标真实高度 }stTrackPlatInfo; //标定误差参数设置 void setBiasPara(Angle3D EoServoToEoBias, BiasXYZ EoToZTPosBias); /********************************************************** *函数声明 **********************************************************/ ///////////////////////////////////////光电目标测角上报//////////////////////////////////////////////// //光电吊舱伺服极坐标 转 （北天东）地理极坐标 Pole getNUEPoleFromEOServoPole(Pole targetServoPole, EulerRPY stEulerRPY); //光电吊舱伺服极坐标 转 光电伺服直角坐标 PointXYZ getXYZFromPole(Pole targetServoPole); //光电伺服直角坐标 转 光电基准直角坐标 PointXYZ getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, Angle3D eoServoToEoBias); //光电基准直角坐标 转 载体惯导直角坐标 PointXYZ getZTXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, BiasXYZ eoToZTPosBias); //函数功能：用于计算最优修正矩阵AngleError3D；在转换过程中找最优的修正矩阵 [载体惯导直角坐标 ->北天东直角坐标 -> 北天东极坐标-> 目标大地坐标(经纬高)] PointBLH getPosFromZTXYZ(PointXYZ targetZTXYZ, EulerRPY stEulerRPY, PointBLH selfPointBLH, PointBLH targetBLH, double* Sum_Error_Min, Angle3D* AngleError3D); //载体惯导直角坐标 转 地理直角坐标 PointXYZ getNUEXYZFromZTXYZ(PointXYZ targetZTXYZ, EulerRPY stEulerRPY); //地理直角坐标 转 地理极坐标 Pole getPoleFromXYZ(PointXYZ targetNUEXYZ); ////////////////////////////////目标大地坐标系下经纬高解算/////////////////////////////////// //目标的地理极坐标 转 大地坐标系下的坐标 PointBLH getPosFromNUEPole(Pole targetNUEPole, PointBLH selfPointBLH); //载机自身的经纬高 转 地心空间直角坐标系 PointXYZ getXYZFromBLH(PointBLH selfPointBLH); //目标的地理极坐标 转 地理直角坐标 PointXYZ getZTFromPole(Pole targetPole); //目标的地心空间直角坐标 PointXYZ getCGCSFromNUE(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ); //目标的地心空间直角坐标转地心大地坐标 PointBLH getBLHFromXYZ(PointXYZ pointXYZ); ///////////////////////////////////////目标地理导引////////////////////////////////////////////////////////// //目标经纬高、载机自身经纬高 转 到 地理极坐标 Pole getNUEPoleFromENUXYZ(PointBLH ptselfBLH, PointBLH ptTargetBLH); //从地心地固直角坐标 转 极坐标 Pole getPoleFromCGCS(PointXYZ ptTargetXYZ, PointXYZ ptSelfXYZ); // 从地心地固直角坐标 转 北天东直角坐标 PointXYZ getENUFromCGCS(PointXYZ ptTargetXYZ, PointXYZ ptSelfXYZ); // 从载体直角坐标 转 极坐标 Pole getPoleFromZT(PointXYZ targetPointXYZ); //（北天东）地理极坐标 转 光电伺服坐标系下的极坐标 Pole getEOServoPoleFromNUEPole(Pole targetNUEPole, EulerRPY stEulerRPY); //地理直角坐标 转 载体惯导直角坐标 PointXYZ getZTXYZFromNUEXYZ(PointXYZ targetNUEXYZ, EulerRPY nueToZTBias); //载体惯导坐标系下的直角坐标 转 光电基准系下的直角坐标 PointXYZ getEOBaseXYZFromZTXYZ(PointXYZ targetZTXYZ, BiasXYZ ztToEOBaseBias); //光电基准系直角 转 光电伺服坐标系下直角坐标 PointXYZ getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, Angle3D eoBaseToEOServoBias); //直角坐标转极坐标（右手系） Pole getPoleFromXYZ(PointXYZ targetXYZ); //极坐标转直角坐标（右手系） PointXYZ getXYZFromPole(Pole targetPole); //误差修正函数：函数返回值为修正矩阵，函数只调用一次 Angle3D getAngle3DFromEOServoPole(Pole targetServoPole, EulerRPY stEulerRPY, PointBLH selfPointBLH, PointBLH targetBLH, double* Sum_Error_Min, Angle3D* AngleError3D); Angle3D TarGetLocate_Cal(stTrackPlatInfo mytrack); EulerRPY Zh_dRPY(stTrackPlatInfo track); EulerRPY Zh_dRPYDown(stTrackPlatInfo track); //矩阵乘法 void MultiMat(DOUBLE64* A, DOUBLE64* B, DOUBLE64* AB, SINT32 m, SINT32 n, SINT32 p); //M1(3*3) * M2(3*3) void MultiMat33(DOUBLE64* A, DOUBLE64* B, DOUBLE64* AB); //M1(3*3) * M2(3*3) * M3(3*3) void MultiMat333(DOUBLE64* A, DOUBLE64* B, DOUBLE64* C, DOUBLE64* ABC); //坐标(矢量)旋转: M1(3*3) * M2(3,1) PointXYZ RtPoint(DOUBLE64* M, PointXYZ Point); //绕X轴旋转矩阵 void Rx(DOUBLE64 omega, DOUBLE64* Opp); //绕Y轴旋转矩阵 void Ry(DOUBLE64 omega, DOUBLE64* Opp); //绕Z轴旋转矩阵 void Rz(DOUBLE64 omega, DOUBLE64* Opp); void R_y(DOUBLE64 omega, DOUBLE64* Opp); EulerRPY SumRPYCacl(Pole ServoAngle, EulerRPY s_RPYAngle); EulerRPY SumRPYCaclDown(Pole ServoAngle, EulerRPY s_RPYAngle); } #endif