基于VIO和动力学模型的水下机器人定位系统及方法与流程

本发明涉及水下机器人动力定位技术领域，更具体的说是涉及一种基于vio和动力学模型的水下机器人定位系统及方法。

背景技术：

水下机器人获取定位信息是水下机器人能够进行自主作业的根本，在auv(autonomousunderwatervehicle，全自主水下机器人)导航过程中最常用的就是dvl(多普勒测速仪)与ins的推位导航的形式，推位导航的误差一般低于航行距离的1％。但是，dvl价格昂贵，且在近海底、池壁条件下，靠近作业目标的情况下，传统的传感器如多普勒测速仪数据误差大，经常出现数据无效，数据出现野值的情况。

水下机器人导航中，视觉方法是一种常用的低成本高精度的导航方法，能够克服dvl的死区问题，而且价格便宜。现有的采用视觉方案主要有两种，一种是采用外部的摄像头获得机器人的全局位置，一种是采用vo(视觉里程计)或者vio(视觉惯性里程计)获得载体的线速度和姿态，但前者几乎只能在实验室中使用，在外场实际环境下很难实现，很难在开阔水域架设摄像头，很难有实用价值。后种方案仅仅使用水下图像获得速度信息，容易受到水下图像模糊，水下光斑、载体晃动等因素的影响，从而导致测量的速度信息噪声很大。

因此，如何在dvl失效时实现高精度的定位是本领域技术人员亟需解决的问题。

技术实现要素：

有鉴于此，本发明提供了一种基于vio和动力学模型的水下机器人定位系统及方法，能够在dvl失效时实现高精度的定位。

为了实现上述目的，本发明采用如下技术方案：

一种基于vio和动力学模型的水下机器人定位系统，包括：水下机器人主体、防水仓、多台防水电机、深度传感器和视觉惯性里程计模块；其中，视觉惯性里程计模块包括：ahrs航姿传感器、双目防水摄像头和嵌入式视觉处理平台；

所述水下机器人主体和嵌入式视觉处理平台均安装在所述防水仓内；

所述防水电机、所述深度传感器、所述ahrs航姿传感器和所述双目防水摄像头均与所述嵌入式视觉处理平台相连。

优选的，所述防水电机包括8台，所述防水电机刚性连接在所述水下机器人主体上，且每台电机分别通过防水插头插入所述防水仓，并通过can总线发送自身的转速信息。

优选的，所述深度传感器通过防水插头连接所述防水仓，并通过can总线发送深度信息。

优选的，所述ahrs航姿传感器和所述双目防水摄像头均通过防水电缆和防水插头连接所述防水仓；其中，所述ahrs航姿传感器通过串口线与所述嵌入式视觉处理平台连接，所述双目防水摄像头通过usb线与所述嵌入式视觉处理平台相连。

一种基于vio和动力学模型的水下机器人定位方法，包括：

步骤1：根据水下机器人多台防水电机的转速向量m以及预先获取的关系式uv＝km，得到推力向量uv；根据预先获取的电机的推力分配矩阵bv，得到水下机器人所受到的6个自由度的力bvuv；其中，uv表示推力向量，k为系数，m表示防水电机的转速向量；

步骤2：读取双目防水摄像头采集的图像数据，对所述图像数据进行增强和矫正处理，得到增强矫正后的双目摄像头数据；

步骤3：读取ahrs航姿传感器采集的数据，并结合所述增强矫正后的双目摄像头数据，基于松耦合的vio算法得到水下机器人的状态测量值νm，其中，状态测量值νm为水下机器人的线速度测量值；

步骤4：将6个自由度的力bvuv作为预测方程的输入值，将状态测量值νm作为观测方程的输入值，采用扩展卡尔曼滤波算法获得机器人载体线速度最优估计值

步骤5：根据ahrs航姿传感器获取姿态角，得到从载体坐标系到地理坐标系的旋转矩阵基于旋转矩阵将机器人载体线速度最优估计值转换到地理坐标系下设解算时间间隔为dt，则第i个时间间隔时位置向量

优选的，预先获取关系式uv＝km的方法包括：通过实验测定不同电机转速下对应的推力曲线，得到关系式uv＝km。

优选的，预先获取电机的推力分配矩阵bv的方法包括：

基于水下动力学方程预先得到的水下机器人的和mv参数以及电机的排布获得推力分配矩阵bv；

其中，mv为机器人质量和机器人围绕机器人质心的转动惯量的矩阵，为恢复力矩矩阵，bv为推力分配矩阵，uv为推力向量，v代表线速度，代表线速度的微分，为浮心与重心的位置差向量；通过试验法中的观测器法获得机器人流体阻尼矩阵drb；通过solidworks软件获得水下机器人的和mv参数。

