CN107270905B - 一种静基座下惯性平台连续翻滚自标定自对准方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，特别涉及一种静基座下惯性平台连续翻滚自标定自对准方法。本发明的方法首先以惯性器件输入轴为基准建立了系统坐标系，其次基于惯性平台工作原理，以惯性平台姿态角为中间量建立系统动力学模型和观测模型，然后通过可观性分析设计惯性平台自标定自对准所需的平台加矩方案，最后选用平台姿态角及平台各项误差系数作为系统状态量，通过降维容积Kalman滤波实现对惯性平台的自主标定与对准。本发明提供的方法能够改变惯性平台现有的标定和对准模式，简化了惯性平台自标定和自对准的过程，消弱了系统标定与对准之间的强耦合作用，为提高惯性平台实际使用精度提供了基础理论和技术支撑。

Description

一种静基座下惯性平台连续翻滚自标定自对准方法

技术领域

发明属于惯性导航技术领域，特别涉及一种静基座下惯性平台连续翻滚自标定自对准方法。

背景技术

平台式惯性导航系统(又称惯性平台)具有自主性强、抗干扰能力强、短时精度高以及导航频率高等其他导航系统难以比拟的优点。因此，在卫星导航系统取得厘米级精度的情况下，高精度的惯性导航系统仍作为某些运载体(如飞机、火箭、航天飞机、潜艇等)的核心导航系统。然而，作为一种递推式自主导航系统，惯性平台导航系统在使用前需要完成两项准备工作，即标定和初始对准，而标定和对准的误差将会累积到系统的导航误差中，且会随导航时间推移逐渐变大。因此，先进的惯性平台自标定和自对准技术是快速提高惯性平台导航系统性能的低成本、高效率手段。正因如此，高性能的惯性平台自标定自对准技术一直是国内外的研究热点。

在众多惯性平台导航系统自标定自对准方法之中，连续翻滚自标定自对准技术是一种适用于惯性平台导航系统的高精度、低成本的自标定和自对准技术。公开文献表明，现有的惯性平台连续翻滚自标定自对准技术均是基于失准角模型和框架角模型进行研究的。然而失准角模型则需要系统首先满足小角度假设条件，这在现实中很难保证，特别是在滤波过程中，需要实时对系统进行补偿，对系统的计算能力要求较为苛刻。框架角模型则会引入框架角观测误差、框架安装误差、平台基座安装误差等，降低了标定和对准的精度。此外，框架角系统模型具有一定局限性，不适用于全姿态三框架四轴等惯性平台导航系统。

发明内容

本发明的目的，就是针对上述问题，提出一种惯性平台静基座下连续翻滚自标定自对准方法。该方法将惯性平台的对准与标定视为整体，采用姿态角作为系统中间状态量，建立系统模型，从而从根源上避免了框架角或失准角模型导致系统可能奇异的弊端。其次，针对系统非线性、高维的特点，本发明中采用降维容积Kalman滤波(Reduced CubatureKalman Filter，RDCKF)算法，在保证系统精度的同时大幅度的降低了计算量。利用该方法可以大幅度的提高惯性平台导航系统的导航精度，具有较强的经济效益和工程指导意义。

本发明的连续翻滚自标定自对准方法步骤如下：第一步，系统坐标系建立；第二步，系统建模；第三步，惯性平台连续翻滚加矩方案设计；第四步，惯性平台连续翻滚自标定自对准算法。

本发明的技术方案是：一种静基座下惯性平台连续翻滚自标定自对准方法，其特征在于，包括以下步骤：

S1、建立系统坐标系，具体为：

如图1和图2所示，根据惯性平台与惯性仪表之间的装配关系，所述惯性仪表包括单自由度三浮陀螺仪Gx、Gy、Gz以及石英加速度计Qx、Qy、Qz，定义如下坐标系：

平台坐标系p系：取平台几何中心O为原点，OX_p轴与Qx石英加速度计敏感轴平行，OY_p轴平行于Qx和Qy石英加速度计敏感轴所确定的平面，并与OX_p轴垂直，OZ_p轴与OX_p轴和OY_p轴构成右手坐标系；

