CN109459046B - 悬浮式水下自主航行器的定位和导航方法 - Google Patents

悬浮式水下自主航行器的定位和导航方法 Download PDF

Info

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
Application number
CN201811407006.5A
Other languages
English (en)
Other versions
CN109459046A (zh
Inventor
史剑光
李芙蓉
于海滨
彭时林
刘敬彪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Hanlu Marine Technology Co ltd
Original Assignee
Hangzhou Dianzi University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hangzhou Dianzi University filed Critical Hangzhou Dianzi University
Priority to CN201811407006.5A priority Critical patent/CN109459046B/zh
Publication of CN109459046A publication Critical patent/CN109459046A/zh
Application granted granted Critical
Publication of CN109459046B publication Critical patent/CN109459046B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

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，对图像数据库进行离线优化。
优选地，所述多传感器实时定位，包括以下步骤：
S41，设k时刻位姿
Figure BDA0001877602750000021
通过惯性导航单元推算位姿变化
Figure BDA0001877602750000022
通过电子罗盘获取方位角变化
Figure BDA0001877602750000023
S42，如
Figure BDA0001877602750000024
为阈值，则确定航向角变化量
Figure BDA0001877602750000025
S43，如
Figure BDA0001877602750000026
则判定电子罗盘受到外部磁场干扰
Figure BDA0001877602750000027
S44，进行视觉图像匹配；
S45，如匹配，则通过单应矩阵推算位姿变化
Figure BDA0001877602750000028
S46，如不匹配，则判定为误匹配，k+1时刻位姿为
Figure BDA0001877602750000029
S47，是否位置偏差
Figure BDA00018776027500000212
且
Figure BDA00018776027500000210
S48，是则判定为正确匹配，k+1时刻位姿为
Figure BDA00018776027500000211
否则S46。
优选地，所述显著性分析通过信息熵来衡量。
优选地，所述特征匹配采用ORB特征算子检测特征点，采用RANSAC算法排除无效特征。
优选地，所述闭环检测包括以下步骤：
S61，从图像数据库中找出跟当前位置的距离在设定值以内的图像；
S62，逐一比较当前图像与其余弦相似度；
S63，如果余弦相似度高于设定值，则进行特征匹配。
优选地，所述离线优化采用BA非线性法。
采用本发明具有如下的有益效果：
1、本发明采用闭环检测的方式，以余弦相似度高于设定值为标准，来判断是否进行图像间的特征匹配。根据闭环图像间的单应矩阵计算当前图像所对应的相机位置，从而消除闭环点之前那段航程的累积误差。通过闭环图像间的相对位置计算，能够对后序图像的位置进行校正，从而提高图像序列的位置精度。
2、本发明用于定位的传感器包括惯性导航单元、电子罗盘、高度计以及摄像头，悬浮式水下自主航行器航行过程中的定位和导航采用在线算法，通过机器视觉配合高度、惯性导航单元信息、方向信息实时推算悬浮式水下自主航行器的当前位姿，并通过位姿反馈控制推进器，使得机器人以预设路径航行。
3、本发明离线算法对数据库中的图像和位置信息进行重新推算，获得更为准确的地图。
附图说明
图1为本发明实施例的步骤流程图；
图2为本发明实施例中多传感器融合定位具体步骤流程图；
图3为本发明实施例中闭环检测具体步骤流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明公开了一种悬浮式水下自主航行器的定位和导航方法，其具体实施例参见图1-3和如下所述。
S10，采集水下图像；
S20，对采集到的图像进行显著性分析；
S30，对采集到的图像进行预处理和特征提取输入特征词袋；
S40，进行特征匹配、多传感器实时定位及推进器控制；
S50，将S20-S40的输出构建图像数据库；
S60，对图像根据图像数据库进行闭环检测；
S70，对图像数据库进行离线优化。
S40中多传感器实时定位，包括以下步骤：
S41，设k时刻位姿
Figure BDA0001877602750000041
通过惯性导航单元推算位姿变化
Figure BDA0001877602750000042
通过电子罗盘获取方位角变化
Figure BDA0001877602750000043
S42，如
Figure BDA0001877602750000044
为阈值，则确定航向角变化量
Figure BDA0001877602750000045
S43，如
Figure BDA0001877602750000046
则判定电子罗盘受到外部磁场干扰
Figure BDA0001877602750000047
S44，进行视觉图像匹配；
S45，如匹配，则通过单应矩阵推算位姿变化
Figure BDA0001877602750000048
S46，如不匹配，则判定为误匹配，k+1时刻位姿为
Figure BDA0001877602750000049
S47，是否位置偏差
Figure BDA00018776027500000410
且
Figure BDA00018776027500000411
S48，是则判定为正确匹配，k+1时刻位姿为
Figure BDA00018776027500000412
否则S46。
具体来讲，上述步骤中，先比较惯性导航单元和电子罗盘的数据，确定航向角变化量
Figure BDA00018776027500000413
筛选依据是电子罗盘对于周围电磁环境比较敏感，而惯性导航单元精度较低，且存在累积误差，但运行稳定，很少被周围环境干扰，故通过阈值
Figure BDA00018776027500000414
来判定电子罗盘数据的有效性，如其有效，则采用电子罗盘所测航向角，如其无效，则采用惯性导航单元所测航向角。
由于水底环境复杂多变，可能出现图像匹配失败的情况，此时采用惯性导航单元所测坐标数据和已定航向角数据更新当前位姿信息。如果图像匹配成功，则利用惯性导航单元数据和航向角信息对视觉定位结果进行检验，如果两者相差不大，可以判定为图像匹配正确，取视觉定位所测坐标和已定航向角数据更新当前位姿信息。反之，则判定图像匹配错误，以惯性导航单元数据为准。
上述步骤中的机器视觉定位采用图像匹配实现，具体公式如下：
Figure BDA00018776027500000415
上式中，(Δx,Δy)是相机中心在世界坐标系中的坐标变化，T是航行器体坐标系到世界坐标系的转换矩阵，这里假定航行器体坐标系跟图像中心坐标系相重合，
Figure BDA0001877602750000051
是当前时刻航行器的航向，K是简化的相机参数矩阵，以相机高度h(由高度计测得)跟相机x，y方向的像素焦距fx和fy的比值来反映成像平面到海底平面的尺寸转换，矩阵中忽略了相机成像的畸变，H是前一刻图像到当前图像的单应矩阵，由于相机的朝向基本垂直海底平面，且相机的高度近似不变，该变换可以近似为等距变换。H矩阵由下文所述的特征点匹配求解得到。θ表示图像的相对旋转角，tx、ty表示图像的相对位移，其单位是像素。P矩阵的作用是将图像平面的坐标原点移动到图像中心，W是图像宽度，H是图像高度，其单位均是像素。
由于上述步骤中的传感器均存在测量误差，采用EKF算法对所测数据进行滤波。为了提高运算效率、简化状态转换，EKF以由融合信息定位算法得到的两幅图像间相机的二维坐标变化(Δx,Δy)和HAUV的当前航向角变化
Figure BDA0001877602750000052
组成状态向量，即
Figure BDA0001877602750000053
其状态方程为，xk＝f(xk-1,υk-1)+wk-1
式中，f(.)表示系统的动力学模型，反应了系统输入υk-1对系统状态变化的影响，wk-1是服从正态分布的过程噪声。本发明的输入参数为两个推进器的转速，通过实验测量，能够粗略得到螺旋桨转速对于悬浮式水下自主航行器前进速度和转向速度的影响函数，从而完成状态方程的建立。
测量方程为，zk＝h(xk)+vk
式中，vk是服从正态分布的观测噪声；h(.)表示状态向量到观测向量的转换函数，实际上就是相机位置的变化在传感器上的反应。
通过EKF的处理后，以起点为原点，相机中心的当前位姿可以表示为
Figure BDA0001877602750000054
由于电子罗盘可测得绝对航向角
Figure BDA0001877602750000055
而不存在误差积累，因此，如果电子罗盘数据正常，则取其读数作为当前航向角，当前位姿可表示为
Figure BDA0001877602750000061
S20中显著性分析通过信息熵来衡量，S40中特征匹配采用ORB特征算子检测特征点，采用RANSAC算法排除无效特征，具体来讲，
采用ORB特征算子检测特征点，用RANSAC算法排除无效特征，通过两幅图像间特征点的映射求解单应矩阵。
用显著性衡量图像的质量。首先对图片进行模糊化处理。这是为了排除细小而繁多的特征点，只保留较大的特征。图像显著性通过信息熵E来衡量，其计算公式如下：
Figure BDA0001877602750000062
其中，wk表示图像中的一个特征词汇，假设其总数为n，p(wk)是wk词汇在图像中出现的频率。从该公式可以看出：特征词汇种类越多，分布越均匀，信息熵就越大，越能体现出图像的显著性。。
S60中闭环检测包括以下步骤：
S61，从图像数据库中找出跟当前位置的距离在设定值以内的图像；
S62，逐一比较当前图像与其余弦相似度；
S63，如果余弦相似度高于设定值，则进行特征匹配。
具体来讲，在地图探索的过程中，特征词典中词汇的数量是不断增加的。用特征词汇直方图向量(nw1,nw2...nwk)来表征图像(nwk表示特征词汇wk在图像中出现的次数)，假设在某一时刻，第i幅图像的特征直方图向量为xi，第j幅图像的特征词汇直方图向量为xj，则两幅图像的相似程度可以用余弦相似度来表示：
Figure BDA0001877602750000063
闭环检测过程中，先从数据库中找出跟当前位置的距离在设定值以内的图像，逐一比较当前图像跟它们的余弦相似度，如果余弦相似度高于设定值，则进行特征匹配。如果特征匹配成功，则根据闭环图像间的单应矩阵计算当前图像所对应的相机位置，从而消除闭环点之间前那段航程的累积误差。如果多于一幅图像匹配成功，则选取显著性较高的图像进行位置推算。
S50中，为了有效的管理图像，储存图像的分析结果，并便于图像的再次利用，建立图像数据库来管理图像。数据库在HAUV航行过程中同步构建，其中拟收录的数据项如下，
Figure BDA0001877602750000071
S70中离线优化，由于机器人内核的运算能力有限，在线算法通过牺牲一定的效率提高执行效率。而在HAUV完成航行任务后，有足够的时间离线进行全局优化，获取更高精度的地图，以便在下一次航行中使用，故离线优化的目标是不计计算代价获取高精度地图(当然，在同等精度下也要做到计算量尽可能小)。本发明采用已被广泛验证的BA非线性优化方法。由于海底地形近似为平面，且相机在平面中运动，运用这两个约束可简化传统BA问题，构造以下代价函数：
Figure BDA0001877602750000072
式中，
Figure BDA0001877602750000073
和
Figure BDA0001877602750000074
分别表示第i幅图像和第j幅图像到公共地图平面(用于拼接所有图像的假想平面)的单应矩阵，
Figure BDA0001877602750000075
和
Figure BDA0001877602750000076
分别是第i幅图像和第j幅图像上的匹配点。优化目标是使得所有匹配点在地图平面上的投影间距离最小。运用列文伯格-马夸尔特方法求解上述最小二乘问题，可以得到最优的地形拼接图和图像-地图间单应矩阵，进而得到优化后的图像-图像间单应矩阵。将单应矩阵代入公式：
Figure BDA0001877602750000077
可求得图像所对应的相机位置，从而完成图像数据的全局非线性优化。
应当理解，本文所述的示例性实施例是说明性的而非限制性的。尽管结合附图描述了本发明的一个或多个实施例，本领域普通技术人员应当理解，在不脱离通过所附权利要求所限定的本发明的精神和范围的情况下，可以做出各种形式和细节的改变。

Claims (5)

1.一种悬浮式水下自主航行器的定位和导航方法，其特征在于，包括以下步骤：
S10，采集水下图像；
S20，对采集到的图像进行显著性分析；
S30，对采集到的图像进行预处理和特征提取输入特征词袋；
S40，进行特征匹配、多传感器实时定位及推进器控制；
S50，将S20-S40的输出构建图像数据库；
S60，对图像根据图像数据库进行闭环检测；
S70，对图像数据库进行离线优化；
其中，所述多传感器实时定位，包括以下步骤：
S41，设k时刻位姿x,y,
Figure FDA0002433437260000011
通过惯性导航单元推算位姿变化Δxi,Δyi,
Figure FDA0002433437260000012
通过电子罗盘获取方位角变化
Figure FDA0002433437260000013
S42，如
Figure FDA0002433437260000014
Figure FDA0002433437260000015
为阈值，则确定航向角变化量
Figure FDA0002433437260000016
S43，如
Figure FDA0002433437260000017
则判定电子罗盘受到外部磁场干扰
Figure FDA0002433437260000018
S44，进行视觉图像匹配；
S45，如匹配，则通过单应矩阵推算位姿变化Δxv,Δyv,
Figure FDA0002433437260000019
S46，如不匹配，则判定为误匹配，k+1时刻位姿为x+Δxi,y+Δyi,
Figure FDA00024334372600000110
S47，是否位置偏差
Figure FDA00024334372600000111
且
Figure FDA00024334372600000112
S48，是则判定为正确匹配，k+1时刻位姿为x+Δxv,y+Δyv,
Figure FDA00024334372600000113
否则S46。
2.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述显著性分析通过信息熵来衡量。
3.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述特征匹配采用ORB特征算子检测特征点，采用RANSAC算法排除无效特征。
4.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述闭环检测包括以下步骤：
S61，从图像数据库中找出跟当前位置的距离在设定值以内的图像；
S62，逐一比较当前图像与其余弦相似度；
S63，如果余弦相似度高于设定值，则进行特征匹配。
5.如权利要求1所述的悬浮式水下自主航行器的定位和导航方法，其特征在于，所述离线优化采用BA非线性法。
CN201811407006.5A 2018-11-23 2018-11-23 悬浮式水下自主航行器的定位和导航方法 Active CN109459046B (zh)

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 (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111951198B (zh) * 2019-05-16 2024-02-02 杭州海康威视数字技术股份有限公司 一种无人机航拍图像拼接优化的方法、装置和存储介质
CN111968035B (zh) * 2020-08-05 2023-06-20 成都圭目机器人有限公司 基于损失函数的图像相对旋转角计算方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
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 成都时代星光科技有限公司 铁路线空中无人机自动巡查与实时图像采集传输监控装置
CN105222760A (zh) * 2015-10-22 2016-01-06 一飞智控(天津)科技有限公司 一种基于双目视觉的无人机自主障碍物检测系统及方法
CN105825517B (zh) * 2016-03-31 2018-09-07 湖北航天技术研究院总体设计所 一种导航高度误差的图像修正方法及系统
CN108592916B (zh) * 2018-04-20 2020-08-07 杭州电子科技大学 悬浮式水下自主航行器的多航次地图定位和导航方法

Also Published As

Publication number Publication date
CN109459046A (zh) 2019-03-12

Similar Documents

Publication Publication Date Title
CN110116407B (zh) 柔性机器人位姿测量方法及装置
CN111813113B (zh) 仿生视觉自运动感知地图绘制方法、存储介质及设备
CN113706626B (zh) 一种基于多传感器融合及二维码校正的定位与建图方法
CN113748357A (zh) 激光雷达的姿态校正方法、装置和系统
Michot et al. Bi-objective bundle adjustment with application to multi-sensor slam
CN112747750B (zh) 一种基于单目视觉里程计和imu融合的定位方法
CN110751123B (zh) 一种单目视觉惯性里程计系统及方法
Palomeras et al. Vision-based localization and mapping system for AUV intervention
CN114136315B (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN109459046B (zh) 悬浮式水下自主航行器的定位和导航方法
JP2920352B2 (ja) 移動体の走行制御方法および装置
CN110598370A (zh) 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计
JPH07103715A (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN112304322B (zh) 一种视觉定位失效后的重启方法及车载终端
JP2778430B2 (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
CN114519671B (zh) 无人机遥感影像动态快速拼接方法
CN115373383A (zh) 一种垃圾回收无人艇的自主避障方法、装置及相关设备
CN114842224A (zh) 一种基于地理底图的单目无人机绝对视觉匹配定位方案
CN114485613A (zh) 一种多信息融合水下机器人定位方法
CN114690226A (zh) 基于载波相位差分技术辅助的单目视觉测距方法及系统
CN110749327B (zh) 一种合作环境下的车辆导航方法
CN112214028A (zh) 一种基于OpenMV的水下机器人位姿控制方法
JPH06262568A (ja) 視覚に基く三次元位置および姿勢の認識方法とその装置
CN112415516A (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
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