CN102997916B - 一种自主提高定位定向系统惯性姿态解算精度的方法 - Google Patents

Abstract

本发明属于姿态解算技术领域，涉及一种自主提高定位定向系统惯性姿态解算精度的方法。本发明的方法具体包括以下步骤：步骤1.建立导航坐标系旋转误差补偿模型；步骤2.采用速率计信息补偿导航坐标系旋转误差；步骤3.利用车载条件下运动特性补偿导航坐标系旋转误差。本发明解决了现有惯性导航系统姿态解算中存在姿态更新误差，给系统精度带来不利影响的技术问题。本发明的方法通过定位定向系统可提供的辅助信息获得更高精度的速度和位置信息，并将该信息引入姿态解算，提高了姿态精度。

Description

一种自主提高定位定向系统惯性姿态解算精度的方法

技术领域

本发明属于姿态解算技术领域，涉及一种自主提高定位定向系统惯性姿态解算精度的方法。

背景技术

现有惯性导航系统姿态解算中通常采用惯性速度和位置计算导航坐标系的转动，实现导航坐标系对地理坐标系的跟踪及姿态角的计算。由于惯性速度和位置存在随时间积累的误差，由此带来姿态更新误差，给系统精度带来不利影响。

发明内容

本发明需要解决的技术问题是现有惯性导航系统姿态解算中存在姿态更新误差问题，给系统精度带来不利影响。

本发明的技术方案如下所述：

该方法具体包括以下步骤：步骤1.建立导航坐标系旋转误差补偿模型；步骤2.采用速率计信息补偿导航坐标系旋转误差；步骤3.利用车载条件下运动特性补偿导航坐标系旋转误差。

本方法在速率计可用时采用航位推算，通过姿态矩阵分解速率计测得的载体里程增量获得导航坐标系的航位推算速度和位置，根据航位推算速度计算载体运动引起的导航坐标系旋转误差，根据航位推算位置计算地球自转角速率在导航坐标系中的投影误差；在速率计不可用时，采用车载条件运动特性计算导航坐标系旋转的误差；通过姿态更新算法进行导航坐标系旋转误差的补偿，实现导航坐标系对地理坐标系的跟踪。

步骤1.建立导航坐标系旋转误差补偿模型具体包括以下步骤：

设导航坐标系的旋转角速度为：

${\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n---\left(1\right)$

为跟踪地球旋转的角速度，为由于载体运动引起的相对地球的旋转角速度；

其中：

V_E——惯导东向速度，单位：米/秒；

V_N——惯导北向速度，单位：米/秒；

R——地球半径，单位：米；

ω_ie——地球自转角速度，单位：弧度/秒；

——当地纬度，单位：弧度；

导航坐标系跟踪载体运动引起的旋转误差为：

其中：

ΔV_E——惯导东向速度误差，单位：米/秒；

ΔV_N——惯导北向速度误差，单位：米/秒；

由于存在惯导纬度误差导航坐标系跟踪地球自转的旋转误差为：

综合式(4)和式(5)，导航坐标系旋转误差补偿模型为：

${\mathrm{\Δ\ω}}_{\mathrm{in}}^n={\mathrm{\Δ\ω}}_{\mathrm{ie}}^n+\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{en}}^n---\left(6\right).$

步骤2.采用速率计信息补偿导航坐标系旋转误差具体通过以下步骤实现：

在某个采样周期内，速率计提供的载体速度为V_s，利用惯导系统提供的姿态角将其投影为载体北向速度和载体东向速度

$V_N^{\mathrm{DR}}=V_s\cos {\mathrm{\θ}}_n\cos {\mathrm{\ψ}}_n---\left(7a\right)$

$V_E^{\mathrm{DR}}=-V_s\cos {\mathrm{\θ}}_n\sin {\mathrm{\ψ}}_n---\left(7b\right)$

其中：

θ_n——惯导输出的俯仰角，单位：弧度；

ψ_n——惯导输出的航向角，单位：弧度；

