[发明专利]一种数据融合导航定位方法

说明书

本发明公开了一种数据融合导航定位方法，步骤1：测量惯性坐标系下水下机器人相对于海底速度，对速度进行积分测得相对位移；步骤2：通过双目摄像机获取目标图片，通过双目测距分别获得相邻时刻水下机器人在双目坐标系下位置，然后得到相邻时刻在惯性坐标系下的位置，然后得到在惯性坐标系下的位移；步骤3：利用步骤1的相对位移和步骤2的在惯性坐标系下的位移值，采用变种NDT算法进行延时补偿，并对步骤2得到的位移进行补偿；步骤4：对补偿后的位移与相对位移进行融合，得出水下机器人抓取过程中的位置。本发明解决通过双目测距延时和频率低问题以及由于光照条件的变化无法连续测距的问题，满足水下机器人在抓取状态时导航定位要求。

技术领域

本发明涉及一种数据融合导航定位方法，尤其涉及一种水下机器人抓取运动过程的数据融合导航定位方法，属于水下导航定位技术领域。

背景技术

由于水下作业技术的困难，特别是对海参，海胆等水下目标的抓取，而且人类的潜水深度有限，水下机器人的水下作业技术的发展受到越来越广泛的关注。水下机器人抓取过程中的导航定位的精度与频率直接决定其能否抓取成功。不同的传感器在水下抓取过程中满足不同的定位要求，因此非常有必要进行数据融合，使其为水下机器人控制提供准确而且频率高的相对定位数据，使得水下机器人能够成功的对水下目标的进行抓取。

虽然国内将视觉信息与传统意义上的数据融合导航定位的方法已经进行了大量的研究，但原理和方法各有不同：本发明的实质是提出了一种水下机器人抓取运动过程的数据融合导航定位方法。与以往的融合导航定位方法有着明显的区别。一种基于多传感器融合的精确定位系统(CN201711242930.8)主要讨论了激光雷达、单天线GPS模块与九轴IMU进行联合定位，该研究方法并不适用于海洋环境。一种多传感器融合的视觉定位方法(CN202110797407.1)，该发明使用的算法对初始化有较高的要求，在低纹理等比较恶劣的环境进行初始化时会导致后面定位漂移变大，使得导航定位数据定位失真。一种视觉与惯性联合定位方法(CN201711122305.X)直接将视觉定位结果与惯性导航结果进行融合，该融合方法缺乏对视觉定位结果的可靠性分析，一旦视觉定位结果误差过大，将导致此后若干时间内融合定位精度下降。一种基于激光及视觉融合的机器人定位方法(CN202011593913.0)，该发明研究对象为激光和视觉，其方法是利用卡尔曼滤波进行数据融合，该方法中激光容易受到光源，高反射材质，低反光率物体，水下颗粒的影响，但发明中并未考虑到这些情况。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种水下机器人抓取运动过程的数据融合导航定位方法，解决水下机器人在目标抓取过程中，通过双目测距的延时和频率低问题，传统的船位推算的累计误差，以及在抓取过程中由于光照条件的变化无法连续测距的问题，满足水下机器人在抓取状态时的导航定位要求。

为解决上述技术问题，本发明的一种数据融合导航定位方法，包括：

步骤1：测量惯性坐标系下水下机器人相对于海底的速度，利用多普勒测速仪对速度进行积分测得水下机器人的相对位移；

步骤2：通过双目摄像机获取目标图片，通过双目测距分别获得相邻时刻水下机器人在双目坐标系下位置，然后转换到目标坐标系下，再转换到惯性坐标系下，分别得到相邻时刻水下机器人在惯性坐标系下的位置，然后得到在惯性坐标系下的位移；

步骤3：利用步骤1得到的水下机器人相对位移和步骤2得到的水下机器人在惯性坐标系下的位移值，采用变种NDT算法进行延时补偿，并利用延时补偿对步骤2得到的位移进行补偿；

步骤4：对步骤3补偿后的位移与步骤1得到的相对位移进行融合，得出水下机器人抓取过程中的位置。

进一步的，步骤1具体为：

步骤1.1：利用磁罗经测得水下机器人的姿态，利用多普勒测速仪测得在载体坐标系下水下机器人相对与海底的速度

步骤1.2：使用扩展卡尔曼滤波对进行滤波；