CN104596546A - 一种单轴旋转惯导系统的姿态输出补偿方法 - Google Patents

Abstract

本发明公开了一种单轴旋转惯导系统的姿态输出补偿方法，步骤如下：在z陀螺输出的角速度中扣除地球自转角速度分量和陀螺漂移，经积分后得到角度；将z陀螺积分的角度与码盘输出的转角作差，对该差值进行拟合可构造航向角的补偿量；利用x、y加速度计的输出和初始对准得到的俯仰角、横滚角，扣除重力加速度投影，进而构造俯仰角和横滚角的补偿量；根据姿态输出补偿算法补偿单轴旋转惯导系统的姿态输出。该姿态输出补偿算法易于实现，不仅可以满足工程应用的实时性要求，而且可大幅提升单轴旋转惯导系统的姿态输出精度，对提高单轴旋转惯导系统的导航性能具有重要意义。

Description

一种单轴旋转惯导系统的姿态输出补偿方法

技术领域

本发明涉及一种单轴旋转惯导系统的姿态输出补偿方法，属于旋转式惯导系统姿态补偿技术领域，适用于补偿单轴旋转惯导系统的姿态输出精度，特别适合对惯导系统短期姿态精度要求较高的应用场合。

背景技术

惯导系统通过正交安装的三轴陀螺仪和加速度计测量载体相对于空间的角速度和加速度，并基于航位推算原理给出载体实时的位置、速度和姿态信息。在惯导系统中，陀螺漂移和加计零偏是影响导航性能的关键因素，无论是平台式惯导还是捷联式惯导，都存在导航误差随时间累积的问题。

旋转式惯导系统通过旋转机构带动IMU(Inertial Measurement Unit，惯性测量单元)绕着某个坐标系按照既定方案旋转，使得惯性组件的常值误差在该坐标系内的投影呈周期振荡形式，而周期振荡误差在一个旋转周期内积分结果为零，因此惯性组件的常值误差对导航结果的影响得到抑制，系统精度得到大幅提升。

单轴旋转惯导系统通过单一旋转轴的正反旋转可以抑制水平两轴的陀螺漂移和加计零偏。目前的文献中已有大量实验结果表明单轴旋转惯导系统可以将位置和速度精度提高约一个数量级，但是实际系统在使用中姿态输出的精度可能并未提高，甚至会变得更差，文献对这方面的研究不多见；而且对于旋转惯导系统的姿态补偿主要是基于对惯性器件与旋转轴以及惯性器件之间的安装偏角的标定，对角度传感器(例如码盘或光栅等)的误差和旋转轴的不规律转动的补偿研究较少。

发明内容

本发明提出一种单轴旋转惯导系统的姿态输出补偿方法，可以有效补偿单轴旋转惯导系统中姿态输出中的波动，大幅提高姿态精度。利用z陀螺积分的角度作为基准修正码盘输出的转角，给出了航向角的补偿信息；利用x、y加速度计扣除初始俯仰角、横滚角带来的重力加速度投影后的剩余分量修正旋转轴的不规律转动，给出了俯仰角和横滚角的补偿信息；结合这些补偿信息和具体的补偿算法从而实现了对单轴旋转惯导系统的姿态输出补偿。

本发明的解决技术方案：一种单轴旋转惯导系统的姿态输出补偿方法，步骤如下：

步骤(1)：在z陀螺输出的角速度中扣除地球自转角速度分量和陀螺漂移，经积分后得到角度；

步骤(2)：将z陀螺积分角度与码盘输出的转角作差，对该差值进行拟合可构造航向角的补偿量；

步骤(3)：利用x、y加速度计的输出和初始对准得到的俯仰角、横滚角，扣除重力加速度投影，进而构造俯仰角和横滚角的补偿量；

步骤(4)：根据姿态输出补偿算法补偿单轴旋转惯导系统的姿态输出。

