wangchongwu
/
StitchVideo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'main'
${ noResults }
StitchVideo/tests/S7215/Arith_zhryp.cpp

1080 lines
35 KiB
Raw Permalink Normal View History Unescape

S7215 载荷1 ts流解析
2 weeks ago
/*********版权所有（C）2020，武汉高德红外股份有限公司***************************************
* 文件名称： Arith_ZHRPY.cpp
* 文件标识： 有源定位模块标准化
* 内容摘要： 光电伺服坐标系 <-> 目标大地坐标系，基于北-天-东 轴序
* 其它说明：
* 当前版本：2.0
* 创建作者：11531yj
* 创建日期：2023.10.26
*****************************************************************************************/
#include "Arith_zhryp.h"
#include <iostream>
#include <assert.h>
#include <fstream>
#include <math.h>
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;
}