所需要了解的相关知识①大地坐标系即东北天坐标系、雷达坐标系的区别及转换②IMU(或TS)模块的内旋方式及旋转矩阵概念③RTK为静态模块IMU(或TS)为动态模块IMU(或TS)的偏航角并不准确需要用RTK的双天线航向角来替代1、坐标系和旋转矩阵假设雷达坐标系XL,YL,ZL与地理坐标系XR,YR,ZR建立在同一个原点上但是三轴都不重合其中以大地坐标系的X轴为中心轴旋转坐标系与雷达坐标系重合此时旋转的夹角θ即为X轴对应的姿态角如下图所示同理X轴和Y轴对应的姿态角也是同样的概念。而姿态补偿的目的就是将雷达探测到的目标信息映射到大地坐标系上即得到目标相对于大地来说的绝对真实位置。因此姿态补偿需要做的就是将雷达坐标系转换到大地坐标系由上图可知通过矢量计算可以得到即其中X轴的旋转矩阵Rx为同理可求得Y轴、Z轴的旋转矩阵为2、IMU(或TS)的内旋方式每个厂家的模块内旋方式可能都不一致需要提前与厂家沟通。假设姿态模块的内旋方式为ZYX那么姿态补偿的顺序为那么假设探测到的目标信息为x0y0z0此时目标为雷达坐标系的坐标假设雷达载体上的IMU模块返回的姿态角分别为θφψ则目标的大地坐标系下的坐标xyz为3、MATLAB补偿的部分代码x ra(i) * sind(azi(i)) * cosd(ele(i)); y ra(i) * cosd(azi(i)) * cosd(ele(i)); z ra(i) * sind(ele(i)); Ori_xyz(:,i) [x;y;z]; RZ [cosd(RTK_yaw(i)), sind(RTK_yaw(i)), 0; -sind(RTK_yaw(i)), cosd(RTK_yaw(i)), 0; 0 , 0, 1]; RX [1, 0, 0; 0, cosd(pit(i)), sind(pit(i)); 0, -sind(pit(i)), cosd(pit(i))]; RY [cosd(rol(i)), 0, -sind(rol(i)); 0, 1, 0; sind(rol(i)), 0, cosd(rol(i))]; % 依次补偿 Imu_xyz(:,i) RZ*RX*RY*Ori_xyz(:,i); range(i) sqrt(Imu_xyz(1,i)*Imu_xyz(1,i) Imu_xyz(2,i)*Imu_xyz(2,i) Imu_xyz(3,i)*Imu_xyz(3,i)); theta(i) atan2(Imu_xyz(1,i), Imu_xyz(2,i))*180/pi; % 范围[-pi, pi]自动匹配所有象限 phi(i) atan(Imu_xyz(3,i)/ hypot(Imu_xyz(1,i),Imu_xyz(2,i))) * 180 / pi; % 一次补偿 RZXY [cosd(rol(i))*cosd(RTK_yaw(i))sind(pit(i))*sind(rol(i))*sind(RTK_yaw(i)), cosd(pit(i))*sind(RTK_yaw(i)), -sind(rol(i))*cosd(RTK_yaw(i))sind(pit(i))*cosd(rol(i))*sind(RTK_yaw(i)); -sind(RTK_yaw(i))*cosd(rol(i))sind(pit(i))*sind(rol(i))*cosd(RTK_yaw(i)), cosd(pit(i))*cosd(RTK_yaw(i)), sind(rol(i))*sind(RTK_yaw(i))sind(pit(i))*cosd(rol(i))*cosd(RTK_yaw(i)); cosd(pit(i))*sind(rol(i)), -sind(pit(i)), cosd(pit(i))*cosd(rol(i))]; Imu_xyz1(:,i) RZXY*Ori_xyz(:,i); range1(i) sqrt(Imu_xyz1(1,i)*Imu_xyz1(1,i) Imu_xyz1(2,i)*Imu_xyz1(2,i) Imu_xyz1(3,i)*Imu_xyz1(3,i)); theta1(i) atan2(Imu_xyz1(1,i), Imu_xyz1(2,i))*180/pi; % 范围[-pi, pi]自动匹配所有象限 phi1(i) atan(Imu_xyz1(3,i)/ hypot(Imu_xyz1(1,i),Imu_xyz1(2,i))) * 180 / pi;上述代码只是完成了“目标相对于雷达而言的大地坐标系”是一个相对的大地坐标系坐标Bt如果想将目标完全转换到绝对大地坐标系B需要将雷达位置的经纬高转换成雷达对应的大地坐标系Br则绝对大地坐标系B为4、经纬高转大地坐标系在这里只提供思路懒得写了首先需要将经纬高转换成地心地固坐标系再将地心地固坐标系转到大地坐标系