在第n个采样周期Δt上，利用惯导系统的速度和速率计提供的载体速度对惯导系统的速度误差进行估计：

$\mathrm{\Δ}{\hat{V}}_N=V_N-V_N^{\mathrm{DR}}=V_N-V_s\cos {\mathrm{\θ}}_n{\cos \mathrm{\ψ}}_n---\left(8a\right)$

${\mathrm{\Δ}\hat{V}}_E=V_E-V_E^{\mathrm{DR}}=V_E+V_s\cos {\mathrm{\θ}}_n\sin {\mathrm{\ψ}}_n---\left(8b\right)$

指北方位定位定向系统的航位推算模型：

其中：

表示航位推算实时纬度，单位：弧度；

表示起点纬度，单位：弧度；

利用航位推算提供的纬度计算惯导的纬度误差：

利用式(8)计算出惯导速度误差，利用式(10)计算出惯导纬度误差后，可分别利用式(4)、式(5)、式(6)计算导航坐标系跟踪载体运动引起的旋转误差，在惯性导航姿态解算中扣除导航坐标系旋转误差。

步骤3.利用车载条件下运动特性补偿导航坐标系旋转误差具体通过以下步骤实现：

当定位定向系统的速率计不可用时，用车载条件下侧向速度为零的特性计算惯导速度和位置误差，并采用计算的速度误差、位置误差补偿导航坐标系跟踪地理坐标系的旋转误差；在短时间0～N_t内，N_t优选值为10min，惯导速度误差采用以下公式描述：

ΔV_N＝ΔV_N0+α_Nn_t                                (11a)

ΔV_E＝ΔV_E0+α_En_t                                (11b)

其中：

ΔV_N0、ΔV_E0分别为计算起点的北速误差和东速误差，单位，米/秒；

α_N、α_E分别为北速误差和东速误差的与时间的一次项系数，单位，米/秒²；

n_t为距计算起点的时间，单位：秒；

采用惯导速度计算的载体前向速度V_X、侧向速度V_Z分别为：

V_X＝V_Ncosψ-V_Esinψ                              (12a)

V_Z＝V_Nsinψ+V_Ecosψ                              (12b)

其中ψ为航向角；

(a)车辆行驶状态情况

在车辆行驶状态，车辆只有前向速度，侧向速度为0，得到式(13)：

ΔV_Nsinψ+ΔV_Ecosψ＝V_Nsinψ+V_Ecosψ             (13)

式(11)代入式(13)可得：

ΔV_N0sinψ+α_Nn_tsinψ+ΔV_E0cosψ+α_En_tcosψ＝V_Nsinψ+V_Ecosψ  (14)

在0～N_t时间内采用式(13)构成下列方程组：

AX＝Y                                            (15)

式中： $X=\left[\begin{array}{cc}{\mathrm{\ΔV}}_{N0}& {\mathrm{\α}}_N\\ {\mathrm{\ΔV}}_{E0}& {\mathrm{\α}}_N\end{array}\right],$ A采用式(14)等式左侧公式计算，Y采用式(14)右侧公式计算；

当A^TA满秩时，采用最小二乘算法计算出X：

X＝(A^TA)^-1A^TY                                    (16)

当A^TA不满秩时，采用上一次A^TA满秩时计算的X；

计算出X后，则采用下式计算惯导速度误差：

ΔV_N＝ΔV_N0+α_NN_t                            (17a)

ΔV_E＝ΔV_E0+α_EN_t                            (17b)

对计算的北速误差进行积分获得惯导纬度误差；

(b)车辆静止状态情况

车辆停止状态，载体前向速度也为0，则下式成立为：

ΔV_Ncosψ-ΔV_Esinψ＝V_Ncosψ-V_Esinψ         (18)

联立式(11)、(13)、(18)采用最小二乘算法计算出X，计算出X后，采用式(17)计算惯导速度误差，对计算的北速误差进行积分获得惯导纬度误差；

