基于航姿和DVL的小型AUV导航定位的方法及装置与流程

基于航姿和dvl的小型auv导航定位的方法及装置
技术领域
1.本发明涉及导航技术领域，具体涉及一种基于航姿和dvl的小型auv导航定位的方法及装置。


背景技术：

2.自主式水下无人潜航器(autonomous underwater vehicle，auv)，由于其体积小、使用成本低、智能化自主作业、保障维护方便、隐蔽性好等诸多优点，在海洋开发等领域正发挥着越来越重要的作用。随着auv应用领域的逐渐扩展，对导航系统的要求也越来越高，需要具备远航程和长航时的高精度导航定位能力。因为高精度导航定位，决定着auv能否安全作业及返回，以及水下目标定位、海底地形测绘、水下定点布放等作业结果的准确性。目前，auv大多使用由fins、gps和dvl构成的组合导航系统。但成本过于高昂，且尺寸重量和功耗较大，不能很好满足小型auv对导航设备的需要。同时，有关小型auv水下导航定位的文献提到基于航姿和dvl的方式，但其仅使用了航向角和dvl三轴速度的方式，未考虑航行体的水平航行姿态(俯仰角、横滚角)对速度矢量在空间旋转中的作用。
3.针对上述不足，为了能够以较低的成本，本发明基于磁罗盘(或ahrs)和高精度dvl来实现水下导航定位，为小型auv获得较高精度的水下导航定位能力。


技术实现要素：

