CN117433511B - 一种多传感器融合定位方法 - Google Patents

一种多传感器融合定位方法 Download PDF

Info

Publication number
CN117433511B
CN117433511B CN202311753361.9A CN202311753361A CN117433511B CN 117433511 B CN117433511 B CN 117433511B CN 202311753361 A CN202311753361 A CN 202311753361A CN 117433511 B CN117433511 B CN 117433511B
Authority
CN
China
Prior art keywords
eye image
point
map
slam
value
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
CN202311753361.9A
Other languages
English (en)
Other versions
CN117433511A (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.)
Huajian Technology Shenzhen Co ltd
Original Assignee
Huajian Technology Shenzhen Co ltd
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 Huajian Technology Shenzhen Co ltd filed Critical Huajian Technology Shenzhen Co ltd
Priority to CN202311753361.9A priority Critical patent/CN117433511B/zh
Publication of CN117433511A publication Critical patent/CN117433511A/zh
Application granted granted Critical
Publication of CN117433511B publication Critical patent/CN117433511B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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
    • 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/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/1656—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 passive imaging devices, e.g. cameras
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W84/00—Network topologies
    • H04W84/02—Hierarchically pre-organised networks, e.g. paging networks, cellular networks, WLAN [Wireless Local Area Network] or WLL [Wireless Local Loop]
    • H04W84/04—Large scale networks; Deep hierarchical networks
    • H04W84/06—Airborne or Satellite Networks
    • 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)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Data Mining & Analysis (AREA)
  • Signal Processing (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Astronomy & Astrophysics (AREA)
  • Image Processing (AREA)

Abstract

本发明实施例公开了一种多传感器融合定位方法，包括：基于RTK、IMU、激光雷达和双目视觉相机等多种传感器的数据得到RTK定位结果、RTK均方根误差、IMU定位结果、IMU标定参数、视觉SLAM定位结果、视觉SLAM精度评估值、激光SLAM定位结果和激光SLAM精度评估值，然后将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权，根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果进行确权和平差，以得到高精度的综合融合定位结果，可以提高整个传感器系统的有效性和正确性。

Description

一种多传感器融合定位方法
技术领域
本发明涉及多源数据融合技术领域，尤其涉及一种多传感器融合定位方法。
背景技术
多源数据融合技术是一种通过整合不同来源的信息，对其进行统一评价并最终得到一致信息的技术。在许多领域，如智能交通、医疗、金融和导航定位中，都有广泛的应用。
在导航定位领域，多源数据通常是来自各类传感器。为了提高定位定姿的精度和可靠性，多传感器融合定位成为一种重要的方法。多传感器融合定位可以将传感器在空间和时间上的互补与冗余信息组合起来，依据某种优化准则进行组合，从而产生对观测环境或对象的一致性解释和描述，这样可以提高整个传感器系统的有效性和正确性。
发明内容
基于此，有必要针对上述问题，提出了一种多传感器融合定位方法，使得可以得到高精度的综合融合定位结果，以提高整个传感器系统的有效性和正确性。
为实现上述目的，本发明在第一方面提供一种多传感器融合定位方法，所述方法包括：
获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；
根据所述RTK数据确定RTK定位结果和RTK均方根误差，根据所述IMU数据确定IMU定位结果和IMU标定参数，根据所述双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据所述激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；
将所述RTK均方根误差、所述IMU标定参数、所述视觉SLAM精度评估值和所述激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、所述RTK定位结果、所述IMU定位结果、所述视觉SLAM定位结果和所述激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。
可选地，根据所述双目视觉数据确定视觉SLAM定位结果，包括：
从所述双目视觉数据提取双目视觉图像，所述双目视觉图像包括左目图像和右目图像；
基于所述左目图像与所述右目图像进行匹配，得到所述左目图像与所述右目图像之间的多对特征点；
根据所述左目图像与所述右目图像之间的多对特征点的像素点坐标计算单应性矩阵；
对所述单应性矩阵进行矩阵分解得到旋转矩阵和平移向量；
将所述旋转矩阵和所述平移向量作为所述视觉SLAM定位结果。
可选地，所述基于所述左目图像与所述右目图像进行匹配，得到所述左目图像与所述右目图像之间的多对特征点，包括：
对所述左目图像进行灰度化处理得到灰度左目图像；
对所述灰度左目图像进行平滑去噪处理，得到去噪后的灰度左目图像；
对去噪后的灰度左目图像进行角点提取，得到去噪后的灰度左目图像的多个候选特征点；
在去噪后的灰度左目图像的第n个候选特征点周围选取一组采样点对，计算每个采样点对的灰度差分值，将所有采样点对的灰度差分值转换为二进制串，并将二进制串作为去噪后的灰度左目图像的第n个候选特征点的BRIEF描述子；重复上述步骤，n依次取大于0的整数，得到去噪后的灰度左目图像的所有候选特征点的BRIEF描述子；
对所述右目图像进行灰度化处理得到灰度右目图像；
对所述灰度右目图像进行平滑去噪处理，得到去噪后的灰度右目图像；
对去噪后的灰度右目图像进行角点提取，得到去噪后的灰度右目图像的多个候选特征点；
在去噪后的灰度右目图像的第m个候选特征点周围选取一组采样点对，计算每个采样点对的灰度差分值，将所有采样点对的灰度差分值转换为二进制串，并将二进制串作为去噪后的灰度右目图像的第m个候选特征点的BRIEF描述子；重复上述步骤，m依次取大于0的整数，得到去噪后的灰度右目图像的所有候选特征点的BRIEF描述子；
根据去噪后的灰度左目图像的所有候选特征点的BRIEF描述子和去噪后的灰度右目图像的所有候选特征点的BRIEF描述子，对去噪后的灰度左目图像的多个候选特征点与去噪后的灰度右目图像的多个候选特征点进行特征点匹配，得到匹配后的特征点对；
将匹配后的特征点对进行筛选，得到去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点；
将去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点作为所述左目图像与所述右目图像之间的多对特征点。
可选地，根据所述双目视觉数据确定视觉SLAM精度评估值，包括：
根据所述左目图像与所述右目图像之间的多对特征点的像素点坐标计算多对特征点的多个视差值；
根据多对特征点的多个视差值、所述双目视觉相机的在X轴方向的焦距和所述双目视觉相机的基线长度计算多对特征点对应的多个深度信息；
根据多对特征点对应的多个深度信息、所述旋转矩阵、所述平移向量、所述双目视觉相机的内参数矩阵和所述左目图像的多个特征点的像素点坐标计算所述左目图像的多个特征点在世界坐标系的世界坐标；
根据所述左目图像中第i个图像帧的时间间隔、运动距离和视角变化与时间间隔阈值、运动距离阈值和视角变化阈值判断第i个图像帧是否为关键帧；
若第i个图像帧为关键帧，则在所述第i个图像帧的特征点的重投影误差小于误差阈值，且该特征点被两个以上的其他关键帧对应的图像帧观测到的情况下，根据该特征点的世界坐标将该特征点和关键帧更新到SLAM地图中；
对SLAM地图进行局部和全局优化，得到优化后的SLAM地图；
重复上述步骤，i依次取大于0的整数，直至i等于所述左目图像中图像帧的总个数；
根据最新优化后的SLAM地图计算所述视觉SLAM精度评估值。
可选地，所述根据最新优化后的SLAM地图计算所述视觉SLAM精度评估值，包括：
在最新优化后的SLAM地图中均匀选取在所述左目图像有对应特征点的J个地图点；
根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在所述左目图像对应的特征点的像素点坐标、所述旋转矩阵、所述平移向量和所述双目视觉相机的内参数矩阵计算第j个地图点到所述左目图像的第一深度值；
根据第j个地图点在所述左目图像对应的特征点的像素点坐标和所述单应性矩阵计算第j个地图点在所述右目图像对应的特征点的像素点坐标；
根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在所述右目图像对应的特征点的像素点坐标、所述旋转矩阵、所述平移向量和所述双目视觉相机的内参数矩阵计算第j个地图点到所述右目图像的第二深度值；
根据所述第一深度值和所述第二深度值得到第j个地图点的深度差值；
确定第j个地图点的深度权重；
重复上述步骤，j依次取大于0的整数，得到所有地图点的深度差值和深度权重；
根据所有地图点的深度差值和深度权重计算第一方差累加值，以及根据所有地图点的深度权重计算深度权重和值；
根据所述第一方差累加值和所述深度权重和值计算所述视觉SLAM精度评估值。
可选地，所述激光雷达数据包括相邻两次扫描帧激光雷达数据，根据所述激光雷达数据确定激光SLAM定位结果，包括：
基于相邻两次扫描帧激光雷达数据中的点云数据进行匹配，得到相邻两次扫描帧点云数据之间的多对特征点云；
基于构建非线性优化函数，使用非线性优化的列文伯格-马夸尔特方法，根据相邻两次扫描帧点云数据之间的多对特征点云进行迭代计算当前状态转移矩阵；
将所述当前状态转移矩阵作为所述激光SLAM定位结果。
可选地，所述第一次扫描帧激光雷达数据包括第一次扫描帧点云数据和第一时间戳信息，所述第二次扫描帧激光雷达数据包括第二次扫描帧点云数据和第二时间戳信息，所述基于相邻两次扫描帧激光雷达数据中的点云数据进行匹配，得到相邻两次扫描帧点云数据之间的多对特征点云，包括：
根据所述第一时间戳信息对所述第一次扫描帧点云数据进行运动补偿得到补偿后的第一次扫描帧点云数据；
计算补偿后的第一次扫描帧点云数据中每个点云的曲率，并将补偿后的第一次扫描帧点云数据中每个时刻的点云数据划分为多个相同的子区域；
在每个子区域中，将曲率最高的多个点云均作为边缘特征点，将曲率最低的多个点云均作为平面特征点，得到补偿后的第一次扫描帧点云数据的多个边缘特征点和多个平面特征点；
根据所述第二时间戳信息对所述第二次扫描帧点云数据进行运动补偿得到补偿后的第二次扫描帧点云数据；
计算补偿后的第二次扫描帧点云数据中每个点云的曲率，并将补偿后的第二次扫描帧点云数据中每个时刻的点云数据划分为多个相同的子区域；
在每个子区域中，将曲率最高的多个点云均作为边缘特征点，将曲率最低的多个点云均作为平面特征点，得到补偿后的第二次扫描帧点云数据的多个边缘特征点和多个平面特征点；
根据补偿后的第一次扫描帧点云数据的多个边缘特征点与补偿后的第二次扫描帧点云数据的多个边缘特征点进行边缘点匹配，得到匹配后的边缘特征点对；以及根据补偿后的第一次扫描帧点云数据的多个平面特征点与补偿后的第二次扫描帧点云数据的多个平面特征点进行平面点匹配，得到匹配后的平面特征点对；
将匹配后的边缘特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点；以及将匹配后的平面特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对平面特征点；
将补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点和多对平面特征点作为相邻两次扫描帧点云数据之间的多对特征点云。
可选地，根据所述激光雷达数据确定激光SLAM精度评估值，包括：
根据所述当前状态转移矩阵和第一次扫描帧点云数据的多个特征点云的点云坐标，将第一次扫描帧点云数据的多个特征点云更新到SLAM地图中；
根据更新后的SLAM地图计算所述激光SLAM精度评估值。
可选地，所述根据更新后的SLAM地图计算所述激光SLAM精度评估值，包括：
在更新后的SLAM地图中均匀选取在第一次帧点云数据有对应特征点云的K个地图点；
根据更新后的SLAM地图的第k个地图点的地图坐标和在第一次帧点云数据对应的特征点云的点云坐标计算第一激光距离值；
获取所述激光雷达输出的第k个地图点对应的第二激光距离值；
根据所述第一激光距离值和所述第二激光距离值计算第k个地图点的距离差值；
根据所述第一激光距离值和所述第二激光距离值计算第k个地图点的距离差值；
确定第k个地图点的距离权重；
重复上述步骤，k依次取大于0的整数，得到所有地图点的距离差值和距离权重；
根据所有地图点的距离差值和距离权重计算第二方差累加值，以及根据所有地图点的距离权重计算距离权重和值；
根据所述第二方差累加值和所述距离权重和值计算所述激光SLAM精度评估值。
可选地，所述在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果，包括：
在迭代计算的过程中，若两两类观测值的单位权方差的估计值之间的估计差值均小于预设阈值，则根据当次迭代计算得到的各类观测值的单位权方差的估计值确定各类观测值的目标权；
根据各类观测值的目标权确定所述综合融合定位结果。
为实现上述目的，本发明在第二方面提供一种多传感器融合定位装置，所述装置包括：
获取模块，用于获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；
确定模块，用于根据所述RTK数据确定RTK定位结果和RTK均方根误差，根据所述IMU数据确定IMU定位结果和IMU标定参数，根据所述双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据所述激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；
融合定位模块，用于将所述RTK均方根误差、所述IMU标定参数、所述视觉SLAM精度评估值和所述激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、所述RTK定位结果、所述IMU定位结果、所述视觉SLAM定位结果和所述激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。
为实现上述目的，本发明在第三方面提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如第一方面中任一项所述的方法。
为实现上述目的，本发明在第四方面提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如第一方面中任一项所述的方法。
采用本发明实施例，具有如下有益效果：上述方法通过获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；根据RTK数据确定RTK定位结果和RTK均方根误差，根据IMU数据确定IMU定位结果和IMU标定参数，根据双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。即通过将RTK、IMU、激光雷达和双目视觉相机等多种传感器的数据进行上述方式的融合定位，从而得到高精度的综合融合定位结果，该高精度的综合融合定位结果可以提高整个传感器系统的有效性和正确性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
其中：
图1为本申请实施例中一种多传感器融合定位方法的示意图；
图2为本申请实施例中一种多传感器融合定位装置的示意图；
图3示出了一些实施例中计算机设备的内部结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
多源数据融合技术是一种通过整合不同来源的信息，对其进行统一评价并最终得到一致信息的技术。在许多领域，如智能交通、医疗、金融和导航定位中，都有广泛的应用。
在导航定位领域，多源数据通常是来自各类传感器。为了提高定位定姿的精度和可靠性，多传感器融合定位成为一种重要的方法。多传感器融合定位可以将传感器在空间和时间上的互补与冗余信息组合起来，依据某种优化准则进行组合，从而产生对观测环境或对象的一致性解释和描述，这样可以提高整个传感器系统的有效性和正确性。其中，各类传感器在本申请中指的是RTK（即实时动态差分定位）、IMU（即惯性测量单元）、激光雷达和双目视觉相机等传感器。
在多传感器融合定位的过程中，单传感器的定位精度和可靠性信息对融合处理至关重要。RTK定位和IMU通常可以提供实时的定位精度信息。然而，对于其他重要的传感器，如激光雷达和双目视觉相机，传统的数据处理方法并没有给出实时的精度信息，此时若是直接将激光雷达和双目视觉相机的数据进行融合，通常是根据经验值来确定其权，这样子融合出来的综合融合定位结果的精度并不高。因此，本申请提供了一种多传感器融合定位方法，使得可以得到高精度的综合融合定位结果，且该高精度的综合融合定位结果可以提高整个传感器系统的有效性和正确性。该多传感器融合定位方法的具体实现方式，将在下面的实施例中进行详细说明。
请参阅图1，为本申请实施例中一种多传感器融合定位方法的示意图，该方法包括：
步骤110：获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据。
需要说明的是，RTK是实时动态定位（Real-Time Kinematic）的缩写，是一种高精度的全球卫星定位系统（GNSS）技术；RTK系统通过使用基站和移动站之间的无线通信，以及GPS或其他卫星信号，提供非常高精度的位置信息；RTK通常用于需要厘米级精度的应用，如地理信息系统（GIS）、测绘、农业、建筑和机器人等领域。IMU是惯性测量单元（InertialMeasurement Unit）的缩写，是一种能够测量和报告加速度和角速度的设备；IMU通常包括加速度计和陀螺仪，用于测量物体的线性加速度和角速度；这些数据可以用来估计物体的运动状态和方向；在无法获得卫星信号或需要更高频率的运动测量时，IMU可以作为惯性导航系统的一部分。激光雷达（Lidar）是一种通过激光束测量目标距离的传感器；激光雷达通过发送激光脉冲并测量脉冲返回的时间来计算目标与传感器之间的距离；这种技术常用于制图、避障、三维建模和自动驾驶等应用。双目视觉相机是一种具有两个镜头的相机系统，模拟人类双眼视觉；通过比较两个镜头拍摄的图像，可以计算出物体的深度信息，从而实现三维视觉效果；双目视觉在计算机视觉领域中常用于目标跟踪、深度感知和虚拟现实等应用。
综上，可以理解的是，RTK是一种用于实时动态定位的卫星定位系统，IMU是一种测量加速度和角速度的设备，激光雷达用于测量目标距离，双目视觉相机则用于获取具有深度信息的图像；本申请将RTK、IMU、激光雷达和双目视觉相机等多类传感器结合一起使用，以得到更全面、准确的环境感知和定位的传感器系统。
步骤120：根据RTK数据确定RTK定位结果和RTK均方根误差，根据IMU数据确定IMU定位结果和IMU标定参数，根据双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值。
需要说明的是，RTK定位结果是RTK的位姿信息，IMU定位结果是IMU的位姿信息，视觉SLAM定位结果是双目视觉相机的位姿信息，激光SLAM定位结果是激光雷达的位姿信息；而RTK均方根误差是RTK的精度估计值，IMU标定参数是IMU的精度估计值，视觉SLAM精度评估值是双目视觉相机的精度估计值，激光SLAM精度评估值是激光雷达的精度估计值。
步骤130：将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。
其中，预设迭代条件可以由操作人员根据大量实验、统计或经验进行设置，当然，也可以由操作人员根据实际需求进行设置。
需要说明的是，本申请通过将各类传感器的位姿信息作为先验信息，以确定各类观测值的初始权，然后根据各类观测值的初始权和各类传感器的精度估计值确定各类观测值的首次平差解算，以根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在迭代的过程中，会不断地重新计算各个观测值的权，再进行平差结算，直至迭代计算得到的各类观测值的单位权方差的估计值满足预设迭代条件时，则可以根据当次迭代计算得到的各类观测值的单位权方差的估计值所对应的权来计算综合融合定位结果；可以理解的是，此时所计算的综合融合定位结果是各类传感器所组成的整个传感器系统的定位结果。
在一些实施例中，可以使用Helmert方差分量估计，将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权，然后根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果确定各类观测值的首次平差解算，再根据各类观测值的首次平差解算，利用方差分量估计公式进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值所对应的权来计算综合融合定位结果。
在本申请实施例中，通过获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；根据RTK数据确定RTK定位结果和RTK均方根误差，根据IMU数据确定IMU定位结果和IMU标定参数，根据双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。即通过将RTK、IMU、激光雷达和双目视觉相机等多种传感器的数据进行上述方式的融合定位，从而得到高精度的综合融合定位结果，该高精度的综合融合定位结果可以提高整个传感器系统的有效性和正确性。
在一种可行的实现方式中，上述实施例中的步骤120，根据双目视觉数据确定视觉SLAM定位结果，包括：从双目视觉数据提取双目视觉图像，双目视觉图像包括左目图像和右目图像；基于左目图像与右目图像进行匹配，得到左目图像与右目图像之间的多对特征点；根据左目图像与右目图像之间的多对特征点的像素点坐标计算单应性矩阵；对单应性矩阵进行矩阵分解得到旋转矩阵和平移向量；将旋转矩阵和平移向量作为视觉SLAM定位结果。
对于计算单应性矩阵，在一些实施例中，可以使用RANSAC方法，根据左目图像与右目图像之间的多对特征点的像素点坐标计算单应性矩阵。可以理解的是，RANSAC是随机样本一致性（Random Sample Consensus）的缩写，其是一种鲁棒性极强的参数估计方法，可以用于处理存在大量异常值的数据集，在本申请用于处理左目图像与右目图像之间的单应性矩阵，可以进一步增强计算单应性矩阵的鲁棒性，有效应对异常值和噪声干扰。
对于矩阵分解，在一些实施例中，可以使用SVD分解，对单应性矩阵进行矩阵分解得到旋转矩阵和平移向量。可以理解的是，SVD是奇异值分解，其是一种矩阵分解技术，可以将单应性矩阵分解为三个矩阵的乘积，其中的U矩阵和V矩阵即为本申请的旋转矩阵和平移向量。
在本申请实施例中，通过从双目视觉数据中提取左目图像和右目图像，并基于左目图像与右目图像之间的特征点匹配和单应性矩阵计算，可以得到较为精确的旋转矩阵和平移向量，从而准确地确定视觉SLAM的定位结果。
在一种可行的实现方式中，上述实施例中的基于左目图像与右目图像进行匹配，得到左目图像与右目图像之间的多对特征点，包括：对左目图像进行灰度化处理得到灰度左目图像；对灰度左目图像进行平滑去噪处理，得到去噪后的灰度左目图像；对去噪后的灰度左目图像进行角点提取，得到去噪后的灰度左目图像的多个候选特征点；在去噪后的灰度左目图像的第n个候选特征点周围选取一组采样点对，计算每个采样点对的灰度差分值，将所有采样点对的灰度差分值转换为二进制串，并将二进制串作为去噪后的灰度左目图像的第n个候选特征点的BRIEF描述子；重复上述步骤，n依次取大于0的整数，得到去噪后的灰度左目图像的所有候选特征点的BRIEF描述子；对右目图像进行灰度化处理得到灰度右目图像；对灰度右目图像进行平滑去噪处理，得到去噪后的灰度右目图像；对去噪后的灰度右目图像进行角点提取，得到去噪后的灰度右目图像的多个候选特征点；在去噪后的灰度右目图像的第m个候选特征点周围选取一组采样点对，计算每个采样点对的灰度差分值，将所有采样点对的灰度差分值转换为二进制串，并将二进制串作为去噪后的灰度右目图像的第m个候选特征点的BRIEF描述子；重复上述步骤，m依次取大于0的整数，得到去噪后的灰度右目图像的所有候选特征点的BRIEF描述子；根据去噪后的灰度左目图像的所有候选特征点的BRIEF描述子和去噪后的灰度右目图像的所有候选特征点的BRIEF描述子，对去噪后的灰度左目图像的多个候选特征点与去噪后的灰度右目图像的多个候选特征点进行特征点匹配，得到匹配后的特征点对；将匹配后的特征点对进行筛选，得到去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点；将去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点作为左目图像与右目图像之间的多对特征点。
需要说明的是，本申请为每个候选特征点的周围均选取一组采样点，其目的是为了得到每个特征点用于匹配的BRIEF描述子；在一些实施例中，一组采样点对通常是256对采样点，即每个特征点周围的256对像素点。
对于平滑去噪处理，在一些实施例中，可以使用高斯滤波器，对灰度左目图像进行平滑去噪处理，得到去噪后的灰度左目图像；以及使用高斯滤波器，对灰度右目图像进行平滑去噪处理，得到去噪后的灰度右目图像。可以理解的是，高斯滤波器是一种平滑滤波器，在本申请可以用于去除灰度左目图像和灰度右目图像中的噪声，以增强图像的质量和特征提取的准确性。
对于角点提取，在一些实施例中，可以使用ORB角点检测算法，对去噪后的灰度左目图像进行角点提取，得到去噪后的灰度左目图像的多个候选特征点；以及使用ORB角点检测算法，对去噪后的灰度右目图像进行角点提取，得到去噪后的灰度右目图像的多个候选特征点。可以理解的是，ORB（Oriented FAST and Rotated BRIEF）是一种角点检测算法，在本申请通过计算去噪后的灰度左目图像和去噪后的灰度右目图像中的关键点周围的像素值差异，来提取出去噪后的灰度左目图像和去噪后的灰度右目图像中的角点，有利于后续的特征点的高效匹配。
对于特征点匹配，在一些实施例中，可以使用最近邻匹配方法，根据去噪后的灰度左目图像的所有候选特征点的BRIEF描述子和去噪后的灰度右目图像的所有候选特征点的BRIEF描述子，对去噪后的灰度左目图像的多个候选特征点与去噪后的灰度右目图像的多个候选特征点进行特征点匹配，得到匹配后的特征点对。可以理解的是，最近邻匹配方法是一种特征点匹配方法，在本申请通过计算两个去噪后的灰度左目图像和去噪后的灰度右目图像之间的特征点的距离，来找到最近的匹配点，可以简单、高效完成特征点的匹配任务。
对于匹配后的特征点对的筛选，在一些实施例中，可以使用匹配距离比方法，将匹配后的特征点对进行筛选，得到去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点。可以理解的是，匹配距离比方法是一种用于筛选特征点匹配对的方法，在本申请通过比较两个特征点之间的匹配距离和其周围的像素点的距离，来评估匹配的可靠性，如果最近邻距离小于次近邻距离的0.6倍时，则认为该匹配点是可靠的，该方法可以有效去除错误的匹配对，提高特征匹配的准确性。
在本申请实施例中，通过对左目图像和右目图像进行灰度化处理、平滑去噪处理和角点提取，可以有效地减少图像噪声和干扰，并提取出更准确的角点信息，这有助于提高后续特征点匹配的准确性和鲁棒性；通过使用BRIEF描述子进行特征点描述和匹配，可以大大减少计算复杂度和时间开销，即只需要计算灰度差分值并转换为二进制串，因此比传统的SIFT、SURF等算法更高效；由于BRIEF算法的计算效率高，因此可以更快地进行特征点匹配和筛选，从而实时地得到去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点，这使得该方法在实际应用中具有较好的实时性；通过使用进行特征点匹配和筛选，可以得到更准确的多对特征点，这有助于提高视觉SLAM的定位精度。
在一种可行的实现方式中，上述实施例中的步骤120，根据双目视觉数据确定视觉SLAM精度评估值，包括：根据左目图像与右目图像之间的多对特征点的像素点坐标计算多对特征点的多个视差值；根据多对特征点的多个视差值、双目视觉相机的在X轴方向的焦距和双目视觉相机的基线长度计算多对特征点对应的多个深度信息；根据多对特征点对应的多个深度信息、旋转矩阵、平移向量、双目视觉相机的内参数矩阵和左目图像的多个特征点的像素点坐标计算左目图像的多个特征点在世界坐标系的世界坐标；根据左目图像中第i个图像帧的时间间隔、运动距离和视角变化与时间间隔阈值、运动距离阈值和视角变化阈值判断第i个图像帧是否为关键帧；若第i个图像帧为关键帧，则在第i个图像帧的特征点的重投影误差小于误差阈值，且该特征点被两个以上的其他关键帧对应的图像帧观测到的情况下，根据该特征点的世界坐标将该特征点和关键帧更新到SLAM地图中；对SLAM地图进行局部和全局优化，得到优化后的SLAM地图；重复上述步骤，i依次取大于0的整数，直至i等于左目图像中图像帧的总个数；根据最新优化后的SLAM地图计算视觉SLAM精度评估值。
需要说明的是，时间间隔阈值、运动距离阈值和视角变化阈值均由操作人员根据大量实验、统计或经验进行设置，当然，也可以根据操作人员的实际需求进行设置，此处并不做限制。
对于视差值的计算，在一些实施例中，可以使用视差公式，根据左目图像与右目图像之间的多对特征点的像素点坐标计算多对特征点的多个视差值。可以理解的是，视差公式是用于计算两个图像的像素坐标之差，在本申请用于计算左目图像与右目图像之间的特征点的视差值。
对于深度信息的计算，在一些实施例中，可以使用深度公式，根据多对特征点的多个视差值、双目视觉相机的在X轴方向的焦距和双目视觉相机的基线长度计算多对特征点对应的多个深度信息。可以理解的是，深度信息是指物体到相机中心的距离，在本申请用于计算左目图像或右目图像中的特征点到双目视觉相机中心的深度信息。
对于特征点的世界坐标的计算，在一些实施例中，可以使用相机投影模型，根据多对特征点对应的多个深度信息、旋转矩阵、平移向量、双目视觉相机的内参数矩阵和左目图像的多个特征点的像素点坐标计算左目图像的多个特征点在世界坐标系的世界坐标。可以理解的是，相机投影模型是一种将三维点投影到二维图像平面的数学模型，在本申请用于根据每个特征点的二维坐标，通过相机投影模型计算其在世界坐标系下的世界坐标（可以是左目图像的多个特征点在世界坐标系的世界坐标，也可以是右目图像的多个特征点在世界坐标系的世界坐标）。
对于SLAM地图的优化，在一些实施例中，可以使用Bundle Adjustment优化算法对SLAM地图进行局部和全局优化，得到优化后的SLAM地图。可以理解的是，BundleAdjustment是一种用于视觉SLAM的优化算法，用于提高定位精度和稳定性，在本申请通过对SLAM地图中的局部地图点和所有地图点的位置和姿态进行优化，使得地图点与实际观测数据更加匹配。
对于视觉SLAM精度评估值的计算，在一些实施例中，可以使用视觉SLAM精度评估方法，根据最新优化后的SLAM地图计算视觉SLAM精度评估值。可以理解的是，视觉SLAM精度评估方法用于评估视觉SLAM的定位精度和稳定性，在本申请用于评估视觉SLAM的精度。
在本申请实施例中，通过计算特征点的视差值和深度信息，并计算特征点的世界坐标，可以获得更精确的定位结果；通过对SLAM地图进行局部和全局优化，可以进一步改进定位精度；通过对关键帧的选择和更新，可以更好地适应环境变化和动态场景，增强视觉SLAM的鲁棒性；通过对SLAM地图进行评估，可以全面地了解视觉SLAM的性能和精度水平，为后续的融合定位提供参考。
此外，通过计算特征点的视差值和深度信息，并计算特征点的世界坐标，可以实时地处理图像数据并更新SLAM地图，适用于需要实时定位的应用场景。
在一种可行的实现方式中，上述实施例中的根据最新优化后的SLAM地图计算视觉SLAM精度评估值，包括：在最新优化后的SLAM地图中均匀选取在左目图像有对应特征点的J个地图点；根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在左目图像对应的特征点的像素点坐标、旋转矩阵、平移向量和双目视觉相机的内参数矩阵计算第j个地图点到左目图像的第一深度值；根据第j个地图点在左目图像对应的特征点的像素点坐标和单应性矩阵计算第j个地图点在右目图像对应的特征点的像素点坐标；根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在右目图像对应的特征点的像素点坐标、旋转矩阵、平移向量和双目视觉相机的内参数矩阵计算第j个地图点到右目图像的第二深度值；根据第一深度值和第二深度值得到第j个地图点的深度差值；确定第j个地图点的深度权重；重复上述步骤，j依次取大于0的整数，得到所有地图点的深度差值和深度权重；根据所有地图点的深度差值和深度权重计算第一方差累加值，以及根据所有地图点的深度权重计算深度权重和值；根据第一方差累加值和深度权重和值计算视觉SLAM精度评估值。
对于选取的J个地图点，在一些实施例中，可以在最新优化后的SLAM地图中划分出在左目图像大小的区域，然后将该区域划分为30个大小相近的方格，在每个方格内分别筛选一个重投影误差最小的地图点，以得到在左目图像有对应特征点的30个地图点。
对于第一深度值和第二深度值的计算，在一些实施例中，可以使用相机投影模型，根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在左目图像对应的特征点的像素点坐标、旋转矩阵、平移向量和双目视觉相机的内参数矩阵计算第j个地图点到左目图像的第一深度值，以及使用相机投影模型，根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在右目图像对应的特征点的像素点坐标、旋转矩阵、平移向量和双目视觉相机的内参数矩阵计算第j个地图点到右目图像的第二深度值。
对于深度差值的计算，在一些实施例中，可以将第一深度值与第二深度值的差值作为深度差值。
对于深度权重确定，在一些实施例中，可以由操作人员根据大量实验、统计或经验设置预设的权重规则，以根据预设的权重规则确定第j个地图点的深度权重。
对于第一方差累加值的计算，在一些实施例中，可以将每个地图点的深度差值的平方与深度权重的乘积累加起来作为第一方差累加值。
对于深度权重和值的计算，在一些实施例中，可以将每个地图点的深度权重累加起来作为深度权重和值。
对于视觉SLAM精度评估值的计算，在一些实施例中，可以第一方差累加值与深度权重和值的商值取平方根号作为视觉SLAM精度评估值。
在本申请实施例中，通过选取地图中的特征点，并计算这些特征点在左右图像帧中的深度信息，进一步计算出每个特征点的深度差值和深度权重，然后利用这些深度差值和深度权重计算出SLAM地图的第一方差累加值和深度权重和值，从而得到视觉SLAM的精度评估值，该评估方法考虑了SLAM地图中所有特征点的信息，而不仅仅是少数几个特定的控制点或关键帧，因此可以更全面地评估视觉SLAM的性能；通过计算每个特征点的深度差值和深度权重，并利用这些值计算出SLAM地图的第一方差累加值和深度权重和值，可以得到一个客观的视觉SLAM精度评估值，避免了主观因素对评估结果的影响，使得评估结果更加客观公正；该评估方法可以实时地计算出SLAM地图的精度评估值，不需要额外的数据采集和处理时间，可以及时了解视觉SLAM的性能，并进行必要的调整和优化。
综上，该视觉SLAM精度评估方法可以提供全面、客观且实时的视觉SLAM精度评估结果，有助于提高定位精度和稳定性，促进发展。
在一种可行的实现方式中，上述实施例中的激光雷达数据包括相邻两次扫描帧激光雷达数据。
上述实施例中的步骤120，根据激光雷达数据确定激光SLAM定位结果，包括：基于相邻两次扫描帧激光雷达数据中的点云数据进行匹配，得到相邻两次扫描帧点云数据之间的多对特征点云；基于构建非线性优化函数，使用非线性优化的列文伯格-马夸尔特方法，根据相邻两次扫描帧点云数据之间的多对特征点云进行迭代计算当前状态转移矩阵；将当前状态转移矩阵作为激光SLAM定位结果。
需要说明的是，非线性优化的列文伯格-马夸尔特方法（Levenberg-Marquardtmethod）是一种迭代算法，用于解决非线性优化问题，本申请用于迭代相邻两次扫描帧点云数据之间的多对特征点云之间的距离误差达到最小，即满足约束函数条件阈值。
在本申请实施例中，通过使用相邻两次扫描帧的激光雷达数据中的点云数据进行匹配，可以获得更准确的多对特征点云数据，有效地减少误匹配和漏匹配的情况，提高匹配精度；通过使用非线性优化的列文伯格-马夸尔特方法进行迭代计算，可以更快速地得到当前状态转移矩阵，避免直接求解大规模非线性优化问题，提高了计算效率；通过将当前状态转移矩阵作为激光SLAM的定位结果，可以获得更高精度的定位结果，以利用激光SLAM的高精度和高分辨率特点，提供更准确的定位结果；通过连续的激光雷达数据匹配和非线性优化计算，可以获得连续的定位结果，使得系统具有更好的稳定性和可靠性，可以在各种环境中进行准确的自主导航。
此外，该方法可以实时地处理激光雷达数据，并生成当前的定位结果，这使得系统可以及时响应环境变化，并快速地进行自主导航。
综上，该方法可以提高数据匹配准确性、优化算法效率和定位结果精度，同时具有实时性和系统稳定性的优点，有助于推动自主导航技术的发展，为机器人、无人驾驶等领域提供更准确、可靠和实时的定位解决方案。
在一种可行的实现方式中，上述实施例中的第一次扫描帧激光雷达数据包括第一次扫描帧点云数据和第一时间戳信息，第二次扫描帧激光雷达数据包括第二次扫描帧点云数据和第二时间戳信息。
上述实施例中的基于相邻两次扫描帧激光雷达数据中的点云数据进行匹配，得到相邻两次扫描帧点云数据之间的多对特征点云，包括：根据第一时间戳信息对第一次扫描帧点云数据进行运动补偿得到补偿后的第一次扫描帧点云数据；计算补偿后的第一次扫描帧点云数据中每个点云的曲率，并将补偿后的第一次扫描帧点云数据中每个时刻的点云数据划分为多个相同的子区域；在每个子区域中，将曲率最高的多个点云均作为边缘特征点，将曲率最低的多个点云均作为平面特征点，得到补偿后的第一次扫描帧点云数据的多个边缘特征点和多个平面特征点；根据第二时间戳信息对第二次扫描帧点云数据进行运动补偿得到补偿后的第二次扫描帧点云数据；计算补偿后的第二次扫描帧点云数据中每个点云的曲率，并将补偿后的第二次扫描帧点云数据中每个时刻的点云数据划分为多个相同的子区域；在每个子区域中，将曲率最高的多个点云均作为边缘特征点，将曲率最低的多个点云均作为平面特征点，得到补偿后的第二次扫描帧点云数据的多个边缘特征点和多个平面特征点；根据补偿后的第一次扫描帧点云数据的多个边缘特征点与补偿后的第二次扫描帧点云数据的多个边缘特征点进行边缘点匹配，得到匹配后的边缘特征点对；以及根据补偿后的第一次扫描帧点云数据的多个平面特征点与补偿后的第二次扫描帧点云数据的多个平面特征点进行平面点匹配，得到匹配后的平面特征点对；将匹配后的边缘特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点；以及将匹配后的平面特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对平面特征点；将补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点和多对平面特征点作为相邻两次扫描帧点云数据之间的多对特征点云。
需要说明的是，对于在每个子区域中的边缘点特征点与平面点特征点的选择，一般将曲率最高的3个点云均作为边缘特征点，将曲率最低的4个点云均作为平面特征点。
对于第一次扫描帧点云数据和第二次扫描帧点云数据的补偿，在一些实施例中，可以使用线性插值，根据第一时间戳信息对第一次扫描帧点云数据进行运动补偿得到补偿后的第一次扫描帧点云数据；以及使用线性插值，根据第二时间戳信息对第二次扫描帧点云数据进行运动补偿得到补偿后的第二次扫描帧点云数据。线性插值是一种常用的数学方法，在本申请用于补偿扫描帧点云数据的点云。
对于边缘点匹配和平面点匹配，在一些实施例中，可以使用最近邻匹配方法，根据补偿后的第一次扫描帧点云数据的多个边缘特征点与补偿后的第二次扫描帧点云数据的多个边缘特征点进行边缘点匹配，得到匹配后的边缘特征点对；以及使用最近邻匹配方法，根据补偿后的第一次扫描帧点云数据的多个平面特征点与补偿后的第二次扫描帧点云数据的多个平面特征点进行平面点匹配，得到匹配后的平面特征点对。
对于匹配后的边缘特征点对的筛选和匹配后的平面特征点对的筛选，在一些实施例中，可以使用匹配距离比方法，将匹配后的边缘特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点；以及使用匹配距离比方法，将匹配后的平面特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对平面特征点。
在本申请实施例中，通过在处理相邻两次扫描帧激光雷达数据之间的点云数据时，采用了线性插值、曲率计算、特征点匹配和筛选等，以得到更准确的多对特征点云数据，有助于提高匹配精度和计算效率，同时减少误匹配和漏匹配的情况。
在一种可行的实现方式中，上述实施例中的步骤120，根据激光雷达数据确定激光SLAM精度评估值，包括：根据当前状态转移矩阵和第一次扫描帧点云数据的多个特征点云的点云坐标，将第一次扫描帧点云数据的多个特征点云更新到SLAM地图中；根据更新后的SLAM地图计算激光SLAM精度评估值。
对于激光SLAM精度评估值的计算，在一些实施例中，可以使用激光SLAM精度评估方法，根据更新后的SLAM地图计算激光SLAM精度评估值。可以理解的是，激光SLAM精度评估方法用于评估激光SLAM的定位精度和稳定性，在本申请用于评估激光SLAM的精度。
在本申请实施例中，通过根据当前状态转移矩阵和第一次扫描帧点云数据的多个特征点云的点云坐标，将第一次扫描帧点云数据的多个特征点云更新到SLAM地图中，提高了SLAM地图的精度和实时性；通过将第一次扫描帧点云数据的多个特征点云更新到SLAM地图中，可以使得SLAM地图更加准确地反映出现实环境中的信息；由于状态转移矩阵的引入，该方法还可以实时地更新SLAM地图，从而提高了激光SLAM的实时性；通过根据更新后的SLAM地图计算激光SLAM精度评估值，使得激光SLAM的精度可以定量评估，可以了解激光SLAM在建图过程中的精度水平，有助于提高激光SLAM的精度和可靠性，同时也为激光SLAM的优化和改进提供了重要的依据。
在一种可行的实现方式中，上述实施例中的根据更新后的SLAM地图计算激光SLAM精度评估值，包括：在更新后的SLAM地图中均匀选取在第一次帧点云数据有对应特征点云的K个地图点；根据更新后的SLAM地图的第k个地图点的地图坐标和在第一次帧点云数据对应的特征点云的点云坐标计算第一激光距离值；获取激光雷达输出的第k个地图点对应的第二激光距离值；根据第一激光距离值和第二激光距离值计算第k个地图点的距离差值；根据第一激光距离值和第二激光距离值计算第k个地图点的距离差值；确定第k个地图点的距离权重；重复上述步骤，k依次取大于0的整数，得到所有地图点的距离差值和距离权重；根据所有地图点的距离差值和距离权重计算第二方差累加值，以及根据所有地图点的距离权重计算距离权重和值；根据第二方差累加值和距离权重和值计算激光SLAM精度评估值。
对于选取的K个地图点，与视觉SLAM类似，在一些实施例中，可以在更新后的SLAM地图中划分出在第一次帧点云数据大小的区域，然后将该区域划分为30个大小相近的方格，在每个方格内分别筛选一个误差最小的地图点，以得到在第一次帧点云数据有对应特征点云的30个地图点。
对于距离差值的计算，与深度差值的计算类似，在一些实施例中，可以将第一激光距离值与第二激光距离值的差值作为距离差值。
对于距离权重确定，与深度权重确定类似，在一些实施例中，可以由操作人员根据大量实验、统计或经验设置预设的权重规则，以根据预设的权重规则确定第k个地图点的距离权重。
对于第二方差累加值的计算，与第一方差累加值的计算类似，在一些实施例中，可以将每个地图点的距离差值的平方与距离权重的乘积累加起来作为第二方差累加值。
对于距离权重和值的计算，与深度权重和值的计算类似，在一些实施例中，可以将每个地图点的深度权重累加起来作为深度权重和值。
对于激光SLAM精度评估值的计算，与视觉SLAM精度评估值的计算类似，在一些实施例中，可以第二方差累加值与距离权重和值的商值取平方根号作为激光SLAM精度评估值。
在本申请实施例中，通过根据更新后的SLAM地图计算激光SLAM精度评估值的方法，提供了一种更为准确和可靠的激光SLAM精度评估方法，该方法通过在更新后的SLAM地图中选取具有代表性的地图点，并计算这些地图点与第一次扫描帧点云数据中的对应特征点云的激光距离值，进而计算出每个地图点的距离差值和距离权重，然后根据所有地图点的距离差值和距离权重计算出第二方差累加值和距离权重和值，最后根据这两个值计算出激光SLAM精度评估值，即：通过考虑了更多的地图点，使得评估结果更具有代表性；结合了激光距离值和地图坐标，能够更准确地反映激光SLAM的定位精度；使用了距离差值和距离权重来衡量每个地图点对总体评估的影响，能够更准确地反映每个地图点对总体评估的贡献程度；计算过程简单明了，易于实现，且评估结果可靠、准确。
综上，该方法对于评估激光SLAM的精度具有重要意义，有助于提高激光SLAM的性能和可靠性。
在一种可行的实现方式中，上述实施例中的步骤130，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果，包括：在迭代计算的过程中，若两两类观测值的单位权方差的估计值之间的估计差值均小于预设阈值，则根据当次迭代计算得到的各类观测值的单位权方差的估计值确定各类观测值的目标权；根据各类观测值的目标权确定综合融合定位结果。
其中，预设阈值可以由操作人员根据大量实验、统计或经验进行设置，当然，可以由操作人员根据实际需求进行设置，此处并不做限制。
在一些实施例中，可以使用Helmert方差分量估计，以利用方差分量估计公式进行迭代计算各类观测值的单位权方差的估计值，在两两类观测值的单位权方差的估计值之间的估计差值均小于预设阈值情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值所对应的目标权来计算综合融合定位结果。可以理解的是，Helmert方差分量估计是一种现有的算法，此处就不再赘述。
在本申请实施例中，在迭代的过程中，通过估计两两类观测值的单位权方差的估计值之间的估计差值，判断是否满足预设阈值，若满足，则根据当次迭代计算得到的各类观测值的单位权方差的估计值确定各类观测值的目标权，再根据各类观测值的目标权确定综合融合定位结果，该方法能够在一定程度上避免因观测值的不稳定或异常值导致的定位误差，提高了定位的准确性和稳定性。
在一些实施例中，本申请还提供了一种多传感器融合定位装置。
请参阅图2，为本申请实施例中一种多传感器融合定位装置的示意图，该装置210包括：
获取模块211，用于获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；
确定模块212，用于根据RTK数据确定RTK定位结果和RTK均方根误差，根据IMU数据确定IMU定位结果和IMU标定参数，根据双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；
融合定位模块213，用于将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。
在本申请实施例中，上述获取模块211、确定模块212及融合定位模块213的相关内容可以参阅图1所示实施例中的内容，此处不做赘述。
需要说明的是，本申请的装置210还包括了其他的一些模块，可以理解的是，本申请的方法与装置210具有一一对应的关系，因此，本申请的装置210的其他的一些模块即为上述实施例中本申请的方法对应的内容。
在本申请实施例中，通过获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；根据RTK数据确定RTK定位结果和RTK均方根误差，根据IMU数据确定IMU定位结果和IMU标定参数，根据双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；将RTK均方根误差、IMU标定参数、视觉SLAM精度评估值和激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、RTK定位结果、IMU定位结果、视觉SLAM定位结果和激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。即通过将RTK、IMU、激光雷达和双目视觉相机等多种传感器的数据进行上述方式的融合定位，从而得到高精度的综合融合定位结果，该高精度的综合融合定位结果可以提高整个传感器系统的有效性和正确性。
在一些实施例中，本申请还提供了一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时，使得处理器执行上述方法实施例中一种多传感器融合定位方法。
在一些实施例中，本申请还提供了一种计算机设备，包括存储器和处理器，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行上述方法实施例中一种多传感器融合定位方法。
图3示出了一些实施例中计算机设备的内部结构图。该计算机设备具体可以是终端，也可以是服务器，或者是网关。如图3所示，该计算机设备包括通过系统总线连接的处理器、存储器和网络接口。
其中，存储器包括非易失性存储介质和内存储器。该计算机设备的非易失性存储介质存储有操作系统，还可存储有计算机程序，该计算机程序被处理器执行时，可使得处理器实现上述方法实施例中的各个步骤。该内存储器中也可储存有计算机程序，该计算机程序被处理器执行时，可使得处理器执行上述方法实施例中的各个步骤。本领域技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。
其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink) DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种多传感器融合定位方法，其特征在于，所述方法包括：
获取RTK的RTK数据、IMU的IMU数据、激光雷达的激光雷达数据和双目视觉相机的双目视觉数据；
根据所述RTK数据确定RTK定位结果和RTK均方根误差，根据所述IMU数据确定IMU定位结果和IMU标定参数，根据所述双目视觉数据确定视觉SLAM定位结果和视觉SLAM精度评估值，以及根据所述激光雷达数据确定激光SLAM定位结果和激光SLAM精度评估值；
将所述RTK均方根误差、所述IMU标定参数、所述视觉SLAM精度评估值和所述激光SLAM精度评估值作为先验信息，确定各类观测值的初始权；根据各类观测值的初始权、所述RTK定位结果、所述IMU定位结果、所述视觉SLAM定位结果和所述激光SLAM定位结果确定各类观测值的首次平差解算；根据各类观测值的首次平差解算进行迭代计算各类观测值的单位权方差的估计值，在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果。
2.根据权利要求1所述的方法，其特征在于，根据所述双目视觉数据确定视觉SLAM定位结果，包括：
从所述双目视觉数据提取双目视觉图像，所述双目视觉图像包括左目图像和右目图像；
基于所述左目图像与所述右目图像进行匹配，得到所述左目图像与所述右目图像之间的多对特征点；
根据所述左目图像与所述右目图像之间的多对特征点的像素点坐标计算单应性矩阵；
对所述单应性矩阵进行矩阵分解得到旋转矩阵和平移向量；
将所述旋转矩阵和所述平移向量作为所述视觉SLAM定位结果。
3.根据权利要求2所述的方法，其特征在于，所述基于所述左目图像与所述右目图像进行匹配，得到所述左目图像与所述右目图像之间的多对特征点，包括：
对所述左目图像进行灰度化处理得到灰度左目图像；
对所述灰度左目图像进行平滑去噪处理，得到去噪后的灰度左目图像；
对去噪后的灰度左目图像进行角点提取，得到去噪后的灰度左目图像的多个候选特征点；
在去噪后的灰度左目图像的第n个候选特征点周围选取一组采样点对，计算每个采样点对的灰度差分值，将所有采样点对的灰度差分值转换为二进制串，并将二进制串作为去噪后的灰度左目图像的第n个候选特征点的BRIEF描述子；重复上述步骤，n依次取大于0的整数，得到去噪后的灰度左目图像的所有候选特征点的BRIEF描述子；
对所述右目图像进行灰度化处理得到灰度右目图像；
对所述灰度右目图像进行平滑去噪处理，得到去噪后的灰度右目图像；
对去噪后的灰度右目图像进行角点提取，得到去噪后的灰度右目图像的多个候选特征点；
在去噪后的灰度右目图像的第m个候选特征点周围选取一组采样点对，计算每个采样点对的灰度差分值，将所有采样点对的灰度差分值转换为二进制串，并将二进制串作为去噪后的灰度右目图像的第m个候选特征点的BRIEF描述子；重复上述步骤，m依次取大于0的整数，得到去噪后的灰度右目图像的所有候选特征点的BRIEF描述子；
根据去噪后的灰度左目图像的所有候选特征点的BRIEF描述子和去噪后的灰度右目图像的所有候选特征点的BRIEF描述子，对去噪后的灰度左目图像的多个候选特征点与去噪后的灰度右目图像的多个候选特征点进行特征点匹配，得到匹配后的特征点对；
将匹配后的特征点对进行筛选，得到去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点；
将去噪后的灰度左目图像与去噪后的灰度右目图像之间的多对特征点作为所述左目图像与所述右目图像之间的多对特征点。
4.根据权利要求2所述的方法，其特征在于，根据所述双目视觉数据确定视觉SLAM精度评估值，包括：
根据所述左目图像与所述右目图像之间的多对特征点的像素点坐标计算多对特征点的多个视差值；
根据多对特征点的多个视差值、所述双目视觉相机的在X轴方向的焦距和所述双目视觉相机的基线长度计算多对特征点对应的多个深度信息；
根据多对特征点对应的多个深度信息、所述旋转矩阵、所述平移向量、所述双目视觉相机的内参数矩阵和所述左目图像的多个特征点的像素点坐标计算所述左目图像的多个特征点在世界坐标系的世界坐标；
根据所述左目图像中第i个图像帧的时间间隔、运动距离和视角变化与时间间隔阈值、运动距离阈值和视角变化阈值判断第i个图像帧是否为关键帧；
若第i个图像帧为关键帧，则在所述第i个图像帧的特征点的重投影误差小于误差阈值，且该特征点被两个以上的其他关键帧对应的图像帧观测到的情况下，根据该特征点的世界坐标将该特征点和关键帧更新到SLAM地图中；
对SLAM地图进行局部和全局优化，得到优化后的SLAM地图；
重复上述步骤，i依次取大于0的整数，直至i等于所述左目图像中图像帧的总个数；
根据最新优化后的SLAM地图计算所述视觉SLAM精度评估值。
5.根据权利要求4所述的方法，其特征在于，所述根据最新优化后的SLAM地图计算所述视觉SLAM精度评估值，包括：
在最新优化后的SLAM地图中均匀选取在所述左目图像有对应特征点的J个地图点；
根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在所述左目图像对应的特征点的像素点坐标、所述旋转矩阵、所述平移向量和所述双目视觉相机的内参数矩阵计算第j个地图点到所述左目图像的第一深度值；
根据第j个地图点在所述左目图像对应的特征点的像素点坐标和所述单应性矩阵计算第j个地图点在所述右目图像对应的特征点的像素点坐标；
根据最新优化后的SLAM地图的第j个地图点的地图坐标、第j个地图点在所述右目图像对应的特征点的像素点坐标、所述旋转矩阵、所述平移向量和所述双目视觉相机的内参数矩阵计算第j个地图点到所述右目图像的第二深度值；
根据所述第一深度值和所述第二深度值得到第j个地图点的深度差值；
确定第j个地图点的深度权重；
重复上述步骤，j依次取大于0的整数，得到所有地图点的深度差值和深度权重；
根据所有地图点的深度差值和深度权重计算第一方差累加值，以及根据所有地图点的深度权重计算深度权重和值；
根据所述第一方差累加值和所述深度权重和值计算所述视觉SLAM精度评估值。
6.根据权利要求1所述的方法，其特征在于，所述激光雷达数据包括相邻两次扫描帧激光雷达数据，根据所述激光雷达数据确定激光SLAM定位结果，包括：
基于相邻两次扫描帧激光雷达数据中的点云数据进行匹配，得到相邻两次扫描帧点云数据之间的多对特征点云；
基于构建非线性优化函数，使用非线性优化的列文伯格-马夸尔特方法，根据相邻两次扫描帧点云数据之间的多对特征点云进行迭代计算当前状态转移矩阵；
将所述当前状态转移矩阵作为所述激光SLAM定位结果。
7.根据权利要求6所述的方法，其特征在于，第一次扫描帧激光雷达数据包括第一次扫描帧点云数据和第一时间戳信息，第二次扫描帧激光雷达数据包括第二次扫描帧点云数据和第二时间戳信息，所述基于相邻两次扫描帧激光雷达数据中的点云数据进行匹配，得到相邻两次扫描帧点云数据之间的多对特征点云，包括：
根据所述第一时间戳信息对所述第一次扫描帧点云数据进行运动补偿得到补偿后的第一次扫描帧点云数据；
计算补偿后的第一次扫描帧点云数据中每个点云的曲率，并将补偿后的第一次扫描帧点云数据中每个时刻的点云数据划分为多个相同的子区域；
在每个子区域中，将曲率最高的多个点云均作为边缘特征点，将曲率最低的多个点云均作为平面特征点，得到补偿后的第一次扫描帧点云数据的多个边缘特征点和多个平面特征点；
根据所述第二时间戳信息对所述第二次扫描帧点云数据进行运动补偿得到补偿后的第二次扫描帧点云数据；
计算补偿后的第二次扫描帧点云数据中每个点云的曲率，并将补偿后的第二次扫描帧点云数据中每个时刻的点云数据划分为多个相同的子区域；
在每个子区域中，将曲率最高的多个点云均作为边缘特征点，将曲率最低的多个点云均作为平面特征点，得到补偿后的第二次扫描帧点云数据的多个边缘特征点和多个平面特征点；
根据补偿后的第一次扫描帧点云数据的多个边缘特征点与补偿后的第二次扫描帧点云数据的多个边缘特征点进行边缘点匹配，得到匹配后的边缘特征点对；以及根据补偿后的第一次扫描帧点云数据的多个平面特征点与补偿后的第二次扫描帧点云数据的多个平面特征点进行平面点匹配，得到匹配后的平面特征点对；
将匹配后的边缘特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点；以及将匹配后的平面特征点对进行筛选，得到补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对平面特征点；
将补偿后的第一次扫描帧点云数据与补偿后的第二次扫描帧点云数据之间的多对边缘特征点和多对平面特征点作为相邻两次扫描帧点云数据之间的多对特征点云。
8.根据权利要求6所述的方法，其特征在于，根据所述激光雷达数据确定激光SLAM精度评估值，包括：
根据所述当前状态转移矩阵和第一次扫描帧点云数据的多个特征点云的点云坐标，将第一次扫描帧点云数据的多个特征点云更新到SLAM地图中；
根据更新后的SLAM地图计算所述激光SLAM精度评估值。
9.根据权利要求8所述的方法，其特征在于，所述根据更新后的SLAM地图计算所述激光SLAM精度评估值，包括：
在更新后的SLAM地图中均匀选取在第一次帧点云数据有对应特征点云的K个地图点；
根据更新后的SLAM地图的第k个地图点的地图坐标和在第一次帧点云数据对应的特征点云的点云坐标计算第一激光距离值；
获取所述激光雷达输出的第k个地图点对应的第二激光距离值；
根据所述第一激光距离值和所述第二激光距离值计算第k个地图点的距离差值；
根据所述第一激光距离值和所述第二激光距离值计算第k个地图点的距离差值；
确定第k个地图点的距离权重；
重复上述步骤，k依次取大于0的整数，得到所有地图点的距离差值和距离权重；
根据所有地图点的距离差值和距离权重计算第二方差累加值，以及根据所有地图点的距离权重计算距离权重和值；
根据所述第二方差累加值和所述距离权重和值计算所述激光SLAM精度评估值。
10.根据权利要求1所述的方法，其特征在于，所述在各类观测值的单位权方差的估计值满足预设迭代条件的情况下，根据当次迭代计算得到的各类观测值的单位权方差的估计值确定综合融合定位结果，包括：
在迭代计算的过程中，若两两类观测值的单位权方差的估计值之间的估计差值均小于预设阈值，则根据当次迭代计算得到的各类观测值的单位权方差的估计值确定各类观测值的目标权；
根据各类观测值的目标权确定所述综合融合定位结果。
CN202311753361.9A 2023-12-20 2023-12-20 一种多传感器融合定位方法 Active CN117433511B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311753361.9A CN117433511B (zh) 2023-12-20 2023-12-20 一种多传感器融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311753361.9A CN117433511B (zh) 2023-12-20 2023-12-20 一种多传感器融合定位方法