单自由度陀螺仪坐标gi系，i＝x,y,z：坐标轴分别与该陀螺仪的输入轴I、输出轴O和自转轴S平行；

陀螺仪敏感轴坐标sg系：坐标轴与三个单自由度陀螺仪的输入轴方向一致，由于陀螺仪安装误差角的存在，该坐标系为非正交坐标系；

单自由度加速度计坐标ai系，i＝x,y,z：原点O与平台中心重合，坐标系各轴分别与石英加速度计i的输入轴I、摆轴P和输出轴O平行；

加速度计敏感轴坐标sa系：原点O与平台中心重合，坐标轴与三个石英加速度计的输入轴方向一致，由于加速度计安装误差角的存在，该坐标系为非正交坐标系；

计算平台坐标c系：将计算机所建立的数字平台坐标系定义为计算平台坐标系，由于初始对准误差、安装误差、陀螺仪漂移等误差因素，使得该坐标系与p系并不重合，存在小角度偏差；

导航坐标n系：选择当地地理系作为导航坐标系，即北天东坐标系；

惯性坐标i系：选择导航初始时刻的当地地理系作为惯性坐标系；

令平台系p至导航系n的欧拉角分别为γ、β和α，对应的转序为3-2-1转序，二者之间的方向余弦矩阵可表示为

S2、系统建模，包括：

S21、建立惯性仪表误差模型：

对Gx陀螺仪，单自由度三浮积分陀螺仪漂移表示为：

ε_x＝D_Fx+D_Ixa_gxI+D_Oxa_gxO+D_Sxa_gxS+w_x (公式2)

其中DFx为陀螺仪零偏，DIx、DOx和DSx为Gx陀螺仪一次项系数，静基座下，有

其中gn为重力矢量在导航系下投影，为平台坐标系到Gx陀螺仪坐标系的姿态矩阵；同理可得出Gy、Gz陀螺仪的漂移模型；

对Qx加速度计，单自由度石英加速度输出方程可表示为

其中k_a0x为加速度计常值零偏，k_a1x和分别为加速度计刻度因子误差系数和刻度因子不对称项系数，υ_x为加速度计测量噪声，a_axI为加速度计I轴方向比力，静基座下，有

其中为平台坐标系到Qx加速度计坐标系的姿态阵，同理可得出Qy、Qz加速度计的输出方程；

根据陀螺力矩器的线性偏差，其误差模型可表示为：

ω_out＝(I₃+△K_G)ω_cmd,

其中ΔkGx、ΔkGy和ΔkGz分别表示三个陀螺仪的力矩误差系数；

S22、建立惯性仪表安装误差模型：

假设惯性器件的安装误差角均满足小角度假设，p系至sa与sg系的姿态矩阵可分别写为

其中θ与Δ分别记为加速度计与陀螺仪安装误差角，下标ij，i＝x,y,z，j＝I,O,S,P；表示i方向上的惯性器件沿j轴的安装误差；

S23、建立系统模型：

令系统状态量为其中θ_A为平台姿态角向量，K_g0为由三个陀螺仪零偏组成的向量，K_g1为由九个陀螺仪一次项组成的向量，△K_G为由三个陀螺力矩误差系数组成的向量，Δg为由六个陀螺仪安装误差角组成的向量，K_a0为由三个加速度计零偏组成的向量，K_a1为由三个加速度计刻度因子误差项组成的向量，θ_a为由三个加速度计安装误差角组成的向量，为由三个加速度计刻度因子不对称项组成的向量；

忽略平台各误差系数之间的乘积，所建立的系统模型为：

其中：

S3、惯性平台连续翻滚加矩：

惯性平台连续翻滚加矩方案中平台相对导航系角速度指令设计如下：

S31、平台归零，即

S32、台体以0.1°/s相对地理系绕南旋转1800s；

S33、台体以0.1°/s相对地理系绕东旋转900s；

S34、台体以0.1°/s相对地理系绕南旋转1800s；

S35、台体以0.1°/s相对地理系绕南旋转900s；

S36、台体以0.1°/s相对地理系绕南旋转900s；

S37、平台跟踪地理系，相对静止600s，即

S38、台体以0.1°/s相对地理系绕南旋转900s；

S39、平台跟踪地理系，相对静止600s，即

