CN102997919B - 采用隔离载体运动提升旋转式捷联惯导误差抑制效果的方法 - Google Patents

Abstract

本发明提出了一种采用隔离载体运动提升旋转式捷联惯导误差抑制效果的方法，克服了载体沿方位轴角运动对旋转式惯导系统误差补偿效果造成的影响。步骤一、将准备实施的旋转指令角速度存储于导航计算机中；步骤二、进行旋转式惯导系统初始对准，并控制转轴将IMU坐标系扶正到与地理坐标系重合；步骤三、计算IMU旋转的第一个周期的指令角速度；步骤四、导航计算机进行导航解算；步骤五、计算IMU坐标系下导航坐标系n相对惯性坐标系i的实时旋转角速度；步骤六、生成下一周期的旋转IMU的绕z轴的指令角速度；步骤七：在各导航解算周期中循环按步骤四～六进行计算，生成下一周期的IMU绕z轴的转动指令角速度，在执行既定旋转方案的同时部分隔离载体的角运动。

Description

采用隔离载体运动提升旋转式捷联惯导误差抑制效果的方法

技术领域

本发明涉及一种对于车辆、舰船等在地面运动的载体航向频繁变化情况下提高旋转式捷联惯导系统误差补偿效果的载体运动隔离方法，属于惯性导航技术领域。

背景技术

旋转式惯导系统通过惯性测量单元(IMU)的旋转将惯性器件（陀螺仪和加速度计）的测量误差信号调制成正弦信号，并通过捷联算法中的积分运算将其消除，从而有效地提高惯性导航系统的精度。

旋转式惯导系统通常在导航工作前设计好旋转方案，而载体运行过程中其姿态角的变化却是不可预知的，只有假定载体坐标系与地理坐标系重合或存在一固定角度来设计旋转方案。一般情况下，地面载体的水平姿态角变化不大，但是航向角将随着舰船的转弯而发生变化，如果IMU只相对于载体旋转，而没有考虑隔离载体航向角的变化，则惯性器件误差不能被调制成正弦函数形式，误差抑制效果降低；若转动角速度刚好和舰船的转弯角速度相反，则相当于在导航坐标系下的惯性器件误差没有受到调制。因此使IMU相对于载体旋转的误差补偿方法存在局限性，需要设计相应的隔离载体角运动（至少须隔载体方位轴角运动）的辅助方案。

发明内容

为了克服载体沿方位轴角运动对旋转式惯导系统误差补偿效果造成的影响，本发明提出了一种采用隔离载体运动提升旋转式捷联惯导误差抑制效果的方法，适用于IMU绕z轴旋转的单轴旋转式惯导系统和z轴为外环轴的双轴旋转式惯导系统。

该采用隔离载体运动提升旋转式捷联惯导误差抑制效果的方法，包括以下步骤：

步骤一、将准备实施的旋转指令角速度以函数或查找表的形式存储于导航计算机中；

步骤二、进行旋转式惯导系统初始对准，并控制转轴将IMU坐标系扶正到与地理坐标系重合；

步骤三、设IMU旋转角速度指令生成与导航解算周期相同，则IMU旋转的第一个周期的指令角速度为：

${\mathrm{\ω}}_{\mathrm{bp}}^p\left(1\right)={\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p\left(1\right)---\left(1\right)$

步骤四、导航计算机进行导航解算，得到载体实时姿态矩阵、东向速度、北向速度、纬度的计算值L_c，角标c表示计算地理坐标系；

步骤五、使用步骤四解得的各参数计算IMU坐标系下导航坐标系n相对惯性坐标系i的实时旋转角速度：

${\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{in}}^p=C_c^p(\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L_c\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L_c\end{array}\right]+\left[\begin{array}{c}-\frac{V_y^c}{R}\\ \frac{V_x^c}{R}\\ \frac{V_x^c}{R}\tan L_c\end{array}\right])---\left(2\right)$

式中ω_ie为地球自转角速率，R为地球半径；

步骤六、生成下一周期的旋转IMU的绕z轴的指令角速度：

${\mathrm{\ω}}_{\mathrm{bpz}}^p(k+1)={\widehat{\mathrm{\ω}}}_{\mathrm{npz}}^p(k+1)-[{\widetilde{\mathrm{\ω}}}_{\mathrm{ipz}}^p\left(k\right)-{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{inz}}^p\left(k\right)-{\mathrm{\ω}}_{\mathrm{bpz}}^p\left(k\right)]---\left(3\right)$

