CN101694389A - 无陀螺捷联惯导***的初始姿态快速测量方法 - Google Patents

Abstract

本发明提供的是一种无陀螺捷联惯导***初始姿态快速测量方法。将低成本无人飞行器上的无陀螺捷联惯性导航***作为子惯导***，低成本无人飞行器装载机构上的捷联惯性导航***作为主惯导***。利用主惯导输出的速度参考信息进行滤波，估测出主惯导***与子惯导***之间的水平方向安装偏差，再利用无陀螺捷联惯性导航***中的加速度计输出信息对主惯导***与子惯导***之间的方位安装偏差进行匹配测量。本发明能快速测量低成本无人飞行器的初始姿态，从而提高低成本无人飞行器的快速反应能力，具有实用价值，本发明适用于低成本无人飞行器配备的中高精度无陀螺捷联惯导***。

Description

无陀螺捷联惯导***的初始姿态快速测量方法

(一)技术领域

本发明提供的是一种无陀螺捷联惯导***的初始姿态快速测量方法。

(二)背景技术

捷联惯导***一般使用加速度计敏感线加速度，陀螺仪敏感载体的角速度。但机械陀螺存在抗冲击能力差的弱点，无法应用于低成本无人飞行器这种有大过载、大角加速度的载体。光学陀螺(光纤陀螺和激光陀螺)具有较大的动态范围，但光学陀螺捷联惯导***的装备会提高低成本无人飞行器的制造成本，并降低了捷联惯导***的可靠性。无陀螺捷联惯导***使用加速度计代替陀螺仪，从加速度计测量的比力中解算出载体的角速度，进而只用加速度计来完成载体导航参数的测量。

无陀螺捷联惯导***的特点包括：

1、成本低：由于不使用结构复杂、加工难度大、维护困难的陀螺，所以无陀螺捷联惯导***成本较低。

2、能耗小：加速度计同陀螺相比，没有高速旋转体，所以功耗小，且不需要复杂的电源。

3、动态范围大：对于有陀螺捷联惯导***而言，陀螺直接安装在载体上，高机动状态时，要求陀螺有很大的动态测量范围，这在工程上实现起来比较困难。而加速度计的测量范围大，能够满足大动态范围的要求。

4、反应快：无论是机械陀螺，还是光学陀螺从启动到正常工作都需要较长起动时间，故反应较慢。

5、可靠性高：陀螺的组成零件较多且结构复杂，加速度计组成零件少且结构简单。

由无陀螺捷联惯导***的特点可以看出：无陀螺捷联惯导***避开了机械陀螺捷联惯导***因机械陀螺的动态范围限制所引起难题。同时，加速度计比较光学陀螺而言价格要低廉的多，高精度的加速度计大概两万元左右，因此使用加速度计的无陀螺捷联惯导***成本比较光学陀螺捷联惯导***低很多，使得配备无陀螺捷联惯导***的低成本无人飞行器适合于大批量生产。

无陀螺捷联惯导***在进入导航工作状态以前需要确定初始姿态参数。初始姿态参数的测量精度直接关系到无陀螺捷联惯导***导航的精度，初始姿态参数的测量过程所需要的时间长短决定了无陀螺捷联惯导***导航的反应时间。

实际工程中要求低成本无人飞行器具有快速反应能力和精确制导以保证其在良好的性能。低成本无人飞行器的反应时间主要取决于无陀螺捷联惯导***初始姿态参数测量所用时间。同时，低成本无人飞行器的精确制导能力与无陀螺捷联惯导***初始姿态参数的测量精度密切相关。因此，利用低成本无人飞行器的装载机构上惯导输出的导航信息与加速度计输出信息快速、准确地测量无陀螺捷联惯导***的初始姿态，对于提高无陀螺捷联惯导***的性能，从而提高低成本无人飞行器的快速反应能力具有实用价值。

(三)发明内容

本发明的目的在于提供一种能快速测量低成本无人飞行器的初始姿态，提高低成本无人飞行器的快速反应能力，具有实用价值的无陀螺捷联惯导***的初始姿态快速测量方法。

本发明的目的是这样实现的：

将低成本无人飞行器上的无陀螺捷联惯性导航***作为子惯导***，低成本无人飞行器装载机构上的捷联惯性导航***作为主惯导***，按照下列步骤测量初始姿态：

步骤1、将低成本无人飞行器上的无陀螺捷联惯性导航***与高精度的低成本无人飞行器装载机构上的捷联惯性导航***通过数据传输电缆相联通；

