CN101793529B - 一种双伪卫星辅助惯性导航系统位置校准方法 - Google Patents

Abstract

一种双伪卫星辅助校准惯性导航系统位置误差的方法，将惯导系统位置误差Δx，Δy、惯导系统与伪卫星之间不同步时间Δτ及伪卫星接收机钟差ΔT₀作为待求变量；当载体开始接收到两颗伪卫星信号的时刻，根据惯导系统输出计算得到的惯导-伪卫星距离，简称计算伪距，将计算伪距与伪卫星接收机输出的测量伪距做差，求出伪卫星接收机钟差，作为伪卫星接收机钟差的初始估值；建立伪卫星接收机测量伪距与计算伪距之差，与所述待求变量之间的数学模型，利用最小二乘法估计所述待求变量；利用估计得到的状态向量，对惯导系统的位置信息进行校准后输出。本发明利用两颗伪卫星的信息校正了惯导系统的位置误差，提高了导航精度。

Description

一种双伪卫星辅助惯性导航系统位置校准方法

技术领域

本发明涉及一种采用两颗伪卫星校准惯性导航系统位置误差的方法，属于导航定位技术领域。

背景技术

惯性导航系统(INS)自主性和隐蔽性好，能连续提供多种较高精度的导航参数(位置、速度、姿态、航向等)的输出信息，频带宽，是海陆空天各种载体的主要导航设备，但其误差(尤其是位置误差)随时间积累，不能长时间单独承担高精度导航的任务。卫星导航系统的定位和测速精度高，且基本不受地域、时间限制。因此，惯性导航和卫星导航组合是目前最常用的组合导航方式，但卫星导航系统最大的缺陷是易受干扰。因此在卫星系统被干扰或不可用的情况下，急需一种提高惯性导航系统精度的方法。

伪卫星也被称为陆基发射机或陆基卫星，其功能和原理与导航卫星类似，主要由接收机，发射机和天线等部分组成，能发出与导航卫星相同格式的电文。与导航卫星相比，伪卫星具有系统成本低、设置灵活机动等特点。此外，用户可以根据应用需要来设计伪卫星的信号结构和电文格式，具有完全的自主权。由于伪卫星与用户的距离远远小于卫星与用户的距离，因此伪卫星具有更强的抗干扰能力，得到了广泛的关注。

理论上，平面实时定位需要至少三颗卫星，本发明的方法使用两颗伪卫星，利用测量序列对惯导的位置误差进行估计并校正，从而提高导航的精度。

发明内容

本发明的技术问题是：针对卫星导航系统被干扰或不可用的情况，提供一种采用两颗伪卫星对惯导系统位置误差进行校准的方法，以提高惯性导航系统的性能。

本发明的技术解决方案：双伪卫星辅助惯性导航系统位置校准的方法，其特点在于步骤如下：

(1)将惯导系统位置误差Δx，Δy、惯导系统与伪卫星之间不同步时间Δτ及伪卫星接收机钟差ΔT₀作为待求变量；

(2)当载体开始接收到两颗伪卫星信号的时刻，根据惯导系统输出计算得到的惯导-伪卫星距离，简称计算伪距，将计算伪距与伪卫星接收机输出的测量伪距做差，求出伪卫星接收机钟差，作为伪卫星接收机钟差的初始估值；

(3)建立伪卫星接收机测量伪距与计算伪距之差Δr，与所述待求变量之间的数学模型：Δr＝r_m-r_I＝(v_x cosγ+v_y sinγ)Δτ+cΔT₀+Δxcosγ+Δysinγ；其中r_m是伪卫星接收机得到的测量伪距，r_I是根据惯导输出的位置得到的计算伪距，v_x，v_y是载体在x、y方向的运动速度，

(X，Y)是伪卫星的位置，(x_I，y_I)是惯导输出的位置；

(4)由补偿了伪卫星接收机钟差初始估值的伪卫星接收机输出的测量伪距和计算伪距得到二者之差，根据上一步得到的数学模型利用最小二乘法估计所述待求变量；

