|
|
/*********版权所有（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;
|
|
|
}
|
