|
|
|
|
|
|
|
|
|
|
|
/*********版权所有（C）2020，武汉高德红外股份有限公司***************************************
|
|
|
|
|
|
* 文件名称： Arith_ZHRPY.h
|
|
|
|
|
|
* 文件标识： 有源定位模块标准化
|
|
|
|
|
|
* 内容摘要： 光电伺服坐标系 <-> 目标大地坐标系，基于北-天-东 轴序
|
|
|
|
|
|
* 其它说明：
|
|
|
|
|
|
* 当前版本：2.0
|
|
|
|
|
|
* 创建作者：11531yj
|
|
|
|
|
|
* 创建日期：2023.10.26
|
|
|
|
|
|
* 包含关系：
|
|
|
|
|
|
*****************************************************************************************/
|
|
|
|
|
|
#ifndef Arith_ZHRPY_h__
|
|
|
|
|
|
#define Arith_ZHRPY_h__
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#include <math.h>
|
|
|
|
|
|
#include <memory.h>
|
|
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
|
|
|