步骤2、无陀螺捷联惯性导航***进行预热后，采集加速度计输出的数据；

步骤3、利用主惯导***将初始速度参数和包括初始的经度、纬度的初始位置参数装订至无陀螺捷联惯性导航***导航计算机中；

步骤4、将主惯导***输出的包括纵摇角、横摇角和航向角的姿态测量信息通过数据传输电缆传输至无陀螺捷联惯性导航***，粗略测量出低成本无人飞行器的初始姿态，完成初始姿态的一次快速传递；

步骤5、无陀螺捷联惯性导航***由步骤3和步骤4得到的姿态初始值，利用无陀螺捷联惯性导航***上加速度计输出测量低成本无人飞行器的比力信息，再由比力解算出低成本无人飞行器的角速度信息；

步骤6、在子惯导***中，利用低成本无人飞行器的比力信息与角速度信息进行导航递推解算，测量出低成本无人飞行器的速度值；

步骤7、将主惯导***测量的速度与子惯导***测量的速度作差，并将差值作为观测向量，采用卡尔曼滤波技术估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y；估测时间为120秒，其中前80秒为滤波估测稳定时间，由80秒到120秒应用平均滤波技术对于数据进行平滑处理；

${\mathrm{\φ}}_x=\frac{{\widetilde{\mathrm{\φ}}}_x\left(1\right)......+{\widetilde{\mathrm{\φ}}}_x\left(k\right)......+{\widetilde{\mathrm{\φ}}}_x\left(N\right)}{N}$

${\mathrm{\φ}}_y=\frac{{\widetilde{\mathrm{\φ}}}_y\left(1\right)......+{\widetilde{\mathrm{\φ}}}_y\left(k\right)......+{\widetilde{\mathrm{\φ}}}_y\left(N\right)}{N}$

其中

表示由80秒到120秒数据平滑过程中，第k个滤波估测时间点的滤波估测值；在80秒到120秒数据平滑过程中，共有N个滤波估测时间点，即k＝1～N；

步骤8、利用步骤7估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y，以及子惯导***测量比力值

主惯导上***测量比力值

测量出主惯导***与子惯导***之间的方位安装偏差φ_z；

${\mathrm{\φ}}_z=\arcsin \left(\frac{k_1{k}_4+k_3{k}_2}{{k_1}^2+{k_2}^2}\right)$

其中参数k₁，k₂，k₃，k₄为

$k_1=f_{\mathrm{ibx}}^m$

$k_2=f_{\mathrm{iby}}^m$

$k_3=\frac{f_{\mathrm{ibx}}^s+\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\φ}}_y}-\frac{\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x{f}_{\mathrm{iby}}^s}{\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y}+\frac{\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y};$

$k_4=\frac{\sin {\mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m-f_{\mathrm{iby}}^s}{\cos {\mathrm{\φ}}_x}$

步骤9、利用步骤7估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y，以及步骤8测量出主惯导***与子惯导***之间的方位安装偏差φ_z构造出安装偏差方向余弦矩阵C_s ^m；

$C_s^m=\left(\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right);$

步骤10、利用步骤8构造的主惯导***与子惯导***之间的安装偏差矩阵C_s ^m，以及主惯导输出的方向余弦矩阵C_m ⁿ构造出低成本无人飞行器的载体坐标系与导航坐标系之间的方向余弦矩阵C_s ⁿ；

$C_s^n=C_m^n{C}_s^m;$

步骤11、由低成本无人飞行器的载体坐标系与导航坐标系之间的方向余弦矩阵C_s ⁿ测量出无人飞行器准确的初始姿态角。

本发明还可以包括如下特征：

1、步骤7中所用到的***状态方程和量测方程如下：

$\left\{\begin{array}{c}\stackrel{.}{X}=\mathrm{AX}\\ Z=\mathrm{HX}\end{array}\right.$

选取***的状态变量为

X＝[δv_x  δv_y  φ_mx  φ_my  φ_mz  φ_x  φ_y  φ_z]^T

其中：

δv为主惯导***与子惯导***之间的速度差值；

φ为主惯导***与子惯导***之间真实的安装偏差角；

φ_m为利用主、子惯导***输出，实际测量得到的主、子惯导***之间的安装偏差角；

下角标x、y、z表示投影所在坐标系三个轴，此处分别为导航坐标系n系的东向轴、北向轴和天向轴；

***的状态矩阵为

$A=\left[\begin{array}{ccc}{A}_1& A_2& -A_2\\ 0_{3\×2}& A_3& -A_3\\ 0_{3\×2}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

其中：

$A_1=\left[\begin{array}{cc}0& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e}\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e})& 0\end{array}\right]$