得到惯导速度误差和纬度误差后，采用式(6)获得导航坐标系旋转误差，反馈到惯性导航姿态解算中，实时进行姿态修正，抑制姿态角的漂移。

本发明的有益效果为：

本发明的自主提高定位定向系统惯性姿态解算精度的方法通过定位定向系统可提供的辅助信息获得更高精度的速度和位置信息，并将该信息引入姿态解算，提高了姿态精度。

具体实施方式

下面实施例对本发明的自主提高定位定向系统惯性姿态解算精度的方法进行详细说明。

本发明的自主提高定位定向系统惯性姿态解算精度的方法，在速率计可用时采用航位推算，即通过姿态矩阵分解速率计测得的载体里程增量获得导航坐标系的航位推算速度和位置，根据航位推算速度计算载体运动引起的导航坐标系旋转误差，根据航位推算位置计算地球自转角速率在导航坐标系中的投影误差；在速率计不可用时，采用车载条件运动特性计算导航坐标系旋转的误差。通过姿态更新算法进行导航坐标系旋转误差的补偿，实现导航坐标系对地理坐标系的跟踪，提高姿态角的计算精度。本发明的方法具体包括以下步骤：

(1)建立导航坐标系旋转误差补偿模型

为跟踪当地地理坐标系，导航坐标系的旋转角速度为：

${\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n---\left(1\right)$

由上式可知，导航坐标系的旋转由跟踪地球旋转的角速度和由于载体运动引起的相对地球的旋转角速度两部分组成。

其中：

V_E——惯导东向速度，单位：米/秒；

V_N——惯导北向速度，单位：米/秒；

R——地球半径，单位：米；

ω_ie——地球自转角速度，单位：弧度/秒；

——当地纬度，单位：弧度。

由于惯导存在速度误差，导航坐标系跟踪载体运动引起的旋转误差为：

其中：

ΔV_E——惯导东向速度误差，单位：米/秒；

ΔV_N——惯导北向速度误差，单位：米/秒；

由于存在惯导纬度误差导航坐标系跟踪地球自转的旋转误差为：

综合式(4)和式(5)，导航坐标系旋转误差补偿公式为：

${\mathrm{\Δ\ω}}_{\mathrm{in}}^n={\mathrm{\Δ\ω}}_{\mathrm{ie}}^n+\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{en}}^n---\left(6\right).$

(2)采用速率计信息补偿导航坐标系旋转误差

跟踪载体运动引起的旋转误差与惯导系统的速度误差相关。利用速率计提供的速度对惯导的北向、东向速度误差进行估计和补偿。

在某个采样周期内，速率计提供的载体速度为V_s，可利用惯导系统提供的姿态角将其投影为载体北向速度和载体东向速度

$V_N^{\mathrm{DR}}=V_s\cos {\mathrm{\θ}}_n\cos {\mathrm{\ψ}}_n---\left(7a\right)$

$V_E^{\mathrm{DR}}=-V_s\cos {\mathrm{\θ}}_n\sin {\mathrm{\ψ}}_n---\left(7b\right)$

其中：

θ_n——惯导输出的俯仰角，单位：弧度；

ψ_n——惯导输出的航向角，单位：弧度。

在第n个采样周期Δt上，利用惯导系统的速度和速率计提供的载体速度对惯导系统的速度误差进行估计：

$V_N^{\mathrm{DR}}=V_s\cos {\mathrm{\θ}}_n\cos {\mathrm{\ψ}}_n---\left(7a\right)$

${\mathrm{\Δ}\hat{V}}_E=V_E-V_E^{\mathrm{DR}}=V_E+V_s\cos {\mathrm{\θ}}_n\sin {\mathrm{\ψ}}_n---\left(8b\right)$

指北方位定位定向系统的航位推算模型：

其中：

表示航位推算实时纬度，单位：弧度；

表示起点纬度，单位：弧度。

利用航位推算提供的纬度计算惯导的纬度误差：

