[发明专利]一种单轴旋转光纤陀螺捷联惯导系统速度误差抑制方法

权利要求书

1.一种单轴旋转光纤陀螺捷联惯导系统速度误差抑制方法，其特征在于，包括以下步骤：

步骤一：通过全球定位GPS系统采集载体位置信息，并装订至导航计算机中；

导航初始时刻，通过全球定位GPS系统采集初始时刻载体位置信息、速度信息，并装订至导航计算机中；载体位置信息包括载体所在位置的经度、纬度信息；

导航过程中，利用该初始信息进行更新，得到任意时刻载体的速度、位置；

步骤二：将旋转机构转动至IMU系与载体系重合的位置，有其中b表示载体坐标系，s表示IMU坐标系，表示s系到b系转换矩阵，I表示单位阵；将光纤陀螺捷联惯导系统进行充分预热后，采集光纤陀螺仪和石英加速度计输出的数据；得到载体的角运动信息和线运动信息；角运动信息包括角速度值，线运动信息包括比力值；

根据加速度计输出的比力值与重力加速度关系、以及陀螺仪输出的角速度值与地球自转角速度关系，确定载体姿态角，完成系统初始对准，建立惯导系统初始捷联矩阵

Csn=CbnCsb=cosφy0cosφz0-sinφx0sinφy0sinφz0-cosφx0sinφz0sinφy0cosφz0+sinφz0sinφx0cosφy0sinφz0cosφy0+sinφx0sinφy0cosφz0cosφx0cosφz0sinφz0sinφy0+cosφz0sinφx0cosφy0-cosφx0sinφy0sinφx0cosφx0cosφy0---(1)]]>

其中，φ_x0、φ_y0、φ_z0分别表示初始时刻载体俯仰角、横滚角、航向角；

步骤三：旋转机构带动惯性组件以ω进行单轴正反转停运动；采用八个转停次序为一个旋转周期的旋转方案；

所述惯性组件简称IMU，IMU转动过程采用八个转停次序为一个旋转周期的转位方案，具体为：

次序1，IMU从A点出发顺时针转动180°，到达位置C，停止时间T_r；次序2，IMU从C点出发逆时针转动90°，到达位置B，停止时间T_r；次序3，IMU从B点出发顺时针转动180°，到达位置D，停止时间T_r；次序4，IMU从D点出发逆时针转动270°，到达位置A，停止时间T_r；次序5，IMU从A点出发逆时针转动180°，到达位置C，停止时间T_r；次序6，IMU从C点出发顺时针转动90°，到达位置D，停止时间T_r；次序7，IMU从D点出发逆时针转动180°，到达位置B，停止时间T_r；次序8，IMU从B点出发顺时针转动270°，到达位置A，停止时间T_r；IMU按照此转动顺序循环进行；

步骤四：实时采集光纤陀螺仪和石英加速度计测量载体运动的线速度和角速度信息，导航解算得到导航信息；

通过旋转调制状态下采集的陀螺仪数据，更新捷联矩阵具体为：

角速度更新：

ωnss==ωiss-(Csn)T(ωies+ωens)---(2)]]>

其中，i表示地心惯性系，e表示地球坐标系，s表示惯性组件坐标系，n表示导航坐标系，这里采用当地地理坐标系；表示s系到n系转换矩阵；表示p系相对m系旋转角速度在q系投影，m＝n,i,e，p＝s,e,n，q＝s；

四元数姿态矩阵更新：

设任意时刻载体坐标系相对平台坐标系的转动四元数为：

Q＝q₀+q₁i_b+q₂j_b+q₃k_b    （3）

其中，Q为四元数；q₀、q₁、q₂、q₃为四元数的四个实数；i_b、j_b、k_b分别表示IMU坐标系ox_s轴、oy_s轴、oz_s轴上的单位方向向量；

四元数Q的及时修正：

q·0q·1q·2q·3=120-ωnsxs-ωnsys-ωnszsωnsxs0ωnszs-ωnsxsωnsys-ωnszs0ωnsxsωnszsωnsys-ωnsxs0q0q1q2q3---(4)]]>