进一步的，步骤(1)中z陀螺输出的角速度可由公式(1)给出：

ω_P＝ω+ω_ie sin L+ε_z

                                (1)

ω_N＝-ω+ω_ie sin L+ε_z

式中，ω为单轴旋转惯导系统的旋转角速度，ω_P为正转时z陀螺输出的角速度，ω_N为反转时z陀螺输出的角速度，ω_ie为地球自转角速度，L为当地地理纬度，ε_z为z陀螺漂移。

将公式(1)中ω_P与ω_N相加除2后取平均，可得到z陀螺输出中需要扣除的地球自转角速度分量和陀螺漂移之和由公式(2)给出：

$\widetilde{\mathrm{\ω}}=\mathrm{mean}\left(\frac{{\mathrm{\ω}}_P+{\mathrm{\ω}}_N}{2}\right)---\left(2\right)$

在z陀螺输出中扣除并积分，可得正转和反转时z陀螺积分角度与

将与合在一起即得到z陀螺积分的角度。

进一步的，步骤(2)中将z陀螺积分的角度与码盘输出的转角作差，对差值按公式(4)进行拟合：

根据拟合参数a、b、c可修正码盘输出的转角，作为航向角的补偿信息。

进一步的，步骤(3)中根据初始对准得到的俯仰角和横滚角，按公式(5)扣除x、y加速度计上的重力加速度投影：

式中，a_x、a_y为x、y加速度计的输出，g为当地重力加速度，θ₀、γ₀为初始对准得到的俯仰角和横滚角，a′_x、a′_y为扣除重力加速度投影后的剩余分量，进而通过公式(6)构造俯仰角和横滚角的补偿量：

$\begin{array}{c}\mathrm{\δ\θ}=\frac{a_y^{\′}}{g}\\ \mathrm{\δ\γ}=-\frac{a_x^{\′}}{g}\end{array}---\left(6\right)$

式中，δθ为俯仰角补偿信息，δγ为横滚角补偿信息。

进一步的，步骤(4)中的姿态输出补偿算法具体过程为：

根据陀螺的输出和上一解算周期的姿态矩阵按公式(7)和公式(8)计算

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

${\mathrm{\ω}}_{\mathrm{ns}}^s={\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{in}}^s---\left(8\right)$

式中， ${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T,$ 为地球自转角速度在导航坐标系中的投影； ${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-\frac{V_y}{R}& \frac{V_x}{R}& \frac{V_x}{R}\tan L\end{array}\right]}^T,$ 为位移角速度在导航坐标系中的投影，其中V_x、V_y为载体的东向速度和北向速度，R为地球半径。根据和四元数姿态微分方程即可实现姿态矩阵的更新。

根据步骤(2)得到的拟合模型，修正码盘的转角按公式(9)构建矩阵

根据步骤(3)得到的δθ、δγ，按公式(10)构建矩阵

$C_s^{s^{\′}}=\left[\begin{array}{ccc}1& 0& -\mathrm{\δ\γ}\\ 0& 1& \mathrm{\δ\θ}\\ \mathrm{\δ\γ}& -\mathrm{\δ\θ}& 1\end{array}\right]---\left(10\right)$

根据更新后的姿态矩阵和公式(9)给出的公式(10)给出的按公式(11)计算矩阵

$C_b^n=C_{s^{\′}}^n{C}_s^{s^{\′}}{C}_b^s---\left(11\right)$

根据矩阵和公式(12)得到单轴旋转惯导系统经过补偿后的输出姿态：

θ＝sin^-1 C₂₃

$\begin{array}{c}\mathrm{\γ}={\tan }^{-1}(-\frac{C_{31}}{C_{33}})\\ \mathrm{\ψ}={\tan }^{-1}(-\frac{C_{12}}{C_{22}})\end{array}---\left(12\right)$

式中，θ为俯仰角，γ为横滚角，ψ为航向角，C_ij,i,j＝1,2,3为矩阵中的各元素。