利用式(8)计算出惯导速度误差，利用式(10)计算出惯导纬度误差后，可分别利用式(4)、式(5)、式(6)计算导航坐标系跟踪载体运动引起的旋转误差，在惯性导航姿态解算中扣除导航坐标系旋转误差，实现姿态角的实时修正，抑制姿态角的漂移，提高惯性姿态解算精度。

(3)利用车载条件下运动特性补偿导航坐标系旋转误差

当定位定向系统的速率计不可用时，可用车载条件下侧向速度为零的特性计算惯导速度和位置误差，并采用计算的速度误差、位置误差补偿导航坐标系跟踪地理坐标系的旋转误差。在短时间0～N_t内，惯导速度误差采用以下公式描述：

ΔV_N＝ΔV_N0+α_Nn_t                              (11a)

ΔV_E＝ΔV_E0+α_En_t                              (11b)

其中：

ΔV_N0、ΔV_E0分别为计算起点的北速误差和东速误差，单位，米/秒；

α_N、α_E分别为北速误差和东速误差的与时间的一次项系数，单位，米/秒²；

n_t为距计算起点的时间，单位：秒。

本实施例中N_t为10min。

采用惯导速度计算的载体前向速度V_X、侧向速度V_Z分别为：

V_X＝V_Ncosψ-V_Esinψ                            (12a)

V_Z＝V_Nsinψ+V_Ecosψ                            (12b)

ψ为航向角。

(a)车辆行驶状态

在车辆行驶状态，车辆只有前向速度，侧向速度为零，由惯导速度计算的侧向速度由惯导速度误差引起，因此下式成立：

ΔV_Nsinψ+ΔV_Ecosψ＝V_Nsinψ+V_Ecosψ           (13)

式(11)代入式(13)可得：

ΔV_N0sinψ+α_Nn_tsinψ+ΔV_E0cosψ+α_En_tcosψ＝V_Nsinψ+V_Ecosψ  (14)

在0～N_t时间内采用式(13)构成下列方程组：

AX＝Y                                          (15)

式中： $X=\left[\begin{array}{cc}{\mathrm{\ΔV}}_{N0}& {\mathrm{\α}}_N\\ {\mathrm{\ΔV}}_{E0}& {\mathrm{\α}}_N\end{array}\right],$ A采用式(14)等式左侧公式计算，Y采用式(14)右侧公式计算。

当A^TA满秩时，采用最小二乘算法可计算出X：

X＝(A^TA)^-1A^TY                                  (16)

当A^TA不满秩时，采用上一次A^TA满秩时计算的X。

计算出X后，则可采用下式计算惯导速度误差：

ΔV_N＝ΔV_N0+α_NN_t                              (17a)

ΔV_E＝ΔV_E0+α_EN_t                              (17b)

对计算的北速误差进行积分可获得惯导纬度误差。

(b)车辆静止状态

车辆停止状态，载体前向速度也为0，则下式成立为：

ΔV_Ncosψ-ΔV_Esinψ＝V_Ncosψ-V_Esinψ           (18)

联立式(11)、(13)、(18)采用最小二乘算法可计算出X，计算出X后，采用式(17)计算惯导速度误差，对计算的北速误差进行积分可获得惯导纬度误差。

计算出惯导速度误差和纬度误差后，则可采用式(6)计算导航坐标系旋转误差，反馈到惯性导航姿态解算中，实时进行姿态修正，抑制姿态角的漂移，提高惯性姿态解算精度。

实施例1

设某个1秒采样周期内，速率计提供的载体速度V_s＝20.36米/秒，计算的航位推算纬度惯导系统给出载体俯仰角为0.03176弧度，航向角1.28386弧度，惯导北向速度V_N＝2.40米/秒，惯导东向速度V_E＝-18.26米/秒，惯导纬度地球半径R取6378137米，地球自转角速度ω_ie＝7.2921311×10^-5弧度/秒。

速率计提供的速度计算北向速度和东向速度为：

利用惯导系统和速率计提供的载体速度对惯导系统的速度误差进行计算：

采用航位推算纬度对惯导系统纬度的误差进行计算：

导航坐标系旋转误差补偿量为：

实施例2

在行车过程速率计不可用的情况下某6分钟时间段内，惯导输出的信息如下表所示：

采用公式(14)构成的方程组满秩，能够计算n_t＝0时刻的惯导速度误差及速度误差变化率：

ΔV_N0＝2.7076米/秒；

ΔV_E0＝-0.1211米/秒；

α_N＝-0.00721米/秒²；

α_E＝0.00295米/秒²。

以上计算结果利用公式(11)计算得到n_t＝360秒时速度误差，为：

ΔV_N＝0.1128米/秒；

ΔV_E＝0.9425米/秒。.

使用n_t在0～360秒计算的北速误差进行积分可获得n_t＝360秒时惯导纬度误差7.95962E-05，并利用当时惯导纬度

使用n_t＝360秒导航坐标系旋转误差补偿量为：

Claims (4)

1.一种自主提高定位定向系统惯性姿态解算精度的方法，其特征在于：该方法具体包括以下步骤：步骤1.建立导航坐标系旋转误差补偿模型；步骤2.采用速率计信息补偿导航坐标系旋转误差；步骤3.利用车载条件下运动特性补偿导航坐标系旋转误差；

本方法在速率计可用时采用航位推算，通过姿态矩阵分解速率计测得的载体里程增量获得导航坐标系的航位推算速度和位置，根据航位推算速度计算载体运动引起的导航坐标系旋转误差，根据航位推算位置计算地球自转角速率在导航坐标系中的投影误差；在速率计不可用时，采用车载条件运动特性计算导航坐标系旋转的误差；通过姿态更新算法进行导航坐标系旋转误差的补偿，实现导航坐标系对地理坐标系的跟踪；

步骤1.建立导航坐标系旋转误差补偿模型具体包括以下步骤：

设导航坐标系的旋转角速度为：

${\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n---\left(1\right)$

为跟踪地球旋转的角速度，为由于载体运动引起的相对地球的旋转角速度；

其中：

V_E——惯导东向速度，单位：米/秒；

V_N——惯导北向速度，单位：米/秒；

R——地球半径，单位：米；

ω_ie——地球自转角速度，单位：弧度/秒；

——当地纬度，单位：弧度；

导航坐标系跟踪载体运动引起的旋转误差为：

其中：

△V_E——惯导东向速度误差，单位：米/秒；

△V_N——惯导北向速度误差，单位：米/秒；

由于存在惯导纬度误差导航坐标系跟踪地球自转的旋转误差为：

综合式(4)和式(5)，导航坐标系旋转误差补偿模型为：

${\mathrm{\Δ\ω}}_{\mathrm{in}}^n={\mathrm{\Δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\Δ\ω}}_{\mathrm{en}}^n---\left(6\right).$

2.根据权利要求1所述的自主提高定位定向系统惯性姿态解算精度的方法，其特征在于：步骤2.采用速率计信息补偿导航坐标系旋转误差具体通过以下步骤实现：

在某个采样周期内，速率计提供的载体速度为V_s，利用惯导系统提供的姿态角将其投影为载体北向速度和载体东向速度

$V_N^{\mathrm{DR}}=V_s\cos {\mathrm{\θ}}_n\cos {\mathrm{\ψ}}_n---\left(7a\right)$

$V_E^{\mathrm{DR}}=-V_s\cos {\mathrm{\θ}}_n\sin {\mathrm{\ψ}}_n---\left(7b\right)$

其中：

θ_n——惯导输出的俯仰角，单位：弧度；

ψ_n——惯导输出的航向角，单位：弧度；

在第n个采样周期△t上，利用惯导系统的速度和速率计提供的载体速度对惯导系统的速度误差进行估计：

${\mathrm{\Δ}\hat{V}}_N=V_N-V_N^{\mathrm{DR}}=V_N-V_s\cos {\mathrm{\θ}}_n\cos {\mathrm{\ψ}}_n---\left(8a\right)$

${\mathrm{\Δ}\hat{V}}_E=V_E-V_E^{\mathrm{DR}}=V_E+V_s\cos {\mathrm{\θ}}_n\sin {\mathrm{\ψ}}_n---\left(8b\right)$

指北方位定位定向系统的航位推算模型：

其中：

表示航位推算实时纬度，单位：弧度；

表示起点纬度，单位：弧度；

利用航位推算提供的纬度计算惯导的纬度误差：

利用式(8)计算出惯导速度误差，利用式(10)计算出惯导纬度误差后，可分别利用式(4)、式(5)、式(6)计算导航坐标系跟踪载体运动引起的旋转误差，在惯性导航姿态解算中扣除导航坐标系旋转误差。

3.根据权利要求1所述的自主提高定位定向系统惯性姿态解算精度的方法，其特征在于：步骤3.利用车载条件下运动特性补偿导航坐标系旋转误差具体通过以下步骤实现：

当定位定向系统的速率计不可用时，用车载条件下侧向速度为零的特性计算惯导速度和位置误差，并采用计算的速度误差、位置误差补偿导航坐标系跟踪地理坐标系的旋转误差；在短时间0～N_t内，惯导速度误差采用以下公式描述：

△V_N＝△V_N0+α_Nn_t  (11a)

△V_E＝△V_E0+α_En_t  (11b)

其中：

△V_N0、△V_E0分别为计算起点的北速误差和东速误差，单位，米/秒；

α_N、α_E分别为北速误差和东速误差的与时间的一次项系数，单位，米/秒²；n_t为距计算起点的时间，单位：秒；

采用惯导速度计算的载体前向速度V_X、侧向速度V_Z分别为：

V_X＝V_Ncosψ-V_Esinψ  (12a)

V_Z＝V_Nsinψ+V_Ecosψ  (12b)

其中ψ为航向角；

(a)车辆行驶状态情况

在车辆行驶状态，车辆只有前向速度，侧向速度为0，得到式(13)：

△V_Nsinψ+△V_Ecosψ＝V_Nsinψ+V_Ecosψ  (13)

式(11)代入式(13)可得：

△V_N0sinψ+α_Nn_tsinψ+△V_E0cosψ+α_En_tcosψ＝V_Nsinψ+V_Eco  (14)

在0～N_t时间内采用式(13)构成下列方程组：

AX＝Y  (15)

式中： $X=\left[\begin{array}{cc}{\mathrm{\ΔV}}_{N0}& {\mathrm{\α}}_N\\ {\mathrm{\ΔV}}_{E0}& {\mathrm{\α}}_N\end{array}\right],$ A采用式(14)等式左侧公式计算，Y采用式(14)右侧公式计算；

当A^TA满秩时，采用最小二乘算法计算出X：

X＝(A^TA)^-1A^TY  (16)

当A^TA不满秩时，采用上一次A^TA满秩时计算的X；

计算出X后，则采用下式计算惯导速度误差：

△V_N＝△V_N0+α_NN_t  (17a)

△V_E＝△V_E0+α_EN_t  (17b)

对计算的北速误差进行积分获得惯导纬度误差；

(b)车辆静止状态情况

车辆停止状态，载体前向速度也为0，则下式成立为：

△V_Ncosψ-△V_Esinψ＝V_Ncosψ-V_Esinψ  (18)

联立式(11)、(13)、(18)采用最小二乘算法计算出X，计算出X后，采用式(17)计算惯导速度误差，对计算的北速误差进行积分获得惯导纬度误差；

得到惯导速度误差和纬度误差后，采用式(6)获得导航坐标系旋转误差，反馈到惯性导航姿态解算中，实时进行姿态修正，抑制姿态角的漂移。

4.根据权利要求3所述的自主提高定位定向系统惯性姿态解算精度的方法，其特征在于：N_t为10min。