其中R_e为地球半径，L为当地的纬度，ω_ie为地球自转角速度；v_x为主惯导***测量的速度；

$A_2=\left[\begin{array}{ccc}{f}_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\\ f_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\end{array}\right]$

其中 $f_{\mathrm{ib}}^s={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^s& f_{\mathrm{iby}}^s& f_{\mathrm{ibz}}^s\end{array}\right]}^T$ 为子惯导***测量比力值；

$A_3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ibz}}& -{\mathrm{\ω}}_{\mathrm{iby}}\\ -{\mathrm{\ω}}_{\mathrm{ibz}}& 0& {\mathrm{\ω}}_{\mathrm{ibx}}\\ {\mathrm{\ω}}_{\mathrm{iby}}& -{\mathrm{\ω}}_{\mathrm{ibx}}& 0\end{array}\right]$

其中ω_ib＝[ω_ibx  ω_iby  ω_ibz]^T为利用无陀螺捷联惯性导航***上加速度计输出比力测量信息解算出的角速度信息；

***的量测向量为

Z＝[δv_x  δv_y]^T

***的观测矩阵为

$H=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right].$

2、无陀螺捷联惯性导航***的无陀螺捷联惯性导航***的三个坐标轴与低成本无人飞行器载体坐标系的三个坐标轴相重合。

本发明提供了一种能快速测量低成本无人飞行器的初始姿态，从而提高低成本无人飞行器的快速反应能力，具有实用价值的无陀螺捷联惯导***初始姿态测量方法。

本发明将低成本无人飞行器上的无陀螺捷联惯性导航***作为子惯导***，低成本无人飞行器装载机构上的捷联惯性导航***作为主惯导***。利用主惯导输出的速度参考信息进行滤波，估测出主惯导***与子惯导***之间的水平方向安装偏差，再利用无陀螺捷联惯性导航***中的加速度计输出信息对主惯导***与子惯导***之间的方位安装偏差进行匹配测量。

本发明的技术具有以下优点：利用低成本无人飞行器的装载机构上捷联惯导输出的导航信息与加速度计输出信息，在保证对准精度的情况下，在短时间内测量出无陀螺捷联惯导***的初始姿态。保证了低成本无人飞行器的快速反应速度。

对本发明的有益效果通过如下方法得以验证：高动态条件下，低成本无人飞行器上无陀螺捷联惯导***初始姿态测量试验。在试验中低成本无人飞行器的装载机构设置为飞机。

试验条件包括：(1)低成本无人飞行器的器件精度，即加速度计的随机常值偏置为0.0001g。(2)飞机的飞行速率为150米/秒，并且绕方位轴作40秒为周期的正弦摇摆运动。(3)飞机与低成本无人飞行器载体坐标系之间的水平方向安装偏差φ_x＝φ_y＝0.2度，飞机与低成本无人飞行器载体坐标系之间的方位安装偏差φ_z＝1度。

试验结果：估测时间从开始到80秒，按照步骤7主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y的滤波估测趋于稳定。估测时间从80秒到100秒应用平均滤波技术对于数据进行平滑处理，并按照步骤8测量出主惯导***与子惯导***之间的方位安装偏差φ_z。主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y的滤波估测效果如图1、图2所示。主惯导***与子惯导***之间的方位安装偏差φ_z的滤波估测效果如图3所示。由图可知：本发明的测量值与理论设定值基本一致，同时估测速度快，可以保证低成本无人飞行器的快速反应的要求。

(四)附图说明

图1为本发明的主惯导***与子惯导***之间的水平x方向安装偏差φ_x的滤波估测曲线图。

图2为本发明的主惯导***与子惯导***之间的水平y方向安装偏差φ_y的滤波估测曲线图。

图3为本发明的主惯导***与子惯导***之间的z方向方位安装偏差φ_z的估测曲线图。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

本实施方式中，将低成本无人飞行器上的无陀螺捷联惯性导航***作为子惯导***，低成本无人飞行器装载机构上的捷联惯性导航***作为主惯导***。初始姿态快速测量的具体实施步骤如下：

步骤1、将低成本无人飞行器上的无陀螺捷联惯性导航***与高精度的低成本无人飞行器装载机构上的捷联惯性导航***通过数据传输电缆相联通，使子惯导与主惯导之间的导航信息数据传输通畅。

