CN116184430B - 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 - Google Patents
一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 Download PDFInfo
- Publication number
- CN116184430B CN116184430B CN202310144825.XA CN202310144825A CN116184430B CN 116184430 B CN116184430 B CN 116184430B CN 202310144825 A CN202310144825 A CN 202310144825A CN 116184430 B CN116184430 B CN 116184430B
- Authority
- CN
- China
- Prior art keywords
- pose
- camera
- laser radar
- odometer
- point
- 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
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 34
- 230000004927 fusion Effects 0.000 claims abstract description 46
- 230000008878 coupling Effects 0.000 claims abstract description 30
- 238000010168 coupling process Methods 0.000 claims abstract description 30
- 238000005859 coupling reaction Methods 0.000 claims abstract description 30
- 230000005484 gravity Effects 0.000 claims description 24
- 238000000034 method Methods 0.000 claims description 21
- 230000001133 acceleration Effects 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 4
- 238000003384 imaging method Methods 0.000 claims description 2
- 239000000284 extract Substances 0.000 claims 2
- 230000007547 defect Effects 0.000 description 3
- 230000002708 enhancing effect Effects 0.000 description 3
- 238000007499 fusion processing Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
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)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明提供了一种激光雷达、相机、惯性测量单元融合的位姿估计算法，通过构建独立运行的紧耦合融合LCO、LIO、CIO，然后三个里程计之间松耦合融合的算法架构，既包含紧耦合的高精度优势，又包含松耦合的强鲁棒性优势，能提升多传感器的位姿估计能力。
Description
技术领域
本发明涉及多传感器融合与位姿估计领域，尤其涉及一种激光雷达、可见光相机、惯性测量单元融合的位姿估计方法。
背景技术
传统的多传感器融合与位姿估计技术往往难以兼顾精度与鲁棒性。传统方法分为两类：(1)松耦合融合。多传感器之间互相独立，单独构建残差函数，优化该函数解算出位姿结果，然后加权平均，其优点在于鲁棒性高。当某一传感器原始数据缺失或求解位姿误差较大时，该方法融合后的位姿依然是可接受的结果。其缺点在于精度较差，测量精度低的传感器会拉低测量精度高的传感器在位姿求解上的效果。(2)紧耦合融合。多传感器原始数据之联合构建残差函数，优化该函数解算出一个位姿结果。其优点在于联合优化是非线性直接处理观测数据，因此优化残差后的位姿精度高。其缺点在于残差项多，导致效率低，某一个传感器数据缺失或误差较大时，残差函数会出错甚至失效，从而无法求解有效位姿，鲁棒性较差。因此为克服两种方法的缺点，吸收其优点，本发明提出了一种激光雷达、可见光相机、惯性测量单元两两之间紧耦合融合，构建激光雷达相机里程计(LCO)、激光雷达惯性里程计(LIO)、相机惯性里程计(CIO)，然后三个里程计之间松耦合融合的算法架构。该算法既包含紧耦合的高精度优势，又包含松耦合的强鲁棒性优势，能提升多传感器的位姿估计能力。
美国专利文献US20100017128A1中公开了一种用于确定车辆动力学的***和方法，采用雷达、激光雷达和或相机信号实时校正车辆的动态估计。
中国专利文献CN101241011A中记载了一种激光雷达平台上高精度定位、定姿的装置和方法，参见图1，设置可量测照相单元、惯性测量单元、高精度位置姿态测量及解算单元、空中位置测量单元、激光扫描单元、三维坐标信息获取单元等等，在激光雷达平台的飞行轨迹上通过可量测照相获得航空数码影像，对获得的航空数码影像进行相对定向计算，获得激光雷达平台在工作航线上的低频高精度姿态参数值，通过惯性测量单元获得激光雷达平台的高频低精度姿态参数，利用所述相对定向获得的低频高精度姿态参数值对所述高频低精度姿态参数进行修正处理，获得激光雷达平台的高精度姿态参数信息。从而可以降低高精度位置与姿态测量***的成本，提高高精度位置与姿态测量***的精度，满足Lidar***高精度测量的要求。
中国专利文献CN110501712B中公开了一种用于确定位置姿态数据的方法，基于由第一激光雷达扫描预定空间而获得的扫描点云数据，确定与预定空间相关联的参考点云数据，预定空间中布置有用于发送模拟定位信号的发送设备和基于模拟定位信号确定定位误差的误差确定设备，基于定位误差、由车辆在预定空间内接收到的模拟定位信号以及车辆的惯性测量单元数据，确定针对车辆的位置姿态数据，基于参考点云数据、车辆的位置姿态数据和由车辆上的第二激光雷达获得的测试点云数据，确定第二激光雷达的位置姿态数据。由此降低对GNNS的环境的依赖，节省了激光雷达的标定时间。
中国专利文献CN105318876A中记载了一种惯性里程计组合高精度姿态测量方法，通过以航位推算结果为基准，进行组合卡尔曼滤波计算，对水平失准角和水平陀螺漂移进行估计并修正，提高姿态精度。
中国专利文献CN114646993A中记载了一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法，利用不同传感器数据源之间的互补性，利用本地数据源提高GNSS的全局定位精度，利用GNSS全局数据源消除本地的累积误差，整体提高GNSS的性能及鲁棒性。
然而，现有技术中尚不存在一种耦合融合LCO、LIO、CIO的位置姿势(下文简称位姿)估算方法。
发明内容
鉴于此，本发明提供一种激光雷达、相机、惯性测量单元融合的位姿估计算法，通过构建独立运行的紧耦合融合LCO、LIO、CIO，然后三个里程计之间松耦合融合的算法架构，既包含紧耦合的高精度优势，又包含松耦合的强鲁棒性优势，能提升多传感器的位姿估计能力。
具体而言，本发明的目的是通过以下技术方案来实现的：
根据本发明的一个方面，提供一种利用计算机实施的激光雷达、相机、惯性测量单元融合的位姿估计算法，该方法包括以下步骤：
(1)构建紧耦合融合的激光雷达惯性里程计。
(2)构建紧耦合融合的相机惯性里程计。
(3)构建紧耦合融合的激光雷达相机里程计。
(4)将三种紧耦合里程计输出结果加权平均，构建松耦合融合里程计。
进一步，根据上述的位姿估计算法，其中，通过激光雷达(LiDAR)获得环境的3D点云数据，通过可见光相机(Camera)获得环境在其成像平面的2D投影图像，通过惯性测量单元(IMU)获得载体的三轴线加速度和三轴角速度。
进一步，根据上述的位姿估计算法，其中，利用LiDAR和Camera的融合构建激光雷达相机里程计(LCO)，利用Camera和IMU的融合构建相机惯性里程计(CIO)，利用LiDAR和IMU的融合构建激光雷达惯性里程计(LIO)。
进一步，根据上述的位姿估计算法，其中，这些LCO、CIO、LIO里程计的内部是紧耦合融合形式，LCO、CIO、LIO之间采用松耦合形式融合输出位姿结果，整体上形成一种多源融合的联合位姿估计算法。
进一步地，所述步骤(1)具体为：LIO在位姿估计时，位置以三维点坐标(x，y，z)表示，姿态以欧拉角(横滚角俯仰角θ，偏航角ψ)表示，即位姿状态变量其中，利用IMU的三轴线加速度数据结合重力约束解算得到横滚角/>与俯仰角θ的全局精确估计值，利用激光雷达点云数据优化剩余的位姿变量(x，y，z，ψ)，达到解耦优化位姿的效果。
进一步地，在x-y-z型欧拉角的表示下，将重力向量[0 0 g]T通过载体的旋转矩阵投影到载体坐标系下，将其与IMU的三轴线加速度数据[ax ay az]T建立等量关系，得
其中解算得到
利用IMU的三轴线加速度数据解算得到了横滚角与俯仰角θ的较精确估计值。
进一步地，将位姿状态变量的最大后验估计解耦成两部分计算，第一部分由IMU的三轴线加速度数据直接计算得到，第二部分由激光雷达点云匹配后的最小二乘非线性优化，目标函数为最小化匹配残差ri，即
进一步地，所述步骤(2)具体为：CIO根据IMU的三轴线加速度数据计算得到较精确的横滚角与俯仰角θ，从而确定重力的方向。将相机图像各帧特征点的方向均旋转至该帧对应的重力方向，即在描述子的方向信息中加入对应帧的重力方向信息，增强匹配时的旋转不变特性。
进一步地，在将各特征点的方向均旋转至该帧对应的重力方向之后计算其描述子时，以特征点为圆心，以一定距离为半径作圆，将该圆上的像素点与特征点一一比较大小，若像素点大于特征点则描述子取1，反之则取0，并且规定该特征点的正上方(重力的反方向)处的像素点为第一个比较点，得到二进制描述子向量。采用汉明距离来度量两个描述子之间的差异，即两个描述子向量对应位置的元素不同(其中一个为1，另一个为0)的个数。这样的描述子不仅具有旋转不变特性，且所采用的比较方式能够更精确地描述不同帧的特征点之间的相似程度。
进一步地，所述步骤(3)具体为：LCO利用相机图像提取环境的2D边缘点，利用激光雷达点云提取环境的3D边缘点，对二者进行跨模态匹配，然后采用PnP(Perspective-n-Point)求解位姿。
进一步地，利用相机图像来辅助激光雷达点云中边缘特征点的提取，具体做法为：在相机图像的潜在的像素边缘点区域内，选取像素一阶导数最大的像素点作为2D边缘点。利用三角化计算该像素点对应的深度，再利用相机和激光雷达的外参数投射到激光雷达的点云坐标系中，以该3D点附近区域为感兴趣的区域作为潜在的点云边缘点，减少计算量，提高提取的效率和准确度。所述相机可以为任何可以拍摄图像的相机，优选为可见光相机，还可以是可量测照相的相机。
进一步地，对潜在的点云边缘点周围空间计算曲率，提取曲率大的点为边缘点，利用外参数，将激光雷达的所有点云边缘点反投射到相机的像素坐标系中，将该像素周围作为潜在的像素边缘点区域，减少计算量，提高提取的效率和准确度。
进一步地，所述步骤(4)具体为：LIO、CIO和LCO均为并行独立运行，内部数据均为紧耦合方式融合，从而相互均能独立计算出位姿矩阵、位姿置信度矩阵。然后采用卡尔曼滤波架构，对三者输出位姿结果按置信度进行加权平均，从而实现松耦合融合，输出融合后的唯一位姿矩阵、位姿置信度矩阵，供机器人或外部程序使用。此处紧耦合和松耦合指的是本领域中通用的紧耦合和松耦合的技术含义，也即紧耦合通常指的是共同构建运动方程和观测方程而进行状态估计，同时优化状态变量数据等等，而松耦合通常指的是分别各自进行自身的运动估计而对位姿估计结果进行融合，分别对各自的状态向量进行优化，最终把不同优化过程中得到的位姿估计结果进行融合等等。
本发明的有益效果是：针对多传感器融合位姿估计的精度和鲁棒性问题，通过激光雷达、相机、惯性测量单元两两之间紧耦合融合，构建LIO、CIO、LCO三个独立运行的里程计。紧耦合的联合优化采用非线性优化方法直接处理观测数据，因此优化残差后的位姿精度高。并且由于紧耦合是并行进行的，且每个里程计只有两种传感器的数据进行融合，残差项数并没有随着传感器增多而显著增多，因此计算效率高。然后三个里程计之间采用松耦合融合，在面临某一传感器失效或数据出错的情况时，不会影响其它不包含此传感器的里程计，且在松耦合融合过程，无数据或数据出错的里程计权值低，融合后位姿结果受影响小，因此鲁棒性强。因此该算法架构能提升多传感器融合的位姿估计能力。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
图1为现有技术中激光雷达平台上高精度定姿装置的结构示意图。
图2为本发明一个实施例中提供的一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法的流程示意图。
具体实施方式
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
在下文中，将更全面地描述本公开的各种实施例。本公开可具有各种实施例，并且可在其中做出调整和改变。然而，应理解：不存在将本公开的各种实施例限于在此公开的特定实施例的意图，而是应将本公开理解为涵盖落入本公开的各种实施例的精神和范围内的所有调整、等同物和/或可选方案。
在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
本发明的利用计算机实施的激光雷达、相机、惯性测量单元融合的位姿估计算法，通过构建独立运行的紧耦合融合LCO、LIO、CIO，然后三个里程计之间松耦合融合的算法架构，提升多传感器的位姿估计能力。在一个优选实施例中，所述相机采用可见光相机。
图2示出本发明一个实施例中利用计算机实施的激光雷达、可见光相机、惯性测量单元融合的位姿估计算法的流程示意图，该方法包括：
步骤S110，将激光雷达的点云数据、可见光相机的图片数据、惯性测量元件的数据按两两组合分发给对应的里程计，即后续步骤中构建的激光雷达惯性里程计(LIO)、相机惯性里程计(CIO)、激光雷达相机里程计(LCO)。
步骤S120，构建激光雷达惯性里程计(LIO)，采用紧耦合融合方式，利用IMU(惯性测量单元)的三轴线加速度数据结合重力约束解算得到横滚角与俯仰角θ的全局精确估计值，利用激光雷达点云数据优化剩余的位姿变量(x，y，z，ψ)，达到解耦优化位姿的效果。
步骤S130，构建相机惯性里程计(CIO)，采用紧耦合融合方式，根据IMU的三轴线加速度数据计算得到较精确的横滚角与俯仰角θ，从而确定重力的方向。将相机图像各帧特征点的方向均旋转至该帧对应的重力方向，即在描述子的方向信息中加入对应帧的重力方向信息，增强匹配时的旋转不变特性。
步骤S140，构建激光雷达相机里程计(LCO)，利用相机图像提取环境的2D边缘点，利用激光雷达点云提取环境的3D边缘点，对二者进行跨模态匹配，然后采用PnP(Perspective-n-Point)求解位姿。
步骤S150，将三种紧耦合里程计输出结果加权平均，构建松耦合融合里程计，在本发明一个优选实施例中，先构建松耦合融合架构，然后采用卡尔曼滤波，对三者输出位姿结果按置信度进行加权平均，从而实现松耦合融合，输出融合后的唯一位姿矩阵、位姿置信度矩阵，供机器人或外部程序使用。
在本发明的一个优选实施例中，所述构建激光雷达惯性里程计的步骤(S120)具体为：利用IMU的三轴线加速度数据结合重力约束解算得到横滚角与俯仰角θ的全局精确估计值，利用激光雷达点云数据优化剩余的位姿变量(x，y，z，ψ)，达到解耦优化位姿的效果。
进一步地，在x-y-z型欧拉角的表示下，将重力向量[0 0 g]T通过载体的旋转矩阵投影到载体坐标系下，将其与IMU的三轴线加速度数据[ax ay az]T建立等量关系，得
其中解算得到
利用IMU的三轴线加速度数据解算得到了横滚角与俯仰角θ的较精确估计值。
进一步地，由激光雷达点云匹配后的最小二乘非线性优化，目标函数为最小化匹配残差ri，即
在本发明的一个优选实施例中，所述构建相机惯性里程计的步骤(S130)具体为：CIO根据IMU的三轴线加速度数据计算得到较精确的横滚角与俯仰角θ，从而确定重力的方向。将相机图像各帧特征点的方向均旋转至该帧对应的重力方向，即在描述子的方向信息中加入对应帧的重力方向信息，增强匹配时的旋转不变特性。
进一步地，在将各特征点的方向均旋转至该帧对应的重力方向之后计算其描述子时，以特征点为圆心，以一定距离为半径作圆，将该圆上的像素点与特征点一一比较大小，若像素点大于特征点则描述子取1，反之则取0，并且规定该特征点的正上方(重力的反方向)处的像素点为第一个比较点，得到二进制描述子向量。采用汉明距离来度量两个描述子之间的差异，即两个描述子向量对应位置的元素不同(其中一个为1，另一个为0)的个数。这样的描述子不仅具有旋转不变特性，且所采用的比较方式能够更精确地描述不同帧的特征点之间的相似程度。
在本发明的一个优选实施例中，所述构建激光雷达相机里程计的步骤(S140)具体为：LCO利用相机图像提取环境的2D边缘点，利用激光雷达点云提取环境的3D边缘点，对二者进行跨模态匹配，然后采用PnP(Perspective-n-Point)求解位姿。
进一步地，利用相机图像来辅助激光雷达点云中边缘特征点的提取，具体做法为：在相机图像的潜在的像素边缘点区域内，选取像素一阶导数最大的像素点作为2D边缘点。利用三角化计算该像素点对应的深度，再利用相机和激光雷达的外参数投射到激光雷达的点云坐标系中，以该3D点附近区域为感兴趣的区域作为潜在的点云边缘点，减少计算量，提高提取的效率和准确度。
进一步地，对潜在的点云边缘点周围空间计算曲率，提取曲率大的点为边缘点，利用外参数，将激光雷达的所有点云边缘点反投射到相机的像素坐标系中，将该像素周围作为潜在的像素边缘点区域，减少计算量，提高提取的效率和准确度。
在本发明的一个优选实施例中，所述构建松耦合融合里程计的步骤(S150)具体为：LIO、CIO和LCO均为并行独立运行，内部数据均为紧耦合方式融合，从而相互均能独立计算出位姿矩阵、位姿置信度矩阵。然后采用卡尔曼滤波架构，对三者输出位姿结果按置信度进行加权平均，从而实现松耦合融合，输出融合后的唯一位姿矩阵、位姿置信度矩阵，供机器人或外部程序使用。
根据本发明，针对多传感器融合位姿估计的精度和鲁棒性问题，通过激光雷达、相机、惯性测量单元两两之间紧耦合融合，构建LIO、CIO、LCO三个独立运行的里程计。三个里程计之间采用松耦合融合，在面临某一传感器失效或数据出错的情况时，不会影响其它不包含此传感器的里程计，且在松耦合融合过程，无数据或数据出错的里程计权值低，融合后位姿结果受影响小，因此鲁棒性强。能提升多传感器融合的位姿估计能力。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。
Claims (13)
1.一种利用计算机实施的激光雷达、相机、惯性测量单元融合的位姿估计算法，该方法包括以下步骤：
(1)构建紧耦合融合的激光雷达惯性里程计；
(2)构建紧耦合融合的相机惯性里程计；
(3)构建紧耦合融合的激光雷达相机里程计；
(4)将三种紧耦合里程计输出结果加权平均，构建松耦合融合里程计。
2.根据权利要求1所述的位姿估计算法，其中，构建上述里程计时，通过激光雷达LiDAR获得环境的3D点云数据，通过相机Camera获得环境在其成像平面的2D投影图像，通过惯性测量单元IMU获得载体的三轴线加速度和三轴角速度。
3.根据权利要求1所述的位姿估计算法，其中，利用激光雷达LiDAR和相机Camera的融合构建激光雷达相机里程计LCO，利用相机Camera和惯性测量单元IMU的融合构建相机惯性里程计CIO，利用激光雷达LiDAR和惯性测量单元IMU的融合构建激光雷达惯性里程计LIO。
4.根据权利要求3所述的位姿估计算法，其中，这些激光雷达相机里程计LCO、相机惯性里程计CIO、激光雷达惯性里程计LIO里程计的内部是紧耦合融合形式，激光雷达相机里程计LCO、相机惯性里程计CIO、激光雷达惯性里程计LIO之间采用松耦合形式融合输出位姿结果，整体上形成一种多源融合的联合位姿估计算法。
5.根据权利要求4所述的位姿估计算法，其特征在于，激光雷达惯性里程计LIO在位姿估计时，位置以三维点坐标(x，y，z)表示，姿态以欧拉角的横滚角俯仰角θ，偏航角ψ表示，即位姿状态变量/>其中，利用惯性测量单元IMU的三轴线加速度数据结合重力约束解算得到横滚角/>与俯仰角θ的全局精确估计值，利用激光雷达点云数据优化剩余的位姿变量(x，y，z，ψ)，达到解耦优化位姿的效果。
6.根据权利要求5所述的位姿估计算法，其特征在于，在x-y-z型欧拉角的表示下，将重力向量[0 0 g]T通过载体的旋转矩阵投影到载体坐标系下，将其与惯性测量单元IMU的三轴线加速度数据[ax ay az]T建立等量关系，得
其中，解算该方程可得
即利用惯性测量单元IMU的三轴线加速度数据解算得到了横滚角与俯仰角θ较精确的估计值。
7.根据权利要求5所述的位姿估计算法，其特征在于，将位姿状态变量的最大后验估计解耦成两部分计算，第一部分由惯性测量单元IMU的三轴线加速度数据直接计算得到，第二部分由激光雷达点云匹配后的最小二乘非线性优化得到，目标函数为最小化匹配残差ri，即
8.根据权利要求4所述的位姿估计算法，其特征在于，相机惯性里程计CIO根据惯性测量单元IMU的三轴线加速度数据计算得到较精确的横滚角与俯仰角θ，从而确定重力的方向；将相机图像各帧特征点的方向均旋转至该帧对应的重力方向。
9.根据权利要求8所述的位姿估计算法，其特征在于，在将各特征点的方向均旋转至该帧对应的重力方向之后计算其描述子时，以特征点为圆心，以一定距离为半径作圆，将该圆上的像素点与特征点一一比较大小，若像素点大于特征点则描述子取1，反之则取0，并且规定位于该特征点正上方，即重力的反方向的像素点为第一个比较点，得到二进制描述子向量；采用汉明距离来度量两个描述子之间的差异，即两个描述子向量对应位置的元素不同的个数，即其中一个为1、另一个为0的个数。
10.根据权利要求4所述的位姿估计算法，其特征在于，激光雷达相机里程计LCO利用相机图像提取环境的2D边缘点，利用激光雷达点云提取环境的3D边缘点，对二者进行跨模态匹配，然后采用PnPPerspective-n-Point求解位姿。
11.根据权利要求10所述的位姿估计算法，其特征在于，利用相机图像来辅助激光雷达点云中边缘特征点的提取，具体做法为：在相机图像潜在的像素边缘点区域内，选取像素一阶导数最大的像素点作为2D边缘点；利用三角化计算该像素点对应的深度，再利用相机和激光雷达的外参数投射到激光雷达的点云坐标系中，以该3D点附近区域为感兴趣的区域作为潜在的点云边缘点。
12.根据权利要求10所述的位姿估计算法，其特征在于，对潜在的点云边缘点周围空间计算曲率，提取曲率大的点为边缘点，利用外参数，将激光雷达的所有点云边缘点反投射到相机的像素坐标系中，将该像素周围作为潜在的像素边缘点区域。
13.根据权利要求4所述的位姿估计算法，其特征在于，激光雷达惯性里程计LIO、相机惯性里程计CIO和激光雷达相机里程计LCO均为并行独立运行，内部数据均为紧耦合方式融合，从而相互均能独立计算出位姿矩阵、位姿置信度矩阵；然后采用卡尔曼滤波架构，对三者输出位姿结果按置信度进行加权平均，从而实现松耦合融合，输出融合后的唯一位姿矩阵、位姿置信度矩阵，供机器人或外部程序使用。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310144825.XA CN116184430B (zh) | 2023-02-21 | 2023-02-21 | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310144825.XA CN116184430B (zh) | 2023-02-21 | 2023-02-21 | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116184430A CN116184430A (zh) | 2023-05-30 |
| CN116184430B true CN116184430B (zh) | 2023-09-29 |
Family
ID=86436212
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310144825.XA Active CN116184430B (zh) | 2023-02-21 | 2023-02-21 | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116184430B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117058474B (zh) * | 2023-10-12 | 2024-01-12 | 南昌航空大学 | 一种基于多传感器融合的深度估计方法及*** |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112781582A (zh) * | 2020-12-26 | 2021-05-11 | 复三人工智能科技(上海)有限公司 | 卫星弱观测条件下的多传感器融合高精度位姿估计算法 |
| CN113436260A (zh) * | 2021-06-24 | 2021-09-24 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和*** |
| CN114608554A (zh) * | 2022-02-22 | 2022-06-10 | 北京理工大学 | 一种手持slam设备以及机器人即时定位与建图方法 |
| CN114966734A (zh) * | 2022-04-28 | 2022-08-30 | 华中科技大学 | 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8855848B2 (en) * | 2007-06-05 | 2014-10-07 | GM Global Technology Operations LLC | Radar, lidar and camera enhanced methods for vehicle dynamics estimation |
-
2023
- 2023-02-21 CN CN202310144825.XA patent/CN116184430B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112781582A (zh) * | 2020-12-26 | 2021-05-11 | 复三人工智能科技(上海)有限公司 | 卫星弱观测条件下的多传感器融合高精度位姿估计算法 |
| CN113436260A (zh) * | 2021-06-24 | 2021-09-24 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和*** |
| CN114608554A (zh) * | 2022-02-22 | 2022-06-10 | 北京理工大学 | 一种手持slam设备以及机器人即时定位与建图方法 |
| CN114966734A (zh) * | 2022-04-28 | 2022-08-30 | 华中科技大学 | 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法 |
Non-Patent Citations (1)
| Title |
|---|
| Jiarong Lin 等.R 2 LIVE: A Robust, Real-Time, LiDAR-Inertial-Visual Tightly-Coupled State Estimator and Mapping.《IEEE ROBOTICS AND AUTOMATION LETTERS》.2021,第6卷(第4期),7469-7476. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116184430A (zh) | 2023-05-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10788830B2 (en) | Systems and methods for determining a vehicle position | |
| JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| WO2018196391A1 (zh) | 一种车载摄像机的外部参数标定方法及装置 | |
| CN110033489B (zh) | 一种车辆定位准确性的评估方法、装置及设备 | |
| CN102435188B (zh) | 一种用于室内环境的单目视觉/惯性全自主导航方法 | |
| JP5832341B2 (ja) | 動画処理装置、動画処理方法および動画処理用のプログラム | |
| CN111912416B (zh) | 用于设备定位的方法、装置及设备 | |
| US20190072392A1 (en) | System and method for self-geoposition unmanned aerial vehicle | |
| CN109443348B (zh) | 一种基于环视视觉和惯导融合的地下车库库位跟踪方法 | |
| US9020204B2 (en) | Method and an apparatus for image-based navigation | |
| CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
| CN110411457B (zh) | 基于行程感知与视觉融合的定位方法、***、终端和存储介质 | |
| US9959625B2 (en) | Method for fast camera pose refinement for wide area motion imagery | |
| JP2001331787A (ja) | 道路形状推定装置 | |
| CN112230242A (zh) | 位姿估计***和方法 | |
| CN110081881A (zh) | 一种基于无人机多传感器信息融合技术的着舰引导方法 | |
| CN110032201A (zh) | 一种基于卡尔曼滤波的imu机载视觉姿态融合的方法 | |
| CN110458885B (zh) | 基于行程感知与视觉融合的定位***和移动终端 | |
| CN113516692A (zh) | 一种多传感器融合的slam方法和装置 | |
| CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
| CN103411587A (zh) | 定位定姿方法及*** | |
| CN113052897A (zh) | 定位初始化方法和相关装置、设备、存储介质 | |
| CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
| CN114777768A (zh) | 一种卫星拒止环境高精度定位方法、***及电子设备 | |
| CN112862818B (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 |