本发明的原理如下：

单轴旋转惯导系统的一般构成如图2中的左图所示，包括IMU、码盘和力矩电机等。IMU中包括了3个正交安装的陀螺Gx、Gy、Gz和3个正交安装的加速度计Ax、Ay、Az，陀螺和加速度计测量载体相对空间的的角速度和加速度信息，力矩电机拖动IMU进行正反旋转，码盘提供转角信息配合电机实现闭环的旋转控制。

图2中的右图给出了单轴旋转惯导系统中的坐标系定义，其中O-XbYbZb为载体坐标系，O-XsYsZs为旋转坐标系，Zs轴与Zb轴重合，两个坐标系之间的转换关系可由码盘转角描述。

惯导系统是一种基于航位推算的导航系统，高精度的位置和速度精度理论上应该具备高精度的姿态精度。而实际使用中，单轴旋转惯导系统的姿态输出精度不高，短期姿态输出存在明显的波动。经分析认为，航向角上的波动是由于码盘自身的输出存在一定误差，而俯仰角和横滚角上的波动是由于旋转轴的不规律转动引起。

若忽略码盘的误差，直接使用码盘的原始输出构建公式(9)中的矩阵则码盘的角度误差会引起的航向角输出的波动。通过z陀螺的角速度进行积分得到角度基准，可以补偿校正码盘的误差。而z陀螺输出的角速度中包含了地球自转角速度分量和陀螺漂移，必须扣除这两部分的影响后才能进行积分，得到准确的角度。根据陀螺积分角度即可对码盘误差进行建模并拟合，从而补偿航向角的输出波动。需要注意的是，步骤(1)和步骤(2)是通过事先的实验完成的，单轴旋转惯导系统在实际工作中仅需要已得到的拟合参数a、b、c，不再需要进行积分、拟合误差等计算过程，使得航向角补偿的计算量大大降低，提高了补偿算法的实时性。

如果忽略旋转轴的不规律转动，认为整个轴系的加工装配和旋转轴的转动都是理想的，则单轴旋转惯导系统的姿态矩阵的计算过程由公式(13)描述：

$C_b^n=C_s^n{C}_b^s---\left(13\right)$

而真实情况下轴系的加工装配存在误差，使得旋转坐标系OZs轴与载体坐标系的OZb轴不重合，旋转平面也会偏离固定平面而存在上下波动，这种不规律转动会导致俯仰角和横滚角的输出存在波动。而旋转平面的上下波动可通过水平加速度计敏感，在扣除初始俯仰角和横滚角引入的重力加速度投影后，水平加速度计剩余分量即可描述轴系真实的转动规律。

此时需要增加一个坐标系OXs’Ys’Zs’系，如图6所示。OXs’Ys’Zs’为理想旋转系，表示理想转动情况下的旋转坐标系，OXsYsZs为瞬时旋转系，表示每个时刻真实的旋转坐标系，二者的关系可通过公式(6)给出的δθ、δγ描述。根据图6的转换关系，理想旋转系与瞬时旋转系的转换矩阵可通过两次欧拉角转动给出：

$C_s^{s^{\′}}=R_y\left(\mathrm{\δ\γ}\right)R_s\left(\mathrm{\δ\θ}\right)---\left(14\right)$

式中， $R_y\left(\mathrm{\δ\γ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\δ\γ}& 0& -\sin \mathrm{\δ\γ}\\ 0& 1& 0\\ \sin \mathrm{\δ\γ}& 0& \cos \mathrm{\δ\γ}\end{array}\right],R_x\left(\mathrm{\δ\θ}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\δ\θ}& \sin \mathrm{\δ\θ}\\ 0& -\sin \mathrm{\δ\θ}& \cos \mathrm{\δ\θ}\end{array}\right].$

经过小角度近似，可对公式(14)进行简化，进一步降低算法的复杂度，即可得公式(10)：