步骤2、无陀螺捷联惯性导航***进行预热，然后采集加速度计输出的数据。预热时间根据具体***设定。

步骤3、利用主惯导***将初始速度参数和初始位置参数(包括初始的经度、纬度)装订至无陀螺捷联惯性导航***导航计算机中。

步骤4、将主惯导***输出的姿态测量信息(包括纵摇角、横摇角和航向角)通过数据传输电缆传输至无陀螺捷联惯性导航***，粗略测量出低成本无人飞行器的初始姿态，完成初始姿态的一次快速传递。

步骤5、无陀螺捷联惯性导航***由步骤3(装订初始速度参数和初始位置参数)和步骤4(初始姿态的一次快速传递)得到的姿态初始值，利用无陀螺捷联惯性导航***上加速度计输出测量低成本无人飞行器的比力信息，再由比力解算出低成本无人飞行器的角速度信息。

步骤6、在子惯导***中，利用低成本无人飞行器的比力信息与角速度信息进行导航递推解算，测量出低成本无人飞行器的速度值；

步骤7、将主惯导***测量的速度与子惯导***测量的速度作差，并将差值作为观测向量，采用卡尔曼滤波技术估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y；估测时间为120秒，其中前80秒为滤波估测稳定时间，由80秒到120秒应用平均滤波技术对于数据进行平滑处理；

${\mathrm{\φ}}_x=\frac{{\widetilde{\mathrm{\φ}}}_x\left(1\right)......+{\widetilde{\mathrm{\φ}}}_x\left(k\right)......+{\widetilde{\mathrm{\φ}}}_x\left(N\right)}{N}$

${\mathrm{\φ}}_y=\frac{{\widetilde{\mathrm{\φ}}}_y\left(1\right)......+{\widetilde{\mathrm{\φ}}}_y\left(k\right)......+{\widetilde{\mathrm{\φ}}}_y\left(N\right)}{N}$

其中

表示由80秒到120秒数据平滑过程中，第k个滤波估测时间点的滤波估测值；在80秒到120秒数据平滑过程中，共有N个滤波估测时间点，即k＝1～N；

步骤8、利用步骤7估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y，以及子惯导***测量比力值

主惯导上***测量比力值

测量出主惯导***与子惯导***之间的方位安装偏差φ_z；

${\mathrm{\φ}}_z=\arcsin \left(\frac{k_1{k}_4+k_3{k}_2}{{k_1}^2+{k_2}^2}\right)$

其中参数k₁，k₂，k₃，k₄为

$k_1=f_{\mathrm{ibx}}^m$

$k_2=f_{\mathrm{iby}}^m$

$k_3=\frac{f_{\mathrm{ibx}}^s+\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\φ}}_y}-\frac{\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x{f}_{\mathrm{iby}}^s}{\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y}+\frac{\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m}{\cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y};$

$k_4=\frac{\sin {\mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m-f_{\mathrm{iby}}^s}{\cos {\mathrm{\φ}}_x}$

步骤9、利用步骤7估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y，以及步骤8测量出主惯导***与子惯导***之间的方位安装偏差φ_z构造出安装偏差方向余弦矩阵C_s ^m；

$C_s^m=\left(\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -{\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right);$

步骤10、利用步骤8构造的主惯导***与子惯导***之间的安装偏差矩阵C_s ^m，以及主惯导输出的方向余弦矩阵C_m ⁿ构造出低成本无人飞行器的载体坐标系与导航坐标系之间的方向余弦矩阵C_s ⁿ；

$C_s^n=C_m^n{C}_s^m;$

步骤11、由低成本无人飞行器的载体坐标系与导航坐标系之间的方向余弦矩阵C_s ⁿ测量出无人飞行器准确的初始姿态角。

步骤7中所用到的***状态方程和量测方程如下：

$\left\{\begin{array}{c}\stackrel{.}{X}=\mathrm{AX}\\ Z=\mathrm{HX}\end{array}\right.$

选取***的状态变量为

X＝[δv_x  δv_y  φ_mx  φ_my  φ_mz  φ_x  φ_y  φ_z]^T

其中：

δv为主惯导***与子惯导***之间的速度差值；

φ为主惯导***与子惯导***之间真实的安装偏差角；

φ_m为利用主、子惯导***输出，实际测量得到的主、子惯导***之间的安装偏差角；

下角标x、y、z表示投影所在坐标系三个轴，此处分别为导航坐标系n系的东向轴、北向轴和天向轴；

***的状态矩阵为

$A=\left[\begin{array}{ccc}{A}_1& A_2& -A_2\\ 0_{3\×2}& A_3& -A_3\\ 0_{3\×2}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

其中：

$A_1=\left[\begin{array}{cc}0& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e}\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e})& 0\end{array}\right]$

其中R_e为地球半径，L为当地的纬度，ω_ie为地球自转角速度；v_x为主惯导***测量的速度；

$A_2=\left[\begin{array}{ccc}{f}_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\\ f_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\end{array}\right]$

其中 $f_{\mathrm{ib}}^s={\left[\begin{array}{ccc}{f}_{\mathrm{ibx}}^s& f_{\mathrm{iby}}^s& f_{\mathrm{ibz}}^s\end{array}\right]}^T$ 为子惯导***测量比力值；

$A_3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ibz}}& -{\mathrm{\ω}}_{\mathrm{iby}}\\ -{\mathrm{\ω}}_{\mathrm{ibz}}& 0& {\mathrm{\ω}}_{\mathrm{ibx}}\\ {\mathrm{\ω}}_{\mathrm{iby}}& -{\mathrm{\ω}}_{\mathrm{ibx}}& 0\end{array}\right]$