(5)利用步骤(4)估计得到的状态向量，对惯导系统的位置信息进行校准后输出。

本发明的原理：利用两颗伪卫星的伪距量测量，采用最小二乘法估计出惯导系统的位置误差、惯导/伪卫星的不同步时间和伪卫星接收机的钟差，从而修正惯导系统的位置误差，提高惯导系统的精度。

本发明与现有技术相比的优点如下：伪卫星设置简单灵活，只要有两颗伪卫星，利用最小二乘法就可估计出惯导的位置误差，对提高惯性导航系统的精度有明显效果。

附图说明

图1是实施本发明的原理示意图。

图2是实施本发明方法的惯导系统位置误差估计精度。

图3是实施本发明方法的惯导系统与伪卫星之间的不同步时间及接收机钟差的估计精度。

具体实施方式

本发明提出的双伪卫星辅助惯导系统位置校准方法，根据两颗伪卫星的伪距量测信息，利用最小二乘法估计出惯导系统的位置误差并校准，从而提高惯性导航系统的精度。

下面来阐述本发明的具体实施过程。

图1给出了本发明的原理示意图。实例中采用东北天地理坐标系为导航坐标系，考虑平面定位的情况。

1、确定待求变量

双伪卫星辅助惯导位置校准算法要估计的变量分别是：惯导系统和伪卫星系统之间的不同步时间Δτ，伪卫星接收机钟差ΔT₀，惯导的位置误差Δx，Δy。

由于在短时间内，惯导系统的位置误差可表示为：