S310、台体以0.1°/s相对地理系绕东旋转900s；

S311、平台跟踪地理系，相对静止600s，即

S312、台体以0.1°/s相对地理系绕南旋转900s；

S313、平台跟踪地理系，相对静止600s，即

S314、台体以0.1°/s相对地理系绕南旋转900s；

S315、平台跟踪地理系，相对静止600s，即

S316、台体以0.1°/s相对地理系绕东旋转900s；

S317、平台跟踪地理系，相对静止600s，即

整个方案共计旋转3圈，需14400s(4h)；

根据上述步骤，获得平台施矩指令角速度ω_cmd：

其中为导航系统下地球自转角速度，为计算平台坐标系相对导航坐标系的姿态阵，且满足：

表示的反对称矩阵；

图3给出了惯性平台加矩指令角速度曲线的示意图；

S4、惯性平台连续翻滚自标定自对准：

假设tk-1时刻系统状态量的统计特性为令系统维数为n，θ_A的维数为3，为P_k-1的前3×3部分，为P_k-1的前(n-3)×(n-3)部分，单步降维容积Kalman滤波算法可表示为：

其中：

状态量预测更新方程：

量测量预测更新方程方程，利用与P_k/k-1计算Sigma点ξ_i,k/k-1，W_i ^m与W_i ^c保持不变：

χ_i,k/k-1＝h_k(ξ_i,k/k-1)+r_k-1,i＝1,2,…,2n (公式21)

滤波器更新方程：

当滤波步数k取到终点时，即可获得状态量X，即θ_A和ξ的估计值，同时实现了系统的初始对准和误差系数的标定。

经过上述四步可最终建立一种惯性平台静基座下连续翻滚自标定自对准方法。该方法具有“精度高、计算速度快、方法误差小”等特征，能够满足高精度惯性平台自标定自对准任务需求。

本发明的有益效果为，本发明的方法以平台姿态角作为中间量，建立了系统模型，利用降维容积Kalman滤波技术，实现了惯性平台快速高精度的自标定与自对准，提高了惯性平台的导航精度，具有很强的经济效益和工程指导意义。

附图说明

图1为惯性平台几何结构示意图

图2为惯性仪表装配示意图

图3为惯性平台加矩指令角速度曲线示意图

图4为姿态角α跟踪误差曲线

图5为姿态角β跟踪误差曲线

图6为姿态角γ跟踪误差曲线

图7为DFX滤波曲线与真值曲线示意图

图8为DFY滤波曲线与真值曲线示意图

图9为DFZ滤波曲线与真值曲线示意图

图10为DIX滤波曲线与真值曲线示意图

图11为DIY滤波曲线与真值曲线示意图

图12为DIZ滤波曲线与真值曲线示意图

图13为DOX滤波曲线与真值曲线示意图

图14为DOY滤波曲线与真值曲线示意图

图15为DOZ滤波曲线与真值曲线示意图

图16为DSX滤波曲线与真值曲线示意图

图17为DSY滤波曲线与真值曲线示意图

图18为DSZ滤波曲线与真值曲线示意图

图19为ka0x滤波曲线与真值曲线示意图

图20为ka0y滤波曲线与真值曲线示意图

图21为ka0z滤波曲线与真值曲线示意图

图22为ka1x滤波曲线与真值曲线示意图

图23为ka1y滤波曲线与真值曲线示意图

图24为ka1z滤波曲线与真值曲线示意图

图25为ΔxS滤波曲线与真值曲线示意图

图26为ΔxO滤波曲线与真值曲线示意图

图27为ΔyS滤波曲线与真值曲线示意图

图28为ΔyO滤波曲线与真值曲线示意图

图29为ΔzS滤波曲线与真值曲线示意图

图30为ΔzO滤波曲线与真值曲线示意图

图31为θyO滤波曲线与真值曲线示意图

图32为θzP滤波曲线与真值曲线示意图

图33为θzO滤波曲线与真值曲线示意图

图34为ΔKGx滤波曲线与真值曲线示意图

图35为ΔKGy滤波曲线与真值曲线示意图

图36为ΔKGz滤波曲线与真值曲线示意图

图37为滤波曲线与真值曲线示意图

图38为滤波曲线与真值曲线示意图