$C_s^{s^{\′}}=R_y\left(\mathrm{\δ\γ}\right)R_x\left(\mathrm{\δ\θ}\right)\≈(I+[\mathrm{\δ\γ}\left]\right)(I+[\mathrm{\δ\θ}\left]\right)\≈\left[\begin{array}{ccc}1& 0& -\mathrm{\δ\γ}\\ 0& 1& \mathrm{\δ\θ}\\ \mathrm{\δ\γ}& -\mathrm{\δ\θ}& 1\end{array}\right]$

根据可将单轴旋转惯导系统的姿态输出由公式(13)修正为公式(11)，从而补偿俯仰角和横滚角的输出波动。

本发明与现有技术相比的优点在于：

(1)本发明提出的姿态输出补偿方法机理清晰，指出单轴旋转惯导系统的码盘误差和旋转轴不规律转动是影响姿态输出精度的主要因素，补偿方法从这两个因素入手，有较强的针对性。

(2)本发明提出的姿态输出补偿方法，在补偿航向角输出时计算z陀螺的积分角度和拟合码盘误差均通过事先处理完成，在补偿俯仰角和横滚角输出时的姿态矩阵计算也较为简单，因此在实际系统工作时算法易于实现，可完全满足工程应用的实时性要求。

(3)本发明提出的姿态输出补偿方法效果显著。将经过输出补偿的姿态与原有未补偿的姿态输出相比，俯仰角、横滚角和航向角的输出精度可提高约5倍，充分证明了该算法的有效性。

附图说明

图1为本发明姿态输出补偿方法的流程图；

图2为本发明具体实施例的单轴旋转惯导系统结构示意图与坐标系定义；

图3为本发明具体实施例的码盘转角原始输出和与z陀螺积分角度之差；

图4为本发明具体实施例的码盘角度误差拟合结果；

图5为本发明具体实施例的x、y加速度计的原始输出与扣除初始俯仰角、横滚角引入重力加速度投影后的剩余分量；

图6为本发明理想旋转系和瞬时旋转系之间的转换关系图；

图7为本发明具体实施例通过姿态输出补偿算法得到补偿后的姿态与未补偿姿态的对比图。

具体实施方式

图1给出了本发明姿态输出补偿方法的流程图，该方法的主要步骤为：在z陀螺输出的角速度中扣除地球自转角速度分量和陀螺漂移，经积分后得到角度；将z陀螺积分角度与码盘输出的转角作差，对该差值进行拟合可构造航向角的补偿量；利用x、y加速度计的输出和初始对准得到的俯仰角、横滚角，扣除重力加速度投影，进而构造俯仰角和横滚角的补偿量；根据姿态输出补偿算法补偿单轴旋转惯导系统的姿态输出。以下结合具体实施例对本发明作进一步说明。

图2中的左图给出了本发明应用的单轴旋转惯导系统结构示意图，包括IMU、码盘和力矩电机等，IMU中包括了3个正交安装的陀螺Gx、Gy、Gz，3个正交安装的加速度计Ax、Ay、Az，提供载体的角速度和加速度信息，力矩电机拖动IMU进行正反旋转，码盘提供转角信息实现闭环控制。本发明应用的单轴旋转惯导系统旋转方案为IMU正反连续旋转，旋转角速度为0.686°/s，一个正反转周期为1050s，实验时间为两个IMU正反转周期。

图3依次给出了本发明具体实施例的码盘转角原始输出和与z陀螺积分角度之差，z陀螺积分角度由公式(1)～公式(3)获得。可见码盘角度误差呈现明显的正余弦规律，波动幅值约±60″，且码盘误差在两个正反转周期中呈现出了明显的重复性，因此可以对其进行建模并补偿，否则将会在航向角的输出上引入±60″的波动。

图4为本发明具体实施例的码盘角度误差拟合结果。拟合时自变量为码盘转角，因变量为角度误差，拟合模型由公式(4)给出，得到的拟合系数为a＝-0.0022，b＝-66.26，c＝12.62，单位均为″。