式中为下一周期原旋转方案沿IMU的z轴方向的预期值，为本周期IMU的z轴陀螺测量值，为本周期IMU的z轴指令角速度，为步骤五中计算结果的z轴分量；

步骤七：在各导航解算周期中循环按步骤四～六进行计算，即可生成下一周期的IMU绕z轴的转动指令角速度，从而在执行既定旋转方案的同时部分隔离载体的角运动。

本发明的有益效果：

(1)本发明的方法可以保证旋转式惯导系统在载体航行期间无论载体航向角如何变化，IMU的旋转调制始终近似相对于地理坐标系进行，从而保证了既定的旋转方案可以发挥预期的旋转调制效果，相对于无角运动隔离机制的旋转式惯导系统，有效的旋转方案可以获得更高的导航精度。

(2)由于惯导系统存在标度因数误差和安装误差，在载体持续向同一方向转弯的情况下将会积累而导致导航误差大幅增加，本发明的方法可以通过对航向角变化的补偿来抑制此误差的积累从而提高导航精度。

(3)本发明方法充分利用了既定旋转方案的角速度信息、当前指令角速度信息、陀螺仪实时测量值和实时更新的导航参数信息，对下一时刻IMU角运动进行预测而生成指令角速度；一般的隔离载体运动的伺服系统仅仅计算当前载体的角运动状态参数，并在下一系统周期对载体之前的角运动进行补偿，对于系统实时导航参数和既定旋转方案角运动信息的利用具有一定滞后性，从而导致了载体角运动隔离的滞后性，补偿后与原定旋转方案的偏差可能较大。

(4)本发明方法实现简便，仅通过运算修正唯一的指令角速度输入，相对于无运动隔离的系统不会附加控制系统的复杂性，而且生成控制指令所用参数都是导航解算所必需的，除了(3)式的加减运算以外不会附加导航解算的复杂性。

(5)本发明所需信息仅为导航计算机解算参数和陀螺的角速度测量值，不需要陀螺力矩，所以可用于采用光学陀螺等无陀螺力矩的惯导系统。

附图说明

图1为本发明隔离载体运动方案流程图；

图2为IMU坐标系（p系)与导航坐标系（n系，即地理坐标系）的示意图；

图3为应用本发明方法后IMU按照生成的指令角速度旋转，旋转方案(a)~(c)方位角的实际值与原定旋转调制方案（预期值）的实时比较示意图；

图4为几种不同的导航参数在旋转方案(A)~(G)下的24h导航参数绝对误差最大值，各直方图从左到右依次对应旋转方案(A)~(G)。

具体实施方式

为了进一步说明本发明的技术方案，下面结合附图和实施例详细描述本发明的具体实施方式。

首先介绍本方法所涉及的各坐标系：角标p表示IMU坐标系（p系三轴x,y,z分别指向IMU的右、前、方向，旋转式惯导各轴角度在零位置时与p系x,y,z轴重合的转轴简称为转轴x轴、y轴、z轴），如图2所示；角标n表示东北天地理坐标系（选为导航坐标系），如图2所示；角标i表示地心惯性坐标系。

如图1所示，本发明的方法根据载体和IMU目前的角运动状态信息和预定旋转方案，预测下一系统周期IMU的角运动状态，生成相应的IMU旋转指令角速度，从而补偿载体坐标系相对于地理坐标系角运动给IMU的姿态矩阵带来的影响，使IMU和导航坐标系之间的角运动始终接近预先设定的旋转方案。

步骤一、旋转式惯导系统的旋转方案通常在设计导航系统时完成，并且具有周期性，可以通过软件函数或查找表的形式存储于导航计算机中，使导航计算机可以自动得出下一时刻的预期的IMU坐标系下IMU相对于导航坐标系的旋转角速度

步骤二、在旋转式惯导系统开始导航任务前，进行初始对准，即使用卡尔曼滤波估计载体的初始姿态角。对于停在平坦地面上的车辆和平静水面上的舰船，当IMU坐标系与载体坐标系重合时（即IMU转轴角度为零），按照估计的方位角旋转IMU的z轴使IMU的前向指向地理坐标系的正北，则可近似认为IMU坐标系与导航坐标系重合。

步骤三、系统开始导航，设IMU旋转角速度指令生成与导航解算周期相同，由于p系与n系近似重合，在第一系统周期IMU z轴旋转角速度与旋转方案的预期角速度相等。