图39为滤波曲线与真值曲线示意图

具体实施方式

下面结合附图和实施例，详细描述本发明的技术方案：

仿真中加矩指令生成周期为0.1s，滤波周期为1s，加速度计测量噪声为0.1μg，陀螺仪测量噪声为1×10^-4°/h，滤波器姿态角初值为粗对准结果，初始对准偏差为0.02°，其余初值为零，Q与R根据惯性仪表噪声特性设置，P矩阵设置为对角阵。其中各状态量初始方差如下：姿态角为1×10^-7，陀螺仪零偏为1×10^-12，陀螺仪一次项为1×10^-14，加速度计零偏为1×10^-8，加速度计刻度因子误差系数为1×10^-8，陀螺仪安装误差为1×10^-8，加速度计安装误差为1×10^-8，陀螺力矩器误差系数为1×10^-8，加速度计零偏不对称项为1×10^-9，加速度计刻度因子不对称项为1×10^-9，系统状态量如下表所示：

表1仿真中状态量真值

仿真环境为Window 7(64位)操作系统，计算软件采用Matlab7.10.0，仿真计算机处理器为Intel(R)Core(TM)i5-3470，主频为3.20GHz，内存为3.39GB。

具体步骤同第一步至第四步。

图4-图6描述了平台三个姿态角的滤波跟踪误差曲线，图7-图39描述了平台各项误差系数的滤波曲线示意图，其中实线表示真值，虚线表示滤波值。为了方便观测，对滤波曲线的坐标轴进行了适当的调整。

从图4-图6中可以看出三个姿态角均能较好地跟踪至真实的姿态角，意味着滤波器能够实现平台的自瞄准。图7-图17描述了陀螺仪零偏和一次项系数滤波曲线示意图，从中可知陀螺仪零偏与一次项系数能够快速收敛至真值，但陀螺仪本轴一次项误差系数大致需要5000s收敛，相对于其他一次项系数较慢，这与第三章中陀螺仪本轴一次项可观测度较差的理论分析结果一致。图19-图24与图37-图39分别为加速度计零偏、刻度因子误差和刻度因子不对称项滤波曲线，与其他状态量相比，加速度计各项误差系数收敛速度较快，这与加速度可观测度较高有关。图25-图33为陀螺仪和加速度计各项安装误差角滤波曲线，可以看出加速度计安装误差角收敛速度快于陀螺仪安装误差角。图34-图36为陀螺力矩器误差系数滤波曲线，可知陀螺仪力矩误差系数收敛速度与陀螺仪安装误差角相似，这与第三章中的结论一致。

取滤波数据最后100个点的平均值作为滤波结果，结果如表2所示：

表2仿真案例1中状态量估计偏差

从表2的仿真结果中可以看出，平台水平姿态角估计偏差小于5×10^-6°，方位角估计偏差小于1×10^-4°，陀螺仪零偏估计偏差小于2×10^-5°/h，一次项估计偏差小于5×10^-5°/h，加速度计零偏与刻度因子及其不对称项估计偏差小于0.06μg，惯性仪表安装误差角估计偏差小于0.05"，陀螺力矩误差系数小于0.1ppm。

上述仿真结果本发明所提出的惯性平台连续翻滚加矩方案和姿态角RDCKF算法的正确性和有效性。

此外，利用MATLAB对单次CKF与RDCKF滤波所用时间进行统计，结果如下。

表3 RDCKF与CKF仿真运算平均时间

从表3中可以看出，RDCKF单次仿真需要47.5675s左右，而CKF单次仿真运算需要95.3227s，相对CKF算法而言，RDCKF滤波能够减少50％的运算时间。

综合上述仿真结果可获得以下结论：

(1)本发明提出的惯性平台连续翻滚自标自对准算法，能够同时实现惯性平台自对准和平台33项误差系数的标定；

(2)本发明确定的方法能够实现水平姿态角小于5×10^-6°，方位角小于1×10^-4°的初始对准，以及陀螺仪零偏小于2×10^-5°/h，陀螺仪一次项小于5×10^-5°/h，加速度计零偏与刻度因子及其不对称项小于0.06μg，惯性仪表安装误差角小于0.05"，陀螺力矩系数小于0.1ppm的标定精度；

