卫星位置解算

鹅一百

已于 2024-06-04 12:28:28 修改

阅读量1.1k

点赞数 12

分类专栏： # 卫星导航原理文章标签：卫星导航原理导航电文卫星位置解算

于 2024-06-03 18:14:57 首次发布

本文链接：https://blog.csdn.net/qq_74260823/article/details/139361271

版权

卫星导航原理专栏收录该内容

5 篇文章 1 订阅

订阅专栏

前言：

本章节代码均在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=M+e*sinE$

和时空转换一样，我们发现，这个公式左边有一个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;
};