一种基于SINS与DVL组合的AUV行进间快速初始对准方法技术

一种基于SINS与DVL组合的AUV行进间快速初始对准方法，满足AUV在航行过程中快速初始对准的技术要求，能在较短时间内进一步提高对准精度，提升其鲁棒性。通过DVL获取的速度信息修正SINS输出的导航信息，结合逆向导航解算的思想对修正后的信息进行正向与逆向反复交替处理，使导航系统充分利用对准过程中产生的导航信息，提高信息的利用率，从而提升对准精度。从仿真结果看出，在一定时间内增加对准阶段正向与逆向的处理次数，可更快的估出失准角误差，且估计精度提升两倍。在AUV大角度失准角条件下，仍有较高的估计精度和较好的通用性。该方法可用于多AUV的航行编队控制、协同作业、海底地形探测与海图测绘等领域，在理论研究与工程实践方面具有指导和应用价值。程实践方面具有指导和应用价值。程实践方面具有指导和应用价值。

【技术实现步骤摘要】
一种基于SINS与DVL组合的AUV行进间快速初始对准方法


[0001]本专利技术属于自主水下航行器(AUV)的初始对准方法，涉及一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法。

技术介绍

[0002]在水下导航领域，应用比较成熟的是捷联惯性导航系统(SINS)与多普勒速度仪(DVL)组合导航系统。通过DVL实时提供的高精度速度信息，抑制SINS位置误差的发散，具有自主性与定位精度高的特点，因此广泛运用于AUV水下导航。惯导系统在正式进入导航任务前，需要完成初始对准，且初始对准的两个重要指标是精确性和快速性，惯导系统的对准精度直接影响着AUV的导航精度，而对准的快速性决定了AUV的是否能够快速投入使用，而这两者之间往往是互相矛盾的。
[0003]传统的岸上静基座初始对准虽然不需要外部提供航行器的运动信息，能够直接实现对惯导系统的补偿与修正，但需要较长的对准时间，会在很大程度上降低AUV的机动能力，限制其使用范围。因此在一些任务场景下，当要求AUV能够快速响应且快速集结投入使用时，则不再适用。因此，研究一种能够快速估计失准角误差的对准方法在AUV工程应用领域具有十分重要的意义。不同与岸上静基座对准，AUV航行过程中的行进间对准需要引入外部量测信息辅助完成初始对准。由于DVL为自主测速设备，能够直接测量出AUV在航行过程中的速度信息，且其自主性强、精度高，满足在水下和在一定速度范围内的使用要求。
[0004]通常AUV的初始对准采用正向导航解算方法进行处理，这种方式需要利用较多的导航数据进行误差估计，估计时间较长，且估计精度不高。因此针对上述问题，提出一种正向与逆向结合的处理方法。通过对AUV在对准阶段导航系统内部陀螺仪与加速计产生的导航信息进行存储，然后对该信息反复进行正向与逆向的导航解算，通过这种方式可以充分利用在对准阶段产生的导航数据，使得系统可以在较短的时间内利用较少的导航数据进行初始对准，并得到较高的估计精度。最终利用仿真设计了一条模拟AUV水下执行海底地形测绘任务的航行轨迹，并对航行开始前一段时间进行对准角误差估计，通过在不同对准时间的估计误差，对常规的对准方法与该专利技术提出的对准方法进行对比分析，验证了该方法的可行性与有效性。此外，通过仿真还验证了在大角度失准角条件下，采用该方法仍能快速准确估计出系统的失准角误差，由此证明了该方法的通用性。

技术实现思路