其中，分别表示旋转机构相对导航系的运动角速度在IMU坐标系ox_s轴、oy_s轴、oz_s轴上的分量；分别表示q₀、q₁、q₂、q₃的变化率；

根据k时刻载体坐标系相对平台坐标系的转动四元数q₀(k)、q₁(k)、q₂(k)、q₃(k)，求取k时刻转动四元数的变化率为：

q·0(k)q·1(k)q·2(k)q·3(k)=120-ωnsxs-ωnsys-ωnszsωnsxs0ωnszs-ωnsxsωnsys-ωnszs0ωnsxsωnszsωnsys-ωnsxs0q0(k)q1(k)q2(k)q3(k)---(5)]]>

在k+1时刻载体的转动四元数具体为：

q0(k+1)=q0(k)+q·0(k)q1(k+1)=q1(k)+q·1(k)q2(k+1)=q2(k)+q·2(k)q3(k+1)=q3(k)+q·3(k)---(6)]]>

利用得到的q₀(k+1)、q₁(k+1)、q₂(k+1)、q₃(k+1)，更新捷联矩阵

Csn=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32---(7)]]>

其中，（7）式中的q_i为（6）式中q_i(k+1)，i＝1,2,3,4；

更新载体姿态信息，具体为：

φx=arcsin(c33)φy=arctan(c32/c31)φz=arctan(c13/c23)---(8)]]>

其中，c_ij表示中第i行第j列矩阵元素，i＝1,2,3，j＝1,2,3；φ_x、φ_y、φ_z表示载体纵摇角、横摇角、航向角；

将加速度计沿IMU坐标系测量的比力信息，通过捷联矩阵进行投影转换：

fn=Csnfs---(9)]]>

其中，fⁿ、f^s分别表示加速度计测量比力在n系和s系投影；

利用下列微分方程求解载体运动速度：

v·xv·yv·z=fxnfynfzn-00g+02ωiezn-(2ωieyn+ωenyn)-ωiezn02ωiexn+ωenxn2ωieyn+ωenyn-(2ωiexn+ωenxn)0vxvyvz---(10)]]>

其中，v_x、v_y、v_z分别表示解算载体速度在导航系ox_n轴、oy_n轴、oz_n轴上的分量；表示v_x、v_y、v_z的变化率，即载体沿导航系ox_n轴、oy_n轴、oz_n轴的运动加速度；分别表示加速度计测量比力在导航系ox_n轴、oy_n轴、oz_n轴上的分量；g为重力加速度；分别表示地球自转角速度在导航系ox_n轴、oy_n轴、oz_n轴上的分量；分别表示由于载体运动而导致导航系相对地球系变化的旋转角速度在导航系ox_n轴、oy_n轴、oz_n轴上的分量；

根据k时刻的载体东向水平速度v_x(k)、北向水平速度v_y(k)和天向速度v_z(k)，求取k时刻载体速度变化率为：

v·x(k)v·y(k)v·z(k)=fxnfynfzn-00g+02ωiezn-(2ωieyn+ωenyn)-ωiezn02ωiexn+ωenxn2ωieyn+ωenyn-(2ωiexn+ωenxn)0vx(k)vy(k)vz(k)---(11)]]>

在k+1时刻载体速度和位置分别为：

vx(k+1)=vx(k)+v·x(k)vy(k+1)=vy(k)+v·y(k)vz(k+1)=vz(k)+v·z(k)---(12)]]>

其中，R表示地球半径；λ分别表示计算载体所在地理位置的纬度和经度信息，当k＝1时，v_x(1)、v_y(1)、v_z(1)为步骤一种利用GPS获得的载体初始速度，λ(1)为步骤一种利用GPS获得的载体初始位置；

至此，根据（8）、（12）、（13）式得到载体的速度、位置、姿态；然后将姿态、位置直接输出至导航计算机显示器，提供载体各项导航信息；速度待进行步骤五中的进一步滤波处理优化；

