CN112562052A - 一种面向近岸水域的实时定位与建图方法 - Google Patents

一种面向近岸水域的实时定位与建图方法 Download PDF

Info

Publication number
CN112562052A
CN112562052A CN202011395614.6A CN202011395614A CN112562052A CN 112562052 A CN112562052 A CN 112562052A CN 202011395614 A CN202011395614 A CN 202011395614A CN 112562052 A CN112562052 A CN 112562052A
Authority
CN
China
Prior art keywords
coordinate system
data
frame
unmanned ship
view
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
CN202011395614.6A
Other languages
English (en)
Other versions
CN112562052B (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 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 CN202011395614.6A priority Critical patent/CN112562052B/zh
Publication of CN112562052A publication Critical patent/CN112562052A/zh
Application granted granted Critical
Publication of CN112562052B publication Critical patent/CN112562052B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T15/00—3D [Three Dimensional] image rendering
    • G06T15/005—General purpose rendering architectures
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • 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
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G3/00—Traffic control systems for marine craft
    • G08G3/02—Anti-collision systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • Business, Economics & Management (AREA)
  • Human Resources & Organizations (AREA)
  • Strategic Management (AREA)
  • Economics (AREA)
  • Computer Graphics (AREA)
  • Biomedical Technology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • General Health & Medical Sciences (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Evolutionary Computation (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computational Linguistics (AREA)
  • Biophysics (AREA)
  • Development Economics (AREA)
  • Molecular Biology (AREA)
  • Game Theory and Decision Science (AREA)
  • Artificial Intelligence (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Marketing (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Ocean & Marine Engineering (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种面向近岸水域的实时定位与建图方法，包括多个鱼眼摄像头采集数据后通过解算得到特征点归一化坐标；惯性测量单元获取无人艇的惯性数据；声呐采集无人艇周围声呐点云数据；北斗卫星导航***定位无人艇当前坐标并解算出全局平面坐标系位置；磁力计测量当前磁场强度的测量值并解算无人艇姿态；气压计测量气压从而计算出无人艇高度；惯性数据和特征点归一化坐标进行线性插值后获得多视图帧位姿；多视图帧位姿结合全局平面坐标系位置、无人艇姿态、无人艇高度和声呐点云数据通过回环检测和建图生成栅格地图和三位点云地图。所述面向近岸水域的实时定位与建图方法，解决了现有的数据采集和处理方法应用于近岸水域无法精确定位的问题。

Description

一种面向近岸水域的实时定位与建图方法
技术领域
本发明涉及近岸水域数据采集和处理技术领域，特别是一种面向近岸水域的实时定位与建图方法。
背景技术
目前市面上的实时定位与建图***大多数针对于室内服务型机器人、无人驾驶汽车和无人机所处的陆地和低空中环境。但在水上近岸环境中，由于近岸环境只有单边参考物，使用现有的基于单目或双目摄像头的视觉惯性里程计，狭窄的视野并不能检测出足够的特征点来实现精确的实时定位，从而出现定位不准的情况。此外，目前使用的构建栅格地图和三维点云地图的实时定位与建图***，主要使用的传感器是激光雷达，但由于激光雷达有效的工作环境是在水面上，对水面下的环境一无所知，所建出的栅格地图对无人艇的路径规划与自主避障提供不了太大的帮助。
发明内容
针对上述缺陷，本发明的目的在于提出一种面向近岸水域的实时定位与建图方法，解决了现有的数据采集和处理方法应用于近岸水域无法精确定位的问题。
为达此目的，本发明采用以下技术方案：一种面向近岸水域的实时定位与建图方法，包括以下步骤：
步骤A包括：
步骤A1：多个鱼眼摄像头采集数据后通过解算得到特征点归一化坐标；
步骤A2：惯性测量单元获取无人艇的惯性数据PVQB；
步骤A3：声呐采集无人艇周围声呐点云数据；
步骤A4：北斗卫星导航***定位无人艇当前坐标并解算出全局平面坐标系位置；
步骤A5：磁力计测量当前磁场强度的测量值并解算出无人艇姿态；
步骤A6：气压计测量气压从而计算出无人艇高度；
步骤B：
所述惯性数据PVQB进行线性插值后结合所述特征点归一化坐标获得多视图帧位姿；
步骤C：所述多视图帧位姿结合所述全局平面坐标系位置、无人艇姿态、无人艇高度和声呐点云数据通过回环检测和建图生成栅格地图和三位点云地图。
本发明的有益效果：所述面向近岸水域的实时定位与建图方法通过所述鱼眼摄像头、北斗卫星导航***、磁力计、气压计、惯性测量单元和声呐，能在水下实现精确的实时定位后，再通过所述回环检测建图步骤对数据进行处理从而构建适用于近岸水域的出栅格地图和三维点云地图，能够实现无人艇位姿估计、实时重定位和闭环检测，达到了无人艇的路径规划和自助避障的目的。
多个所述鱼眼摄像头设置于在无人艇的顶部，单个鱼眼摄像头能够提供180°的视野，视野重叠的部分有助于获取特征点准确的深度信息；所述北斗卫星导航***设置于无人艇的两侧；所述声呐设置于无人艇的底部；所述惯性测量单元、气压计和磁力计安装于所述无人艇并设置于所述鱼眼摄像头的下方。
所述面向近岸水域的实时定位与建图方法能够实现鱼眼摄像头、北斗卫星导航***、磁力计、气压计、惯性测量单元和声呐组成的多传感器的时间戳自适应同步，解决了各传感器因延时、数据传输或独立时钟所产生不同的时间戳从而影响***状态估计的问题。
此外，多视图帧的位姿与北斗卫星定位数据、磁力计数据、气压计数据一同构建最大似然估计问题，能够在近岸环境中实现精确的全局定位。所述最大似然估计问题是指已知各传感器的测量值，求在什么位姿的情况下能得到这些测量值的概率最大的算法。
所述声呐为ping360声呐，使用两个ping360声呐构建栅格地图和三维点云地图，能够检测出水面下对无人艇有威胁的障碍物，同时对水下环境进行实时地图构建，协助无人艇实现自主导航。
附图说明
图1是本发明的一个实施例中的流程图；
图2是本发明的一个实施例中步骤A的流程图；
图3是本发明的一个实施例中步骤B的流程图；
图4是本发明的一个实施例中步骤C的流程图。
具体实施方式
下面详细描述本发明的实施方式，实施方式的示例在附图中示出，其中，相同或类似的标号自始至终表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
下文的公开提供了许多不同的实施方式或例子用来实现本发明的实施方式的不同结构。为了简化本发明的实施方式的公开，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在于限制本发明。此外，本发明的实施方式可以在不同例子中重复参考数字和/或参考字母，这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，本发明的实施方式提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以意识到其他工艺的应用和/或其他材料的使用。
如图1-4所示，一种面向近岸水域的实时定位与建图方法，包括以下步骤：
步骤A包括：
步骤A1：多个鱼眼摄像头采集数据后通过解算得到特征点归一化坐标；
步骤A2：惯性测量单元获取无人艇的惯性数据PVQB；
步骤A3：声呐采集无人艇周围声呐点云数据；
步骤A4：北斗卫星导航***定位无人艇当前坐标并解算出全局平面坐标系位置；
步骤A5：磁力计测量当前磁场强度的测量值并解算出无人艇姿态；
步骤A6：气压计测量气压从而计算出无人艇高度；
步骤B：
所述惯性数据PVQB进行线性插值后结合所述特征点归一化坐标获得多视图帧位姿；
步骤C：所述多视图帧位姿结合所述全局平面坐标系位置、无人艇姿态、无人艇高度和声呐点云数据通过回环检测和建图生成栅格地图和三位点云地图。
所述面向近岸水域的实时定位与建图方法通过所述鱼眼摄像头、北斗卫星导航***、磁力计、气压计、惯性测量单元和声呐，能在水下实现精确的实时定位后，再通过所述回环检测建图步骤对数据进行处理从而构建适用于近岸水域的出栅格地图和三维点云地图，能够实现无人艇位姿估计、实时重定位和闭环检测，达到了无人艇的路径规划和自助避障的目的。
多个所述鱼眼摄像头设置于在无人艇的顶部，单个鱼眼摄像头能够提供180°的视野，视野重叠的部分有助于获取特征点准确的深度信息；所述北斗卫星导航***设置于无人艇的两侧；所述声呐设置于无人艇的底部；所述惯性测量单元、气压计和磁力计安装于所述无人艇并设置于所述鱼眼摄像头的下方。
所述面向近岸水域的实时定位与建图方法能够实现鱼眼摄像头、北斗卫星导航***、磁力计、气压计、惯性测量单元和声呐组成的多传感器的时间戳自适应同步，解决了各传感器因延时、数据传输或独立时钟所产生不同的时间戳从而影响***状态估计的问题。
此外，多视图帧的位姿与北斗卫星定位数据、磁力计数据、气压计数据一同构建最大似然估计问题，能够在近岸环境中实现精确的全局定位。所述最大似然估计问题是指已知各传感器的测量值，求在什么位姿的情况下能得到这些测量值的概率最大的算法。
所述声呐为ping360声呐，使用两个ping360声呐构建栅格地图和三维点云地图，能够检测出水面下对无人艇有威胁的障碍物，同时对水下环境进行实时地图构建，协助无人艇实现自主导航。
所述惯性数据PVQB根据上一帧图片和当前帧图片的时间间隔进行线性插值后结合所述特征点归一化坐标获得多视图帧位姿。
一些实施例中，如图2所示，所述步骤A1具体为：首先，多个所述鱼眼摄像头的数据通过自适应时间戳同步算法的预处理，组成一个多视图帧；
接着，对多视图帧进行SuperPoint特征点的提取与跟踪获取当前多视图帧各特征点的像素坐标；
然后，经过相机投影模型和畸变模型的处理得到特征点的归一化平面坐标
Figure BDA0002814958150000051
其中，Pf表示特征点在第i个相机坐标系下的三维坐标，Zf表示特征点在第i个相机坐标系下的三维坐标的z值，un和vn表示特征点在第i个相机像素坐标系下的坐标。
自适应时间戳同步为现有的方法，具体步骤为：首先，为每个消息通道设立一个标志位，有消息到来就使用回调函数把标志位置一，并把消息存放到消息缓存队列里；若每个消息通道的标志位都为一，则把最新的那个消息定为关键点，然后在每个消息缓存队列中寻找在这个关键点附近且时间差不超过阈值的消息。
SuperPoint是一种基于自监督卷积神经网络获得特征点的算法。相机投影模型是指图像在CCD图像平面上的光学投影方程。畸变模型是描述光线投影到相机镜头上的透镜所产生畸变的物理方程。
例如，所述步骤A2具体为：所述惯性测量单元通过四阶Runge-kutta法预积分出当前时刻与上一时刻多视图帧之间的惯性测量单元的惯性数据PVQB，其中所述惯性数据PVQB包括转移矩阵
Figure BDA0002814958150000061
速度vk+1、旋转矩阵
Figure BDA0002814958150000062
重力偏置
Figure BDA0002814958150000063
和加速度偏置
Figure BDA0002814958150000064
所述转移矩阵
Figure BDA0002814958150000065
的四阶Runge-kutta法为：
Figure BDA0002814958150000071
Figure BDA0002814958150000072
所述旋转矩阵
Figure BDA0002814958150000073
的四阶Runge-kutta法为：
Figure BDA0002814958150000074
Figure BDA0002814958150000075
Figure BDA0002814958150000076
其中，
Figure BDA0002814958150000077
表示惯性测量单元中陀螺仪的测量值；
所述速度vk+1的四阶Runge-kutta法为：
Figure BDA0002814958150000081
Figure BDA0002814958150000082
其中，
Figure BDA0002814958150000083
q＝[qw qv]是从四元数q转换到旋转矩阵R的函数，
Figure BDA0002814958150000084
的作用是把四元数归一化，
Figure BDA00028149581500000810
表示惯性测量单元中加速度计的测量值，gw表示当前位置的地球加速度；
所述重力偏置
Figure BDA0002814958150000086
的算法为：
Figure BDA0002814958150000087
所述加速度偏置
Figure BDA0002814958150000088
的算法为：
Figure BDA0002814958150000089
惯性测量单元是测量物体三轴姿态角或角速率以及加速度的装置，主要用于导航。其包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标***独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
值得说明的是，所述步骤A3具体为：
所述声呐通过数据解算
Figure BDA0002814958150000091
再经过声呐运动畸变模型的处理，把声波数据转化为声呐点云数据并去除测量噪声；
其中，Swater为水中声音的传播速度，T为测量时间；
所述声呐运动畸变模型的处理为：
Figure BDA0002814958150000092
Figure BDA0002814958150000093
其中，是ts声呐数据的时间戳，ts落在时间t0和t1之间，Gpb,t表示第t时刻无人艇在世界坐标系G下的位置，r是声呐的距离信息，α是方位角，ε是俯仰角，Gps,t表示t时刻声呐点云在世界坐标系G下的位置。
所述声呐包括水平安装的水平声呐和垂直安装的垂直声呐，所述水平声呐用于构建栅格地图，垂直声呐用于构建三维点云地图。
可选地，所述步骤A4具体为：所述北斗卫星导航***经过数据解算得到经纬度坐标系下的坐标，然后把经纬度坐标系下的坐标转换成以第一帧北斗卫星信号为原点的全局平面坐标系下的无人艇的当前坐标：
Figure BDA0002814958150000094
上述的BDS指北斗卫星导航***，北斗卫星导航***由空间段、地面段和用户段三部分组成，可在全球范围内全天候、全天时为各类用户提供高精度、高可靠定位、导航和授时服务，定位精度为分米、厘米级别。
具体地，所述步骤A5中，所述磁力计通过数据解算得到无人艇在全局坐标系下的偏航角，从而得到所述无人艇姿态，解算公式为：
Figure BDA0002814958150000101
Figure BDA0002814958150000102
其中，bMXYZ是磁力计的测量值，
Figure BDA0002814958150000103
表示无人艇坐标系到全局坐标水平面的旋转矩阵，MY′和MX′是MXYZ′的两个分量，wyaw′是无人艇在全局坐标系下的偏航角；
所述步骤A6具体为：所述气压计根据气压进行数据解算：
Figure BDA0002814958150000104
得到无人艇的高度；
其中，上述气压解算公式中，P为气压计测量气压，Pb为参考大气压，Tb为参考温度，Lb为温度下降率，hb为参考高度，R为气体常数，为g0重力加速度，M为地球空气摩尔质量，Bzt为无人艇的高度。
磁力计通过测量和计算得到的无人艇在全局坐标系下的偏航角为无人艇姿态，气压计通过测量和计算得到无人艇的高度。
优选的，如图3所示，所述步骤B中，所述惯性数据PVQB的转移矩阵
Figure BDA0002814958150000111
进行线性插值的公式为：
P(t)＝(1-t)P0+tP1 t∈[0,1]；
所述惯性数据PVQB的速度vk+1进行线性插值的公式为：
V(t)＝(1-t)V0+tV1 t∈[0,1]；
所述惯性数据PVQB的旋转矩阵
Figure BDA0002814958150000112
进行线性插值的公式为：
Figure BDA0002814958150000113
其中，Δθ表示的是欧几里德空间中两个四元数向量之间的夹角。
进行线性插值能使惯性测量单元的惯性数据得到充分地应用。所述惯性数据PVQB的转移矩阵
Figure BDA0002814958150000114
为惯性测量单元在世界坐标系下的位置；所述惯性数据PVQB的速度vk+1为惯性测量单元在世界坐标系下的速度；所述惯性数据PVQB的旋转矩阵
Figure BDA0002814958150000115
为惯性测量单元在世界坐标系下的姿态；所述惯性数据PVQB的加速度偏置
Figure BDA0002814958150000116
和重力偏置
Figure BDA0002814958150000117
为惯性测量单元的角加速度和角速度的偏置，加速度偏置
Figure BDA0002814958150000118
和重力偏置
Figure BDA0002814958150000119
不需要做线性插值。
一些实施例中，所述步骤B中，检测***是否有初始化；
当未初始化时，检测惯性测量单元的激励是否足够；当所述激励满足阈值时，对滑窗内所有的多视图帧做Structure from Motion(SfM)得到路标点；根据所述路标点使用PnP算法求解出滑窗内其余帧的位姿，并使用三角测量算法恢复出滑窗内各帧所看到的路标点的三维坐标；通过对滑窗内的多视图帧与惯性测量单元进行对齐，从而校正惯性测量单元的bias，以及各多视图帧的速度、重力和尺度因子；
当已初始化时，使用三角测量算法恢复出当前多视图帧所看到的路标点的三维坐标；计算惯性测量单元误差相对于优化变量的Jacobian矩阵以及多视图帧的重投影误差的Jacobian矩阵；对滑窗内所有的多视图帧进行Bundle Adjustment算法从而得到优化后的多视图帧位姿、陀螺仪的bias、加速度计的bias、各鱼眼摄像头与惯性测量单元之间的外参以及逆深度；计算下一次滑窗优化的先验约束。
所述***为面向近岸水域的实时定位与建图***。滑窗优化中使用了多个鱼眼摄像头的视觉约束和惯性测量单元约束对多视图帧的位姿进行估计，并与先前的多视图关键帧进行闭环检测，能够解决无人艇定位失效问题以及消除多视图帧位姿估计的累计误差，从而在近岸环境中实现精确的局部定位。
计算惯性测量单元误差相对于优化变量的Jacobian矩阵以及多视图帧的重投影误差的Jacobian矩阵为：把先验约束、视觉约束和惯性测量单元约束组成Hessian矩阵，并使用DogLeg算法求解非线性最小二乘问题，从而得到滑窗内所有的帧优化后的位姿，还能得到惯性测量单元的bias、加速度计的bias、各摄像头与惯性测量单元之间的外参以及逆深度。其中Hessian矩阵是由一个多元函数的二阶偏导数构成的方阵，描述了函数的局部曲率；DogLeg算法是一种基于信赖域的非线性优化算法。
Bundle Adjustment算法为：计算下一次滑窗优化的先验约束，即计算滑窗内倒数第二帧和倒数第三帧的视差，若视差少于阈值，则边缘化倒数第二帧的数据；若视差大于阈值，则边缘化最老帧的数据。
Structure from Motion(SfM)为：寻找滑窗内两帧视差较大的多视图帧，并使用对极几何算法恢复出第一批路标点。
PnP算法是一种已知n个特征点的三维空间坐标及其二维投影位置，从而估计位姿的算法。
所述三角测量法具体如下，假设特征点f同时被鱼眼摄像头i和鱼眼摄像头j观测到，则有：
Figure BDA0002814958150000131
其中
Figure BDA0002814958150000132
是特征点f在鱼眼摄像头i和鱼眼摄像头j中归一化平面的表示形式，旋转矩阵
Figure BDA0002814958150000133
和转移矩阵
Figure BDA0002814958150000134
是鱼眼摄像头i与鱼眼摄像头j之间的外参，可通过鱼眼摄像头标定获得。最后，等式两边同时左乘[jpf]×，可得：
Figure BDA0002814958150000135
通过求解上述等式就可以获得iz和jz，也就是可以获得特征点f的深度信息。
例如，如图4所示，所述步骤C具体为：先检测当前多视图帧位姿是否为关键帧；
当所述多视图帧位姿为关键帧时，所述多视图帧位姿与多视图关键帧数据库进行相似度计算，相似度计算的得分大于阈值时，所述多视图帧位姿与闭环帧进行特征点匹配，并使用PnP算法计算所述多视图帧位姿与闭环帧之间的相对位姿；固定多视图关键帧数据库中最老的帧、添加序列边和闭环边，并进行四自由度的位姿图优化形成多视图关键帧，把所述多视图关键帧添加到多视图关键帧数据库；
当所述多视图帧位姿不是关键帧时，所述多视图帧位姿形成多视图非关键帧；
然后，通过对所述多视图关键帧或多视图非关键帧结合所述全局平面坐标系位置、无人艇姿态和无人艇高度的约束进行非线性最大似然估计得到无人艇的位姿，所述位姿与所述声呐点云数据进行自适应时间戳同步算法和位姿关联，然后使用覆盖栅格地图算法构建出栅格地图以及使用降采样滤波器构建三维点云地图；
所述覆盖栅格地图算法为：
Figure BDA0002814958150000141
其中mi表示第i个栅格的状态，z1:t表示1到t时刻的声呐点云数据，x1:t表示1到t时刻的无人艇位姿。
四自由度表示位置x、y、z和偏航角yaw这四个自由度。
值得说明的是，所述步骤C中，北斗卫星导航***的全局平面坐标系位置的约束为：
Figure BDA0002814958150000151
其中，BDSZt表示北斗卫星导航***t时刻位姿的观测值，BDSht(X)表示北斗卫星导航***t时刻的观测方程，wpt表示无人艇t时刻在全局坐标系下的位置；
磁力计的无人艇姿态的约束为：
Figure BDA0002814958150000152
其中，mht(X)表示磁力计t时刻的观测方程，wyaw′t为磁力计在t时刻经数据解算后的无人艇在世界坐标系下的偏航角，wyaw′t为无人艇t时刻在世界坐标系下的偏航角的估计值；
气压计的无人艇高度的约束为：
Figure BDA0002814958150000153
其中，BZt表示气压计t时刻在全局坐标系下的观测值，Bht(X)表示气压计t时刻的观测方程,wZt表示无人艇t时刻在全局坐标系z轴的数值。
上述方法能获得更高精度的全局定位。
在本说明书的描述中，参考术语“一个实施方式”、“一些实施方式”、“示意性实施方式”、“示例”、“具体示例”或“一些示例”等的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施方式，可以理解的是，上述实施方式是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施实施进行变化、修改、替换和变型。

Claims (10)

1.一种面向近岸水域的实时定位与建图方法，其特征在于：包括以下步骤：
步骤A包括：
步骤A1：多个鱼眼摄像头采集数据后通过解算得到特征点归一化坐标；
步骤A2：惯性测量单元获取无人艇的惯性数据PVQB；
步骤A3：声呐采集无人艇周围声呐点云数据；
步骤A4：北斗卫星导航***定位无人艇当前坐标并解算出全局平面坐标系位置；
步骤A5：磁力计测量当前磁场强度的测量值并解算出无人艇姿态；
步骤A6：气压计测量气压从而计算出无人艇高度；
步骤B：
所述惯性数据PVQB进行线性插值后结合所述特征点归一化坐标获得多视图帧位姿；
步骤C：所述多视图帧位姿结合所述全局平面坐标系位置、无人艇姿态、无人艇高度和声呐点云数据通过回环检测和建图生成栅格地图和三位点云地图。
2.根据权利要求1所述的一种面向近岸水域的实时定位与建图方法，其特征在于，所述步骤A1具体为：首先，多个所述鱼眼摄像头的数据通过自适应时间戳同步算法的预处理，组成一个多视图帧；
接着，对多视图帧进行SuperPoint特征点的提取与跟踪获取当前多视图帧各特征点的像素坐标；
然后，经过相机投影模型和畸变模型的处理得到特征点的归一化平面坐标
Figure FDA0002814958140000021
其中，Pf表示特征点在第i个相机坐标系下的三维坐标，Zf表示特征点在第i个相机坐标系下的三维坐标的z值，un和vn表示特征点在第i个相机像素坐标系下的坐标。
3.根据权利要求2所述的一种面向近岸水域的实时定位与建图方法，其特征在于，所述步骤A2具体为：
所述惯性测量单元通过四阶Runge-kutta法预积分出当前时刻与上一时刻多视图帧之间的惯性测量单元的惯性数据PVQB，其中所述惯性数据PVQB包括转移矩阵
Figure FDA0002814958140000022
速度vk+1、旋转矩阵
Figure FDA0002814958140000023
重力偏置
Figure FDA0002814958140000024
和加速度偏置
Figure FDA0002814958140000025
所述转移矩阵
Figure FDA0002814958140000026
的四阶Runge-kutta法为：
Figure FDA0002814958140000027
Figure FDA0002814958140000028
所述旋转矩阵
Figure FDA0002814958140000031
的四阶Runge-kutta法为：
Figure FDA0002814958140000032
Figure FDA0002814958140000033
Figure FDA0002814958140000034
其中，
Figure FDA0002814958140000035
表示惯性测量单元中陀螺仪的测量值；
所述速度vk+1的四阶Runge-kutta法为：
Figure FDA0002814958140000036
Figure FDA0002814958140000037
其中，
Figure FDA0002814958140000038
q＝[qw qv]是从四元数q转换到旋转矩阵R的函数，
Figure FDA0002814958140000041
的作用是把四元数归一化，
Figure FDA00028149581400000410
表示惯性测量单元中加速度计的测量值，gw表示当前位置的地球加速度；
所述重力偏置
Figure FDA0002814958140000043
的算法为：
Figure FDA0002814958140000044
所述加速度偏置
Figure FDA0002814958140000045
的算法为：
Figure FDA0002814958140000046
4.根据权利要求3所述的一种面向近岸水域的实时定位与建图方法，其特征在于，所述步骤A3具体为：
所述声呐通过数据解算
Figure FDA0002814958140000047
再经过声呐运动畸变模型的处理，把声波数据转化为声呐点云数据并去除测量噪声，其中，Swater为水中声音的传播速度，T为测量时间；
所述声呐运动畸变模型的处理为：
Figure FDA0002814958140000048
Figure FDA0002814958140000049
其中，是ts声呐数据的时间戳，ts落在时间t0和t1之间，
Figure FDA0002814958140000051
表示第t时刻无人艇在世界坐标系G下的位置，r是声呐的距离信息，α是方位角，ε是俯仰角，Gps,t表示t时刻声呐点云在世界坐标系G下的位置。
5.根据权利要求4所述的一种面向近岸水域的实时定位与建图方法，其特征在于，所述步骤A4具体为：所述北斗卫星导航***经过数据解算得到经纬度坐标系下的坐标，然后把经纬度坐标系下的坐标转换成以第一帧北斗卫星信号为原点的全局平面坐标系下的无人艇的当前坐标：
BDSpt＝[BDSxt BDSyt BDSzt]T。
6.根据权利要求5所述的一种面向近岸水域的实时定位与建图方法，其特征在于：所述步骤A5中，所述磁力计通过数据解算得到无人艇在全局坐标系下的偏航角，从而得到所述无人艇姿态，解算公式为：
Figure FDA0002814958140000052
Figure FDA0002814958140000053
其中，bMXYZ是磁力计的测量值，
Figure FDA0002814958140000054
表示无人艇坐标系到全局坐标水平面的旋转矩阵，MY′和MX′是MXYZ′的两个分量，wyaw′是无人艇在全局坐标系下的偏航角；
所述步骤A6具体为：所述气压计根据气压进行数据解算：
Figure FDA0002814958140000061
得到无人艇的高度；
其中，上述气压解算公式中，P为气压计测量气压，Pb为参考大气压，Tb为参考温度，Lb为温度下降率，hb为参考高度，R为气体常数，为g0重力加速度，M为地球空气摩尔质量，Bzt为无人艇的高度。
7.根据权利要求6所述的一种面向近岸水域的实时定位与建图方法，其特征在于：所述步骤B中，所述惯性数据PVQB的转移矩阵
Figure FDA0002814958140000062
进行线性插值的公式为：
P(t)＝(1-t)P0+tP1 t∈[0,1]；
所述惯性数据PVQB的速度vk+1进行线性插值的公式为：
V(t)＝(1-t)V0+tV1 t∈[0,1]；
所述惯性数据PVQB的旋转矩阵
Figure FDA0002814958140000063
进行线性插值的公式为：
Figure FDA0002814958140000064
其中，Δθ表示的是欧几里德空间中两个四元数向量之间的夹角。
8.根据权利要求7所述的一种面向近岸水域的实时定位与建图方法，其特征在于：所述步骤B中，检测***是否有初始化；
当未初始化时，检测惯性测量单元的激励是否足够；当所述激励满足阈值时，对滑窗内所有的多视图帧做Structure from Motion(SfM)得到路标点；根据所述路标点使用PnP算法求解出滑窗内其余帧的位姿，并使用三角测量算法恢复出滑窗内各帧所看到的路标点的三维坐标；通过对滑窗内的多视图帧与惯性测量单元进行对齐，从而校正惯性测量单元的bias，以及各多视图帧的速度、重力和尺度因子；
当已初始化时，使用三角测量算法恢复出当前多视图帧所看到的路标点的三维坐标；计算惯性测量单元误差相对于优化变量的Jacobian矩阵以及多视图帧的重投影误差的Jacobian矩阵；对滑窗内所有的多视图帧进行Bundle Adjustment算法从而得到优化后的多视图帧位姿、陀螺仪的bias、加速度计的bias、各鱼眼摄像头与惯性测量单元之间的外参以及逆深度；计算下一次滑窗优化的先验约束。
9.根据权利要求8所述的一种面向近岸水域的实时定位与建图方法，其特征在于，所述步骤C具体为：先检测当前多视图帧位姿是否为关键帧；
当所述多视图帧位姿为关键帧时，所述多视图帧位姿与多视图关键帧数据库进行相似度计算，相似度计算的得分大于阈值时，所述多视图帧位姿与闭环帧进行特征点匹配，并使用PnP算法计算所述多视图帧位姿与闭环帧之间的相对位姿；固定多视图关键帧数据库中最老的帧、添加序列边和闭环边，并进行四自由度的位姿图优化形成多视图关键帧，把所述多视图关键帧添加到多视图关键帧数据库；
当所述多视图帧位姿不是关键帧时，所述多视图帧位姿形成多视图非关键帧；
然后，通过对所述多视图关键帧或多视图非关键帧结合所述全局平面坐标系位置、无人艇姿态和无人艇高度的约束进行非线性最大似然估计得到无人艇的位姿，所述位姿与所述声呐点云数据进行自适应时间戳同步算法和位姿关联，然后使用覆盖栅格地图算法构建出栅格地图以及使用降采样滤波器构建三维点云地图；
所述覆盖栅格地图算法为：
Figure FDA0002814958140000081
其中mi表示第i个栅格的状态，z1:t表示1到t时刻的声呐点云数据，x1:t表示1到t时刻的无人艇位姿。
10.根据权利要求9所述的一种面向近岸水域的实时定位与建图方法，其特征在于：所述步骤C中，北斗卫星导航***的全局平面坐标系位置的约束为：
Figure FDA0002814958140000082
其中，BDSZt表示北斗卫星导航***t时刻位姿的观测值，BDSht(X)表示北斗卫星导航***t时刻的观测方程，wpt表示无人艇t时刻在全局坐标系下的位置；
磁力计的无人艇姿态的约束为：
Figure FDA0002814958140000083
其中，mht(X)表示磁力计t时刻的观测方程，wyaw′t为磁力计在t时刻经数据解算后的无人艇在世界坐标系下的偏航角，wyaw′t为无人艇t时刻在世界坐标系下的偏航角的估计值；
气压计的无人艇高度的约束为：
Figure FDA0002814958140000091
其中，BZt表示气压计t时刻在全局坐标系下的观测值，Bht(X)表示气压计t时刻的观测方程,wZt表示无人艇t时刻在全局坐标系z轴的数值。
CN202011395614.6A 2020-12-03 2020-12-03 一种面向近岸水域的实时定位与建图方法 Active CN112562052B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011395614.6A CN112562052B (zh) 2020-12-03 2020-12-03 一种面向近岸水域的实时定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011395614.6A CN112562052B (zh) 2020-12-03 2020-12-03 一种面向近岸水域的实时定位与建图方法

Publications (2)

Publication Number Publication Date
CN112562052A true CN112562052A (zh) 2021-03-26
CN112562052B CN112562052B (zh) 2021-07-27

Family

ID=75047617

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011395614.6A Active CN112562052B (zh) 2020-12-03 2020-12-03 一种面向近岸水域的实时定位与建图方法

Country Status (1)

Country Link
CN (1) CN112562052B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113256697A (zh) * 2021-04-27 2021-08-13 武汉理工大学 水下场景的三维重建方法、***、装置和存储介质
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113744337A (zh) * 2021-09-07 2021-12-03 江苏科技大学 一种融合视觉、imu与声纳的同步定位与建图方法
CN114485613A (zh) * 2021-12-31 2022-05-13 海南浙江大学研究院 一种多信息融合水下机器人定位方法
CN115560757A (zh) * 2022-09-01 2023-01-03 中国人民解放军战略支援部队信息工程大学 随机姿态误差条件下基于神经网络的无人机直接定位校正方法

Citations (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170315247A1 (en) * 2016-04-27 2017-11-02 Sean Robert Griffin Ship-towed hydrophone volumetric array system method
CN108364319A (zh) * 2018-02-12 2018-08-03 腾讯科技（深圳）有限公司 尺度确定方法、装置、存储介质及设备
CN108648240A (zh) * 2018-05-11 2018-10-12 东南大学 基于点云特征地图配准的无重叠视场相机姿态标定方法
CN109816769A (zh) * 2017-11-21 2019-05-28 深圳市优必选科技有限公司 基于深度相机的场景地图生成方法、装置及设备
CN110223389A (zh) * 2019-06-11 2019-09-10 中国科学院自动化研究所 融合图像与激光数据的场景建模方法、***、装置
CN110309883A (zh) * 2019-07-01 2019-10-08 哈尔滨理工大学 一种基于视觉slam的无人机自主定位方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法
CN110617814A (zh) * 2019-09-26 2019-12-27 中国科学院电子学研究所 单目视觉和惯性传感器融合的远距离测距***及方法
CN110692027A (zh) * 2017-06-05 2020-01-14 杭州零零科技有限公司 用于提供无人机应用的易用的释放和自动定位的***和方法
CN110726415A (zh) * 2019-10-21 2020-01-24 哈尔滨工程大学 一种自适应的水下多波束同步定位与建图方法
US20200132456A1 (en) * 2018-10-29 2020-04-30 University Of New Hampshire Apparatus and method for fault-proof collection of imagery for underwater survey
CN111190424A (zh) * 2020-02-19 2020-05-22 苏州大学 一种用于室内移动机器人自主定位建图的一体化传感器
CN111353198A (zh) * 2020-04-21 2020-06-30 大连理工大学 一种无颤振铣削加工表面形貌仿真方法
CN111415375A (zh) * 2020-02-29 2020-07-14 华南理工大学 一种基于多鱼眼摄像机和双针孔投影模型的slam方法
CN111458471A (zh) * 2019-12-19 2020-07-28 中国科学院合肥物质科学研究院 一种基于图神经网络的水域检测预警方法
CN111457902A (zh) * 2020-04-10 2020-07-28 东南大学 基于激光slam定位的水域测量方法及***
CN111595334A (zh) * 2020-04-30 2020-08-28 东南大学 基于视觉点线特征与imu紧耦合的室内自主定位方法
CN111895988A (zh) * 2019-12-20 2020-11-06 北京空天技术研究所 无人机导航信息更新方法及装置
CN111914686A (zh) * 2020-07-15 2020-11-10 云南电网有限责任公司带电作业分公司 基于周域关联和模式识别的sar遥感图像水域提取方法、装置及***

Patent Citations (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170315247A1 (en) * 2016-04-27 2017-11-02 Sean Robert Griffin Ship-towed hydrophone volumetric array system method
CN110692027A (zh) * 2017-06-05 2020-01-14 杭州零零科技有限公司 用于提供无人机应用的易用的释放和自动定位的***和方法
CN109816769A (zh) * 2017-11-21 2019-05-28 深圳市优必选科技有限公司 基于深度相机的场景地图生成方法、装置及设备
CN108364319A (zh) * 2018-02-12 2018-08-03 腾讯科技（深圳）有限公司 尺度确定方法、装置、存储介质及设备
CN108648240A (zh) * 2018-05-11 2018-10-12 东南大学 基于点云特征地图配准的无重叠视场相机姿态标定方法
US20200132456A1 (en) * 2018-10-29 2020-04-30 University Of New Hampshire Apparatus and method for fault-proof collection of imagery for underwater survey
CN110223389A (zh) * 2019-06-11 2019-09-10 中国科学院自动化研究所 融合图像与激光数据的场景建模方法、***、装置
CN110309883A (zh) * 2019-07-01 2019-10-08 哈尔滨理工大学 一种基于视觉slam的无人机自主定位方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法
CN110617814A (zh) * 2019-09-26 2019-12-27 中国科学院电子学研究所 单目视觉和惯性传感器融合的远距离测距***及方法
CN110726415A (zh) * 2019-10-21 2020-01-24 哈尔滨工程大学 一种自适应的水下多波束同步定位与建图方法
CN111458471A (zh) * 2019-12-19 2020-07-28 中国科学院合肥物质科学研究院 一种基于图神经网络的水域检测预警方法
CN111895988A (zh) * 2019-12-20 2020-11-06 北京空天技术研究所 无人机导航信息更新方法及装置
CN111190424A (zh) * 2020-02-19 2020-05-22 苏州大学 一种用于室内移动机器人自主定位建图的一体化传感器
CN111415375A (zh) * 2020-02-29 2020-07-14 华南理工大学 一种基于多鱼眼摄像机和双针孔投影模型的slam方法
CN111457902A (zh) * 2020-04-10 2020-07-28 东南大学 基于激光slam定位的水域测量方法及***
CN111353198A (zh) * 2020-04-21 2020-06-30 大连理工大学 一种无颤振铣削加工表面形貌仿真方法
CN111595334A (zh) * 2020-04-30 2020-08-28 东南大学 基于视觉点线特征与imu紧耦合的室内自主定位方法
CN111914686A (zh) * 2020-07-15 2020-11-10 云南电网有限责任公司带电作业分公司 基于周域关联和模式识别的sar遥感图像水域提取方法、装置及***

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
MAXIME FERRERA 等: "AQUALOC: An underwater dataset for visual–inertial–pressure localization", 《THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH》 *
何静: "水面机器人作业导航研究", 《中国优秀硕士学位论文全文数据库 基础科学辑》 *
吴博: "基于IMU融合的视觉SLAM研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
周阳: "基于多传感器融合的移动机器人", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
张景东: "视觉SLAM中多鱼眼相机***研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
牛伯城: "基于声视觉的UUV水下同步定位与建图方法研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *
范玉洁: "空间***制导与控制技术研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 *
黄鹤: "融合IMU的单目视觉SLAM方法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113256697A (zh) * 2021-04-27 2021-08-13 武汉理工大学 水下场景的三维重建方法、***、装置和存储介质
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113744337A (zh) * 2021-09-07 2021-12-03 江苏科技大学 一种融合视觉、imu与声纳的同步定位与建图方法
CN113744337B (zh) * 2021-09-07 2023-11-24 江苏科技大学 一种融合视觉、imu与声纳的同步定位与建图方法
CN114485613A (zh) * 2021-12-31 2022-05-13 海南浙江大学研究院 一种多信息融合水下机器人定位方法
CN114485613B (zh) * 2021-12-31 2024-05-17 浙江大学海南研究院 一种多信息融合水下机器人定位方法
CN115560757A (zh) * 2022-09-01 2023-01-03 中国人民解放军战略支援部队信息工程大学 随机姿态误差条件下基于神经网络的无人机直接定位校正方法
CN115560757B (zh) * 2022-09-01 2023-08-22 中国人民解放军战略支援部队信息工程大学 随机姿态误差条件下基于神经网络的无人机直接定位校正方法

Also Published As

Publication number Publication date
CN112562052B (zh) 2021-07-27

Similar Documents

Publication Publication Date Title
CN112562052B (zh) 一种面向近岸水域的实时定位与建图方法
CN113340295B (zh) 一种多个测距传感器的无人艇近岸实时定位与建图方法
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
Piniés et al. Inertial aiding of inverse depth SLAM using a monocular camera
KR101192825B1 (ko) Ｇｐｓ／ｉｎｓ／영상ａｔ를 통합한 라이다 지오레퍼린싱 장치 및 방법
EP2133662B1 (en) Methods and system of navigation using terrain features
CN110044354A (zh) 一种双目视觉室内定位与建图方法及装置
CN113551665B (zh) 一种用于运动载体的高动态运动状态感知***及感知方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航***及方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
JP3900365B2 (ja) 測位装置および測位方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN115930977A (zh) 特征退化场景的定位方法、***、电子设备和可读存介质
JP5355443B2 (ja) 位置補正システム
CN112747749B (zh) 一种基于双目视觉和激光融合定位导航***
CN115542363B (zh) 一种适用于垂直下视航空吊舱的姿态测量方法
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN116026323A (zh) 一种用于发动机油加注机定位与区域防错方法
CN115290090A (zh) 基于多传感器信息融合的slam地图构建方法
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN117128951B (zh) 适用于自动驾驶农机的多传感器融合导航定位***及方法
Wei Multi-sources fusion based vehicle localization in urban environments under a loosely coupled probabilistic framework

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