$\left\{\begin{array}{c}\mathrm{\Δ}{x}_i=a_x\sin {\mathrm{\ω}}_0{t}_i^{\′}+b_x(1-\cos {{\mathrm{\ω}}_{0}t}_i^{\′})+c_x(t_i^{\′}-\frac{1}{{\mathrm{\ω}}_0}\sin {\mathrm{\ω}}_0{t}_i^{\′})\\ \mathrm{\Δ}{y}_i=a_y\sin {\mathrm{\ω}}_0{t}_i^{\′}+b_y(1-{\cos \mathrm{\ω}}_0{t}_i^{\′})+c_y(t_i^{\′}-\frac{1}{{\mathrm{\ω}}_0}\sin {\mathrm{\ω}}_0{t}_i^{\′})\end{array}\right.---\left(1\right)$

其中[a_x，b_x，c_x，a_y，b_y，c_y]^T为惯导系统位置误差系数，

ΔV_x0，ΔV_y0分别表示惯导系统的初始速度误差，

为舒拉角频率，

Δa_x，Δa_y分别表示惯导系统的加计零偏，R＝6378km是地球半径，g＝9.81m/s²是地球的重力加速度，c_x＝R·Δε_x，c_y＝R·Δε_y，Δε_x，Δε_y分别表示惯导系统的陀螺漂移。在本实施例中，取a_x＝a_y＝500m，b_x＝-b_y＝100m，c_x＝c_y＝1.5m/s；t′_i＝i·Δt+t₀，为惯导导航时间，i·Δt为估计时间，Δt为估计步长，t₀＝200s，为开始估计时惯导的导航时间。

因此，估计参数向量为[ΔT₀，Δτ，a_x，b_x，c_x，a_y，b_y，c_y]^T。

2、求伪卫星接收机钟差的初始估值

将接收到伪卫星信号的时刻定为估计起始时刻。在此时刻，根据惯导系统输出计算得到的计算伪距与伪卫星接收机输出的测量伪距做差，求出伪卫星接收机的钟差，作为接收机钟差的初始估值。

3、建立接收机测量伪距与计算伪距之差与待求变量之间的数学模型

接收机测量伪距与计算伪距之差Δr与待求变量[ΔT₀，Δτ，a_x，b_x，c_x，a_y，b_y，c_y]^T之间的数学模型可以表示为：

Δr＝r_m-r_I＝(v_x cosγ+v_y sinγ)Δτ+cΔT₀+Δxcosγ+Δysinγ    (2)

其中r_m是伪卫星接收机得到的测量伪距，r_I是根据惯导输出的位置得到的计算伪距，v_x，v_y是载体在x、y方向的运动速度，

(X，Y)是伪卫星的位置，(x_I，y_I)是惯导输出的位置。

4、由补偿了接收机钟差初始估值的接收机测量伪距和计算伪距得到二者之差，根据上一步得到的数学模型利用最小二乘法估计待求变量

采用最小二乘法估计待求变量。因为有两颗伪卫星，所以每一时刻可以得到两个测量伪距和计算伪距差值的方程。要估计八个参数，至少需要八个方程，因而从t₄＝4·Δt时刻开始估计。

最小二乘估计所采用的价值函数为：

$A=\underset{k=1}{\overset{2}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{({\mathrm{\Δr}}_{\mathrm{ki}}-\mathrm{\Δ}{\hat{r}}_{\mathrm{ki}})}^2---\left(3\right)$

其中，i表示估计的时刻，k表示伪卫星的编号。

将式(2)代入式(3)中得到价值函数：

$A=\underset{k=1}{\overset{2}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{[{\mathrm{\Δr}}_{\mathrm{ki}}-\mathrm{c\Δ}{\hat{T}}_{0(i-1)}-(v_x\cos {\mathrm{\γ}}_{\mathrm{ki}}+v_y\sin {\mathrm{\γ}}_{\mathrm{ki}})\mathrm{\Δ}{\widehat{\mathrm{\τ}}}_{i-1}-\cos {\mathrm{\γ}}_{\mathrm{ki}}\·\mathrm{\Δ}{\hat{x}}_{i-1}-{\sin \mathrm{\γ}}_{\mathrm{ki}}\·\mathrm{\Δ}{\hat{y}}_{i-1}]}^2---\left(4\right)$

问题化为估计伪卫星接收机钟差ΔT₀，惯导系统和伪卫星系统之间的不同步时间Δτ，惯导的位置误差Δx，Δy，使得价值函数A为最小。由于电磁波的速度为3×10⁸米/秒，因此定义接收机钟差的等效距离为ΔD₀＝c·ΔT₀，则t_N时刻要估计的参数向量为

也即解下述方程组(5)

$\left\{\begin{array}{c}\frac{\∂A}{\∂\mathrm{\Δ}{\hat{D}}_{0N}}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\widehat{\mathrm{\τ}}}_N}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\hat{a}}_{\mathrm{xN}}}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\hat{b}}_{\mathrm{xN}}}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\hat{c}}_{\mathrm{yN}}}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\hat{a}}_{\mathrm{yN}}}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\hat{b}}_{\mathrm{yN}}}=0\\ \frac{\∂A}{\∂\mathrm{\Δ}{\hat{c}}_{\mathrm{yN}}}=0\end{array}\right.---\left(5\right)$

将式(1)代入式(2)得：

$\mathrm{\Δ}{\hat{r}}_{\mathrm{ki}}=\mathrm{c\Δ}{\hat{T}}_{0(i-1)}+(v_x\cos {\mathrm{\γ}}_{\mathrm{ki}}+v_y\sin {\mathrm{\γ}}_{\mathrm{ki}})\mathrm{\Δ}{\widehat{\mathrm{\τ}}}_{i-1}+\cos {\mathrm{\γ}}_{\mathrm{ki}}\·\mathrm{\Δ}{\hat{x}}_{i-1}+\sin {\mathrm{\γ}}_{\mathrm{ki}}\·\mathrm{\Δ}{\hat{y}}_{i-1}$

$=\mathrm{\Δ}{\hat{D}}_{0(i-1)}+(v_x\cos {\mathrm{\γ}}_{\mathrm{ki}}+v_y\sin {\mathrm{\γ}}_{\mathrm{ki}})\mathrm{\Δ}{\widehat{\mathrm{\τ}}}_{i-1}+\cos {\mathrm{\γ}}_{\mathrm{ki}}\·\sin {\mathrm{\ω}}_0{t}_i^{\′}\·{\hat{a}}_{\mathrm{xi}}+\cos {\mathrm{\γ}}_{\mathrm{ki}}\·(1-\cos {\mathrm{\ω}}_0{t}_i^{\′})\·{\hat{b}}_{\mathrm{xi}}$