图5为依次给出了本发明具体实施例中x、y加速度计的原始输出与扣除初始俯仰角、横滚角带来重力加速度投影后的剩余分量。前两幅图给出了x、y加计的原始输出，可见由于存在初始俯仰角和横滚角(实验中θ₀＝0.091396°，γ₀＝0.10551°)，加计的输出存在明显的一倍频规律的波动，这种波动不是由旋转轴的不规律转动引起的，须按公式(5)扣除重力加速度在水平加计上的投影。后两幅图给出了扣除重力加速度投影后的x、y加计剩余分量，可见此时的剩余分量主要呈现出二倍频规律的波动，波动的幅值约100ug～150ug，根据公式(6)得到的俯仰角、横滚角补偿量的幅值约20″～30″。

图6为本发明理想旋转系和瞬时旋转系之间的转换关系图。图中OXs’Ys’Zs’为理想旋转系，表示理想转动情况下的旋转坐标系，OXsYsZs为瞬时旋转系，表示每个时刻真实的旋转坐标系，二者的关系可通过公式(6)给出的俯仰角和横滚角补偿信息δθ、δγ描述。根据图6的转换关系，理想旋转系与瞬时旋转系的转换矩阵可通过两次欧拉角转动给出，先绕x轴旋转δθ，再绕y轴旋转δγ，即可得到公式(14)给出的理想旋转系和瞬时旋转系之间的转换矩阵

图7为本发明具体实施例通过姿态输出补偿方法得到补偿后的姿态输出与原始未补偿姿态的对比图，具体的补偿算法由公式(7)～公式(12)给出。由图7可见，如果不进行姿态输出补偿，则该单轴旋转惯导系统输出的俯仰角和横滚角输出存在高倍频的波动，波动幅值约为20″～30″，而航向角的输出存在一倍频的波动，波动幅值约为100″～150″，俯仰角和横滚角的波动幅值与根据公式(6)得到的俯仰角、横滚角补偿量相一致，航向角的波动幅值与码盘角度误差幅值相一致，这也说明了单轴旋转惯导系统的码盘误差和旋转轴不规律转动确实是影响姿态输出精度的主要因素。而经过本发明提出的算法进行姿态输出补偿后，该单轴旋转惯导系统输出的俯仰角和横滚角波动幅值降至不到5″，航向角的波动幅值降至20″～30″，可见补偿后的姿态输出精度提高了约5倍，证明该单轴旋转惯导系统的姿态输出补偿方法十分有效，可大幅提升单轴旋转惯导系统的导航性能。

本发明未详细公开的部分属于本领域的公知技术。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (5)

1.一种单轴旋转惯导系统的姿态输出补偿方法，其特征在于包括如下步骤：

步骤(1)：在z陀螺输出的角速度中扣除地球自转角速度分量和陀螺漂移，经积分后得到角度；

步骤(2)：将z陀螺积分的角度与码盘输出的转角作差，对该差值进行拟合可构造航向角的补偿量；

步骤(3)：利用x、y加速度计的输出和初始对准得到的俯仰角、横滚角，扣除重力加速度投影，进而构造俯仰角和横滚角的补偿量；

步骤(4)：根据姿态输出补偿算法补偿单轴旋转惯导系统的姿态输出。

2.根据权利要求1所述的一种单轴旋转惯导系统的姿态输出补偿方法，其特征在于：所述步骤(1)z陀螺输出的角速度由公式(1)给出：

ω_P＝ω+ω_iesin L+ε_z

                       (1)

ω_N＝-ω+ω_iesin L+ε_z

式中，ω为单轴旋转惯导系统的旋转角速度，ω_P为正转时z陀螺输出的角速度，ω_N为反转时z陀螺输出的角速度，ω_ie为地球自转角速度，L为当地地理纬度，ε_z为z陀螺漂移；

