一种水下机器人实时高精度经纬度推算方法技术

本发明专利技术涉及一种水下机器人实时高精度经纬度推算方法，包括以下步骤：利用超短基线对水下机器人进行定位，间断获取不连续的经纬度信息；利用船位推算方法获取实时的、连续的水下机器人相对位移信息；在当前周期收到一包有效的USBL定位数据时，对水下机器人的基准经纬度进行更新；在当前周期未收到有效的USBL定位数据时，则利用相对位移信息和基准经纬度对水下机器人所处位置的经纬度进行实时推算。本发明专利技术简单实用，大大提升了水下机器人在水下导航定位时经纬度的精度及实时性。

【技术实现步骤摘要】
一种水下机器人实时高精度经纬度推算方法
本专利技术涉及水下机器人的导航定位
，具体地说是一种水下机器人实时高精度经纬度推算方法。
技术介绍
人类为了开发海洋，就应该了解海洋，观察和考察海洋，由于水下机器人能够在水下进行观察、摄像、打捞和施工作业，因此在海洋开发中得到广泛的应用，其开发技术受到了各国的极大重视。由于深入水下，水下机器人一般采用超短基线水声定位系统(USBL)完成对水下机器人在海底的经纬度定位，引导水下机器人到达作业目的地以及对特定目标进行位置标定等，是水下机器人能顺利完成水下任务的关键技术之一。水下机器人工作深度一般为水下数千米，在如此大的深度下，USBL水声定位系统会产生长达数秒的时延，并会因为声速随深度的变化而产生较大误差。另外，水下机器人作业过程中水面母船和自身推进器所产生的噪声，会对USBL产生巨大的干扰，导致定位精度进一步降低，难以满足水下机器人导航定位的需求。为了提高水下机器人的USBL定位精度，常用的方法为将水下机器人坐底或者悬停，然后获取多个USBL的定位数据后，将野值去掉，再平均的方法。该方法为了获得较高的USBL定位精度，需要水下机器人尽量静止，如果需要多次进行精确定位，则耗时较多，浪费了水下机器人水下宝贵的作业时间。因此研究一种简单实用的水下机器人水底经纬度推算方法，不需水下机器人近似静止，同时还可以连续的、实时的输出高精度的经纬度定位数据，提升水下机器人水底有效的作业时间，是目前水下机器人控制中的一个重点需要解决的问题。专利技术内容针对传统USBL经纬度定位方法的不足，本专利技术提供一种即简单又实用的经纬度实时高精度推算方法，该方法可融合导航传感器和USBL的定位数据，有效提高水下机器人经纬度推算精度，减少ROV坐底和悬停的时间。同时与传统推算方法方法相比，增加的运算量也可以忽略不计。本专利技术为实现上述目的所采用的技术方案是：一种水下机器人实时高精度经纬度推算方法，包括以下步骤：利用USBL对水下机器人进行定位，获取不连续的经纬度信息；利用船位推算方法获取实时连续的水下机器人相对陆地的位移信息；收到USBL的定位数据时，对该周期水下机器人的基准经纬度进行推算；在未收到USBL的定位数据的周期时，利用水下机器人相对陆地的位移信息和基准经纬度对水下机器人的经纬度进行实时推算。每次通过USBL获取一包水下机器人定位数据时，记录下此时USBL定位的经纬度和经船位推算方法获取的相对陆地的位移信息。所述收到USBL的定位数据时，对该周期水下机器人的基准经纬度进行推算，包括以下步骤：当收到USBL的定位数据组数大于或等于N时，以前面记录的N-1组数据推算当前时刻水下机器人的经纬度，连同第N组USBL定位数据，一共获得N个经纬度数据；推算公式如下：J(i)＝J0(k-i)+(SE(k)-SE(k-i))ρ1W(i)＝W0(k-i)+(SN(k)-SN(k-i))ρ2其中，J(i)为第i组推算得到的经度，W(i)为第i组推算得到的纬度；J0(k-i)为第k-i组记录的USBL定位经度，W0(k-i)为第k-i组记录的USBL定位纬度，SE(k)为第k组记录的水下机器人相对陆地的东向位移，SN(k)为第k组记录的水下机器人相对陆地的北向位移，k代表当前数据组的序号，k≥N-1；SE(k-i)为第k-i组记录的水下机器人相对陆地的东向位移，SN(k-i)为第k-i组记录的水下机器人相对陆地的北向位移，i＝0、1、2……N-1；ρ1、ρ2分别为当前位置的经度曲率和纬度曲率。所述基准经纬度通过以下步骤得到：分别将推算的N个经、纬度数据按照大小进行排序，取中间的N/2个值并进行平均，将平均值作为k组数据对应时刻精度更高的经度和纬度数据，即新的基准经纬度，分别记为Jref，Wref。在程序执行的每一周期，利用水下机器人相对陆地的位移信息和基准经纬度对水下机器人的经纬度进行实时推算，如下：J＝Jref+(SE-SE(k))ρ1W＝Wref+(SN-SN(k))ρ2其中，Jref、Wref分别为基准经、纬度，J为实时推算的经度，W为实时推算的维度，SE为相对陆地的实时东向位移值，SN为相对陆地的实时北向位移值；SE(k)、SN(k)分别为第k组水下机器人相对陆地的东向位移、第k组水下机器人相对陆地的北向位移；ρ1、ρ2分别为当前位置的经度曲率和纬度曲率。本专利技术具有以下有益效果和优点：1.本专利技术的经纬度推算方法，可以在线运行，实时输出水下机器人的当前经纬度，将动辄3秒以上的USBL定位周期提升到200ms级别。2.本专利技术的经纬度推算方法，可以将USBL获取的不连续的、跳跃的水下运动定位轨迹修正为连续的、平滑的轨迹，可避免操作人员因原始USBL定位数据跳跃而增加的过多操作。3.本专利技术的经纬度推算方法，不需要坐底或者悬停，当到达水底运行一段时间后，即可实时输出高精度的经纬度信息，有效增加了水下机器人水底执行任务的时间。4.本专利技术的经纬度推算方法，在水下机器人在水底执行任务且USBL定位失效的情况下，也可在较长一段时间内维持较高精度的经纬度推算。5.本专利技术的经纬度推算方法，仅是将连续获取的N个USBL定位数据与船位推算获取的相对位置进行融合，在当今计算机技术飞速发展的情况下，未明显增加计算机的负担，调试也简单易行。附图说明图1是本专利技术的组成示意图；图2是本专利技术的方法执行流程图。具体实施方式下面结合附图及实施例对本专利技术做进一步的详细说明。本专利技术由水下机器人、母船、USBL(超短基线定位系统)、控制计算机组成，其中水下机器人需要搭载深度计、DVL(多普勒计程仪)、光纤陀螺、声信标等，如图1所示。其中由USBL系统获取水下机器人的定位经纬度信息，由深度计获取水下机器人与水面的距离，由DVL、光纤陀螺等输出的数据进行融合滤波，然后经船位推算获取水下机器人的相对陆地的位移信息(一般为北向位移和东向位移)。本专利技术方法的推算流程图如图2所示。当控制软件接收到有效(数据值在阈值范围内)的USBL定位经纬度数据时，记录该经纬度及该时刻的相对位移，并更新基准经纬度；紧接着以基准经纬度和相对位移变化量信息实时推算水下机器人的经纬度。第一步，判断当前周期是否接收到了有效的USBL定位数据，是，则将USBL定位的经纬度及该时刻对应的船位推算的相对位移信息作为一组数据，记录到程序的内存中，并进入第二步；否，则进入第四步。第二步，判断记录的数据组数是否已经大于等于N，是，则依据前面记录的N-1组数据，推算获取N-1个当前位置的经纬度，加上当前USBL定位的经纬度，一共N个，接着进入第三步；否则返回第一步。该N-1个经纬度推算方式为：首先，分别以当前记录的相对位移(北向位移和东向位移)作为参考，分别与前面N-1个记录的相对位移(北向位移和东向位移)作差；接着，分别计本文档来自技高网...