其中ω_ib＝[ω_ibx  ω_iby  ω_ibz]^T为利用无陀螺捷联惯性导航***上加速度计输出比力测量信息解算出的角速度信息；

***的量测向量为

Z＝[δv_x  δv_y]^T

***的观测矩阵为

$H=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right].$

无陀螺捷联惯性导航***的安装应保证无陀螺捷联惯性导航***的三个坐标轴与低成本无人飞行器载体坐标系的三个坐标轴相重合(通过安装时的光学标校来保证)。因此测量无陀螺捷联惯性导航***的初始姿态就是测量低成本无人飞行器的初始姿态。

Claims (3)

1.一种无陀螺捷联惯导***的初始姿态快速测量方法，其特征是包括如下步骤：

步骤1、将低成本无人飞行器上的无陀螺捷联惯性导航***与高精度的低成本无人飞行器装载机构上的捷联惯性导航***通过数据传输电缆相联通；

步骤2、无陀螺捷联惯性导航***进行预热后，采集加速度计输出的数据；

步骤3、利用主惯导***将初始速度参数和包括初始的经度、纬度的初始位置参数装订至无陀螺捷联惯性导航***导航计算机中；

步骤4、将主惯导***输出的包括纵摇角、横摇角和航向角的姿态测量信息通过数据传输电缆传输至无陀螺捷联惯性导航***，粗略测量出低成本无人飞行器的初始姿态，完成初始姿态的一次快速传递；

步骤5、无陀螺捷联惯性导航***由步骤3和步骤4得到的姿态初始值，利用无陀螺捷联惯性导航***上加速度计输出，测量低成本无人飞行器的比力信息，再由比力解算出低成本无人飞行器的角速度信息；

步骤6、在子惯导***中，利用低成本无人飞行器的比力信息与角速度信息进行导航递推解算，测量出低成本无人飞行器的速度值；

步骤7、将主惯导***测量的速度与子惯导***测量的速度作差，并将差值作为观测向量，采用卡尔曼滤波技术估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y；估测时间为120秒，其中前80秒为滤波估测稳定时间，由80秒到120秒应用平均滤波技术对于数据进行平滑处理；

${\mathrm{\φ}}_x=\frac{{\widetilde{\mathrm{\φ}}}_x\left(1\right)......+{\widetilde{\mathrm{\φ}}}_x\left(k\right)......+{\widetilde{\mathrm{\φ}}}_x\left(N\right)}{N}$

${\mathrm{\φ}}_y=\frac{{\widetilde{\mathrm{\φ}}}_y\left(1\right)......+{\widetilde{\mathrm{\φ}}}_y\left(k\right)......+{\widetilde{\mathrm{\φ}}}_y\left(N\right)}{N}$

其中

表示由80秒到120秒数据平滑过程中，第k个滤波估测时间点的滤波估测值；在80秒到120秒数据平滑过程中，共有N个滤波估测时间点，即k＝1～N；

步骤8、利用步骤7估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y，以及子惯导***测量比力值

主惯导上***测量比力值测量出主惯导***与子惯导***之间的方位安装偏差φ_z；

${\mathrm{\φ}}_z=\arcsin \left(\frac{k_1{k}_4+k_3{k}_2}{{k_1}^2+{k_2}^2}\right)$

其中参数k₁，k₂，k₃，k₄为

$k_1=f_{\mathrm{ibx}}^m$