[0005]本专利技术的技术解决问题是：克服现有技术的不足，提供了一种基于逆向解算的领航AUV导航数据后处理方法，解决了载波跟踪环动态性能与低噪声的矛盾问题。
[0006]本专利技术的技术解决方案是：
[0007]一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，步骤如下：
[0008]步骤1：建立SINS与DVL的数学模型，包括：
[0009](1.1)建立SINS的状态方程；
[0010](1.2)建立SINS的误差微分方程；
[0011](1.3)建立DVL的误差模型；
[0012](1.4)建立SINS/DVL的量测方程；
[0013]步骤2：根据步骤1建立的SINS与DVL的数学模型，建立SINS/DVL的微分更新方程，包括：
[0014](2.1)建立姿态微分更新方程；
[0015](2.2)建立速度微分更新方程；
[0016](2.3)建立位置微分更新方程；
[0017]步骤3：对姿态、速度和位置更新微分方程进行离散化处理，得到离散化更新方程；
[0018]步骤4：根据步骤3得到的离散化更新方程，建立逆向解算的离散化更新方程；
[0019]步骤5：对步骤3和步骤4得到的离散化更新方程进行卡尔曼滤波估计；
[0020]步骤6：正向与逆向导航数据交替解算，实现基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准。
[0021]进一步的，所述建立SINS状态方程，具体为：
[0022][0023]其中，X为SINS系统的状态向量，为X的微分形式，F为SINS的状态矩阵，系统噪声W为零均值高斯白噪声。选取地理坐标系为参考系，并选取位置误差、速度误差、姿态误差、陀螺仪漂移与加速计零偏为状态量，则状态向量X为：
[0024][0025]其中，分别为北向、天向和东向失准角；δV
N
、δV
U
、δV
E
分别为北向、天向和东向速度误差；δL、δλ分别为纬度和经度误差；ε
N
、ε
U
、ε
E
分别为陀螺仪在北东天方向上的陀螺漂移；分别为加速度计北东天方向上的加速度计偏置，且状态矩阵F表示如下：
[0026][0027]其中，状态矩阵F内的各变量表示如下：
[0028][0029][0030][0031][0032][0033]上式中，R
M
,R
N
分别为载体所在位置地球子午圈和卯酉圈曲率半径，近似计算公式为：R
M
≈R
e
(1
‑
2e+3esin2L)与R
N
≈R
e
(1+esin2L)；Re为地球参考椭球的长半轴，e为参考椭球的扁率；V
N
、V
U
、V
E
为AUV在北天东方向上的速度，L和h分别表示载体的经度与深度；ω
ie
为地球自转速率；f
N
、f
U
、f
E
分别为载体在北天东方向上的比力；为AUV坐标系b到地理坐标系n的状态转移矩阵
[0034][0035]其中，φ、ψ、θ分别表示AUV的横滚角、航向角与偏航角。
[0036]进一步的，建立的SINS的误差微分方程，具体为：
[0037][0038]进一步的，所述建立的DVL误差模型，具体为：
[0039]δV
b
＝ΔC
·
V
b
+u；
[0040]其中，δV
b
和V
b
分别表示AUV坐标系下的测速误差与AUV的航行速度；u为量测噪声；ΔC为刻度系数误差；
[0041]SINS/DVL组合导航方式下，DVL坐标系与AUV坐标系间的速度关系表示为
[0042][0043]其中，为DVL坐标系d系到AUV坐标系b系之间的转换矩阵；V
d
表示DVL坐标系下测得的速度。
[0044]进一步的，建立的SINS/DVL的量测方程，具体为：
[0045]首先，在SINS在地理坐标系上的速度方程为
[0046]V
E
＝V
E0
+δV
E
[0047]V
N
＝V
N0
+δV
N
[0048]V<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于步骤如下：步骤1：建立SINS与DVL的数学模型，包括：(1.1)建立SINS的状态方程；(1.2)建立SINS的误差微分方程；(1.3)建立DVL的误差模型；(1.4)建立SINS/DVL的量测方程；步骤2：根据步骤1建立的SINS与DVL的数学模型，建立SINS/DVL的微分更新方程，包括：(2.1)建立姿态微分更新方程；(2.2)建立速度微分更新方程；(2.3)建立位置微分更新方程；步骤3：对姿态、速度和位置更新微分方程进行离散化处理，得到离散化更新方程；步骤4：根据步骤3得到的离散化更新方程，建立逆向解算的离散化更新方程；步骤5：对步骤3和步骤4得到的离散化更新方程进行卡尔曼滤波估计；步骤6：正向与逆向导航数据交替解算，实现基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准。2.根据权利要求1所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述建立SINS状态方程，具体为：其中，X为SINS系统的状态向量，为X的微分形式，F为SINS的状态矩阵，系统噪声W为零均值高斯白噪声；选取地理坐标系为参考系，并选取位置误差、速度误差、姿态误差、陀螺仪漂移与加速计零偏为状态量，则状态向量X为：其中，分别为北向、天向和东向失准角；δV
N
、δV
U
、δV
E
分别为北向、天向和东向速度误差；δL、δλ分别为纬度和经度误差；ε
N
、ε
U
、ε
E
分别为陀螺仪在北东天方向上的陀螺漂移；分别为加速度计北东天方向上的加速度计偏置，且状态矩阵F表示如下：其中，状态矩阵F内的各变量表示如下：
上式中，R
M
,R
N
分别为载体所在位置地球子午圈和卯酉圈曲率半径，近似计算公式为：R
M
≈R
e
(1
‑
2e+3esin2L)与R
N
≈R
e
(1+esin2L)；R
e
为地球参考椭球的长半轴，e为参考椭球的扁率；V
N
、V
U
、V
E
为AUV在北天东方向上的速度，L和h分别表示载体的经度与深度；ω
ie
为地球自转速率；f
N
、f
U
、f
E
分别为载体在北天东方向上的比力；为AUV坐标系b到地理坐标系n的状态转移矩阵其中，φ、ψ、θ分别表示AUV的横滚角、航向角与偏航角。
3.根据权利要求2所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述步骤(1.2)建立的SINS的误差微分方程，具体为：4.根据权利要求3所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述步骤(1.3)建立的DVL误差模型，具体为：δV
b
＝ΔC
·
V
b
+u；其中，δV
b
和V
b
分别表示AUV坐标系下的测速误差与AUV的航行速度；u为量测噪声；ΔC为刻度系数误差；SINS/DVL组合导航方式下，DVL坐标系与AUV坐标系间的速度关系表示为其中，为DVL坐标系d系到AUV坐标系b系之间的转换矩阵；V
d
表示DVL坐标系下测得的速度。5.根据权利要求4所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述步骤(1.4)建立的SINS/DVL的量测方程，具体为：SINS在地理坐标系上的速度方程为V
E
＝V
E0
+δV
E
V
N
＝V
N0
+δV
N
V
U
＝V
U0
+δV
U
其中，V
N0
、V
U0
、V
E0
分别表示SINS在北天东方向上测得AUV的理想速度；δV
N
、δV
U
、δV
E
分别表示SINS在北天东方向上的速度误差；在地理坐标系n下DVL在东北天方向上的速度为在地理坐标系n下DVL在东北天方向上的速度为在地理坐标系n下DVL在东北天方向上的速度为其中，分别表示地理坐标系n下DVL在北天东方向上测得的速度；
分别表示量测误差在东向、北向和天向上的投影，分别表示量测误差在东向、北向和天向上的投影，分别表示量测值随机干扰误差在东向、北向和天向上的投影；将SINS解算后的速度与DVL测得的速度在地理坐标系下的投影作差得到SINS/DVL的量测方程Z(t)为6.根据权利要求5所述的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，其特征在于：所述步骤(2.1)建立的SINS姿态微分更新方程...

【专利技术属性】
技术研发人员：董权威，岳才谦，王奥博，王亭亭，田锋，
申请(专利权)人：中国航天空气动力技术研究院，
类型：发明
国别省市：