步骤四、导航计算机进行导航解算，得到载体实时姿态矩阵、东向速度、北向速度、纬度的计算值，具体方法如下：

①IMU姿态解算：

IMU绕导航坐标系的角速度为：

${\mathrm{\ω}}_{\mathrm{np}}^p={\mathrm{\ω}}_{\mathrm{ip}}^p-C_n^p{\mathrm{\ω}}_{\mathrm{in}}^n---\left(4\right)$

式中为IMU姿态矩阵，为p系下IMU相对惯性空间的旋转角速度向量，实际取值为IMU的光纤陀螺组合测量值向量可按式(2)根据载体实时位置和实时地速推算。

再由更新IMU姿态矩阵，可采用四元数算法和旋转矢量法等算法。采用二阶龙格－库塔数值积分算法的实现方法如下：

$C_p^n(t+T)=C_p^n\left(t\right)+\frac{T}{2}\{C_p^n\left(t\right)\mathrm{\Ω}\left(t\right)+[C_p^n\left(t\right)+{\mathrm{TC}}_p^n\left(t\right)\mathrm{\Ω}\left(t\right)\left]\mathrm{\Ω}(t+T)\right\}---\left(5\right)$

式中Ω为的反对称矩阵形式。

②根据比力方程解算IMU在导航坐标系下速度变化率，即载体地速变化率：

${\stackrel{\·}{V}}_{\mathrm{en}}^n=f^n+g^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V_{\mathrm{en}}^n$

$=C_p^n{f}^p+g^n-\begin{array}{c}(2\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]+\left[\begin{array}{c}-\frac{V_N}{R}\\ \frac{V_E}{R}\\ \frac{V_E}{R}\tan L\end{array}\right])\end{array}\×V_{\mathrm{en}}^n---\left(6\right)$

式中为载体地速，f^p为p系下比力向量，实际取值为IMU的加速度计组合测量值向量gⁿ为重力加速度向量，为地球自转角速度向量，为导航坐标系n相对于地球坐标系e的角速度向量。

则载体地速可用数值积分梯形公式递推计算：（积分计算也可采用其他数值方法）

$V_{\mathrm{en}}^n(k+1)=V_{\mathrm{en}}^n\left(k\right)+\frac{T}{2}[{\stackrel{\·}{V}}_{\mathrm{en}}^n\left(k\right)+{\stackrel{\·}{V}}_{\mathrm{en}}^n(k+1)]---\left(7\right)$

式中T为系统导航解算周期。

③载体纬度经度、可用数值积分梯形公式递推计算：

$L(k+1)=L\left(k\right)+\frac{T}{2R}[V_N\left(k\right)+V_N(k+1)]$

$\mathrm{\λ}(k+1)=\mathrm{\λ}\left(k\right)+\frac{T}{2R\cos \left[L\left(n\right)\right]}[V_E\left(k\right)+V_E(k+1)]---\left(8\right)$

式中L为载体纬度，λ为载体经度。

步骤五、可隔离载体角运动的IMU旋转指令角速度生成：

对于旋转式惯导系统，在IMU坐标系下，IMU相对于惯性坐标系的角速度为：

${\mathrm{\ω}}_{\mathrm{ip}}^p={\mathrm{\ω}}_{\mathrm{in}}^p+{\mathrm{\ω}}_{\mathrm{nb}}^p+{\mathrm{\ω}}_{\mathrm{bp}}^p---\left(9\right)$

其中IMU相对于导航坐标系的角速度（即实现旋转调制功能的角速度）为：

${\mathrm{\ω}}_{\mathrm{np}}^p={\mathrm{\ω}}_{\mathrm{nb}}^p+{\mathrm{\ω}}_{\mathrm{bp}}^p---\left(10\right)$

生成指令角速度的目的是使下一系统周期内IMU相对于导航坐标系的角速度与所设计的旋转方案的预期值近似相等，即

