一种惯性导航系统精对准方法与流程

技术特征：
1.一种惯性导航系统精对准方法，其特征在于，包括以下步骤：步骤一：初始化ins/gps组合系统；步骤二：采集惯性组件输出的角速度ω
b
、加速度f
b
和gps输出的位置p
gps
；步骤三：对步骤二中采集的ω
b
和f
b
进行导航解算，得到载体的位置速度姿态步骤四：构造卡尔曼滤波器，滤波状态量为：x＝[δp
t δv
t δφ
t δ
t ε
t
]
t
，其中δp为ins位置误差，δp＝[δp
x δp
y δp
z
]
t
，δv为速度误差，δv＝[δv
x δv
y δv
z
]
t
，δφ为姿态失准角，δφ＝[δφ
x δφ
y δφ
z
]
t
，δ为加速度计零偏，δ＝[δ
x δ
y δ
z
]
t
，ε为陀螺零偏，ε＝[ε
x ε
y ε
z
]
t
，以gps输出的位置p
gps
和步骤三中解算位置之差为观测量z(k)，进行卡尔曼滤波时间更新，计算状态一步预测阵x(k/k-1)和一步预测均方误差阵p(k/k-1)；步骤五：根据步骤四的z
k
和x(k/k-1)，计算k时刻新息二阶矩c(k)，即c(k)＝e((z(k)-hx(k/k-1))(z(k)-hx(k/k-1))
t
)步骤六：自适应方法选取：比较c(k)与hp(k/k-1)h
t
r(k-1)，同时比较k与m，当c(k)＝hp(k/k-1)h
t
r(k-1)，且k≥m时，则转至步骤七，否则转至步骤十，其中h表示量测矩阵，r(k-1)表示k-1时刻量测噪声阵；步骤七：计算(k-m 1)～k时刻m个点新息二阶矩的均值即其中，c(j)表示j时刻新息二阶矩；步骤八：根据步骤五计算的c(k)和步骤七计算的计算自适应因子α，即其中，tr表示对矩阵求迹；步骤九：根据步骤八中的α，计算k时刻r(k)，即：r(k)＝(1 α)r(k-1)计算完成后，转至步骤十二。步骤十：计算k时刻指数渐消记忆加权因子β(k)，即：其中，b表示遗忘因子；步骤十一：根据步骤四的z(k)和步骤十的β(k)，基于allan方差估计k时刻r(k)，即：计算完成后，转至步骤十二；步骤十二：进行卡尔曼滤波测量更新，计算k时刻滤波增益k(k)、估计均方误差p(k)和状态估计值x(k)；步骤十三：根据步骤十二中得到的状态估计值x(k)，对步骤三中得到的载体姿态进行补偿，得到载体姿态角φ；存储并输出载体姿态角。2.根据权利要求1所述的一种惯性导航系统精对准方法，其特征在于：步骤一所述初始
化ins/gps组合系统包括：初始化ins导航解算初值：载体的位置信息初始化ins导航解算初值：载体的位置信息λ0和h0分别为初始纬度、初始经度和初始高度，三维速度信息v
x0
、v
y0
、v
z0
，以及三个粗对准姿态角信息φ
x0
、φ
y0
、φ
z0
，初始转换矩阵初始化四元数q0；初始转换矩阵满足：其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；初始化四元数q0满足：其中，q0＝[q
00 q
10 q
20 q
30
]
t
，c
bnij
(i,j＝1,2,3)表示中第i行、第j列矩阵元素；初始化常值参量，包括新息二阶矩均值窗长m和遗忘因子b；初始化指数渐消记忆加权因子初值β0；初始化卡尔曼滤波器初值：状态变量初值x0＝[δp
t δv
t δφ
t δ
t ε
t
]
t
，均方误差阵p0，系统噪声方差阵q，量测噪声方差阵r0，量测阵h
k
，h
k
＝[ι3×
3 ο3×
3 ο3×
3 ο3×
3 ο3×3]，其中，ι表示单位矩阵；ο表示零矩阵。3.根据权利要求1所述的一种惯性导航系统精对准方法，其特征在于：步骤三所述对步骤二中采集的ω
b
和f
b
进行导航解算，得到载体的位置速度姿态包括：四元数姿态矩阵更新运算：其中，其中，表示ins解算纬度；ω
ie
表示地球自转角速度；
分别表示ins解算载体速度在导航系ox
n
轴、oy
n
轴轴上的分量；r
e
表示地球半径；t为采样时间；当k＝1时，q(1)为步骤1中的载体初始四元数；根据q(k)中元素q
i
(k)，i＝0,1,2,3，更新捷联矩阵(k)，i＝0,1,2,3，更新捷联矩阵其中，上式中的q
i
，i＝0,1,2,3为式(9)中q
i
(k)(i＝0,1,2,3)；更新姿态信息：其中，表示中第i行、第j列矩阵元素；则姿态中第i行、第j列矩阵元素；则姿态和分别表示横滚角、俯仰角、航向角解算值；速度更新运算：其中，g
n
＝[0 0
ꢀ‑
g]
t
，g表示当地重力加速度，速度，g表示当地重力加速度，速度和分别表示解算载体速度在导航系ox
n
轴、oy
n
轴、oz
n
轴上的分量；当k＝1时，为步骤一中初始化系统时获得的载体初始速度。位置更新计算：位置位置表示解算载体纬度，表示解算载体经度，表示解算载体高度；当t＝1时，为步骤一中载体初始位置。
4.根据权利要求1所述的一种惯性导航系统精对准方法，其特征在于：步骤四所述计算状态一步预测阵x(k/k-1)和一步预测均方误差阵p(k/k-1)具体为：x(k/k-1)＝f(k/k-1)x(k-1)其中，x(k/k-1)表示状态一步预测阵；f(k/k-1)表示k-1时刻到k时刻的状态转移矩阵；x(k-1)表示k-1时刻状态估计值；p(k/k-1)＝f(k/k-1)p(k-1)f
t
(k/k-1) q其中，p(k/k-1)表示一步预测均方误差阵；p(k-1)表示k-1时刻状态估计均方误差；q表示系统噪声。5.根据权利要求1所述的一种惯性导航系统精对准方法，其特征在于：步骤十二所述进行卡尔曼滤波测量更新，计算k时刻滤波增益k(k)、估计均方误差p(k)和状态估计值x(k)具体为：滤波增益k(k)满足：k(k)＝p(k,k-1)h
t
(hp(k,k-1)h
t
r(k))估计均方误差p(k)满足：p(k)＝(i
15
×
15-k(k)h)p(k/k-1)状态估计值x(k)满足：x(k)＝x(k/k-1) k(k)(z(k)-hx(k/k-1))其中，x(k)表示k时刻状态估计值。6.根据权利要求1所述的一种惯性导航系统精对准方法，其特征在于：步骤十三所述根据步骤十二中得到的状态估计值x(k)，对步骤三中得到的载体姿态进行补偿，得到载体姿态角φ具体为：对步骤三中得到的四元数进行补偿：根据上式中元素，计算更新捷联矩阵更新姿态信息：其中，c
ij
(i,j＝1,2,3)表示中第i行、第j列矩阵元素；，φ
i
(i＝x,y,z)分别表示横滚角、俯仰角、航向角解算值。

技术总结
本发明公开了一种惯性导航系统精对准方法，包括初始化INS/GPS系统；采集INS和GPS的输出数据；对惯性组件输出进行导航解算，以位置误差为观测量，INS导航误差为状态量，进行卡尔曼滤波时间更新；根据时间更新结果计算新息二阶矩，与判决条件进行比较，当满足判决条件时，使用自适应因子计算k时刻量测噪声阵，否则，基于Allan方差估计k时刻量测噪声阵；依据k时刻量测噪声阵进行卡尔曼滤波量测更新，对INS导航误差进行估计，补偿INS失准角，提高初始对准精度。本发明解决了受工况影响测量噪声发生变化时姿态角估计不准确问题，消除了由变化测量噪声引起的初始对准误差，提高INS定位精度。提高INS定位精度。提高INS定位精度。


技术研发人员：王秋滢 刘凯悦 黄平宇 孙大军 张殿伦 张硕
受保护的技术使用者：哈尔滨工程大学
技术研发日：2021.11.25
技术公布日：2022/3/22