CN103674059A - 一种基于外测速度信息的sins水平姿态误差修正方法 - Google Patents

Abstract

本发明公开了一种基于外测速度信息的SINS水平姿态误差修正方法。通过将速度误差信号先进行方位角解耦，再进行视加速度解耦，然后经过控制器后形成控制量对水平姿态进行修正，实现了对惯性导航系统误差的修正。相比其它组合导航方法，本发明实现了对惯导位置和水平姿态的修正，同时通过设计控制器实现了对信号噪声的滤除，不仅提高了组合导航系统的鲁棒性，而且提高了组合导航系统的导航精度。

Description

一种基于外测速度信息的SINS水平姿态误差修正方法

技术领域

本发明涉及一种误差修正方法，尤其涉及一种基于外测速度信息的SINS水平姿态误差修正方法，属于组合导航系统设计技术，可用于航空航天、测绘等具有高精度导航要求的场合。

背景技术

惯性导航系统是一个基于加速度二次积分的航程推算系统，它完全依靠机械设备和相应的算法自动、独立完成导航任务，和外界不发生任何光、电联系。由于其具有隐蔽性好、工作环境不受气象条件限制等优点，成为航天、航空、航海领域中一种广泛使用的主要导航系统。但惯性导航系统本身存在仪表误差、初始对准误差、重力异常等问题，长时间工作时导航误差随时间发散。提高惯性导航系统性能的有效途径是采用组合导航技术，即用两种或两种以上的非相似导航系统对同一导航信息做测量并解算以形成量测量，从这些量测量中计算出各导航系统的误差并校正。采用组合导航技术的系统称为组合导航系统，参与组合的各个系统称为子系统。组合导航系统设计的一个重要方面是信息融合算法，即将不同导航系统获得导航信息进行融合得到高精度的导航输出。目前常见的信息融合算法一般基于最优估计理论，例如卡尔曼滤波理论以及其衍生算法。在使用卡尔曼滤波算法进行组合导航系统设计时需要建立系统的误差模型，除此之外，卡尔曼滤波算法是一种递推算法，启动算法时需要设置参数初值，误差模型的正确性和参数初值的合理性将对算法的收敛性产生直接影响，工程应用时需要针对不同产品设置合理的参数初值，调试工作量很大。

另外，基于外测信息的位置速度修正方法，只能修正导航的位置和速度，不能修正姿态，在短时间内可行，当时间较长时，需要对水平姿态进行修正。因此，为了提高组合导航系统对参数的鲁棒稳定性和修正算法的快速性，降低系统设计难度，修正导航水平姿态，提高导航结果的精确性，需要研究一种新型组合导航系统设计方法。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供一种基于外测速度信息的SINS水平姿态误差修正方法，实现了对惯导位置和水平姿态的修正，提高了组合导航系统的精度。

本发明的技术解决方案是：一种基于外测速度信息的SINS水平姿态误差修正方法，步骤如下：

（2）对惯性导航系统测量获得的速度

与GPS获得的载体真实北向速度V作差，得到速度误差信号

（2）对步骤（1）得到的速度误差信号进行方位角解耦，解耦矩阵为A；

（3）对由步骤（2）解耦得到的信号再进行视加速度解耦，解耦矩阵为B；

（4）将由步骤（3）解耦得到的信号经过控制器C_φ，得到姿态修正信号u_φ；

（5）再将步骤（4）得到的姿态修正信号u_φ输入至用欧拉克雷洛夫角法表示的姿态微分方程中，经积分运算得到修正后的姿态角。

所述步骤（2）解耦矩阵A的形式为：

$A=\left[\begin{array}{cc}\sin \mathrm{\γ}& -\cos \mathrm{\γ}\\ \cos \mathrm{\γ}\sec {\mathrm{\φ}}_x& \sin \mathrm{\γ}\sec {\mathrm{\φ}}_x\end{array}\right]$

其中，γ为航向角，φ_x为俯仰角。

所述步骤（3）中解耦矩阵B的形式为：

$B=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{cc}-a_x\sin {\mathrm{\φ}}_y\sec {\mathrm{\φ}}_x+a_z\cos {\mathrm{\φ}}_y\sec {\mathrm{\φ}}_x& a_x\cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x+a_z\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\\ 0& -a_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x+a_y\sin {\mathrm{\φ}}_x+a_z\cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x\end{array}\right]$

其中，Δ=(a_xsinφ_ycosφ_x-a_ysinφ_x-a_zcosφ_ycosφ_x)(a_xsinφ_y-a_zcosφ_y)secφ_x，a_x为东向加速度，a_y为北向加速度，a_z为天向加速度，φ_x为俯仰角，φ_y为横滚角。

