CN109737950A - 基于vio和动力学模型的水下机器人定位系统及方法 - Google Patents
基于vio和动力学模型的水下机器人定位系统及方法 Download PDFInfo
- Publication number
- CN109737950A CN109737950A CN201910080899.5A CN201910080899A CN109737950A CN 109737950 A CN109737950 A CN 109737950A CN 201910080899 A CN201910080899 A CN 201910080899A CN 109737950 A CN109737950 A CN 109737950A
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- vio
- underwater
- binocular
- kinetic model
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Navigation (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于VIO和动力学模型的水下机器人定位系统及方法，结合水下机器人的动力学模型，在视觉惯性里程计精度低和计算错误时进行修正，获得高精度的位置估计结果。
Description
技术领域
本发明涉及水下机器人动力定位技术领域，更具体的说是涉及一种基于VIO和动力学模型的水下机器人定位系统及方法。
背景技术
水下机器人获取定位信息是水下机器人能够进行自主作业的根本，在AUV(Autonomous UnderwaterVehicle，全自主水下机器人)导航过程中最常用的就是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版本的暗通道去雾方法对水下图像进行增强。
此外，还需要说明的是，本发明未尽之处为本领域常规技术手段或方法，这里不做详细说明。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
Claims (8)
1.一种基于VIO和动力学模型的水下机器人定位系统，其特征在于，包括：水下机器人主体、防水仓、多台防水电机、深度传感器和视觉惯性里程计模块；其中，视觉惯性里程计模块包括：AHRS航姿传感器、双目防水摄像头和嵌入式视觉处理平台；
所述水下机器人主体和嵌入式视觉处理平台均安装在所述防水仓内；
所述防水电机、所述深度传感器、所述AHRS航姿传感器和所述双目防水摄像头均与所述嵌入式视觉处理平台相连。
2.根据权利要求1所述的一种基于VIO和动力学模型的水下机器人定位系统，其特征在于，所述防水电机包括8台，所述防水电机刚性连接在所述水下机器人主体上，且每台电机分别通过防水插头插入所述防水仓，并通过CAN总线发送自身的转速信息。
3.根据权利要求1所述的一种基于VIO和动力学模型的水下机器人定位系统，其特征在于，所述深度传感器通过防水插头连接所述防水仓，并通过CAN总线发送深度信息。
4.根据权利要求1所述的一种基于VIO和动力学模型的水下机器人定位系统，其特征在于，所述AHRS航姿传感器和所述双目防水摄像头均通过防水电缆和防水插头连接所述防水仓；其中，所述AHRS航姿传感器通过串口线与所述嵌入式视觉处理平台连接，所述双目防水摄像头通过USB线与所述嵌入式视觉处理平台相连。
5.一种基于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个时间间隔时位置向量
6.根据权利要求5所述的基于VIO和动力学模型的水下机器人定位方法，其特征在于，预先获取关系式uv＝kM的方法包括：通过实验测定不同电机转速下对应的推力曲线，得到关系式uv＝kM。
7.根据权利要求5所述的基于VIO和动力学模型的水下机器人定位方法，其特征在于，预先获取电机的推力分配矩阵Bv的方法包括：
基于水下动力学方程预先得到的水下机器人的和Mv参数以及电机的排布获得推力分配矩阵Bv；
其中，Mv为机器人质量和机器人围绕机器人质心的转动惯量的矩阵，为恢复力矩矩阵，Bv为推力分配矩阵，uv为推力向量，v代表线速度，代表线速度的微分，为浮心与重心的位置差向量；通过试验法中的观测器法获得机器人流体阻尼矩阵DRB；通过Solidworks软件获得水下机器人的和Mv参数。
8.根据权利要求5所述的基于VIO和动力学模型的水下机器人定位方法，其特征在于，在步骤1之前还包括：在水下对双目防水摄像头进行标定，以及对AHRS航姿传感器进行校准。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910080899.5A CN109737950B (zh) | 2019-01-28 | 2019-01-28 | 基于vio和动力学模型的水下机器人定位系统及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910080899.5A CN109737950B (zh) | 2019-01-28 | 2019-01-28 | 基于vio和动力学模型的水下机器人定位系统及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109737950A true CN109737950A (zh) | 2019-05-10 |
| CN109737950B CN109737950B (zh) | 2020-04-17 |
Family
ID=66366404
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910080899.5A Active CN109737950B (zh) | 2019-01-28 | 2019-01-28 | 基于vio和动力学模型的水下机器人定位系统及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109737950B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112652001A (zh) * | 2020-11-13 | 2021-04-13 | 山东交通学院 | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170337749A1 (en) * | 2016-05-18 | 2017-11-23 | Google Inc. | System and method for fault detection and recovery for concurrent odometry and mapping |
| CN108151713A (zh) * | 2017-12-13 | 2018-06-12 | 南京航空航天大学 | 一种单目vo快速位姿估计方法 |
| CN108398139A (zh) * | 2018-03-01 | 2018-08-14 | 北京航空航天大学 | 一种融合鱼眼图像与深度图像的动态环境视觉里程计方法 |
-
2019
- 2019-01-28 CN CN201910080899.5A patent/CN109737950B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170337749A1 (en) * | 2016-05-18 | 2017-11-23 | Google Inc. | System and method for fault detection and recovery for concurrent odometry and mapping |
| CN108151713A (zh) * | 2017-12-13 | 2018-06-12 | 南京航空航天大学 | 一种单目vo快速位姿估计方法 |
| CN108398139A (zh) * | 2018-03-01 | 2018-08-14 | 北京航空航天大学 | 一种融合鱼眼图像与深度图像的动态环境视觉里程计方法 |
Non-Patent Citations (2)
| Title |
|---|
| 许宪东: "复杂室内环境下仿人机器人定位与运动规划研究", 《中国博士学位论文全文数据库信息科技辑》 * |
| 赵亮: "水下机器人视觉SLAM方法中的图像特征点提取技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112652001A (zh) * | 2020-11-13 | 2021-04-13 | 山东交通学院 | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109737950B (zh) | 2020-04-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103245360B (zh) | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 | |
| Kinsey et al. | A survey of underwater vehicle navigation: Recent advances and new challenges | |
| Chutia et al. | A review of underwater robotics, navigation, sensing techniques and applications | |
| CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN108955675A (zh) | 一种基于惯性测量的地下管道轨迹检测系统及方法 | |
| CN108375370A (zh) | 一种面向智能巡防无人机的复合导航系统 | |
| CN102042835A (zh) | 自主式水下机器人组合导航系统 | |
| Arnold et al. | Robust model-aided inertial localization for autonomous underwater vehicles | |
| CN111596333B (zh) | 一种水下定位导航方法及系统 | |
| CN210719199U (zh) | 一种水下机器人多设备组合导航系统 | |
| CN106507913B (zh) | 用于管道测绘的组合定位方法 | |
| CN116295511B (zh) | 一种用于管道潜航机器人的鲁棒初始对准方法及系统 | |
| CN110806209A (zh) | 一种水下机器人多设备组合导航系统及方法 | |
| CN107883951A (zh) | 一种水下机器人三维姿态的计算方法及终端 | |
| Bonin-Font et al. | Inertial sensor self-calibration in a visually-aided navigation approach for a micro-AUV | |
| CN109443356A (zh) | 一种含测量噪声的无人船位置与速度估计结构及设计方法 | |
| CN108801250B (zh) | 基于水下机器人的实时姿态获取方法及装置 | |
| CN101556154B (zh) | 定位及路径地图生成系统及其数据采集分析方法 | |
| CN104406592B (zh) | 一种用于水下滑翔器的导航系统及姿态角校正和回溯解耦方法 | |
| CN103616026B (zh) | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 | |
| CN109737950A (zh) | 基于vio和动力学模型的水下机器人定位系统及方法 | |
| CN110441736A (zh) | 多关节水下无人潜行器变基线三维空间定位方法 | |
| CN112197765B (zh) | 一种实现水下机器人精细导航的方法 | |
| Brown et al. | An overview of autonomous underwater vehicle research and testbed at PeRL | |
| CN108759864A (zh) | 基于单应答器的水下捷联惯导系统初始对准方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20200317 Address after: 264200 Huancui District, Weihai City, Shandong Province, 298-1 Sun Jiatuan Huanhai Road Applicant after: Zhizhen Marine Science and Technology (Weihai) Co.,Ltd. Applicant after: HEILONGJIANG MAIPUSI ENTERPRISE MANAGEMENT CENTER (LIMITED PARTNERSHIP) Address before: Room 510-37, Torch E-Commerce Building, Building 14, Innovation Plaza, Science and Technology Innovation City, Harbin Hi-tech Industrial Development Zone, Heilongjiang Province, 150000 (236 Mingyue Street) Applicant before: HEILONGJIANG MAIPUSI ENTERPRISE MANAGEMENT CENTER (LIMITED PARTNERSHIP) |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |