CN109459046B - 悬浮式水下自主航行器的定位和导航方法 - Google Patents
悬浮式水下自主航行器的定位和导航方法 Download PDFInfo
- Publication number
- CN109459046B CN109459046B CN201811407006.5A CN201811407006A CN109459046B CN 109459046 B CN109459046 B CN 109459046B CN 201811407006 A CN201811407006 A CN 201811407006A CN 109459046 B CN109459046 B CN 109459046B
- Authority
- CN
- China
- Prior art keywords
- image
- positioning
- pose
- matching
- suspended
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种悬浮式水下自主航行器的定位和导航方法,包括以下步骤,S10,采集水下图像;S20,对采集到的图像进行显著性分析;S30,对采集到的图像进行预处理和特征提取输入特征词袋;S40,进行特征匹配、多传感器实时定位及推进器控制;S50,将S20‑S40的输出构建图像数据库;S60,对图像根据图像数据库进行闭环检测;S70,对图像数据库进行离线优化。本发明是一种针对海底基站的悬浮式水下自主航行器应用环境,融合多传感器信息的定位和导航方法,能够有效提高悬浮式水下自主航行器在水下的定位精度。
Description
技术领域
本发明属于水下航行器领域,具体涉及一种悬浮式水下自主航行器的定位和导航方法。
背景技术
悬浮式水下航行器(Hover Autonomous Underwater Vehicle,简称HAUV)是一种具备强大机动能力的自主水下航行器。HAUV具备多个推进器,以及具备能够在水层中悬浮、具备强大的垂直机动能力、能够贴近海底航行等多项优点。
在HAUV中采用声学定位、航位推算定位等传统水下定位方法的使用成本高,且存在声学定位的有效距离有限,航位推算具有误差累积问题等缺陷。现阶段,基于海底地形的机器视觉方法是解决HAUV水下高精度定位和导航问题的有效手段。
发明内容
本发明要解决的技术问题是提供一种针对海底基站的HAUV应用环境,融合多传感器信息的定位和导航方法,能够有效提高HAUV在水下的定位精度。
为解决上述技术问题,本发明采用如下的技术方案:
一种悬浮式水下自主航行器的定位和导航方法,包括以下步骤,
S10,采集水下图像;
S20,对采集到的图像进行显著性分析;
S30,对采集到的图像进行预处理和特征提取输入特征词袋;
S40,进行特征匹配、多传感器实时定位及推进器控制;
S50,将S20-S40的输出构建图像数据库;
S60,对图像根据图像数据库进行闭环检测;
S70,对图像数据库进行离线优化。
优选地,所述多传感器实时定位,包括以下步骤:
S44,进行视觉图像匹配;
优选地,所述显著性分析通过信息熵来衡量。
优选地,所述特征匹配采用ORB特征算子检测特征点,采用RANSAC算法排除无效特征。
优选地,所述闭环检测包括以下步骤:
S61,从图像数据库中找出跟当前位置的距离在设定值以内的图像;
S62,逐一比较当前图像与其余弦相似度;
S63,如果余弦相似度高于设定值,则进行特征匹配。
优选地,所述离线优化采用BA非线性法。
采用本发明具有如下的有益效果:
1、本发明采用闭环检测的方式,以余弦相似度高于设定值为标准,来判断是否进行图像间的特征匹配。根据闭环图像间的单应矩阵计算当前图像所对应的相机位置,从而消除闭环点之前那段航程的累积误差。通过闭环图像间的相对位置计算,能够对后序图像的位置进行校正,从而提高图像序列的位置精度。
2、本发明用于定位的传感器包括惯性导航单元、电子罗盘、高度计以及摄像头,悬浮式水下自主航行器航行过程中的定位和导航采用在线算法,通过机器视觉配合高度、惯性导航单元信息、方向信息实时推算悬浮式水下自主航行器的当前位姿,并通过位姿反馈控制推进器,使得机器人以预设路径航行。
3、本发明离线算法对数据库中的图像和位置信息进行重新推算,获得更为准确的地图。
附图说明
图1为本发明实施例的步骤流程图;
图2为本发明实施例中多传感器融合定位具体步骤流程图;
图3为本发明实施例中闭环检测具体步骤流程图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明公开了一种悬浮式水下自主航行器的定位和导航方法,其具体实施例参见图1-3和如下所述。
S10,采集水下图像;
S20,对采集到的图像进行显著性分析;
S30,对采集到的图像进行预处理和特征提取输入特征词袋;
S40,进行特征匹配、多传感器实时定位及推进器控制;
S50,将S20-S40的输出构建图像数据库;
S60,对图像根据图像数据库进行闭环检测;
S70,对图像数据库进行离线优化。
S40中多传感器实时定位,包括以下步骤:
S44,进行视觉图像匹配;
具体来讲,上述步骤中,先比较惯性导航单元和电子罗盘的数据,确定航向角变化量筛选依据是电子罗盘对于周围电磁环境比较敏感,而惯性导航单元精度较低,且存在累积误差,但运行稳定,很少被周围环境干扰,故通过阈值来判定电子罗盘数据的有效性,如其有效,则采用电子罗盘所测航向角,如其无效,则采用惯性导航单元所测航向角。
由于水底环境复杂多变,可能出现图像匹配失败的情况,此时采用惯性导航单元所测坐标数据和已定航向角数据更新当前位姿信息。如果图像匹配成功,则利用惯性导航单元数据和航向角信息对视觉定位结果进行检验,如果两者相差不大,可以判定为图像匹配正确,取视觉定位所测坐标和已定航向角数据更新当前位姿信息。反之,则判定图像匹配错误,以惯性导航单元数据为准。
上述步骤中的机器视觉定位采用图像匹配实现,具体公式如下:
上式中,(Δx,Δy)是相机中心在世界坐标系中的坐标变化,T是航行器体坐标系到世界坐标系的转换矩阵,这里假定航行器体坐标系跟图像中心坐标系相重合,是当前时刻航行器的航向,K是简化的相机参数矩阵,以相机高度h(由高度计测得)跟相机x,y方向的像素焦距fx和fy的比值来反映成像平面到海底平面的尺寸转换,矩阵中忽略了相机成像的畸变,H是前一刻图像到当前图像的单应矩阵,由于相机的朝向基本垂直海底平面,且相机的高度近似不变,该变换可以近似为等距变换。H矩阵由下文所述的特征点匹配求解得到。θ表示图像的相对旋转角,tx、ty表示图像的相对位移,其单位是像素。P矩阵的作用是将图像平面的坐标原点移动到图像中心,W是图像宽度,H是图像高度,其单位均是像素。
由于上述步骤中的传感器均存在测量误差,采用EKF算法对所测数据进行滤波。为了提高运算效率、简化状态转换,EKF以由融合信息定位算法得到的两幅图像间相机的二维坐标变化(Δx,Δy)和HAUV的当前航向角变化组成状态向量,即
其状态方程为,xk=f(xk-1,υk-1)+wk-1
式中,f(.)表示***的动力学模型,反应了***输入υk-1对***状态变化的影响,wk-1是服从正态分布的过程噪声。本发明的输入参数为两个推进器的转速,通过实验测量,能够粗略得到螺旋桨转速对于悬浮式水下自主航行器前进速度和转向速度的影响函数,从而完成状态方程的建立。
测量方程为,zk=h(xk)+vk
式中,vk是服从正态分布的观测噪声;h(.)表示状态向量到观测向量的转换函数,实际上就是相机位置的变化在传感器上的反应。
S20中显著性分析通过信息熵来衡量,S40中特征匹配采用ORB特征算子检测特征点,采用RANSAC算法排除无效特征,具体来讲,
采用ORB特征算子检测特征点,用RANSAC算法排除无效特征,通过两幅图像间特征点的映射求解单应矩阵。
用显著性衡量图像的质量。首先对图片进行模糊化处理。这是为了排除细小而繁多的特征点,只保留较大的特征。图像显著性通过信息熵E来衡量,其计算公式如下:
其中,wk表示图像中的一个特征词汇,假设其总数为n,p(wk)是wk词汇在图像中出现的频率。从该公式可以看出:特征词汇种类越多,分布越均匀,信息熵就越大,越能体现出图像的显著性。。
S60中闭环检测包括以下步骤:
S61,从图像数据库中找出跟当前位置的距离在设定值以内的图像;
S62,逐一比较当前图像与其余弦相似度;
S63,如果余弦相似度高于设定值,则进行特征匹配。
具体来讲,在地图探索的过程中,特征词典中词汇的数量是不断增加的。用特征词汇直方图向量(nw1,nw2...nwk)来表征图像(nwk表示特征词汇wk在图像中出现的次数),假设在某一时刻,第i幅图像的特征直方图向量为xi,第j幅图像的特征词汇直方图向量为xj,则两幅图像的相似程度可以用余弦相似度来表示:
闭环检测过程中,先从数据库中找出跟当前位置的距离在设定值以内的图像,逐一比较当前图像跟它们的余弦相似度,如果余弦相似度高于设定值,则进行特征匹配。如果特征匹配成功,则根据闭环图像间的单应矩阵计算当前图像所对应的相机位置,从而消除闭环点之间前那段航程的累积误差。如果多于一幅图像匹配成功,则选取显著性较高的图像进行位置推算。
S50中,为了有效的管理图像,储存图像的分析结果,并便于图像的再次利用,建立图像数据库来管理图像。数据库在HAUV航行过程中同步构建,其中拟收录的数据项如下,
S70中离线优化,由于机器人内核的运算能力有限,在线算法通过牺牲一定的效率提高执行效率。而在HAUV完成航行任务后,有足够的时间离线进行全局优化,获取更高精度的地图,以便在下一次航行中使用,故离线优化的目标是不计计算代价获取高精度地图(当然,在同等精度下也要做到计算量尽可能小)。本发明采用已被广泛验证的BA非线性优化方法。由于海底地形近似为平面,且相机在平面中运动,运用这两个约束可简化传统BA问题,构造以下代价函数:
式中,和分别表示第i幅图像和第j幅图像到公共地图平面(用于拼接所有图像的假想平面)的单应矩阵,和分别是第i幅图像和第j幅图像上的匹配点。优化目标是使得所有匹配点在地图平面上的投影间距离最小。运用列文伯格-马夸尔特方法求解上述最小二乘问题,可以得到最优的地形拼接图和图像-地图间单应矩阵,进而得到优化后的图像-图像间单应矩阵。将单应矩阵代入公式:
可求得图像所对应的相机位置,从而完成图像数据的全局非线性优化。
应当理解,本文所述的示例性实施例是说明性的而非限制性的。尽管结合附图描述了本发明的一个或多个实施例,本领域普通技术人员应当理解,在不脱离通过所附权利要求所限定的本发明的精神和范围的情况下,可以做出各种形式和细节的改变。
Claims (5)
1.一种悬浮式水下自主航行器的定位和导航方法,其特征在于,包括以下步骤:
S10,采集水下图像;
S20,对采集到的图像进行显著性分析;
S30,对采集到的图像进行预处理和特征提取输入特征词袋;
S40,进行特征匹配、多传感器实时定位及推进器控制;
S50,将S20-S40的输出构建图像数据库;
S60,对图像根据图像数据库进行闭环检测;
S70,对图像数据库进行离线优化;
其中,所述多传感器实时定位,包括以下步骤:
S44,进行视觉图像匹配;
2.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法,其特征在于,所述显著性分析通过信息熵来衡量。
3.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法,其特征在于,所述特征匹配采用ORB特征算子检测特征点,采用RANSAC算法排除无效特征。
4.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法,其特征在于,所述闭环检测包括以下步骤:
S61,从图像数据库中找出跟当前位置的距离在设定值以内的图像;
S62,逐一比较当前图像与其余弦相似度;
S63,如果余弦相似度高于设定值,则进行特征匹配。
5.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法,其特征在于,所述离线优化采用BA非线性法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811407006.5A CN109459046B (zh) | 2018-11-23 | 2018-11-23 | 悬浮式水下自主航行器的定位和导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811407006.5A CN109459046B (zh) | 2018-11-23 | 2018-11-23 | 悬浮式水下自主航行器的定位和导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109459046A CN109459046A (zh) | 2019-03-12 |
CN109459046B true CN109459046B (zh) | 2020-05-26 |
Family
ID=65611503
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811407006.5A Active CN109459046B (zh) | 2018-11-23 | 2018-11-23 | 悬浮式水下自主航行器的定位和导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109459046B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111951198B (zh) * | 2019-05-16 | 2024-02-02 | 杭州海康威视数字技术股份有限公司 | 一种无人机航拍图像拼接优化的方法、装置和存储介质 |
CN111968035B (zh) * | 2020-08-05 | 2023-06-20 | 成都圭目机器人有限公司 | 基于损失函数的图像相对旋转角计算方法 |
CN118226884A (zh) * | 2024-05-22 | 2024-06-21 | 江苏园上园智能科技有限公司 | 一种空中飞行机器人精确巡航控制的方法 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107690840B (zh) * | 2009-06-24 | 2013-07-31 | 中国科学院自动化研究所 | 无人机视觉辅助导航方法及*** |
DE102015013104A1 (de) * | 2015-08-22 | 2017-02-23 | Dania Lieselotte Reuter | Verfahren zur Zielanflungsteuerung von unbemannten Flugobjekten, insbesondere Lieferdrohen |
CN105208347B (zh) * | 2015-10-08 | 2018-08-14 | 成都时代星光科技有限公司 | 铁路线空中无人机自动巡查与实时图像采集传输监控装置 |
CN108594851A (zh) * | 2015-10-22 | 2018-09-28 | 飞智控(天津)科技有限公司 | 一种基于双目视觉的无人机自主障碍物检测***、方法及无人机 |
CN105825517B (zh) * | 2016-03-31 | 2018-09-07 | 湖北航天技术研究院总体设计所 | 一种导航高度误差的图像修正方法及*** |
CN108592916B (zh) * | 2018-04-20 | 2020-08-07 | 杭州电子科技大学 | 悬浮式水下自主航行器的多航次地图定位和导航方法 |
-
2018
- 2018-11-23 CN CN201811407006.5A patent/CN109459046B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109459046A (zh) | 2019-03-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110116407B (zh) | 柔性机器人位姿测量方法及装置 | |
CN103149939B (zh) | 一种基于视觉的无人机动态目标跟踪与定位方法 | |
CN111813113B (zh) | 仿生视觉自运动感知地图绘制方法、存储介质及设备 | |
CN109239709B (zh) | 一种无人船的局部环境地图自主构建方法 | |
CN109459046B (zh) | 悬浮式水下自主航行器的定位和导航方法 | |
CN113706626B (zh) | 一种基于多传感器融合及二维码校正的定位与建图方法 | |
CN112747750B (zh) | 一种基于单目视觉里程计和imu融合的定位方法 | |
CN113748357A (zh) | 激光雷达的姿态校正方法、装置和*** | |
Michot et al. | Bi-objective bundle adjustment with application to multi-sensor slam | |
Palomeras et al. | Vision-based localization and mapping system for AUV intervention | |
CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及*** | |
JP2920352B2 (ja) | 移動体の走行制御方法および装置 | |
CN110598370A (zh) | 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计 | |
CN112304322B (zh) | 一种视觉定位失效后的重启方法及车载终端 | |
CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及*** | |
JP2778430B2 (ja) | 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置 | |
CN114485613B (zh) | 一种多信息融合水下机器人定位方法 | |
CN114519671B (zh) | 无人机遥感影像动态快速拼接方法 | |
CN115373383A (zh) | 一种垃圾回收无人艇的自主避障方法、装置及相关设备 | |
CN114842224A (zh) | 一种基于地理底图的单目无人机绝对视觉匹配定位方案 | |
CN110749327B (zh) | 一种合作环境下的车辆导航方法 | |
CN112214028A (zh) | 一种基于OpenMV的水下机器人位姿控制方法 | |
Li et al. | Indoor Localization for an Autonomous Model Car: A Marker-Based Multi-Sensor Fusion Framework | |
Bustos et al. | Matching radar and satellite images for ship trajectory estimation using the Hausdorff distance | |
CN116543057B (zh) | 一种水下多相机与imu一体化标定方法 |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20221020 Address after: Room c-1108, No. 198, Qidi Road, economic and Technological Development Zone, Xiaoshan District, Hangzhou City, Zhejiang Province Patentee after: Hangzhou Hanlu Marine Technology Co.,Ltd. Address before: 310018 No.1, No.2 street, Xiasha Higher Education Park, Hangzhou City, Zhejiang Province Patentee before: HANGZHOU DIANZI University |