步骤五：构造Butterworth带阻滤波器，将导航系下得到的载体速度进行Butterworth滤波器处理，滤除导航系下与旋转角速度有关的振荡误差项，滤波后的速度作为最终导航解算输出信息；

（1）构造Butterworth带阻滤波器，具体为：

步骤三中旋转机构采用旋转角速度为ω，此角速度引起导航信息的振荡误差频率约为f＝1/ω；设定Butterworth带阻滤波器的通带下限截止频率为(0.59～0.61)f，通带上限截止频率为(1.67～1.69)f，阻带下限截止频率为(0.83～0.85)f，阻带上限截止频率为(1.13～1.15)f，则有：

Ω1=(0.59-0.61)fΩ3=(1.67-1.69)fΩsl=(0.83-0.85)fΩsh=(1.13-1.15)f---(14)]]>

其中，Ω₁表示通带下限截止频率；Ω₃表示通带上限截止频率；Ω_sl表示阻带下限截止频率；Ω_sh表示阻带上限截止频率；

ΩBW=Ω3-Ω1Ω22=Ω1Ω3---(15)]]>

其中，Ω_BW表示通带带宽，Ω₂表示阻带中心频率；频率归一化，为：

η1=Ω1/ΩBWη3=Ω3/ΩBWηsl=Ωsl/ΩBWηsh=Ωsh/ΩBWη22=η1η3=Ω1Ω3/ΩBW2---(16)]]>

其中，η_i分别表示相应频率Ω_i对应的归一化频率，i＝1,2,3,sl,sh；

带阻滤波器对应低通滤波器的频率转换，具体过程为：

λp=η3-η1=1-λs′=ηsl/(ηsl2-η22)λs′=ηsh/(ηsh2-η22)---(17)]]>

其中，λ_p表示低通滤波器的通带截止频率；表示低通滤波器的阻带下限截止频率；表示低通滤波器的阻带下限截止频率的对称频率；

依据（17）式，取λ_s与-λ_s绝对值较小的一个为最终阻带下限截止频率，即设计低通滤波器G(p)；求取滤波器阶次为：

N=lg10αs/10-110αp/10-1/lgλs---(18)]]>

其中，N表示滤波器的阶次，α_s表示阻带应达到的最小衰减，α_p表示通带允许最大衰减通常采用α_s＝14dB、α_p＝3dB；

因此，

G(p)=1pN+1---(19)]]>

其中，p表示滤波器参量；

低通滤波器与带阻滤波器之间的参量固定关系为，

p=s(Ω3-Ω1)s2+Ω1Ω3|s=z-1z+1=(z2-1)(Ω3-Ω1)(z-1)2+Ω1Ω3(z+1)2---(20)]]>

（20）式代入（19）式，构造出离散型二阶带阻Butterworth滤波器的转移函数，

H(z)=G(p)|p=(z2-1)(Ω3-Ω1)(z-1)2+Ω1Ω3(z+1)2=1[(z2-1)(Ω3-Ω1)(z-1)2+Ω1Ω3(z+1)2]N+1---(21)]]>

（2）将步骤四中解算得到的导航系下载体速度进行带阻滤波器处理，滤除该速度信息中与旋转角速度有关的振荡误差项，滤波后的速度作为最终导航解算输出信息，具体为：

对单轴旋转捷联惯导系统解算速度建模为：

V_INS＝V+δV_INS(r)+δV_INS(s)（22）

其中，V_INS表示步骤四中经导航解算得到的速度信息；V表示载体运动速度；δV_INS(r)表示由旋转调制引起的振荡误差；δV_INS(s)表示惯导解算舒勒、地球振荡误差和常值误差；

由于所在频段与其它几项误差所在频段相差悬殊，对使用步骤（1）构建的滤波器，得到：

VINS′=H(z)·VINS---(23)]]>

式中，H(z)表示根据旋转角速度构建的带阻滤波器；为滤波后的速度，其中只包含V、δV_INS(s)；

以滤波后的速度作为系统最终解算速度信息，并输出至导航计算机的显示装置，提供载体航行中的运动速度信息。