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

1.本发明属于惯性技术领域，涉及一种惯性导航系统精对准方法，特别是一种基于新息检测进行自适应方法调整的惯性导航系统精对准方法。


背景技术：

2.初始对准是为惯性导航系统(inertial navigation system，ins)提供初始姿态角，对准精度和对准时间是影响ins系统后续工作性能的两项重要指标。初始对准分粗对准和精对准两个阶段：粗对准阶段粗略估计初始姿态角；精对准在粗对准基础上提高对准精度，因此初始对准最终精度主要与精对准精度有关。基于gps辅助的ins精对准方法以gps位置信息为基准，获取ins位置误差，根据ins误差模型，通过卡尔曼滤波对ins姿态角校正。然而，受工况影响，ins测量噪声统计特性随时间发生变化，导致卡尔曼滤波的量测噪声阵不适用，影响卡尔曼滤波性能，降低ins失准角的估计精度，进而影响ins初始对准精度。
3.授权号为cn107479076b的中国发明专利在2017年8月8日公开的《一种动基座下联合滤波初始对准方法》中，提出了一种根据失准角判决门限实现无迹卡尔曼滤波和卡尔曼滤波算法切换的ins初始对准方法，缩短了对准时间。公开号为cn112857398a的中国发明专利在2021年05月28日公开的《一种系泊状态下舰船的快速初始对准方法》中，对初始对准误差模型进行了改进，利用四元数无味估计器进行滤波估计，提升对准精度。《武汉大学学报
·
信息科学版》期刊2020年45卷7期，由查峰等人撰写的《一种改进的捷联惯导参数辨识初始对准方法》一文中，采用泰勒展开方式对地球自转角的三角函数进行高阶近似，减少因地球自转角的正弦函数一阶近似对参数辨识模型的影响，提高初始对准精度。《中国惯性技术学报》期刊2020年28卷6期，由徐博等人撰写的《基于粒子群优化的舰船捷联惯导初始对准方法》一文中，提出了一种基于惯性系的粒子群算法优化方法，降低舰船基座摇摆对对准性能影响，提高航向角误差收敛速度，缩短对准时间。《ieee transactions on instrumentation and measurement》期刊2020年69卷10期，由yusen lin等人撰写的《an improved mcmc-based particle filter for gps-aided sins in-motion initial alignment》一文中，提出了一种改进的基于马尔科夫链蒙特卡罗的粒子滤波算法，通过避免粒子滤波中的粒子退化和粒子贫化现象，提高滤波精度，减少计算量，缩短对准时间。《ieee transactions on instrumentation and measurement》期刊2020年70卷，由xiaoren zhou等人撰写的《a robust quaternion kalman filter method for mimu/gps in-motion alignment》一文中，提出了一种新的鲁棒四元数卡尔曼滤波算法，消除gps异常值对对准精度的影响。以上技术都通过改进滤波模型或改变滤波方法的角度，提高初始对准精度、缩短对准时间，增加了算法的复杂度。


技术实现要素：

