/*********版权所有（C）2020，武汉高德红外股份有限公司*************************************** * 文件名称： Arith_ZHRPY.cpp * 文件标识：有源定位模块标准化 * 内容摘要：光电伺服坐标系 <-> 目标大地坐标系，基于北-天-东轴序 * 其它说明： * 当前版本：2.0 * 创建作者：11531yj * 创建日期：2023.10.26 *****************************************************************************************/ #include "Arith_zhryp.h" #include #include #include #include using namespace std; using namespace sRPYCacl; /********************************************************** *标定偏差 **********************************************************/ Angle3D g_SlantingAngleBias = { 0 }; //光电伺服与光电基准坐标系的安装旋角误差// BiasXYZ g_PositionXYZBias = { 0 }; //光电基准与平台直角坐标系位置偏差// /********************************************************** *标定误差参数设置 **********************************************************/ void sRPYCacl::setBiasPara(Angle3D eoServoToEoBias, BiasXYZ eoToZTPosBias) { g_SlantingAngleBias.dHAngle = eoServoToEoBias.dHAngle; //光电伺服与光电基准坐标系的航向角误差// g_SlantingAngleBias.dVAngle = eoServoToEoBias.dVAngle; //光电伺服与光电基准坐标系的俯仰角误差// g_SlantingAngleBias.dSAngle = eoServoToEoBias.dSAngle; //光电伺服与光电基准坐标系的横滚角误差// g_PositionXYZBias.deltaX = eoToZTPosBias.deltaX; //光电基准与平台直角坐标系X坐标的偏差 g_PositionXYZBias.deltaY = eoToZTPosBias.deltaY; //光电基准与平台直角坐标系Y坐标的偏差 g_PositionXYZBias.deltaZ = eoToZTPosBias.deltaZ; //光电基准与平台直角坐标系Z坐标的偏差 } /********************************************************************************************* * 函数名称：误差修正函数 * 功能描述：计算误差修正矩阵 * 输入参数： * targetServoPole: 光电吊舱伺服极坐标 * stEulerRPY: 载体惯导姿态角 * selfPointBLH: 载机自身经纬高 * targetBLH：目标真实经纬高（修正模式需要已知目标的真实经纬高） * 输出参数：AngleError3D: 姿态角度偏差（误差修正矩阵） * NUEPole: 北、天、东极坐标 * Sum_Error_Min：最小误差 * 返回值：AngleError3D：误差修正矩阵 * 备注：函数只运行一次 *********************************************************************************************/ Pole sRPYCacl::getNUEPoleFromEOServoPole(Pole targetServoPole, EulerRPY stEulerRPY) { //伺服方位角限制在0-360 if (targetServoPole.Azimuth > 360) { targetServoPole.Azimuth -= AzimuthMax; } //限制伺服俯仰角度// (80、-5、360改成宏(已改20231115)) alpha改成方位俯仰对应的Amizith targetServoPole.Pitch = MIN(targetServoPole.Pitch, PitchMax); targetServoPole.Pitch = MAX(targetServoPole.Pitch, PitchMin); //1.光电吊舱伺服极坐标转光电伺服直角坐标 PointXYZ targetServoXYZ = getXYZFromPole(targetServoPole); //2.光电伺服直角坐标转光电基准直角坐标 PointXYZ targetEOBaseXYZ = getEOBaseXYZFromEOServoXYZ(targetServoXYZ, g_SlantingAngleBias); //3.光电基准直角坐标转载体惯导直角坐标 PointXYZ targetZTXYZ = getZTXYZFromEOBaseXYZ(targetEOBaseXYZ, g_PositionXYZBias); //4.载体惯导直角坐标转地理直角坐标 PointXYZ targetNUEXYZ = getNUEXYZFromZTXYZ(targetZTXYZ, stEulerRPY); //5.地理直角坐标转地理极坐标 Pole targetNUEPole = getPoleFromXYZ(targetNUEXYZ); //上报目标北天东地理极坐标 return targetNUEPole; } /********************************************************************************************* * 功能描述：光电吊舱伺服直角坐标转光电基准直角坐标 * 输入参数：targetServoXYZ：光电吊舱伺服直角坐标 * dSAngle：横滚角补偿角 * dHAngle：航向角补偿角 * dVAngle：俯仰角补偿角 * 输出参数：无 * 返回值：光电基准直角坐标 *********************************************************************************************/ PointXYZ sRPYCacl::getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, Angle3D eoServoToEoBias) { //先绕x轴横滚旋转 -dSAngle，再绕z轴俯仰旋转 -dVAngle，再绕y轴方位旋转-dHAngle DOUBLE64 oppRx[9]; DOUBLE64 oppRy[9]; DOUBLE64 oppRz[9]; DOUBLE64 oppRyzx[9]; //计算旋转矩阵 Rx(-eoServoToEoBias.dSAngle, oppRx); Rz(-eoServoToEoBias.dVAngle, oppRz); Ry(-eoServoToEoBias.dHAngle, oppRy); MultiMat333(oppRy, oppRz, oppRx, oppRyzx); //坐标转换 return RtPoint(oppRyzx, targetServoXYZ); } /********************************************************************************************* * 功能描述：光电基准直角坐标系转载体惯导直角坐标系 * 输入参数：targetEOBaseXYZ：光电基准直角坐标 * deltaX： X坐标的偏移量 * deltaY： Y坐标的偏移量 * deltaZ： Z坐标的偏移量 * 输出参数：无 * 返回值：载体惯导直角坐标 *********************************************************************************************/ PointXYZ sRPYCacl::getZTXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, BiasXYZ eoToZTPosBias) { //平台坐标系下目标坐标 PointXYZ targetZTXYZ; targetZTXYZ.X = targetEOBaseXYZ.X + eoToZTPosBias.deltaX; targetZTXYZ.Y = targetEOBaseXYZ.Y + eoToZTPosBias.deltaY; targetZTXYZ.Z = targetEOBaseXYZ.Z + eoToZTPosBias.deltaZ; return targetZTXYZ; } /********************************************************************************************* * 功能描述：载体惯导直角坐标转地理直角坐标 * 输入参数：targetZTXYZ：载体惯导直角坐标 * stEulerRPY：惯导的姿态角 * selfPointBLH：载体自身经纬高 * targetBLH 目标真实经纬高（误差标定模式下给出） * 输出参数：double *Sum_Error_Min 最优定位目标，真实经纬高误差和，即定位精度（单位/米） Angle3D *AngleError3D 载机方位、俯仰、横滚偏差，即最优修正矩阵（单位/°） Pole *NUEPole 最优定位下，目标北天东极坐标 * 返回值：地理目标经纬高 *********************************************************************************************/ PointBLH sRPYCacl::getPosFromZTXYZ(PointXYZ targetZTXYZ, EulerRPY stEulerRPY, PointBLH selfPointBLH, PointBLH targetBLH, double* Sum_Error_Min, Angle3D* AngleError3D) { DOUBLE64 oppRx[9]; DOUBLE64 oppRy[9]; DOUBLE64 oppRz[9]; DOUBLE64 oppRyzx[9]; PointBLH pointBLH = { 0 }; PointBLH pointMinBLH = { 0 }; EulerRPY stEulerRPYAddError; // double Sum_Error_Min_Yaw, Sum_Error_Min_Pitch, Sum_Error_Min_Roll; //定位精度最小时偏航角/俯仰角/横滚角 double i, j, k; //横滚角度i / 偏航角度j / 俯仰角度k偏差// // double Error_Yaw = 0; //方位角偏差// // double Error_Pitch = 0; //俯仰角偏差// // double Error_Roll = 0; //横滚角偏差// double Error_L = 0; //经、纬误差转化成米// double Error_B = 0; double Error_H = 0; double Sum_Error; for (i = -0.5; i <= 0.5; i = i + 0.05) { //横滚// for (j = -8; j <= 8; j = j + 0.05) { //方位// for (k = -1; k <= 1; k = k + 0.05) { //俯仰// // Error_Yaw = j * PI / 180; // Error_Roll = i * PI / 180; // Error_Pitch = k * PI / 180; //给量测载机姿态角添加误差// //载机姿态角参与计算 20240229 stEulerRPYAddError.Roll = stEulerRPY.Roll + i; stEulerRPYAddError.Pitch = stEulerRPY.Pitch + k; stEulerRPYAddError.Yaw = stEulerRPY.Yaw + j; //计算旋转矩阵// Rx(-stEulerRPYAddError.Roll, oppRx); Rz(-stEulerRPYAddError.Pitch, oppRz); Ry(-stEulerRPYAddError.Yaw, oppRy); MultiMat333(oppRy, oppRz, oppRx, oppRyzx); PointXYZ targetNUEXYZ = RtPoint(oppRyzx, targetZTXYZ); //地理直角坐标转地理极坐标 // Pole targetNUEPole = getPoleFromXYZ(targetNUEXYZ); //2、由目标的北天东极坐标转到经、纬、高。 stTrgOutput Target1; stTrgOutput* Target4 = &Target1; pointBLH = getPosFromNUEPole(targetNUEPole, selfPointBLH); //0131zqq: 计算经纬误差 Error_L = fabs(pointBLH.L - targetBLH.L) * 111000; Error_B = fabs(pointBLH.B - targetBLH.B) * 111000; Error_H = fabs(pointBLH.H - targetBLH.H); Sum_Error = sqrt(Error_L * Error_L + Error_B * Error_B + Error_H * Error_H); if (Sum_Error < *Sum_Error_Min) { // Sum_Error_Min_Yaw = Error_Yaw * 57.3; // Sum_Error_Min_Pitch = Error_Pitch * 57.3; // Sum_Error_Min_Roll = Error_Roll * 57.3; *Sum_Error_Min = Sum_Error; if (*Sum_Error_Min < 100) { pointMinBLH = pointBLH; //返回误差最小的经纬高// AngleError3D->dHAngle = j; //方位误差// AngleError3D->dVAngle = k; //俯仰误差// AngleError3D->dSAngle = i; //横滚误差// } } } } } return pointMinBLH; } /********************************************************************************************* * 功能描述：载体惯导直角坐标转地理直角坐标 * 输入参数：targetZTXYZ：载体惯导直角坐标 * stEulerRPY：惯导的姿态角 * * 输出参数：无 * 返回值：目标地理直角坐标 *********************************************************************************************/ PointXYZ sRPYCacl::getNUEXYZFromZTXYZ(PointXYZ targetZTXYZ, EulerRPY stEulerRPY) { DOUBLE64 oppRx[9]; DOUBLE64 oppRy[9]; DOUBLE64 oppRz[9]; DOUBLE64 oppRyzx[9]; //计算旋转矩阵// Rx(-stEulerRPY.Roll, oppRx); Rz(-stEulerRPY.Pitch, oppRz); Ry(-stEulerRPY.Yaw, oppRy); MultiMat333(oppRy, oppRz, oppRx, oppRyzx); return RtPoint(oppRyzx, targetZTXYZ); } /********************************************************************************************* * 函数名称：目标大地坐标系经纬高解算 * 功能描述：由地理极坐标转到大地坐标 * 输入参数：targetNUEPole：目标地理极坐标 * selfPointBLH: 载机的经纬高 * 输出参数：无 * 返回值：大地坐标系B、L、H *********************************************************************************************/ PointBLH sRPYCacl::getPosFromNUEPole(Pole targetNUEPole, PointBLH selfPointBLH) { //载机自身的经纬高转地心空间直角坐标系// PointXYZ selfPointXYZ = getXYZFromBLH(selfPointBLH); //目标的地理极坐标转地理直角坐标// PointXYZ targetPointXYZ = getZTFromPole(targetNUEPole); //目标的地心空间直角坐标// PointXYZ targetCGCSPointXYZ = getCGCSFromNUE(targetPointXYZ, selfPointXYZ); //目标的地心空间直角坐标转地心大地坐标 return getBLHFromXYZ(targetCGCSPointXYZ); } /********************************************************************************************* * 功能描述：将载机的经纬高B、L、H转为地心空间直角坐标 * 输入参数：pointBLH：载机的经纬高 * 输出参数：无 * 返回值：地心空间直角坐标 *********************************************************************************************/ PointXYZ sRPYCacl::getXYZFromBLH(PointBLH pointBLH) { PointXYZ pointXYZ; DOUBLE64 W, N; W = sqrt(1 - ee * pow(sin(RADIAN(pointBLH.B)), 2)); N = a / W; // 椭球的卯酉圈曲率// //将B、L、H变换到地心空间直角坐标 pointXYZ.X = (N + pointBLH.H) * cos(RADIAN(pointBLH.B)) * cos(RADIAN(pointBLH.L)); pointXYZ.Y = (N + pointBLH.H) * cos(RADIAN(pointBLH.B)) * sin(RADIAN(pointBLH.L)); pointXYZ.Z = (N * (1 - ee) + pointBLH.H) * sin(RADIAN(pointBLH.B)); return pointXYZ; } /********************************************************************************************* * 功能描述：由目标的地理极坐标变换到目标的地理直角坐标 * 输入参数：targetPole：目标的地理极坐标 * 输出参数：无 * 返回值：目标的地理直角坐标 *********************************************************************************************/ PointXYZ sRPYCacl::getZTFromPole(Pole targetPole) { PointXYZ result; DOUBLE64 aa, bb, cc; cc = sin(RADIAN(targetPole.Pitch)) * targetPole.distance; aa = sin(RADIAN(targetPole.Azimuth)) * sqrt(fabs(targetPole.distance * targetPole.distance - cc * cc)); bb = sqrt(fabs(targetPole.distance * targetPole.distance - aa * aa - cc * cc)); if (targetPole.Azimuth > 90 && targetPole.Azimuth < 270) { bb = -bb; } //此处的X、Y、Z为目标的北天东地理直角坐标 20231125yj result.X = bb; result.Y = cc; result.Z = aa; return result; } /********************************************************************************************* * 功能描述：由目标的地理直角坐标转目标的地心直角坐标 * 输入参数： * targetPointXYZ：目标的地理直角坐标 * selfPointXYZ：载机的地心空间直角坐标 * 输出参数：无 * 返回值：目标的地心直角坐标 *********************************************************************************************/ PointXYZ sRPYCacl::getCGCSFromNUE(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ) { DOUBLE64 dx, dy, dz, sinL, cosL, sinB, cosB; PointXYZ result; //载机自身的X、Y、Z转载机自身的经纬高 PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ); //目标的北天东地理直角坐标// dx = targetPointXYZ.X; dy = targetPointXYZ.Y; dz = targetPointXYZ.Z; sinL = sin(RADIAN(selfPointBLH.L)); cosL = cos(RADIAN(selfPointBLH.L)); sinB = sin(RADIAN(selfPointBLH.B)); cosB = cos(RADIAN(selfPointBLH.B)); //目标的地心直角坐标=目标相对于载机的地心坐标+载机的地心直角坐标// result.X = -sinB * cosL * dx + cosB * cosL * dy - sinL * dz + selfPointXYZ.X; //修改了dx，dy，dz矩阵系数 20231125yj result.Y = -sinB * sinL * dx + cosB * sinL * dy + cosL * dz + selfPointXYZ.Y; result.Z = cosB * dx + sinB * dy + selfPointXYZ.Z; return result; } /********************************************************************************************* * 功能描述: 地心直角坐标转大地坐标 * 输入参数 PointXYZ：地心直角坐标 * 输出参数：无 * 返回值：大地坐标系的B、L、H * 备注：在XYZ转BLH过程中，加了对L取值前的判断. yj2023.11.9 * 若转后的值与之前对不上，考虑arctan取值相差180度. yj2023.11.9 **********************************************************************************************/ PointBLH sRPYCacl::getBLHFromXYZ(PointXYZ pointXYZ) { DOUBLE64 U, B0, N, L, H, B; U = atan(pointXYZ.Z * a / (sqrt(pointXYZ.X * pointXYZ.X + pointXYZ.Y * pointXYZ.Y) * b)); B0 = atan((pointXYZ.Z + b * epep * pow(sin(U), 3)) / (sqrt(pointXYZ.X * pointXYZ.X + pointXYZ.Y * pointXYZ.Y) - a * ee * pow(cos(U), 3))); N = a / sqrt(1 - ee * pow(sin(B0), 2)); L = atan(pointXYZ.Y / pointXYZ.X); //在XYZ转到BLH过程中，atan的取值有多个解，对L取值分情况考虑 if (pointXYZ.X < 0 && L < 0) { L = L + PI; } else if (pointXYZ.X < 0 && L > 0) { L = L - PI; } else if (pointXYZ.X > 0) { L = L; } H = sqrt(pointXYZ.X * pointXYZ.X + pointXYZ.Y * pointXYZ.Y + pow(pointXYZ.Z + N * ee * sin(B0), 2)) - N; B = atan(pointXYZ.Z / sqrt(pointXYZ.X * pointXYZ.X + pointXYZ.Y * pointXYZ.Y) / (1 - ee * N / (N + H))); PointBLH pointBLH; pointBLH.B = ANGLE(B); pointBLH.L = ANGLE(L); pointBLH.H = H; return pointBLH; } /********************************************************************************************* * 功能描述：地理引导部分----已知真实目标点的经纬高、载机的经纬高计算东北天地理极坐标 * 输入参数：ptselfBLH：载机经纬高 * ptTargetBLH: 目标真实经纬高 * 输出参数：无 * 返回值：地理坐标系下的极坐标20240228 *********************************************************************************************/ Pole sRPYCacl::getNUEPoleFromENUXYZ(PointBLH ptselfBLH, PointBLH ptTargetBLH) { PointXYZ ptSelfXYZ = getXYZFromBLH(ptselfBLH); PointXYZ ptTargetXYZ = getXYZFromBLH(ptTargetBLH); Pole poleTarget = getPoleFromCGCS(ptTargetXYZ, ptSelfXYZ); return poleTarget; } Pole sRPYCacl::getPoleFromCGCS(PointXYZ ptTargetXYZ, PointXYZ ptSelfXYZ) { PointXYZ pointENUXYZ = getENUFromCGCS(ptTargetXYZ, ptSelfXYZ); Pole targetPole = getPoleFromZT(pointENUXYZ); if (targetPole.Pitch > 90) { targetPole.Pitch = 180 - targetPole.Pitch; targetPole.Azimuth += 180; } if (targetPole.Pitch < -90) { targetPole.Pitch = -180 - targetPole.Pitch; targetPole.Azimuth += 180; } if (targetPole.Azimuth > 360) { targetPole.Azimuth -= 360; } if (targetPole.Azimuth < 0) { targetPole.Azimuth += 360; } return targetPole; } PointXYZ sRPYCacl::getENUFromCGCS(PointXYZ ptTargetXYZ, PointXYZ ptSelfXYZ) { PointBLH selfPointBLH = getBLHFromXYZ(ptSelfXYZ); double dx = ptTargetXYZ.X - ptSelfXYZ.X; double dy = ptTargetXYZ.Y - ptSelfXYZ.Y; double dz = ptTargetXYZ.Z - ptSelfXYZ.Z; double sinL = sin(RADIAN(selfPointBLH.L)); double cosL = cos(RADIAN(selfPointBLH.L)); double sinB = sin(RADIAN(selfPointBLH.B)); double cosB = cos(RADIAN(selfPointBLH.B)); PointXYZ result; result.X = -sinL * dx + cosL * dy; result.Y = -sinB * cosL * dx - sinB * sinL * dy + cosB * dz; result.Z = cosB * cosL * dx + cosB * sinL * dy + sinB * dz; return result; } Pole sRPYCacl::getPoleFromZT(PointXYZ targetPointXYZ) { double x = targetPointXYZ.X; double y = targetPointXYZ.Y; double z = targetPointXYZ.Z; Pole result; result.distance = sqrt(x * x + y * y + z * z); result.Pitch = ANGLE(asin(z / result.distance)); result.Azimuth = ABS(ANGLE(asin(x / sqrt(x * x + y * y)))); if (y < 0 && x > 0) //90-180 { result.Azimuth = 180 - result.Azimuth; } else if (x < 0 && y <= 0) //180-270 { result.Azimuth = result.Azimuth + 180; } else if (x <= 0 && y >= 0) //270-360 { result.Azimuth = 360 - result.Azimuth; } return result; } /********************************************************************************************* * 函数名称：接受导引信息 * 功能描述：（北天东）地理极坐标转换到光电伺服坐标系下的极坐标 * 输入参数：targetNUEPole：（北天东）地理极坐标 * stEulerRPY: 载体惯导姿态角 * 输出参数：无 * 返回值：光电伺服坐标系下的极坐标 *********************************************************************************************/ Pole sRPYCacl::getEOServoPoleFromNUEPole(Pole targetNUEPole, EulerRPY stEulerRPY) { //1.地理坐标系下的极坐标转地理坐标系下的直角坐标 PointXYZ targetNUEXYZ = getXYZFromPole(targetNUEPole); //2.地理坐标系下的直角坐标转换为载体惯导坐标系下的直角坐标 PointXYZ targetZTXYZ = getZTXYZFromNUEXYZ(targetNUEXYZ, stEulerRPY); //3.载体惯导坐标系下的直角坐标转光电基准系下的直角坐标 PointXYZ targetEOBaseXYZ = getEOBaseXYZFromZTXYZ(targetZTXYZ, g_PositionXYZBias); //4.光电基准系直角转光电伺服坐标系下直角坐标 PointXYZ targetEOServoXYZ = getEOServoXYZFromEOBaseXYZ(targetEOBaseXYZ, g_SlantingAngleBias); //5.光电伺服坐标系下直角坐标转光电伺服坐标系下极坐标 Pole targetEOServoPole = getPoleFromXYZ(targetEOServoXYZ); return targetEOServoPole; } /********************************************************************************************* * 功能描述：地理坐标系下的直角坐标转换为载体坐标系下的直角坐标 * 输入参数：targetPointXYZ：地理坐标系直角坐标 * Roll：载体横滚角 * Yaw：载体航向角 * Pitch：载体俯仰角 * 输出参数：无 * 返回值：载体惯导直角坐标 **********************************************************************************************/ PointXYZ sRPYCacl::getZTXYZFromNUEXYZ(PointXYZ targetPointXYZ, EulerRPY stEulerRPY) { //先绕y轴方位旋转Yaw -> 再绕z轴俯仰旋转 Pitch -> 绕x轴横滚旋转 Roll DOUBLE64 oppRx[9]; DOUBLE64 oppRy[9]; DOUBLE64 oppRz[9]; DOUBLE64 oppRxzy[9]; //计算旋转矩阵 Ry(stEulerRPY.Yaw, oppRy); Rz(stEulerRPY.Pitch, oppRz); Rx(stEulerRPY.Roll, oppRx); MultiMat333(oppRx, oppRz, oppRy, oppRxzy); //坐标转换 return RtPoint(oppRxzy, targetPointXYZ); } /********************************************************************************************* * 功能描述：载体惯导坐标系下的直角坐标转光电基准系下的直角坐标 * 输入参数：targetZTXYZ：载体惯导坐标系下的直角坐标 * deltaX： X坐标的偏移量 * deltaY： Y坐标的偏移量 * deltaZ: Z坐标的偏移量 * 输出参数：无 * 返回值：目标在光电基准系下的直角坐标 **********************************************************************************************/ PointXYZ sRPYCacl::getEOBaseXYZFromZTXYZ(PointXYZ targetZTXYZ, BiasXYZ ztToEoBaseBias) { //平台坐标系下目标坐标 PointXYZ targetEOBaseXYZ; targetEOBaseXYZ.X = targetZTXYZ.X - ztToEoBaseBias.deltaX; targetEOBaseXYZ.Y = targetZTXYZ.Y - ztToEoBaseBias.deltaY; targetEOBaseXYZ.Z = targetZTXYZ.Z - ztToEoBaseBias.deltaZ; return targetEOBaseXYZ; } /********************************************************************************************* * 功能描述：光电基准系直角坐标转光电伺服坐标系下直角坐标 * 输入参数：targetEOBaseXYZ：光电基准系下目标的坐标 * dHAngle：航向角误差 * dVAngle：俯仰角误差 * dSAngle: 横滚角误差 * 输出参数：无 * 返回值：目标在光电伺服坐标系下的直角坐标 **********************************************************************************************/ PointXYZ sRPYCacl::getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, Angle3D eoBaseToEOServoBias) { //先绕y轴方位旋转dHAngle,再绕z轴俯仰旋转 dVAngle,再绕x轴横滚旋转 dSAngle DOUBLE64 oppRx[9]; DOUBLE64 oppRy[9]; DOUBLE64 oppRz[9]; DOUBLE64 oppRxzy[9]; //计算旋转矩阵 Rx(eoBaseToEOServoBias.dSAngle, oppRx); Rz(eoBaseToEOServoBias.dVAngle, oppRz); Ry(eoBaseToEOServoBias.dHAngle, oppRy); MultiMat333(oppRx, oppRz, oppRy, oppRxzy); //坐标转换 return RtPoint(oppRxzy, targetEOBaseXYZ); } /********************************************************************************************* * 功能描述：从直角坐标转换为极坐标 * 输入参数：直角坐标x,y,z * 输出参数：无 * 返回值：极坐标alpha、beta * 备注： 1.从光电伺服坐标系下直角坐标转光电伺服坐标系下极坐标 * 2.从地理直角坐标转地理极坐标 * （以上两个均调用这个函数） **********************************************************************************************/ Pole sRPYCacl::getPoleFromXYZ(PointXYZ targetXYZ) { DOUBLE64 x, y, z; x = targetXYZ.X; y = targetXYZ.Y; z = targetXYZ.Z; Pole result; result.distance = sqrt(x * x + y * y + z * z); result.Pitch = ANGLE(asin(y / result.distance)); result.Azimuth = ABS(ANGLE(atan(z / x))); //方位角约束0-360 if (x < 0 && z >= 0) // 90 - 180 { result.Azimuth = 180 - result.Azimuth; } else if (x < 0 && z < 0) // 180 - 270 { result.Azimuth = result.Azimuth + 180; } else if (x >= 0 && z < 0) // 270 - 360 { result.Azimuth = 360 - result.Azimuth; } return result; } /********************************************************************************************* * 功能描述：从极坐标到直角坐标 * 输入参数：极坐标alpha、beta * 输出参数：无 * 返回值：直角坐标x,y,z * 备注： 1.从光电伺服极坐标到光电伺服直角坐标 * 2.从地理极坐标到地理直角坐标 * （以上两个均调用这个函数） **********************************************************************************************/ //0131zqq：伺服由极坐标转为直角坐标// PointXYZ sRPYCacl::getXYZFromPole(Pole targetPole) { PointXYZ result = { 0 }; DOUBLE64 aa, bb, cc, ac; //激光或雷达测距时的判断(异常判断) if (targetPole.distance < 100) { targetPole.distance = 1000; } bb = sin(RADIAN(targetPole.Pitch)) * targetPole.distance; ac = sqrt(targetPole.distance * targetPole.distance - bb * bb); aa = cos(RADIAN(targetPole.Azimuth)) * ac; cc = sin(RADIAN(targetPole.Azimuth)) * ac; result.X = aa; result.Y = bb; result.Z = cc; return result; } // 绕X轴旋转矩阵横滚角 /* 1 0 0 0 cos sin 0 -sin cos *///备注：对应矩阵M3 void sRPYCacl::Rx(DOUBLE64 omega, DOUBLE64* Opp) { DOUBLE64 coso = cos(RADIAN(omega)); DOUBLE64 sino = sin(RADIAN(omega)); Opp[0] = 1; Opp[1] = 0; Opp[2] = 0; Opp[3] = 0; Opp[4] = coso; Opp[5] = sino; Opp[6] = 0; Opp[7] = -sino; Opp[8] = coso; } // 绕Y轴旋转矩阵方位角 /* cos 0 sin 0 1 0 -sin 0 cos */ void sRPYCacl::Ry(DOUBLE64 omega, DOUBLE64* Opp) { DOUBLE64 coso = cos(RADIAN(omega)); DOUBLE64 sino = sin(RADIAN(omega)); Opp[0] = coso; Opp[1] = 0; Opp[2] = sino; //S3311中此处为-sino Opp[3] = 0; Opp[4] = 1; Opp[5] = 0; Opp[6] = -sino; //S3311中此处为sino Opp[7] = 0; Opp[8] = coso; } // 绕Y轴旋转矩阵方位角对应矩阵M1、M4 /* cos 0 -sin 0 1 0 sin 0 cos *///M1、M4 void sRPYCacl::R_y(DOUBLE64 omega, DOUBLE64* Opp) { DOUBLE64 coso = cos(RADIAN(omega)); DOUBLE64 sino = sin(RADIAN(omega)); Opp[0] = coso; Opp[1] = 0; Opp[2] = -sino; Opp[3] = 0; Opp[4] = 1; Opp[5] = 0; Opp[6] = sino; Opp[7] = 0; Opp[8] = coso; } // 绕Z轴旋转矩阵俯仰角 /* cos sin 0 -sin cos 0 0 0 1 *///备注：对应矩阵M2、M5 void sRPYCacl::Rz(DOUBLE64 omega, DOUBLE64* Opp) { DOUBLE64 coso = cos(RADIAN(omega)); DOUBLE64 sino = sin(RADIAN(omega)); Opp[0] = coso; Opp[1] = sino; Opp[2] = 0; Opp[3] = -sino; Opp[4] = coso; Opp[5] = 0; Opp[6] = 0; Opp[7] = 0; Opp[8] = 1; } /********************************************************************************************* * 矩阵乘法，矩阵A*矩阵B *********************************************************************************************/ void sRPYCacl::MultiMat(DOUBLE64* A, DOUBLE64* B, DOUBLE64* AB, SINT32 m, SINT32 n, SINT32 p) { // A为m*n ; B为n*p ;输出AB为m*p SINT32 i, j, k; DOUBLE64 Multisum; for (i = 0; i < m; i++) //输出矩阵的m行 { for (j = 0; j < p; j++) //输出矩阵的p列 { Multisum = 0; for (k = 0; k < n; k++) Multisum += A[i * n + k] * B[k * p + j]; AB[i * p + j] = Multisum; } } } /********************************************************************************************* * 3阶矩阵乘法，矩阵A*矩阵B *********************************************************************************************/ void sRPYCacl::MultiMat33(DOUBLE64* A, DOUBLE64* B, DOUBLE64* AB) { MultiMat(A, B, AB, 3, 3, 3); } /********************************************************************************************* * 3阶矩阵乘法，矩阵A*矩阵B*矩阵C *********************************************************************************************/ void sRPYCacl::MultiMat333(DOUBLE64* A, DOUBLE64* B, DOUBLE64* C, DOUBLE64* ABC) { DOUBLE64 BC[9]; MultiMat(B, C, BC, 3, 3, 3); MultiMat(A, BC, ABC, 3, 3, 3); } /********************************************************************************************* * 3维空间旋转矩阵执行坐标转换 *********************************************************************************************/ //0131zqq：第一个参数有点问题，没用到！ PointXYZ sRPYCacl::RtPoint(DOUBLE64* M, PointXYZ targetPointXYZ) { DOUBLE64 A[3]; DOUBLE64 G[3]; A[0] = targetPointXYZ.X; A[1] = targetPointXYZ.Y; A[2] = targetPointXYZ.Z; MultiMat(M, A, G, 3, 3, 1); PointXYZ result; result.X = G[0]; result.Y = G[1]; result.Z = G[2]; return result; } /********************************************************************************************* * 函数名称：误差修正函数 * 功能描述：计算误差修正矩阵 * 输入参数： * targetServoPole: 光电吊舱伺服极坐标 * stEulerRPY: 载体惯导姿态角 * selfPointBLH: 载机自身经纬高 * targetBLH：目标真实经纬高（修正模式需要已知目标的真实经纬高） * 输出参数：AngleError3D: 姿态角度偏差（误差修正矩阵） * NUEPole: 北、天、东极坐标 * Sum_Error_Min：最小误差 * 返回值：AngleError3D：误差修正矩阵 * 备注：函数只运行一次 *********************************************************************************************/ Angle3D sRPYCacl::getAngle3DFromEOServoPole(Pole targetServoPole, EulerRPY stEulerRPY, PointBLH selfPointBLH, PointBLH targetBLH, double* Sum_Error_Min, Angle3D* AngleError3D) { //伺服方位角限制在0-360 if (targetServoPole.Azimuth > 360) { targetServoPole.Azimuth -= AzimuthMax; } //限制伺服俯仰角度// (80、-5、360改成宏(已改20231115)) alpha改成方位俯仰对应的Amizith targetServoPole.Pitch = MIN(targetServoPole.Pitch, PitchMax); targetServoPole.Pitch = MAX(targetServoPole.Pitch, PitchMin); //1.光电吊舱伺服极坐标转光电伺服直角坐标 PointXYZ targetServoXYZ = getXYZFromPole(targetServoPole); //2.光电伺服直角坐标转光电基准直角坐标 PointXYZ targetEOBaseXYZ = getEOBaseXYZFromEOServoXYZ(targetServoXYZ, g_SlantingAngleBias); //3.光电基准直角坐标转载体惯导直角坐标 PointXYZ targetZTXYZ = getZTXYZFromEOBaseXYZ(targetEOBaseXYZ, g_PositionXYZBias); //4.如果是标定模式，就通过标定函数getPosFromZTXYZ计算修正误差 getPosFromZTXYZ(targetZTXYZ, stEulerRPY, selfPointBLH, targetBLH, Sum_Error_Min, AngleError3D); return *AngleError3D; } /********************************************************************************************* * 功能描述：根据伺服角度、飞机姿态角计算目标综合航向角 * 输入参数： * 输出参数：无 * 返回值：直角坐标x,y,z * 备注： **********************************************************************************************/ EulerRPY sRPYCacl::SumRPYCacl(Pole ServoAngle, EulerRPY s_RPYAngle) { //伺服角度相乘得到3*3矩阵 DOUBLE64 m1_R[9]; DOUBLE64 m2_R[9]; DOUBLE64 M1_M2[9]; DOUBLE64 M1_M2_M3[9]; DOUBLE64 m3_R[9]; DOUBLE64 m4_R[9]; DOUBLE64 m5_R[9]; //计算旋转矩阵 R_y(-ServoAngle.Pitch, m1_R); Rz(-ServoAngle.Azimuth, m2_R); Rx(-s_RPYAngle.Roll, m3_R); R_y(-s_RPYAngle.Pitch, m4_R); Rz(-s_RPYAngle.Yaw, m5_R); MultiMat33(m2_R, m1_R, M1_M2); MultiMat33(m3_R, M1_M2, M1_M2_M3); DOUBLE64 DiliMuller[9]; //计算出地理坐标3*3矩阵DiliMuller MultiMat333(m5_R, m4_R, M1_M2_M3, DiliMuller); // EulerRPY SumRPY; SumRPY.Roll = ANGLE(atan2(DiliMuller[7], DiliMuller[8])); SumRPY.Pitch = ANGLE(-asin(DiliMuller[6])); SumRPY.Yaw = ANGLE(atan2(DiliMuller[3], DiliMuller[0])); return SumRPY; } EulerRPY sRPYCacl::SumRPYCaclDown(Pole ServoAngle, EulerRPY s_RPYAngle) { //伺服角度相乘得到3*3矩阵 DOUBLE64 m1_R[9]; DOUBLE64 m2_R[9]; DOUBLE64 M1_M2[9]; DOUBLE64 M1_M2_M3[9]; DOUBLE64 m3_R[9]; DOUBLE64 m4_R[9]; DOUBLE64 m5_R[9]; //s7215a 载荷1使用，计算安装面姿态 R_y(ServoAngle.Pitch, m1_R); Rx(ServoAngle.fRoll, m2_R); Rx(s_RPYAngle.Roll, m3_R); R_y(s_RPYAngle.Pitch, m4_R); Rz(s_RPYAngle.Yaw, m5_R); MultiMat33(m2_R, m1_R, M1_M2); MultiMat33(m4_R, M1_M2, M1_M2_M3); DOUBLE64 DiliMuller[9]; //计算出地理坐标3*3矩阵DiliMuller MultiMat333(m5_R, m3_R, M1_M2_M3, DiliMuller); // EulerRPY SumRPY; SumRPY.Roll =-ANGLE(asin(DiliMuller[7])); SumRPY.Pitch = ANGLE(atan2(DiliMuller[6], DiliMuller[8])); SumRPY.Yaw = ANGLE(atan2(DiliMuller[1], DiliMuller[4])); return SumRPY; } //说明：目标定位主接口函数 //输入：载机结构体（9个参数，目标经纬高、姿态角、伺服角度和激光测距值）、 //输出：目标结构体（目标经纬高、地心地固直角坐标、目标三向速度以及合速度） Angle3D sRPYCacl::TarGetLocate_Cal(stTrackPlatInfo mytrack) { //伺服方位俯仰 Pole targetServoPole; targetServoPole.Azimuth = mytrack.fServoAz; targetServoPole.Pitch = mytrack.fServoPt; targetServoPole.distance = mytrack.Distance; //载机方位、俯仰、横滚 EulerRPY stEulerRPY; stEulerRPY.Yaw = mytrack.fPlatAz; stEulerRPY.Pitch = mytrack.fPlatPt; stEulerRPY.Roll = mytrack.fPlatRoll; //载机经、纬、高 PointBLH selfPointBLH; selfPointBLH.L = mytrack.fLon; selfPointBLH.B = mytrack.fLat; selfPointBLH.H = mytrack.fWGS_H; //目标真实经、纬、高// PointBLH targetBLH; targetBLH.L = mytrack.tarLon; targetBLH.B = mytrack.tarLat; targetBLH.H = mytrack.tarH; double* Sum_Error_Min; double Error_Min = 111000; Sum_Error_Min = &Error_Min; Angle3D* AngleError3D; Angle3D Error3D = { 0 }; AngleError3D = &Error3D; //=========================目标定位=========================== //目标定位 //模式1--误差修正（当Error有数值后，getAngle3DFromEOServoPole不再调用（注释掉93-98行）） Angle3D AngleError = getAngle3DFromEOServoPole(targetServoPole, stEulerRPY, selfPointBLH, targetBLH, Sum_Error_Min, AngleError3D);//注：只计算一次 return AngleError; } /********************************************************************************************* * 功能描述：根据北东地坐标系下的姿态角与修正姿态角计算综合姿态 * 输入参数： * 输出参数：无 * 返回值： * 备注： **********************************************************************************************/ int Counter = 0; Angle3D RPY = { 0 }; EulerRPY sRPYCacl::Zh_dRPY(stTrackPlatInfo track) { EulerRPY s_RPYAngle; Pole ServoAngle; Angle3D d_RPY = { 0 }; Counter++; s_RPYAngle.Yaw = track.fPlatAz; s_RPYAngle.Pitch = track.fPlatPt; s_RPYAngle.Roll = track.fPlatRoll; ServoAngle.Azimuth = track.fServoAz; ServoAngle.Pitch = track.fServoPt; ServoAngle.distance = track.Distance; //计算目标在北东地坐标系下的姿态角 EulerRPY S_RPY = SumRPYCacl(ServoAngle, s_RPYAngle); ////计算修正姿态角 //if (Counter == 1) //{ // d_RPY = TarGetLocate_Cal(track); // RPY = d_RPY; //} EulerRPY FinalAngle; FinalAngle.Yaw = RPY.dHAngle + S_RPY.Yaw; FinalAngle.Pitch = RPY.dVAngle + S_RPY.Pitch; FinalAngle.Roll = RPY.dSAngle + S_RPY.Roll; return FinalAngle; } EulerRPY sRPYCacl::Zh_dRPYDown(stTrackPlatInfo track) { EulerRPY s_RPYAngle; Pole ServoAngle; Angle3D d_RPY = { 0 }; Counter++; s_RPYAngle.Yaw = track.fPlatAz; s_RPYAngle.Pitch = track.fPlatPt; s_RPYAngle.Roll = track.fPlatRoll; ServoAngle.Azimuth = track.fServoAz; ServoAngle.Pitch = track.fServoPt; ServoAngle.fRoll = track.fRoll; ServoAngle.distance = track.Distance; //计算目标在北东地坐标系下的姿态角 EulerRPY S_RPY = SumRPYCaclDown(ServoAngle, s_RPYAngle); ////计算修正姿态角 //if (Counter == 1) //{ // d_RPY = TarGetLocate_Cal(track); // RPY = d_RPY; //} EulerRPY FinalAngle; FinalAngle.Yaw = RPY.dHAngle + S_RPY.Yaw; FinalAngle.Pitch = RPY.dVAngle + S_RPY.Pitch; FinalAngle.Roll = RPY.dSAngle + S_RPY.Roll; return FinalAngle; }