全球导航卫星拒止条件下的鲁棒优化对准方法及航行器与流程

1.本发明属于惯性导航系统(inertial navigation system，ins)初始对准技术领域，尤其涉及一种全球导航卫星系统(global navigation satellite system，gnss)拒止条件下的鲁棒优化对准方法及水下航行器。


背景技术：

2.目前，为提高水下航行器(underwater vehicle，uv)的快速反应能力，要求捷联式惯导系统(strapdown inertial navigation system，sins)在动态条件下实现初始对准，sins动态粗对准作为初始对准的重要环节，成为近年来关注和研究的热点。现有技术1针对摇摆状态，提出了基于重力加速度信息的准动态对准方法，并取得了较好的效果；现有技术2在现有技术1的基础上，提出了一种适用于sins运动基座的惯性系对准方法；现有技术3在sins微分方程的基础上，通过构建不同时刻的连续观测矢量，将姿态确定问题转化为典型的wahba问题，提出了优化对准方法(optimisation
‑
based alignment，oba)。但是，oba属于解析式粗对准方法，不具备鲁棒性。针对多普勒计程仪(doppler velocity log，dvl)输出速度易受野值等非高斯噪声污染以及oba不具备鲁棒性的问题，现有技术4结合uv的运动特性，提出了一种基于速度门限检测的抗野值优化对准方法(atoba)，在外部辅助信息受野值污染的条件下，实现了oba算法的鲁棒化。atoba虽然可以对高强度野值进行准确辨识，但在dvl测速受野值污染时刻的初始姿态阵等效于前一时刻的初始姿态阵，在一定程度上损失了观测矢量的有效信息。另外，atoba速度检测门限的选取过于依赖经验，因此难以对其他类型的非高斯噪声进行准确辨识。
3.通过上述分析，现有技术存在的问题及缺陷为：现有技术对野值的检测手段过于依赖经验，且对其他类型的非高斯噪声不具备鲁棒性。
4.解决以上问题及缺陷的难度为：初始对准是sins导航解算前的必经阶段，粗对准是初始对准的重要环节。在有gnss条件下，运载体可以通过卫星定位测姿或卫星/惯导系统组合测姿获取准确的姿态信息。而在无gnss信号的水下环境中，sins初始姿态的确定尤其是动态条件下初始对准尤为困难，现有技术难以有效解决水下非高斯条件下sins动态粗对准问题，且对dvl测速信息的检测手段也过于依赖经验。因此，在非高斯复杂条件下，设计sins优化对准过程中的观测矢量和观测矩阵鲁棒更新策略，实现sins鲁棒优化对准是难题和挑战。
5.解决以上问题及缺陷的意义为：gnss拒止条件下的鲁棒优化对准方法(robust oba，roba)，对于提升运载体在非高斯复杂环境中的快速反应能力和机动能力具有重要的现实意义。


技术实现要素：