4.针对上述现有技术，本发明要解决的技术问题是提供一种基于新息检测进行自适应方法调整的惯性导航系统精对准方法，解决测量噪声发生变化情况下的姿态角估计不准
确问题,消除由变化测量噪声引起的初始对准误差。
5.为解决上述技术问题，本发明的一种惯性导航系统精对准方法，包括以下步骤：
6.步骤一：初始化ins/gps组合系统；
7.步骤二：采集惯性组件输出的角速度ωb、加速度fb和gps输出的位置p
gps
；
8.步骤三：对步骤二中采集的ωb和fb进行导航解算，得到载体的位置速度姿态
9.步骤四：构造卡尔曼滤波器，滤波状态量为：x＝[δp
t δv
t δφ
t δ
t ε
t
]
t
，其中δp为ins位置误差，δp＝[δp
x δp
y δpz]
t
，δv为速度误差，δv＝[δv
x δv
y δvz]
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)；
[0010]
步骤五：根据步骤四的zk和x(k/k-1)，计算k时刻新息二阶矩c(k)，即
[0011]
c(k)＝e((z(k)-hx(k/k-1))(z(k)-hx(k/k-1))
t
)
[0012]
步骤六：自适应方法选取：比较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时刻量测噪声阵；
[0013]
步骤七：计算(k-m 1)～k时刻m个点新息二阶矩的均值即
[0014][0015]
其中，c(j)表示j时刻新息二阶矩；
[0016]
步骤八：根据步骤五计算的c(k)和步骤七计算的计算自适应因子α，即
[0017][0018]
其中，tr表示对矩阵求迹；
[0019]
步骤九：根据步骤八中的α，计算k时刻r(k)，即：
[0020]
r(k)＝(1 α)r(k-1)
[0021]
计算完成后，转至步骤十二。
[0022]
步骤十：计算k时刻指数渐消记忆加权因子β(k)，即：
[0023][0024]
其中，b表示遗忘因子；
[0025]
步骤十一：根据步骤四的z(k)和步骤十的β(k)，基于allan方差估计k时刻r(k)，即：
[0026][0027]
计算完成后，转至步骤十二；
[0028]
步骤十二：进行卡尔曼滤波测量更新，计算k时刻滤波增益k(k)、估计均方误差p(k)和状态估计值x(k)；
[0029]
步骤十三：根据步骤十二中得到的状态估计值x(k)，对步骤三中得到的载体姿态进行补偿，得到载体姿态角φ；存储并输出载体姿态角。
[0030]
进一步的，步骤一中初始化ins/gps组合系统包括：
[0031]
初始化ins导航解算初值：载体的位置信息λ0和h0分别为初始纬度、初始经度和初始高度，三维速度信息v
x0
、v
y0
、v
z0
，以及三个粗对准姿态角信息φ
x0
、φ
y0
、φ
z0
，初始转换矩阵初始化四元数q0；
[0032]
初始转换矩阵满足：
[0033][0034]
其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；
[0035]
初始化四元数q0满足：
[0036][0037]
其中，q0＝[q
00 q
10 q
20 q
30
]
t
，c
bnij
(i,j＝1,2,3)表示中第i行、第j列矩阵元素；
[0038]
初始化常值参量，包括新息二阶矩均值窗长m和遗忘因子b；
[0039]
初始化指数渐消记忆加权因子初值β0；
[0040]
初始化卡尔曼滤波器初值：状态变量初值x0＝[δp
t δv
t δφ
t δ
t ε
t
]
t
，均方误差阵p0，系统噪声方差阵q，量测噪声方差阵r0，量测阵hk，hk＝[ι3×
3 ο3×
3 ο3×
3 ο3×
3 ο3×3]，其中，ι表示单位矩阵；ο表示零矩阵。
[0041]
进一步的，步骤三中对步骤二中采集的ωb和fb进行导航解算，得到载体的位置速度姿态包括：
[0042]
四元数姿态矩阵更新运算：
[0043][0044]
其中，表示ins解算纬度；ω
ie
表示地球自转角速度；分别表示ins解算载体速度在导航系oxn轴、oyn轴轴上的分量；re表示地球半径；t为采样时间；当k＝1时，q(1)为步骤1中的载体初始四元数；
[0045]
根据q(k)中元素qi(k)，i＝0,1,2,3，更新捷联矩阵
[0046][0047]
其中，上式中的qi，i＝0,1,2,3为式(9)中qi(k)(i＝0,1,2,3)；
[0048]
更新姿态信息：
[0049][0050]
其中，i,j＝1,2,3表示中第i行、第j列矩阵元素；则姿态中第i行、第j列矩阵元素；则姿态和分别表示横滚角、俯仰角、航向角解算值；
[0051]
速度更新运算：
[0052][0053]
其中，gn＝[0 0
ꢀ‑
g]
t
，g表示当地重力加速度，速度和分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量；当k＝1时，为步骤一中初始化系统时获得的载体初始速度；
[0054]
位置更新计算：
[0055][0056]
位置表示解算载体纬度，表示解算载体经度，表示解算载体高度；当t＝1时，为步骤一中载体初始位置。
[0057]
进一步的，步骤四中计算状态一步预测阵x(k/k-1)和一步预测均方误差阵p(k/k-1)具体为：
[0058]
x(k/k-1)＝f(k/k-1)x(k-1)
[0059]
其中，x(k/k-1)表示状态一步预测阵；f(k/k-1)表示k-1时刻到k时刻的状态转移矩阵；x(k-1)表示k-1时刻状态估计值；
[0060]
p(k/k-1)＝f(k/k-1)p(k-1)f
t
(k/k-1) q
[0061]
其中，p(k/k-1)表示一步预测均方误差阵；p(k-1)表示k-1时刻状态估计均方误差；q表示系统噪声。
[0062]
进一步的，步骤十二中进行卡尔曼滤波测量更新，计算k时刻滤波增益k(k)、估计均方误差p(k)和状态估计值x(k)具体为：
[0063]
滤波增益k(k)满足：
[0064]
k(k)＝p(k,k-1)h
t
(hp(k,k-1)h
t
r(k))
[0065]
估计均方误差p(k)满足：
[0066]
p(k)＝(i
15
×
15-k(k)h)p(k/k-1)
[0067]
状态估计值x(k)满足：
[0068]
x(k)＝x(k/k-1) k(k)(z(k)-hx(k/k-1))
[0069]
其中，x(k)表示k时刻状态估计值。
[0070]
进一步的，步骤十三中根据步骤十二中得到的状态估计值x(k)，对步骤三中得到的四元数进行补偿，得到载体姿态角φ具体为：
[0071]
对步骤三中得到的四元数进行补偿：
[0072][0073]
根据上式中元素，计算更新捷联矩阵更新姿态信息：
[0074]
[0075]
其中，c
ij
(i,j＝1,2,3)表示中第i行、第j列矩阵元素；，φi(i＝x,y,z)分别表示横滚角、俯仰角、航向角解算值。
[0076]
本发明的有益效果：本发明针对初始精对准过程中由测量噪声统计特性变化引起的对准误差增大问题，提出了一种基于新息检测进行自适应方法调整的ins精对准方法，以位置误差为观测量，ins导航误差为状态量，进行卡尔曼滤波时间更新；根据时间更新结果计算新息二阶矩，与判决条件进行比较，当满足判决条件时，使用自适应因子计算k时刻量测噪声阵，否则，基于allan方差估计k时刻量测噪声阵；依据k时刻量测噪声阵进行卡尔曼滤波量测更新，对ins导航误差进行估计，补偿ins失准角，提高初始对准精度。
[0077]
本发明在进行卡尔曼滤波时，根据新息矩阵进行自适应r阵方法调整，对卡尔曼滤波测量噪声阵进行估计，经过进一步解算得到姿态失准角估计值，对姿态角进行校正，消除了由变化测量噪声引起的姿态角估计不准确问题，提高ins精对准精度。其优点在于：(1)解决了受工况影响，测量噪声发生变化情况下的姿态角估计不准确问题,消除了由变化测量噪声引起的初始对准误差；(2)不需要任何外界辅助信息就可提高定位精度；(3)计算量小，简单易操作。
附图说明
[0078]
图1为本发明的方法流程图；
[0079]
图2为利用本发明进行仿真试验结果；
[0080]
图3为利用本发明进行实测试验结果。
具体实施方式
[0081]
下面结合说明书附图和具体实施例对本发明做进一步说明。
[0082]
结合图1，本发明包括以下步骤：
[0083]
步骤一：上电，初始化ins/gps组合系统。导航初始时刻，初始化系统：
[0084]
(1)初始化ins导航解算初值：船的位置信息λ0(单位均为度)，h0(单位为m)分别为初始纬度、初始经度和初始高度，三维速度信息v
x0
、v
y0
、v
z0
(单位均为m/s)，以及三个粗对准姿态角信息φ
x0
、φ
y0
、φ
z0
(单位均为度)，初始转换矩阵初始化四元数q0；
[0085]
(2)初始化常值参量：新息二阶矩均值窗长m；遗忘因子b；
[0086]
(3)初始化指数渐消记忆加权因子初值：β0；
[0087]
(4)初始化卡尔曼滤波器初值：状态变量初值x0＝]δp
t δv
t δφ
t δ
t ε
t
]
t
，均方误差阵p0，系统噪声方差阵q，量测噪声方差阵r0，量测阵hk，hk＝[ι3×
3 ο3×
3 ο3×
3 ο3×
3 ο3×3]，其余初值根据实际情况设定。其中，ι表示单位矩阵；ο表示零矩阵。
[0088]
初始转换矩阵计算如下：
[0089][0090]
其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵。
[0091]
初始化四元数q0计算如下：
[0092][0093]
其中，q0＝[q
00 q
10 q
20 q
30
]
t
，c
bnij
(i,j＝1,2,3)表示中第i行、第j列矩阵元素；
[0094]
将以上初始化信息装订至导航计算机中。
[0095]
导航过程中，利用该初始信息进行更新，得到任意时刻载体的位置、速度和姿态信息。
[0096]
步骤二：实时采集惯性组件加速度计输出的三轴加速度陀螺输出的三轴角速度和gps输出的位置信息其中，f
ib
(i＝x,y,z)表示加速度fb在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s2)；分别表示角速度ωb在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rad/s)；表示gps输出纬度，λ
gps
表示gps输出经度，h
gps
表示gps输出高度；角标t表示矩阵转置。
[0097]
步骤三：对步骤二中获取的惯性组件输出数据进行导航解算：
[0098]
四元数姿态矩阵更新运算：
[0099][0100]
其中，表示ins解算纬度；ω
ie
表示地球自转角速度；分别表示ins解算载体速度在导航系oxn轴、oyn轴轴上的分量；re表示地球半径；t为采样时间；当k＝1时，q(1)为步骤1中的载体初始四元数。
[0101]
根据q(k)中元素qi(k)(i＝0,1,2,3)，更新捷联矩阵
[0102][0103]
其中，上式中的qi(i＝0,1,2,3)为式(3)中qi(k)(i＝0,1,2,3)，更新姿态信息：
[0104][0105]
其中，表示中第i行、第j列矩阵元素；中第i行、第j列矩阵元素；分别表示横滚角、俯仰角、航向角解算值。
[0106]
速度更新运算：
[0107][0108]
其中，分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量；gn＝[0 0
ꢀ‑
g]
t
，g表示当地重力加速度。当k＝1时，为步骤一中初始化系统时获得的载体初始速度。
[0109]
位置更新计算：
[0110][0111]
其中，表示解算载体纬度，表示解算载体经度，表示解算载体高度；当t＝1时，为步骤一中载体初始位置。
[0112]
至此，根据式(5)、(6)、(7)得到ins解算的载体姿态角、速度和位置。
[0113]
步骤四：构造卡尔曼滤波器，滤波状态量：x＝[δp
t δv
t δφ
t δ
t ε
t
]
t
，以步骤二中gps位置信息p
gps
和步骤三中解算位置信息之差为观测量，即：进行卡尔
曼滤波时间更新。其中，δp表示ins位置误差，δp＝[δp
x δp
y δpz]
t
；δv表示ins速度误差，δv＝[δv
x δv
y δvz]
t
；δφ表示ins姿态失准角，δφ＝[δφ
x δφ
y δφz]
t
；δ表示加速度计零偏，δ＝[δ
x δ
y δz]
t
；ε表示陀螺零偏，ε＝[ε
x ε
y εz]
t
，具体更新过程如下：
[0114]
状态一步预测阵运算：
[0115]
x(k/k-1)＝f(k/k-1)x(k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0116]
其中，x(k/k-1)表示状态一步预测阵；f(k/k-1)表示k-1时刻到k时刻的状态转移矩阵；x(k-1)表示k-1时刻状态估计值。
[0117]
一步预测均方误差阵运算：
[0118]
p(k/k-1)＝f(k/k-1)p(k-1)f
t
(k/k-1) q
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0119]
其中，p(k/k-1)表示一步预测均方误差阵；p(k-1)表示k-1时刻状态估计均方误差；q表示系统噪声。
[0120]
步骤五：根据步骤四的z(k)和x(k/k-1)，计算k时刻新息二阶矩c(k)，即
[0121]
c(k)＝e((z(k)-hx(k/k-1))(z(k)-hx(k/k-1))
t
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0122]
步骤六：自适应方法选取。比较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时刻量测噪声阵。
[0123]
步骤七：计算(k-m 1)～k时刻，m个点新息二阶矩均值即
[0124][0125]
其中，c(j)表示j时刻新息二阶矩。
[0126]
步骤八：根据步骤五的c(k)和步骤七的计算自适应因子α，即
[0127][0128]
其中，tr表示对矩阵求迹。
[0129]
步骤九：根据步骤八中的自适应因子，计算k时刻r(k)，即：
[0130]
r(k)＝(1 α)r(k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0131]
计算完成后，转至步骤十二。
[0132]
步骤十：计算k时刻指数渐消记忆加权因子β(k)，即：
[0133][0134]
其中，b表示遗忘因子。
[0135]
步骤十一：根据步骤四的z(k)和步骤十的β(k)，基于allan方差估计k时刻r(k)，即：
[0136][0137]
计算完成后，转至步骤十二。
[0138]
步骤十二：根据步骤四的z(k)、x(k/k-1)、p(k,k-1)和步骤九或步骤十一的r(k)，进行卡尔曼滤波测量更新。具体计算过程如下：
[0139]
滤波增益计算：
[0140]
k(k)＝p(k,k-1)h
t
(hp(k,k-1)h
t
r(k))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0141]
估计均方误差计算：
[0142]
p(k)＝(i
15
×
15-k(k)h)p(k/k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0143]
状态估计值计算：
[0144]
x(k)＝x(k/k-1) k(k)(z(k)-hx(k/k-1))
ꢀꢀꢀꢀꢀꢀꢀꢀ
(18)
[0145]
其中，x(k)表示k时刻状态估计值。
[0146]
步骤十三：根据步骤十二中得到的状态估计值x(k)，对步骤三中得到的四元数进行补偿，得到载体姿态角φ。具体为：
[0147][0148]
根据上式中元素，结合公式计算更新捷联矩阵更新姿态信息：
[0149][0150]
其中，c
ij
(i,j＝1,2,3)表示中第i行、第j列矩阵元素；，φi(i＝x,y,z)分别表示横滚角、俯仰角、航向角解算值；存储并输出载体姿态角。
[0151]
下面结合具体参数对本发明的有益效果如下方式得以验证：
[0152]
船舶航行路线：直线运动
[0153]
(1)系统初始化参数如下：
[0154]
舰船运动参数：舰船速度：v
x0
＝0m/s，v
y0
＝0m/s，v
z0
＝0m/s；
[0155]
舰船姿态：φ
x0
＝0
°
，φ
y0
＝0
°
，φ
z0
＝30
°
；
[0156]
舰船姿态角误差：0.5
°
，0.5
°
，5
°
；
[0157]
当地经纬度：λ0＝126.6705
°
；
[0158]
陀螺参数：陀螺常值漂移：0.01
°
/h
[0159]
陀螺白噪声误差：0.001
°
/h
[0160]
加速度计参数：加速度计零偏：10-5g[0161]
加速度计白噪声误差：10-6g[0162]
(2)常值参量：
[0163]
采样时间：t＝0.005s
[0164]
新息二阶矩均值窗长：m＝5
[0165]
遗忘因子：b＝0.95
[0166]
(3)指数渐消记忆加权因子初值设置：β0＝1
[0167]
(4)卡尔曼滤波器参数设置：
[0168]
状态变量初值:x0＝[o1×
3 o1×
3 o1×
3 o1×
3 o1×3]
t
[0169]
均方误差阵:
[0170]
p
a0
＝diag[2.12
×
10-8 2.12
×
10-8 3.58
×
10-4 10-2 10-2 10-4 2.46
×
10-14 2.46
×
10-14 1 2.12
×
10-14 2.12
×
10-14 2.12
×
10-14 9.57
×
10-7 9.57
×
10-7 9.57
×
10-7
]
[0171]
其中，diag表示矩阵对角线上元素。
[0172]
系统噪声方差阵:
[0173]
q＝diag[8.46
×
10-14 8.46
×
10-14 8.46
×
10-14 9.57
×
10-9 9.57
×
10-9 9.57
×
10-9 0 0 0 0 0 0 0 0 0]
[0174]
量测噪声方差阵：ra＝diag[2.46
×
10-12 2.46
×
10-12 2.46
×
10-12
]
[0175]
利用本发明方法，得到应用此方法的失准角曲线，图2表示失准角曲线。因实测试验没有外姿态基准，因此采用在初始对准完成后进行组合导航，用组合导航的定位误差反映对准精度；图3表示有、无自适应调整r阵进行精对准再进行组合导航的定位误差曲线。结果表明本发明较好抑制了由测量噪声变化引起的初始对准精度下降，可以满足实际需求。