4.有鉴于此，本发明提供了一种基于航姿和dvl的小型auv导航定位的方法及装置，能够解决现有的auv普遍采用的gps/ins/dvl组合导航系统，存在设备尺寸重量过大无法在小型auv上安装、功耗较大、成本过高、系统标校复杂，以及仅采用航向角和dvl的方式未考虑速度矢量在空间中的旋转等技术问题。
5.为了解决上述技术问题，本发明是这样实现的。
6.一种基于航姿和dvl的小型auv导航定位的方法，包括：
7.步骤s1：基于多普勒计程仪(dvl)的标定参数，对dvl载体系下速度进行修正，修正后的速度记为vb；所述dvl配置于所述小型auv中；
8.步骤s2：将所述修正后的速度转换为地理系下的速度；
9.步骤s3：设置所述小型auv的导航系统的解算周期为dvl的测速周期；基于所述解算周期、当前解算周期的上一解算周期确定的所述小型auv的位置信息、以及所述地理系下的速度对所述小型auv的位置信息进行实时推算，实时确定所述小型auv的位置信息；若当前解算周期为第一个解算周期，所述上一解算周期确定的所述小型auv的位置信息为导航系统启动时刻的gps定位信息；
10.步骤s4：实时检测所述小型auv是否获得gps定位信息，若是，基于gps定位信息更新当前解算周期确定的所述小型auv的位置信息；否则，将实时推算的所述小型auv的位置信息作为当前解算周期确定的所述小型auv的位置信息。
11.优选地，所述步骤s1，其中，所述多普勒计程仪(dvl)的标定参数的确定方式包括：
12.步骤s11：所述dvl初次使用时，标定参数[ε,k]的初始值为[0,1]；其中，ε为磁罗盘/或ahrs和dvl的两者载体坐标系间航向安装夹角，k为所述dvl的标度因数；
[0013]
步骤s12：使所述小型auv在水域开展一段水下航行，在所述小型auv入水前设置所述导航系统不再使用gps定位信息来刷新导航系统的经纬度；获取num组所述小型auv在入水前和出水后的同步数据，每组同步数据包括两对数据，每对代表同一位置处所述小型auv的gps定位信息和所述小型auv推算的位置信息；两对数据代表的位置彼此不同，一对代表所述小型auv入水前的同步数据，另一对代表所述小型auv出水后的同步数据；
[0014]
步骤s13：对num组中的每一组所述小型auv的同步数据，均执行以下操作：
[0015]
获取当前组中所述小型auv的gps定位信息(la，λa)及(lb，λb)，获取当前组中所述小型auv的经解算获得的位置信息(l
a1
，λ
a1
)及(lc，λc)，其中(la，λa)与(l
a1
，λ
a1
)对应入水前的同一位置，(lb，λb)与(lc，λc)对应出水后的同一位置，la为auv入水前gps纬度，λa为auv入水前gps经度，l
a1
为auv入水前所述小型auv的经解算获得的纬度，λ
a1
为auv入水前所述小型auv的经解算获得的经度；lb为auv出水后gps纬度，λb为auv出水后gps经度，lc为auv出水后所述小型auv的经解算获得的纬度，λc为auv出水后所述小型auv的经解算获得的经度；
[0016]
则基于该组所述小型auv的同步数据，确定标定参数[ε,k]：
[0017][0018]
ε＝atan(d
n_gnss
/d
e_gnss
)-atan(d
n_dvl
/d
e_dvl
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0019]
其中：
[0020]dn_dvl
＝(l
c-l
a1
)
·
re
[0021][0022]dn_gnss
＝(l
b-la)
·
re
[0023][0024]
其中，re为地球半径，d
n_dvl
、d
e_dvl
分别为第一位置与第二位置对应的所述小型auv解算的位置之间的北向距离和东向距离；d
n_gnss
、d
e_gnss
分别为第一位置与第二位置之间的gps定位的实际的北向距离和东向距离；第一位置是所述小型auv入水前的位置，第二位置是所述小型auv入水潜行后露出水面的位置；
[0025]
步骤s14：对num组中每个组确定出的标定参数[ε,k]，去掉最大的ε值及最小的ε值，余下的ε值求和后取平均，作为最终的ε值；去掉最大的k值及最小的k值，余下的k值求和后取平均，作为最终的k值；将最终的ε值及最终的k值作为所述多普勒计程仪(dvl)的标定参数[ε,k]。
[0026]
优选地，所述步骤s1，包括：
[0027]
记dvl实时输出的b'系下所述小型auv的航速为使用dvl标定参数[ε,k]进行修正，得到即：
[0028][0029]b′
系为dvl载体坐标系。
[0030]
优选地，所述步骤s2，包括：
[0031]
所述小型auv的姿态由电子磁罗盘或ahrs实时测量获得，记实时姿态为att＝[ψ θ φ]
t
；dvl实时测量的修正后的航速为
[0032]
将修正后的速度vb转换为n系下的速度vn，n系为导航坐标系：
[0033]
si＝sin(ψ)sj＝sin(θ)sk＝sin(φ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0034]
ci＝cos(ψ)cj＝cos(θ)ck＝cos(φ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0035][0036][0037]
其中：ψ为实时航向角，θ为实时俯仰角，φ为实时横滚角，均由电子磁罗盘或ahrs实时给出。
[0038]
优选地，所述步骤s3，包括：
[0039]
设所述小型auv的导航系统的解算周期为δt，δt为所述小型auv的姿态和dvl两者中更新慢的更新周期；
[0040]
实时推算的所述小型auv的位置信息为：
[0041][0042][0043][0044]
其中，l
t-1
为上一解算周期确定的所述小型auv的纬度、λ
t-1
为上一解算周期确定的所述小型auv的经度，h
t-1
为上一解算周期确定的所述小型auv的海拔高度；l
t
为当前解算周期确定的所述小型auv的纬度、λ
t
为当前解算周期确定的所述小型auv的经度，h
t
为当前解算周期确定的所述小型auv的海拔高度，rm为当前位置处的地球卯酉圈半径，为当前dvl转换为地理系下北向速度，rn为当前位置处的地球子午圈半径，为当前dvl转换为地理系下东向速度，为当前dvl转换为地理系下天向速度。
[0045]
本发明所提供的一种基于航姿和dvl的小型auv导航定位的装置，所述装置包括：
[0046]
修正模块：配置为基于多普勒计程仪(dvl)的标定参数，对dvl载体系下速度进行修正，修正后的速度记为vb；所述dvl配置于所述小型auv中；
[0047]
转换模块：配置为将所述修正后的速度转换为地理系下的速度；
[0048]
解算模块：配置为设置所述小型auv的导航系统的解算周期为dvl的测速周期；基
于所述解算周期、当前解算周期的上一解算周期确定的所述小型auv的位置信息、以及所述地理系下的速度对所述小型auv的位置信息进行实时推算，实时确定所述小型auv的位置信息；若当前解算周期为第一个解算周期，所述上一解算周期确定的所述小型auv的位置信息为导航系统启动时刻的gps定位信息；
[0049]
更新模块：配置为实时检测所述小型auv是否获得gps定位信息，若是，基于gps定位信息更新当前解算周期确定的所述小型auv的位置信息；否则，将实时推算的所述小型auv的位置信息作为当前解算周期确定的所述小型auv的位置信息。
[0050]
本发明所提供的一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述方法。
[0051]
本发明所提供的一种电子设备，其特征在于，所述电子设备，包括：
[0052]
处理器，用于执行多条指令；
[0053]
存储器，用于存储多条指令；
[0054]
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述方法。
[0055]
有益效果：
[0056]
(1)本发明适用于小型auv，成本低、功耗小。
[0057]
(2)本发明导航系统标校简便，易于实施。
[0058]
(3)本发明综合考虑了小型auv的俯仰角、横滚角、航向角等信息，考虑了速度矢量在三维空间中的旋转，水下导航定位精度高。
附图说明
[0059]
图1为本发明提供的基于航姿和dvl的小型auv导航定位的方法流程示意图；
[0060]
图2为本发明提供的基于航姿和dvl的小型auv导航定位的原理示意图；
[0061]
图3为本发明提供的基于航姿和dvl的小型auv导航定位的方法的标定原理示意图；
[0062]
图4为本发明提供的基于航姿和dvl的小型auv导航定位的装置结构示意图。
具体实施方式
[0063]
下面结合附图和实施例，对本发明进行详细描述。
[0064]
定义：
[0065]
所涉及的坐标系定义如下：
[0066]
b系：惯性测量单元坐标系(右-前-上)；
[0067]b′
系：dvl载体坐标系(右-前-上)；
[0068]
n系：导航坐标系(东-北-天)；
[0069]
e系：地球坐标系，oxe轴在赤道平面内且指向本初子午线，oze轴平行于地球自转轴，oye由右手坐标系确定。
[0070]
如图1-2所示，本发明提出了一种基于航姿和dvl的小型auv导航定位的方法，包括如下步骤：
[0071]
步骤s1：基于多普勒计程仪(dvl)的标定参数，对dvl载体系下速度进行修正，修正
后的速度记为vb；所述dvl配置于所述小型auv中；
[0072]
步骤s2：将所述修正后的速度转换为地理系下的速度；
[0073]
步骤s3：设置所述小型auv的导航系统的解算周期为dvl的测速周期；基于所述解算周期、当前解算周期的上一解算周期确定的所述小型auv的位置信息、以及所述地理系下的速度对所述小型auv的位置信息进行实时推算，实时确定所述小型auv的位置信息；若当前解算周期为第一个解算周期，所述前一解算周期确定的所述小型auv的位置信息为导航系统启动时刻的gps定位信息；
[0074]
步骤s4：实时检测所述小型auv是否获得gps定位信息，若是，基于gps定位信息更新当前解算周期确定的所述小型auv的位置信息；否则，将实时推算的所述小型auv的位置信息作为当前解算周期确定的所述小型auv的位置信息。
[0075]
进一步地，将所述当前解算周期确定的所述小型auv的位置信息作为部署于所述小型auv的导航系统的经纬度信息。
[0076]
本发明中，当所述小型auv在码头、甲板或水面时，即所述小型auv未潜入水下时，能够获得gps定位信息。所述小型auv潜入水下时，不能获得gps定位信息，此时，能够获得dvl的测速信息，进而计算当前解算周期的所述小型auv的位置信息。本发明中，所述小型auv开机后一旦获得gps定位信息，则部署于所述小型auv的导航系统开始运行，当所述小型auv处于码头、甲板或水面时，一旦实时收到gps定位信息，则对导航系统的经纬度进行更新。当所述小型auv上浮出水后，接收gps并实时刷新导航系统的经纬度。
[0077]
如图3所示，所述步骤s1，其中，所述多普勒计程仪(dvl)的标定参数的确定方式包括：
[0078]
步骤s11：所述dvl初次使用时，标定参数[ε,k]的初始值为[0,1]；其中，ε为磁罗盘/或ahrs和dvl的两者载体坐标系间航向安装夹角，k为所述dvl的标度因数；
[0079]
步骤s12：使所述小型auv水域开展一段水下航行，在所述小型auv入水前设置所述导航系统不再使用gps定位信息来刷新导航系统的经纬度；获取num组所述小型auv在入水前和出水后的同步数据，每组同步数据包括两对数据，每对代表同一位置处所述小型auv的gps定位信息和所述小型auv推算的位置信息，两对数据代表的位置彼此不同，一对代表所述小型auv入水前的同步数据，另一对代表所述小型auv出水后的同步数据；
[0080]
本实施例中，所述同步数据是通过驱动所述小型auv潜行一段距离，并使之露出水面获得的。例如，所述小型auv潜行时，所述小型auv基于标定参数[ε,k]的初始值及dvl实时测速信息对所述小型auv的位置信息进行解算。在所述小型auv入水前设置导航系统不再使用gps定位信息来刷新导航系统的经纬度，之后记录并获取此时解算出的所述小型auv的位置信息和此时所述小型auv的gps定位信息；记录的所述小型auv的位置信息和此时所述小型auv的gps定位信息形成一对数据。所述小型auv在水下潜行一段距离后，在所述小型auv露出水面后，记录并获取此时解算出的所述小型auv的位置信息和此时所述小型auv的gps定位信息；记录的所述小型auv的位置信息和此时所述小型auv的gps定位信息形成一对数据。这两对数据形成了一组所述小型auv的同步数据。
[0081]
步骤s13：对num组中的每一组所述小型auv的同步数据，均执行以下操作：
[0082]
获取当前组中所述小型auv的gps定位信息(la，λa)及(lb，λb)，获取当前组中所述小型auv的经解算获得的位置信息(l
a1
，λ
a1
)及(lc，λc)，其中(la，λa)与(l
a1
，λ
a1
)对应入水前
的同一位置，(lb，λb)与(lc，λc)对应出水后的同一位置，la为auv入水前gps纬度，λa为auv入水前gps经度，l
a1
为auv入水前所述小型auv的经解算获得的纬度，λ
a1
为auv入水前所述小型auv的经解算获得的经度；lb为auv出水后gps纬度，λb为auv出水后gps经度，lc为auv出水后所述小型auv的经解算获得的纬度，λc为auv出水后所述小型auv的经解算获得的经度；
[0083]
本实施例中，所述小型auv的经解算获得的经度、纬度能够确定一个位置信息，第一位置是所述小型auv入水前的位置，因此，gps定位的位置与所述小型auv解算的位置都对应同一位置点。第二位置是所述小型auv入水潜行后露出水面的位置，因此，gps定位的位置是该第二位置的实际位置，所述小型auv解算的位置与该第二位置的实际位置可能存在一定的偏差。
[0084]
则基于该组所述小型auv的同步数据，确定标定参数[ε,k]：
[0085][0086]
ε＝atan(d
n_gnss
/d
e_gnss
)-atan(d
n_dvl
/d
e_dvl
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0087]
其中：
[0088]dn_dvl
＝(l
c-l
a1
)
·
re
[0089][0090]dn_gnss
＝(l
b-la)
·
re
[0091][0092]
其中，re为地球半径，d
n_dvl
、d
e_dvl
分别为第一位置与第二位置对应的所述小型auv解算的位置(lc，λc)之间的北向距离和东向距离；d
n_gnss
、d
e_gnss
分别为第一位置与第二位置之间的gps定位的实际的北向距离和东向距离；第一位置是所述小型auv入水前的位置，第二位置是所述小型auv入水潜行后露出水面的位置。
[0093]
步骤s14：对num组中每个组确定出的标定参数[ε,k]，去掉最大的ε值及最小的ε值，余下的ε值求和后取平均，作为最终的ε值；去掉最大的k值及最小的k值，余下的k值求和后取平均，作为最终的k值；将最终的ε值及最终的k值作为所述多普勒计程仪(dvl)的标定参数[ε,k]。
[0094]
进一步地，可以将所述多普勒计程仪(dvl)的标定参数[ε,k]写入flash。
[0095]
所述步骤s1：基于多普勒计程仪(dvl)的标定参数[ε,k]对dvl载体系下的所述小型auv的速度进行修正，修正后的速度记为vb，包括：
[0096]
记dvl实时输出的b'系下所述小型auv的航速为使用dvl标定参数[ε,k]进行修正，得到即：
[0097][0098]
所述步骤s2：将所述修正后的速度转换为地理系下的速度，包括：
[0099]
所述小型auv的姿态由电子磁罗盘或ahrs实时测量获得，记实时姿态为att＝[ψ θ φ]
t
；dvl实时测量的修正后的航速为
[0100]
将修正后的速度vb转换为n系下的速度vn：
[0101]
si＝sin(ψ)sj＝sin(θ)sk＝sin(φ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0102]
ci＝cos(ψ)cj＝cos(θ)ck＝cos(φ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0103][0104][0105]
其中：ψ为实时航向角，θ为实时俯仰角，φ为实时横滚角，均由电子磁罗盘或ahrs实时给出。
[0106]
所述步骤s3：设置所述小型auv的导航系统的解算周期为dvl测速周期，基于所述解算周期、当前解算周期的上一解算周期确定的所述小型auv的位置信息、以及所述地理系下的速度对所述小型auv的位置信息进行实时推算，实时确定所述小型auv的位置信息；若当前解算周期为第一个解算周期，所述前一解算周期确定的所述小型auv的位置信息为导航系统启动时刻的gps定位信息，包括：
[0107]
设所述小型auv的导航系统的解算周期为δt，δt为所述小型auv的姿态和dvl两者中更新慢的更新周期(一般为dvl更新周期)；
[0108]
实时推算的所述小型auv的位置信息为：
[0109][0110][0111][0112]
其中，l
t-1
为上一解算周期确定的所述小型auv的纬度、λ
t-1
为上一解算周期确定的所述小型auv的经度，h
t-1
为上一解算周期确定的所述小型auv的海拔高度；l
t
为当前解算周期确定的所述小型auv的纬度、λ
t
为当前解算周期确定的所述小型auv的经度，h
t
为当前解算周期确定的所述小型auv的海拔高度，rm为当前位置处的地球卯酉圈半径，为当前dvl转换为地理系下北向速度，rn为当前位置处的地球子午圈半径，为当前dvl转换为地理系下东向速度，为当前dvl转换为地理系下天向速度。
[0113]
本发明中，步骤s1，对所述多普勒计程仪(dvl)的标定参数进行确定的过程中，涉及对所述小型auv的位置信息进行解算，其解算方式与本发明的步骤s1至步骤s4相同，其中，步骤s1中的基于多普勒计程仪(dvl)的标定参数取值为[0,1]，在此不再赘述。
[0114]
本发明还提供了一种基于航姿和dvl的小型auv导航定位的装置，如图4所示，该装置包括：
[0115]
修正模块：配置为基于多普勒计程仪(dvl)的标定参数，对dvl载体系下速度进行
修正，修正后的速度记为vb；所述dvl配置于所述小型auv中；
[0116]
转换模块：配置为将所述修正后的速度转换为地理系下的速度；
[0117]
解算模块：配置为设置所述小型auv的导航系统的解算周期为dvl的测速周期；基于所述解算周期、当前解算周期的上一解算周期确定的所述小型auv的位置信息、以及所述地理系下的速度对所述小型auv的位置信息进行实时推算，实时确定所述小型auv的位置信息；若当前解算周期为第一个解算周期，所述上一解算周期确定的所述小型auv的位置信息为导航系统启动时刻的gps定位信息；
[0118]
更新模块：配置为实时检测所述小型auv是否获得gps定位信息，若是，基于gps定位信息更新当前解算周期确定的所述小型auv的位置信息；否则，将实时推算的所述小型auv的位置信息作为当前解算周期确定的所述小型auv的位置信息。
[0119]
以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。