$k_2=f_{\mathrm{iby}}^m$

$k_3=\frac{f_{\mathrm{ibx}}^s+\sin {\mathrm{\φ}}_y{\cos \mathrm{\φ}}_y{f}_{\mathrm{ibz}}^m}{{\cos \mathrm{\φ}}_y}-\frac{{\sin \mathrm{\φ}}_y{\sin \mathrm{\φ}}_x{f}_{\mathrm{iby}}^s}{{\cos \mathrm{\φ}}_x{\cos \mathrm{\φ}}_y}+\frac{{\sin \mathrm{\φ}}_y{\sin \mathrm{\φ}}_x{\sin \mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m}{{\cos \mathrm{\φ}}_x\cos {\mathrm{\φ}}_y};$

$k_4=\frac{{\sin \mathrm{\φ}}_x{f}_{\mathrm{ibz}}^m-f_{\mathrm{iby}}^s}{{\cos \mathrm{\φ}}_x}$

步骤9、利用步骤7估测出主惯导***与子惯导***之间的水平方向安装偏差φ_x、φ_y，以及步骤8测量出主惯导***与子惯导***之间的方位安装偏差φ_z构造出安装偏差方向余弦矩阵C_s ^m；

$C_s^m=\left(\begin{array}{ccc}1& {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ {-\mathrm{\φ}}_z& 1& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y& -{\mathrm{\φ}}_x& 1\end{array}\right);$

步骤10、利用步骤8构造的主惯导***与子惯导***之间的安装偏差矩阵C_s ^m，以及主惯导输出的方向余弦矩阵C_m ⁿ构造出低成本无人飞行器的载体坐标系与导航坐标系之间的方向余弦矩阵C_s ⁿ；

$C_s^n=C_m^n{C}_s^m;$

步骤11、由低成本无人飞行器的载体坐标系与导航坐标系之间的方向余弦矩阵C_s ⁿ测量出无人飞行器的初始姿态角。

2.根据权利要求1所述的无陀螺捷联惯导***的初始姿态快速测量方法，其特征是步骤7中所用到的***状态方程和量测方程如下：

$\left\{\begin{array}{c}\stackrel{\·}{X}=\mathrm{AX}\\ Z=\mathrm{HX}\end{array}\right.$

选取***的状态变量为

X＝[δv_x δv_y φ_mx φ_my φ_mz φ_x φ_y y_z]^T

其中：

δv为主惯导***与子惯导***之间的速度差值；

φ为主惯导***与子惯导***之间真实的安装偏差角；

φ_m为利用主、子惯导***输出，实际测量得到的主、子惯导***之间的安装偏差角；

下角标x、y、z表示投影所在坐标系三个轴，此处分别为导航坐标系n系的东向轴、北向轴和天向轴；

***的状态矩阵为

$A=\left[\begin{array}{ccc}{A}_1& A_2& {-A}_2\\ 0_{3\×2}& A_3& -A_3\\ 0_{3\×2}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

其中：

$A_1=\left[\begin{array}{cc}0& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e}\\ -({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x\tan L}{R_e})& 0\end{array}\right]$

其中R_e为地球半径，L为当地的纬度，ω_ie为地球自转角速度；v_x为主惯导***测量的速度；

$A_2=\left[\begin{array}{ccc}{f}_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\\ f_{\mathrm{iby}}^s-f_{\mathrm{ibz}}^s& f_{\mathrm{ibz}}^s-f_{\mathrm{ibx}}^s& f_{\mathrm{ibx}}^s-f_{\mathrm{iby}}^s\end{array}\right]$

其中为子惯导***测量比力值；

$A_3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ibz}}& -{\mathrm{\ω}}_{\mathrm{iby}}\\ -{\mathrm{\ω}}_{\mathrm{ibz}}& 0& {\mathrm{\ω}}_{\mathrm{ibx}}\\ {\mathrm{\ω}}_{\mathrm{iby}}& -{\mathrm{\ω}}_{\mathrm{ibx}}& 0\end{array}\right]$

其中ω_ib＝[ω_ibx ω_iby ω_ibz]^T为利用无陀螺捷联惯性导航***上加速度计输出比力测量信息解算出的角速度信息；

***的量测向量为

Z＝[δv_x δv_y]^T

***的观测矩阵为

$H=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right].$

3.根据权利要求1或2所述的无陀螺捷联惯导***的初始姿态快速测量方法，其特征是：无陀螺捷联惯性导航***的三个坐标轴与无人飞行器载体坐标系的三个坐标轴相重合。

Images