CN113340295B - 一种多个测距传感器的无人艇近岸实时定位与建图方法 - Google Patents

一种多个测距传感器的无人艇近岸实时定位与建图方法 Download PDF

Info

Publication number
CN113340295B
CN113340295B CN202110668955.4A CN202110668955A CN113340295B CN 113340295 B CN113340295 B CN 113340295B CN 202110668955 A CN202110668955 A CN 202110668955A CN 113340295 B CN113340295 B CN 113340295B
Authority
CN
China
Prior art keywords
point cloud
factor
unmanned ship
follows
laser
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
CN202110668955.4A
Other languages
English (en)
Other versions
CN113340295A (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.)
Guangdong Haiqinyuan Information Technology Co ltd
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202110668955.4A priority Critical patent/CN113340295B/zh
Publication of CN113340295A publication Critical patent/CN113340295A/zh
Application granted granted Critical
Publication of CN113340295B publication Critical patent/CN113340295B/zh
Priority to US17/584,598 priority patent/US11450016B1/en
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/15—Correlation function computation including computation of convolution operations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T15/00—3D [Three Dimensional] image rendering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Electromagnetism (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Computing Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Optics & Photonics (AREA)
  • Computer Graphics (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种多个测距传感器的无人艇近岸实时定位与建图方法，包括以下步骤：步骤A：惯性测量单元获取无人艇的预测位姿数据；步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点。步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维云点图。

Description

一种多个测距传感器的无人艇近岸实时定位与建图方法
技术领域
本发明涉及无人艇技术领域，特别是一种多个测距传感器的无人艇近岸实时定位与建图方法。
背景技术
目前市面上的测绘无人艇大多采用惯导系统定位加激光雷达建图或惯导系统定位加声呐建图的方案。基于惯导系统定位的无人艇在经过桥洞或一些多遮挡物地区时，由于惯导系统信号不稳定，会造成无人艇位姿估计产生误差从而造成测绘建图的不精确。
此外，目前多传感器融合定位仍有很多未解决的问题。如何在无人艇上使用多个激光雷达进行厘米级的定位和多个声呐构建高精度的三维点云地图，是无人艇多传感器融合定位的热门话题。
发明内容
针对上述缺陷，本发明的目的在于提出一种多个测距传感器的无人艇近岸实时定位与建图方法。
为达此目的，本发明采用以下技术方案：
一种多个测距传感器的无人艇近岸实时定位与建图方法包括以下步骤：
步骤A：惯性测量单元获取无人艇的预测位姿数据；
步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点。
步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；
步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；
步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；
步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维点云图。
在一个实施例中，所述步骤A的具体如下：
所惯性测量单元根据当前时刻的角速度
Figure GDA0003352462300000021
与角加速度
Figure GDA0003352462300000022
通过四阶龙格库塔法预积分得到无人艇的预测位姿，其中所述无人艇的预测位姿包括当前帧与上一帧的相对旋转
Figure GDA0003352462300000023
相对位移
Figure GDA0003352462300000024
速度Vt、角速度计偏置bg，t和角加速度计偏置ba，t；
其中相对旋转
Figure GDA0003352462300000025
的四阶龙格库塔预积分公式为：
Figure GDA0003352462300000026
其中
Figure GDA0003352462300000027
的取值范围如下
Figure GDA0003352462300000028
Figure GDA0003352462300000031
角速度
Figure GDA0003352462300000032
是一个三维的向量，
Figure GDA0003352462300000033
和
Figure GDA0003352462300000034
分别表示三个方向轴的数据；
Figure GDA0003352462300000035
所述速度Vt的四阶龙格库塔预积分公式为：
Figure GDA0003352462300000036
Figure GDA0003352462300000037
Figure GDA0003352462300000038
Figure GDA0003352462300000039
其中，Vt为当前t时刻的速度，Vt-1为t-1时刻的速度，C(q)函数是从四元数转换到旋转矩阵R的函数；
所述相对位移
Figure GDA00033524623000000310
的四阶龙格库塔预积分公式为：
Figure GDA00033524623000000311
其中
Figure GDA0003352462300000041
的取值范围如下
Figure GDA0003352462300000042
所述角速度计偏置bg，t的四阶龙格库塔预积分公式为：
bg，t＝bg，t-1，其中bg，t-1为t-1时刻的角速度计偏置；
所述角加速度计偏置ba，t的四阶龙格库塔预积分公式为：
ba，t＝ba，t-1，其中ba，t-1为t-1时刻的角加速度计偏置；
Figure GDA0003352462300000043
是一个四元数，qw表示实部，qv表示虚部，
Figure GDA0003352462300000044
的作用是把四元数归一化，wg表示当前位置的地球加速度、q0表示t-1时刻的姿态。
在一个实施例中，所述步骤B的具体步骤如下：
步骤B1：激光雷达获取当前时刻t的雷达点云数据
Figure GDA0003352462300000045
其中P代表点云，并将雷达点云数据
Figure GDA0003352462300000046
对应投射于分辨率为1800*16深度图的像素上；
步骤B2：对深度图进行有序化、深度值划分、降维、直线拟合、地面点筛选处理，获取地面点并进行标记；
步骤B3：使用基于深度图的障碍物快速分割算法把深度图中的点云分割成一个一个的聚类；
步骤B4：剔除点数少于30的聚类，将同一个聚类的激光点标记为分割点，删除分割点与地面点以外的激光点，得到处理深度图。
在一个实施例中，所述步骤C的具体步骤如下：
步骤C1：将处理深度图分割成六个分辨率为300*16的子深度图，获取子深度图的激光点Lipj的曲率，其中激光点Lipj的曲率获取公式如下：
Figure GDA0003352462300000051
rk和rj表示为子深度图中像素点的深度值，k、j为子深度图中的索引值，S是同一行连续十个激光点的索引的集合；
步骤C2：根据激光点Lipj的曲率，得到边缘点与平面点；
选取每个子深度图中每一行中两个分割点组成边缘特征点集，其中组成边缘特征点集的分割点为子深度图每一行中拥有最大曲率的两个边缘点；
选取每个子深度图中每一行中最小曲率C的四个平面点组成平面特征点集；
步骤C3：所有激光雷达的平面特征点集与边缘特征点集通过外参
Figure GDA0003352462300000052
将激光雷达的坐标转换到无人艇机体坐标系，并组成当前时刻的特征点集bFt，所述特征点集bFt包括边缘特征点集与平面特征点集。
在一个实施例中，所述步骤D的具体步骤如下：
步骤D1：将当前时刻相邻的二十五个关键帧中的特征点集bFt进行位姿变换得到局部特征点云图wMt，其中局部特征点云图wMt包括局部边缘特征点云图
Figure GDA0003352462300000053
和局部平面特征点云图
Figure GDA0003352462300000054
Figure GDA0003352462300000055
为局部边缘特征点云图的特征点集；
Figure GDA0003352462300000056
为局部平面特征点云图的特征点集；
步骤D2：使用四阶龙格库塔法预积分获取局部特征点云图的位姿变换矩阵
Figure GDA0003352462300000057
步骤D3：获取局部特征点云图wMt的所述特征点集
Figure GDA0003352462300000058
并将
Figure GDA0003352462300000059
从无人艇机体坐标系转换到导航坐标系；
步骤D4：在局部边缘特征点云图
Figure GDA0003352462300000061
选择三个激光点wpa e、wpb e和wpc e并构建边缘特征残差函数，其边缘特征残差函数公式如下：
Figure GDA0003352462300000062
在局部平面特征点云图
Figure GDA0003352462300000063
选择四个激光点wpa p、wpb p、wpc p、wpd p并构建平面特征残差函数，其平面特征残差函数公式如下：
Figure GDA0003352462300000064
步骤D5：获取边缘特征残差函数和平面特征残差函数的累加和，对边缘特征残差函数和平面特征残差函数的累加和构建非线性最小二乘问题，获得当前的无人艇的相对位姿变换矩阵
Figure GDA0003352462300000065
其中非线性最小二乘问题的公式如下：
Figure GDA0003352462300000066
其中ρa()表示的是第a个残差对应的自适应损失函数，其函数表示为：
Figure GDA0003352462300000067
其中其中，a∈R是一个随着残差的概率分布变化的参数。同时，随着a值的不同损失函数能自适应地计算残差的权重。c是一个比例系数且c＞0。
在一个实施例中，所述步骤E的具体步骤如下：
步骤E1：获取激光雷达里程计因子和惯性测量单元因子，其中惯性测量单元因子与所述无人艇的预测位姿一致，所述激光雷达里程计因子与所述相对位姿变换矩阵一致，使用激光雷达里程计因子与惯性测量单元因子约束因子图，其公式如下：
Figure GDA0003352462300000068
其中li(xj)为因子的负对数，i，j为自然数；
步骤E2：通过北斗卫星定位系统获取经纬度坐标，然后把经纬度坐标通过外参转换到导航坐标系下，通过北斗卫星导航获取北斗卫星定位信息BDSP，对北斗卫星定位信息BDSP与相邻的两个关键帧进行线性插值，获取北斗卫星定位因子；
其公式如下：BDSp(t)＝(1-t)*BDSp0-t*BDSp1，t∈[0，1]，其中BDSp0和BDSp1表示需要进行插值的两帧数据；
步骤E3：通过北斗卫星定位系统的数据解算获得北斗卫星定位的协方差矩阵，判断北斗卫星定位的协方差矩阵是否小于相对位姿变换矩阵，若是，根据北斗卫星定位因子约束因子图，其公式如下：
Figure GDA0003352462300000071
其中，BDSpt是北斗卫星t时刻在导航坐标系下的定位信息；
Figure GDA0003352462300000072
分别为t时刻无人艇相对于导航坐标系的旋转和平移；BDS∑t为北斗卫星t时刻的协方差矩阵。
步骤E4：通过关键帧的时间戳t与其相邻关键帧的时间戳t-1对多普勒速度计进行预积分，得到多普勒速度计因子，其公式如下：
Figure GDA0003352462300000073
其中DVLV(t)dt表示多普勒速度计在t时刻的速度；根据多普勒速度计因子约束因子图，其公式为：
Figure GDA0003352462300000074
其中DVL∑t为t时刻多普勒速度计的协方差矩阵，
Figure GDA0003352462300000075
表示t时刻无人艇相对于导航坐标系的平移，
Figure GDA0003352462300000076
表示t-1时刻无人艇相对于导航坐标系的平移，
Figure GDA0003352462300000077
表示t-1时刻到t时刻无人艇的相对平移；
步骤E5：在因子图中使用Scan Contest算法找到最新关键帧xt的先验关键帧xa，先验关键帧xa的周围通过位姿变换矩阵建立导航坐标系下的局部关键帧特征点云图wMa，wMa＝{wFt-12∪......∪wFt+12}；
步骤E6：通过最新关键帧xt的特征点集wFt与局部关键帧特征点云图wMa，得到两者之间的相对位姿矩阵
Figure GDA0003352462300000081
并向因子图添加闭环因子，其公式为：
Figure GDA0003352462300000082
在一个实施例中，所述步骤F的具体步骤如下：
步骤F1：对无人艇的位姿根据时间戳做线性插值和球面插值计算，去除激光雷达的运动畸变，其线性插值公式如下：bp(t)＝(1-t)*bp0*bp1，t∈[0，1]，其中bp0和bp1表示需要进行插值的两帧位置信息；
其球面线性插值公式如下：
Figure GDA0003352462300000083
其中bq0和bq1表示需要进行插值的两帧姿态信息；
步骤F2：对各个声呐点云数据使用PCL开源库中的VoxelGrid滤波器进行降采样滤波器；
步骤F3：将雷达点云数据、声呐点云数据使用PCL开源库中的函数根据插值后的无人艇的位姿拼接，组成三维点云地图。
本发明的有益效果：本发明使用了多个激光雷达一同构建帧间约束，并与惯性测量单元一同构成激光惯性里程计，能够计算出相邻两帧之间的位姿变换矩阵。其中，各个激光雷达的点云数据经过点云分割和特征点提取等预处理，能够消除环境中的噪声。同时，激光惯性里程计中的无约束优化函数采用自适损失函数，能够提高里程计的鲁棒性，最终实现高精度的局部定位。
附图说明
图1是本发明的一个实施例的流程示意图；
图2为本发明一个实施例中因子获取流程示意图；
图3为本发明一个实施例中因子约束获取三维点云地图流程图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
如图1～3所示，一种多个测距传感器的无人艇近岸实时定位与建图方法，包括以下步骤：
步骤A：惯性测量单元获取无人艇的预测位姿数据；
步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点。
步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；
步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；
步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；
步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维点云图。
本发明使用线性插值和球面插值的方法实现多个传感器之间的时间戳同步，能够解决各个传感器由于时钟不同而无法进行联合状态估计和建图的问题。
其次，本发明使用了多个激光雷达一同构建帧间约束，并与惯性测量单元一同构成激光惯性里程计，能够计算出相邻两帧之间的位姿变换矩阵。其中，各个激光雷达的点云数据经过点云分割和特征点提取等预处理，能够消除环境中的噪声。同时，激光惯性里程计中的无约束优化函数采用自适损失函数，能够提高里程计的鲁棒性，最终实现高精度的局部定位。
此外，激光惯性里程计得到的最新无人艇位姿将会加入到因子图优化中，并与北斗卫星定位因子、多普勒速度计因子以及闭环因子一同进行联合优化。其中，闭环因子能够消除激光惯性里程计的累计误差，从而能够实现无人艇在近岸环境下的厘米级全局定位；
最后，可使用多个激光雷达和多个多波束声呐一同构建陆地和水下的高精三维点云地图，能够帮助无人艇进行测绘、障碍物识别和自主导航等功能。
在一个实施例中，所述步骤A的具体如下：
所惯性测量单元根据当前时刻的角速度
Figure GDA0003352462300000111
与角加速度
Figure GDA0003352462300000112
通过四阶龙格库塔法预积分得到无人艇的预测位姿，其中所述无人艇的预测位姿包括当前帧与上一帧的相对旋转
Figure GDA0003352462300000113
相对位移
Figure GDA0003352462300000114
速度Vt、角速度计偏置bg，t和角加速度计偏置ba，t；
其中相对旋转
Figure GDA0003352462300000115
的四阶龙格库塔预积分公式为：
Figure GDA0003352462300000116
其中
Figure GDA0003352462300000117
的取值范围如下
Figure GDA0003352462300000118
Figure GDA0003352462300000119
角速度
Figure GDA00033524623000001110
是一个三维的向量，
Figure GDA00033524623000001111
和
Figure GDA00033524623000001112
分别表示三个方向轴的数据；
Figure GDA0003352462300000121
所述速度Vt的四阶龙格库塔预积分公式为：
Figure GDA0003352462300000122
Figure GDA0003352462300000123
Figure GDA0003352462300000124
Figure GDA0003352462300000125
其中，Vt为当前t时刻的速度，Vt-1为t-1时刻的速度，c(q)函数是从四元数转换到旋转矩阵R的函数；
所述相对位移
Figure GDA0003352462300000126
的四阶龙格库塔预积分公式为：
Figure GDA0003352462300000127
其中
Figure GDA0003352462300000131
的取值范围如下
Figure GDA0003352462300000132
所述角速度计偏置bg，t的四阶龙格库塔预积分公式为：
bg，t＝bg，t-1，其中bg，t-1为t-1时刻的角速度计偏置；
所述角加速度计偏置ba，t的四阶龙格库塔预积分公式为：
ba，t＝ba，t-1，其中ba，t-1为t-1时刻的角加速度计偏置；
Figure GDA0003352462300000133
是一个四元数，qw表示实部，qv表示虚部，
Figure GDA0003352462300000134
的作用是把四元数归一化，wg表示当前位置的地球加速度、q0表示t-1时刻的姿态。
使用惯性测量单元预积分出无人艇的预测位姿，目的是为了提高算法的实时性，同时为之后的因子图优化提供无人艇位置、姿态和速度上的约束。由于使用了惯性测量单元进行预积分，使得激光雷达里程计的输出频率从原来的10hz提升到了500hz，500hz的高精度位姿输出基本上能够满足无人艇的控制；同时，在因子图优化中加入惯性测量单元的预积分因子，不但能够使无人艇位姿估计更加准确，也能对惯性测量单元的随机游走噪声进行估计，使得下一次预积分的结果更加准确。总的来说，惯性测量单元与激光雷达进行紧耦合得到激光惯性里程计，并且使用因子图做优化是一个互补的过程。
在一个实施例中，所述步骤B的具体步骤如下：
步骤B1：激光雷达获取当前时刻t的雷达点云数据
Figure GDA0003352462300000141
其中P代表点云，并将雷达点云数据
Figure GDA0003352462300000142
对应投射于分辨率为1800*16深度图的像素上；
步骤B2：对深度图进行有序化、深度值划分、降维、直线拟合、地面点筛选处理，获取地面点并进行标记；
步骤B3：使用基于深度图的障碍物快速分割算法把深度图中的点云分割成一个一个的聚类；
步骤B4：剔除点数少于30的聚类，将同一个聚类的激光点标记为分割点，删除分割点与地面点以外的激光点，得到处理深度图。
本步骤的目的是为了去除激光雷达传感器的噪声，使得无人艇的位姿估计变得更鲁棒和稳定。而小于30个点的聚类一般是激光雷达扫到小的物体，比如树上的叶子，飞过的小鸟之类的。而且小物体是噪声的几率很大，所以直接剔除，减少不必要的运算。
在一个实施例中，所述步骤C的具体步骤如下：
步骤C1：将处理深度图分割成六个分辨率为300*16的子深度图，获取子深度图的激光点Lipj的曲率，其中激光点Lipj的曲率获取公式如下：
Figure GDA0003352462300000143
rk和rj表示为子深度图中像素点的深度值，k、j为子深度图中的索引值，S是同一行连续十个激光点的索引的集合；
步骤C2：根据激光点Lipj的曲率，得到边缘点与平面点；
选取每个子深度图中每一行中两个分割点组成边缘特征点集，其中组成边缘特征点集的分割点为子深度图每一行中拥有最大曲率的两个边缘点；
选取每个子深度图中每一行中最小曲率C的四个平面点组成平面特征点集；
步骤C3：所有激光雷达的平面特征点集与边缘特征点集通过外参
Figure GDA0003352462300000144
将激光雷达的坐标转换到无人艇机体坐标系，并组成当前时刻的特征点集bFt，所述特征点集bFt包括边缘特征点集与平面特征点集。
在激光点Lipj的曲率的判定中会引入判定阈值，当Cj小于判定阈值时该激光点Lipj为边缘点，反之当Cj大于判定阈值时该激光点Lipj为平面点，普通16线的激光雷达每一帧数据就有16*1800个点，如果对每个点都构建点-线约束或点-面约束这样运算量就太大了。通过提取雷达点云数据中有代表性的特征点，并只对特征点构建约束能使得无约束优化问题的运算量大大下降，同时也能提高鲁棒性。相比于对所有的激光点进行计算，基于特征点的方法对运行时间有13倍的提升，满足实时性和轻量级的要求。
在一个实施例中，所述步骤D的具体步骤如下：
步骤D1：将当前时刻相邻的二十五个关键帧中的特征点集bFt进行位姿变换得到局部特征点云图wMt，其中局部特征点云图wMt包括局部边缘特征点云图
Figure GDA0003352462300000151
和局部平面特征点云图
Figure GDA0003352462300000152
Figure GDA0003352462300000153
为局部边缘特征点云图的特征点集；
Figure GDA0003352462300000154
为局部平面特征点云图的特征点集；
步骤D2：使用四阶龙格库塔法预积分获取局部特征点云图的位姿变换矩阵
Figure GDA0003352462300000155
步骤D3：获取局部特征点云图wMt的所述特征点集
Figure GDA0003352462300000156
并将
Figure GDA0003352462300000157
从无人艇机体坐标系转换到导航坐标系；
步骤D4：在局部边缘特征点云图
Figure GDA0003352462300000158
选择三个激光点wpa e、wpb e和wpc e并构建边缘特征残差函数，其边缘特征残差函数公式如下：
Figure GDA0003352462300000161
在局部平面特征点云图
Figure GDA0003352462300000162
选择四个激光点wpa p、wpb p、wpc p、wpd p并构建平面特征残差函数，其平面特征残差函数公式如下：
Figure GDA0003352462300000163
步骤D5：获取边缘特征残差函数和平面特征残差函数的累加和，对边缘特征残差函数和平面特征残差函数的累加和构建非线性最小二乘问题，获得当前的无人艇的相对位姿变换矩阵
Figure GDA0003352462300000164
其中非线性最小二乘问题的公式如下：
Figure GDA0003352462300000165
其中ρa()表示的是第a个残差对应的自适应损失函数，其函数表示为：
Figure GDA0003352462300000166
其中其中，a∈R是一个随着残差的概率分布变化的参数。同时，随着a值的不同损失函数能自适应地计算残差的权重。c是一个比例系数且c＞0。
激光惯性里程计使用了自适应损失函数目的是为了减少特征点误匹配或传感器测量离群值对无约束优化问题求解的影响。使得无人艇在复杂水域环境下还能进行高精度的定位，同时在外界高动态环境下无人艇定位基本不受影响。
在一个实施例中，所述步骤E的具体步骤如下：
步骤E1：获取激光雷达里程计因子和惯性测量单元因子，其中惯性测量单元因子与所述无人艇的预测位姿一致，所述激光雷达里程计因子与所述相对位姿变换矩阵一致，使用激光雷达里程计因子与惯性测量单元因子约束因子图，其公式如下：
Figure GDA0003352462300000171
其中li(xj)一个正比于高斯条件概率密度函数的似然函数，i，j为自然数，X∈x0，...，xn，表示无人艇状态的集合，x0，...，xn表示无人艇0～n时刻的状态，p(X)无人艇状态服从高斯分布的联合概率密度函数；
步骤E2：通过北斗卫星定位系统获取经纬度坐标，然后把经纬度坐标通过外参转换到导航坐标系下，通过北斗卫星导航获取北斗卫星定位信息BDSp，对北斗卫星定位信息BDSp与相邻的两个关键帧进行线性插值，获取北斗卫星定位因子；
其公式如下：BDSp(t)＝(1-t)*BDSp0-t*BDSp1，t∈[0，1]，其中BDSp0和BDSp1表示需要进行插值的两帧数据；
步骤E3：通过北斗卫星定位系统的数据解算获得北斗卫星定位的协方差矩阵，判断北斗卫星定位的协方差矩阵是否小于相对位姿变换矩阵，若是，根据北斗卫星定位因子约束因子图，其公式如下：
Figure GDA0003352462300000172
其中，BDSpt是北斗卫星t时刻在导航坐标系下的定位信息；
Figure GDA0003352462300000173
分别为t时刻无人艇相对于导航坐标系的旋转和平移；BDS∑t为北斗卫星t时刻的协方差矩阵。
步骤E4：通过关键帧的时间戳t与其相邻关键帧的时间戳t-1对多普勒速度计进行预积分，得到多普勒速度计因子，其公式如下：
Figure GDA0003352462300000174
其中DVLV(t)dt表示多普勒速度计在t时刻的速度；根据多普勒速度计因子约束因子图，其公式为：
Figure GDA0003352462300000181
其中DVL∑t为t时刻多普勒速度计的协方差矩阵，
Figure GDA0003352462300000182
表示t时刻无人艇相对于导航坐标系的平移，
Figure GDA0003352462300000183
表示t-1时刻无人艇相对于导航坐标系的平移，
Figure GDA0003352462300000184
表示t-1时刻到t时刻无人艇的相对平移；
步骤E5：在因子图中使用Scan Contest算法找到最新关键帧xt的先验关键帧xa，先验关键帧xa的周围通过位姿变换矩阵建立导航坐标系下的局部关键帧特征点云图wMa，wMa＝{wFt-12∪......∪wFt+12}；
步骤E6：通过最新关键帧xt的特征点集wFt与局部关键帧特征点云图wMa，得到两者之间的相对位姿矩阵
Figure GDA0003352462300000185
并向因子图添加闭环因子，其公式为：
Figure GDA0003352462300000186
因子图优化目的是对多个传感器的信息进行松耦合。每个传感器都有自己的优势和劣势，比如北斗定位系统能够提供精确的位置信息，但会受到信号干扰的影响或者会出现没有信号的情况；惯性测量单元能提供比较准确的姿态信息并且不受外界干扰，但位置信息误差很大；激光惯性里程计能提供局部地区的厘米级高精度定位，但提供不了全局的定位信息，并且在大场景情况下会出现位姿漂移。所以，把这些传感器信息通过因子图进行融合，能做到取长补短，充分利用各个传感器相对准确的信息。同时，也加入闭环因子，对激光惯性里程计的漂移进行补偿。通过因子图，即使在gps信号不好的情况下，也能使得本算法能够提供全球厘米级高精度定位。
在一个实施例中，所述步骤F的具体步骤如下：
步骤F1：对无人艇的位姿根据时间戳做线性插值和球面插值计算，去除激光雷达的运动畸变，其线性插值公式如下：bp(t)＝(1-t)*bp0*bp1，t∈[0，1]，其中bp0和bp1表示需要进行插值的两帧位置信息；
其球面线性插值公式如下：
Figure GDA0003352462300000191
其中bq0和bq1表示需要进行插值的两帧姿态信息；
步骤F2：对各个声呐点云数据使用PCL开源库中的VoxelGrid滤波器进行降采样滤波器；
步骤F3：将雷达点云数据、声呐点云数据使用PCL开源库中的函数根据插值后的无人艇的位姿拼接，组成三维点云地图。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (7)

1.一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于，包括以下步骤：
步骤A：惯性测量单元获取无人艇的预测位姿数据；
步骤B：激光雷达获取雷达点云数据，将雷达点云数据投影至深度图，保留深度图上的地面点与分割点，其中所述分割点为同一个聚类的激光点；
步骤C：将深度图分割成六个子深度图，通过每个激光点的曲率得到特征点集，并将所有激光雷达的特征点集转化至无人艇的坐标；
步骤D：通过相邻两帧的雷达点云数据得到当前无人艇的相对位姿变换矩阵；
步骤E：获取激光雷达里程计因子、惯性测量单元因子、多普勒速度计因子、北斗卫星定位因子和闭环因子并通过因子图的形式优化当前无人艇位姿；
步骤F：通过各个激光雷达点云数据与各个声呐点云数据以及无人艇位姿构建出三维点云图。
2.根据权利要求1所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤A的步骤具体如下：
所惯性测量单元根据当前时刻的角速度
Figure FDA0003352462290000011
与角加速度
Figure FDA0003352462290000012
通过四阶龙格库塔法预积分得到无人艇的预测位姿，其中所述无人艇的预测位姿包括当前帧与上一帧的相对旋转
Figure FDA0003352462290000013
相对位移
Figure FDA0003352462290000014
速度Vt、角速度计偏置bg，t和角加速度计偏置ba，t；
其中相对旋转
Figure FDA0003352462290000015
的四阶龙格库塔预积分公式为：
Figure FDA0003352462290000016
其中
Figure FDA0003352462290000021
的取值范围如下
Figure FDA0003352462290000022
Figure FDA0003352462290000023
角速度
Figure FDA0003352462290000024
是一个三维的向量，
Figure FDA0003352462290000025
Figure FDA0003352462290000026
和
Figure FDA0003352462290000027
分别表示三个方向轴的数据；
Figure FDA0003352462290000028
所述速度Vt的四阶龙格库塔预积分公式为：
Figure FDA0003352462290000029
Figure FDA00033524622900000210
Figure FDA0003352462290000031
Figure FDA0003352462290000032
其中，Vt为当前t时刻的速度，Vt-1为t-1时刻的速度，C(q)函数是从四元数转换到旋转矩阵R的函数；
所述相对位移
Figure FDA0003352462290000033
的四阶龙格库塔预积分公式为：
Figure FDA0003352462290000034
其中
Figure FDA0003352462290000035
的取值范围如下
Figure FDA0003352462290000036
所述角速度计偏置bg，t的四阶龙格库塔预积分公式为：
bg，t＝bg，t-1，其中bg，t-1为t-1时刻的角速度计偏置；
所述角加速度计偏置ba，t的四阶龙格库塔预积分公式为：
ba，t＝ba，t-1，其中ba，t-1为t-1时刻的角加速度计偏置；
Figure FDA0003352462290000037
是一个四元数，qw表示实部，qv表示虚部，
Figure FDA0003352462290000038
的作用是把四元数归一化，wg表示当前位置的地球加速度、q0表示t-1时刻的姿态。
3.根据权利要求1所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤B的具体步骤如下：
步骤B1：激光雷达获取当前时刻t的雷达点云数据
Figure FDA0003352462290000041
其中P代表点云，并将雷达点云数据
Figure FDA0003352462290000042
对应投射于分辨率为1800*16深度图的像素上；
步骤B2：对深度图进行有序化、深度值划分、降维、直线拟合、地面点筛选处理，获取地面点并进行标记；
步骤B3：使用基于深度图的障碍物快速分割算法把深度图中的点云分割成一个一个的聚类；
步骤B4：剔除点数少于30的聚类，将同一个聚类的激光点标记为分割点，删除分割点与地面点以外的激光点，得到处理深度图。
4.根据权利要求3所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤C的具体步骤如下：
步骤C1：将处理深度图分割成六个分辨率为300*16的子深度图，获取子深度图的激光点
Figure FDA0003352462290000043
的曲率，其中激光点
Figure FDA0003352462290000044
的曲率获取公式如下：
Figure FDA0003352462290000045
rk和rj表示为子深度图中像素点的深度值，k、j为子深度图中的索引值，S是同一行连续十个激光点的索引的集合；
步骤C2：根据激光点
Figure FDA0003352462290000046
的曲率，得到边缘点与平面点；
选取每个子深度图中每一行中两个分割点组成边缘特征点集，其中组成边缘特征点集的分割点为子深度图每一行中拥有最大曲率的两个边缘点；
选取每个子深度图中每一行中最小曲率C的四个平面点组成平面特征点集；
步骤C3：所有激光雷达的平面特征点集与边缘特征点集通过外参
Figure FDA0003352462290000047
将激光雷达的坐标转换到无人艇机体坐标系，并组成当前时刻的特征点集bFt，所述特征点集bFt包括边缘特征点集与平面特征点集。
5.根据权利要求4所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤D的具体步骤如下：
步骤D1：将当前时刻相邻的二十五个关键帧中的特征点集bFt进行位姿变换得到局部特征点云图wMt，其中局部特征点云图wMt包括局部边缘特征点云图
Figure FDA0003352462290000051
和局部平面特征点云图
Figure FDA0003352462290000052
Figure FDA0003352462290000053
wFt e为局部边缘特征点云图的特征点集；
Figure FDA0003352462290000054
wFt p为局部平面特征点云图的特征点集；
步骤D2：使用四阶龙格库塔法预积分获取局部特征点云图的位姿变换矩阵
Figure FDA0003352462290000055
步骤D3：获取局部特征点云图wMt的所述特征点集bFt l，并将bFt l从无人艇机体坐标系转换到导航坐标系；
步骤D4：在局部边缘特征点云图
Figure FDA0003352462290000056
选择三个激光点wpa e、wpb e和wpc e并构建边缘特征残差函数，其边缘特征残差函数公式如下：
Figure FDA0003352462290000057
在局部平面特征点云图
Figure FDA0003352462290000058
选择四个激光点wpa p、wpb p、wpc p、wpd p并构建平面特征残差函数，其平面特征残差函数公式如下：
Figure FDA0003352462290000059
步骤D5：获取边缘特征残差函数和平面特征残差函数的累加和，对边缘特征残差函数和平面特征残差函数的累加和构建非线性最小二乘问题，获得当前的无人艇的相对位姿变换矩阵
Figure FDA00033524622900000510
其中非线性最小二乘问题的公式如下：
Figure FDA0003352462290000061
其中ρa( )表示的是第a个残差对应的自适应损失函数，其函数表示为：
Figure FDA0003352462290000062
其中其中，a∈R是一个随着残差的概率分布变化的参数，同时随着a值的不同损失函数能自适应地计算残差的权重，c是一个比例系数且c＞0。
6.根据权利要求5所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤E的具体步骤如下：
步骤E1：获取激光雷达里程计因子和惯性测量单元因子，其中惯性测量单元因子与所述无人艇的预测位姿一致，所述激光雷达里程计因子与所述相对位姿变换矩阵一致，使用激光雷达里程计因子与惯性测量单元因子约束因子图，其公式如下：
Figure FDA0003352462290000063
其中li(xj)一个正比于高斯条件概率密度函数的似然函数，i，j为自然数，X∈x0，...，xn，表示无人艇状态的集合，x0，...，xn表示无人艇0～n时刻的状态，p(X)无人艇状态服从高斯分布的联合概率密度函数；
步骤E2：通过北斗卫星定位系统获取经纬度坐标，然后把经纬度坐标通过外参转换到导航坐标系下，通过北斗卫星导航获取北斗卫星定位信息BDSP，对北斗卫星定位信息BDSP与相邻的两个关键帧进行线性插值，获取北斗卫星定位因子；
其公式如下：BDSp(t)＝(1-t)*BDSp0-t*BDSp1，t∈[0，1]，其中BDSp0和BDSp1表示需要进行插值的两帧数据；
步骤E3：通过北斗卫星定位系统的数据解算获得北斗卫星定位的协方差矩阵，判断北斗卫星定位的协方差矩阵是否小于相对位姿变换矩阵，若是，根据北斗卫星定位因子约束因子图，其公式如下：
Figure FDA0003352462290000071
其中，BDSpt是北斗卫星t时刻在导航坐标系下的定位信息；
Figure FDA0003352462290000072
分别为t时刻无人艇相对于导航坐标系的旋转和平移；BDS∑t为北斗卫星t时刻的协方差矩阵；
步骤E4：通过关键帧的时间戳t与其相邻关键帧的时间戳t-1对多普勒速度计进行预积分，得到多普勒速度计因子，其公式如下：
Figure FDA0003352462290000073
其中DVLV(t)dt表示多普勒速度计在t时刻的速度；根据多普勒速度计因子约束因子图，其公式为：
Figure FDA0003352462290000074
其中DVL∑t为t时刻多普勒速度计的协方差矩阵，
Figure FDA0003352462290000075
表示t时刻无人艇相对于导航坐标系的平移，
Figure FDA0003352462290000076
表示t-1时刻无人艇相对于导航坐标系的平移，
Figure FDA0003352462290000077
表示t-1时刻到t时刻无人艇的相对平移；
步骤E5：在因子图中使用Scan Contest算法找到最新关键帧xt的先验关键帧xa，先验关键帧xa的周围通过位姿变换矩阵建立导航坐标系下的局部关键帧特征点云图wMa，wMa＝{wFt-12∪......∪wFt+12}；
步骤E6：通过最新关键帧xt的特征点集wFt与局部关键帧特征点云图wMa，得到两者之间的相对位姿矩阵
Figure FDA0003352462290000078
并向因子图添加闭环因子，其公式为：
Figure FDA0003352462290000079
7.根据权利要求1所述的一种多个测距传感器的无人艇近岸实时定位与建图方法，其特征在于：所述步骤F的具体步骤如下：
步骤F1：对无人艇的位姿根据时间戳做线性插值和球面插值计算，去除激光雷达的运动畸变，其线性插值公式如下：bp(t)＝(1-t)*bp0*bp1，t∈[0，1]，其中bp0和bp1表示需要进行插值的两帧位置信息；
其球面线性插值公式如下：
Figure FDA0003352462290000081
其中bq0和bq1表示需要进行插值的两帧姿态信息；
步骤F2：对各个声呐点云数据使用PCL开源库中的VoxelGrid滤波器进行降采样滤波器；
步骤F3：将雷达点云数据、声呐点云数据使用PCL开源库中的函数根据插值后的无人艇的位姿拼接，组成三维点云地图。
CN202110668955.4A 2021-06-16 2021-06-16 一种多个测距传感器的无人艇近岸实时定位与建图方法 Active CN113340295B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110668955.4A CN113340295B (zh) 2021-06-16 2021-06-16 一种多个测距传感器的无人艇近岸实时定位与建图方法
US17/584,598 US11450016B1 (en) 2021-06-16 2022-01-26 Nearshore real-time positioning and mapping method for unmanned surface vehicle with multiple distance measuring sensors

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110668955.4A CN113340295B (zh) 2021-06-16 2021-06-16 一种多个测距传感器的无人艇近岸实时定位与建图方法

Publications (2)

Publication Number Publication Date
CN113340295A CN113340295A (zh) 2021-09-03
CN113340295B true CN113340295B (zh) 2021-12-21

Family

ID=77475748

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110668955.4A Active CN113340295B (zh) 2021-06-16 2021-06-16 一种多个测距传感器的无人艇近岸实时定位与建图方法

Country Status (2)

Country Link
US (1) US11450016B1 (zh)
CN (1) CN113340295B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113689471B (zh) * 2021-09-09 2023-08-18 中国联合网络通信集团有限公司 目标跟踪方法、装置、计算机设备及存储介质
CN114111767B (zh) * 2021-10-28 2023-11-03 北京自动化控制设备研究所 基于多信息融合对线路设计线型进行优化的方法
CN114518111A (zh) * 2022-03-11 2022-05-20 六安智梭无人车科技有限公司 一种激光雷达与惯性测量单元标定方法及系统
CN114777775A (zh) * 2022-05-06 2022-07-22 浙江师范大学 一种多传感器融合的定位方法及系统
CN115031718B (zh) * 2022-05-25 2023-10-31 合肥恒淏智能科技合伙企业(有限合伙) 一种多传感器融合的无人船同步定位与建图方法(slam)及系统
US20230394691A1 (en) * 2022-06-07 2023-12-07 Toyota Research Institute, Inc. Depth estimation with sparse range sensor depth and uncertainty projection
CN114863075B (zh) * 2022-07-05 2022-10-14 深圳市新天泽消防工程有限公司 基于多传感器的消防疏散路径规划方法、装置、设备
CN115326068B (zh) * 2022-10-17 2023-01-24 北京理工大学 激光雷达-惯性测量单元融合里程计设计方法及系统
CN115629374B (zh) * 2022-12-16 2023-03-10 陕西欧卡电子智能科技有限公司 基于毫米波雷达的无人船桥下定位方法及相关设备
CN115856931B (zh) * 2023-03-01 2023-06-02 陕西欧卡电子智能科技有限公司 基于激光雷达的无人船停泊库位重定位方法
CN116222544B (zh) * 2023-05-09 2023-08-04 浙江大学湖州研究院 一种面向饲养场的投料车自动导航定位方法及装置
CN116958439B (zh) * 2023-07-28 2024-02-23 南京安透可智能系统有限公司 一种满水环境中基于多传感融合的管道三维重建方法
CN116698046B (zh) * 2023-08-04 2023-12-01 苏州观瑞汽车技术有限公司 一种物业室内服务机器人建图定位和回环检测方法及系统

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3248029A4 (en) * 2015-01-19 2018-10-03 The Regents of the University of Michigan Visual localization within lidar maps
CN107943038A (zh) * 2017-11-28 2018-04-20 广东工业大学 一种移动机器人嵌入式激光slam方法及系统
CN108777553B (zh) * 2018-06-01 2019-08-23 广东工业大学 一种基于龙格库塔法的压电陶瓷驱动器控制方法
CN109961440B (zh) * 2019-03-11 2021-06-18 重庆邮电大学 一种基于深度图的三维激光雷达点云目标分割方法
CN110689622B (zh) * 2019-07-05 2021-08-27 电子科技大学 一种基于点云分割匹配闭环校正的同步定位与构图方法
CN110706279B (zh) * 2019-09-27 2021-09-07 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111221018B (zh) * 2020-03-12 2022-04-08 南京航空航天大学 一种用于抑制海上多路径的gnss多源信息融合导航方法
CN111929699B (zh) * 2020-07-21 2023-05-09 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN112362072B (zh) * 2020-11-17 2023-11-14 西安恒图智源信息科技有限责任公司 一种复杂城区环境中的高精度点云地图创建系统及方法
CN112562052B (zh) * 2020-12-03 2021-07-27 广东工业大学 一种面向近岸水域的实时定位与建图方法
CN216070513U (zh) * 2021-08-23 2022-03-18 海南中农航服科技有限公司 一种用于测绘的航拍无人机

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于三维激光雷达与RTK融合的SLAM研究;倪志康 等;《制造业自动化》;20201231(第7期);第51-54页 *

Also Published As

Publication number Publication date
US11450016B1 (en) 2022-09-20
CN113340295A (zh) 2021-09-03

Similar Documents

Publication Publication Date Title
CN113340295B (zh) 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN111486845B (zh) 基于海底地形匹配的auv多策略导航方法
CN104848867B (zh) 基于视觉筛选的无人驾驶汽车组合导航方法
CN109239709B (zh) 一种无人船的局部环境地图自主构建方法
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN112882059B (zh) 一种基于激光雷达的无人船内河障碍物感知方法
CN114216454B (zh) 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法
Chambers et al. Perception for a river mapping robot
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN112562052B (zh) 一种面向近岸水域的实时定位与建图方法
US20220172464A1 (en) Water non-water segmentation systems and methods
US20220392211A1 (en) Water non-water segmentation systems and methods
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN112346463A (zh) 一种基于速度采样的无人车路径规划方法
Han et al. GPS-less coastal navigation using marine radar for USV operation
JP2023525927A (ja) 車両位置推定システム及び方法
CN111090283A (zh) 一种无人艇组合定位定向方法和系统
CN114077249B (zh) 一种作业方法、作业设备、装置、存储介质
CN112802195B (zh) 一种基于声呐的水下机器人连续占据建图方法
Jung et al. Terrain based navigation for an autonomous surface vehicle with a multibeam sonar
CN115031718B (zh) 一种多传感器融合的无人船同步定位与建图方法(slam)及系统
CN115930948A (zh) 一种果园机器人融合定位方法
CN115031726A (zh) 一种数据融合导航定位方法
CN115290090A (zh) 基于多传感器信息融合的slam地图构建方法
WO2020244467A1 (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

Effective date of registration: 20220921

Address after: 510000 No. 729 Dongfeng East Road, Guangdong, Guangzhou

Patentee after: GUANGDONG University OF TECHNOLOGY

Patentee after: Guangdong haiqinyuan Information Technology Co.,Ltd.

Address before: 510062 Dongfeng East Road, Yuexiu District, Guangzhou, Guangdong 729

Patentee before: GUANGDONG University OF TECHNOLOGY

TR01 Transfer of patent right