6.针对现有技术存在的问题，本发明提供了一种全球导航卫星系统gnss拒止条件下的鲁棒优化对准方法、介质及水下航行器。
7.本发明是这样实现的，一种全球导航卫星系统(global navigation satellite system，gnss)拒止条件下的鲁棒优化对准方法，包括：
8.在标准卡尔曼滤波(kalman filter，kf)框架下，利用ps算法对存储的异常载体系速度进行辨识；设计观测矢量鲁棒更新策略，利用惯性测量单元(inertial measurement unit，imu)及多普勒计程仪(doppler velocity log，dvl)输出数据通过观测矢量鲁棒更新策略进行观测矢量更新，并进行优化对准。
9.进一步，所述观测矢量更新策略包括：
10.当载体系速度受到非高斯噪声污染时，由imu输出数据计算得到的观测矢量正常更新，由载体系测速数据计算得到的观测矢量不进行更新；
11.当载体系测速正常时，由imu输出数据和载体系测速数据计算得到的观测矢量均正常更新。
12.进一步，所述gnss拒止条件下的鲁棒优化对准方法包括以下步骤：
13.步骤一，通过ps算法对滑动窗存储的异常新息向量进行辨识；
14.步骤二，设计观测矢量及观测矩阵的更新策略，进行优化对准。
15.进一步，所述通过ps算法对滑动窗存储的异常新息向量进行辨识之前还需进行：
16.获取k时刻新息向量μ
k
：
[0017][0018]
其中，z
k
示k时刻观测向量；h
k
表示k时刻观测矩阵；x
k
表示k时刻状态向量；f
k
表示k(k＝1,2,...)时刻状态转移矩阵。
[0019]
进一步，所述通过ps算法对滑动窗存储的异常新息向量进行辨识包括：
[0020]
采用ps算法对滑动窗内的y＝[μ
k
‑
wl+1
,...,μ
k
‑1,μ
k
]的统计特性进行分析，判断μ
k
是否异常。
[0021]
进一步，步骤一中，所述通过ps算法对滑动窗存储的异常新息向量进行辨识还包括：
[0022]
根据ps算法计算出新息向量的ps值，并将ps值与统计门限进行比较，辨识得到异常的新息。
[0023]
进一步，所述根据ps算法计算出新息向量的ps值，并将ps值与统计门限进行比较，辨识得到异常的新息包括：
[0024]
满足ps值大于统计门限η的信息为异常新息，即ps＞η对应的dvl测速为异常速度。
[0025]
进一步，所述统计门限η＝χ
n,α
，且服从自由度为n的卡方分布
[0026]
进一步，步骤二，所述设计观测矢量及观测矩阵的更新策略包括：
[0027]
当dvl输出速度对应的ps值大于统计门限η即ps＞η时，则dvl的输出受到野值污染；此时对应的观测矢量β
v,t
中的β
v,t,imu
正常更新，β
v,t,dvl
不进行更新，并令β
v,t,dvl
等于β
v,t
‑
1,dvl
；观测矢量α
v,t
正常更新；观测矩阵k
t
正常更新；
[0028]
当dvl输出k时刻速度对应的ps值小于等于统计门限η即ps≤η时，则dvl输出没有受到野值污染；此时对应的观测矢量β
v,t
正常更新；观测矢量α
v,t
正常更新；观测矩阵k
t
正常更新。
[0029]
本发明的另一目的在于提供一种实施所述鲁棒优化对准方法的基于ps算法的鲁
棒优化对准控制系统；所述基于ps算法的鲁棒优化对准控制系统包括：
[0030]
异常载体系速度辨识模块，用于在标准卡尔曼滤波框架下，利用ps算法对存储的异常载体系速度进行辨识；
[0031]
更新策略获取模块，用于设计观测矢量鲁棒更新策略；
[0032]
观测矢量更新优化模块，用于利用惯性测量单元及多普勒计程仪输出数据通过观测矢量鲁棒更新策略进行观测矢量更新，并进行优化对准。
[0033]
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行所述的gnss拒止条件下的鲁棒优化对准方法。
[0034]
本发明的另一目的在于提供一种水下航行器，所述水下航行器执行所述的gnss拒止条件下的鲁棒优化对准方法。
[0035]
结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明利用ps算法辨识dvl测速异常点，进而设计了观测矢量更新策略以克服非高斯噪声的不良影响。考虑了dvl测速受到野值污染以及混合高斯分布噪声污染情形，基于船载实测数据分别利用oba、atoba和roba进行水下sins优化对准半物理仿真实验，结果初步表明，相比于oba和atoba，roba具有更高的对准精度和更强的鲁棒性。将本发明用于水下等gnss拒止环境中的sins优化对准中是可行、有效的。
[0036]
相比于oba、atoba，本发明能够智能地辨识异常速度并有效抑制不同类型非高斯噪声对动态粗对准结果的影响，具有更优的动态对准性能。
附图说明
[0037]
图1是本发明实施例提供的gnss拒止条件下的鲁棒优化对准方法(roba)流程图。
[0038]
图2是本发明实施例提供的利用roba进行初始对准的流程图。
[0039]
图3是本发明实施例提供的坐标系示意图。
[0040]
图4是本发明实施例提供的野值污染情形的dvl输出速度示意图。
[0041]
图5是本发明实施例提供的高斯情形的dvl输出速度示意图。
[0042]
图6是本发明实施例提供的滑动窗中存储新息的ps值示意图。
[0043]
图7是本发明实施例提供的俯仰角对准误差示意图。
[0044]
图8是本发明实施例提供的横滚角对准误差示意图。
[0045]
图9是本发明实施例提供的航向角对准误差示意图。
[0046]
图10是本发明实施例提供的不同方法的俯仰角对准误差示意图。
[0047]
图11是本发明实施例提供的不同方法的横滚角对准误差示意图。
[0048]
图12是本发明实施例提供的不同方法的航向角对准误差示意图。
具体实施方式
[0049]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0050]
针对现有技术存在的问题，本发明提供了一种gnss拒止条件下的鲁棒优化对准方
法，下面结合附图对本发明作详细的描述。
[0051]
本发明实施例提供的gnss拒止条件下的鲁棒优化对准方法包括：
[0052]
在kf框架下，利用ps算法对存储的异常速度进行准确辨识；通过设计观测矢量鲁棒更新策略进行观测矢量更新，并进行优化对准。
[0053]
本发明实施例提供的观测矢量鲁棒更新策略包括：
[0054]
当载体系速度受到非高斯噪声污染时，由imu输出数据计算得到的观测矢量正常更新，由载体系测速数据计算得到的观测矢量不进行更新；
[0055]
当载体系测速正常时，由imu输出数据和载体系测速数据计算得到的观测矢量均正常更新。
[0056]
如图1所示，本发明实施例提供的gnss拒止条件下的鲁棒优化对准方法包括以下步骤：
[0057]
s101，通过ps算法对滑动窗存储的异常新息向量进行辨识；
[0058]
s102，设计观测矢量及观测矩阵的更新策略，进行优化对准。
[0059]
本发明实施例提供的通过ps算法对滑动窗存储的异常新息向量进行辨识之前还需进行：
[0060]
获取k时刻新息向量μ
k
：
[0061][0062]
其中，z
k
示k时刻观测向量；h
k
表示k时刻观测矩阵；x
k
表示k时刻状态向量；f
k
表示k(k＝1,2,...)时刻状态转移矩阵。
[0063]
本发明实施例提供的通过ps算法对滑动窗存储的异常新息向量进行辨识包括：
[0064]
采用ps算法对滑动窗内的y＝[μ
k
‑
wl+1
,...,μ
k
‑1,μ
k
]的统计特性进行分析，判断μ
k
是否异常。
[0065]
本发明实施例提供的通过ps算法对滑动窗存储的异常新息向量进行辨识还包括：
[0066]
根据ps算法计算出新息向量的ps值，并将ps值与统计门限进行比较，辨识得到异常的信息。
[0067]
本发明实施例提供的根据ps算法计算出新息向量的ps值，并将ps值与统计门限进行比较，辨识得到异常的信息包括：
[0068]
满足ps值大于统计门限η的信息为异常信息，即ps＞η对应的dvl测速为异常速度。
[0069]
本发明实施例提供的统计门限η＝χ
n,α
，且服从自由度为n的卡方分布
[0070]
本发明实施例提供的设计观测矢量及观测矩阵的鲁棒更新策略包括：
[0071]
当dvl输出速度对应的ps值大于统计门限η即ps＞η时，则dvl的输出受到野值污染；此时对应的观测矢量β
v,t
中的β
v,t,imu
正常更新，β
v,t,dvl
不进行更新，并令β
v,t,dvl
等于β
v,t
‑
1,dvl
；观测矢量α
v,t
正常更新；观测矩阵k
t
正常更新；
[0072]
当dvl输出k时刻速度对应的ps值小于等于统计门限η即ps≤η时，则dvl的输出没有受到野值污染；此时对应的观测矢量β
v,t
正常更新；观测矢量α
v,t
正常更新；观测矩阵k
t
正常更新。
[0073]
本发明还提供一种基于ps算法的鲁棒优化对准控制系统包括：
[0074]
异常载体系速度辨识模块，用于在标准卡尔曼滤波框架下，利用ps算法对存储的异常载体系速度进行辨识；
[0075]
更新策略获取模块，用于设计观测矢量鲁棒更新策略；
[0076]
观测矢量更新优化模块，用于利用惯性测量单元及多普勒计程仪输出数据通过观测矢量鲁棒更新策略进行观测矢量更新，并进行优化对准。
[0077]
下面结合具体实施例对本发明的技术方案做进一步说明。
[0078]
实施例1：
[0079]
1、载体系测速辅助优化对准方法(oba)
[0080]
在水下环境中，dvl可以为uv提供载体系测速信息，sins与dvl组合能够实现全天候、广水域的动态粗对准。记导航坐标系为n系；载体坐标系为b系；计算坐标系为n
′
系；地球坐标系为e系；惯性坐标系为i系；e(0)系、n(0)系、b(0)系分别为凝固(初始时刻)地球坐标系、导航坐标系、载体坐标系，相对于i系固定不动。捷联惯性测量单元(inertial measurement unit，imu)的姿态更新方程、速度更新方程和位置更新方程分别如式(1)
‑
(3)所示。
[0081][0082][0083][0084]
式中，表示由b系至n系的方向余弦阵，即姿态阵；表示地速，即运载体的速度；表示b系相对于n系的角速度在b系上的投影，表示b系相对于n系的角速度在b系上的投影，表示地球自转角速度在n系上的投影；表示n系相对于e系的角速度在n系上的投影；g
n
表示重力加速度；表示陀螺仪测得的载体角速度；f
b
表示加速度计测得的比力；[
·
×
]表示向量[
·
]的反对称矩阵。l、λ、h分别表示纬度、经度和高度；r
m
、r
n
分别表示地球子午圈半径、卯酉圈半径。在水下导航问题中，由于地球半径r
e
远大于高(深)度h，因此可以将r
m
+h、r
n
+h近似为地球半径r
e
。
[0085]
假设向量a＝[a1；a2；a3]，则[a
×
]的计算公式如式(4)所示。
[0086][0087][0088]
式(1)所示的姿态矩阵在t时刻可分解为如式(6)所示多个方向余弦阵连乘的形式。
[0089]
[0090]
式(6)中，和的求解表达式分别如式(7)
‑
(9)所示。
[0091][0092][0093][0094]
式中，l0、λ0分别为初始时刻的纬度和经度。对式(2.1.2)两端同时左乘并整理可得：
[0095][0096]
式(10)两端同时左乘并在区间[0,t]进行积分可得：
[0097][0098]
对式(11)进行整理得：
[0099][0100]
由于在水下粗对准过程中，n系相对于e系的转动角速度很小，可以令式(12)可以转化为：
[0101][0102]
定义t时刻观测矢量α
v,t
和β
v,t
分别如式(14)和(15)所示。
[0103][0104][0105]
式中，
[0106][0107][0108]
结合式(13)
‑
(15)，可将式(13)转化为如式(18)的形式。
[0109][0110]
因此，初始对准问题就转化成了利用双矢量α
v,t
、β
v,t
确定姿态阵进而确定初始姿态阵的问题。对式(18)的求解问题为典型的wahba问题，利用q
‑
method方法对式
(18)进行求解即可得矩阵对应的四元数为q，进而得到初始姿态阵具体过程如下，定义q＝[s η
t
]
t
，则与q的关系如式(19)所示。
[0111][0112]
式中，s为标量，η为向量。定义四元数矩阵如式(20)所示。
[0113][0114]
则式(18)对应的实对称观测矩阵k如式(21)所示。
[0115][0116]
根据q
‑
method方法，对应的四元数q为矩阵k
t
的最小特征值对应的特征向量。在实际应用中，需对式(18)和式(21)进行离散化，分别如式(22)和式(23)所示。
[0117][0118][0119]
式中，α
v,k
和β
v,k
的具体求解过程如实施例2。根据q
‑
method方法，对应的四元数q为矩阵k
t
的最小特征值对应的特征向量。
[0120]
以上推导过程是基于外部辅助速度符合高斯分布特性条件的，oba本质上属于解析式对准方法，不具备鲁棒性。由式(15)和式(16)可知，dvl输出的速度v
b
对构建观测矢量β
v,t
发挥着不可替代的作用：当v
b
受到非高斯类噪声如混合高斯分布噪声、野值等污染时，基于dvl测速信息计算得出的观测矢量β
v,t
的精度将会下降，这将会降低oba方法的性能。
[0121]
2.鲁棒优化对准方法(roba)
[0122]
2.1异常速度辨识方法
[0123]
为抑制测速野值对oba性能的影响，抗野值鲁棒优化对准方法(atoba)通过设置速度检测门限来辨识测速野值，而后设计初始姿态矩阵更新策略消除测速野值对初始对准结果的影响，具体过程如下。
[0124]
考虑uv在水下航行的过程中，绝大多数情况处于平稳的前向运动，因此dvl输出的右向速度和上向速度近似为0m/s。atoba选取速度门限当dvl输出t时刻的右向或上向速度的绝对值超过速度门限即或时，则认为dvl的输出受到野值污染。此时对应的初始姿态矩阵等于前一时刻的初始姿态阵如式(24)所示。
[0125][0126]
由式(6)、(14)
‑
(18)及式(24)可知，当dvl测速受到野值污染时，atoba中的观测矢
量α
v,t
和β
v,t
均不进行更新，仅使初始姿态阵等于上一时刻初始姿态阵因此，在野值出现条件下将会损失有效的imu信息，进而影响对准性能。另外，atoba选取速度检测门限通常依赖经验，难以实现对其他类型非高斯噪声的灵活、准确辨识。也就是说，atoba对其他类型非高斯噪声并不完全具备鲁棒性。
[0127]
2.2优化对准鲁棒化
[0128]
考虑如下离散时间线性动态对准状态空间模型：
[0129][0130]
式中，f
k
表示k(k＝1,2,...)时刻状态转移矩阵；h
k
表示k时刻观测矩阵；x
k
、z
k
分别表示k时刻状态向量、观测向量；ω
k
～n(0,q
k
)表示系统噪声向量，一般服从高斯分布，q
k
为系统噪声协方差矩阵；υ
k
～n(0,r
k
)表示量测噪声向量，一般服从高斯分布，r
k
为量测噪声协方差矩阵。k时刻标准kf更新方程如下。
[0131]
(a)时间更新
[0132][0133][0134]
式中，表示k时刻状态量先验估计，p
k|k
‑1表示k时刻状态误差协方差的先验估计。
[0135]
(b)量测更新
[0136][0137][0138][0139]
p
k|k
＝(i
‑
k
k
h
k
)p
k|k
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0140]
式中，μ
k
表示k时刻新息向量，k
k
表示k时刻卡尔曼滤波增益，λ
k
表示膨胀因子，表示k时刻状态量的后验估计，p
k|k
表示k时刻状态估计误差协方差的后验估计。
[0141]
当观测量(测速信息)受到野值的干扰时，atoba为降低干扰的影响，通过设置速度门限将受到污染的观测量检测出并剔除。但是这种方式在诸如连续强干扰的影响下长时间不进行观测矢量更新，将会造成初始对准误差发散。ps算法是辨识观测异常点的有效方法，对于由观测新息向量{μ
i
},i＝1,2,...,m构成的向量矩阵y，当δx
k|k
‑1等于0时，有
[0142]
y＝[μ
k
‑
wl+1
,...,μ
k
‑1,μ
k
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(32)
[0143]
式中，表示长度为wl滑动窗中存储第i个新息向量，i＝wl,...,m，m为观测量的个数。对于y中第j个元素l
j
。
[0144]
根据ps算法计算出新息向量的ps值，并将ps值与统计门限η进行比较以辨识出异常的新息，认为ps值满足ps＞η的新息为异常新息，即ps＞η对应的dvl测速为异常速度。统
计门限η＝χ
n,α
，且服从自由度为n的卡方分布文中，设置统计门限为η＝χ
2,0.99
＝3.0332。roba中，设计观测矢量更新策略具体如下。
[0145]
当dvl输出速度对应的ps值大于统计门限η即ps＞η时，则认为dvl的输出受到野值污染。此时对应的观测矢量β
v,t
中的β
v,t,imu
正常更新，β
v,t,dvl
不进行更新，并令β
v,t,dvl
等于β
v,t
‑
1,dvl
；观测矢量α
v,t
正常更新；观测矩阵k
t
正常更新。反之，当dvl输出k时刻速度对应的ps值小于统计门限η即ps≤η时，则认为dvl的输出没有受到野值污染。此时对应的观测矢量β
v,t
正常更新；观测矢量α
v,t
正常更新；观测矩阵k
t
正常更新。即有：
[0146]
if ps
t
＞η
[0147]
α
v,t
→
α
v,t
，β
v,t,imu
→
β
v,t,imu
，β
v,t,dvl
→
β
v,t
‑
1,dvl
，k
t
→
k
t
ꢀꢀ
(33)
[0148]
else
[0149]
α
v,t
→
α
v,t
，β
v,t
→
β
v,t
，k
t
→
k
t
ꢀꢀ
(34)
[0150]
end
[0151]
在实际应用中，需要对式(33)和式(34)进行离散化，如式(35)和式(36)所示。
[0152]
if ps
k
＞η
[0153]
α
v,k
→
α
v,k
，β
v,k,imu
→
β
v,k,imu
，β
v,kk,dvl
→
β
v,kk
‑
1,dvl
，k
kk
→
k
kk
ꢀꢀ
(35)
[0154]
else
[0155]
α
v,k
→
α
v,k
，β
v,kk
→
β
v,kk
，k
kk
→
k
kk
ꢀꢀ
(36)
[0156]
end
[0157]
式中，k为α
v
的更新时刻，与imu的采样周期相对应；kk为β
v
和k的更新时刻，与dvl的采样周期相对应。当imu与dvl的采样周期相同时，则有k＝kk，此时α
v
、β
v
和k的更新率相同；当dvl的采样周期高于imu时，根据式(14)、式(15)和式(21)可知，β
v
和k只有在dvl数据刷新时才进行一次完整的更新。
[0158]
2.3鲁棒优化对准方法(roba)
[0159]
利用roba进行初始对准的流程如图2所示。由图2可以看出，roba是基于oba框架下的粗对准方法，相比于atoba，roba是通过ps算法对滑动窗存储的异常新息向量进行辨识，进而通过设计观测矢量β
v,t
及观测矩阵k
t
的更新策略有效消除异常速度对初始对准结果的不良影响。值得注意的是，在对准时间小于等于滑动窗长度即kk≤wl时，与atoba一样，roba利用速度检测门限辨识非高斯噪声。本发明在时间信息满足kk≤wl条件时，选取速度检测门限为
[0160]
3.半物理仿真实验与结果分析
[0161]
为了验证roba的有效性和可行性，选取两组180s船载实测数据进行水下sins优化对准半物理仿真实验，数据是从船载惯性级惯性测量单元和dvl采集得到的，船上安装了单天线gps接收机，数据更新率为1hz，sins/gps组合导航结果作为参考基准。sins和dvl主要性能指标分别如表1和表2所示。两组180s船载数据分别如图4和图5所示。
[0162]
表1 imu性能指标
[0163] 陀螺仪加速度计更新频率200hz200hz精度等级优于0.02
°
/h(1σ)<50ug(1σ)
[0164]
表2 dvl性能指标
[0165]
测速精度0.5％v
±
0.5cm/s更新频率1hz发射频率300khz底跟踪深度300m
[0166]
考虑到外部环境的复杂性，研究如下两种不同非高斯量测噪声情形：野值污染情形和混合高斯分布噪声污染情形。
[0167]
3.1野值污染情形
[0168]
实验中，设置滑动窗的长度wl＝60s。如图5所示，在时刻69s、72s、89s和155s，dvl输出速度受到野值的污染。在优化对准过程中，选取某个包含野值(69s、72s、89s对应的野值)且长度为60s的滑动窗，通过ps算法计算存储信息的ps值，如图6所示。由图6可明显看出，时刻69s、72s和89s的存储新息向量对应的ps值明显大于其他时刻存储新息向量的ps值。通过试验及计算可知，时刻45s，48s和65s对应的存储新息向量的ps值分别为369.2、393.5和356.7，远大于统计门限η＝3.0332。因此，通过ps算法可准确辨识出观测信息异常点，有效检测出异常速度。
[0169]
为进一步验证roba在野值污染情形下的有效性和可行性，分别利用oba、atoba和roba进行优化对准，得到的俯仰角对准误差、横滚角对准误差和航向角对准误差分别如图7
‑
图9所示。
[0170]
由图7
‑
图9可以明显看出，oba对非高斯噪声没有鲁棒性，在利用oba进行初始对准的过程中，当dvl输出受野值污染时，oba对准误差曲线呈现发散趋势。由图7
‑
图9还可看出，atoba和roba的初始对准性能明显优于oba，且roba与atoba对准误差曲线收敛速度相当。根据实验可知，在对准结束时刻，oba对姿态角的对准误差分别为：
‑
0.0020
°
、0.1458
°
、26.4376
°
；atoba对姿态角的对准误差分别为：
‑
0.0003
°
、
‑
0.0035
°
、
‑
0.8475
°
；roba对姿态角的对准误差分别为：
‑
0.0009
°
、
‑
0.0013
°
、
‑
0.5858
°
。由此可看出，roba的对准精度明显优于atoba。这是因为：(1)相比于oba，当野值出现时，roba首先利用ps算法对观测信息异常进行抑制，使得roba在dvl输出受非高斯噪声污染时具有更强的对准鲁棒性。(2)当野值出现时，roba通过设计如图2所示的观测矢量更新策略抑制异常速度，对imu输出数据进行有效利用，而atoba令污染时刻的姿态阵等效于上一时刻的姿态阵，并不对观测矢量进行更新，损失了imu输出的有效信息。相比于atoba，roba观测矢量的更新策略更为合理。
[0171]
3.2混合高斯分布噪声污染情形
[0172]
量测噪声的“名义”高斯分布(即主分布)受到另一高斯分布的污染。基于维纳近似定理，任何非高斯噪声分布可用已知概率密度的高斯噪声分布的有限和来表示或充分近似。假设观测噪声的实际概率分布如式(37)所示。
[0173]
ρ
actual
＝(1
‑
α)n(0,r
c
)+αn(0,r
p
)
ꢀꢀ
(37)
[0174]
式中，干扰因子α满足0≤α≤0.1；r
c
为外部辅助速度数据的量测噪声协方差阵，r
p
为具有较大标准偏差的干扰噪声协方差阵。实验中，设置α＝0.1，r
c
＝diag[(0.12,0.12)](m/s)2，r
p
＝100r
c
。人为地将式(37)引入dvl输出速度中，分别利用oba、atoba和roba进行100次优化对准蒙特卡洛仿真实验。
[0175]
由图可看出，当dvl测速受到混合高斯分布噪声干扰时，roba得到的对准误差的稳
定性优于oba和atoba。通过实验可知，100次蒙特卡洛优化对准实验结束时刻oba、atoba和roba得到的俯仰角对准误差绝对值的平均值分别为0.0027
°
、0.0027
°
、0.0020
°
；横滚角对准误差绝对值的平均值分别为0.0065
°
、0.0064
°
、0.0030
°
；航向角对准误差绝对值的平均值分别为0.8325
°
、0.8245
°
、0.2296
°
，如表3所示。
[0176]
表3对准结束时刻不同方法对准误差绝对值的平均值(100次蒙特卡洛实验)
[0177][0178][0179]
根据实验结果和表3可知，在外部辅助速度受到混合高斯分布噪声干扰的条件下，roba的对准精度优于oba和atoba；atoba优化对准性能与oba相当。也就是说，在此情形下atoba利用速度门限检测速度异常点的方法基本失效，其将会退化为oba。因此，在混合高斯分布噪声干扰的条件下，相比于oba和atoba，roba具备更强的对准鲁棒性和更高的对准精度。
[0180]
需要注意的是：在进行dvl异常速度数据的检测时，基于kf的滤波过程(式26
‑
式31)的目的是获取k时刻新息向量μ
k
，从而采用ps算法对滑动窗内的y＝[μ
k
‑
wl+1
,...,μ
k
‑1,μ
k
]的统计特性进行分析，判断μ
k
是否异常，滤波过程并非对准过程，姿态阵实质上还是通过式(22
‑
23)解析得到。
[0181]
4、sins是北斗等gnss拒止条件下自主导航的重要手段。初始对准是sins进行导航解算前的必经阶段，粗对准是sins初始对准的重要环节。本发明针对dvl测速易受野值等非高斯噪声污染以及传统oba不具备鲁棒性的问题，提出一种基于投影统计(ps)算法的鲁棒优化对准方法(roba)，利用ps算法辨识dvl测速异常点，进而设计了观测矢量更新策略以克服非高斯噪声的不良影响。考虑了dvl测速受到野值污染以及混合高斯分布噪声污染情形，基于船载实测数据分别利用oba、atoba和roba进行水下sins优化对准半物理仿真实验，结果初步表明，相比于oba和atoba，roba具有更高的对准精度和更强的鲁棒性。将roba用于水下等北斗拒止条件下的sins优化对准中是可行、有效的。
[0182]
本发明在卡尔曼滤波算法(kalman filter，kf)框架下，利用投影统计(projection statistics，ps)算法对存储的异常速度进行准确辨识；通过观测矢量更新策略进行观测矢量更新：当多普勒计程仪输出的载体系速度受到非高斯噪声污染时，由惯性测量单元输出数据计算得到的观测矢量正常更新，由载体系测速数据计算得到的观测矢量不进行更新；当载体系测速正常时，由惯性测量单元(inertial measurement unit，imu)输出数据和载体系测速数据计算得到的观测矢量均正常更新；并进行优化对准。本发明能够智能地辨识异常速度并有效抑制不同类型非高斯噪声对捷联式惯导系统(strapdown inertial navigation system，sins)动态粗对准结果的影响，有效保持sins在复杂环境中的动态粗对准性能。
[0183]
实施例2：
[0184]
假设sins和dvl的采样周期为t
s
，α
v,t
、β
v,t
在离散条件下的求解过程如下。
[0185][0186][0187]
式中，t＝c
·
t
s
，c为子样数，c一般取1和2。现分别对基于单子样算法和基于双子样算法求解α
v,t
、β
v,t
进行阐述。可以分解为矩阵连乘的形式，分别如式(a3)和式(a4)所示。
[0188][0189][0190]
(1)基于单子样(c＝1)算法的和求解方法
[0191]
记速度增量为δv，角增量为δθ。则和的表达式分别如式(a5)
‑
(a7)所示。
[0192][0193][0194][0195]
式中，phin＝||pn||2；pb＝δθ，phib＝||pb||2。
[0196]
(2)基于双子样(c＝2)算法的和求解方法
[0197]
记速度增量为δv1、δv2，角增量为δθ1、δθ2。则和的表达式分别如式(a8)
‑
(a10)所示。
[0198][0199]
[0200][0201]
式中，phin＝||pn||2；phib＝||pb||2。速度增量δv1、δv2，角增量δθ1、δθ2满足如下关系式。
[0202][0203][0204]
在实际应用中，当惯导系统采样周期足够小时(t
s
≤0.01s)，采用单子样算法即可满足sins对解算精度的要求。
[0205]
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、cd或dvd
‑
rom的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
[0206]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。