(3)由本发明确定的方法进行惯性平台标定与对准，其计算速度和效率较CKF算法提高了一倍；

本发明有助于加深对惯性平台标定和对准机理的认识和建立补偿机制，具有广阔的工程应用前景。

需要补充说明的是：

(1)本发明中惯性平台不限于框架式惯性平台，该项技术也同样适用于浮球式惯性平台；

(2)在惯性平台施矩方案(即第三步)中，所谓的“东”与“南”主要指水平面内两个相互垂直的方向(发明中为了方便描述)，对具体指向不做要求；

(3)值得注意的是，本发明中的平台施矩方案不一定是最优的，其他施矩方案或许亦能满足需求，原则上而言，凡是可观的施矩方案都是可行的。

同传统方法相比，本发明的优点在于以下几个方面：

(1)首次以惯性器件输入轴为基准定义了平台坐标系，建立系统模型。避免了因坐标系定义而导致系统不可观的问题；

(2)将惯性平台的标定和对准视为一个问题考虑。避免了传统方法中，惯性平台标定与对准之间强耦合引起的标定和对准的精度误差；

(3)以惯性平台姿态角作为出发点，建立系统模型，并在静基座条件下推导了惯性平台连续翻滚自标定自对准算法，为惯性平台自标定和自对准提供了一种新的思路。姿态角的使用使得该方法避免了框架角和失准角模型的不足，有效的提高了算法的精度；

(4)在参数辨识中采用了RDCKF技术。同EKF相比，RDCKF更适用于系统非线性的特征；同CKF相比，RDCKF更适合系统高维的特征。与此同时，降维算法的使用使得系统状态量预测更新步骤中Sigma点采样点数由2n减少为6个，极大程度上减少了滤波器的计算量，缩短了单步滤波运行时间。因此，该项技术的使用，能在提高系统精度的同时，大幅度的减少了系统计算量。

Claims (1)

1.一种静基座下惯性平台连续翻滚自标定自对准方法，其特征在于，包括以下步骤：

S1、建立系统坐标系，具体为：

根据惯性平台与惯性仪表之间的装配关系，所述惯性仪表包括单自由度三浮陀螺仪Gx、Gy、Gz以及石英加速度计Qx、Qy、Qz，定义如下坐标系：

平台坐标系p系：取平台几何中心O为原点，OX_p轴与Qx石英加速度计敏感轴平行，OY_p轴平行于Qx和Qy石英加速度计敏感轴所确定的平面，并与OX_p轴垂直，OZ_p轴与OX_p轴和OY_p轴构成右手坐标系；

单自由度陀螺仪坐标gi系，i＝x,y,z：坐标轴分别与该陀螺仪的输入轴I、输出轴O和自转轴S平行；

陀螺仪敏感轴坐标sg系：坐标轴与三个单自由度陀螺仪的输入轴方向一致，由于陀螺仪安装误差角的存在，该坐标系为非正交坐标系；

单自由度加速度计坐标ai系，i＝x,y,z：原点O与平台中心重合，坐标系各轴分别与石英加速度计i的输入轴I、摆轴P和输出轴O平行；

加速度计敏感轴坐标sa系：原点O与平台中心重合，坐标轴与三个石英加速度计的输入轴方向一致，由于加速度计安装误差角的存在，该坐标系为非正交坐标系；

计算平台坐标c系：将计算机所建立的数字平台坐标系定义为计算平台坐标系，由于初始对准误差、安装误差、陀螺仪漂移误差因素，使得该坐标系与p系并不重合，存在小角度偏差；

导航坐标n系：选择当地地理系作为导航坐标系，即北天东坐标系；

惯性坐标i系：选择导航初始时刻的当地地理系作为惯性坐标系；

令平台系p至导航系n的欧拉角分别为γ、β和α，对应的转序为3-2-1转序，二者之间的方向余弦矩阵可表示为

S2、系统建模，包括：

S21、建立惯性仪表误差模型：

对Gx陀螺仪，单自由度三浮积分陀螺仪漂移表示为：

ε_x＝D_Fx+D_Ixa_gxI+D_Oxa_gxO+D_Sxa_gxS+w_x(公式2)

