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

摘要

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

说明书

技术领域

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

背景技术

在水下导航领域，应用比较成熟的是捷联惯性导航系统(SINS)与多普勒速度仪(DVL)组合导航系统。通过DVL实时提供的高精度速度信息，抑制SINS位置误差的发散，具有自主性与定位精度高的特点，因此广泛运用于AUV水下导航。惯导系统在正式进入导航任务前，需要完成初始对准，且初始对准的两个重要指标是精确性和快速性，惯导系统的对准精度直接影响着AUV的导航精度，而对准的快速性决定了AUV的是否能够快速投入使用，而这两者之间往往是互相矛盾的。

传统的岸上静基座初始对准虽然不需要外部提供航行器的运动信息，能够直接实现对惯导系统的补偿与修正，但需要较长的对准时间，会在很大程度上降低AUV的机动能力，限制其使用范围。因此在一些任务场景下，当要求AUV能够快速响应且快速集结投入使用时，则不再适用。因此，研究一种能够快速估计失准角误差的对准方法在AUV工程应用领域具有十分重要的意义。不同与岸上静基座对准，AUV航行过程中的行进间对准需要引入外部量测信息辅助完成初始对准。由于DVL为自主测速设备，能够直接测量出AUV在航行过程中的速度信息，且其自主性强、精度高，满足在水下和在一定速度范围内的使用要求。

通常AUV的初始对准采用正向导航解算方法进行处理，这种方式需要利用较多的导航数据进行误差估计，估计时间较长，且估计精度不高。因此针对上述问题，提出一种正向与逆向结合的处理方法。通过对AUV在对准阶段导航系统内部陀螺仪与加速计产生的导航信息进行存储，然后对该信息反复进行正向与逆向的导航解算，通过这种方式可以充分利用在对准阶段产生的导航数据，使得系统可以在较短的时间内利用较少的导航数据进行初始对准，并得到较高的估计精度。最终利用仿真设计了一条模拟AUV水下执行海底地形测绘任务的航行轨迹，并对航行开始前一段时间进行对准角误差估计，通过在不同对准时间的估计误差，对常规的对准方法与该发明提出的对准方法进行对比分析，验证了该方法的可行性与有效性。此外，通过仿真还验证了在大角度失准角条件下，采用该方法仍能快速准确估计出系统的失准角误差，由此证明了该方法的通用性。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供了一种基于逆向解算的领航AUV导航数据后处理方法，解决了载波跟踪环动态性能与低噪声的矛盾问题。

本发明的技术解决方案是：

一种基于捷联惯导系统与多普勒速度仪组合的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行进间快速初始对准。

进一步的，所述建立SINS状态方程，具体为：

其中，X为SINS系统的状态向量，

其中，

其中，状态矩阵F内的各变量表示如下：

上式中，R

其中，φ、ψ、θ分别表示AUV的横滚角、航向角与偏航角。

进一步的，建立的SINS的误差微分方程，具体为：

进一步的，所述建立的DVL误差模型，具体为：

δV

其中，δV

SINS/DVL组合导航方式下，DVL坐标系与AUV坐标系间的速度关系表示为

其中，

进一步的，建立的SINS/DVL的量测方程，具体为：

首先，在SINS在地理坐标系上的速度方程为

V

V

V

其中，V

在地理坐标系n下DVL在东北天方向上的速度为

其中，

将SINS解算后的速度与DVL测得的速度在地理坐标系下的投影作差得到

因此，量测方程Z(t)为

进一步的，建立的SINS姿态微分更新方程，具体为：

各变量表示如下

其中，

进一步的，建立的SINS速度微分更新方程，具体为：

g

其中，f

进一步的，建立的SINS位置微分更新方程，具体为：

其中，

进一步的，所述步骤3建立的姿态、速度和位置的离散化更新方程，具体为：

其中，

且，

进一步的，所述步骤4建立的逆向导航解算的离散更新方程，具体为：

假设AUV在T

令p＝k-1，p-1＝k,并对上式变量下表进行替换、化简得到

其中，

进一步的，对所述的步骤3和步骤4得到的方程进行卡尔曼滤波估计。

