CN117554989A - 视觉融合激光雷达slam定位导航的方法及其无人机系统 - Google Patents

视觉融合激光雷达slam定位导航的方法及其无人机系统 Download PDF

Info

Publication number
CN117554989A
CN117554989A CN202210929221.1A CN202210929221A CN117554989A CN 117554989 A CN117554989 A CN 117554989A CN 202210929221 A CN202210929221 A CN 202210929221A CN 117554989 A CN117554989 A CN 117554989A
Authority
CN
China
Prior art keywords
pose
motion information
fusion
error
slam
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.)
Pending
Application number
CN202210929221.1A
Other languages
English (en)
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.)
Beijing Hydrogen Source Intelligent Technology Co ltd
Original Assignee
Beijing Hydrogen Source Intelligent Technology 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 Beijing Hydrogen Source Intelligent Technology Co ltd filed Critical Beijing Hydrogen Source Intelligent Technology Co ltd
Priority to CN202210929221.1A priority Critical patent/CN117554989A/zh
Publication of CN117554989A publication Critical patent/CN117554989A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/933—Lidar systems specially adapted for specific applications for anti-collision purposes of aircraft or spacecraft
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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

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)
  • Electromagnetism (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种视觉融合激光雷达SLAM定位导航的方法及其无人机系统，包括获取三维激光SLAM系统生成的第一融合位姿和运动信息；获取视觉SLAM系统生成的第二融合位姿和运动信息；比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，将第三比较误差与系统标定误差进行比较；当所述第三比较误差大于系统标定误差时，切换到视觉SLAM系统，用于无人机的导航。本发明提供的方法和系统可以使无人机遇到大雨、浓烟或浓雾等恶劣天气时，仍然能够在复杂的环境中做出运动决策，提高无人机的准确度和稳定性，使用方便，同时提高了安全性。

Description

视觉融合激光雷达SLAM定位导航的方法及其无人机系统
技术领域
本发明属于无人机技术领域，具体涉及一种视觉融合激光雷达SLAM定位导航的方法及其无人机系统。
背景技术
由于激光雷达具有精度高、视角大、测距范围广等优点，因此目前现有无人机一部分采用激光雷达技术进行导航，但是申请人在研究中发现，激光雷达的激光在晴朗的天气里衰减较小，传播距离较远，而在大雨、浓烟、浓雾等坏天气里，衰减急剧加大，激光的传播距离大受影响。因此，当采用激光雷达技术导航的无人机在遇到大雨、浓烟或浓雾等时，激光雷达获取到的数据会不准确，从而导致无人机无法确定自身的位置和姿态，进而导致无人机对复杂环境难以理解以及难以做出运动决策。
因此，如何在无人机遇到大雨、浓烟或浓雾等恶劣天气时，仍然能够在复杂的环境中做出运动决策是急需解决的技术问题。
发明内容
本发明实施例提供一种视觉融合激光雷达SLAM定位导航的方法及其无人机系统，其能够达到无人机在遇到大雨、浓烟或浓雾等恶劣天气时，仍然能够在复杂的环境中做出运动决策的效果。
一方面，本发明实施例提供一种视觉融合激光雷达SLAM定位导航的方法，用于无人机定位导航，包括以下步骤：
(1)、获取三维激光SLAM系统生成的第一融合位姿和运动信息；
(2)、获取视觉SLAM系统生成的第二融合位姿和运动信息；
(3)、比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，将第三比较误差与系统标定误差进行比较；
(4)、当所述第三比较误差大于系统标定误差时，切换到视觉SLAM系统，用于无人机的导航。
优选的，在上述实施例中，进一步包括
(5)、步骤(4)中，进一步为：当所述第三比较误差大于系统标定误差时，切换到视觉SLAM系统，获取第三融合位姿和运动信息，供无人机定位、导航；或当所述第三比较误差小于或等于系统标定误差时，获取第二融合位姿和运动信息，继续采用激光SLAM系统供无人机定位导航；
(6)、获取三维激光SLAM系统的地图更新信息和/或获取视觉SLAM系统的地图更新信息；
(7)、获取用户指定的位置信息、并根据用户指定的位置信息、地图更新信息、第三融合位姿和运动信息或第二融合位姿和运动信息，生成最佳航路，对无人机进行即时定位和自主导航，并显示无人机的位置及控制无人机的飞行；
本发明实施例中，所述系统标定误差为系统预设值。
优选的，所述步骤(1)中的三维激光SLAM系统运行生成第一融合位姿和运动信息和地图更新信息还包括以下步骤：
(1-1)、获取第一IMU单元采集当前帧的第一IMU位姿和运动信息，获取三维激光雷达采集当前帧的点云数据信息；
(1-2)、对采集到的点云数据进行滤波和预处理；
(1-3)、对滤波处理后的点云数据进行解算，得到当前帧的激光雷达位姿和运动信息和当前帧在激光系统中的周围环境的局部地图，进行激光系统中的地图更新；
(1-4)、融合全部的局部地图生成全局地图；
(1-5)、结合上一帧的激光雷达位姿和运动信息与当前帧的激光雷达位姿和运动信息生成第一运动轨迹；
(1-6)、结合上一帧的第一IMU位姿和运动信息与当前帧的第一IMU位姿和运动信息生成第二运动轨迹；
(1-7)、比较第一运动轨迹和第二运动轨迹，得到第一比较误差，将第一比较误差与激光SLAM标定误差进行比较，其中所述激光SLAM标定误差为系统预设值；
(1-8)、根据第一比较误差与激光SLAM标定误差的比较结果融合第一IMU与激光雷达的位姿和运动信息，并结合当前更新的全局地图模型，得到当前帧的第一融合位姿和运动信息；
(1-9)、输出第一融合位姿和运动信息和全局地图信息。
优选的，所述步骤1-2中的对采集到的点云数据进行滤波和预处理还包括以下步骤：
(1-2-1)、对采集到的点云数据进行滤波处理；
(1-2-2)、对滤波后的点云数据进行预处理；
(1-2-3)、根据预处理后的点云数据更新临时局部地图。
优选的，所述步骤(1-6)中的将第一比较误差与激光SLAM标定误差进行比较还包括以下步骤：
(1-6-1)、若第一比较误差大于激光SLAM标定误差，则将当前帧的第一IMU位姿和运动信息替换为当前帧的激光雷达位姿和运动信息；
(1-6-2)、若第一比较误差小于或等于激光SLAM标定误差，则存储当前帧的第一IMU位姿和运动信息。
优选的，所述步骤(2)中的视觉SLAM系统运行生成第二融合位姿和运动信息还包括以下步骤：
(2-1)、获取第二IMU单元采集当前帧的第二IMU位姿和运动信息，获取摄像头采集当前帧的图像像素信息；本实施例中为双目摄像头；
(2-2)、对所述当前帧的图像像素信息进行前端特征匹配跟踪；
(2-3)、对特征进行特征点三角化，并解算得到当前帧的摄像头位姿和运动信息和当前帧在视觉系统中的周围环境的局部地图，并进行视觉系统中的地图更新；
(2-4)、结合上一帧的摄像头位姿和运动信息与当前帧的摄像头位姿和运动信息生成第三运动轨迹；
(2-5)、结合上一帧第二IMU位姿和运动信息与当前帧的第二IMU位姿和运动信息生成第四运动轨迹；
(2-6)、比较第三运动轨迹和第四运动轨迹，得到第二比较误差，将第二比较误差与视觉SLAM标定误差进行比较，其中所述视觉SLAM标定误差为系统预设值；
(2-7)、根据第二比较误差与视觉SLAM标定误差的比较结果融合第二IMU与摄像头位姿和运动信息,并结合视觉系统地图信息得到当前帧在视觉系统地图中的第二融合位姿和运动信息；
优选的，所述步骤2-6中的将第二比较误差与视觉SLAM标定误差进行比较还包括以下步骤：
(2-6-1)、若第二比较误差大于视觉SLAM标定误差，则将当前帧的第二IMU位姿和运动信息替换为当前帧的摄像头位姿和运动信息；
(2-6-2)、若第二比较误差小于或等于视觉SLAM标定误差，则存储当前帧的第二IMU位姿和运动信息。
优选的，所述步骤(3)中的将第三比较误差与系统标定误差进行比较还包括以下步骤：
(3-1)、若第三比较误差大于系统标定误差，则切换到视觉SLAM系统，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第二融合位姿和运动信息；
(3-2)、若第三比较误差小于或等于系统标定误差，则根据权重算法计算三维激光SLAM系统与视觉SLAM系统的使用权重，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第三融合位姿和运动信息。
优选的，所述步骤(3-1)中的若第三比较误差大于系统标定误差还包括以下步骤：
(3-1-1)、将第三比较误差与系统切换误差进行比较；
(3-1-2)、若第三比较误差小于或等于系统切换误差，则将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并存储到视觉SLAM系统中；
(3-1-3)、若第三比较误差大于系统切换误差，进入步骤(4)切换到视觉SLAM系统，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第二融合位姿和运动信息。
另一方面，本发明实施例提供一种基于视觉融合三维激光雷达SLAM技术的无人机系统，包括用于无人机定位、导航的三维激光SLAM系统、用于无人机定位、导航的视觉SLAM系统、用于控制无人机的飞行控制单元和用于处理各个系统的数据的系统处理单元，其中，所述三维激光SLAM系统提供第一融合位姿和运动信息，所述视觉SLAM系统提供第二融合位姿和运动信息，所述系统处理单元比较第一融合位姿和运动信息与第二融合位姿和运动信息，获得第三比较误差，将第三比较误差与系统预设的系统标定误差进行比较；当所述第三比较误差大于系统预设的系统标定误差时，将当前的导航系统切换到视觉SLAM系统，用于无人机的定位导航。
优选的，所述系统处理单元包括系统标定模块、系统切换模块和系统融合模块，其中，
所述系统标定模块用于实时修正视觉SLAM系统的误差，所述系统标定模块比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，并将第三比较误差与预设好的系统标定误差进行比较；
所述系统切换模块用于视觉SLAM系统与三维激光SLAM系统之间的切换；
所述系统融合模块用于融合视觉SLAM系统和三维激光SLAM系统的位姿和运动信息；
当第三比较误差大于系统标定误差，则将第三比较误差与预设好的系统切换误差进行比较；若第三比较误差小于或等于系统标定误差，则将第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块；
若第三比较误差小于或等于系统切换误差，则系统标定模块将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块；
若第三比较误差大于系统切换误差，则把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统切换模块，系统切换模块仅使用视觉SLAM系统输出的第二融合位姿和运动信息，以定位导航无人机；
系统融合模块根据系统标定模块发送的结果，依据权重算法计算三维激光SLAM系统与视觉SLAM系统的使用权重，融合得出融合位姿运动信息。
优选的，本发明实施例所述的视觉融合三维激光雷达SLAM技术的无人机系统还包括多传感器融合模块，根据系统融合模块发送的融合位姿运动信息、更新的全局三维地图信息进行融合，当有外部GPS和/或RTK信号时，同时融合GPS和/或RTK信息，获得带有地球坐标系的融合位姿和运动信息。
优选的，所述三维激光SLAM系统还包括激光信息采集单元和激光信息处理单元，其中，
所述激光信息采集单元用于为三维激光SLAM系统采集信息；
所述激光信息处理单元用于处理激光信息采集单元采集到的信息。
优选的，所述激光信息采集单元还包括第一IMU单元和至少一个激光雷达，其中，
所述第一IMU单元用于为三维激光SLAM系统提供预测位姿和运动信息；
所述激光雷达用于采集周围环境的点云数据。
优选的，所述激光雷达为多线三维激光扫描雷达。
优选的，所述为三维激光SLAM系统提供预测位姿和运动信息还包括第一IMU单元先采集里程信息，通过无人机惯性里程计运动学的模型，将里程信息转化为无人机位姿变化信息，送入贝叶斯滤波器初步计算出预测位姿和运动信息。通过系统观测模型修正贝叶斯滤波器的预测位姿，有效提高了滤波估计的无人机位姿精度
优选的，所述激光信息处理单元还包括点云处理模块、雷达数据处理模块、雷达系统存储模块、雷达系统地图更新模块和全局地图融合模块，其中，
所述点云处理模块用于处理点云数据；
所述雷达数据处理模块用于处理雷达系统的数据；
所述雷达系统存储模块用于存储激光信息处理单元产生的信息；
所述雷达系统地图更新模块用于实时更新三维激光SLAM系统100的地图信息；
所述全局地图融合模块用于融合全部的局部地图信息生成全局地图信息，使无人机在所述全局地图信息中规划航行路径。
优选的，所述点云处理模块还包括滤波模块和预处理模块，其中，
所述滤波模块用于对激光雷达采集到的点云数据滤波处理；
所述预处理模块用于对滤波之后的点云数据进行初步处理，得到临时局部地图信息。
优选的，所述滤波处理包括对点云数据进行降噪、去除异常点和减少多余的点云数据量。
优选的，所述雷达数据处理模块还包括雷达解算模块、雷达系统轨迹生成模块、雷达系统轨迹比较模块、雷达系统标定模块、激光雷达融合模块，其中，
所述雷达解算模块用于对滤波之后的点云数据进行解算，得到对应的激光雷达位姿和运动信息和当前帧周围环境的局部地图信息；
所述雷达系统轨迹生成模块用于结合连续两帧的位姿和运动信息生成无人机在连续两帧的运动轨迹；
所述雷达系统轨迹比较模块用于比较运动轨迹和误差；
所述雷达系统标定模块用于实时修正第一IMU单元误差；
所述激光雷达融合模块用于融合第一IMU单元与激光雷达输出的位姿和运动信息。
优选的，所述视觉SLAM系统还包括视觉信息采集单元和视觉信息处理单元，其中，
所述视觉信息采集单元用于为视觉SLAM系统采集信息；
所述视觉信息处理单元用于处理视觉信息采集单元采集到的信息。
优选的，所述视觉信息采集单元还包括第二IMU单元和至少一组双目摄像头，其中，
所述第二IMU单元用于为视觉SLAM系统提供预测位姿和运动信息；
所述至少一组双目摄像头用于拍摄周围环境的图像像素信息。
优选的，所述为视觉SLAM系统提供预测位姿和运动信息还包括，第二IMU单元先采集里程信息，通过无人机惯性里程计运动学的模型，将里程信息转化为无人机位姿变化信息，送入贝叶斯滤波器初步计算出预测位姿和运动信息。
优选的，所述视觉信息处理单元包括特征处理模块、视觉数据处理模块、视觉系统存储模块和视觉系统地图更新模块，其中，
所述特征处理模块用于处理图像特征；
所述视觉数据处理模块用于处理视觉系统的数据；
所述视觉系统存储模块用于存储视觉信息处理单元产生的信息；
所述视觉系统地图更新模块用于实时更新视觉SLAM系统的地图信息。
优选的，所述特征处理模块还包括特征匹配跟踪模块、特征点处理模块，其中，
所述特征匹配跟踪模块用于实时跟踪双目摄像头采集到的图像像素信息并将连续两帧的图像像素信息进行匹配特征；
所述特征点处理模块用于对特征进行特征点处理化。
优选的，所述特征点处理化的方法如下：
利用一个6维向量多关键帧匹配点三角化的特征点深度获取方法来表示一个环境特征点的三维位置，包括无人机的摄像机的位置以及无人机所处的环境特征点方向及深度信息；不断地进行环境更新，当特征估计协方差小于某一设定阈值时，将无人机所处的环境特征点的6维表示转换为3维的欧式坐标，利用在同一帧图像建立多个特征点，将属于同一帧特征点的表示形式缩减为一个摄像机姿态加多个深度的形式，以有效降低无人机系统状态的长度。
优选的，所述视觉数据处理模块还包括视觉解算模块、视觉系统轨迹生成模块、视觉系统轨迹比较模块、视觉系统标定模块、视觉双目融合模块，其中，
所述视觉解算模块用于将处理后的环境特征点信息解算为摄像头位姿和运动信息和当前帧周围环境的局部地图信息；
所述视觉系统轨迹生成模块用于结合连续两帧的位姿和运动信息生成无人机在连续两帧的运动轨迹；
所述视觉系统轨迹比较模块用于比较运动轨迹和误差；
所述视觉系统标定模块用于实时修正第二IMU单元误差；
所述视觉双目融合模块用于融合第二IMU单元与双目摄像头输出的位姿和运动信息。
优选的，所述飞行控制单元包括全局规划模块、局部规划模块和底层控制模块，其中，
所述全局规划模块用于规划全局地图的航行最佳路径；
所述局部规划模块用于对预处理后得到的实时的局部地图信息规划全局最佳路径；
所述底层控制模块用于对无人机进行控制分配。
本发明提供的方法和系统，基于系统融合模块同时使用了视觉系统和三维激光系统，并且当其中一个系统的导航定位出现偏差时，能够通过系统切换模块无缝切换到另外一个系统，提高了无人机的环境适应性和稳定性，同时通过系统处理单元实时对双系统的数据进行比较修正，使系统定位导航更加精确，可以应用到更加广泛的场景。
本发明的有益效果如下：
1、无人机遇到大雨、浓烟或浓雾等恶劣天气时，仍然能够在复杂的环境中做出运动决策，提高无人机的准确度和稳定性，使用方便，同时提高了安全性。
2、通过双系统的切换可以避免其中一个系统出现故障时使无人机整体停止运作。
3、可在室内或任何不具备卫星GPS信号接收的条件下进行飞行，安全可靠，不容易被无线信号干扰飞行，可在完全无通讯连接的情况下自主飞行。
4、实现无人机高可靠性的悬停和自主飞行，可自主的进行快速航路规划，自主飞达目的地。
5、系统的激光雷达属于自主发光的传感器，可以在完全黑暗的环境下飞行。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例中的视觉融合激光雷达SLAM定位导航无人机的方法总流程图。
图2为本发明实施例中的三维激光SLAM系统运行流程图。
图3为本发明实施例中的视觉SLAM系统运行流程图。
图4为本发明实施例中的基于视觉融合三维激光雷达SLAM定位导航的无人机系统模块图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
一方面，请参阅图1，本发明实施例提供一种视觉融合激光雷达SLAM定位导航的方法，用于无人机定位导航，包括以下步骤：
(1)、获取三维激光SLAM系统生成的第一融合位姿和运动信息；
(2)、获取视觉SLAM系统生成的第二融合位姿和运动信息；
(3)、比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，将第三比较误差与系统标定误差进行比较；
(4)、当所述第三比较误差大于系统标定误差时，切换到视觉SLAM系统，用于无人机的导航。
在上述实施例中，进一步包括：
(5)、步骤(4)中，进一步为：当所述第三比较误差大于系统标定误差时，切换到视觉SLAM系统，获取第三融合位姿和运动信息，供无人机定位、导航；或当所述第三比较误差小于或等于系统标定误差时，获取第二融合位姿和运动信息，继续采用激光SLAM系统供无人机定位导航；
(6)、获取三维激光SLAM系统的地图更新信息和/或获取视觉SLAM系统的地图更新信息；
(7)、获取用户指定的位置信息、并根据用户指定的位置信息、地图更新信息、第三融合位姿和运动信息或第二融合位姿和运动信息，生成最佳航路，对无人机进行即时定位和自主导航，并显示无人机的位置及控制无人机的飞行；
本发明实施例中，所述系统标定误差为系统预设值。
进一步的，在本发明实施例中，所述步骤(1)中的获取三维激光SLAM系统生成第一融合位姿和运动信息还包括以下步骤，请同时参阅图2：
(1-1)、获取第一IMU单元采集当前帧的第一IMU位姿和运动信息，获取三维激光雷达采集当前帧的点云数据信息；
(1-2)、对采集到的点云数据进行滤波和预处理；
(1-3)、对滤波处理后的点云数据进行解算，得到当前帧的激光雷达位姿和运动信息和当前帧在激光系统中的周围环境的局部地图，进行激光系统中的地图更新；
(1-4)、融合全部的局部地图生成全局地图；
(1-5)、结合上一帧的激光雷达位姿和运动信息与当前帧的激光雷达位姿和运动信息生成第一运动轨迹；
(1-6)、结合上一帧的第一IMU位姿和运动信息与当前帧的第一IMU位姿和运动信息生成第二运动轨迹；
(1-7)、比较第一运动轨迹和第二运动轨迹，得到第一比较误差，将第一比较误差与激光SLAM标定误差进行比较，其中所述激光SLAM标定误差为系统预设值；
(1-8)、根据第一比较误差与激光SLAM标定误差的比较结果融合第一IMU与激光雷达的位姿和运动信息，并结合当前更新的全局地图模型，得到当前帧的第一融合位姿和运动信息；
(1-9)、输出第一融合位姿和运动信息和全局地图信息。
进一步的，本发明实施例中步骤(6)中获取三维激光SLAM系统的地图更新信息为根据对滤波处理后的点云数据进行解算，当前帧在激光系统中的周围环境的局部地图信息，根据该局部地图信息进行激光SLAM系统中的地图更新。
进一步的，本发明实施例中还包括步骤(1-8)：融合全部的局部地图信息生成全局地图。
进一步的，本发明实施例中步骤(1-7)中，获得当前帧的第一融合位姿和运动信息包括结合当前更新的全局地图信息输出第一融合位姿和运动信息；
进一步的，本发明实施例中，所述步骤(1-2)中的对采集到的点云数据进行滤波和预处理还包括以下步骤：
(1-2-1)、对采集到的点云数据进行滤波处理；
(1-2-2)、对滤波后的点云数据进行预处理；
(1-2-3)、根据预处理后的点云数据更新临时局部地图。
所述步骤(1-6)中的将第一比较误差与激光SLAM标定误差进行比较还包括以下步骤：
(1-6-1)、若第一比较误差大于激光SLAM标定误差，则将当前帧的第一IMU位姿和运动信息替换为当前帧的激光雷达位姿和运动信息；
(1-6-2)、若第一比较误差小于或等于激光SLAM标定误差，则存储当前帧的第一IMU位姿和运动信息。
所述步骤(2)中的视觉SLAM系统运行生成第二融合位姿和运动信息如图3所示，还包括以下步骤：
(2-1)、获取第二IMU单元采集当前帧的第二IMU位姿和运动信息，获取摄像头采集当前帧的图像像素信息；本实施例中为双目摄像头。
(2-2)、对所述当前帧的图像像素信息进行前端特征匹配跟踪；
(2-3)、对特征进行特征点三角化，并解算得到当前帧的摄像头位姿和运动信息和当前帧在视觉系统中的周围环境的局部地图，并进行视觉系统中的地图更新；
(2-4)、结合上一帧的摄像头位姿和运动信息与当前帧的摄像头位姿和运动信息生成第三运动轨迹；
(2-5)、结合上一帧第二IMU位姿和运动信息与当前帧的第二IMU位姿和运动信息生成第四运动轨迹；
(2-6)、比较第三运动轨迹和第四运动轨迹，得到第二比较误差，将第二比较误差与视觉SLAM标定误差进行比较，其中所述视觉SLAM标定误差为系统预设值；
(2-7)、根据第二比较误差与视觉SLAM标定误差的比较结果融合第二IMU与摄像头位姿和运动信息,并结合视觉系统地图信息得到当前帧在视觉系统地图中的第二融合位姿和运动信息。
进一步的，本发明实施例中，所述步骤(2-6)中的将第二比较误差与视觉SLAM标定误差进行比较还包括以下步骤：
(2-6-1)、若第二比较误差大于视觉SLAM标定误差，则将当前帧的第二IMU位姿和运动信息替换为当前帧的摄像头位姿和运动信息；
(2-6-2)、若第二比较误差小于或等于视觉SLAM标定误差，则存储当前帧的第二IMU位姿和运动信息；
进一步的，本发明实施例中，所述步骤(3)中的将第三比较误差与系统标定误差进行比较还包括以下步骤：
(3-1)、若第三比较误差大于系统标定误差，进入步骤(4)，切换到视觉SLAM系统，并结合GPS接收信号得到带有地球坐标系的第二融合位姿和运动信息；
(3-2)、若第三比较误差小于或等于系统标定误差，则根据权重算法计算三维激光SLAM系统与视觉SLAM系统的使用权重，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第三融合位姿和运动信息。
所述步骤(3-1)中的若第三比较误差大于系统标定误差还包括以下步骤：
(3-1-1)、将第三比较误差与系统切换误差进行比较；
(3-1-2)、若第三比较误差小于或等于系统切换误差，则将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并存储到视觉SLAM系统中；
(3-1-3)、若第三比较误差大于系统切换误差，进入步骤(4)切换到视觉SLAM系统，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第二融合位姿和运动信息。
另一方面，本发明实施例还提供一种基于视觉融合激光雷达SLAM定位导航的无人机系统图4为本发明实施例中的基于视觉融合三维激光雷达SLAM定位导航的无人机系统模块图包括三维激光SLAM系统100、视觉SLAM系统200、飞行控制单元300和系统处理单元400，其中，所述三维激光SLAM系统100用于无人机定位导航。三维激光SLAM系统100提供第一融合位姿和运动信息，所述视觉SLAM系统200用于优化三维激光SLAM系统并在三维激光SLAM系统不适用时提供无人机导航的作用。所述视觉SLAM系统200提供第二融合位姿和运动信息，所述飞行控制单元300用于控制无人机，所述系统处理单元400用于处理各个系统的数据，所述系统处理单元400比较第一融合位姿和运动信息与第二融合位姿和运动信息，获得第三比较误差，将第三比较误差与系统预设的系统标定误差进行比较；当所述第三比较误差大于系统预设的系统标定误差时，将当前的导航系统切换到视觉SLAM系统，用于无人机的定位导航。其中，所述系统处理单元400包括系统标定模块410、系统切换模块420和系统融合模块430。其中，
所述系统标定模块410用于实时修正视觉SLAM系统200的误差，所述系统标定模块比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，并将第三比较误差与预设好的系统标定误差进行比较；
所述系统切换模块420用于视觉SLAM系统200与三维激光SLAM系统100之间的切换；
所述系统融合模块430用于融合视觉SLAM系统200和三维激光SLAM系统100的位姿和运动信息；
当第三比较误差大于系统标定误差，则将第三比较误差与预设好的系统切换误差进行比较；若第三比较误差小于或等于系统标定误差，则将第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块；
若第三比较误差小于或等于系统切换误差，则系统标定模块将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块；
若第三比较误差大于系统切换误差，则把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统切换模块，系统切换模块仅使用视觉SLAM系统输出的第二融合位姿和运动信息，以定位导航无人机；
系统融合模块根据系统标定模块发送的结果，依据权重算法计算三维激光SLAM系统与视觉SLAM系统的使用权重，融合得出融合位姿运动信息。
进一步的，在本发明实施例中，还包括多传感器融合模块440，根据系统融合模块发送的融合位姿运动信息、更新的全局三维地图信息进行融合，当有外部GPS和/或RTK信号时，同时融合GPS和/或RTK信息，获得带有地球坐标系的融合位姿和运动信息。
进一步的，在本发明实施例中，所述三维激光SLAM系统100还包括激光信息采集单元110和激光信息处理单元120，其中所述激光信息采集单元110用于为三维激光SLAM系统100采集信息，所述激光信息处理单元120用于处理激光信息采集单元采集到的信息。
所述激光信息采集单元110还包括第一IMU单元111和至少一个激光雷达112，其中，
所述第一IMU单元111用于为三维激光SLAM系统100提供预测位姿和运动信息；
所述激光雷达112用于采集周围环境的点云数据。
所述激光雷达112为多线三维激光扫描雷达。
所述为三维激光SLAM系统100提供预测位姿和运动信息还包括第一IMU单元111先采集里程信息，通过无人机惯性里程计运动学的模型，将里程信息转化为无人机位姿变化信息，送入贝叶斯滤波器初步计算出预测位姿和运动信息。
进一步的，在本发明实施例中，所述激光信息处理单元120还包括点云处理模块130、雷达数据处理模块140、雷达系统存储模块150、雷达系统地图更新模块160和全局地图融合模块170，其中，
所述点云处理模块130用于处理点云数据；
所述雷达数据处理模块140用于处理雷达系统的数据；
所述雷达系统存储模块150用于存储激光信息处理单元产生的信息；
所述雷达系统地图更新模块160用于实时更新三维激光SLAM系统100的地图信息；
所述全局地图融合模块170用于融合全部的局部地图信息生成全局地图信息，使无人机在所述全局地图信息中规划航行路径。
所述点云处理模块130还包括滤波模块131和预处理模块132，其中，
所述滤波模块131用于对激光雷达112采集到的点云数据滤波处理；
所述预处理模块132用于对滤波之后的点云数据进行初步处理，得到临时局部地图信息。
进一步的，在本发明实施例中，所述滤波处理包括对点云数据进行降噪、去除异常点和减少多余的点云数据量。
所述雷达数据处理模块140还包括雷达解算模块141、雷达系统轨迹生成模块142、雷达系统轨迹比较模块143、雷达系统标定模块144、激光雷达融合模块145，其中，
所述雷达解算模块141用于对滤波之后的点云数据进行解算，得到对应的激光雷达位姿和运动信息和当前帧周围环境的局部地图信息；
所述雷达系统轨迹生成模块142用于结合连续两帧的位姿和运动信息生成无人机在连续两帧的运动轨迹；
所述雷达系统轨迹比较模块143用于比较运动轨迹和误差；
所述雷达系统标定模块144用于实时修正第一IMU单元111误差；
所述激光雷达融合模块145用于融合第一IMU单元111与激光雷达输出的位姿和运动信息。
进一步的，在本发明实施例中，所述视觉SLAM系统200还包括视觉信息采集单元210和视觉信息处理单元220，其中所述视觉信息采集单元210用于为视觉SLAM系统200采集信息，所述视觉信息处理单元220用于处理视觉信息采集单元采集到的信息。
所述视觉信息采集单元210还包括第二IMU单元211和至少一组双目摄像头212，其中，
所述第二IMU单元211用于为视觉SLAM系统200提供预测位姿和运动信息；
所述至少一组双目摄像头212用于拍摄周围环境的图像像素信息。
所述为视觉SLAM系统200提供预测位姿和运动信息还包括，第二IMU单元211先采集里程信息，通过无人机惯性里程计运动学的模型，将里程信息转化为无人机位姿变化信息，送入贝叶斯滤波器初步计算出预测位姿和运动信息。
所述视觉信息处理单元220包括特征处理模块230、视觉数据处理模块240、视觉系统存储模块250和视觉系统地图更新模块260，其中，
所述特征处理模块230用于处理图像特征；
所述视觉数据处理模块240用于处理视觉系统的数据；
所述视觉系统存储模块250用于存储视觉信息处理单元产生的信息；
所述视觉系统地图更新模块260用于实时更新视觉SLAM系统200的地图信息。
所述特征处理模块230还包括特征匹配跟踪模块231、特征点处理模块232，其中，
所述特征匹配跟踪模块231用于实时跟踪双目摄像头212采集到的图像像素信息并将连续两帧的图像像素信息进行匹配特征；
所述特征点处理模块232用于对特征进行特征点处理化。
所述特征点处理化的方法如下：
利用一个6维向量多关键帧匹配点三角化的特征点深度获取方法来表示一个环境特征点的三维位置，包括无人机的摄像机的位置以及无人机所处的环境特征点方向及深度信息；不断地进行环境更新，当特征估计协方差小于某一设定阈值时，将无人机所处的环境特征点的6维表示转换为3维的欧式坐标，利用在同一帧图像建立多个特征点，将属于同一帧特征点的表示形式缩减为一个摄像机姿态加多个深度的形式，以有效降低无人机系统状态的长度。
所述视觉数据处理模块240还包括视觉解算模块241、视觉系统轨迹生成模块242、视觉系统轨迹比较模块243、视觉系统标定模块244、视觉双目融合模块245，其中，
所述视觉解算模块241用于将处理后的环境特征点信息解算为摄像头位姿和运动信息和当前帧周围环境的局部地图信息；
所述视觉系统轨迹生成模块242用于结合连续两帧的位姿和运动信息生成无人机在连续两帧的运动轨迹；
所述视觉系统轨迹比较模块243用于比较运动轨迹和误差；
所述视觉系统标定模块244用于实时修正第二IMU单元211误差；
所述视觉双目融合模块245用于融合第二IMU单元211与双目摄像头212输出的位姿和运动信息。
所述飞行控制单元300包括全局规划模块310、局部规划模块320和底层控制模块330，其中，
所述全局规划模块310用于规划全局地图的航行最佳路径；
所述局部规划模块320用于对预处理后得到的实时的局部地图信息规划全局最佳路径；
所述底层控制模块330用于对无人机进行控制分配。
在本实施例中的一种基于视觉融合三维激光雷达SLAM定位技术的无人机系统总体运行情况如下：
三维激光SLAM系统100和视觉SLAM系统200各自运行，并分别生成第一融合位姿和运动信息与第二融合位姿和运动信息输出到系统标定模块410中；
系统标定模块410比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，并将第三比较误差与预设好的系统标定误差进行比较；
若第三比较误差大于系统标定误差，则将第三比较误差与预设好的系统切换误差进行比较；若第三比较误差小于或等于系统标定误差，则将第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块430；
若第三比较误差小于或等于系统切换误差，则系统标定模块410将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块430；
若第三比较误差大于系统切换误差，则把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统切换模块420，系统切换模块420仅使用视觉SLAM系统200输出的第二融合位姿和运动信息定位无人机的位置；
多传感器融合模块440根据系统融合模块发送的融合位姿运动信息、更新的全局三维地图信息进行融合，当有外部GPS和/或RTK信号时，同时融合GPS和/或RTK信息，获得带有地球坐标系的第三融合位姿和运动信息；
全局规划模块310根据用户指定的位置、地图更新信息和第三融合位姿和运动信息或第二融合位姿和运动信息规划无人机到达目标点的最佳航行路径，即路线最短且行进过程中无障碍的航行路径，并将全部数据输出到局部规划模块320；
局部规划模块320根据激光信息处理单元中的预处理模块132发送的实时的局部地图信息与全局规划模块310输出的数据规划出局部最佳航行路径，并与最佳航行路径比较取得最优路径，将最优路径输出到底层控制模块330；
底层控制模块330根据最优路径对无人机进行控制分配，控制无人机的飞行速度、角度和方位等信息；
无人机接收到指令并开始航行。
所述三维激光SLAM系统100运行情况如下：
第一IMU单元111获取三维激光SLAM系统100的里程信息，通过无人机惯性里程计运动学的模型，将里程信息转化为无人机位姿变化信息，送入贝叶斯滤波器初步计算出当前帧的第一IMU位姿和运动信息并输出给雷达系统轨迹生成模块142，多线三维激光扫描雷达采集当前帧的点云数据信息并输出给滤波模块131；
滤波模块131对采集到的点云数据进行降噪、去除异常点和减少多余的点云数据量等滤波处理，并将滤波后的点云数据分别输出给雷达解算模块141和预处理模块132；
预处理模块132对滤波后的点云数据进行初步处理得到临时局部地图信息并输出给局部规划模块320；
雷达解算模块141对滤波之后的点云数据解算，得到当前帧的激光雷达位姿和运动信息和当前帧在激光系统中的周围环境的局部地图信息，并将当前帧的激光雷达位姿和运动信息输出到雷达系统轨迹生成模块142，将当前帧在激光系统中的周围环境的局部地图信息输出到激光系统地图更新模块160；
激光系统地图更新模块160实时更新连续两帧的局部地图信息并输出给全局地图融合模块170；
全局地图融合模块170融合全部的局部地图信息，生成全局地图信息，当无人机下次在同一位置起飞时，可以直接使用该全局地图信息，将全局地图信息输出给激光雷达融合模块145和全局规划模块310；
雷达系统轨迹生成模块142结合上一帧的激光雷达位姿和运动信息与当前帧的激光雷达位姿和运动信息生成第一运动轨迹，结合上一帧的第一IMU位姿和运动信息与当前帧的第一IMU位姿和运动信息生成第二运动轨迹，并将第一运动轨迹和第二运动轨迹输出给雷达系统轨迹比较模块143；
雷达系统轨迹比较模块143比较第一运动轨迹和第二运动轨迹，得到第一比较误差，将第一比较误差与预设好的激光SLAM标定误差进行比较，并将比较结果输出给雷达系统标定模块144；
若第一比较误差大于激光SLAM标定误差，则雷达系统标定模块144将当前帧的第一IMU位姿和运动信息替换为当前帧的激光雷达位姿和运动信息，若第一比较误差小于或等于激光SLAM标定误差，则存储当前帧的第一IMU位姿和运动信息；
将第一IMU与激光雷达的位姿和运动信息输出给激光雷达融合模块145，激光雷达融合模块145根据融合算法融合第一IMU与激光雷达的位姿和运动信息并结合当前更新的全局地图模型，得到当前帧的第一融合位姿和运动信息，将当前帧的第一融合位姿和运动信息输出给系统标定模块410。
所述视觉SLAM系统200运行情况如下：
第二IMU单元211获取视觉SLAM系统200的里程信息，通过无人机惯性里程计运动学的模型，将里程信息转化为无人机位姿变化信息，送入贝叶斯滤波器初步计算出当前帧的第二IMU位姿和运动信息并输出给视觉系统轨迹生成模块242，双目摄像头212采集当前帧的图像像素并输出给特征匹配跟踪模块231；
特征匹配跟踪模块231实时跟踪双目摄像头212采集到的图像像素信息并将连续两帧的图像像素信息进行匹配特征并把匹配到的特征点输出给特征点处理模块232；
特征点处理模块232进行特征点三角化，具体包括，利用一个6维向量多关键帧匹配点三角化的特征点深度获取方法来表示一个环境特征点的三维位置，包括无人机的摄像机的位置以及无人机所处的环境特征点方向及深度信息；不断地进行环境更新，当特征估计协方差小于某一设定阈值时，将无人机所处的环境特征点的6维表示转换为3维的欧式坐标，利用在同一帧图像建立多个特征点，将属于同一帧特征点的表示形式缩减为一个摄像机姿态加多个深度的形式，以有效降低无人机系统状态的长度。将处理后的环境特征点信息输出给视觉解算模块241；
视觉解算模块241对处理后的环境特征点信息解算，得到当前帧的摄像头位姿和运动信息和当前帧在视觉系统中的周围环境的局部地图，并将当前帧的摄像头位姿和运动信息输出到视觉系统轨迹生成模块242，将当前帧在视觉系统中的周围环境的局部地图信息输出到视觉系统地图更新模块260；
视觉系统地图更新模块260实时更新视觉SLAM系统200的地图信息并将地图信息输出给视觉双目融合模块245；
视觉系统轨迹生成模块242结合上一帧的摄像头位姿和运动信息与当前帧的摄像头位姿和运动信息生成第三运动轨迹，结合上一帧第二IMU位姿和运动信息与当前帧的第二IMU位姿和运动信息生成第四运动轨迹，并将第三运动轨迹和第四运动轨迹输出给视觉系统轨迹比较模块243；
视觉系统轨迹比较模块243比较第三运动轨迹和第四运动轨迹，得到第二比较误差，将第二比较误差与预设好的视觉SLAM标定误差进行比较，并将比较结果输出给视觉系统标定模块244；
若第二比较误差大于视觉SLAM标定误差，则视觉系统标定模块244将当前帧的第二IMU位姿和运动信息替换为当前帧的摄像头位姿和运动信息，若第二比较误差小于或等于视觉SLAM标定误差，则存储当前帧的第二IMU位姿和运动信息；
将第二IMU与双目的位姿和运动信息输出给视觉双目融合模块245，视觉双目融合模块245根据融合算法融合第二IMU与摄像头位姿和运动信息,并结合视觉系统地图信息得到当前帧在视觉系统地图中的第二融合位姿和运动信息，将当前帧的第二融合位姿和运动信息输出给系统标定模块410。
本发明提供的方法和系统，基于系统融合模块同时使用了视觉系统和三维激光系统，并且当其中一个系统的导航定位出现偏差时，能够通过系统切换模块无缝切换到另外一个系统，提高了无人机的环境适应性和稳定性，同时通过系统处理单元实时对双系统的数据进行比较修正，使系统定位导航更加精确，可以应用到更加广泛的场景。
同时，本发明的激光SLAM系统采用激光雷达+惯性里程计实时标定策略实现里程计误差的实时修正，有效提高了SLAM算法的无人机定位与构建地图精度。
另外，相对于传统方法，多关键帧匹配点三角化的特征点深度获取方法无需手动提取特征或光流图像、无需构建特征描述子、无需帧间特征匹配，更无需进行复杂的几何运算，可实现全新无人机不断深度学习提高系统整体可靠性。
以上对本发明所提供的一种视觉融合三维激光雷达SLAM定位方法及其无人机系统进行了详细介绍，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种视觉融合激光雷达SLAM定位导航的方法，用于无人机定位导航，其特征在于，包括以下步骤：
(1)获取三维激光SLAM系统生成的第一融合位姿和运动信息；
(2)获取视觉SLAM系统生成的第二融合位姿和运动信息；
(3)比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，将第三比较误差与系统标定误差进行比较；
(4)当所述第三比较误差大于系统标定误差时，将当前的导航系统切换到视觉SLAM系统，用于无人机的导航。
2.根据权利要求1所述的视觉融合激光雷达SLAM定位导航的方法，其特征在于，还包括如下步骤：
(5)步骤(4)中，进一步为：当所述第三比较误差大于系统标定误差时，切换到视觉SLAM系统，获取第三融合位姿和运动信息，供无人机定位、导航；或当所述第三比较误差小于或等于系统标定误差时，获取第二融合位姿和运动信息，继续采用激光SLAM系统供无人机定位导航；
(6)获取三维激光SLAM系统的地图更新信息和/或获取视觉SLAM系统的地图更新信息；
(7)获取用户指定的位置信息、并根据用户指定的位置信息、地图更新信息、第三融合位姿和运动信息或第二融合位姿和运动信息，生成最佳航路，对无人机进行即时定位和自主导航，并显示无人机的位置及控制无人机的飞行。
3.根据权利要求2所述的视觉融合激光雷达SLAM定位导航的方法，其特征在于，所述步骤(1)中的获取三维激光SLAM系统生成第一融合位姿和运动信息还包括以下步骤：
(1-1)获取第一IMU单元采集当前帧的第一IMU位姿和运动信息，获取三维激光雷达采集当前帧的点云数据信息；
(1-2)对采集到的点云数据进行滤波和预处理；
(1-3)对滤波处理后的点云数据进行解算，得到当前帧的激光雷达位姿和运动信息；
(1-4)结合上一帧的激光雷达位姿和运动信息与当前帧的激光雷达位姿和运动信息生成第一运动轨迹；
(1-5)结合上一帧的第一IMU位姿和运动信息与当前帧的第一IMU位姿和运动信息生成第二运动轨迹；
(1-6)比较第一运动轨迹和第二运动轨迹，得到第一比较误差，将第一比较误差与激光SLAM标定误差进行比较，其中所述激光SLAM标定误差为系统预设值；
(1-7)根据第一比较误差与激光SLAM标定误差的比较结果融合第一IMU与激光雷达的位姿和运动信息，得到当前帧的第一融合位姿和运动信息。
4.根据权利要求2所述的视觉融合激光雷达SLAM定位导航的方法，其特征在于，所述步骤(2)中的视觉SLAM系统运行生成第二融合位姿和运动信息还包括以下步骤：
(2-1)、获取第二IMU单元采集当前帧的第二IMU位姿和运动信息，获取摄像头采集当前帧的图像像素信息；
(2-2)、对所述当前帧的图像像素信息进行前端特征匹配跟踪；
(2-3)、对特征进行特征点三角化，并解算得到当前帧的摄像头位姿和运动信息和当前帧在视觉系统中的周围环境的局部地图，并进行视觉系统中的地图更新；
(2-4)、结合上一帧的摄像头位姿和运动信息与当前帧的摄像头位姿和运动信息生成第三运动轨迹；
(2-5)、结合上一帧第二IMU位姿和运动信息与当前帧的第二IMU位姿和运动信息生成第四运动轨迹；
(2-6)、比较第三运动轨迹和第四运动轨迹，得到第二比较误差，将第二比较误差与视觉SLAM标定误差进行比较，其中所述视觉SLAM标定误差为系统预设值；
(2-7)、根据第二比较误差与视觉SLAM标定误差的比较结果融合第二IMU与摄像头位姿和运动信息,并结合视觉系统地图信息得到当前帧在视觉系统地图中的第二融合位姿和运动信息。
5.根据权利要求4所述的视觉融合激光雷达SLAM定位导航的方法，其特征在于，所述步骤(2-6)中的将第二比较误差与视觉SLAM标定误差进行比较还包括以下步骤：
(2-6-1)若第二比较误差大于视觉SLAM标定误差，则将当前帧的第二IMU位姿和运动信息替换为当前帧的摄像头位姿和运动信息；
(2-6-2)若第二比较误差小于或等于视觉SLAM标定误差，则存储当前帧的第二IMU位姿和运动信息。
6.根据权利要求5所述的视觉融合激光雷达SLAM定位导航的方法，其特征在于，所述步骤(3)中的将第三比较误差与系统标定误差进行比较还包括以下步骤：
(3-1)若第三比较误差大于系统标定误差，进入步骤(4)，切换到视觉SLAM系统，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第二融合位姿和运动信息；
(3-2)若第三比较误差小于或等于系统标定误差，则根据权重算法计算三维激光SLAM系统与视觉SLAM系统的使用权重，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第三融合位姿和运动信息；
所述步骤(3-1)中的若第三比较误差大于系统标定误差还包括以下步骤：
(3-1-1)将第三比较误差与系统切换误差进行比较；
(3-1-2)若第三比较误差小于或等于系统切换误差，则将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并存储到视觉SLAM系统中；
(3-1-3)若第三比较误差大于系统切换误差，进入步骤(4)切换到视觉SLAM系统，当有外部GPS信号时，结合GPS接收信号得到带有地球坐标系的第二融合位姿和运动信息。
7.一种视觉融合激光雷达SLAM定位导航的无人机系统，其特征在于，包括用于无人机定位、导航的三维激光SLAM系统(100)、用于无人机定位、导航的视觉SLAM系统(200)、用于控制无人机的飞行控制单元(300)和用于处理各个系统的数据的系统处理单元(400)，其中，所述三维激光SLAM系统(100)提供第一融合位姿和运动信息，所述视觉SLAM系统(200)提供第二融合位姿和运动信息，所述系统处理单元(400)比较第一融合位姿和运动信息与第二融合位姿和运动信息，获得第三比较误差，将第三比较误差与系统预设的系统标定误差进行比较；当所述第三比较误差大于系统预设的系统标定误差时，将当前的导航系统切换到视觉SLAM系统，用于无人机的定位导航。
8.如权利要求7所述的视觉融合激光雷达SLAM定位导航的无人机系统，其特征在于，所述的系统处理单元(400)包括系统标定模块(410)、系统切换模块(420)、系统融合模块(430)；
所述系统标定模块(410)用于实时修正视觉SLAM系统(200)的误差；
所述系统标定模块比较第一融合位姿和运动信息与第二融合位姿和运动信息，得到第三比较误差，并将第三比较误差与预设好的系统标定误差进行比较；所述系统切换模块(420)用于视觉SLAM系统(200)与三维激光SLAM系统(100)之间的切换；所述系统融合模块(430)用于融合视觉SLAM系统(200)和三维激光SLAM系统(100)的位姿和运动信息；
当第三比较误差大于系统标定误差，则将第三比较误差与预设好的系统切换误差进行比较；若第三比较误差小于或等于系统标定误差，则将第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块；
若第三比较误差小于或等于系统切换误差，则系统标定模块将第二融合位姿和运动信息替换为第一融合位姿和运动信息，并把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统融合模块；
若第三比较误差大于系统切换误差，则把第一融合位姿和运动信息与第二融合位姿和运动信息输出给系统切换模块，系统切换模块仅使用视觉SLAM系统输出的第二融合位姿和运动信息，以定位导航无人机；
系统融合模块根据系统标定模块发送的结果，依据权重算法计算三维激光SLAM系统与视觉SLAM系统的使用权重，融合得出融合位姿运动信息。
9.如权利要求8所述的视觉融合激光雷达SLAM定位导航的无人机系统，其特征在于，所述的激光SLAM系统还包括用于根据更新的第一融合位姿和运动信息更新其内的地图信息的激光系统地图更新模块，以及融合所有局部激光SLAM系统的局部地图信息生成全局三维地图的全局地图融合模块；
所述的视觉SLAM系统还包括视觉系统地图更新模块，根据更新的第二融合位姿和运动信息更新其内的地图信息。
10.如权利要求9所述的视觉融合激光雷达SLAM定位导航的无人机系统，其特征在于，还包括多传感器融合模块(440)，根据系统融合模块发送的融合位姿运动信息、更新的全局三维地图信息进行融合，当有外部GPS和/或RTK信号时，同时融合GPS和/或RTK信息，获得带有地球坐标系的融合位姿信息。
CN202210929221.1A 2022-08-03 2022-08-03 视觉融合激光雷达slam定位导航的方法及其无人机系统 Pending CN117554989A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210929221.1A CN117554989A (zh) 2022-08-03 2022-08-03 视觉融合激光雷达slam定位导航的方法及其无人机系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210929221.1A CN117554989A (zh) 2022-08-03 2022-08-03 视觉融合激光雷达slam定位导航的方法及其无人机系统

Publications (1)

Publication Number Publication Date
CN117554989A true CN117554989A (zh) 2024-02-13

Family

ID=89822054

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210929221.1A Pending CN117554989A (zh) 2022-08-03 2022-08-03 视觉融合激光雷达slam定位导航的方法及其无人机系统

Country Status (1)

Country Link
CN (1) CN117554989A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117906598A (zh) * 2024-03-19 2024-04-19 深圳市其域创新科技有限公司 无人机设备的定位方法、装置、计算机设备和存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117906598A (zh) * 2024-03-19 2024-04-19 深圳市其域创新科技有限公司 无人机设备的定位方法、装置、计算机设备和存储介质
CN117906598B (zh) * 2024-03-19 2024-07-09 深圳市其域创新科技有限公司 无人机设备的定位方法、装置、计算机设备和存储介质

Similar Documents

Publication Publication Date Title
CN106908775B (zh) 一种基于激光反射强度的无人车实时定位方法
CN110969655B (zh) 用于检测车位的方法、装置、设备、存储介质以及车辆
CN112197770B (zh) 一种机器人的定位方法及其定位装置
CN109211241B (zh) 基于视觉slam的无人机自主定位方法
Kanade et al. Real-time and 3D vision for autonomous small and micro air vehicles
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN112379681B (zh) 无人机避障飞行方法、装置及无人机
CN112596071B (zh) 无人机自主定位方法、装置及无人机
AU2014203381A1 (en) Systems and methods for autonomous landing using a three dimensional evidence grid
CN112378397B (zh) 无人机跟踪目标的方法、装置及无人机
WO2019208101A1 (ja) 位置推定装置
CN112380933B (zh) 无人机识别目标的方法、装置及无人机
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN115993825A (zh) 一种基于空地协同的无人车集群控制系统
CN115049910A (zh) 一种基于双目视觉里程计的足式机器人建图与导航方法
CN117554989A (zh) 视觉融合激光雷达slam定位导航的方法及其无人机系统
Bednář et al. Deployment of reliable visual inertial odometry approaches for unmanned aerial vehicles in real-world environment
Song et al. Real-time localization measure and perception detection using multi-sensor fusion for Automated Guided Vehicles
CN112731918B (zh) 一种基于深度学习检测跟踪的地面无人平台自主跟随系统
Sabatini et al. Low-cost vision sensors and integrated systems for unmanned aerial vehicle navigation
CN117572459A (zh) 一种自动切换导航系统的无人机
JP7302966B2 (ja) 移動体
CN114459474A (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
Hu et al. An Enhanced-LiDAR/UWB/INS Integrated Positioning Methodology for Unmanned Ground Vehicle in Sparse Environments
CN117572879A (zh) 基于激光雷达slam定位导航的无人机

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