本文介绍了基于C语言的GPS卫星位置解算原理与程序设计。针对每个原理、公式、代码设计进行了详细讲解,希望能够给测绘学子们带来帮助。
参考书籍:
李征航 黄劲松:GPS测量与数据处理(第三版)
目录
基础原理
1)卫星位置解算流程
如流程图所示,在程序设计中对于一颗卫星的位置解算,是要经过多次迭代,直到信号传播时间收敛。
2)卫星有关参数
在正式开始程序设计之前,我们还需要了解一下卫星的相关参数及其含义。
平近点角M
卫星平均运动角速度为
所以卫星平均运动角速度n:
因此,平近点角M定义为:
t为信号发射时刻,t0为卫星过近地点时刻。
偏近点角E
如下图所示:以卫星椭圆轨道的焦点为原点,以指向近地点的方向为X轴,Y轴平行于椭圆短半轴,建立轨道平面坐标系。以卫星椭圆中心为圆心,以卫星轨道的长半轴a为半径作一个辅助圆。
过卫星S作X轴的垂线,其延长线与辅助圆交于S‘,圆心到S’的向径与X轴的夹角定义为偏近点角E。
平近点角与偏近点角的关系可用开普勒方程表示:
在计算偏近点角E时,E0可用平近点角M做初始值,然后进行迭代计算,因为E非常小,因此迭代几次即可收敛,可将收敛标准设为1.0e-5。
真近点角V
由上图所示,真近点角V与偏近点角E存在着一定关系,真近点角V可又下述式子得出:
其中,e为第一偏心率。
如果已知真近点角,那么卫星在轨道坐标系中的位置向量可以表示为:
卫星的运动速度
由上式得得卫星的运动速度为:
由开普勒方程可得:
因此,可以推导出卫星的运动方程:
卫星钟差
卫星钟差方程为:
a0,a1,a2为N文件中的钟频,钟偏和钟漂。(广播星历中的卫星钟差误差为10ns,等效距离为3m)
其中
3)卫星位置计算过程
(1)求平均角速度n:
(2)求规划时刻
t为信号发射时刻,
信号发射时刻为:O文件观测时刻-信号传播时间。
近似信号传播时间=伪距观测值/光速-接收机钟差+卫星钟差+电离层改正+对流层改正
上式中,用参考时刻toe时刻代替了卫星过近地点t0时刻。
这是因为:广播星历2h更新一次,间给参考时刻设在中央时刻时,外推间隔小于1h。而卫星的运行周期为12h,采取卫星过近地点时刻t0,外推间隔最大可能达到6h。用toe代替t0,模型简单,并且可以保证精度。
计算t和toe时,需要保证二者之差在两小时内,计算才能生效。因此,在计算卫星位置前,需要根据O文件中的观测选择N文件中的最佳观测历元(代码见后文)。
(3)求平近角
(4)求偏近点角E
e为N文件中轨道偏心率;偏近点角需要迭代计算,将平近角M作为E的初始值带入,因为E很小,一般迭代几次即可收敛,收敛条件为
1.0e-12为科学计数法,表示10的负12次方。
(5)求真近点角
(6)求升交角距u
(7)摄动改正
升交角距改正项:
轨道向径改正项:
轨道向径改正项:
(8)计算改正后的升交角距、轨道向径、轨道向径
(9)卫星在升交点轨道直角坐标系的坐标
(10)计算升交点经度L
(11) 计算卫星在地固坐标系中的空间直角坐标系
(12)计算Z轴旋转变换,得到的新坐标
(13)重新计算卫地距
(14)重新计算信号传播时间
R为新的卫地距,Cv表示光速。
(15)重复步骤1~14,直到信号传播时间收敛,收敛条件为
程序设计
单颗卫星位置解算流程:
- 将年月日转换成GPS周内秒
- 为卫星选择最佳历元
- 计算信号近似传播时间
- 卫星位置解算
数据预处理
1)GPS时转换(TimetoGPSsec)
GPS时,也称GPS time,GPST,由GPS星载原子钟和地面监控站原子钟组成的一种原子时基准,其起点为1980年1月6日0时00分00秒。
观测值文件中的参考时刻是以年月日进行记录的,不利于计算,需转换成周内秒的形式。
程序设计主要思想:
- 先计算到1980年1月1号0时有多少天
- 考虑闰年(4年一润,百年不润,四百年一润),闰月(润年润月为29天),以及大小月。
- 1980年1月6日为星期日,一周的开始,解算出来的天数需要减6(1980.1.6为GPS时起算时刻)
- 将得到天数除以7,得到周内的天数,将周内天数转换成周内秒数
TimetoGPSsec代码如下:
double TimetoGPSsec(int year, int month, int day, int hour, int min, double sec)
{
int DayofYear = 0, DayofMonth = 0, SecofWeek = 0;
int i = 0;
for (i = 1980; i < year; i++)
{
//判断闰年
if ((i % 4 == 0 && i % 100 != 0) || i % 400 == 0)
DayofYear += 366;
else
DayofYear += 365;
}
for (i = 1; i < month; i++)
{
//判断大小月
if (i == 1 || i == 3 || i == 5 || i == 7 || i == 8 || i == 10 || i == 12)
DayofMonth += 31;
else if (i == 4 || i == 6 || i == 9 || i == 11)
DayofMonth += 30;
else
{
if ((year % 4 == 0 && year % 100 != 0) || year % 400 == 0)
DayofMonth += 29;
else
DayofMonth += 28;
}
}
int Day = DayofYear + DayofMonth + day - 6;//计算到1980年1月6号(星期日)有多少天
SecofWeek = (double)(Day % 7) * 24 * 60 * 60 + (double)hour * 3600 + (double)min * 60 + sec;
return SecofWeek;
}
其中:
- 传入的参数为O、N文件中得到的年、月、日、时、分、秒。
- 返回值为计算得到的周内秒。
2)选择最佳历元(select_epoch)
根据卫星的PRN号以及观测时间,选择与N文件中时间间隔最小的数据块进行解算。
主要思想:
- 遍历整个N文件数据块部分
- 先判断卫星的PRN号是否相等
- 设置最小时间差初始值为10000秒,每次更新最小值,并记录最小值所对应的历元数
- 返回时间差最小的历元数
select_epoch代码如下:
//挑选合适的星历
extern int select_epoch(double SecofWeek, int sPRN, pnav_body nav_b, int n_n)
{
int n_epoch = 0;
double min=10000;//假设最小值是1万秒
double Min;
int i;
for (i = 0; i < n_n / 8; i++)
{
if (sPRN == nav_b[i].sPRN)
{
Min = fabs(SecofWeek - nav_b[i].TOE);
if (Min <= min)
{
n_epoch = i;
min = Min;
}
}
}
return n_epoch;
}
其中:
- SecofWeek为观测时刻的GPS周内秒,sPRN为卫星的PRN号,nav_b为导航文件数据块结构体,n_n为导航文件数据块总行数(不包含头部)
- 需要传入的参数有:观测的周内秒时间、卫星的PRN号、N文件中数据块的结构体以及N文件数据块部分的行数。
- 假设最小值min是一万秒,当比min小时,记录新的min,并记下该数据块的位置i到n_epoch中
- 循环所有N文件中所有的数据块,返回最佳值
代码与解析
在程序设计时,将单次解算卫星位置的过程封装成一个函数,然后放入while循环中迭代计算,当信号传播时间收敛后,输出卫星位置结果。
定义一个结构体,用来当作函数的传参
//卫星位置传参
typedef struct Pos_t
{
double X;
double Y;
double Z;
double deltat;//改正前的信号传播时间
double delta_t;//改正后的信号传播时间
}Pos_t;
#define C_V 299792458//光速(m)
#define GM 398600500000000//地心引力常数
#define math_e 2.718281828459 //e值
#define PI 3.141592653589793
#define Earth_e 7.2921151467e-5 //地球自转角速度
#define C1 4
//时间转换
double GPSsec = TimetoGPSsec(obs_e[i].y, obs_e[i].m, obs_e[i].d, obs_e[i].h, obs_e[i].min, obs_e[i].sec);
//根据O文件历元参考时间选择合适的N文件数据
int n_epoch = select_epoch(GPSsec, obs_e[i].sPRN[j], nav_b, n_n);
//观测时刻 - 参考时刻
double detat_toc = GPSsec - nav_b[n_epoch].TOE;
//计算近似的信号传播时间,接收机钟差已初始化为0(伪距/光速-接收机钟差+卫星钟差)
double delta_t = (obs_b[i].obs[j][C1] / C_V) - sta[i].delta_TR + nav_b[n_epoch].sa0 + nav_b[n_epoch].sa1 * detat_toc + nav_b[n_epoch].sa2 * pow(detat_toc, 2);
//计算卫星位置
double deltat = 0.0;//判断收敛
while (fabs(delta_t - deltat) > 0.0001)
{
//临时结构体变量tmp,用来传参
Pos_t tmp = { 0 };
//计算卫星位置函数
tmp = sat_pos(nav_b[n_epoch].deltan, nav_b[n_epoch].sqrtA, nav_b[n_epoch].TOE,
nav_b[n_epoch].M0, nav_b[n_epoch].e, nav_b[n_epoch].omega, nav_b[n_epoch].OMEGA,
nav_b[n_epoch].deltaomega, nav_b[n_epoch].Cuc, nav_b[n_epoch].Crc, nav_b[n_epoch].Cic,
nav_b[n_epoch].Cus, nav_b[n_epoch].Crs, nav_b[n_epoch].Cis, nav_b[n_epoch].i0, nav_b[n_epoch].IDOT,
delta_t, deltat, sta[i].X, sta[i].Y, sta[i].Z, GPSsec);
//传参,更新信号传播时间
deltat = tmp.deltat;
delta_t = tmp.delta_t;
sat[i][j].X = tmp.X;
sat[i][j].Y = tmp.Y;
sat[i][j].Z = tmp.Z;
}
其中:
- obs_e:观测文件历元结构体,记录了每个观测值历元第一行时间及卫星数等信息
- obs_b:观测文件数据块结构体,记录了每个观测历元观测值的数据
- nav_b:导航文件数据块结构体,记录了每个历元的卫星参数
- 观测值文件和导航文件的读取可参考:RINEX O文件读取和RINEX N文件读取
- detat_toc:计算卫星钟差所用的时间
- delta_t:近似信号传播时间
- obs_b[i].obs[j][C1] / C_V:表示第i个历元第j颗卫星的伪距值除以光速,C1和C_V为宏定义
- sta[i].delta_TR:接收机钟差,i表示第i个历元,接收机钟差初始化时需为0
- nav_b[n_epoch].sa0 + nav_b[n_epoch].sa1 * detat_toc + nav_b[n_epoch].sa2 * pow(detat_toc, 2):该部分表示卫星钟差的计算,对照上文公式,省略了末项
- deltat = 0.0:判断信号收敛,第一次计算时,需要初始化为0
- Pos_t tmp:上文中用于传参的结构体
- sat_pos:计算卫星位置函数,下文会详细展示
计算卫星位置函数
//计算卫星位置
Pos_t sat_pos( double deltan,double sqrtA,double TOE,double M0,double e,
double omega,double OMEGA,double deltaomega,double Cuc,double Crc, double Cic,
double Cus,double Crs,double Cis,double i0,double IDOT,double delta_t, double deltat, double X_R,
double Y_R, double Z_R, double GPSsec)
{
Pos_t pos_t={0};
//计算信号发射时刻=O文件观测时刻-信号传播时间
double T_S = GPSsec - delta_t;
//计算卫星的平均角速度n
double n0 = sqrt(GM) / pow(sqrtA, 3);
double n = n0 + deltan;
//计算归划时间tk
double tk = T_S - TOE;
if (tk > 32400)tk -= 604800;//规划时间改正
else if (tk < -32400)tk += 604800;
//计算卫星发射时卫星的平近角M
double M = M0 + n * tk;
//迭代计算偏近点角
double E = M, E0;
do
{
E0 = E;
E = M + e * sin(E);
} while (fabs(E - E0) > 1.0e-5);
//计算真近点角V
double cosv = (cos(E) - e) / (1 - e * cos(E));//cosV
double sinv = sqrt(1 - pow(e, 2)) * sin(E) / (1 - e * cos(E));//sinV
double Vk = atan2(sinv, cosv);//注意atan与atan2的不同
//计算升交角距u
double u = Vk + omega;
//计算摄动改正项
double Epsilon_u = Cuc * cos(2 * u) + Cus * sin(2 * u);
double Epsilon_r = Crc * cos(2 * u) + Crs * sin(2 * u);
double Epsilon_i = Cic * cos(2 * u) + Cis * sin(2 * u);
//计算改正后的升交角距、轨道向径、轨道倾角
double uk = u + Epsilon_u;
double rk = pow(sqrtA, 2) * (1 - e * cos(E)) + Epsilon_r;
double ik = i0 + Epsilon_i + IDOT * tk;
//计算卫星在升交点轨道直角坐标系的坐标
double xk = rk * cos(uk);
double yk = rk * sin(uk);
//计算升交点经度
double L = OMEGA + (deltaomega - Earth_e) * tk - Earth_e * TOE;
//计算卫星在地固系下的直角坐标
double X = xk * cos(L) - yk * cos(ik) * sin(L);
double Y = xk * sin(L) + yk * cos(ik) * cos(L);
double Z = yk * sin(ik);
//通过 Z 轴的旋转变换,对该坐标进行地球自转改正,得到新坐标
pos_t.X = cos(Earth_e * delta_t) * X + sin(Earth_e * delta_t) * Y;
pos_t.Y = -sin(Earth_e * delta_t) * X + cos(Earth_e * delta_t) * Y;
pos_t.Z = Z;
//重新计算信号传播的时间
double R = sqrt(pow(X - X_R, 2) + pow(Y - Y_R, 2) + pow(Z - Z_R, 2));
pos_t.deltat = delta_t;
pos_t.delta_t = R / C_V;
return pos_t;
}
(上述代码为了更加清晰的体现哪个是新变量、哪个是旧变量,将开辟变量的工作放在函数中间;读者自己写代码时,建议将开辟变量的工作统一放到函数的起始位置,增加运行效率;有些语言不支持程序运行后再开辟变量,例如:Fortran)
传入参数:从deltann到IDOT,均为N文件中读取的参数,delta_t和deltat为信号传播时间,用于判断信号传播时间是否收敛,X_R,Y_R,Z_R为测站坐标,首次迭代时,可初始化为0或概略坐标(O文件头),GPSsec为转化成GPS时的观测时刻
上述代码中有关时间的参数比较多,这里再次梳理一遍,防止混淆:
观测时刻(GPSsec):接收机收到信号的时刻,O文件每个历元的第一行记录,需转换成GPS周内秒
卫星钟差时间(detat_toc):由观测时刻减去卫星信号发射时刻TOE,TOE以GPS周内秒的形式存储,这个时间只用于卫星钟差改正
信号近似传播时间(delta_t):由伪距进行计算,需要进行卫星钟差、接收机钟差、电离层和对流层改正(上述代码没有加入电离层和流层改正,推荐两种简单的改正模型:电离层经验模型Klobuchar,对流层经验模型GCAT)
信号发射时刻(T_S):由观测时刻减去信号近似传播时间
规划时间(tk):将信号发射时刻规划到以TOE为基准的时刻
上述代码与公式一一对应,需要注意以下几点:
1)if (tk > 32400)tk -= 604800;else if (tk < -32400)tk += 604800;规划时间改正,604800为一周的秒数,32400为一周半的秒数。该部分内容可参考其他博主:卫星发射时刻归化
2)计算偏近点角E时,因为E极小,迭代几次即可收敛,因此收敛条件只要保证精度即可,在计算卫星钟差时,末项的改正数用到了偏近点角E,如需改正,可将E保留进行传参
3)C语言中角度计算均是以弧度为单位;atan和atan2均是求反正切函数,后者的使用范围是是四象限角
4)上述中用到的sqrt,pow,fabs分别是:开方函数,幂函数,取绝对值函数,均需包含头文件<math.h>
BDS卫星位置
BDS的卫星位置解算与GPS卫星有很大相似之处,其中MEO与IGSO卫星位置解算与GPS一致,GEO卫星从计算升交点赤经开始有所变化,在解算位置前,需要根据卫星的PRN号来判断卫星的种类,GEO计算公式如下(公式选自本科时期的论文,如有错误多多包涵):
注意:(4-27)中-5°表示角度,为GEO卫星的倾角
版权声明:本文为博主作者:是王久久阿原创文章,版权归属原作者,如果侵权,请联系我们删除!
原文链接:https://blog.csdn.net/why1472587/article/details/127519933