紧组合的初始对准量测方程及仿真结果：以某型火箭弹飞行轨迹为例

9.4.3.1　状态方程的建立

紧组合状态方程由MEMS惯性导航系统的状态方程和卫星的状态方程两部分组成,其中惯性导航系统的状态方程与松组合中的一致。

根据卫星伪距定位原理,通常取卫星定位的状态向量XG为时钟漂移δtu和时钟率漂移δtru,即

并满足

式中,βtru——反相关时间；

ωtu,ωtru——零均值的高斯白噪声。

因此,根据式(9.18)可建立卫星的状态方程：

综合MEMS惯性导航系统和卫星的状态方程,可得基于伪距和伪距率的滤波器的系统状态方程为

9.4.3.2　观测方程的建立

紧组合的观测量为卫星接收到的伪距、伪距率与惯导利用信标位置等信息解算得到的伪距和伪距率之差,因此,观测方程建立的关键在于如何利用惯导数据解算得到相应的伪距和伪距率。

1.伪距观测方程

在地心地固坐标系中,弹体的真实位置为(x,y,z),惯导解算得到的地理位置为(LI,λI,hI),可利用如下坐标转换关系得到(xI,yI,zI)

则有

假设第j颗卫星在地心地固坐标系下的位置为则惯导解算点与第j颗卫星之间的距离为

将式(9.23)进行泰勒展开,可得

式中——惯导解算点到第j颗卫星点连线的方向余弦,即

卫星解算得到的伪距方程为

式中,δtu——卫星时钟偏差；

Vrj——量测噪声。

因此紧组合系统的伪距观测方程为

由于本书选择的状态向量中位置误差由δL、δλ、δh表示,所以上式中的δx、δy、δz 还需经过如下转换：

综合上述各式,可得伪距观测方程为

2.伪距率观测方程

载体的真实位置(x,y,z)到第j颗卫星点的距离为

距离变化率为

惯导解算得到的载体位置(xI,yI,zI)到第j颗卫星点的距离为

对该式求导,得

式中,

(https://www.xing528.com)

是弹体在地心地固坐标系中的真实位置。

下面求在地心地固坐标系中的惯导解算速度误差与当地地理坐标系中的速度误差(δVe,δVn,δVu)之间的转换关系。

对两边微分,可知

其中的求解过程如下：

根据式(9.22),有

并且弹体真实速度和惯导解算速度满足下式：

将在弹体真实位置(L,λ)处按照泰勒公式展开,可得

式中,

并记

因此,大地坐标系下惯导解算速度值投影到当地地理坐标系下为

综合上述各式,可得惯导解算的伪距率为

卫星测量的伪距率为

式中,δtru——卫星的时钟频率偏差。

因此,伪距率量测方程为

综合伪距、伪距率量测方程,可以得出紧组合初始对准的量测方程为

式中

9.4.3.3　仿真及结果

以某型火箭弹为例,其飞行轨迹如图9.12所示。利用紧组合模型,得到的位置、速度、姿态误差滤波结果如图9.13～图9.15所示。

图9.12　某型火箭弹弹道

图9.13　位置误差滤波结果(书后附彩插)

(a)纬度误差；(b)经度误差；(c)高度误差

图9.14　速度误差滤波结果(书后附彩插)

(a)东向速度误差；(b)北向速度误差；(c)天向速度误差

图9.15　姿态误差滤波结果(书后附彩插)

(a)偏航角误差；(b)俯仰角误差；(c)滚转角误差

从图9.13中可以看出,在前15 km,卡尔曼滤波校正后的弹丸东向位置稳态误差为2.77 m,北向位置稳态误差为4.1 m,天向位置稳态误差为6.3 m。

由图9.14 中可知,在前15 km,卡尔曼滤波校正后的弹丸东向速度稳态误差为0.19 m/s,北向速度稳态误差为1.05 m/s,天向速度稳态误差为0.57 m/s。

从图9.15可知,在前15 km,卡尔曼滤波校正后的弹丸偏航角稳态误差为0.64°,俯仰角稳态误差为0.45°,滚转角稳态误差为0.26°。