一种水下自主导航定位系统的标定方法

1.本发明涉及水下导航定位技术领域，尤其涉及一种水下自主导航定位系统的标定方法。


背景技术：

2.因为水下环境封闭、无线电信号衰减等问题使得水下可用的导航方式非常有限。而捷联惯性导航系统（strapdown inertial navigation system，简称“sins”）以其独立性、自主性、精度高以及体积小等特点，成为水下导航中较为实用的导航方式。但sins的误差会随时间积累，因此多采用捷联惯性导航系统（strapdown inertial navigation system, 简称sins）与多普勒计程仪（doppler velocity log，简称dvl）组合的方式进行自主导航。
3.sins/dvl组合导航定位系统中的dvl存在测速误差，需要做标定、补偿，而且sins和dvl之间不可避免存在安装误差角，也需要标定、补偿。现有的技术标定流程复杂，可操作性低，所得到的标定参数不完整，标定的参数是否准确，没有可验证的手段。
4.具体的，在sins/dvl组合导航定位系统中，影响系统精度的跟dvl有关的主要原因有3个：（1）dvl的测速精度：dvl是利用声波信号的多普勒效应进行测速，声波信号在水中的传播速度与海水的温度、深度以及盐度等因素有关，因此，dvl在不同的环境下的速度刻度系数和零偏都有变化，从而导致测量的速度不准确。
5.（2）dvl与sins之间的安装误差角：在实际情况中，导航定位系统中的sins与dvl之间不可避免地存在安装误差角。如图1所示。安装误差会引起sins/dvl组合导航定位系统的测速误差，从而影响系统的定位精度。
6.（3）dvl的杆臂引起的速度误差：dvl通常安装在载体的底部，通常情况下它的安装位置和载体的捷联惯性导航系统中心点之间存在着杆臂。如图2和3所示，如果作为载体的水下机器人存在姿态角动态，杆臂效应产生的速度（v=ω
×
r）会被计入dvl的量测值，该速度并不是auv真实的线运动速度，这将直接导致导航定位误差。
7.解决上述问题的直接方法也具有以下问题：（1）用声速仪提前测试不同环境下的声速，因为局部海域的温度、盐度不是常值，导致声速在变化，而且声速仪本身也有误差。
8.（2）安装sins和dvl时，尽量保证它们的测量轴与载体坐标系（比如“右前上”）一致，然后sins、dvl和auv的轴系都是虚拟、不可见的，很难保证让其重合、或平行。
9.（3）将dvl尽量装在载体的中心位置，但这在实际应用时难以保证，因为需要优化整个载体的传感器布局，尽量规避推进器的水流对dvl造成的影响。
10.因此，“减少dvl测速误差”、“消除dvl安装误差”和“消除dvl的杆臂误差”等问题，用直接的解决方法难度很大，工作量巨大，一般采用间接方法，比如以第三方提供的外观测信息，采用最优估计理论，估计出dvl刻度系数误差、零偏和安装误差角等。
11.中国发明专利，申请号为：cn202010817669.5《一种基于quest的sins/dvl安装误差标定方法》，是将dvl标度因数误差与安装误差角进行解耦，对sins与dvl之间的安装误差角进行在线标定，忽略了dvl标度因数误差和零偏的影响。
12.中国发明专利，申请号为：cn201310041084.9《一种用于水下sins和dvl组合导航系统的参数标定方法》，设计了sins和dvl组合导航定位系统的dvl参数标定方法，标定时试验路程需要折返多次，每次的距离8km，以此完成4次校准。这在水域较小时，比如水池中，方法实施的效果会大打折扣，甚至不可能完成。
13.现有文献中标定dvl时，需要借助一种提供外参考速度的仪器，然后设计卡尔曼滤波完成对dvl安装误差、刻度因数误差和常值误差的估计。该方法的标定精度直接取决于外参考速度基准的精度。在水面和水下，外参考速度基准一般采用更高精度的dvl，它在安装时，同样存在安装误差，导致标定结果不准确。


技术实现要素：

14.本发明所要解决的技术问题是克服现有技术中存在的不足，提供一种水下自主导航定位系统的标定方法，使标定参数全面、标定流程简单、可操作性行强，且外参考基准要求低。
15.本发明是通过以下技术方案予以实现：一种水下自主导航定位系统的标定方法，包括以下步骤：s1，粗标定：将导航定位系统的捷联惯性导航系统和多普勒计程仪安装在载体上，测量载体的捷联惯性导航系统中心点与多普勒计程仪的中心点的杆臂长度，设定捷联惯性导航系统的坐标系与多普勒计程仪的坐标系的三轴方向一致，使载体在水中沿直线匀速移动至终点获得终点位置坐标的观测值，通过终点位置坐标的观测值与导航定位系统的计算值获得刻度系数误差、纵摇安装误差角和方位安装误差角，完成粗标定；s2，精标定：建立关于25维状态向量的标定误差模型,将粗标定获得的刻度系数误差、纵摇安装误差角和方位安装误差角代入25维状态向量中的对应状态向量，使载体在水中沿着精标定路径移动，以载体的位置和速度为观测量，采用卡尔曼滤波器获得25维状态向量的最优估计值，若25维状态向量的最优估计值在合理区间且25维状态向量中的杆臂与机械测量的杆臂实际值一致，则将25维状态向量中刻度系数误差、纵摇安装误差角和方位安装误差角的最优估计值写入导航定位系统中，完成精标定。
16.根据上述技术方案，优选地，s1中杆臂长度包括在多普勒计程仪坐标系的三个坐标轴上的杆臂长度。
17.根据上述技术方案，优选地，s1的具体方法包括：载体在水中行进第一航次，第一航次的路径为直线，第一航次的实际距离为l1，导航定位系统计算的距离为l2，则多普勒计程仪的刻度系数误差为δk=(l
2-l1)/l1；载体沿着第一航次的路径在水中行进第二航次，导航定位系统计算出的终点坐标、起点坐标与观测获得的终点坐标之间的夹角为方位安装误差角；载体在室内水池进行航行实验，导航定位系统计算的终点高度为l4，则纵摇安装误差角为γ=
×
57.3。
18.根据上述技术方案，优选地，s2中25维状态向量包括：3维姿态误差角、3维速度误差、3维位置误差、3维陀螺常值漂移、3维加速度计零偏、多普勒计程仪的3个安装误差角、多普勒计程仪的刻度系数误差、多普勒计程仪的3维杆臂长度、多普勒计程仪的零偏、东向洋流分量和北向洋流分量。
19.根据上述技术方案，优选地，s2中卡尔曼滤波器对观测量采用压缩数据快速标定算法，其中：压缩数据快速标定算法具体为：载体在水中沿着精标定路径移动时，导航定位系统在解算过程中，剔除有害加速度和干扰角速度后进行每秒累加，得到并保存每秒的速度增量和角增量，载体移动结束时刻，将保存的每秒的角增量和速度增量在导航定位系统中进行多次的正向解算与逆向解算。
20.根据上述技术方案，优选地，完成精标定后验证导航定位系统的精度。
21.根据上述技术方案，优选地，完成标定的导航定位系统在使用过程中，采用卡尔曼滤波器进行校正，校正方法包括开环输出校正和闭环反馈校正。
22.根据上述技术方案，优选地，通过卡尔曼滤波器输出多普勒计程仪的误差估计值，根据误差估计值对多普勒计程仪的导航参数进行校正，完成开环输出校正。
23.根据上述技术方案，优选地，通过卡尔曼滤波器输出多普勒计程仪的误差最优估计值，将误差最优估计值反馈至捷联惯性导航系统的导航计算中，完成对输出的导航信息进行闭环反馈校正。
24.本发明的有益效果是：（1）粗、精两级标定方法：由于sins和dvl的坐标轴是虚拟、不可观，在装配时，难免存在较大的安装误差角。如果该误差角较大，将导致系统方程非线性较为严重，使得标准的卡尔曼滤波器对参数估计不准确，避免初始误差太大，导致滤波器发散的风险。
25.因此，先粗标定出sins与dvl之间的安装误差角，完成物理调整或数学补偿，确保该误差角达到较小的范围，为后续的精标定使用卡尔曼滤波器估计参数提供基础，缩小最优值搜索范围，缩短标定时间，避免初始误差大，导致滤波器发散的风险。
26.（2）精标定误差模型，精标定模型最大限度贴合实际应用情况，较完整地表达出标定的各项误差因素，避免模型不完整，因为误差分配错误导致参数估计错误。本方案中选用的误差状态量为25维，除了通常的安装误差角之外，考虑到了方位陀螺的刻度误差、洋流误差等因素。
27.为了保证卡尔曼滤波器能较好地估计出各项参数，需结合试验环境，在试验场中选用最优的标定路径，对各个参数进行最优估计，得到可信的结果，并进行标定验证。
28.（3）标定计算采用压缩数据快速标定算法，循环反复利用数据，可以缩短载体进行精标定水上试验的时间和航行距离，从而降低标定成本，使标定实验可以在室内水池中进行，并且只需在较短距离往返一次即可完成。
29.（4）外参考基准要求低，不需要昂贵的、精度更高的dvl或其它设备作为外参考基准，只需要事先测定出发点和终点的位置坐标，如果在水域较小时，使用全站仪测量；在室外开阔的大水域，只需要一般精度gps定位出出发点和终点的位置。
附图说明
30.图1示出了现有技术的dvl与sins之间的安装误差角示意图。
31.图2示出了现有技术的dvl的杆臂引起的速度误差的正面示意图。
32.图3示出了现有技术的dvl的杆臂引起的速度误差的侧面示意图。
33.图4示出了本发明的实施例的四波束janus结构dvl的“十”字型安装方式示意图。
34.图5示出了本发明的实施例的四波束janus结构dvl的
“×”
型安装方式示意图。
35.图6示出了本发明的实施例的s1调整dvl坐标系示意图。
36.图7示出了本发明的实施例的s1粗标定的第一个航次示意图。
37.图8示出了本发明的实施例的s1粗标定的第二个航次示意图。
38.图9示出了本发明的实施例的s2的阶梯形精标定路径的示意图。
39.图10示出了本发明的实施例的s2的往返型精标定路径的示意图。
40.图11示出了本发明的实施例的s2的8字形精标定路径的示意图。
41.图12示出了本发明的实施例的s2中压缩数据快速标定算法时序图。
42.图13示出了本发明的实施例的导航定位系统组合卡尔曼滤波器校正的工作原理图。
具体实施方式
43.为了使本技术领域的技术人员更好地理解本发明的技术方案，下面结合附图和最佳实施例对本发明作进一步的详细说明。
44.一种水下自主导航定位系统的标定方法，包括以下步骤：s1，粗标定：将导航定位系统的捷联惯性导航系统（sins）和多普勒计程仪（dvl）安装在载体上。载体包括但不限于无缆水下机器人（auv）、遥控无人潜水器（rov）和无人水面艇（usv）。捷联惯性导航系统（sins）和多普勒计程仪（dvl）均为导航定位系统的一部分。sins一般安装在载体中心位置，因为dvl的换能器的探头要没入水中，因此dvl安装在载体腹部。dvl通常采用janus结构的4波束dvl，如图4和5所示，其具有“十”字型和
“×”
型2种安装方式。sins和dvl安装在载体上后，需连接电源和通信电缆，经检查无误后，通电采集sins和dvl数据。
45.测量载体捷联惯性导航系统中心点与dvl中心点的杆臂长度。由于dvl安装在载体腹部，使探头伸入水中，载体捷联惯性导航系统中心点与dvl中心点之间存在距离。在大型auv中，艏艉方向的杆臂距离能达到数米。如果直接依赖精标定阶段的卡尔曼滤波器进行最优估计，由于可观性较差，因此收敛时间较长，甚至无法收敛到准确值。因此，在精标定之前，可以在计算机仿真的机械结构模型中测量杆臂长度，或直接在载体上用尺子测量，残余的误差在精标定中予以标定、补偿。杆臂长度包括在dvl坐标系的三个坐标轴上的杆臂长度。
46.如图6所示，设定捷联惯性导航系统的坐标系与多普勒计程仪的坐标系的三轴方向一致。由于sins与dvl安装在不同的位置点，因此不可能保证sins坐标系和dvl坐标系重合。由于sins坐标系和dvl坐标系无法重合,不可避免出现sins坐标系和dvl坐标系的坐标轴之间的安装误差角，且坐标系的三个坐标轴对应三个安装误差角。为了减小安装误差角，只能在安装时，尽量让两坐标系的中心点靠近，并且使两者的测量坐标轴方向（正负极性）保持一致，或者说使对应坐标轴相互平行。本次调整只能达到使sins与dvl两者之间的坐标系大致平行，由于两个坐标系不可见，不便于调整，一般还会存在较大的安装误差角。众所
周知，当角度θ＞5
°
时，sinθ不再近似等于θ，将导致在简化测量模型时，存在较大的非线性误差。因此，在粗标定的后续步骤中，需要将可能存在较大的安装误差角标定出来，采用物理方式（调整安装螺丝孔），或者用数字补偿技术（做坐标转换）的方式补偿掉，残余的小误差角留待精标定时补偿。
47.载体在码头系泊对准完毕后，即可进入下一步骤在水中进行粗标定航行实验，使载体在水中沿直线匀速移动至终点获得终点位置坐标的观测值，通过终点位置坐标的观测值与导航定位系统的计算值获得刻度系数误差、纵摇安装误差角和方位安装误差角。
48.如果在室外实验水池或其他开阔水域进行粗标定实验，可以通过全球导航卫星系统gnss观测载体获得观测路径；如果在室内实验水池进行粗标定实验，需要使用经纬仪或其他长度测量工具，给粗标定实验的始点、终点做好标志。
49.使载体在水中沿直线移动，得到载体的观测路径，导航定位系统得出计算路径，根据观测路径与计算路径得到误差，完成粗标定。
50.观测路径为通过外部测量获得载体在水中的航行路径，同时也是粗标定实验预先设计的路径，观测路径的信息中包括起点坐标和终点坐标。sins和dvl组成的导航定位系统在载体移动时计算出的坐标信息，包括导航定位系统的始点坐标和终点坐标。虽然观测得到的信息和计算出的信息都是对同一载体的同一运动过程的记录，但是因为导航定位系统存在误差，因此观测得到的信息和计算出的信息存在区别。因此以观测得到的信息为准，将观测得到的信息和计算出的信息之间的误差作为修正值，输入导航定位系统，即可提高导航定位系统的精度，完成粗标定。
51.误差包括dvl的刻度系数误差、纵摇安装误差角、方位安装误差角。
52.粗标定实验的具体实验方法如下：第一个航次如图7所示，预定路径和观测得到的路径是从点a到点b，距离为l1，导航定位系统的终点定位位置为点c，距离为l2。应修正l2，使其与l1一样长，导航定位系统的终点定位位置从点c移至点d。根据上述实验数据，能够得到dvl的刻度系数误差为δk=(l
2-l1)/l1。
53.第二个航次如图8所示，载体重复沿着第一个航次的路径移动。导航定位系统经过刻度系统的误差修正后，l2的长度与l1的长度基本相等，但是∠dab=θ不一定等于0。因为l1≈l2，因此可以认为l3是一段圆弧，测量出的l3长度，可计算得到图8中的dvl的方位安装误差角θ=l3/l1×
57.3。
54.也可以进行多个航次，不断修正dvl的刻度系数和方位安装角，使其尽可能接近真值，为后续的精标定提供更精准的初始值。
55.并且在室内实验时，点a的初始高度可定为0，在室内没有风浪干扰，导航定位系统的定位高度应保持为0。假设导航定位系统的点c高度为l4，则dvl的纵摇（俯仰）安装误差角为γ=l4/l1×
57.3。
56.完成粗标定后，dvl的误差较小，导航定位系统的定位误差也较小，这对于后续精标定步骤中的卡尔曼滤波算法的误差状态量估计非常有利。
57.s2，精标定，完成上述粗表定后，经过检验dvl的坐标系与sins坐标系基本一致，并且已将误差控制在接近用户定位误差要求的范围内。此时可以进入精标定步骤。精标定包括以下步骤：
建立关于25维状态向量的标定误差模型：25维状态向量包括sins的误差、dvl的误差和洋流影响的误差。sins的误差包括3维姿态误差角、3维速度误差、3维位置误差、3维陀螺常值漂移、3维加速度计零偏，共15个状态量。dvl的误差包括dvl的3个安装误差角、dvl刻度系数、dvl的3维杆臂长度、dvl的零偏，共8个状态量。洋流误差包括东向洋流分量和北向洋流分量，共2个状态量。
58.标定误差模型包括系统误差方程和观测方程。
59.系统误差方程为： 。
60.式中，，为状态转移矩阵， ，表示由sins测得的载体在导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系的投影，为sins的加速度计所测量相对于导航坐标系的比力值，为载体坐标系与导航坐标系之间的姿态变换矩阵的转置矩阵， ，为地球子午圈主曲率半径，为地球卯酉圈主曲率半径，为载体当前高度值，为载体当前纬度值，，，为由sins测得的载体相对于惯性坐标系的旋转角速度在地球坐标系的投影，，为sins在导航坐标系下测量得到的东向速度值，表示sins在导航坐标系下测量得到的北向速度值，，表示sins在导航坐标系下测量得到的速度值，表示由sins测得的载体在导航坐标系相对于惯性坐标系的旋转角速度在地球坐标系的投影，表示由sins测得的载体在导航坐标系相对于地球坐标系的旋转角速度在导航坐标系的投
影，，，。
61.为25维状态向量，，为3维姿态误差角，为3维速度误差，为3维位置误差，为3维陀螺常值漂移，为3维加速度计零偏，为dvl的3个安装误差角，为dvl刻度系数，为dvl的3维杆臂长度，为dvl的零偏，为东、北向洋流分量，组成25维状态向量。
62.为根据sins、dvl和使用的gnss精度等级设置的系统噪声。
63.观测方程为：
ꢀꢀ
。
64.式中，，为gnss所观测的位置向量，为gnss所观测的速度向量， 为dvl所观测的速度向量， 为深度计所观测的深度向量 。
65.，表示dvl所测得运载体在导航坐标系下的速度信息，为载体坐标系与导航坐标系之间的姿态变换矩阵的转置矩阵，为单位矩阵。
66.为根据sins、dvl和使用的gnss精度等级设置的量测噪声。
67.首先将粗标定获得的刻度系数误差、纵摇安装误差角和方位安装误差角代入25维状态向量中的对应值。
68.设计精标定路径，精标定时如果采用全球导航卫星系统gnss观测载体，则只能获得3个观测量。而上述标定误差模型有25个状态量，因此，需要通过设定精标定路径，提高各状态量的可观性，使卡尔曼滤波器能完整、准确地估计出25维状态向量中的各个误差状态量。根据ins可观性理论，高度通道一般是自由独立的，只要做一些简单的下潜、上浮等机动，甚至通过观察标定始点和终点的导航定位系统的高度变化，就可以很方便地将dvl纵摇安装误差角和天向加速度计零偏估计出来；auv作为载体，在任务中大多数时间采取匀速直线航行，再配合一些拐弯、掉头等机动，时间维持较长一段时间（中等精度sins在10分钟以上），就可以完成dvl刻度系数误差、零偏、杆臂和大部分sins误差参数估计。典型的精标定
路径如图9、10、11所示，根据实际的试验场景，选择恰当的标定途径。
69.航车拖动载体在水中沿着精标定路径移动，导航定位系统和gnss观测得到载体的的位置和速度，基于标定误差模型建立卡尔曼滤波器，将上述观测量（即载体的的位置和速度）输入卡尔曼滤波器求得25维状态向量的最优估计值。如果导航定位系统的计算能力较强，满足滤波周期（一般情况下，状态更新100hz，量测更新1hz）的计算要求，可以实时估计、补偿，在线观察标定的效果；如果计算能力不够，可以对观测量数据采用压缩数据快速标定算法进行事后处理，它的好处是可以反复调整滤波器的参数，得到最优的结果。
70.如图12所示，卡尔曼滤波器对观测量数据采用压缩数据快速标定算法进行处理，压缩数据快速标定算法具体为：导航定位系统解算姿态、速度和位置过程中，将剔除有害值后的加速度和角速度进行每秒累加，得到并保存每秒的角增量和速度增量，载体移动结束时刻，将保存的数据在导航定位系统中进行多次的正向解算与反向解算。这样可以缩短载体的实验时间，比如保存时长10分钟的数据，正反运行一次，即可等效于20分钟。这对标定的卡尔曼滤波器的稳定非常有帮助，因为对于中等精度的光纤陀螺惯组（精度等级约为0.01
°
/h），卡尔曼滤波器的稳定时间约20分钟，而实际实验仅需进行10分钟。这样可以节省实验时间和成本。
71.卡尔曼滤波器具体如下：标准卡尔曼滤波递推过程由：
①
状态一步预测、
②
一步预测均方差阵、
③
滤波增益、
④
状态最优估计、
⑤
状态估计均方差阵等五个步骤构成，卡尔曼滤波递推公式为：。
72.其中，为状态向量的一步预测值，为状态转移矩阵，为上一时刻状态向量的估计值，为一步预测均方误差值，为上一时刻均方误差值，为状态转移矩阵的转置，表示系统噪声驱动阵，为上一时刻系统噪声方差阵，表示系统噪声驱动阵的转置，为状态预测误差与量测预测误差之间协方差，为量测矩阵的转置，为量测预测误差均方差，为量测矩阵，为噪声序列量测方差阵，为卡尔曼滤波增益，为量测向量，为单位矩阵，为卡尔曼滤波增益的转置矩阵。
73.若25维状态向量的最优估计在合理区间，则将25维状态向量的最优估计写入导航定位系统中，完成精标定。具体的判断方法为：观测卡尔曼滤波器的25维状态向量中各项参数的估计协方差，如果估计协方差在合理区间，则完成精标定。
74.完成精标定后，对标定结果进行验证。在各种场景下进行导航定位实验验证，以检
验标定参数的有效性。例如auv作为载体时，使auv按往返路径在水中移动，导航定位系统测量位置、速度与gnss提供的位置、速度进行实时对比，得到定位误差，判断定位误差是否符合需求。
75.完成标定的导航定位系统在实际使用过程中，采用卡尔曼滤波器进行校正，校正方法包括开环输出校正和闭环反馈校正。开环输出校正是通过卡尔曼滤波器直接输出dvl误差估计值，然后对dvl的导航参数进行校正；闭环反馈校正则是通过卡尔曼滤波器将误差估计值输出以后，反馈至sins的导航计算中，从而对输出的航向、姿态以及位置等信息进行校正。工作原理如图13所示。
76.本发明的有益效果是：（1）粗、精两级标定方法：由于sins和dvl的坐标轴是虚拟、不可观，在装配时，难免存在较大的安装误差角。如果该误差角较大，将导致系统方程非线性较为严重，使得标准的卡尔曼滤波器对参数估计不准确，甚至发散。
77.因此，先粗标定出sins与dvl之间的安装误差角，完成物理调整或数学补偿，确保该误差角达到较小的范围，为后续的精标定使用卡尔曼滤波器估计参数提供保障。
78.（2）精标定误差模型：精标定模型最大限度贴合实际情况，较完整地表达出各种误差因素，本方案中选用的误差状态量为25维，除了通常的安装误差角之外，考虑到了方位陀螺的刻度误差、洋流误差等因素。
79.为了保证卡尔曼滤波器能较好地估计出各项参数，需结合试验环境，在试验场中选用最优的标定路径，对各个参数进行最优估计，得到可信的结果，并进行标定验证。
80.（3）导航定位系统采用压缩数据快速标定算法：可以缩短载体进行精标定水上实验的时间和航行距离，从而降低实验成本，使标定实验可以在室内水池中完成。
81.（4）外参考基准要求低：不需要昂贵的、精度更高的dvl或其它设备作为外参考基准，只需要事先测定出发点和终点的位置坐标，如果在水域较小时，使用全站仪测量；在室外开阔的大水域，只需要一般精度gps定位出出发点和终点的位置。
82.以上仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。