${\mathrm{\ω}}_{\mathrm{np}}^p(k+1)={\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p(k+1)---\left(11\right)$

则IMU绕转轴旋转的角速度为：

${\mathrm{\ω}}_{\mathrm{bp}}^p(k+1)={\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p(k+1)-{\mathrm{\ω}}_{\mathrm{nb}}^p(k+1)---\left(12\right)$

其中的值已知，的表达式为

${\mathrm{\ω}}_{\mathrm{nb}}^p(k+1)={\mathrm{\ω}}_{\mathrm{ip}}^p(k+1)-{\mathrm{\ω}}_{\mathrm{in}}^p\left(k\right)-{\mathrm{\ω}}_{\mathrm{bp}}^p(k+1)$

$={\mathrm{\ω}}_{\mathrm{ip}}^p(k+1)-C_n^p\left(k\right){\mathrm{\ω}}_{\mathrm{in}}^n\left(k\right)-{\mathrm{\ω}}_{\mathrm{bp}}^p(k+1)---\left(13\right)$

其中为下一系统周期进行姿态更新的必要运算。通过载体角运动隔离，

${\mathrm{\ω}}_{\mathrm{bp}}^p(k+1)\≈{\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p(k+1)---\left(14\right)$

用增量形式表示为：

${\mathrm{\ω}}_{\mathrm{ip}}^p(k+1)={\mathrm{\ω}}_{\mathrm{ip}}^p\left(k\right)+\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{ip}}^p\left(k\right)---\left(15\right)$

其中可用本周期陀螺测量值近似表达，而如式(9)所示的各分量中，和在系统周期足够取得足够短的情况下变化量很小，因此仅用的变化量近似表示的增量：

$\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{ip}}^p\left(k\right)\≈{\mathrm{\ω}}_{\mathrm{bp}}^p(k+1)-{\mathrm{\ω}}_{\mathrm{bp}}^p\left(k\right)$

$\≈{\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p(k+1)-{\mathrm{\ω}}_{\mathrm{bp}}^p\left(k\right)---\left(16\right)$

式(14)~(16)代入式(13)可以得到下一系统周期的指令角速度：

${\mathrm{\ω}}_{\mathrm{nb}}^p(k+1)={\mathrm{\ω}}_{\mathrm{ip}}^p(k+1)-C_n^p\left(k\right){\mathrm{\ω}}_{\mathrm{in}}^n\left(k\right)-{\mathrm{\ω}}_{\mathrm{bp}}^p(k+1)$

$\≈{\widetilde{\mathrm{\ω}}}_{\mathrm{ip}}^p\left(k\right)={\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p(k+1)-{\mathrm{\ω}}_{\mathrm{bp}}^p\left(k\right){-C_n^p\left(k\right)\mathrm{\ω}}_{\mathrm{in}}^n\left(k\right)-{\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p(k+1)$

$={\widetilde{\mathrm{\ω}}}_{\mathrm{ip}}^p\left(k\right)-{\mathrm{\ω}}_{\mathrm{bp}}^p\left(k\right)-C_n^p\left(k\right){\mathrm{\ω}}_{\mathrm{in}}^n\left(k\right)---\left(17\right)$

在应用中，由于地面载体的水平姿态角变化较小，是航向角随着舰船的转弯变化较大，因此应以隔离方位轴方向的角运动为主，对于绕z轴旋转的单轴旋转式惯导和z轴为外环轴的双轴旋转式惯导，z轴旋转的指令角速度如式(3)所示。

下面通过基于导航误差方程和matlab仿真实验说明本发明的隔离载体运动方法的效果。

仿真条件如下：系统导航解算和指令角速度生成的频率为10Hz；载体地初始纬度为北纬30°，初始航向为正北方向，地速为10m/s，做角速率为6°/s的匀速圆周运动，载体的俯仰角、横滚角分别按幅值为10°、11°，频率为0.1Hz、0.2Hz的正弦函数规律变化；旋转式惯导系统为绕z轴旋转的单轴旋转式惯导，或执行绕z轴单轴旋转方案的双轴旋转式惯导；加速度计与陀螺的零偏分别为10^-4g和0.01°/h，白噪声标准差的值为器件零偏的1/2，各惯性器件标度因数误差均为10ppm，各安装误差角均为10”。

对三种旋转方案的仿真实验：

(a)IMU绕z轴匀速连续旋转，每转一周改变转向，角速率为6°/s；

(b)IMU绕z轴匀速连续旋转，每转一周改变转向，角速率为12°/s；

(c)IMU绕z轴连续旋转，每转一周改变转向，角速率按执行函数规律变化，单向旋转一周平均角速率为12°/s；

通过仿真实验验证应用本发明方法后在载体航向角变化情况下指令角速度执行既定旋转方案的准确程度，仿真结果表明：根据实时生成的指令角速度旋转IMU，IMU的方位角Ψ_IMU的实际值接近于原定旋转方案的预期值，从而确保IMU能够按原定旋转方案调制惯性器件误差。如图3所示，旋转方案(a)~(c)方位角的实际值与原定旋转调制方案（预期值）的实时比较，二者接近则表示本发明的方法可以隔离载体运动从而较准确地执行预期的旋转调制方案。

通过仿真实验验证在载体航向角变化情况下应用本发明方法对旋转式捷联惯导系统误差补偿效果的改善，进行24小时导航仿真，选择以下7种旋转方案：

(A)普通捷联惯导，IMU无旋转；

(B)IMU绕z轴匀速连续旋转，每转一周改变转向，角速率为6°/s，未使用载体运动隔离方案；

(C)IMU绕z轴匀速连续旋转，每转一周改变转向，角速率为12°/s，未使用载体运动隔离方案；

(D)IMU绕z轴连续旋转，每转一周改变转向，角速率按执行函数规律变化，单向旋转一周平均角速率为12°/s，未使用载体运动隔离方案；

(E)IMU绕z轴匀速连续旋转，每转一周改变转向，角速率为6°/s，使用本发明载体运动隔离方案；

(F)IMU绕z轴匀速连续旋转，每转一周改变转向，角速率为12°/s，使用本发明载体运动隔离方案；

(G)IMU绕z轴连续旋转，每转一周改变转向，角速率按执行函数规律变化，单向旋转一周平均角速率为12°/s，使用本发明载体运动隔离方案；

各旋转方案的24h各导航参数绝对误差最大值如图4所示，仿真实验表明：应用本发明的方法后，由于预期的旋转方案的误差旋转调制被较为准确地执行，旋转式惯导系统的误差补偿效果得到了保证，导航精度因此提高。

Claims (1)

1.采用隔离载体运动提升旋转式捷联惯导误差抑制效果的方法，其特征在于，包括以下步骤：

步骤一、将准备实施的旋转指令角速度以函数或查找表的形式存储于导航计算机中；

步骤二、进行旋转式惯导系统初始对准，并控制转轴将IMU坐标系扶正到与地理坐标系重合；

步骤三、设IMU旋转角速度指令生成与导航解算周期相同，则IMU旋转的第一个周期的指令角速度为：

${\mathrm{\ω}}_{\mathrm{bp}}^p\left(1\right)={\widehat{\mathrm{\ω}}}_{\mathrm{np}}^p\left(1\right)---\left(1\right)$

步骤四、通过导航计算机进行导航解算，得到载体实时姿态矩阵、东向速度、北向速度、纬度的计算值L_c，角标c表示计算地理坐标系；

步骤五、使用步骤四解得的各参数计算IMU坐标系下导航坐标系n相对惯性坐标系i的实时旋转角速度：

${\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{in}}^p=C_c^p(\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L_c\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L_c\end{array}\right]+\left[\begin{array}{c}-\frac{V_y^c}{R}\\ \frac{v_x^c}{R}\\ \frac{V_x^c}{R}\tan L_c\end{array}\right])---\left(2\right)$

式中ω_ie为地球自转角速率，R为地球半径；

步骤六、生成下一周期的旋转IMU的绕z轴的指令角速度：

${\mathrm{\ω}}_{\mathrm{bpz}}^p(k+1)={\widehat{\mathrm{\ω}}}_{\mathrm{npz}}^p(k+1)-[{\widetilde{\mathrm{\ω}}}_{\mathrm{ipz}}^p\left(k\right)-{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{inz}}^p\left(k\right)-{\mathrm{\ω}}_{\mathrm{bpz}}^p\left(k\right)]---\left(3\right)$

式中为下一周期原旋转方案沿IMU的z轴方向的预期值，为本周期IMU的z轴陀螺测量值，为本周期IMU的z轴指令角速度，为步骤五中计算结果的z轴分量；

步骤七：在各导航解算周期中循环按步骤四～六进行计算，即生成下一周期的IMU绕z轴的转动指令角速度，从而在执行既定旋转方案的同时部分隔离载体的角运动；

其中,角标p表示IMU坐标系,p系三轴x,y,z分别指向IMU的右、前、方向，旋转式惯导各轴角度在零位置时与p系x,y,z轴重合的转轴为转轴x轴、y轴、z轴；角标n表示东北天地理坐标系。