上式中，

本发明与现有技术相比的有益效果是：

(1)本发明提出的一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，针对AUV在航行过程中的对准速度与对准精度无法同时满足的问题，进行相关研究与仿真。通过建立SINS与DVL的误差模型，对获取的导航信息进行正向与逆向反复交替解算，进一步增大了信息的利用率。

(2)通过在不同解算时间和不同解算次数的条件下进行仿真测试，并将该方法与传统的对准方法进行了综合对比。最终结果表明，利用该方法可实现在更短的对准时间内，进一步提高系统的估计精度，从而提升了系统整体的导航性能，并通过仿真对比对该方法进行了有效验证。

(3)通过仿真表明，该方法仍适用于AUV大失准角条件，可在保证估计精度的前提下进行快速估计。该对准方法可应用于AUV编队快速集结、AUV地形探测与海图测绘等领域，在理论研究与工程实践方面具有很好的指导和应用价值。

附图说明

图1：惯性导航系统工作原理图；

图2：SINS/DVL组合导航系统解算原理图；

图3：AUV航行轨迹示意图；

图4：100s内正向解算的失准角估计误差曲线；

图5：100s内正逆向交替解算3次的失准角估计误差曲线；

图6：100s内正逆向交替解算6次的失准角估计误差曲线；

图7：300s内正向解算的失准角估计误差曲线；

图8：300s内正逆向交替解算3次的失准角估计误差曲线；

图9：300s内正逆向交替解算6次的失准角估计误差曲线；

图10：大失准角下300s内正向解算的失准角估计误差曲线；

图11：大失准角下300s内正逆向交替解算3次的失准角估计误差曲线；

图12：大失准角下300s内正逆向交替解算6次的失准角估计误差曲线。

具体实施方式

现结合实例、附图、附表对本发明的特征做进一步描述：

本发明提出一种基于捷联惯导系统与多普勒速度仪组合的AUV行进间快速初始对准方法，步骤如下：

步骤1：建立SINS与DVL的数学模型，包括：

(1)建立SINS的状态方程；

具体的，根据SINS的工作原理，如图1所示，建立SINS状态方程，具体为：

其中，X为SINS系统的状态向量，

其中，

其中，状态矩阵F内的各变量表示如下：

上式中，R

其中，φ、ψ、θ分别表示AUV的横滚角、航向角与偏航角。

(2)建立SINS的误差微分方程；

建立的SINS的误差微分方程，具体为：

(3)建立DVL的误差模型；

建立的DVL误差模型，具体为：

δV

其中，δV

SINS/DVL组合导航方式下，DVL坐标系与AUV坐标系间的速度关系表示为

其中，

(4)建立SINS/DVL的量测方程；

建立的SINS/DVL的量测方程，具体为：

首先，在SINS在地理坐标系上的速度方程为

V

V

V

其中，V

在地理坐标系n下DVL在东北天方向上的速度为

其中，

将SINS解算后的速度与DVL测得的速度在地理坐标系下的投影作差得到

因此，量测方程Z(t)为

步骤2：根据步骤1建立的SINS与DVL的数学模型，如图2所示，建立SINS/DVL的微分更新方程。

(1)建立姿态微分更新方程；

建立的SINS姿态微分更新方程，具体为：

各变量表示如下

其中，

(2)建立速度微分更新方程；

建立的SINS速度微分更新方程，具体为：

g

其中，f

(3)建立位置微分更新方程；

建立的SINS位置微分更新方程，具体为：

其中，

步骤3：对姿态、速度和位置更新微分方程进行离散化处理，得到离散化更新方程；

建立姿态、速度和位置的离散化更新方程，具体为：

其中，

且，

步骤4：根据步骤3，建立基于逆向解算的离散化更新方程；

假设AUV在T

令p＝k-1，p-1＝k,并对上式变量下表进行替换、化简得到

其中，

步骤5：根据步骤4，对得到的离散化方程进行卡尔曼滤波估计；

对所述的步骤3和步骤4得到的方程进行卡尔曼滤波估计。

上式中，