所述步骤（4）中控制器C_φ的形式为：

$C_{\mathrm{\φ}}=\left[\begin{array}{cc}{C}_x\left(s\right)& 0\\ 0& C_y\left(s\right)\end{array}\right]$

其中，控制器C_x(s)和C_y(s)的形式为

$C_i\left(s\right)=K\frac{T_{2}s+1}{T_{1}s+1},i=x,y$

其中，T₁为惯性环节时间常数，T₂为一阶微分环节时间常数，T₂≥100T₁，K为放大倍数；

所述步骤（4）中姿态修正信号u_φ的计算方法为：

μ_φ=C_φBAδV。

所述步骤（5）的实现方法为：

将姿态修正信号u_φ输入至用欧拉克雷洛夫角法表示的姿态微分方程 $\stackrel{\stackrel{\·}{\→}}{\mathrm{\φ}}=U{R}_b^w{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b-U{R}_L^w({\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ie}}^L+{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{eL}}^L)+u_{\mathrm{\φ}}$ 中；

然后，经积分得到修正后的姿态角，公式如下：

$\left[\begin{array}{c}{\mathrm{\φ}}_x^{\′}\\ {\mathrm{\φ}}_y^{\′}\\ {\mathrm{\γ}}^{\′}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ \mathrm{\γ}\end{array}\right]+\stackrel{\stackrel{\·}{\→}}{\mathrm{\φ}}*T;$

其中，

表示姿态角对时间T的导数，T是GPS采样周期， $U=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sec {\mathrm{\φ}}_x& 0\\ 0& -\tan {\mathrm{\φ}}_x& 1\end{array}\right],$

$R_b^w=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& \sin {\mathrm{\φ}}_y\\ \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right],R_L^w=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

由陀螺仪测量得出，ν_n是上一周期计算得到的载体平台的北向速度，ν_e是上一周期计算得到的载体平台的东向速度，ω_ie为地球自转速度，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，h是上一周期计算得到的载体平台的高度，

是上一周期计算得到的载体平台的纬度，φ_x是上一周期计算得到的载体平台的俯仰角，φ_y是上一周期计算得到的载体平台的横滚角，γ是上一周期计算得到的载体平台的方位角，φ_x′是这一周期计算得到的载体平台的俯仰角，φ_y′是这一周期计算得到的载体平台的横滚角，γ′是这一周期计算得到的载体平台的方位角。

本发明与现有技术相比的优点如下：

（1）现有的卡尔曼滤波算法需要建立系统的误差模型，算法稳定性严重依赖于导航误差模型的正确性和精确程度，本发明的方法不依赖于系统误差模型的精确性，通过设计合适的控制器即可完成对输入信号噪声的滤除，完成系统输出对系统输入的跟踪，即完成对惯导系统位置误差和水平姿态误差的修正。

（2）现有的卡尔曼滤波算法时间开销较大，滤波周期较长，本发明的方法运算速度快，时间开销短，当外测数据频率较高时也可完成对惯导系统位置误差和水平姿态误差的修正。

（3）与现有的基于外测信息的位置速度修正相比，本发明不仅能够修正位置、速度等，而且减小了位置速度的噪声，同时能够修正水平姿态，比现有的方法更进了一步。

附图说明

图1为本发明的水平姿态修正回路图；

图2为本发明的控制器的修正回路开环传递函数伯德图；

图3为使用本发明方法设计的组合导航系统位置导航曲线。

具体实施方式

由于惯性导航系统本身存在仪表误差、初始对准误差、重力异常等，惯性导航系统长时间工作时导航误差随时间发散。为此，需要引入外部位置信息、外部速度信息或者外部姿态信息以对惯性导航误差进行修正。

如图1所示，本发明提供了一种基于外测速度信息的SINS水平姿态误差修正方法，步骤如下：

（1）对惯性导航系统测量获得的速度

与GPS获得的载体真实北向速度V作差，得到速度误差信号

（2）对步骤（1）得到的速度误差信号进行方位角解耦，解耦矩阵为A；

$A=\left[\begin{array}{cc}\sin \mathrm{\γ}& -\cos \mathrm{\γ}\\ \cos \mathrm{\γ}\sec {\mathrm{\φ}}_x& \sin \mathrm{\γ}\sec {\mathrm{\φ}}_x\end{array}\right]$

其中，γ为航向角，φ_x为俯仰角。

