CN115451964A - 基于多模态混合特征的船舶场景同时建图与定位方法 - Google Patents

基于多模态混合特征的船舶场景同时建图与定位方法 Download PDF

Info

Publication number
CN115451964A
CN115451964A CN202210990299.4A CN202210990299A CN115451964A CN 115451964 A CN115451964 A CN 115451964A CN 202210990299 A CN202210990299 A CN 202210990299A CN 115451964 A CN115451964 A CN 115451964A
Authority
CN
China
Prior art keywords
point
laser radar
line
feature
ship
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
Application number
CN202210990299.4A
Other languages
English (en)
Other versions
CN115451964B (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.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
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 Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202210990299.4A priority Critical patent/CN115451964B/zh
Publication of CN115451964A publication Critical patent/CN115451964A/zh
Application granted granted Critical
Publication of CN115451964B publication Critical patent/CN115451964B/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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于多模态混合特征的船舶场景同时建图与定位方法，包括以下步骤，S1、搭建信息采集标定装置，对该装置内各传感器进行内外参标定，通过该装置获得包含激光雷达点云和环视视觉图像的多模态传感器信息；S2、利用多模态传感器信息及标定结果，结合基于混合特征约束的船舶场景点簇、线和面特征选取方法，获得船舶不同功能区域的典型形貌特征；S3、使用融合视觉和点云信息的自适应点‑点簇‑线‑面里程计构建方法获得船舶不同功能区域下的传感器运动估计；S4、采用基于多模态混合特征的多阶段回环检测策略方法确定候选候选回环点；S5、采用基于位姿图优化的方法实现全局优化并获得混合点‑点簇‑线‑面特征的船舶场景地图。

Description

基于多模态混合特征的船舶场景同时建图与定位方法
技术领域
本发明涉及智能船舶领域，具体涉及一种基于多模态混合特征的船舶场景同时建图与定位方法。
背景技术
智能船舶是未来船舶发展的主流方向，其中船舶智能运维、监控是核心环节。具体来看，智能运维中利用移动机器人代替人在船舶上执行巡检、检修等重复、枯燥的运维工作是一个很有前景的研究内容，但是机器人首先要知道船舶的场景地图才能执行移动操作任务。其次，数字孪生是船舶智能监控的核心技术，但数字孪生的前提也是构建一个船舶场景地图。因此，船舶场景地图的构建对于智能船舶直观重要。
目前针对船舶甲板、货舱、舱室机舱等不同功能区域的场景地图构建方法研究几乎为空白，例如专利“基于视频图像与激光雷达点云融合建立船舶模型的方法202111542134.2”，该专利只关注船体轮廓的构建，但是真正船上作业需要的是舱室、甲板、机舱等内部场景地图的构建。专利“一种无人船舶SLAM及其应用方法”偏重于利用船舶构建外部世界，如桥梁、泊位等场景地图。
现在存在一些室内或者室外同时定位与建图方法为船舶场景建图方法提供一定的研究基础。例如，论文“Lego-loam:Lightweight and ground-optimized lidarodometry andmapping on variable terrain”使用激光雷达构建了户外场景三维点云地图。专利“同时定位与建图技术中的特征点提取与匹配方法及系统201811138259.7”提取视觉点特征进行场景建图。论文“RGB-D SLAM在室内高精度三维测图中的应用”使用RGB-D相机实现了室内场景的建图。不难发现，传统方法存在以下缺陷：
针对的场景较为单一且环境特征较为统一，例如室外场景多为开阔场景、室内场景是标准的结构化环境，但是船舶场景具有众多的功能区域例如连续且众多的小舱室(线特征明显)、空旷的甲板区域(点簇杆状特征明显)、大尺度的货舱(面特征明显)、以及复杂多变的机舱(点、点簇和线特征明显)等。因此传统建图方法难以应对如此复杂的船舶场景。
目前建图方法通常依赖于单一感知传感器(相机或激光雷达)以及单一的特征(点或线特征)，这很难满足复杂船舱场景的建图鲁棒性和精度的需求，同时船舶不同功能区域的典型环境特征各异，单一传感器和单一特征是难以满足实用需求的。
总之，船舶场景地图的需求是十分迫切的，但是面向船舶不同功能能区域的建图方法目前研究很少，尤其是能使用统一的建图定位方法来完成船舶场景众多功能区域建图仍是一个挑战。
发明内容
本发明的目的在于提供一种基于多模态混合特征的船舶场景同时建图与定位方法，以完成船舶场景众多功能区域建图。
为解决上述技术问题，本发明提供了一种技术方案：基于多模态混合特征的船舶场景同时建图与定位方法，包括以下步骤，
S1、搭建信息采集标定装置，通过该装置获得包含激光雷达点云和环视视觉图像的多模态传感器信息，并对该装置内各传感器进行内外参标定；
S2、利用多模态传感器信息及标定结果，结合基于混合特征约束的船舶场景点簇、线和面特征选取方法，获得船舶不同功能区域的典型形貌特征；
S3、使用融合视觉和点云信息的自适应点-点簇-线-面里程计构建方法获得船舶不同功能区域下的传感器运动估计；具体地，首先采用图像点特征和线特征构建自适应视觉里程计来估计环视视觉图像两帧间的粗略运动Tck；然后利用外部参数将估计结果转化为激光雷达坐标系，作为激光雷达扫描的初始变换估计，然后使用点簇特征和面特征构建自适应激光雷达里程计确定精准的运动估计Tlk，并获得该时刻“点-点簇-线-面”多模态混合特征信息及信息采集标定装置坐标系在世界坐标系下的坐标Pi,w；最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图；
S4、依据该时刻多模态混合特征信息、Pi,w和未优化的船舶场景地图，采用基于多模态混合特征的多阶段回环检测策略方法确定候选候选回环点，并形成回环约束；
S5、依据所有历史获得的Tlk、Pi,w以及回环约束，采用基于位姿图优化的方法实现全局优化并获得混合点-点簇-线-面特征的船舶场景地图。
按上述方案，所述信息采集标定装置包括环视相机外参标定板，环视相机外参标定板上设置有传感器便携支架，传感器便携支架上连接有传感器安装板，传感器安装板通过激光雷达支架连接有三维激光雷达；传感器安装板周侧设置有环视相机组，环视相机组包括若干鱼眼相机，环视相机组周侧设置有若干与鱼眼相机相对应的圆形镂空标定板；
激光雷达支架的长度尺寸使三维激光雷达产生的向下射线不接触传感器安装板和环视相机组，三维激光雷达及各鱼眼相机与各圆形镂空标定板之间的位置关系使三维激光雷达及各语言相机能够完整观测到所有的圆形镂空标定板；环视相机外参标定板位于传感器安装板的正下方，且具有一定的面积，使得每个鱼眼相机都能够从环视相机外参标定板上获取一定数量的特征点。
按上述方案，传感器安装板为顶面和底面是正方形的长方体形，其每个侧面的中间位置均设置有用于固定鱼眼相机的相机支架；圆形镂空板为正方体板，其上均匀设置有若干圆形镂空孔洞。
按上述方案，各鱼眼相机的内参标定过程具体如下，使用Opencv中鱼眼相机标定模块对各鱼眼相机进行标定，得到各鱼眼相机的畸变系数和内参矩阵K；
各鱼眼相机的外参标定过程具体如下，首先定义环视相机外参标定板置于Z＝0平面，并以传感器安装板中心为原点建立信息采集标定装置坐标系；然后获取环视相机外参标定板中各角点的在信息采集标定装置坐标系下的坐标，以及获取图像中的角点的像素坐标，求取图像平面与环视相机外参标定板之间的单应性矩阵；最后分解单应性矩阵求取所设信息采集标定装置坐标系与每个鱼眼相机坐标系之间的相对位姿以完成四个鱼眼相机外参标定，并获得环视相机组坐标系与信息采集标定装置坐标系的位姿关系；
三维激光雷达与环视相机组标定过程具体如下，采用三维激光雷达与环视相机组探测各圆形镂空标定板，通过对三维激光雷达的三维点云进行边缘检测获得圆形镂空标定板中圆形镂空孔洞的圆心，并利用OpenCV获取图像中的Blob质心；随后使用基于3D-3D的迭代最近点算法估计外参，具体地，将三维激光雷达以及环视相机组获取的所有圆心空间位置作为迭代最近点算法的输入，获得三维激光雷达在在信息采集标定装置坐标系下的坐标下的位姿。
按上述方案，S2中利用三维雷达的雷达点云中提取点簇特征和面特征，利用环视相机组的环视图像中提取点和线特征；其中点簇特征用于描述包括船舶机舱、甲板中的围栏杆、信号杆，面特征用于描述包括船舱、货舱典型中的舱壁、舱底、走廊地面；
具体地，三维雷达的雷达点云中点簇特征和面特征的提取过程如下，将雷达点云数据转换为距离图像，将在t时刻获取的雷达点云数据投影到一个距离图像上，其中三维激光雷达的水平和垂直角度分辨率分别为sl和cl，而投影的距离图像的分辨率为360/sl乘30/cl+1；然后进行点云噪声处理，将连续三帧激光雷达的雷达点云合并为一个点云并转换为距离图像，将距离图像分割为若干个聚类，同一个聚类的点被标记上唯一的标识，其中点数少于一定数目的聚类被作为噪声去除；最后基于rangNet++提取出舱室中的点簇特征{Pli}L和面特征{PLi}L；
环视相机组的环视图像的点和线特征提取过程如下，对于每个输入RGB图像帧，提取图像ORB角点特征获得点特征{Pci}C，同时使用LSD直线检测方法获得图片中的线特征{Li}C；
随后将通过上述方法获取到的点、点簇、线、面特征进行约束条件构建，选取鲁棒混合点、点簇、线和面特征，具体如下，
首先，将三维激光雷达的点云以及面特征向图像平面进行平面投影，获得带有激光雷达深度信息和点簇、面特征的复合环视图像；
检测激光雷达点簇特征附近是否出现连续等距的图像线特征，若存在则保留该点簇特征，否则进行特征删除；
使用k-d树搜索获得图像ORB角特征附近的三个最近的三维激光雷达点，计算三个激光雷达点的距离极差，如果大于距离阈值，则在{Pci}C中删除该点特征，否则，取三点距离平均值赋予该点特征；
若某一线特征Li处于{PLi}L中任意两个舱室或者货舱面特征的边缘，则将其计算权重wLi设置为普通线特征的两倍；
在面特征{PLi}L中删除由海面形成的面特征，并根据面特征的尺寸由小至大依次设置由低到高的权重值。
按上述方案，S3具体如下，
对环视图像的每个输入RGB图像帧，依据S2步骤，提取提取图像中鲁棒的ORB特征和线特征；计算Tck＝{Rck，tck}，其中Rck和tck是Tck的旋转矩阵和平移矩阵，使得第k帧和k-1帧对应的点和线特征集之间的距离最小，获得粗略的视觉里程计结果；
随后，对于每个输入激光雷达点云信息，依据S2步骤，提取点云数据中鲁棒的点簇特征和面特征，计算Tlk＝{Rlk，tlk}，Rlk和tlk是Tlk的旋转矩阵和平移矩阵，使得第k帧和k-1帧对应的点簇和面特征集之间的距离最小，获得精准的激光雷达里程计结果，即标定装置坐标系在世界坐标系下的坐标Pi,w以及该时刻“点-点簇-线-面”多模态混合特征信息；
最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图。
按上述方案，所述视觉里程计结果获取过程具体如下，
对于k帧中的第i个点Pci，其能够被投影到图像平面，于是有，
Figure BDA0003803583810000061
给定一个该点特征的观测数据pci,k，并定义误差为eci,k，并有
Figure BDA0003803583810000062
对于k帧中的第i个边特征Li(Lsi,Lei)，Lsi和Lei是特征起点和终点，Lsi和Lei在同一关键帧上对应的图像投影表示为，
Figure BDA0003803583810000063
Figure BDA0003803583810000064
π(·)是投影变换，K是相机内参矩阵，给定图像第i个线特征的端点观测值
Figure BDA0003803583810000065
和Lei,k，并估计观察线特征系数
Figure BDA0003803583810000066
并定义线特征误差向量如下，
Figure BDA0003803583810000067
Figure BDA0003803583810000068
建立视觉里程计的统一的代价函数，其中每个误差项集成如下，
Figure BDA0003803583810000069
其中，hr(·)为Huber鲁棒代价函数，Ωci,k，Ω′ci,k和Ω′c′i,k为检测到的点特征和线特征端点尺度相关联的协方差矩阵。wLi为自适应调节参数，用于自适应地调整不同线特征对误差项的影响。
按上述方案，所述激光雷达里程计结果获取过程具体如下，
将环视相机组坐标系中的运动估计转化到三维激光雷达坐标系下，并作为激光雷达点云扫描匹配的初始值，于是有，
TckTCL＝TCLTlk
其中，TCL为环视相机组坐标系与三维激光雷达坐标系的外参矩阵，Tck为两帧图像之间的初始变换，Tlk为对应的激光雷达扫描点云数据之间的变换；
激光雷达里程计的初始值表示为，
Figure BDA0003803583810000071
对于k帧三维激光雷达点云中的第i个点簇特征Pli，其经过坐标转换后的预计位姿为，
Figure BDA0003803583810000072
给定一个该点特征的观测数据pli,k，并定义误差为，
Figure BDA0003803583810000073
对于k帧三维激光雷达点云中的第i个面特征PLi，其经过坐标转换后的预计位姿为，
Figure BDA0003803583810000074
其中，Rlk和tlk为Tlk中的旋转矩阵和位移向量。ni为面特征PLi的单位法线，di为原点到平面的垂直距离；
给定一个该面特征的观测数据pli,k，并定义误差为，
Figure BDA0003803583810000075
建立三维激光雷达的统一的代价函数，将每个误差项集成如下，
Figure BDA0003803583810000081
其中，hr(·)为Huber鲁棒代价函数，Ωli,k和Ωpli,k为检测到的点簇特征和面特征端点尺度相关联的协方差矩阵，WPL和wPLi分别是朝向权重和尺寸权重，
用于自适应地调整不同线特征对误差项的影响；
WPL权重大小取决于面特征的法向量一致性，其计算公式如下，
Figure BDA0003803583810000082
其中，zero(·)表示将{PLi}L中所有面特征进行配对并对他们的方向量进行叉乘并获得约等于0的相互平行的平面特征数量，
Figure BDA0003803583810000083
是整体配对次数，N为面特征数量，同时为了避免面特征权重过小，限制最小权重为0.5；
将Tlk转换为前后时刻信息采集标定装置坐标系之间的变换Tsk；以初始位置为世界坐标系原点，将中间所有Tsk连乘获得该时刻信息采集标定装置坐标系在世界坐标系下的坐标Pi,w，并生成该获得该时刻“点-点簇-线-面”多模态混合特征信息；
最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图。
按上述方案，S4所述基于多模态混合特征的多阶段回环检测策略方法包括以下步骤，
首先采用基于点线视觉词袋的多候选回环点选择方法获得初始搜素空间{CC}，然后再利用激光雷达平面数量约束对多个候选点进行进一步筛选获得{CP}，最后利用全部点簇特征进行精准匹配，获得最终的回环检测点CF；
具体地，首先，每隔一定距离长度或一定时间间隔，将通过环视相机和三维激光雷达获取的混合特征信息进行存储，作为关键帧数据用于回环检测；利用通过环视相机获取的数据中的关键点和线条特征建立视觉词汇表和数据库，利用通过三维激光雷达获取的雷达点云中的面特征数量设置特殊环境描述子；在环视相机进行运动采集的过程中，根据其实时获取的环视图像帧中的关键点和线条特征形成视觉描述符，而后利用词袋搜索在上述数据库中寻找与关键帧词汇表视觉匹配程度超过一定阈值的候选区域，以此获得多个候选回环点{CC}；
随后，获得当前激光雷达的面特征数量，并且与{CC}中每个位置的面特征数量进行比对，选取面特征数量相近的区域构成{CP}；
最后，计算{CP}中每个位置出的激光雷达点簇特征与激光雷达关键帧点簇的匹配结果，如果存在匹配结果超过匹配阈值，则选取匹配值最大的CF与当前信息采集标定装置位姿形成回环约束，二者之间的位姿变换表示为Tic；该时刻信息采集标定装置坐标系在世界坐标系下的坐标Pi,w表示为Pi,w＝TicCF。
按上述方案，S5过程具体为，
将信息采集标定装置的全局位姿作为姿态节点插入到图中，每个时刻下之间的变换表示为边，将两个相邻姿态节点之间的转换误差定义如下，
Figure BDA0003803583810000091
随后计算计算全局优化的最小二乘代价，
Figure BDA0003803583810000092
Figure BDA0003803583810000093
其中，Σo是里程计噪声协方差矩阵；
而后，使用ceres优化库得到最佳姿态，将所有位姿节点与其中包含的视觉点特征、线特征以及三维激光雷达点簇特征和面特征一起重新排列，以形成混合点-点簇-线-面特征的全局地图。
本发明的有益效果是：本发明提出了一种用于进行船舶信息采集的信息采集标定装置，实现三维激光雷达点云和环视视觉图像的内外参精准标定以及复杂船舶场景中多传感器数据的便捷采集；并且提出了一种基于混合特征约束的船舶场景点簇、线和面特征选取方法，可以精准提取出船舶不同功能区域的典型环境特点，并增强了特征的鲁棒性；并且提出了一种使用融合环视视觉和激光点云的融合视觉和点云信息的自适应点-点簇-线-面里程计构建方法，使用统一的算法就可以获得船舶不同功能区域下传感器运动的精准鲁棒估计；并且提出了一种基于多模态混合特征的多阶段回环检测策略方法，高效精准确定候选回环点，从而消除复杂船舶场景下的建图累积误差；并且提出了一种基于位姿图优化的方法实现全局优化并获得混合点-点簇-线-面特征的船舶场景地图，该地图可以同时满足不同船舶区域的导航定位需求。
附图说明
图1是本发明一实施例的基于多模态混合特征的船舶场景同时建图与定位方法流程图；
图2是本发明一实施例的信息采集标定装置结构示意图；
图3是本发明一实施例的典型船舶场景中不同功能区域示意图。
图中：1-三维激光雷达，2-激光雷达支架，3-传感器安装板，4-鱼眼相机，5-相机支架，6-传感器便携支架，7-环视相机外参标定板，8-圆形镂空标定板，9-舱室，10-甲板围栏，11-货舱舱壁，12-海面。
具体实施方式
为使本公开实施例的目的、技术方案和优点更加清楚，下面将结合本公开实施例的附图，对本公开实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本公开的一部分实施例，而不是全部的实施例。基于所描述的本公开的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本公开保护的范围。
如图1所示，基于多模态混合特征的船舶场景同时建图与定位方法及系统包括以下步骤：
S1，搭建便捷式船舶环境信息采集及标定硬件系统，获得激光雷达点云和环视视觉图像，并进行多传感器内外参标定。
S2，使用基于混合特征约束的船舶场景点簇、线和面特征选取方法，获得船舶不同功能区域的典型形貌特征。
S3，使用融合视觉和点云信息的自适应点-点簇-线-面里程计构建方法获得船舶不同功能区域下的传感器运动估计。
S4，依据该时刻多模态混合特征信息、Pi,w和未优化的船舶场景地图，采用基于多模态混合特征的多阶段回环检测策略方法高效精准确定候选回环点。
S5，依据所有历史获得的Tlk，Pi,w以及回环约束，采用基于位姿图优化的方法实现全局优化并获得混合点-点簇-线-面特征的船舶场景地图。
S1要搭建能在船舶舱室9、甲板和货仓等不同功能区域进行数据采集的便捷式船舶环境信息采集及标定硬件系统，并对多传感器的内外参进行标定。具体包括如下子步骤：
S1.1，搭建便捷式船舶环境信息采集及标定硬件系统。如采集及标定硬件系统由三维激光雷达1、激光雷达支架2、传感器安装板3、环视相机组(包括四个鱼眼相机4、四个相机支架5)、传感器便携支架6、环视相机外参标定板7和四个圆形镂空标定板8组成。
S1.1.1，激光雷达支架2应保证三维激光雷达1的下方射线不触碰传感器安装板3和环视相机组，同时三维激光雷达1保证可以观测到全部圆形镂空标定板8。
S1.1.2，传感器安装板3为正方形，尺寸为200mm*200mm*5mm的方形板材，正上方安装激光雷达支架2，每个侧边的中心位置环形安装四个相机支架5，每个相机支架5上安装鱼眼相机4。
S1.1.3，传感器便携支架6用于手持移动采集及标定硬件系统，同时可替换为板状支撑架用于背负移动或者安装到其他移动机器人上。
S1.1.4，环视相机外参标定板7尺寸应足够大且放于传感器安装板3正下方，保证每个鱼眼相机4可以从环视相机外参标定板7上获取至少10个特征点。
S1.1.5，圆形镂空标定板8尺寸为500mm*500mm*5mm，包含25个圆形镂空区域，圆形镂空标定板8围绕传感器安装板3环形布置且尽可能正对鱼眼相机4，安装距离保证三维激光雷达1和四个鱼眼相机4能完全观测到全部圆形镂空标定板8。
S1.2，环视相机组内外参标定。获得每个鱼眼相机4畸变系数和内参矩阵，然后利用环视相机外参标定板7对四个鱼眼相机4的外参标定。
S1.2.1四个鱼眼相机4的内参标定。使用Opencv中鱼眼相机标定模块对所使用的相机传感器进行标定，求得每个鱼眼相机4的畸变系数和内参矩阵K。
S1.2.2四个鱼眼相机4的外参标定。首先定义环视相机外参标定板置于Z＝0平面，并以传感器安装板3中心为原点建立信息采集及标定硬件系统坐标系。然后，获取环视相机外参标定板7中各角点的在信息采集及标定硬件系统坐标系下的坐标，以及获取图像中的角点的像素坐标，求取图像平面与环视相机外参标定板7之间的单应性矩阵。最后分解单应性矩阵求取所设信息采集及标定硬件系统坐标系与每个鱼眼相机坐标系之间的相对位姿以完成四个鱼眼4相机外参标定，并可获得环视相机组坐标系与信息采集及标定硬件系统坐标系的位姿关系。
S1.3，三维激光雷达1与环视相机组标定。使用三维激光雷达1以及环视相机组分别获取4个圆形镂空标定板8中100个标准圆心空间位置，然后通过点云配准获取激光雷达在在信息采集及标定硬件系统坐标系下的坐标下的位姿，完成三维激光雷达坐标系与环视相机组坐标系的外参标定。
S1.3.1，三维激光雷达1与环视相机组获取圆心空间位置。通过对激光雷达的三维点云进行边缘检测获得圆心，并利用OpenCV获取图像中的Blob质心。
S1.3.2，使用基于3D-3D的迭代最近点算法估计外参。将三维激光雷达1以及环视相机组获取的100个标准圆心空间位置作为迭代最近点算法的输入，获得三维激光雷达1在在信息采集及标定硬件系统坐标系下的坐标下的位姿。
S2，使用基于混合特征约束的船舶场景点簇、线和面特征选取方法，获得船舶不同功能区域的典型形貌特征。主要从三维激光雷达点云中提取点簇特征和面特征，其中点簇特征用于描述船舶机舱、甲板中常见的围栏杆、信号杆等物体，面特征用于描述船舱、货舱典型的舱壁、舱底、走廊地面等特征。并且，从环视图像中提取点和线特征。
S2.1，三维激光雷达点云中点簇和面特征提取。首先将点云数据转换为距离图像，然后进行点云噪声处理，最后基于rangNet++获得点簇和面特征。
S2.1.1，生成距离图像。将在t时刻获取的点云数据投影到一个距离图像上，其中三维激光雷达1的水平和垂直角度分辨率分别为sl和cl，而投影的距离图像的分辨率为360/sl乘30/cl+1。
S2.1.2，距离图像去噪。将连续三帧激光雷达合并为一个点云并转换为距离图像，将距离图像分割为很多个聚类，同一个聚类的点被标记上唯一的标识。点数较少的聚类(少于50点)被作为噪声去除，以减少环境噪音点的干扰。
S2.1.3，激光雷达点云中的点簇和面特征提取。基于rangNet++提取出舱室9中的扶梯点簇、墙面和地面、甲板围栏10点簇、货舱舱壁11、海面12等点簇{Pli}L和面特征{PLi}L。
S2.2，环视图像中点和线特征提取。对于每个输入RGB图像帧，提取图像ORB角点特征获得点特征{Pci}C，同时使用LSD直线检测方法获得图片中的线特征{Li}C。
S2.3，混合特征约束构建及鲁棒混合特征选取。利用激光雷达以及环视图像获得的点、点簇、线和面特征进行约束条件构建，进一步选取鲁棒混合点、点簇、线和面特征。
S2.3.1，三维激光雷达特征与图像特征融合。首先进行将三维激光雷达的点云以及面特征进行向图像平面进行平面投影，获得带有激光雷达深度信息和点簇、面特征的复合环视图像。
S2.3.2，激光雷达鲁棒点簇特征提取。典型的甲板围栏10在图像中是以阵列的线特征形式存在的，依据此原则，检测激光雷达点簇特征附近是否出现连续等距的图像线特征，若存在则保留该点簇特征，否则进行特征删除。
S2.3.3，环视图像中鲁棒点选取。使用k-d树搜索获得图像ORB角特征附近的三个最近的三维激光雷达点，然后计算三个激光雷达点的距离极差，如果大于距离阈值，则在{Pci}C中删除该点特征，否则，取三点距离平均值赋予该点特征。
S2.3.4，环视图像中鲁棒线特征集构建。在船舱走廊、甲板、大型船舱中线特征是保证建图鲁棒性的重要环境特征，为此所有的线特征都将被保留用于后续计算，但是不同线特征会赋予不同鲁棒权重。如果Li处于{PLi}L中任意两个舱室或者货舱面特征的边缘，则认为该线特征是更为鲁棒的线特征，其计算权重wLi设置为普通线特征的两倍。
S2.3.5，激光雷达鲁棒面特征提取。面特征{PLi}L中将删除海面形成的平面特征，其次将依据面特征的尺寸设置自适应的权重值
Figure BDA0003803583810000151
尺寸越大，权重越高。
S3，使用融合视觉和点云信息的自适应点-点簇-线-面里程计构建方法获得船舶不同功能区域下的传感器运动估计。首先采用图像点特征和线特征构建自适应视觉里程计来估计图像两帧间的粗略运动Tck。然后利用外部参数将估计结果转化为激光雷达坐标系，作为激光雷达扫描的初始变换估计，然后使用点簇特征和面特征构建自适应激光雷达里程计确定精准的运动估计Tlk，并获得该时刻信息采集及标定硬件系统坐标系在世界坐标系下的坐标Pi,w。
S3.1，对于每个输入RGB图像帧，依据S2步骤，提取图像中鲁棒的ORB特征和线特征。计算Tck＝{Rck，tck}，使得第k帧和k-1帧对应的点和线特征集之间的距离最小，获得粗略的视觉里程计结果。
S3.1.1，对于k帧中的第i个点Pci，其可以被投影到图像平面
Figure BDA0003803583810000152
S3.1.2，给定一个这个点特征的观测数据pci,k，可以定义误差为
Figure BDA0003803583810000153
S3.1.3，对于k帧中的第i个边特征Li(Lsi,Lei)，Lsi和Lei在同一关键帧上对应的图像投影(以齐次坐标表示)可以写成
Figure BDA0003803583810000154
Figure BDA0003803583810000155
S3.1.4，给定图像第i个线特征的端点观测值
Figure BDA0003803583810000156
和Lei,k，并估计观察线特征系数
Figure BDA0003803583810000157
定义以下线特征误差向量:
Figure BDA0003803583810000161
Figure BDA0003803583810000162
S3.1.5，为视觉里程计建立一个统一的代价函数，将每个误差项集成为
Figure BDA0003803583810000163
其中，hr(·)为Huber鲁棒代价函数，Ωci,k，Ω′ci,k和Ω″ci,k为检测到的点特征和线特征端点尺度相关联的协方差矩阵。wLi可以自适应的调整不同线特征对误差项的影响。
S3.2，对于每个输入激光雷达点云信息，依据S2步骤，提取点云数据中鲁棒的点簇特征和面特征。计算Tlk＝{Rlk，tlk}，使得第k帧和k-1帧对应的点簇和面特征集之间的距离最小，获得精准的激光雷达里程计结果。
S3.2.1，将环视相机组坐标系中的运动估计转化到三维激光雷达坐标系下，并作为激光雷达点云扫描匹配的初始值。问题表示为AX＝XB，X为环视相机组坐标系与三维激光雷达坐标系的外参矩阵，A,B分别为两个传感器所经历的变换。在该系统中，它可以被描述为：
TckTCL＝TCLTlk
其中，TCL为环视相机组坐标系与三维激光雷达坐标系的外参矩阵，Tck为两帧图像之间的初始变换，Tlk为对应的激光雷达扫描点云数据之间的变换。
S3.2.2，激光雷达里程计的初始值可表示为
Figure BDA0003803583810000164
S3.2.3，对于k帧三维激光雷达点云中的第i个点簇特征Pli，其经过坐标转换后的预计位姿为
Figure BDA0003803583810000165
S3.2.4，给定一个这个点特征的观测数据pli,k，可以定义误差为
Figure BDA0003803583810000166
S3.2.5，对于k帧三维激光雷达点云中的第i个面特征PLi，其经过坐标转换后的预计位姿为
Figure BDA0003803583810000171
其中，Rlk和tlk为Tlk中的旋转矩阵和位移向量。ni为面特征PLi的单位法线，di为原点到平面的垂直距离。
S3.2.6，给定一个这个面特征的观测数据pli,k，可以定义误差为
Figure BDA0003803583810000172
S3.2.7，为三维激光雷达建立一个统一的代价函数，将每个误差项集成为
Figure BDA0003803583810000173
其中，hr(·)为Huber鲁棒代价函数，Ωli,k和Ωpli,k为检测到的点簇特征和面特征端点尺度相关联的协方差矩阵。WPL和wPLi分别是朝向权重和尺寸权重可以自适应的调整不同线特征对误差项的影响。
S3.2.8，WPL权重大小取决于面特征的法向量一致性，如果法向量基本平行，那么就是退化环境，这是应降低面特征的权重，其计算公式如下
Figure BDA0003803583810000174
其中，zero(·)表示将{PLi}L中所有面特征进行配对并对他们的方向量进行叉乘并获得约等于0的相互平行的平面特征数量，
Figure BDA0003803583810000175
是整体配对次数，N为面特征数量，同时为了避免面特征权重过小，限制最小权重为0.5。
S3.2.9，Tlk转换为前后时刻信息采集标定硬件坐标系之间的变换Tsk。以初始位置为世界坐标系原点，将中间所有Tsk连乘获得该时刻信息采集及标定硬件系统坐标系在世界坐标系下的坐标Pi,w，并生成该获得该时刻“点-点簇-线-面”多模态混合特征信息
S3.3，最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图。
S4，依据该时刻多模态混合特征信息、Pi,w和未优化的船舶场景地图，采用基于多模态混合特征的多阶段回环检测策略方法高效精准确定候选回环点。首先采用基于点线视觉词袋的多候选回环点选择方法获得初始搜素空间{CC}，然后再利用激光雷达平面数量约束对多个候选点进行进一步筛选获得{CP},最后利用全部点簇特征进行精准匹配，获得最终的回环检测点CF及回环约束。
S4.1，在每隔5米或者10秒钟就将环视视觉系统和三维激光雷达的混合特征信息存储起来作为关键帧数据用于回环检测。
S4.2，环视视觉系统数据中的关键点和线条特征被用于建立特定的视觉词汇表和数据库。三维激光雷达点云中面特征数量也被设置为特殊环境描述子。
S4.3，在环视视觉系统运动过程中，实时获得的环视图像帧中关键点和线条特征形成视觉描述符，并用词袋搜索在数据库中于寻找与关键帧词汇表视觉匹配程度高的候选区域，以此获得多个候选回环点{CC}。
S4.4，获的当前激光雷达的面特征数量，并且与{CC}中每个位置的面特征数量进行比对，选取面特征数量相近的区域构成{CP}。
S4.5，计算{CP}中每个位置出的激光雷达点簇特征与激光雷达关键帧点簇的匹配结果，如果存在超过匹配阈值，则选取匹配值最大的CF与当前信息采集及标定硬件系统位姿形成回环约束，他们之间的位姿变换表示为Tic。
S4.6，该时刻信息采集及标定硬件系统坐标系在世界坐标系下的坐标表示为Pi,w＝TicCF。
S5，依据所有历史获得的Tlk，Pi,w以及回环约束，进行混合点-点簇-线-面特征的船舶场景地图优化与构建。
S5.1，将信息采集及标定硬件系统的全局位姿作为姿态节点插入到图中，每个时刻下之间的变换表示为边，将两个相邻姿态节点之间的转换误差定义为：
Figure BDA0003803583810000191
S5.2，计算全局优化的最小二乘代价：
Figure BDA0003803583810000192
Figure BDA0003803583810000193
Σo是里程计噪声协方差矩阵。
S5.3，使用ceres优化库得到最佳姿态，将所有位姿节点与其中包含的视觉点特征、线特征以及三维激光雷达点簇特征和面特征一起重新排列，以形成混合点-点簇-线-面特征的全局地图。
总体而言，本发明公开了一种基于多模态混合特征的船舶场景同时建图与定位方法及系统。首先搭建便捷式船舶环境信息采集及标定硬件系统，并进行环视相机和三维激光雷达的内外参标定。其次，采用基于混合特征约束的船舶典型点、点簇、线和面特征方法，以全面描述船舶不同功能区域的形貌特性。其次，提出一种融合视觉和点云信息的自适应点-点簇-线-面里程计构建方法，以鲁棒地获得不同船舶区域下的运动估计。此外，采用基于多模态混合特征的多阶段回环检测策略方法，以高效精准确定候选回环点及回环约束。。最终，采用基于位姿图优化的方法实现全局优化并获得包含不同功能区域形貌的混合点-点簇-线-面特征的船舶场景地图。与传统方法相比，本方法可以鲁棒地应用于船舶舱室、货仓和甲板等不同船舶功能区域的定位与地图构建，并结合了激光雷达和视觉信息生成了新型的混合点-点簇-线-面特征船舶地图满足不同船舶区域的导航定位需求。
以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：包括以下步骤，
S1、搭建信息采集标定装置，信息采集标定装置包括激光雷达和环视相机组，而后对该装置内的激光雷达和环视相机组进行内外参标定，并通过该装置获得包含激光雷达点云和环视视觉图像的多模态传感器信息；
S2、利用多模态传感器信息及标定结果，结合基于混合特征约束的船舶场景点簇、线和面特征选取方法，获得船舶不同功能区域的典型形貌特征；
S3、使用融合视觉和点云信息的自适应点-点簇-线-面里程计构建方法获得船舶不同功能区域下的传感器运动估计；具体地，首先采用图像点特征和线特征构建自适应视觉里程计来估计环视视觉图像两帧间的粗略运动Tck；然后利用外部参数将估计结果转化为激光雷达坐标系，作为激光雷达扫描的初始变换估计，然后使用点簇特征和面特征构建自适应激光雷达里程计确定精准的运动估计Tlk，并获得该时刻“点-点簇-线-面”多模态混合特征信息及信息采集标定装置坐标系在世界坐标系下的坐标Pi,w；最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图；
S4、依据该时刻多模态混合特征信息、Pi,w和未优化的船舶场景地图，采用基于多模态混合特征的多阶段回环检测策略方法确定候选回环点，并形成回环约束；
S5、依据所有历史获得的Tlk、Pi,w以及回环约束，采用基于位姿图优化的方法实现全局优化并获得混合点-点簇-线-面特征的船舶场景地图。
2.根据权利要求1所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：所述信息采集标定装置包括环视相机外参标定板，环视相机外参标定板上设置有传感器便携支架，传感器便携支架上连接有传感器安装板，传感器安装板通过激光雷达支架连接有三维激光雷达；传感器安装板周侧设置有环视相机组，环视相机组包括若干鱼眼相机，环视相机组周侧设置有若干与鱼眼相机相对应的圆形镂空标定板；
激光雷达支架的长度尺寸使三维激光雷达产生的向下射线不接触传感器安装板和环视相机组，三维激光雷达及各鱼眼相机与各圆形镂空标定板之间的位置关系使三维激光雷达及各语言相机能够完整观测到所有的圆形镂空标定板；环视相机外参标定板位于传感器安装板的正下方，且具有一定的面积，使得每个鱼眼相机都能够从环视相机外参标定板上获取一定数量的特征点。
3.根据权利要求2所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：传感器安装板为顶面和底面是正方形的长方体形，其每个侧面的中间位置均设置有用于固定鱼眼相机的相机支架；圆形镂空板为正方体板，其上均匀设置有若干圆形镂空孔洞。
4.根据权利要求2所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：各鱼眼相机的内参标定过程具体如下，使用Opencv中鱼眼相机标定模块对各鱼眼相机进行标定，得到各鱼眼相机的畸变系数和内参矩阵K；
各鱼眼相机的外参标定过程具体如下，首先定义环视相机外参标定板置于Z＝0平面，并以传感器安装板中心为原点建立信息采集标定装置坐标系；然后获取环视相机外参标定板中各角点的在信息采集标定装置坐标系下的坐标，以及获取图像中的角点的像素坐标，求取图像平面与环视相机外参标定板之间的单应性矩阵；最后分解单应性矩阵求取所设信息采集标定装置坐标系与每个鱼眼相机坐标系之间的相对位姿以完成四个鱼眼相机外参标定，并获得环视相机组坐标系与信息采集标定装置坐标系的位姿关系；
三维激光雷达与环视相机组标定过程具体如下，采用三维激光雷达与环视相机组探测各圆形镂空标定板，通过对三维激光雷达的三维点云进行边缘检测获得圆形镂空标定板中圆形镂空孔洞的圆心，并利用OpenCV获取图像中的Blob质心；随后使用基于3D-3D的迭代最近点算法估计外参，具体地，将三维激光雷达以及环视相机组获取的所有圆心空间位置作为迭代最近点算法的输入，获得三维激光雷达在在信息采集标定装置系统坐标系下的坐标下的位姿。
5.根据权利要求2所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：S2中利用三维雷达的雷达点云中提取点簇特征和面特征，利用环视相机组的环视图像中提取点和线特征；其中点簇特征用于描述包括船舶机舱、甲板中的围栏杆、信号杆，面特征用于描述包括船舱、货舱典型中的舱壁、舱底、走廊地面；
具体地，三维雷达的雷达点云中点簇特征和面特征的提取过程如下，将雷达点云数据转换为距离图像，将在t时刻获取的雷达点云数据投影到一个距离图像上，其中三维激光雷达的水平和垂直角度分辨率分别为sl和cl，而投影的距离图像的分辨率为360/sl乘30/cl+1；然后进行点云噪声处理，将连续三帧激光雷达的雷达点云合并为一个点云并转换为距离图像，将距离图像分割为若干个聚类，同一个聚类的点被标记上唯一的标识，其中点数少于一定数目的聚类被作为噪声去除；最后基于rangNet++提取出舱室中的点簇特征{Pli}L和面特征{PLi}L；
环视相机组的环视图像的点和线特征提取过程如下，对于每个输入RGB图像帧，提取图像ORB角点特征获得点特征{Pci}C，同时使用LSD直线检测方法获得图片中的线特征{Li}C；
随后将通过上述方法获取到的点、点簇、线、面特征进行约束条件构建，选取鲁棒混合点、点簇、线和面特征，具体如下，
首先，将三维激光雷达的点云以及面特征向图像平面进行平面投影，获得带有激光雷达深度信息和点簇、面特征的复合环视图像；
检测激光雷达点簇特征附近是否出现连续等距的图像线特征，若存在则保留该点簇特征，否则进行特征删除；
使用k-d树搜索获得图像ORB角特征附近的三个最近的三维激光雷达点，计算三个激光雷达点的距离极差，如果大于距离阈值，则在{Pci}C中删除该点特征，否则，取三点距离平均值赋予该点特征；
若某一线特征Li处于{PLi}L中任意两个舱室或者货舱面特征的边缘，则将其计算权重
Figure FDA0003803583800000041
设置为普通线特征的两倍；
在面特征{PLi}L中删除由海面形成的面特征，并根据面特征的尺寸由小至大依次设置由低到高的权重值。
6.根据权利要求5所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：S3具体如下，
对环视图像的每个输入RGB图像帧，依据S2步骤，提取提取图像中鲁棒的ORB特征和线特征；计算相机位姿变化矩阵Tck＝{Rck，tck}，其中Rck和tck是Tck的旋转矩阵和平移矩阵，使得第k帧和k-1帧对应的点和线特征集之间的距离最小，获得粗略的视觉里程计结果；
随后，对于每个输入激光雷达点云信息，依据S2步骤，提取点云数据中鲁棒的点簇特征和面特征，计算激光雷达位姿Tlk＝{Rlk，tlk}，Rlk和tlk是Tlk的旋转矩阵和平移矩阵，使得第k帧和k-1帧对应的点簇和面特征集之间的距离最小，获得精准的激光雷达里程计结果，即标定装置坐标系在世界坐标系下的坐标Pi,w以及该时刻“点-点簇-线-面”多模态混合特征信息；
最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图。
7.根据权利要求6所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：所述视觉里程计结果获取过程具体如下，
对于k帧中的第i个点Pci，其能够被投影到图像平面，于是有，
Figure FDA0003803583800000051
给定一个该点特征的观测数据pci,k，并定义误差为eci,k，并有
Figure FDA0003803583800000052
对于k帧中的第i个边特征Li(Lsi,Lei)，Lsi和Lei是特征起点和终点，Lsi和Lei在同一关键帧上对应的图像投影表示为，
Figure FDA0003803583800000053
Figure FDA0003803583800000054
π(·)是投影变换，K是相机内参矩阵，给定图像第i个线特征的端点观测值
Figure FDA0003803583800000055
和Lei,k，并估计观察线特征系数
Figure FDA0003803583800000056
并定义线特征误差向量如下，
Figure FDA0003803583800000057
Figure FDA0003803583800000058
建立视觉里程计的统一的代价函数，其中每个误差项集成如下，
Figure FDA0003803583800000059
其中，hr(·)为Huber鲁棒代价函数，Ωci,k，Ω′ci,k和Ω″ci,k为检测到的点特征和线特征端点尺度相关联的协方差矩阵。
Figure FDA00038035838000000510
为自适应调节参数，用于自适应地调整不同线特征对误差项的影响。
8.根据权利要求6所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：所述激光雷达里程计结果获取过程具体如下，
将环视相机组坐标系中的运动估计转化到三维激光雷达坐标系下，并作为激光雷达点云扫描匹配的初始值，于是有，
TckTCL＝TCLTlk
其中，TCL为环视相机组坐标系与三维激光雷达坐标系的外参矩阵，Tck为两帧图像之间的初始变换，Tlk为对应的激光雷达扫描点云数据之间的变换；
激光雷达里程计的初始值表示为，
Figure FDA0003803583800000061
对于k帧三维激光雷达点云中的第i个点簇特征Pli，其经过坐标转换后的预计位姿为，
Figure FDA0003803583800000062
给定一个该点特征的观测数据pli,k，并定义误差为，
Figure FDA0003803583800000063
对于k帧三维激光雷达点云中的第i个面特征PLi，其经过坐标转换后的预计位姿为，
Figure FDA0003803583800000064
其中，Rlk和tlk为Tlk中的旋转矩阵和位移向量。ni为面特征PLi的单位法线，di为原点到平面的垂直距离；
给定一个该面特征的观测数据pli,k，并定义误差为，
Figure FDA0003803583800000065
建立三维激光雷达的统一的代价函数，将每个误差项集成如下，
Figure FDA0003803583800000066
其中，hr(·)为Huber鲁棒代价函数，Ωli,k和Ωpli,k为检测到的点簇特征和面特征端点尺度相关联的协方差矩阵，WPL和
Figure FDA0003803583800000067
分别是朝向权重和尺寸权重，用于自适应地调整不同线特征对误差项的影响；
WPL权重大小取决于面特征的法向量一致性，其计算公式如下，
Figure FDA0003803583800000071
其中，zero(·)表示将{PLi}L中所有面特征进行配对并对他们的方向量进行叉乘并获得约等于0的相互平行的平面特征数量，
Figure FDA0003803583800000072
是整体配对次数，N为面特征数量，同时为了避免面特征权重过小，限制最小权重为0.5；
将Tlk转换为前后时刻信息采集标定装置坐标系之间的变换Tsk；以初始位置为世界坐标系原点，将中间所有Tsk连乘获得该时刻信息采集标定装置坐标系在世界坐标系下的坐标Pi,w，并生成该获得该时刻“点-点簇-线-面”多模态混合特征信息；
最后，依据历史所有的Pi,w及对应的多模态混合特征信息，拼接成未优化的船舶场景地图。
9.根据权利要求2所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：S4所述基于多模态混合特征的多阶段回环检测策略方法包括以下步骤，
首先采用基于点线视觉词袋的多候选回环点选择方法获得初始搜素空间{CC}，然后再利用激光雷达平面数量约束对多个候选点进行进一步筛选获得{CP}，最后利用全部点簇特征进行精准匹配，获得最终的回环检测点CF；
具体地，首先，每隔一定距离长度或一定时间间隔，将通过环视相机和三维激光雷达获取的混合特征信息进行存储，作为关键帧数据用于回环检测；利用通过环视相机获取的数据中的关键点和线条特征建立视觉词汇表和数据库，利用通过三维激光雷达获取的雷达点云中的面特征数量设置特殊环境描述子；在环视相机进行运动采集的过程中，根据其实时获取的环视图像帧中的关键点和线条特征形成视觉描述符，而后利用词袋搜索在上述数据库中寻找与关键帧词汇表视觉匹配程度超过一定阈值的候选区域，以此获得多个候选回环点{CC}；
随后，获得当前激光雷达的面特征数量，并且与{CC}中每个位置的面特征数量进行比对，选取面特征数量相近的区域构成{CP}；
最后，计算{CP}中每个位置出的激光雷达点簇特征与激光雷达关键帧点簇的匹配结果，如果存在匹配结果超过匹配阈值，则选取匹配值最大的CF与当前信息采集标定装置位姿形成回环约束，二者之间的位姿变换表示为Tic；该时刻信息采集标定装置坐标系在世界坐标系下的坐标Pi,w表示为Pi,w＝TicCF。
10.根据权利要求9所述的基于多模态混合特征的船舶场景同时建图与定位方法，其特征在于：S5过程具体为，
将信息采集标定装置的全局位姿作为姿态节点插入到图中，每个时刻下之间的变换表示为边，将两个相邻姿态节点之间的转换误差定义如下，
Figure FDA0003803583800000081
随后计算计算全局优化的最小二乘代价，
Figure FDA0003803583800000082
Figure FDA0003803583800000083
其中，Σo是里程计噪声协方差矩阵；
而后，使用ceres优化库得到最佳姿态，将所有位姿节点与其中包含的视觉点特征、线特征以及三维激光雷达点簇特征和面特征一起重新排列，以形成混合点-点簇-线-面特征的全局地图。
CN202210990299.4A 2022-08-18 2022-08-18 基于多模态混合特征的船舶场景同时建图与定位方法 Active CN115451964B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210990299.4A CN115451964B (zh) 2022-08-18 2022-08-18 基于多模态混合特征的船舶场景同时建图与定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210990299.4A CN115451964B (zh) 2022-08-18 2022-08-18 基于多模态混合特征的船舶场景同时建图与定位方法

Publications (2)

Publication Number Publication Date
CN115451964A true CN115451964A (zh) 2022-12-09
CN115451964B CN115451964B (zh) 2024-04-30

Family

ID=84298832

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210990299.4A Active CN115451964B (zh) 2022-08-18 2022-08-18 基于多模态混合特征的船舶场景同时建图与定位方法

Country Status (1)

Country Link
CN (1) CN115451964B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116958265A (zh) * 2023-09-19 2023-10-27 交通运输部天津水运工程科学研究所 一种基于双目视觉的船舶位姿测量方法及系统
CN117315617A (zh) * 2023-09-06 2023-12-29 武汉理工大学 基于师徒模式的网络优化方法、系统、电子设备及介质
CN117671022A (zh) * 2023-11-02 2024-03-08 武汉大学 一种室内弱纹理环境的移动机器人视觉定位系统及方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112149471A (zh) * 2019-06-28 2020-12-29 北京初速度科技有限公司 一种基于语义点云的回环检测方法及装置
CN112985416A (zh) * 2021-04-19 2021-06-18 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及系统
WO2021147546A1 (zh) * 2020-01-20 2021-07-29 深圳市普渡科技有限公司 多传感器融合slam系统、多传感器融合方法及机器人、介质

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112149471A (zh) * 2019-06-28 2020-12-29 北京初速度科技有限公司 一种基于语义点云的回环检测方法及装置
WO2021147546A1 (zh) * 2020-01-20 2021-07-29 深圳市普渡科技有限公司 多传感器融合slam系统、多传感器融合方法及机器人、介质
CN112985416A (zh) * 2021-04-19 2021-06-18 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张伟伟;陈超;徐军;: "融合激光与视觉点云信息的定位与建图方法", 计算机应用与软件, no. 07, 12 July 2020 (2020-07-12) *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117315617A (zh) * 2023-09-06 2023-12-29 武汉理工大学 基于师徒模式的网络优化方法、系统、电子设备及介质
CN117315617B (zh) * 2023-09-06 2024-06-07 武汉理工大学 基于师徒模式的网络优化方法、系统、电子设备及介质
CN116958265A (zh) * 2023-09-19 2023-10-27 交通运输部天津水运工程科学研究所 一种基于双目视觉的船舶位姿测量方法及系统
CN117671022A (zh) * 2023-11-02 2024-03-08 武汉大学 一种室内弱纹理环境的移动机器人视觉定位系统及方法

Also Published As

Publication number Publication date
CN115451964B (zh) 2024-04-30

Similar Documents

Publication Publication Date Title
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN115451964A (zh) 基于多模态混合特征的船舶场景同时建图与定位方法
CN112396656B (zh) 一种视觉与激光雷达融合的室外移动机器人位姿估计方法
CN111340939B (zh) 一种室内三维语义地图构建方法
CN109961497A (zh) 基于无人机影像的实时三维重建方法
CN106529538A (zh) 一种飞行器的定位方法和装置
CN112819903A (zh) 基于l型标定板的相机和激光雷达联合标定的方法
GB2506411A (en) Determination of position from images and associated camera positions
CN106997614A (zh) 一种基于深度相机的大规模场景3d建模方法及其装置
CN112991534B (zh) 一种基于多粒度物体模型的室内语义地图构建方法及系统
CN113642463B (zh) 一种视频监控和遥感图像的天地多视图对齐方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN112489099A (zh) 点云配准方法、装置、存储介质及电子设备
CN114677435A (zh) 一种点云全景融合要素提取方法和系统
CN113313659A (zh) 一种多机协同约束下高精度图像拼接方法
CN116309817A (zh) 一种基于rgb-d相机的托盘检测与定位方法
CN112767459A (zh) 基于2d-3d转换的无人机激光点云与序列影像配准方法
Jiang et al. Learned local features for structure from motion of uav images: A comparative evaluation
Deng et al. Joint calibration of dual lidars and camera using a circular chessboard
CN113313824A (zh) 一种三维语义地图构建方法
CN114627365B (zh) 场景重识别方法、装置、电子设备及存储介质
CN115908564A (zh) 自动运输设备的仓储巡线方法和自动运输设备
CN107784666B (zh) 基于立体影像的地形地物三维变化检测和更新方法
CN112507776A (zh) 一种快速大范围语义地图构建方法
CN112132029A (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