优选的，在步骤1之前还包括：在水下对双目防水摄像头进行标定，以及对ahrs航姿传感器进行校准。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种基于vio和动力学模型的水下机器人定位系统及方法，结合水下机器人的动力学模型，在视觉惯性里程计精度低和计算错误时进行修正，获得高精度的位置估计结果。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明提供的水下机器人的结构示意图；

图2为本发明提供的水下机器人导航定位方法的示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见附图1，本发明实施例公开了一种基于vio和动力学模型的水下机器人定位系统，包括：水下机器人主体、防水仓、多台防水电机、深度传感器和视觉惯性里程计模块；其中，视觉惯性里程计模块包括：ahrs航姿传感器、双目防水摄像头和嵌入式视觉处理平台；

水下机器人主体和嵌入式视觉处理平台均安装在防水仓内；

防水电机、深度传感器、ahrs航姿传感器和双目防水摄像头均与嵌入式视觉处理平台相连。

为了进一步优化上述技术方案，防水电机包括8台，防水电机刚性连接在水下机器人主体上，且每台电机分别通过防水插头插入防水仓，并通过can总线发送自身的转速信息。

为了进一步优化上述技术方案，深度传感器通过防水插头连接防水仓，并通过can总线发送深度信息。

为了进一步优化上述技术方案，ahrs航姿传感器和双目防水摄像头均通过防水电缆和防水插头连接防水仓；其中，ahrs航姿传感器通过串口线与嵌入式视觉处理平台连接，双目防水摄像头通过usb线与嵌入式视觉处理平台相连。

此外，参见附图2，本发明实施例还公开了一种基于vio和动力学模型的水下机器人定位方法，包括：

步骤1：根据水下机器人多台防水电机的转速向量m以及预先获取的关系式uv＝km，得到推力向量uv；根据预先获取的电机的推力分配矩阵bv，得到水下机器人所受到的6个自由度的力bvuv；其中，uv表示推力向量，k为系数，m表示防水电机的转速向量；

步骤2：读取双目防水摄像头采集的图像数据，对图像数据进行增强和矫正处理，得到增强矫正后的双目摄像头数据；

步骤3：读取ahrs航姿传感器采集的数据，并结合增强矫正后的双目摄像头数据，基于松耦合的vio算法得到水下机器人的状态测量值νm，其中，状态测量值νm为水下机器人的线速度测量值；包括：三轴线速度。

步骤4：将6个自由度的力bvuv作为预测方程的输入值，将状态测量值νm作为观测方程的输入值，采用扩展卡尔曼滤波算法获得机器人载体线速度最优估计值

步骤5：根据ahrs航姿传感器获取姿态角，得到从载体坐标系到地理坐标系的旋转矩阵基于旋转矩阵将机器人载体线速度最优估计值转换到地理坐标系下设解算时间间隔为dt，则第i个时间间隔时位置向量

本发明将ahrs航姿传感器采集的数据结合增强矫正后的双目摄像头数据，基于松耦合的vio算法得到水下机器人的状态测量值uv，得到高精度的位置估计结果，能够在水下机器人靠近池底和池壁目标物，即dvl失效时获得机器人高精度的定位信息，且该系统能够克服纯视觉定位系统容易受到干扰的缺点。

为了进一步优化上述技术方案，预先获取关系式uv＝km的方法包括：通过实验测定不同电机转速下对应的推力曲线，得到关系式uv＝km。

为了进一步优化上述技术方案，预先获取电机的推力分配矩阵bv的方法包括：

基于水下动力学方程预先得到的水下机器人的和mv参数以及电机的排布获得推力分配矩阵bv；

其中，mv为机器人质量和机器人围绕机器人质心的转动惯量矩阵，为恢复力矩矩阵，bv为推力分配矩阵，uv为推力向量，v代表线速度，代表线速度的微分，为浮心与重心的位置差向量；通过试验法中的观测器法获得机器人流体阻尼矩阵drb；通过solidworks软件获得水下机器人的和mv参数。

采用solidwors软件和观测器法对水下机器人进行动力学建模，动力学参数包含水动力参数，以及转动惯量，浮心重心等，具体的过程为，通过soildworks软件获得机器人的重心，绕各轴的转动惯量等参数，将水动力参数作为系统状态量，采用ekf对系统状态量进行预测，多次迭代后获得水动力参数的估计值。该方法为现有方法，即观测器法。

为了进一步优化上述技术方案，在步骤1之前还包括：在水下对双目防水摄像头进行标定，以及对ahrs航姿传感器进行校准。

其中，ahrs航姿传感器校准，具体包括加速计刻度因数校准，零偏校准，对三轴磁罗经采用椭球校准。

对双目摄像机进行畸变校准，极线约束校准，采用cuda版本的暗通道去雾方法对水下图像进行增强。

此外，还需要说明的是，本发明未尽之处为本领域常规技术手段或方法，这里不做详细说明。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。