Publications (2)

Publication Number Publication Date
CN117433511A CN117433511A (zh) 2024-01-23
CN117433511B true CN117433511B (zh) 2024-03-12

Family

ID=89553792

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311753361.9A Active CN117433511B (zh) 2023-12-20 2023-12-20 一种多传感器融合定位方法

Country Status (1)

Country Link
CN (1) CN117433511B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112270698A (zh) * 2019-12-31 2021-01-26 山东理工大学 基于最邻近曲面的非刚性几何配准方法
JP2022039906A (ja) * 2020-08-28 2022-03-10 中国計量大学 マルチセンサによる複合キャリブレーション装置及び方法
CN115407357A (zh) * 2022-07-05 2022-11-29 东南大学 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN115880364A (zh) * 2023-02-09 2023-03-31 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法
CN116047565A (zh) * 2022-12-09 2023-05-02 上汽大众汽车有限公司 一种多传感器数据融合定位系统
CN116630442A (zh) * 2023-07-19 2023-08-22 绘见科技(深圳)有限公司 一种视觉slam位姿估计精度评估方法及装置
CN116819580A (zh) * 2023-08-23 2023-09-29 浙江中裕通信技术有限公司 一种惯性辅助的双天线gnss海上船舶姿态确定方法
CN116893425A (zh) * 2023-07-18 2023-10-17 南京理工大学 一种果园采摘机器人超高精度定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111258313B (zh) * 2020-01-20 2022-06-07 深圳市普渡科技有限公司 多传感器融合slam系统及机器人

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112270698A (zh) * 2019-12-31 2021-01-26 山东理工大学 基于最邻近曲面的非刚性几何配准方法
JP2022039906A (ja) * 2020-08-28 2022-03-10 中国計量大学 マルチセンサによる複合キャリブレーション装置及び方法
CN115407357A (zh) * 2022-07-05 2022-11-29 东南大学 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN116047565A (zh) * 2022-12-09 2023-05-02 上汽大众汽车有限公司 一种多传感器数据融合定位系统
CN115880364A (zh) * 2023-02-09 2023-03-31 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法
CN116893425A (zh) * 2023-07-18 2023-10-17 南京理工大学 一种果园采摘机器人超高精度定位方法
CN116630442A (zh) * 2023-07-19 2023-08-22 绘见科技(深圳)有限公司 一种视觉slam位姿估计精度评估方法及装置
CN116819580A (zh) * 2023-08-23 2023-09-29 浙江中裕通信技术有限公司 一种惯性辅助的双天线gnss海上船舶姿态确定方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
多传感器信息融合的自动驾驶车辆定位与速度估计;彭文正等;传感技术学报;20200815(08);66-74 *