（3）对由步骤（2）解耦得到的信号再进行视加速度解耦，解耦矩阵为B；

$B=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{cc}-a_x\sin {\mathrm{\φ}}_y\sec {\mathrm{\φ}}_x+a_z\cos {\mathrm{\φ}}_y\sec {\mathrm{\φ}}_x& a_x\cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x+a_z\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\\ 0& -a_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x+a_y\sin {\mathrm{\φ}}_x+a_z\cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x\end{array}\right]$

其中，Δ=(a_xsinφ_ycosφ_x-a_ysinφ_x-a_zcosφ_ycosφ_x)(a_xsinφ_y-a_zcosφ_y)secφ_x，a_x为东向加速度，a_y为北向加速度，a_z为天向加速度，φ_x为俯仰角，φ_y为横滚角。

（4）将由步骤（3）解耦得到的信号经过控制器C_φ，得到姿态修正信号u_φ；

控制器C_φ的形式为：

$C_{\mathrm{\φ}}=\left[\begin{array}{cc}{C}_x\left(s\right)& 0\\ 0& C_y\left(s\right)\end{array}\right]$

其中，控制器C_x(s)和C_y(s)的形式为

$C_i\left(s\right)=K\frac{T_{2}s+1}{T_{1}s+1},i=x,y$

其中，T₁为惯性环节时间常数，T₂为一阶微分环节时间常数，T₂≥100T₁，K为放大倍数；

所述步骤（4）中姿态修正信号u_φ的计算方法为：

μ_φ=C_φBAδV。

（5）再将步骤（4）得到的姿态修正信号u_φ输入至用欧拉克雷洛夫角法表示的姿态微分方程中，经积分运算得到修正后的姿态角。

实现方法为：

将姿态修正信号u_φ输入至用欧拉克雷洛夫角法表示的姿态微分方程 $\stackrel{\stackrel{\·}{\→}}{\mathrm{\φ}}=U{R}_b^w{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b-U{R}_L^w({\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ie}}^L+{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{eL}}^L)+u_{\mathrm{\φ}}$ 中；

然后，经积分得到修正后的姿态角，公式如下：

$\left[\begin{array}{c}{\mathrm{\φ}}_x^{\′}\\ {\mathrm{\φ}}_y^{\′}\\ {\mathrm{\γ}}^{\′}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ \mathrm{\γ}\end{array}\right]+\stackrel{\stackrel{\·}{\→}}{\mathrm{\φ}}*T;$

其中，

表示姿态角对时间T的导数，T是GPS采样周期， $U=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sec {\mathrm{\φ}}_x& 0\\ 0& -\tan {\mathrm{\φ}}_x& 1\end{array}\right],$

$R_b^w=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& \sin {\mathrm{\φ}}_y\\ \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right],R_L^w=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

由陀螺仪测量得出，ν_n是上一周期计算得到的载体平台的北向速度，ν_e是上一周期计算得到的载体平台的东向速度，ω_ie为地球自转速度，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，h是上一周期计算得到的载体平台的高度，

是上一周期计算得到的载体平台的纬度，φ_x是上一周期计算得到的载体平台的俯仰角，φ_y是上一周期计算得到的载体平台的横滚角，γ是上一周期计算得到的载体平台的方位角，φ_x′是这一周期计算得到的载体平台的俯仰角，φ_y′是这一周期计算得到的载体平台的横滚角，γ′是这一周期计算得到的载体平台的方位角。

例：有

首先，确定系统的剪切频率ω_c,则阻尼系统的开环传递函数为

图2中实线给出了T₁=0.1、T₂=10时的阻尼系统开环伯德图，因为

故K=0.1。设采样频率为5Hz，在采用双线性离散化后的系统开环伯德图如图2中虚线。

通过上述方法即可完成基于外部位置和速度信息的惯性导航系统修正方法组合导航系统设计，图1为水平姿态修正回路原理图，姿态修正控制量反馈到姿态更新回路中计算得到新的姿态信息，得到新的姿态矩阵。图3为组合导航系统的导航位置曲线，曲线1为GPS导航结果，曲线2为组合导航系统导航结果，可以看出在GPS没有丢帧的情况下组合导航系统位置导航结果和实际相符。

本发明未详细描述内容为本领域技术人员公知技术。

Claims (5)

1.一种基于外测速度信息的SINS水平姿态误差修正方法，其特征在于步骤如下：

（1）对惯性导航系统测量获得的速度

与GPS获得的载体真实北向速度V作差，得到速度误差信号

