基于三维坐标旋转补偿和动态滤波的双向地基定位方法

1.本发明涉及导航定位、航天测控技术领域，具体涉及一种基于三维坐标旋转补偿和动态滤波的双向地基定位方法。


背景技术：

2.基于双向测量的地基定位在导航定位、航天测控中具有广泛的应用。
3.在地基站和航天器进行双向测量时，地球自转和信号处理延时期间航天器的机动，会导致上行和下行传输延时不相等。地球自转和航天器在信号处理延时期间的机动，改变了地基站和航天器在惯性坐标系下的位置，导致上行和下行传输链路不一致，进而使得上行和下行传输延时不相等。然而，在传统双向地基定位观测模型中，认为无线电信号在航天器和地基站间的双向传输延时相同，这种模型误差会导致双向地基定位精度变差。将造成传输延时不一致的因素进行建模和补偿，可以有效提高双向定位精度，在航天测控中具有十分重要的意义。
4.在2018年8月ieee transactions on ultrasonics,ferroelectrics,and frequency control期刊第65卷，第8期1475-1486页的“an improved protocol for performing two-way satellite time and frequency transfer using a satellite in an inclined geo-synchronous orbit”中，在已知地基站和航天器坐标的前提下，对地球自转导致的双向传输延时不一致特性进行了建模和补偿。另外，通过测量航天器相对地基站的俯仰角和方位角，将地球自转造成的地基站位移矢量，投影到地基站和航天器间的径向矢量上，可以一定程度上补偿地球自转导致的双向传输延时不一致特性，相比认为双向传输链路对称下传输延时相同的方法，可以提高定位精度，但该补偿方法仍然存在一定的缺陷，短时间内将地基站随地球自转的运动，近似为一维匀速直线运动的模型假设，存在模型误差，并且需要准确的角度测量值计算补偿量。
5.因此如何将因地球自转和航天器机动导致的对双向传输延时不一致因素去除，从而实现更为准确的定位，是目前尚未解决的问题。


技术实现要素：