$+{\cos \mathrm{\γ}}_{\mathrm{ki}}\·(t_i^{\′}-\frac{1}{{\mathrm{\ω}}_0}\sin {\mathrm{\ω}}_0{t}_i^{\′})\·{\hat{c}}_{\mathrm{xi}}+\sin {\mathrm{\γ}}_{\mathrm{ki}}\·\sin {\mathrm{\ω}}_0{t}_i^{\′}\·{\hat{a}}_{\mathrm{yi}}+{\sin \mathrm{\γ}}_{\mathrm{ki}}\·(1-\cos {\mathrm{\ω}}_0{t}_i^{\′})\·{\hat{b}}_{\mathrm{yi}}$

$+\sin {\mathrm{\γ}}_{\mathrm{ki}}\·(t_i^{\′}-\frac{1}{{\mathrm{\ω}}_0}\sin {\mathrm{\ω}}_0{t}_i^{\′})\·{\hat{c}}_{\mathrm{yi}}$

$\stackrel{\mathrm{\Δ}}{=}{a}_{1\mathrm{ki}}\·\mathrm{\Δ}{\hat{D}}_{0(i-1)}+a_{2\mathrm{ki}}\·\mathrm{\Δ}{\widehat{\mathrm{\τ}}}_{i-1}+a_{3\mathrm{ki}}\·{\hat{a}}_{\mathrm{xi}}+a_{4\mathrm{ki}}\·{\hat{b}}_{\mathrm{xi}}+a_{5\mathrm{ki}}\·{\hat{c}}_{\mathrm{xi}}+a_{6\mathrm{ki}}\·{\hat{a}}_{\mathrm{yi}}+a_{7\mathrm{ki}}\·{\hat{b}}_{\mathrm{yi}}+a_{8\mathrm{ki}}\·{\hat{c}}_{\mathrm{yi}}---\left(6\right)$

定义：

