一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法

技术领域

本发明涉及多传感器融合与位姿估计领域，尤其涉及一种激光雷达、可见光相机、惯性测量单元融合的位姿估计方法。

背景技术

传统的多传感器融合与位姿估计技术往往难以兼顾精度与鲁棒性。传统方法分为两类：(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)表示，姿态以欧拉角(横滚角

进一步地，在x-y-z型欧拉角的表示下，将重力向量[0 0 g]

其中

利用IMU的三轴线加速度数据解算得到了横滚角

进一步地，将位姿状态变量的最大后验估计

进一步地，所述步骤(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(惯性测量单元)的三轴线加速度数据结合重力约束解算得到横滚角

步骤S130，构建相机惯性里程计(CIO)，采用紧耦合融合方式，根据IMU的三轴线加速度数据计算得到较精确的横滚角

步骤S140，构建激光雷达相机里程计(LCO)，利用相机图像提取环境的2D边缘点，利用激光雷达点云提取环境的3D边缘点，对二者进行跨模态匹配，然后采用PnP(Perspective-n-Point)求解位姿。

步骤S150，将三种紧耦合里程计输出结果加权平均，构建松耦合融合里程计，在本发明一个优选实施例中，先构建松耦合融合架构，然后采用卡尔曼滤波，对三者输出位姿结果按置信度进行加权平均，从而实现松耦合融合，输出融合后的唯一位姿矩阵、位姿置信度矩阵，供机器人或外部程序使用。

在本发明的一个优选实施例中，所述构建激光雷达惯性里程计的步骤(S120)具体为：利用IMU的三轴线加速度数据结合重力约束解算得到横滚角

进一步地，在x-y-z型欧拉角的表示下，将重力向量[0 0 g]

其中

利用IMU的三轴线加速度数据解算得到了横滚角

进一步地，由激光雷达点云匹配后的最小二乘非线性优化，目标函数为最小化匹配残差r

在本发明的一个优选实施例中，所述构建相机惯性里程计的步骤(S130)具体为：CIO根据IMU的三轴线加速度数据计算得到较精确的横滚角

进一步地，在将各特征点的方向均旋转至该帧对应的重力方向之后计算其描述子时，以特征点为圆心，以一定距离为半径作圆，将该圆上的像素点与特征点一一比较大小，若像素点大于特征点则描述子取1，反之则取0，并且规定该特征点的正上方(重力的反方向)处的像素点为第一个比较点，得到二进制描述子向量。采用汉明距离来度量两个描述子之间的差异，即两个描述子向量对应位置的元素不同(其中一个为1，另一个为0)的个数。这样的描述子不仅具有旋转不变特性，且所采用的比较方式能够更精确地描述不同帧的特征点之间的相似程度。

在本发明的一个优选实施例中，所述构建激光雷达相机里程计的步骤(S140)具体为：LCO利用相机图像提取环境的2D边缘点，利用激光雷达点云提取环境的3D边缘点，对二者进行跨模态匹配，然后采用PnP(Perspective-n-Point)求解位姿。

进一步地，利用相机图像来辅助激光雷达点云中边缘特征点的提取，具体做法为：在相机图像的潜在的像素边缘点区域内，选取像素一阶导数最大的像素点作为2D边缘点。利用三角化计算该像素点对应的深度，再利用相机和激光雷达的外参数投射到激光雷达的点云坐标系中，以该3D点附近区域为感兴趣的区域作为潜在的点云边缘点，减少计算量，提高提取的效率和准确度。

进一步地，对潜在的点云边缘点周围空间计算曲率，提取曲率大的点为边缘点，利用外参数，将激光雷达的所有点云边缘点反投射到相机的像素坐标系中，将该像素周围作为潜在的像素边缘点区域，减少计算量，提高提取的效率和准确度。

在本发明的一个优选实施例中，所述构建松耦合融合里程计的步骤(S150)具体为：LIO、CIO和LCO均为并行独立运行，内部数据均为紧耦合方式融合，从而相互均能独立计算出位姿矩阵、位姿置信度矩阵。然后采用卡尔曼滤波架构，对三者输出位姿结果按置信度进行加权平均，从而实现松耦合融合，输出融合后的唯一位姿矩阵、位姿置信度矩阵，供机器人或外部程序使用。

根据本发明，针对多传感器融合位姿估计的精度和鲁棒性问题，通过激光雷达、相机、惯性测量单元两两之间紧耦合融合，构建LIO、CIO、LCO三个独立运行的里程计。三个里程计之间采用松耦合融合，在面临某一传感器失效或数据出错的情况时，不会影响其它不包含此传感器的里程计，且在松耦合融合过程，无数据或数据出错的里程计权值低，融合后位姿结果受影响小，因此鲁棒性强。能提升多传感器融合的位姿估计能力。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。