6.有鉴于此，本发明提供了一种基于三维坐标旋转补偿和动态滤波的双向地基定位方法，能够根据实际无线电信号传输路径，在观测方程中将地球自转和航天器机动等，造成双向传输延时不一致的因素，通过三维坐标旋转矩阵进行建模，根据估计的航天器状态，将航天器位置和速度，对齐到待定位时刻的地固坐标系中，获得更为精确的定位结果。
7.为达到上述目的，本发明的技术方案为：利用双向地基定位系统对航天器进行定位，双向地基定位系统由多个钟差已知的固定地基站组成，该双向地基定位方法包括如下步骤：
8.收集所有地基站在一次双向地基定位期间获得的相位差。
9.根据动力学建模获得航天器状态空间方程，根据航天器状态空间方程进一步得到
离散形式的状态更新方程，利用状态更新方程根据历史信息预测得到当前航天器预测状态。
10.获取航天器的观测值，利用观测值修正当前航天器预测状态，得到当前航天器估计状态，其中包含航天器位置估计值和速度估计值。
11.根据航天器位置估计值获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，坐标系修正后得到航天器在待定位时刻地固坐标系下的三维位置和速度。
12.进一步地，收集所有地基站在一次双向地基定位期间获得的相位差，具体为：每次定位期间地基站i发射上行信号，信号经过上行传输延时τ
ui
后被航天器接收；航天器采样上行信号相位，并经过信号处理延时t
p
后发送下行信号给地基站；信号经过下行传输延时τ
di
后被地基站i接收，同时地基站i对下行信号的相位进行采样和解帧，获得航天器采样的上行相位，并计算得到上行和下行相位差将所有地基站在一次双向地基定位期间获得的相位差收集并记为其中n为地基站数目。
13.进一步地，根据动力学建模获得航天器状态空间方程，根据航天器状态空间方程进一步得到离散形式的状态更新方程，利用状态更新方程根据历史信息预测得到当前航天器预测状态，具体为：
14.根据动力学建模获得的航天器状态空间方程为
[0015][0016]
其中pm为航天器位置，为pm的一阶导数；vm为航天器的速度；为航天器燃料推进力对应的加速度，为航天器所受空气阻力对应的加速度，为航天器重力对应加速度，为航天器所受科里奥利力对应的加速度，为航天器向心力对应的加速度；
[0017]
待定位航天器的状态为k时刻时间一步预测的输入为上一时刻估计得到的航天器状态xm(k-1)、输入向量u(k-1)和后验均方误差阵p(k-1)，输出为时间一步预测状态和先验估计均方误差阵根据(1)得到离散形式状态更新方程为
[0018][0019]
其中a为状态转移矩阵、b为噪声分配矩阵，q为过程噪声协方差阵；
[0020][0021]
其中，t动态滤波更新时间，i3×3为维度为3
×
3的单位阵，σ1为扩散系数；利用上述离散形式状态更新方程根据历史信息预测得到当前航天器预测状态。
[0022]
进一步地，获取航天器的观测值，利用观测值修正当前航天器预测状态，得到当前航天器估计状态，具体为：
[0023]
k时刻航天器预测状态为k时刻的观测向量为y(k)和先验均方误差阵k时刻航天器估计状态为和后验估计均方误差阵为p(k)：
[0024][0025]
其中k(k)为卡尔曼增益，h(k)为非线性观测方程的雅克比矩阵，ν(k)为残差向量。
[0026]
根据双向地基定位期间，实际无线电信号传输情况，得到地基站i的时差观测模型
[0027][0028]
其中f
pn
为码速率，c为光速，为相位差集合，pm为航天器位置，c(
·
)代表地球自转有关的旋转矩阵，δ为其他地基站与地基站i的钟差，pg为地基站位置，vm为航天器的速度，t
p
为信号处理延时，am为航天器加速度，为上行和下行传输延时总和，η为零均值方差为σ的高斯噪声。
[0029]
在上行和下行通信链路获得多普勒频移观测量为fd：
[0030][0031]
其中，fd为多普勒频移观测量，vg为地基站发射上行信号时刻地固坐标系下的速度，对于固定地基站速度为0，ωi为零均值均方差为σ
fi
的高斯噪声。
[0032]
由此获得航天器位置估计值，航天器位置估计值记为
[0033]
进一步地，根据航天器位置估计值获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，坐标系修正后得到航天器在待定位时刻地固坐标系下的三维位置和速度，具体为：
[0034]
根据卡尔曼滤波得到估算航天器位置和地基站位置计算上行传输延时进而将这段时间内的地球自转引起的位置和速度修正为
[0035][0036]
其中为单边传输延时。
[0037]
坐标系修正后完成航天器在待定位时刻地固坐标系下的三维位置和速度。
[0038]
有益效果：
[0039]
本发明利用三维坐标旋转矩阵，对双向地基定位期间，地球自转和航天器机动等，造成双向传输延时不一致的因素进行三维建模，构建非对称双向测量模型，并通过卡尔曼动态滤波完成双向地基定位。相对于常规认为双向传输延时一致的地基定位方法，和将地基站位移投影到径向矢量上补偿双向传输延时不一致的地基定位方法，本方法观测模型更加符合实际无线电信号传输特性，因此定位精度更高。此外，本方法不需要高精度的角度测量对传输延时不一致特性进行补偿。
附图说明
[0040]
图1为本发明提供了一种基于三维坐标旋转补偿和动态滤波的双向地基定位方法流程框图。
具体实施方式
[0041]
下面结合附图并举实施例，对本发明进行详细描述。
[0042]
本发明提供了一种基于三维坐标旋转补偿和动态滤波的双向地基定位方法，该方法利用双向地基定位系统对航天器进行定位，双向地基定位系统由多个钟差已知的固定地基站组成，该双向地基定位方法包括如下步骤：
[0043]
状态时间一步预测步骤：收集所有地基站在一次双向地基定位期间获得的相位差；根据动力学建模获得航天器状态空间方程，根据航天器状态空间方程进一步得到离散形式的状态更新方程，利用状态更新方程根据历史信息预测得到当前航天器预测状态；
[0044]
量测更新步骤：获取航天器的观测值，利用观测值修正当前航天器预测状态，得到当前航天器估计状态，其中包含航天器位置估计值和速度估计值；
[0045]
坐标系修正步骤：根据航天器位置估计值获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，坐标系修正后得到航天器在待定位时刻地固坐标系下的三维位置和速度。
[0046]
双向地基定位中通过动态滤波模块完成航天器位置解算。本发明的动态滤波模块的流程框图如图1所示。动态滤波模块的输入为一次上行和下行通信过程中地基站的相位差，输出为航天器的位置和速度。动态滤波包括三个步骤分别是状态时间一步预测、量测更新和坐标系修正步骤。首先，根据历史信息预测当前航天器状态，完成状态时间一步预测步骤。然后，根据当前的观测值修正预测的航天器状态，完成量测更新步骤。最后，根据估计得到的航天器位置获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，完成坐标系修正步骤。
[0047]
本发明的具体步骤如下：
[0048]
不失一般性，对双向地基定位进行如下描述：双向地基定位系统由多个钟差已知的固定地基站组成，对航天器进行定位。每次定位期间地基站i发射上行信号，信号经过上行传输延时τ
ui
后被航天器接收。航天器采样上行信号相位，并经过信号处理延时t
p
后发送下行信号给地基站。信号经过下行传输延时τ
di
后被地基站i接收，同时地基站i对下行信号的相位进行采样和解帧，获得航天器采样的上行相位，并计算得到上行和下行相位差将所有地基站在一次双向地基定位期间获得的相位差收集并记为其中n为地基站数目。假设待定位航天器的状态为其中地心地固坐标系下的航天器位置和速度分别为pm＝[xm,ym,zm]
t
和航天器加速度为地心地固坐标下第i个地基站的位置为p
gi
＝[x
gi
,y
gi
,z
gi
]
t
。
[0049]
下面依次从动态滤波模块中的状态时间一步预测、量测更新和坐标系修正步骤进行分析。
[0050]
1)状态时间一步预测步骤
[0051]
根据动力学建模航天器的状态空间方程为
[0052][0053]
其中pm为航天器位置，为pm的一阶导数；vm为航天器的速度；am为航天器加速度；为航天器燃料推进力对应的加速度，为航天器所受空气阻力对应的加速度，为航天器重力对应加速度，为航天器所受科里奥利力对应的加速度，为航天器向心力对应的加速度；
[0054]
k时刻时间一步预测的输入为上一时刻估计得到的航天器状态xm(k-1)、输入向量u(k-1)和后验均方误差阵p(k-1)，输出为时间一步预测状态和先验估计均方误差阵根据(1)得到离散形式状态更新方程为
[0055][0056]
其中状态转移矩阵a、噪声分配矩阵b以及过程噪声协方差阵q，表示为
[0057][0058]
其中t动态滤波更新时间，i3×3为3
×
3的单位阵，σ1为扩散系数。
[0059]
输入向量航天器燃料推进加速度的方向与航天器速度相同，大小表示为
[0060][0061]
其中g为重力加速度，i
sp
为比冲量，m为航天器质量。空气阻力对应的加速度方向和航天器运动速度相反，大小表示为
[0062][0063]
其中cd为阻力系数和速度大小有关，s定义为垂直于速度的航天器体横截面积，ρ为空气密度。航天器重力方向由航天器位置指向地心，大小为
[0064][0065]
其中ug＝3.99
×
10
14
nm2/kg。航天器所受科里奥利力和向心力对应的加速度表示为
[0066][0067]
其中
“×”
表示叉乘运算，we＝[0,0,-ω]
t
，ω为地球自转角速度。
[0068]
2)量测更新步骤
[0069]
k时刻量测更新步骤的输入为预测状态观测向量y(k)和先验均方误差阵输出为估计状态和后验估计均方误差阵p(k)。量测更新步骤为
[0070][0071]
其中k(k)为卡尔曼增益，h(k)为非线性观测方程的雅克比矩阵，ν(k)为残差向量；卡尔曼增益为卡尔曼增益，观测向量为时差测量量和多普勒频移测量量的增广形式，c为光速，前面有，后面有，上行多普勒平移观测量。向量值函数形式观测方程为y＝h(x,v
t
)，观测噪声向量v
t
＝[η
τ
,ω
τ
]
t
建模为零均值高斯模型，r为观测噪声向量v
t
对应协方差矩阵。向量值函数h(
·
)由时差观测模型和多普勒频移观测模型确定。
[0072]
残差向量定义为
[0073][0074]
时差测量量部分以地基站i作为解算参考站为例进行说明，根据双向地基定位期
间，实际无线电信号传输情况，得到地基站i的时差观测模型为
[0075][0076]
其中f
pn
为码速率，c为光速，为相位差集合，pm为航天器位置，c(
·
)代表地球自转有关的旋转矩阵，δ为其他地基站与地基站i的钟差，pg为地基站位置，vm为航天器的速度，t
p
为信号处理延时，am为航天器加速度，为上行和下行传输延时总和，η为零均值方差为σ的高斯噪声；
[0077]
c(
·
)代表地球自转有关的旋转矩阵，可以表示为
[0078][0079]
其中i3×3为3
×
3的单位阵，ω为地球自转角速率为7.29
×
10-5
，u
×
为u的反对称阵。
[0080]
除了时差测量量在上行和下行通信链路还可以获得多普勒频移观测量。上行多普勒频移观测模型为
[0081][0082]
其中，fd为多普勒频移观测量，vg为地基站发射上行信号时刻地固坐标系下的速度，对于固定地基站速度为0，ωi为零均值均方差为σ
fi
的高斯噪声。根据(10)和(12)可通过前两个步骤获得航天器位置估计值，将航天器位置估计值记为
[0083]
3)坐标系修正步骤
[0084]
由于建立观测模型时将其他地基站发射时刻坐标器旋转对齐到地基站i的发射时刻地固坐标系。为了得到航天器在接收时刻的地固坐标系下的位置，需要根据卡尔曼滤波得到估算航天器位置和地基站位置计算上行传输延时进而将这段时间内的地球自转引起的位置和速度修正为
[0085][0086]
其中为通过时间一步预测和量测更新步骤解算获得的单边传输延时，可以表示为
[0087][0088]
其中p
gi
为地心地固坐标下第i个地基站的位置为p
gi
＝[x
gi
,y
gi
,z
gi
]
t
。
[0089]
坐标系修正后完成航天器在待定位时刻地固坐标系下的三维位置和速度。
[0090]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.基于三维坐标旋转补偿和动态滤波的双向地基定位方法，其特征在于，该方法利用双向地基定位系统对航天器进行定位，所述双向地基定位系统由多个钟差已知的固定地基站组成，该双向地基定位方法包括如下步骤：收集所有地基站在一次双向地基定位期间获得的相位差；根据动力学建模获得航天器状态空间方程，根据航天器状态空间方程进一步得到离散形式的状态更新方程，利用所述状态更新方程根据历史信息预测得到当前航天器预测状态；获取航天器的观测值，利用观测值修正当前航天器预测状态，得到当前航天器估计状态，其中包含航天器位置估计值和速度估计值；根据所述航天器位置估计值获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，坐标系修正后得到航天器在待定位时刻地固坐标系下的三维位置和速度。2.如权利要求1所述的基于三维坐标旋转补偿和动态滤波的双向地基定位方法，其特征在于，所述收集所有地基站在一次双向地基定位期间获得的相位差，具体为：每次定位期间地基站i发射上行信号，信号经过上行传输延时τ
ui
后被航天器接收；航天器采样上行信号相位，并经过信号处理延时t
p
后发送下行信号给地基站；信号经过下行传输延时τ
di
后被地基站i接收，同时地基站i对下行信号的相位进行采样和解帧，获得航天器采样的上行相位，并计算得到上行和下行相位差将所有地基站在一次双向地基定位期间获得的相位差收集并记为其中n为地基站数目。3.如权利要求1或2所述的基于三维坐标旋转补偿和动态滤波的双向地基定位方法，其特征在于，所述根据动力学建模获得航天器状态空间方程，根据航天器状态空间方程进一步得到离散形式的状态更新方程，利用所述状态更新方程根据历史信息预测得到当前航天器预测状态，具体为：所述根据动力学建模获得的航天器状态空间方程为其中p
m
为航天器位置，为p
m
的一阶导数；v
m
为航天器的速度；为航天器燃料推进力对应的加速度，为航天器所受空气阻力对应的加速度，为航天器重力对应加速度，为航天器所受科里奥利力对应的加速度，为航天器向心力对应的加速度；待定位航天器的状态为k时刻时间一步预测的输入为上一时刻估计得到的航天器状态x
m
(k-1)、输入向量u(k-1)和后验均方误差阵p(k-1)，输出为时间一步预测状态和先验估计均方误差阵根据(1)得到离散形式状态更新方程为其中a为状态转移矩阵、b为噪声分配矩阵，q为过程噪声协方差阵；
其中，t动态滤波更新时间，i3×3为维度为3
×
3的单位阵，σ1为扩散系数；利用上述离散形式状态更新方程根据历史信息预测得到当前航天器预测状态。4.如权利要求3所述的基于三维坐标旋转补偿和动态滤波的双向地基定位方法，其特征在于，所述获取航天器的观测值，利用观测值修正当前航天器预测状态，得到当前航天器估计状态，具体为：k时刻航天器预测状态为k时刻的观测向量为y(k)和先验均方误差阵k时刻航天器估计状态为和后验估计均方误差阵为p(k)：其中k(k)为卡尔曼增益，h(k)为非线性观测方程的雅克比矩阵，ν(k)为残差向量；根据双向地基定位期间，实际无线电信号传输情况，得到地基站i的时差观测模型其中f
pn
为码速率，c为光速，为相位差集合，p
m
为航天器位置，c(
·
)代表地球自转有关的旋转矩阵，δ为其他地基站与地基站i的钟差，p
g
为地基站位置，v
m
为航天器的速度，t
p
为信号处理延时，a
m
为航天器加速度，为上行和下行传输延时总和，η为零均值方差为σ的高斯噪声；在上行和下行通信链路获得多普勒频移观测量为f
d
：其中，f
d
为多普勒频移观测量，v
g
为地基站发射上行信号时刻地固坐标系下的速度，对于固定地基站速度为0，ω
i
为零均值均方差为σ
fi
的高斯噪声；由此获得航天器位置估计值，航天器位置估计值记为5.如权利要求4所述的基于三维坐标旋转补偿和动态滤波的双向地基定位方法，其特征在于，所述根据所述航天器位置估计值获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，坐标系修正后得到航天器在待定位时刻地固坐标系下的三维位置
和速度，具体为：根据卡尔曼滤波得到估算航天器位置和地基站位置计算上行传输延时进而将这段时间内的地球自转引起的位置和速度修正为：其中为单边传输延时；坐标系修正后完成航天器在待定位时刻地固坐标系下的三维位置和速度。

技术总结
本发明公开了基于三维坐标旋转补偿和动态滤波的双向地基定位方法，涉及导航定位、航天测控技术领域，该方法包括如下步骤：收集所有地基站在一次双向地基定位期间获得的相位差；根据动力学建模获得航天器状态空间方程，根据航天器状态空间方程进一步得到离散形式的状态更新方程，利用状态更新方程根据历史信息预测得到当前航天器预测状态；获取航天器的观测值，利用观测值修正当前航天器预测状态，得到当前航天器估计状态，其中包含航天器位置估计值和速度估计值；根据航天器位置估计值获得传输延时，并将航天器坐标修正到待定位时刻的地心地固坐标系中，坐标系修正后得到航天器在待定位时刻地固坐标系下的三维位置和速度。在待定位时刻地固坐标系下的三维位置和速度。在待定位时刻地固坐标系下的三维位置和速度。


技术研发人员：王永庆 申宇瑶 杨振华 陈岩 何宜根
受保护的技术使用者：北京理工大学
技术研发日：2023.07.14
技术公布日：2023/10/20