Also Published As

Publication number Publication date
CN117433511A (zh) 2024-01-23

Similar Documents

Publication Publication Date Title
US9466143B1 (en) Geoaccurate three-dimensional reconstruction via image-based geometry
CN102472609B (zh) 位置和姿势校准方法及设备
CN112802096A (zh) 实时定位和建图的实现装置和方法
Indelman et al. Incremental light bundle adjustment for robotics navigation
CN114494629A (zh) 一种三维地图的构建方法、装置、设备及存储介质
CN116625354A (zh) 一种基于多源测绘数据的高精度地形图生成方法及系统
CN115451948A (zh) 一种基于多传感器融合的农业无人车定位里程计方法及系统
Leclerc et al. Self-consistency: A novel approach to characterizing the accuracy and reliability of point correspondence algorithms
Sibley et al. A sliding window filter for incremental SLAM
CN114111791B (zh) 一种智能机器人室内自主导航方法、系统及存储介质
CN117590362B (zh) 一种多激光雷达外参标定方法、装置和设备
CN113450334B (zh) 一种水上目标检测方法、电子设备及存储介质
D’Adamo et al. Registration of three‐dimensional scanning LiDAR sensors: An evaluation of model‐based and model‐free methods
CN116630442B (zh) 一种视觉slam位姿估计精度评估方法及装置
CN114022552A (zh) 一种融合激光雷达和相机的目标定位方法及相关装置
CN117433511B (zh) 一种多传感器融合定位方法
Chen et al. 3d map building based on stereo vision
CN115222815A (zh) 障碍物距离检测方法、装置、计算机设备和存储介质
CN111462321B (zh) 点云地图处理方法、处理装置、电子装置和车辆
Walvoord et al. Geoaccurate three-dimensional reconstruction via image-based geometry
CN113269857A (zh) 坐标系关系获取方法及装置
Steffen Visual SLAM from image sequences acquired by unmanned aerial vehicles
CN115077467B (zh) 清洁机器人的姿态估计方法、装置及清洁机器人
Pugliese et al. Non-Line-of-Sight GNSS Ray Tracing and Rectification in Urban Canyons by LiDAR-Based Surface Reconstruction
CN117437552B (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