（2）对步骤（1）得到的速度误差信号进行方位角解耦，解耦矩阵为A；

（3）对由步骤（2）解耦得到的信号再进行视加速度解耦，解耦矩阵为B；

（4）将由步骤（3）解耦得到的信号经过控制器C_φ，得到姿态修正信号u_φ；

（5）再将步骤（4）得到的姿态修正信号u_φ输入至用欧拉克雷洛夫角法表示的姿态微分方程中，经积分运算得到修正后的姿态角。

2.根据权利要求1所述的一种基于外测速度信息的SINS水平姿态误差修正方法，其特征在于：所述步骤（2）解耦矩阵A的形式为：

$A=\left[\begin{array}{cc}\sin \mathrm{\γ}& -\cos \mathrm{\γ}\\ \cos \mathrm{\γ}\sec {\mathrm{\φ}}_x& \sin \mathrm{\γ}\sec {\mathrm{\φ}}_x\end{array}\right]$

其中，γ为航向角，φ_x为俯仰角。

3.根据权利要求1所述的一种基于外测速度信息的SINS水平姿态误差修正方法，其特征在于：所述步骤（3）中解耦矩阵B的形式为：

$B=\frac{1}{\mathrm{\Δ}}\left[\begin{array}{cc}-a_x\sin {\mathrm{\φ}}_y\sec {\mathrm{\φ}}_x+a_z\cos {\mathrm{\φ}}_y\sec {\mathrm{\φ}}_x& a_x\cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x+a_z\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\\ 0& -a_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x+a_y\sin {\mathrm{\φ}}_x+a_z\cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x\end{array}\right]$

其中，Δ=(a_xsinφ_ycosφ_x-a_ysinφ_x-a_zcosφ_ycosφ_x)(a_xsinφ_y-a_zcosφ_y)secφ_x，a_x为东向加速度，ay为北向加速度，az为天向加速度，φx为俯仰角，φy为横滚角。

4.根据权利要求1所述的一种基于外测速度信息的SINS水平姿态误差修正方法，其特征在于：所述步骤（4）中控制器C_φ的形式为：

$C_{\mathrm{\φ}}=\left[\begin{array}{cc}{C}_x\left(s\right)& 0\\ 0& C_y\left(s\right)\end{array}\right]$

其中，控制器C_x(s)和C_y(s)的形式为

$C_i\left(s\right)=K\frac{T_{2}s+1}{T_{1}s+1},i=x,y$

其中，T₁为惯性环节时间常数，T₂为一阶微分环节时间常数，T₂≥100T₁，K为放大倍数；

所述步骤（4）中姿态修正信号u_φ的计算方法为：

μ_φ=C_φBAδV。

5.根据权利要求1所述的一种基于外测速度信息的SINS水平姿态误差修正方法，其特征在于：所述步骤（5）的实现方法为：

将姿态修正信号u_φ输入至用欧拉克雷洛夫角法表示的姿态微分方程 $\stackrel{\stackrel{\·}{\→}}{\mathrm{\φ}}=U{R}_b^w{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b-U{R}_L^w({\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ie}}^L+{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{eL}}^L)+u_{\mathrm{\φ}}$ 中；

然后，经积分得到修正后的姿态角，公式如下：

$\left[\begin{array}{c}{\mathrm{\φ}}_x^{\′}\\ {\mathrm{\φ}}_y^{\′}\\ {\mathrm{\γ}}^{\′}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ \mathrm{\γ}\end{array}\right]+\stackrel{\stackrel{\·}{\→}}{\mathrm{\φ}}*T;$

其中，

表示姿态角对时间T的导数，T是GPS采样周期， $U=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sec {\mathrm{\φ}}_x& 0\\ 0& -\tan {\mathrm{\φ}}_x& 1\end{array}\right],$

$R_b^w=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& \sin {\mathrm{\φ}}_y\\ \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right],R_L^w=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

由陀螺仪测量得出，ν_n是上一周期计算得到的载体平台的北向速度，ν_e是上一周期计算得到的载体平台的东向速度，ω_ie为地球自转速度，R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径，h是上一周期计算得到的载体平台的高度，

是上一周期计算得到的载体平台的纬度，φ_x是上一周期计算得到的载体平台的俯仰角，φ_y是上一周期计算得到的载体平台的横滚角，γ是上一周期计算得到的载体平台的方位角，φ_x′是这一周期计算得到的载体平台的俯仰角，φ_y′是这一周期计算得到的载体平台的横滚角，γ′是这一周期计算得到的载体平台的方位角。

Images