【技术保护点】
1.一种水下机器人实时高精度经纬度推算方法，其特征在于包括以下步骤：/n利用USBL对水下机器人进行定位，获取不连续的经纬度信息；/n利用船位推算方法获取实时连续的水下机器人相对陆地的位移信息；/n收到USBL的定位数据时，对该周期水下机器人的基准经纬度进行推算；/n在未收到USBL的定位数据的周期时，利用水下机器人相对陆地的位移信息和基准经纬度对水下机器人的经纬度进行实时推算。/n

【技术特征摘要】
1.一种水下机器人实时高精度经纬度推算方法，其特征在于包括以下步骤：
利用USBL对水下机器人进行定位，获取不连续的经纬度信息；
利用船位推算方法获取实时连续的水下机器人相对陆地的位移信息；
收到USBL的定位数据时，对该周期水下机器人的基准经纬度进行推算；
在未收到USBL的定位数据的周期时，利用水下机器人相对陆地的位移信息和基准经纬度对水下机器人的经纬度进行实时推算。


2.根据权利要求1所述的一种水下机器人实时高精度经纬度推算方法，其特征在于，每次通过USBL获取一包水下机器人定位数据时，记录下此时USBL定位的经纬度和经船位推算方法获取的相对陆地的位移信息。


3.根据权利要求1所述的一种水下机器人实时高精度经纬度推算方法，其特征在于，所述收到USBL的定位数据时，对该周期水下机器人的基准经纬度进行推算，包括以下步骤：
当收到USBL的定位数据组数大于或等于N时，以前面记录的N-1组数据推算当前时刻水下机器人的经纬度，连同第N组USBL定位数据，一共获得N个经纬度数据；推算公式如下：
J(i)＝J0(k-i)+(SE(k)-SE(k-i))ρ1
W(i)＝W0(k-i)+(SN(k)-SN(k-i))ρ2
其中，J(i)为第i组推算得到的经度，W(i)为第i组推算得到的纬度；J0(k-i)为第k-i组记录的USBL定位经度，W0(k-i)为第k-i组记录的U...

【专利技术属性】
技术研发人员：崔胜国，冀萌凡，张奇峰，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：辽宁;21