$\left\{\begin{array}{c}{a}_{1\mathrm{ki}}=1\\ a_{2\mathrm{ki}}=v_x\cos {\mathrm{\γ}}_{\mathrm{ki}}+v_y\sin {\mathrm{\γ}}_{\mathrm{ki}}\\ a_{3\mathrm{ki}}={\cos \mathrm{\γ}}_{\mathrm{ki}}\·\sin {\mathrm{\ω}}_0{t}_i^{\′}\\ a_{4\mathrm{ki}}=\cos {\mathrm{\γ}}_{\mathrm{ki}}\·(1-\cos {\mathrm{\ω}}_0{t}_i^{\′})\\ a_{5\mathrm{ki}}={\cos \mathrm{\γ}}_{\mathrm{ki}}\·(t_i^{\′}-\frac{1}{{\mathrm{\ω}}_0}\sin {\mathrm{\ω}}_0{t}_i^{\′})\\ a_{6\mathrm{ki}}=\sin {\mathrm{\γ}}_{\mathrm{ki}}\·\sin {\mathrm{\ω}}_0{t}_i^{\′}\\ a_{7\mathrm{ki}}=\sin {\mathrm{\γ}}_{\mathrm{ki}}\·(1-\cos {\mathrm{\ω}}_0{t}_i^{\′})\\ a_{8\mathrm{ki}}=\sin {\mathrm{\γ}}_{\mathrm{ki}}\·(t_i^{\′}-\frac{1}{{\mathrm{\ω}}_0}\sin {\mathrm{\ω}}_0{t}_i^{\′})\end{array}\right.---\left(7\right)$

式(5)可写为：

$\left\{\begin{array}{c}\mathrm{\Σ\Σ\Δ}{\hat{r}}_{\mathrm{ki}}\·a_{1\mathrm{ki}}=\mathrm{\Σ\Σ\Δ}{r}_{\mathrm{ki}}\·a_{1\mathrm{ki}}\\ \mathrm{\Σ\Σ\Δ}{\hat{r}}_{\mathrm{ki}}\·a_{2\mathrm{ki}}=\mathrm{\Σ\Σ\Δ}{r}_{\mathrm{ki}}\·a_{2\mathrm{ki}}\\ .\\ .\\ .\\ .\\ .\\ .\\ \mathrm{\Σ\Σ\Δ}{\hat{r}}_{\mathrm{ki}}\·a_{8\mathrm{ki}}=\mathrm{\Σ\Σ\Δ}{r}_{\mathrm{ki}}\·a_{8\mathrm{ki}}\end{array}\right.---\left(8\right)$

将t_N＝N·Δt时的估计参数向量表示为

式(8)可写成矩阵形式：

A_NX_PN＝B_N    (9)

其中A_N为8×8的系数矩阵，

B_N为8×1的测量向量， ${\left(B_N\right)}_m=\underset{k=1}{\overset{2}{\mathrm{\Σ}}}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{a}_{\mathrm{mki}}\·\mathrm{\Δ}{r}_{\mathrm{ki}}.$

解线性方程组(9)即可得到t_N＝N·Δt时的估计参数向量。

4、输出校正

用估计出的参数对惯导系统的输出进行校准，提高惯导系统的导航精度。

根据以上步骤对双伪卫星校准惯导系统位置误差算法的效果进行了计算机仿真。

仿真中，假设两颗伪卫星的位置分别为：(X_k，Y_k)＝(0，0；30000，-30000)(k＝1，2)(单位：米)。

假定运动体沿x轴匀速向东运动，x、y方向的速度v_x＝60m/s，v_y＝0m/s。仿真从位置(-30000，0)开始，运动体的真实轨迹为：

$\left\{\begin{array}{c}{x}_i=v_x{t}_i\\ y_i=v_y{t}_i\end{array}\right.$

其中，t_i＝i.Δt，Δt＝1s。

待求变量分别设为：

ΔT₀＝1ms，Δτ＝0.5s，a_x＝a_y＝500m，b_x＝-b_y＝100m，c_x＝c_y＝1.5m/s。

图2和图3给出了仿真结果，可以看出，惯导系统的位置误差均可估计到米级，惯导/伪卫星不同步时间估计精度为0.1秒，接收机钟差的估计精度达10^-7秒，输出校正可明显提高惯导系统精度。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

最后所应说明的是：以上实施实例仅用以说明而非限制本发明的技术方案，所有的不脱离本发明的精神和范围的修改或局部替换，均应涵盖在本发明的权利要求范围当中。

Claims (1)

1.双伪卫星校准惯性导航系统位置误差方法，其特征在于步骤如下：

(1)将惯导系统位置误差Δx，Δy、惯导系统与伪卫星之间不同步时间Δτ及伪卫星接收机钟差ΔT₀作为待求变量；

(2)当载体开始接收到两颗伪卫星信号的时刻，根据惯导系统输出计算得到的惯导-伪卫星距离，简称计算伪距，将计算伪距与伪卫星接收机输出的测量伪距做差，求出伪卫星接收机钟差，作为伪卫星接收机钟差的初始估值；

(3)建立伪卫星接收机测量伪距与计算伪距之差Δr，与所述待求变量之间的数学模型：Δr＝r_m-r_I＝(v_xcosγ+v_ysinγ)Δτ+cΔT₀+Δxcosγ+Δysinγ；其中r_m是伪卫星接收机得到的测量伪距，r_I是根据惯导输出的位置得到的计算伪距，v_x，v_y是载体在x、y方向的运动速度，

(X，Y)是伪卫星的位置，(x_I，y_I)是惯导输出的位置；

(4)由补偿了伪卫星接收机钟差初始估值的伪卫星接收机输出的测量伪距和计算伪距得到二者之差，根据上一步得到的数学模型利用最小二乘法估计所述待求变量；

(5)利用步骤(4)估计得到的状态向量，对惯导系统的位置信息进行校准后输出。

Images