将公式(1)中ω_P与ω_N相加除2后取平均，得到z陀螺输出中需要扣除的地球自转角速度分量和陀螺漂移之和由公式(2)给出：

$\widetilde{\mathrm{\ω}}=\mathrm{mean}\left(\frac{{\mathrm{\ω}}_P+{\mathrm{\ω}}_N}{2}\right)---\left(2\right)$

在z陀螺输出中扣除并积分，可得正转和反转时z陀螺积分角度与

将与合在一起即得到z陀螺积分的角度。

3.根据权利要求1所述的一种单轴旋转惯导系统的姿态输出补偿方法，其特征在于：所述步骤(2)中将z陀螺积分的角度与码盘输出的转角作差，对差值按公式(4)进行拟合：

根据拟合参数a、b、c可修正码盘输出的转角，作为航向角的补偿信息。

4.根据权利要求1所述的一种单轴旋转惯导系统的姿态输出补偿方法，其特征在于：所述步骤(3)中根据初始对准得到的俯仰角和横滚角，按公式(5)扣除x、y加速度计上的重力加速度投影：

式中，a_x、a_y为x、y加速度计的输出，g为当地重力加速度，θ₀、γ₀为初始对准得到的俯仰角和横滚角，a′_x、a′_y为扣除重力加速度投影后的剩余分量，进而通过公式(6)构造俯仰角和横滚角的补偿量：

$\begin{array}{c}\mathrm{\δ\θ}=\frac{a_y^{\′}}{g}\\ \mathrm{\δ\γ}=-\frac{a_x^{\′}}{g}\end{array}---\left(6\right)$

式中，δθ为俯仰角补偿信息，δγ为横滚角补偿信息。

5.根据权利要求1所述的一种单轴旋转惯导系统的姿态输出补偿方法，其特征在于：所述步骤(4)中的姿态输出补偿算法具体过程为：

根据陀螺的输出和上一解算周期的姿态矩阵按公式(7)和公式(8)计算

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

${\mathrm{\ω}}_{\mathrm{ns}}^s={\mathrm{\ω}}_{\mathrm{is}}^s-{\mathrm{\ω}}_{\mathrm{in}}^s---\left(8\right)$

式中， ${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T,$ 为地球自转角速度在导航坐标系中的投影； ${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-\frac{V_y}{R}& \frac{V_x}{R}& \frac{V_x}{R}\tan L\end{array}\right]}^T,$ 为位移角速度在导航坐标系中的投影，其中V_x、V_y为载体的东向速度和北向速度，R为地球半径；根据和四元数姿态微分方程即可实现姿态矩阵的更新；

根据步骤(2)得到的拟合模型，修正码盘的转角按公式(9)构建矩阵

根据步骤(3)得到的δθ、δγ，按公式(10)构建矩阵

$C_s^{s^{\′}}=\left[\begin{array}{ccc}1& 0& -\mathrm{\δ\γ}\\ 0& 1& \mathrm{\δ\θ}\\ \mathrm{\δ\γ}& -\mathrm{\δ\θ}& 1\end{array}\right]---\left(10\right)$

根据更新后的姿态矩阵和公式(9)给出的公式(10)给出的按公式(11)计算矩阵

$C_b^n=C_{s^{\′}}^n{C}_s^{s^{\′}}{C}_b^s---\left(11\right)$

根据矩阵和公式(12)得到单轴旋转惯导系统经过补偿后的输出姿态：

θ＝sin^-1C₂₃

$\mathrm{\γ}={\tan }^{-1}(-\frac{C_{31}}{C_{33}})---\left(12\right)$

$\mathrm{\ψ}={\tan }^{-1}(-\frac{C_{12}}{C_{22}})$

式中，θ为俯仰角，γ为横滚角，ψ为航向角，C_ij,i,j＝1,2,3为矩阵中的各元素。