前言:
本章节代码均在Gitee中开源:
卫星位置计算代码https://gitee.com/Ehundred/navigation-engineering/tree/master/%E5%8D%AB%E6%98%9F%E5%AF%BC%E8%88%AA%E5%8E%9F%E7%90%86/GPS%E5%8D%AB%E6%98%9F%E4%BD%8D%E7%BD%AE%E8%A7%A3%E7%AE%97/Satellite_position_calculation其中涉及到时空转换的部分,在另一章节有详细讲解,如有疑惑可以先阅读上一章节:
时空坐标转换https://blog.csdn.net/qq_74260823/article/details/139271746?spm=1001.2014.3001.5501
因为这章是学校作业,所以稍微正经点.
卫星参数的读取
首先,关于导航电文的理解和构成,如果还不太了解,请移步另一篇文章:
卫星信号的构成http://t.csdnimg.cn/6yY29
导航电文会发出很多参数,最终会被接收机接收到,然后转换为人类可以看懂的具体的数字。接收机在转换之后,会把适用于我们定位的数据单独列出来,然后写入文件之中成为广播星历,其中文件的格式长这样:
其具体对应的含义是:
因为写入的格式一定是固定的,所以我们读取数据的时候,也可以利用这些固定的格式,按顺序定义一个结构体:
struct satellite_data_block
{
double Serial;//序列号
double year;
double month;
double day;
double hour;
double minute;
double second;//年月日时分秒
double a0;//卫星钟差
double a1;//卫星钟偏
double a2;//卫星钟偏移
double AODE;//数据龄期
double Crs;//轨道半径改正项
double deltan;//平均角速度改正项
double M0;//平近点角
double Cuc;//升交点角距改正项
double e;//轨道偏心率
double Cus;//升交点角距改正项
double sqrtA;//轨道长半轴平方根
double TOE;//星历的参考时刻 周内秒
double Cic;//轨道倾角的改正项
double OMEGA;//升交点经度
double Cis;//轨道倾角改正项
double i0;//轨道倾角
double Crc;//轨道半径的改正项
double omega;//近地点角距
double deltaomega;//升交点赤经变化
double IDOT;//轨道倾角的变率
//对于单点定位,以上数据足以,但是为了保持卫星数据完整,以下保持了所有的数据
double CA;//CA码
double GPSweek;//gps周数
double L2P;//L2P码
double SVA;//卫星精度
double SVH;//卫星健康
double TGD;//电离层延迟
double IODC;//数据质量
double shoot_time;//发射时间
double h;//拟合区间
};
为什么序列号,年月日时分秒也要用double,在待会会讲解。
而读取的时候,因为格式是固定的,每一个完整的数据都有8行,我们可以每次就读8行,然后把这8行合并成同一个字符串,用来表示同一个数据块。而这个函数只负责读取,把读取的数据转换为可用的数据并导入在结构体中,就交给另外一个函数来完成。
void read()
{
fstream ifs(_path_name, ios_base::in);
if (!ifs) exit(-1);
while (!ifs.eof())
{
string data;
for (int i = 0; i < 8; i++)
{
//采用的是读取整个数据块的方法
char buffer[82];
ifs.getline(buffer, 81);
data += buffer;
}
get_data(data);
}
}
这个把字符串转换成有用的数据的方法,可以有很多实现形式,完全凭个人习惯。因为有36个数据,我们就定义一个36大小的double数组,每次读一个数据,就把数据放在double数组之中,一直到读完整个36个数据。
而读完之后,因为结构体采用的是内存连续区间,而数组也是内存连续区间,我们之间用memcpy,把数组的内存copy到结构体的内存上,就省略了大量的代码量。
void get_data(string& data)
{
double init[36];
int cur = 0;
for (int i = 0; i < 36; i++)
{
string tmp = get_part(data, cur);//获取一串数字的字符串
init[i] = _to_double(tmp);//把获得的字符串转换成double
//两个函数的具体实现看个人习惯,我的实现可以去看完整代码
}
satellite_data_block ins;
memcpy(&ins, init, sizeof(double) * 36);
_serial.insert(make_pair(ins.Serial, _data.size()));
_data.push_back(ins);
}
读取之后,我们直接把他放在数组中,然后接着读取下一个。最终数组中便完整记录了所有卫星的参数,我们可以访问任意卫星的参数。
但是,这里会有一个问题:我们怎么知道数组中对应的是哪个卫星?难道第5个元素对应的一定是序号为5的卫星吗?当然不一定。我们有两个解决方案:
- 每次遍历数组,查看每个元素的序列号
- 用哈希表,把元素下标对应上卫星的序列号
第一个方法的时间复杂度每次都是O(n),自然是比较不靠谱的,所以我们就采用了第二个方法,用哈希表的搜索降低时间复杂度。
卫星位置的解算
导航电文是两个小时更新一次的。因为这个特性,注定了导航电文无法发送即时的位置数据,所以卫星只能发送更新时刻的位置,然后根据一些参数来模拟卫星的运动,推算出卫星每一刻的位置。
用最简单的思想来看待这个问题,其实也很简单:
只不过具体实现的时候,要考虑的影响因素有很多就是了。
因为每个卫星参数对应的位置是独立的,所以对每一个卫星参数,我们都用一个类来表示,类中包含所有可能要用到的参数:
class calculate
{
private:
double sqrtA;
double angular_velocity;
double _tk;
double _angular_velocity;
double _Mk;
double _Ek;
double _Vk;
double _argument_of_latitude;
double _Cuk;
double _Crk;
double _Cik;
double _Uk;
double _Rk;
double _Ik;
double _OMGk;
};
具体的符号会在计算的过程中讲解。
1.计算角速度
第一个在广播星历中已经给出,我们只需要计算第二个:
#define GM 3.9860050E14
void sqrt_A(double sqrt_a)
{
sqrtA = sqrt_a;
angular_velocity = sqrt(GM) / pow(sqrtA, 3);
}
2.计算卫星运动时间
因为广播星历给出的时间Toe是每次更新的瞬间时间,而Tobs是当前的观测时间,所以Tobs-Toe,再消去卫星信号传播时间导致的误差,就是卫星运动的时间Tk。
注意:在广播星历中,给出的Toe是周内秒,而实际的计算计算的是总时间差。这里的Toe并非表示周内秒,而是GPS包含周的总时间。为了计算方便,我们可以直接把他转换成Unix时来进行加减。
double get_time(double Toe,double week,atime cur = curtime)
{
//因为没有考虑信号传播时间,最终有小部分误差
GPStime _bt(week, Toe);
atime bt(_bt);
double ret = cur.GetGtime()._time-bt.GetGtime()._time;
if (ret > ROUND_SECOND/2) ret -= ROUND_SECOND;
else if (ret < -ROUND_SECOND / 2) ret += ROUND_SECOND;
//ROUND_SECOND GPS周一周的秒数
_tk = ret;
return ret;
}
3.对平均角速度改正
在广播星历中,会给出一个参数:Δn,这个参数就是给我们用以修正角速度的,我们直接把计算出的角速度修正上Δn便可以了:
double new_angular_velocity(double deltan)
{
_angular_velocity = angular_velocity + deltan;
return _angular_velocity;
}
4.计算平近点角
广播星历会给出一个参数M0,是在数据更新时刻的近点角。而此时,卫星运动了Tk时刻,我们用角速度n乘以运动时间Tk,就能得出卫星距离更新时刻转动的角度,再加上更新时刻的角度,就是当前时刻的平近点角
double mean_anomaly(double M0)
{
_Mk = M0 + _tk * _angular_velocity;
return _Mk;
}
5.计算偏近点角
公式:
和时空转换一样,我们发现,这个公式左边有一个E,右边有一个E,怎么可以用自己来计算自己呢?这里就要用迭代的方法,取E一个初值En,然后不断求下一个自己的值En+1,一直到En+1和En相差很小很小,就代表他们收敛到了一个具体的值上,这个值就是我们要求得的值。
double Eccentric_Anomaly(double e)
{
double Mk = _Mk;
auto get_Ek = [e,Mk](double Ep)
{
return Ep - (Ep - e * sin(Ep) - Mk) / (1 - e * cos(Ep));
};
double E0 = _Mk;
double E1 = get_Ek(E0);
while (abs(E1 - E0) >= 1E-14)
{
E0 = E1;
E1 = get_Ek(E0);
}
_Ek = E1;
return _Ek;
}
6.计算真近点角
double True_anomaly(double e)
{
_Vk = atan((sqrt(1 - e * e) * sin(_Ek) )
/ (cos(_Ek) - e));
if (abs(_Vk - _Mk) > Pi / 2)
_Vk += _Vk > _Mk ? -Pi : Pi;
return _Vk;
}
这里也可以直接用atan2来实现。
有一点要注意:导航电文中给出的所有数据都是弧度,而非角度,所以我们在使用sin等函数的时候,直接使用广播星历中的数据便可以了,不需要角度转弧度即*180/Pi
7.计算升交角距
double argument_of_latitude(double omega)
{
_argument_of_latitude = _Vk + omega;
return _argument_of_latitude;
}
8.计算二阶调和改正数
double Correction_of_uk(double cuc,double cus)
{
_Cuk = cus * sin(2 * _argument_of_latitude) + cuc * cos(2 * _argument_of_latitude);
return _Cuk;
}
double Correction_of_rk(double crc, double crs)
{
_Crk = crs * sin(2 * _argument_of_latitude) + crc * cos(2 * _argument_of_latitude);
return _Crk;
}
double Correction_of_ik(double cic, double cis)
{
_Cik = cis * sin(2 * _argument_of_latitude) + cic * cos(2 * _argument_of_latitude);
return _Cik;
}
9.计算改正后的开普勒轨道根数
double get_uk()
{
_Uk = _argument_of_latitude + _Cuk;
return _Uk;
}
double get_rk(double e)
{
_Rk = sqrtA*sqrtA * (1 - e * cos(_Ek)) + _Crk;
return _Rk;
}
double get_ik(double i0, double idot)
{
_Ik = i0 + _Cik + idot * _tk;
return _Ik;
}
double get_OMGk(double OMG0,double OMGd,double toe)
{
_OMGk = OMG0 + (OMGd - OMGE) * _tk - OMGE * toe;
return _OMGk;
}
10.计算卫星的位置
在求出卫星在轨道平面坐标系的坐标后,把该坐标绕x轴旋转i角(轨道倾角),绕z轴旋转OmegaK角(升交点经度),得到的便是在地心地固坐标系下的坐标。这个部分是线代中的坐标系转换,具体的方程感兴趣可以自己推导
vector<double> get_xyz()
{
double _xk = _Rk * cos(_Uk);
double _yk = _Rk * sin(_Uk);
double x = _xk * cos(_OMGk) - _yk * cos(_Ik) * sin(_OMGk);
double y = _xk * sin(_OMGk) + _yk * cos(_Ik) * cos(_OMGk);
double z = _yk * sin(_Ik);
return { x,y,z };
}
最终,便可以得到卫星坐标的位置xyz。
代码汇总:
我们采用了一个类来完成,在传入一个卫星数据块后,便在构造函数里直接计算出该数据块对应的卫星位置。而因为读取卫星数据,计算卫星数据是分离的,所以我们可以再设计另外一个接口,将两个类的功能结合在一起,而这个实现方法可以根据个人习惯,也可以直接参考我完整的代码。
#define GM 3.9860050E14
#define OMGE 7.2921151467E-5
commontime _curtime = commontime({ 2021,2,1,0,0 }, 30);//当前观测时间
atime curtime = atime(_curtime);
//计算的完整过程,必须按顺序依次进行
class calculate
{
public:
vector<double> _calculate(const satellite_data_block& s)
{
sqrt_A(s.sqrtA);
get_time(s.TOE, s.GPSweek);
new_angular_velocity(s.deltan);
mean_anomaly(s.M0);
Eccentric_Anomaly(s.e);
True_anomaly(s.e);
argument_of_latitude(s.omega);
Correction_of_uk(s.Cuc, s.Cus);
Correction_of_rk(s.Crc, s.Crs);
Correction_of_ik(s.Cic, s.Cis);
get_uk();
get_rk(s.e);
get_ik(s.i0, s.IDOT);
get_OMGk(s.OMEGA, s.deltaomega, s.TOE);
vector<double> xyz = get_xyz();
return xyz;
}
private:
void sqrt_A(double sqrt_a)
{
sqrtA = sqrt_a;
angular_velocity = sqrt(GM) / pow(sqrtA, 3);
}
double get_time(double Toe,double week,atime cur = curtime)
{
//因为没有考虑信号传播时间,最终有小部分误差
GPStime _bt(week, Toe);
atime bt(_bt);
double ret = cur.GetGtime()._time-bt.GetGtime()._time;
if (ret > ROUND_SECOND/2) ret -= ROUND_SECOND;
else if (ret < -ROUND_SECOND / 2) ret += ROUND_SECOND;
_tk = ret;
return ret;
}
double new_angular_velocity(double deltan)
{
_angular_velocity = angular_velocity + deltan;
return _angular_velocity;
}
double mean_anomaly(double M0)
{
_Mk = M0 + _tk * _angular_velocity;
return _Mk;
}
double Eccentric_Anomaly(double e)
{
double Mk = _Mk;
auto get_Ek = [e,Mk](double Ep)
{
return Ep - (Ep - e * sin(Ep) - Mk) / (1 - e * cos(Ep));
};
double E0 = _Mk;
double E1 = get_Ek(E0);
while (abs(E1 - E0) >= 1E-14)
{
E0 = E1;
E1 = get_Ek(E0);
}
_Ek = E1;
return _Ek;
}
double True_anomaly(double e)
{
_Vk = atan((sqrt(1 - e * e) * sin(_Ek) )
/ (cos(_Ek) - e));
if (abs(_Vk - _Mk) > Pi / 2)
_Vk += _Vk > _Mk ? -Pi : Pi;
return _Vk;
}
double argument_of_latitude(double omega)
{
_argument_of_latitude = _Vk + omega;
return _argument_of_latitude;
}
double Correction_of_uk(double cuc,double cus)
{
_Cuk = cus * sin(2 * _argument_of_latitude) + cuc * cos(2 * _argument_of_latitude);
return _Cuk;
}
double Correction_of_rk(double crc, double crs)
{
_Crk = crs * sin(2 * _argument_of_latitude) + crc * cos(2 * _argument_of_latitude);
return _Crk;
}
double Correction_of_ik(double cic, double cis)
{
_Cik = cis * sin(2 * _argument_of_latitude) + cic * cos(2 * _argument_of_latitude);
return _Cik;
}
double get_uk()
{
_Uk = _argument_of_latitude + _Cuk;
return _Uk;
}
double get_rk(double e)
{
_Rk = sqrtA*sqrtA * (1 - e * cos(_Ek)) + _Crk;
return _Rk;
}
double get_ik(double i0, double idot)
{
_Ik = i0 + _Cik + idot * _tk;
return _Ik;
}
double get_OMGk(double OMG0,double OMGd,double toe)
{
_OMGk = OMG0 + (OMGd - OMGE) * _tk - OMGE * toe;
return _OMGk;
}
vector<double> get_xyz()
{
double _xk = _Rk * cos(_Uk);
double _yk = _Rk * sin(_Uk);
double x = _xk * cos(_OMGk) - _yk * cos(_Ik) * sin(_OMGk);
double y = _xk * sin(_OMGk) + _yk * cos(_Ik) * cos(_OMGk);
double z = _yk * sin(_Ik);
return { x,y,z };
}
private:
double sqrtA;
double angular_velocity;
double _tk;
double _angular_velocity;
double _Mk;
double _Ek;
double _Vk;
double _argument_of_latitude;
double _Cuk;
double _Crk;
double _Cik;
double _Uk;
double _Rk;
double _Ik;
double _OMGk;
};
调试数据:
为了方便大家调试,我列出了对应的调试数据:
对应我的文件中序列号为1的卫星:
广播星历参数:
计算出的数据:最终结果:
更多的测试用例可以直接在完整源码中查看所有31个卫星数据
最后,给自己叠个甲。因为自己才是导航工程大二的本科生,有些概念理解可能不到位,而又想用最容易理解的方式表达出来,所以可能正确性会稍微有些偏差。但是对初学者来说,应该不会存在太大的错误,如果可以帮到你,真的荣幸之极。还有