200 lines
7.2 KiB
Matlab
200 lines
7.2 KiB
Matlab
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
% 该程序用于获取A机位姿信息和B机位置信息，并计算A机对B机指向
|
||
% By ZXJ，2025.10.02
|
||
|
||
clear all;
|
||
close all;
|
||
clc;
|
||
|
||
Re=6378.14; %% 地球半径，km
|
||
|
||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
% 双机位置（地心球面坐标系）
|
||
% A机位置
|
||
Lon_A=dms2degrees([86 4 51.16]); %% A机经度，deg
|
||
Lat_A=dms2degrees([41 45 20.24]); %% A机纬度，deg
|
||
Hei_A=4.5; %% A机高度，km
|
||
% Lon_A=120.2994; %% A机经度，deg
|
||
% Lat_A=30.250212; %% A机纬度，deg
|
||
% Hei_A=375.29/1000; %% A机高度，km
|
||
|
||
% B机位置
|
||
Lon_B=dms2degrees([86 4 51.161]); %% B机经度，deg
|
||
Lat_B=dms2degrees([41 45 20.24]); %% B机纬度，deg
|
||
Hei_B=4.5; %% B机高度，km
|
||
% Lon_B=dms2degrees([76 12 51.16]); %% B机经度，deg
|
||
% Lat_B=dms2degrees([41 45 20.24]); %% B机纬度，deg
|
||
% Hei_B=5.1; %% B机高度，km
|
||
% Lon_B=120.29950478; %% B机经度，deg
|
||
% Lat_B=30.250028015; %% B机纬度，deg
|
||
% Hei_B=375.29/1000; %% B机高度，km
|
||
|
||
% 将角度由度转化为弧度
|
||
Lon_A_Rad=deg2rad(Lon_A); %% A机经度，rad
|
||
Lat_A_Rad=deg2rad(Lat_A); %% A机纬度，rad
|
||
|
||
Lon_B_Rad=deg2rad(Lon_B); %% B机经度，rad
|
||
Lat_B_Rad=deg2rad(Lat_B); %% B机纬度，rad
|
||
|
||
% 双机位置（地心直角坐标系）及双机距离，km
|
||
X_A=(Re+Hei_A)*cos(Lat_A_Rad)*cos(Lon_A_Rad);
|
||
Y_A=(Re+Hei_A)*cos(Lat_A_Rad)*sin(Lon_A_Rad);
|
||
Z_A=(Re+Hei_A)*sin(Lat_A_Rad);
|
||
|
||
X_B=(Re+Hei_B)*cos(Lat_B_Rad)*cos(Lon_B_Rad);
|
||
Y_B=(Re+Hei_B)*cos(Lat_B_Rad)*sin(Lon_B_Rad);
|
||
Z_B=(Re+Hei_B)*sin(Lat_B_Rad);
|
||
|
||
% B机相对于A机的直角坐标，km
|
||
XYZ_BA=[X_B-X_A; Y_B-Y_A; Z_B-Z_A];
|
||
% 双机距离，km
|
||
D_BA=sqrt(sum(XYZ_BA.^2));
|
||
% D_BA=sqrt((X_B-X_A)^2+(Y_B-Y_A)^2+(Z_B-Z_A)^2);
|
||
|
||
% % 根据坐标转换矩阵计算B机相对于A机的方位角、俯仰角
|
||
% % 坐标转换矩阵
|
||
% Matrix_UEN2XYZ=[cos(Lat_A_Rad)*cos(Lon_A_Rad) cos(Lat_A_Rad)*sin(Lon_A_Rad) sin(Lat_A_Rad); ...
|
||
% -sin(Lon_A_Rad) cos(Lon_A_Rad) 0;...
|
||
% -sin(Lat_A_Rad)*cos(Lon_A_Rad) -sin(Lat_A_Rad)*sin(Lon_A_Rad) cos(Lat_A_Rad)];
|
||
%
|
||
% % B机相对于A机的直角坐标转换为天东北坐标
|
||
% UEN_BA = Matrix_UEN2XYZ * XYZ_BA;
|
||
% U_BA=UEN_BA(1);
|
||
% E_BA=UEN_BA(2);
|
||
% N_BA=UEN_BA(3);
|
||
%
|
||
% % B机相对于A机的方位角和俯仰角
|
||
% Az_BA=mod(rad2deg(atan2(E_BA,N_BA)),360);
|
||
% Alt_BA=rad2deg(atan2(U_BA,sqrt(E_BA^2+N_BA^2)));
|
||
|
||
% 根据三轴坐标旋转矩阵计算B机相对于A机的方位角、俯仰角
|
||
Lon_A_Rad1=Lon_A_Rad;
|
||
Lat_A_Rad1=-Lat_A_Rad;
|
||
MR_X=[1 0 0;...
|
||
0 1 0;...
|
||
0 0 1];
|
||
MR_Y=[cos(Lat_A_Rad1) 0 -sin(Lat_A_Rad1);...
|
||
0 1 0;...
|
||
sin(Lat_A_Rad1) 0 cos(Lat_A_Rad1)];
|
||
MR_Z=[cos(Lon_A_Rad1) sin(Lon_A_Rad1) 0;...
|
||
-sin(Lon_A_Rad1) cos(Lon_A_Rad1) 0;...
|
||
0 0 1];
|
||
|
||
% 天东北坐标
|
||
UEN_BA0=MR_X*MR_Y*MR_Z*XYZ_BA;
|
||
U_BA0=UEN_BA0(1);
|
||
E_BA0=UEN_BA0(2);
|
||
N_BA0=UEN_BA0(3);
|
||
|
||
% B机相对于A机在大地坐标系中的方位角和俯仰角
|
||
Az_BA0=mod(rad2deg(atan2(E_BA0,N_BA0)),360);
|
||
Alt_BA0=rad2deg(atan2(U_BA0,sqrt(E_BA0^2+N_BA0^2)));
|
||
|
||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
% 根据POS设备实时姿态角解算B机在A机载体坐标系中的位置
|
||
% POS设备测量到的实时姿态角
|
||
% HeadingReal=63.802;
|
||
% PitchReal=-0.109;
|
||
% RollReal=-0.135;
|
||
HeadingReal=65.516;
|
||
PitchReal=-0.112;
|
||
RollReal=1.648;
|
||
|
||
% 角度换算为弧度
|
||
HeadingReal_Rad=deg2rad(HeadingReal);
|
||
PitchReal_Rad=deg2rad(PitchReal);
|
||
RollReal_Rad=deg2rad(RollReal);
|
||
|
||
% 由实时姿态计算位置的坐标旋转矩阵
|
||
MR_E_Real=[1 0 0;...
|
||
0 cos(PitchReal_Rad) -sin(PitchReal_Rad);...
|
||
0 sin(PitchReal_Rad) cos(PitchReal_Rad)];
|
||
MR_N_Real=[cos(RollReal_Rad) 0 -sin(RollReal_Rad);...
|
||
0 1 0;...
|
||
sin(RollReal_Rad) 0 cos(RollReal_Rad)];
|
||
MR_U_Real=[cos(HeadingReal_Rad) -sin(HeadingReal_Rad) 0;...
|
||
sin(HeadingReal_Rad) cos(HeadingReal_Rad) 0;...
|
||
0 0 1];
|
||
|
||
% B机在A机载体坐标系中的位置
|
||
ENU_BA_Real=MR_N_Real*MR_E_Real*MR_U_Real*[E_BA0;N_BA0;U_BA0];
|
||
E_BA_Real=ENU_BA_Real(1);
|
||
N_BA_Real=ENU_BA_Real(2);
|
||
U_BA_Real=ENU_BA_Real(3);
|
||
|
||
% B机在A机载体坐标系中的方位角和俯仰角
|
||
Az_BA_Real=mod(rad2deg(atan2(E_BA_Real,N_BA_Real)),360);
|
||
Alt_BA_Real=rad2deg(atan2(U_BA_Real,sqrt(E_BA_Real^2+N_BA_Real^2)));
|
||
|
||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
% 静止状态下POS设备安装面的姿态角计算零点指向
|
||
% 静止状态下，POS设备在自身安装面上测到的姿态角
|
||
Heading_IMUBase=63.888;
|
||
Pitch_IMUBase=-0.121;
|
||
Roll_IMUBase=-0.082;
|
||
|
||
% POS设备在自身安装面上姿态角换算为弧度
|
||
Heading_IMUBase_Rad=-deg2rad(Heading_IMUBase);
|
||
Pitch_IMUBase_Rad=-deg2rad(Pitch_IMUBase);
|
||
Roll_IMUBase_Rad=-deg2rad(Roll_IMUBase);
|
||
|
||
% 由静止姿态计算零点位置的坐标旋转矩阵
|
||
MR_E_IMUBase=[1 0 0;...
|
||
0 cos(-Pitch_IMUBase_Rad) -sin(-Pitch_IMUBase_Rad);...
|
||
0 sin(-Pitch_IMUBase_Rad) cos(-Pitch_IMUBase_Rad)];
|
||
MR_N_IMUBase=[cos(-Roll_IMUBase_Rad) 0 -sin(-Roll_IMUBase_Rad);...
|
||
0 1 0;...
|
||
sin(-Roll_IMUBase_Rad) 0 cos(-Roll_IMUBase_Rad)];
|
||
MR_U_IMUBase=[cos(Heading_IMUBase_Rad) -sin(Heading_IMUBase_Rad) 0;...
|
||
sin(Heading_IMUBase_Rad) cos(Heading_IMUBase_Rad) 0;...
|
||
0 0 1];
|
||
|
||
% B机在A机载体坐标系中的位置
|
||
ENU_BA_IMUBase=MR_N_IMUBase*MR_E_IMUBase*MR_U_IMUBase*ENU_BA_Real;
|
||
E_BA_IMUBase=ENU_BA_IMUBase(1);
|
||
N_BA_IMUBase=ENU_BA_IMUBase(2);
|
||
U_BA_IMUBase=ENU_BA_IMUBase(3);
|
||
|
||
% B机在A机载体坐标系中的方位角和俯仰角
|
||
Az_BA_IMUBase=mod(rad2deg(atan2(E_BA_IMUBase,N_BA_IMUBase)),360);
|
||
Alt_BA_IMUBase=rad2deg(atan2(U_BA_IMUBase,sqrt(E_BA_IMUBase^2+N_BA_IMUBase^2)));
|
||
|
||
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
% 静止状态下样机安装面的姿态角计算零点指向
|
||
% POS设备在样机安装面上测到的姿态角
|
||
Heading_OPABase=64.062;
|
||
Pitch_OPABase=-0.332;
|
||
Roll_OPABase=89.812;
|
||
|
||
% 静止状态下，样机安装面法线方向对应的姿态角
|
||
Heading_OPABaseNormal=Heading_OPABase+90; %% 样机安装在前进方向右侧加90°，安装在左侧则减90°
|
||
Pitch_OPABaseNormal=Pitch_OPABase-90;
|
||
Roll_OPABaseNormal=Roll_OPABase;
|
||
|
||
% 法线方向对应的姿态角换算为弧度
|
||
Heading_OPABaseNormal_Rad=deg2rad(Heading_OPABaseNormal);
|
||
PitchRad_OPABaseNormal_Rad=deg2rad(Pitch_OPABaseNormal);
|
||
RollRad_OPABaseNormal_Rad=deg2rad(Roll_OPABaseNormal);
|
||
|
||
% 由静止姿态计算零点位置的坐标旋转矩阵
|
||
MR_E_OPABaseNormal=[1 0 0;...
|
||
0 cos(PitchRad_OPABaseNormal_Rad) -sin(PitchRad_OPABaseNormal_Rad);...
|
||
0 sin(PitchRad_OPABaseNormal_Rad) cos(PitchRad_OPABaseNormal_Rad)];
|
||
MR_N_OPABaseNormal=[cos(RollRad_OPABaseNormal_Rad) 0 -sin(RollRad_OPABaseNormal_Rad);...
|
||
0 1 0;...
|
||
sin(RollRad_OPABaseNormal_Rad) 0 cos(RollRad_OPABaseNormal_Rad)];
|
||
MR_U_OPABaseNormal=[cos(Heading_OPABaseNormal_Rad) -sin(Heading_OPABaseNormal_Rad) 0;...
|
||
sin(Heading_OPABaseNormal_Rad) cos(Heading_OPABaseNormal_Rad) 0;...
|
||
0 0 1];
|
||
|
||
% B机在A机载体坐标系中的位置（由东北天转换）
|
||
ENU_BA_OPABaseNormal=MR_N_OPABaseNormal*MR_E_OPABaseNormal*MR_U_OPABaseNormal*ENU_BA_IMUBase;
|
||
E_BA_OPABaseNormal=ENU_BA_OPABaseNormal(1);
|
||
N_BA_OPABaseNormal=ENU_BA_OPABaseNormal(2);
|
||
U_BA_OPABaseNormal=ENU_BA_OPABaseNormal(3);
|
||
|
||
% B机在A机载体坐标系中的方位角和俯仰角
|
||
Az_BA_OPABaseNormal=mod(rad2deg(atan2(E_BA_OPABaseNormal,N_BA_OPABaseNormal)),360);
|
||
Alt_BA_OPABaseNormal=rad2deg(atan2(U_BA_OPABaseNormal,sqrt(E_BA_OPABaseNormal^2+N_BA_OPABaseNormal^2)));
|
||
|