步骤6：正向与逆向导航数据交替解算；

按照上述步骤，可实现一次正向-逆向导航解算过程，据实际的解算结果，只进行一次正逆向解算虽然能够在一定程度上提高导航系统的估计精度，随着正逆向交替解算次数增加，系统失准角的估计精度与速度可进一步提高，但由于交替解算次数的无限增加会给导航系统带来计算量的问题，因此对该问题需做进一步工作。按照上述步骤，可以进行一次完整的正向和逆向处理过程，当进行多次正逆向处理时，只需对导航信息进行反复保存及加载，并修改系统仿真时间，可实现不同解算次数与不同解算时间的正逆向处理结果。

本发明实施例：

(1)导航参数初始化。

1)AUV的初始经纬度为(24°30',102°30')；

2)初始航行速度为2m/s；

3)初始姿态角分别为：偏航角ψ＝45°、俯仰角θ＝0°、横滚角

4)DVL测速误差为2mm±1％FS；

5)初始失准角误差分别为：北向失准角误差

6)当进行大失准角误差估计时，系统的初始失准角分别为：北向失准角误差

7)捷联惯导设备中陀螺仪与加速度计的误差参数分别为：陀螺仪常值漂移误差为(0.02°/h,0.02°/h,0.02°/h)、陀螺仪随机漂移误差为(0.02°/h,0.02°/h,0.02°/h)；加速度计零偏为(100ug,100ug,100ug)、陀螺仪随机偏移误差为(50ug,50ug,50ug)。

(2)根据初始化参数，进行AUV航行路径规划与仿真

设计AUV的航行路径为水下梳妆扫描路线，如图3所示。航行轨迹参数设置如下表

表1 AUV航行轨迹参数设置

开始时刻，AUV在水面以2m/s的速度沿正北方向航行，航行时间经过105s后，进入下潜状态。100s后到达预定35m的作业深度开始进行梳妆地形测绘过程，经过1900s后任务完成并开始上浮。AUV出水后按照设计的任务流程返回至起始原点，且设置为达到原点周围50m距离即为达到原点，整个航行过程持续时间为2710s。对AUV在航行阶段前一定时间内获取的导航信息进行不同次数和不同对准时间的正向解算和正逆向解算，并进行对比分析。

图4-图6为AUV航行开始前100s内的对准结果，其中图4为100s内的正向解算后得到的失准角估计误差曲线，图5为100s内正逆向交替解算3次后得到的失准角估计误差曲线，图6为100s内正逆向交替解算6次后得到的失准角估计误差曲线。结果表明，正逆向交替解算3次比只进行正向解算具有更快的估计速度，且估计精度更高，而由于100s内的导航信息包含的AUV状态信息仍不充分，导致正逆向解算6次的失准角估计误差发散，如图6所示。

图7-图9为AUV航行前300s内的对准结果，其中图7为前300s内正向解算的失准角估计误差曲线，图8为前300s内正逆向解算3次的失准角估计误差曲线，图9为前300s内正逆向解算6次得到的失准角估计误差曲线。与前100s相同，正逆向交替解算3次比只进行正向解算具有更快的估计速度，同时估计精度更高。与前100s估计结果不同的是，由于300s内包含的AUV状态信息更为充分，正逆向交替解算6次的估计精度比解算3次的估计精度更高，对失准角估计的性能有比较明显提升，如图9所示。

为验证该方法在AUV大失准角条件下仍然有效，进行了如下仿真分析、对比，如图10-图12。

其中，图10为AUV大失准角下前300s内正向解算的失准角估计误差曲线，图11为AUV大失准角下前300s内正逆向交替解算3次的失准角误差估计曲线，图12为AUV大失准角下前300s内正逆向交替解算6次的失准角误差估计曲线。仿真结果表明，当AUV处于大失准角情况下，利用该方法估计的失准角误差仍具有良好的估计精度，从而验证了该方法的具有较宽的适用范围，在AUV编队的快速集结、水下长时间航行与测绘等领域具有重要的工程指导与应用价值。

本发明说明书中未作详细描述的内容属于本领域技术人员的公知技术。