%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 该程序用于获取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)));