不同的卫星导航系统有不同的坐标系,例如GPS采用的坐标系为WGS-84,我国北斗导航系统采用的是CGCS2000,俄罗斯GLONASS采用的是PZ-90,这些坐标系的不同在于椭球的参数不同,高程指的是物体到椭球切平面的距离,不同坐标系同一经纬度点高程相差较大。根据不同卫星导航系统解算得到的坐标通常为LBH,经度L、纬度B和高程H,这是一致的。但是对于无人机导航来说用经纬度导航是不现实的,因为经度差1°地表距离相差100km,我们通常要将LBH坐标转化为地心固连坐标系的XYZ坐标,再转换为当地北东地坐标系下的坐标(x,y,z),这样便于计算。所以无人机利用卫星导航系统信息的关键是坐标转换的问题。
%度转为弧度
function r=deg2rad(du,fen,miao)
a=miao/60;
b=(fen+a)/60;
r=pi*(du+b)/180;
end
%WGS-84坐标转换为ECEF坐标
function ECEF=WGS842ECEF(B,L,H)
a=6378137;
f=0.00669437999014;
N=a/sqrt(1-f*sin(B)*sin(B));
X=(N+H)*cos(B)*cos(L);
Y=(N+H)*cos(B)*sin(L);
Z=(N*(1-f)+H)*sin(B);
ECEF=[X Y Z]';
end
%ECEF坐标系到NED坐标系的坐标转换矩阵
B= 0.546444427678930;
L=2.121652685023481;
R=[-sin(B)*cos(L) -sin(B)*sin(L) cos(B);
-sin(L) cos(L) 0;
-cos(B)*cos(L) -cos(B)*sin(L) -sin(B)]