其中D_Fx为陀螺仪零偏，D_Ix、D_Ox和D_Sx为Gx陀螺仪一次项系数，静基座下，有

其中gⁿ为重力矢量在导航系下投影，为平台坐标系到Gx陀螺仪坐标系的姿态矩阵；同理可得出Gy、Gz陀螺仪的漂移模型；

对Qx加速度计，单自由度石英加速度输出方程可表示为

其中k_a0x为加速度计常值零偏，k_a1x和分别为加速度计刻度因子误差系数和刻度因子不对称项系数，υ_x为加速度计测量噪声，a_axI为加速度计I轴方向比力，静基座下，有

其中为平台坐标系到Qx加速度计坐标系的姿态阵，同理可得出Qy、Qz加速度计的输出方程；

根据陀螺力矩器的线性偏差，其误差模型可表示为：

ω_out＝(I₃+△K_G)ω_cmd,

其中△k_Gx、△k_Gy和△k_Gz分别表示三个陀螺仪的力矩误差系数；

S22、建立惯性仪表安装误差模型：

假设惯性器件的安装误差角均满足小角度假设，p系至sa与sg系的姿态矩阵可分别写为

其中θ与Δ分别记为加速度计与陀螺仪安装误差角，下标ij，i＝x,y,z，j＝I,O,S,P；表示i方向上的惯性器件沿j轴的安装误差；

S23、建立系统模型：

令系统状态量为其中θ_A为平台姿态角向量，K_g0为由三个陀螺仪零偏组成的向量，K_g1为由九个陀螺仪一次项组成的向量，△K_G为由三个陀螺力矩误差系数组成的向量，Δg为由六个陀螺仪安装误差角组成的向量，K_a0为由三个加速度计零偏组成的向量，K_a1为由三个加速度计刻度因子误差项组成的向量，θ_a为由三个加速度计安装误差角组成的向量，为由三个加速度计刻度因子不对称项组成的向量；

忽略平台各误差系数之间的乘积，所建立的系统模型为：

其中：

S3、惯性平台连续翻滚加矩：

惯性平台连续翻滚加矩方案中平台相对导航系角速度指令设计如下：

S31、平台归零，即

S32、台体以0.1°/s相对地理系绕南旋转1800s；

S33、台体以0.1°/s相对地理系绕东旋转900s；

S34、台体以0.1°/s相对地理系绕南旋转1800s；

S35、台体以0.1°/s相对地理系绕南旋转900s；

S36、台体以0.1°/s相对地理系绕南旋转900s；

S37、平台跟踪地理系，相对静止600s，即

S38、台体以0.1°/s相对地理系绕南旋转900s；

S39、平台跟踪地理系，相对静止600s，即

S310、台体以0.1°/s相对地理系绕东旋转900s；

S311、平台跟踪地理系，相对静止600s，即

S312、台体以0.1°/s相对地理系绕南旋转900s；

S313、平台跟踪地理系，相对静止600s，即

S314、台体以0.1°/s相对地理系绕南旋转900s；

S315、平台跟踪地理系，相对静止600s，即

S316、台体以0.1°/s相对地理系绕东旋转900s；

S317、平台跟踪地理系，相对静止600s，即

根据上述步骤，获得平台施矩指令角速度ω_cmd：

其中为导航系统下地球自转角速度，为计算平台坐标系相对导航坐标系的姿态阵，且满足：

表示的反对称矩阵；

S4、惯性平台连续翻滚自标定自对准：

假设tk-1时刻系统状态量的统计特性为令系统维数为n，θ_A的维数为3，为P_k-1的前3×3部分，为P_k-1的前(n-3)×(n-3)部分，单步降维容积Kalman滤波算法可表示为：

其中：

状态量预测更新方程：

观测量预测更新方程，利用与P_k/k-1计算Sigma点ξ_i,k/k-1，W_i ^m与W_i ^c保持不变：

χ_i,k/k-1＝h_k(ξ_i,k/k-1)+r_k-1,i＝1,2,…,2n (公式21)

滤波器更新方程：

当滤波步数k取到终点时，即可获得状态量X，即θ_A和ξ的估计值，同时实现了系统的初始对准和误差系数的标定。