CN117237789A - 基于全景相机和激光雷达融合生成纹理信息点云地图方法 - Google Patents

Abstract

本发明公开了一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，首先根据实时采集得到的每一帧的激光点云数据和每一帧的IMU数据通过迭代误差扩展卡尔曼滤波算法融合解算得到IMU位姿之后，将激光点云数据根据通过根据IMU位姿和激光雷达外参、IMU外参计算得到的激光雷达位姿进行拼接生成点云地图；然后通过对双目鱼眼相机进行标定后将激光点云数据投影到对双目鱼眼图像进行校正后的针孔相机模型图像上得到投影点；根据投影点将激光点云数据进行分类得到激光点对；根据激光点对以及激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息；最终使用贝叶斯公式对激光点云进行赋色，从而得到有纹理信息点云地图。

Description

基于全景相机和激光雷达融合生成纹理信息点云地图方法

技术领域

本发明涉及点云建图领域，尤其涉及一种基于全景相机和激光雷达融合生成纹理信息点云地图方法。

背景技术

三维激光雷达作为一个高精度测量仪器，可以提供周围环境的几何特征，同时受光照等环境因素很小，常常用于无人车、无人机以及虚拟现实等领域。使用激光雷达可以快速并准确的获取周围环境的深度信息，使用激光雷达配合SLAM技术可以很好的构建环境三维地图，可以辅助测绘，地质灾害预测等户外工作，极大提高户外作业的安全性以及效率。但是激光雷达常常获取周围环境强度信息而不是纹理信息，最终得到的点云地图上纹理信息较少。因此，时常会使用相机对点云地图进行赋色，使地图能够得到颜色纹理信息，更直观的反应周围环境信息；

传统的点云地图建图方法中点云赋色通常使用slam系统，slam系统主要使用单目相机进行赋色，使用单目相机一次只能对单边环境进行赋色，如图10、图11、图12及图13所示，使用slam系统进行赋色效率不高，并且单目相机提供的约束较少，从而导致slam系统鲁棒性相对较低。

发明内容

本发明的目的在于提供一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，解决了现有技术中指出的上述技术问题。

本发明提出了一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，包括如下操作步骤：

根据实时采集得到的每一帧的激光点云数据和每一帧的IMU数据通过迭代误差扩展卡尔曼滤波算法融合解算得到IMU位姿之后，将所述激光点云数据根据通过根据所述IMU位姿和激光雷达外参、IMU外参计算得到的激光雷达位姿进行拼接生成点云地图；

根据实时获取得到的每一帧的双目鱼眼图像使用相机畸变模型对双目鱼眼相机进行标定得到所述双目鱼眼相机的内参及双目鱼眼相机的外参；

根据所述双目鱼眼相机的内参对双目鱼眼图像进行校正得到针孔相机模型图像后将所述激光点云数据进行投影，获取每个激光点云数据对应的投影点；根据所述投影点将所述激光点云数据进行分类得到激光点对；

根据所述激光点对以及所述激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息；

根据所述最优状态信息及所述针孔相机模型图像对所述点云地图中的各个所述激光点云数据进行着色，得到各个所述激光点云数据对应的颜色信息；

所述激光点云数据对应的颜色信息为：

式中，和c_s表示赋色前后的地图点云投影点；

γ_s表示观测投影点；

以及/>分别代表赋色前后地图点协方差以及观测投影点协方差；

σ_s表示跟随时间变化的协方差变化。

较佳的，所述双目鱼眼相机包括左鱼眼相机与右鱼眼相机；

较佳的，所述双目鱼眼相机的内参为相机畸变参数；

较佳的，所述双目鱼眼相机的外参为相机的坐标转换矩阵

较佳的，所述根据实时采集得到的每一帧的激光点云数据和每一帧的IMU数据通过迭代误差扩展卡尔曼滤波算法融合解算得到IMU位姿，包括如下操作步骤：

基于所述IMU数据对所述激光点云数据分别进行运动畸变去除操作，得到多个目标激光点云数据；基于所有所述目标激光点云数据构建目标激光点云数据集合；

根据所述目标激光点云数据集合获取每一帧对应的点云数据集合；遍历各帧对应的点云数据集合，根据各个所述目标激光点云数据的坐标，计算获取平面距离中距离最短对应的四个对照平面点构建对照平面；并获取所述对照平面的中心点pj；对当前所述帧k中的各个平面点pk构建总激光点云残差约束r；

所述总激光点云残差约束r表示为：

r＝u^T(p_k-p_j)；

式中，u^T表示拟合平面的法向量；

p_k为当前帧k中的一个平面点；

p_j为对照平面的中心点。

根据当前帧k的第一IMU数据及下一帧k+1的第二IMU数据进行积分解算，得到在第k+1帧的IMU位姿信息。

较佳的，所述第k+1帧的IMU位姿信息包括第k+1帧的IMU姿态^WR_Ik+1、第k+1帧的IMU速度^wv_k+1、第k+1帧的IMU位置^wp_k+1；

其中，所述第k+1帧的IMU姿态^WR_Ik+1、第k+1帧的IMU速度^wv_k+1、第k+1帧的IMU位置^wp_k+1分别表示为：

式中，代表k+1帧的IMU姿态信息；/>表示k帧的IMU姿态信息；/>表示k帧IMU的角速度信息；b_gk表示陀螺仪随机游走误差信息；/>表示陀螺仪测量白噪声误差信息；^wv_k+1表示第k+1帧的IMU速度信息；^wv_k表示第k帧的IMU速度信息；^wg表示IMU重力加速度信息；/>表示k帧IMU加速度测量值；/>表示IMU加速度计的随机游走噪声；/>表示k帧的加速度计白噪声；^wp_k表示k帧的IMU位置信息；

较佳的，所述平面距离为当前帧k中的平面点pk至前一帧k-1中平面激光点云集合中各个平面点的距离；

较佳的，所述点云数据集合包括平面激光点云集合；

所述平面激光点云数据集合包括多个平面点；所述平面点为目标激光点云数据集合中的目标激光点云数据；

较佳的，所述根据所述IMU位姿和激光雷达外参、IMU外参计算得到的激光雷达位姿，包括如下操作步骤：

根据第k帧的状态量x_k进行先验推导得到第k+1帧的状态量x_k+1；并根据所述第k帧的状态量x_k及所述第k+1帧的状态量x_k+1计算获取第k帧的状态误差量δx_k；

所述第k帧的状态量x_i为：

式中，表示第k帧的IMU姿态信息，/>表示k帧的IMU位置信息，^GV_k ^T表示第k帧的IMU速度信息，/>分别表示陀螺仪和加速度在第k帧的随机游走误差信息，^Gg^T表示重力加速度信息；/>分别表示IMU坐标系到相机坐标系的转换矩阵以及传感器采集时间差，φ表示相机内参f_x、f_y、c_x以及c_y；

所述第k+1帧的状态量x_k+1为：

其中，

所述第k帧的状态误差量δx_k为：

根据所述第k帧的状态误差量δx_i通过状态递推方程计算获取第k+1帧的状态先验值根据所述第k+1帧的状态先验值/>计算获取先验协方差矩阵/>

所述第k+1帧的状态先验值为：

其中， 表示第k帧状态的先验值；

所述先验协方差矩阵为：

其中Q代表噪声协方差矩阵，和/>代表状态递推矩阵；

根据所述状态递推方程及所述先验协方差矩阵/>根据测量方程进行计算获取后验状态误差；

所述后验状态误差的计算方式为：

式中，H为状态观测矩阵；所述状态观测矩阵为：

根据所述先验协方差矩阵计算获取测量协方差矩阵R；

所述测量协方差矩阵为：

根据所述状态观测矩阵H及所述测量协方差矩阵R以及所述先验协方差矩阵计算获取卡尔曼滤波增益；

所述卡尔曼滤波增益为：

K＝(H^TR^-1H+P^-1)^-1H^TR^-1；

利用所述卡尔曼滤波增益更新获取第k+1帧的后验状态矩阵并基于所述第k+1帧的后验状态矩阵/>对所述协方差矩阵进行更新得到后验协方差矩阵，完成首次迭代并设定迭代计数器，将所述迭代计数器迭代次数加一；并重复上述操作，记录所述迭代次数；

所述第k+1帧的后验状态矩阵为：

所述后验协方差矩阵为：

根据所述第k+1帧的后验状态矩阵与所述第k+1帧的状态先验值/>计算获取先后变化量E；根据预设的先后变化量最大阈值E'与最大迭代次数阈值通过重复上述步骤进行迭代操作获取激光雷达位姿。

较佳的，所述根据所述投影点将所述激光点云数据进行分类得到激光点对，包括如下操作步骤：

判断所述投影点p_k坐标是否为负或者处于所述针孔相机模型图像的边缘；

若是，则将所述激光点云数据筛除；若否，则将所述激光点云数据分为左3d激光点对ζ_l与右3d激光点对ζ_r；并根据所述左3d激光点对ζ_l与右3d激光点对ζ_r对应的投影点p_k通过光流跟踪获取下一帧对应的投影点p_k。

较佳的，所述投影点p_k包括左鱼眼相机投影点p_kl与右鱼眼相机投影点p_kr；

其中，所述左鱼眼相机投影点p_kl为：

所述右鱼眼相机投影点p_kr为：

式中，表示左鱼眼相机投影点；/>表示右鱼眼相机投影点；K为相机内参；^aT_b表示a坐标系到b坐标系的坐标系转换矩阵；P_k为点云地图中的激光点云数据；n为归一化因子；

较佳的，根据所述激光点对以及所述激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息，包括如下操作步骤：

根据所述激光点对计算获取重投影误差；

所述重投影误差为：

式中，^IT_c代表IMU到左鱼眼相机与右鱼眼相机的坐标系转换矩阵；

基于所述重投影误差进行重投影误差作为观测，进行迭代误差卡尔曼滤波估计视觉里程计，得到视觉里程计的最优状态信息。

与现有技术相比，本发明实施例至少存在如下方面的技术优势：

分析本发明提供的上述一种基于全景相机和激光雷达融合生成纹理信息点云地图方法可知，在具体应用时首先利用激光雷达实时采集每一帧的激光点云数据，同时利用陀螺仪采集每一帧对应的IMU数据；将每一帧的激光点云数据和IMU数据通过迭代误差扩展卡尔曼滤波算法进行融合解算得到IMU位姿，通过迭代误差扩展卡尔曼滤波算法可以保障位姿解算速度快，并且其解算位姿的结果精准；进而通过激光雷达外参和IMU外参对IMU数据进行解算得到每一帧对应的激光雷达位姿，将激光点云数据根据激光雷达位姿进行拼接从而得到点云地图；使得到的点云地图更加精准；

进一步获取每一帧对应的双目鱼眼图像，根据双目鱼眼图像对双目鱼眼相机进行标定，得到相机内参(即相机畸变参数)和相机的坐标转换矩阵，可为后续点云地图的颜色处理提供处理的依据；

进而根据相机内参对双目鱼眼图像进行校正得到针孔相机模型图像后，将点云地图中的激光点云数据投影到针孔相机模型图像中，得到2d图像(即将3d的激光点云数据投影到针孔相机模型图像中得到每个3d的激光点云数据对应的2d像素点(即为投影点)，从而由所有的2d像素点得到的整个2d图像)；根据2d像素点的投影位置从而将激光点云数据分类为激光点对，并同时根据投影点的位置通过光流得到下一帧的投影点；

进一步根据激光点对和激光雷达位姿进行视觉里程估计，从而得到鱼眼相机的最优状态，保障点云地图中每一帧激光点云数据都处在鱼眼相机的拍摄范围内，提高后续着色效果；

最终使用贝叶斯公式对激光点云进行赋色，从而得到有纹理信息点云地图。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法的操作步骤示意图；

图2为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法的赋色点云图以及对应轨迹模拟示意图；

图3为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法的局部赋色效果模拟示意图；

图4为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法的局部赋色效果模拟示意图；

图5为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法的局部赋色效果模拟示意图；

图6为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法中得到IMU位姿的操作步骤示意图；

图7为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法中得到激光雷达位姿的操作步骤示意图；

图8为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法中得到激光点对的操作步骤示意图；

图9为本发明实施例提供的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法中得到视觉里程计的最优状态信息的操作步骤示意图；

图10为现有技术中的点云赋色效果模拟示意图；

图11为现有技术中的局部赋色效果模拟示意图；

图12为现有技术中的局部赋色效果模拟示意图；

图13为现有技术中的局部赋色效果模拟示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面通过具体的实施例并结合附图对本发明做进一步的详细描述。

如图1所示，本发明提出了一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，包括如下操作步骤：

步骤S10：利用激光雷达实时采集获取每一帧的激光点云数据，并利用IMU实时采集获取每一帧的IMU数据；获取所述激光雷达的激光雷达外参与所述IMU的IMU外参；根据所述激光点云数据及所述IMU数据通过迭代误差扩展卡尔曼滤波算法(esikf)进行融合解算得到IMU位姿；根据所述IMU位姿以及所述激光雷达外参与IMU外参计算得到的激光雷达位姿；将所述激光点云数据根据所述激光雷达位姿进行拼接生成点云地图；

需要说明的是，上述本申请实施例所采用的技术方案，通过使用迭代误差扩展卡尔曼滤波算法对激光点云数据及IMU数据进行融合解算得到IMU位姿后，基于融合解算得到的IMU位姿及激光雷达外参与IMU外参将激光点云数据转移到世界坐标系中生成点云地图；

上述迭代误差扩展卡尔曼滤波算法是一种常用的位姿解算算法，其位姿解算速度快，并且其解算位姿的结果精准；

上述IMU外参指的是IMU数据坐标系至激光点云数据坐标系的转换矩阵，可通过开源算法进行标定得到。

步骤S20：实时获取每一帧对应的双目鱼眼图像；根据所述双目鱼眼图像，使用相机畸变模型对双目鱼眼相机进行标定得到相机畸变参数(相机畸变参数为鱼眼相机的内参)，并对左鱼眼相机与右鱼眼相机之间的外参进行标定得到相机的坐标转换矩阵

所述双目鱼眼相机包括左鱼眼相机与右鱼眼相机；

需要说明的是，相机畸变标定模型可以选择多种，由于本技术主要针对全景相机，故使用MEI模型效果最佳；

上述标定是利用开源算法Kalibr算法进行标定相机内外参数。

步骤S30：根据所述相机畸变参数对双目鱼眼图像进行校正得到针孔相机模型图像后将所述激光点云数据进行投影，获取每个激光点云数据对应的投影点；根据所述投影点将所述激光点云数据进行分类得到激光点对，并根据所述投影点获取下一帧双目鱼眼图像对应的所有投影点；

步骤S40:根据所述激光点对以及所述激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息；

步骤S50：根据所述最优状态信息及所述针孔相机模型图像对所述点云地图中的各个所述激光点云数据进行着色，得到各个所述激光点云数据对应的颜色信息；

所述激光点云数据对应的颜色信息为：

式中，和c_s表示更新前后的地图点云点像素，γ_s表示观测像素，/>以及分别代表更新前后地图点协方差以及观测像素协方差；σ_s表示跟随时间变化的协方差变化，本申请实施例中的σ_s根据经验值选择0.1。

需要说明的是，上述本申请实施例在步骤S30中将激光点云数据进行投影得到2d图像，其中，首先对左鱼眼相机进行投影，然后根据投影的像素坐标通过双线性插值计算RGB值，然后对对应的点云颜色参数进行更新；更新点云颜色使用贝叶斯公式进行更新赋色，如图2、图3、图4及图5所示，得到包含纹理信息的点云地图；

上述本申请实施例由于将双目相机的残差加入至VIO系统(即上述视觉里程估计操作的系统)中，最终得到的约束更加准确，赋色系统鲁棒性相比之前有了一定的提高，同时使用双目鱼眼相机进行赋色，使赋色效果有了一定提升；

上述本申请首先利用激光雷达实时采集每一帧的激光点云数据，同时利用陀螺仪采集每一帧对应的IMU数据；将每一帧的激光点云数据和IMU数据通过迭代误差扩展卡尔曼滤波算法进行融合解算得到IMU位姿，通过迭代误差扩展卡尔曼滤波算法可以保障位姿解算速度快，并且其解算位姿的结果精准；进而通过激光雷达外参和IMU外参对IMU数据进行解算得到每一帧对应的激光雷达位姿，将激光点云数据根据激光雷达位姿进行拼接从而得到点云地图；使得到的点云地图更加精准；

进一步获取每一帧对应的双目鱼眼图像，根据双目鱼眼图像对双目鱼眼相机进行标定，得到相机内参(即相机畸变参数)和相机的坐标转换矩阵，可为后续点云地图的颜色处理提供处理的依据；

进而根据相机内参对双目鱼眼图像进行校正得到针孔相机模型图像后，将点云地图中的激光点云数据投影到针孔相机模型图像中，得到2d图像(即将3d的激光点云数据投影到针孔相机模型图像中得到每个3d的激光点云数据对应的2d像素点(即为投影点)，从而由所有的2d像素点得到的整个2d图像)；根据2d像素点的投影位置从而将激光点云数据分类为激光点对，并同时根据投影点的位置通过光流得到下一帧的投影点；

进一步根据激光点对和激光雷达位姿进行视觉里程估计，从而得到鱼眼相机的最优状态，保障点云地图中每一帧激光点云数据都处在鱼眼相机的拍摄范围内，提高后续着色效果；

最终使用贝叶斯公式对激光点云进行赋色，从而得到有纹理信息点云地图。

具体地，如图6所示，在步骤S10中，根据所述激光点云数据及所述IMU数据通过迭代误差扩展卡尔曼滤波算法(esikf)进行融合解算得到IMU位姿，包括如下操作步骤：

步骤S11：基于所述IMU数据对所述激光点云数据分别进行运动畸变去除操作，得到多个目标激光点云数据；基于所有所述目标激光点云数据构建目标激光点云数据集合；

步骤S12：根据所述目标激光点云数据集合获取每一帧对应的点云数据集合；遍历各帧对应的点云数据集合，根据各个所述目标激光点云数据的坐标(目标激光点云数据的坐标通常指3D坐标，利用3D坐标计算距离的方式为现有技术，本申请不再赘述)，计算获取当前帧k中的平面点pk至前一帧k-1中平面激光点云集合中各个平面点的平面距离；获取所述平面距离中距离最短对应的(前一帧k-1中的平面点)四个对照平面点构建对照平面；并获取所述对照平面的中心点pj；对当前所述帧k中的各个平面点pk构建总激光点云残差约束r；

所述总激光点云残差约束r表示为：

r＝u^T(p_k-p_j)；

式中，u^T表示拟合平面的法向量，由上述四个对照平面点行拟合求出(四个对照平面点即为前一帧k-1中的四个平面点)；p_k为当前帧k中的一个平面点；p_j为四个对照平面点中的中心点；最终计算出的r为点p_k到对照平面的距离(即上述总激光点云残差约束r)。

所述点云数据集合包括平面激光点云集合；进一步地，上述平面激光点云集合中处在各帧对应的平面的边缘处的多个平面点(或称目标激光点云数据)构成了边缘激光点云集合；

所述平面激光点云数据集合包括多个平面点；所述平面点为目标激光点云数据集合中的目标激光点云数据；

所述边缘激光点云集合包括多个边缘点；所述边缘点为目标激光点云数据集合中的具有边缘性质(边缘性质即指的是处在各帧对应的平面的边缘处)的目标激光点云数据；

步骤S13：根据当前帧k的第一IMU数据及下一帧k+1的第二IMU数据进行积分解算，得到在第k+1帧的IMU位姿信息；

所述第k+1帧的IMU状态信息包括第k+1帧的IMU姿态信息^WR_Ik+1、第k+1帧的IMU速度信息^wv_k+1、第k+1帧的IMU位置信息^wp_k+1；

其中，所述第k+1帧的IMU姿态信息^WR_Ik+1、第k+1帧的IMU速度信息^wv_k+1、第k+1帧的IMU位置信息^wp_k+1分别表示为：

式中，代表k+1帧的IMU姿态信息；/>表示k帧的IMU姿态信息；/>表示k帧IMU的角速度信息；/>表示陀螺仪随机游走误差信息；/>表示陀螺仪测量白噪声误差信息(随机游走误差以及白噪声误差初始会使用开源算法imu_utils进行标定得到，后续在算法进行后续的优化)；^wv_k+1表示第k+1帧的IMU速度信息；^wv_k表示第k帧的IMU速度信息；^wg表示IMU重力加速度信息；/>表示k帧IMU加速度测量值；/>表示IMU加速度计的随机游走噪声；表示k帧的加速度计白噪声；^wp_k表示k帧的IMU位置信息；

需要说明的是，上述本申请实施例首先对激光点云数据进行运动畸变去除操作得到目标激光点云数据，激光点云数据是通过激光雷达扫描物体并测量反射光的时间和强度得到的，然而，在物体运动或传感器运动的情况下，由于扫描过程中存在相对运动，会引入运动畸变，导致点云数据偏离真实位置或出现形变；这种畸变会影响后续的点云处理任务，如建图、定位、目标识别等；因此，进行激光点云数据运动畸变去除操作可以提高激光雷达提高激光点云数据的定位精度，并可改善建图质量，同时可以降低后续算法的计算复杂度；

进而根据每一帧的目标激光雷达点云数据的坐标进行分析得到每一帧下的平面点，利用当前帧的各个平面点分别计算得到与前一帧的平面点中距离最短的四个平面点(即上述对照平面点)，从而得到对照平面以及对照平面的中心点，进而对当前帧的平面点进行构建总激光点云残差约束r，用来衡量匹配的质量和准确性的指标；

进一步根据当前帧的IMU数据和下一帧的IMU数据进行计算得到下一帧的IMU位姿信息；

上述第k帧为第k时刻采集到的激光点云数据，这是因为在三维重建、视觉SLAM(Simultaneous Localization and Mapping)、自动驾驶等领域中，通常使用连续的帧(frames)来表示不同时刻的点云数据，以捕捉环境的运动和变化；每一帧对应着某个时刻的点云数据，可以用于建立环境模型、进行定位与地图构建等任务。

具体地，如图7所示，在步骤S10中，根据所述IMU位姿以及所述激光雷达外参与IMU外参计算得到的激光雷达位姿，包括如下操作步骤：

步骤S101：获取第k帧的状态量x_k；根据所述第k帧的状态量x_k进行先验推导得到第k+1帧的状态量x_k+1；并根据所述第k帧的状态量x_k及所述第k+1帧的状态量x_k+1计算获取第k帧的状态误差量δx_k；

所述第k帧的状态量x_i为：

式中，表示第k帧的IMU姿态，/>表示k帧的IMU位置，/>表示第k帧的IMU速度，/>分别表示陀螺仪和加速度在第k帧的随机游走误差，^Gg^T表示重力加速度，分别表示IMU坐标系到相机坐标系的转换矩阵以及传感器采集时间差，φ表示相机内参f_x、f_y、c_x以及c_y；

所述第k+1帧的状态量x_k+1为：

其中，

所述第k帧的状态误差量δx_k为：

需要说明的是，上述本实施例通过当前帧k的状态量进行先验推导从而得到下一帧k+1的状态量，从而计算得到当前帧k的状态误差量，可以作为实际观测数据与预测状态之间的差异，同时可为后续计算提供观测更新值。

步骤S102：根据所述第k帧的状态误差量δx_i通过状态递推方程计算获取第k+1帧的状态先验值根据所述第k+1帧的状态先验值/>计算获取先验协方差矩阵

所述第k+1帧的状态先验值为：

其中， 表示第k帧状态的先验值；

所述先验协方差矩阵为：

其中Q代表噪声协方差矩阵，和/>代表状态递推矩阵；

需要说明的是，上述实施例中先验协方差矩阵提供了对系统状态估计的置信度信息，可以在状态更新过程中对观测数据和先验估计进行加权，从而得到更准确的后验估计。

步骤S103：根据所述状态递推方程及所述先验协方差矩阵/>根据测量方程进行计算获取后验状态误差；

所述后验状态误差的计算方式为：

其中，(状态观测矩阵)；

需要说明的是，上述本申请实施例通过计算后验状态误差，可以将观测数据与先验状态进行比较并融合，从而获得对系统当前状态的更准确估计。后验状态误差的计算提供了一种机制，将先验状态校正为更接近真实状态的后验状态。

步骤S104：根据所述先验协方差矩阵计算获取测量协方差矩阵R；

所述测量协方差矩阵为：

步骤S105：根据所述状态观测矩阵H及所述测量协方差矩阵R以及所述先验协方差矩阵计算获取卡尔曼滤波增益；

所述卡尔曼滤波增益为：

K＝(H^TR^-1H+P^-1)^-1H^TR^-1；

需要说明的是，卡尔曼滤波增益用于将测量数据与先验估计进行加权融合，从而得到更新后的后验估计。增益的计算利用了观测模型(状态观测矩阵)和测量噪声的特性(测量协方差矩阵)，以及先验估计的不确定性(先验协方差矩阵)。通过信息的融合，卡尔曼滤波增益能够有效地结合先验估计和观测数据，提供更准确的后验估计结果。

步骤S106：利用所述卡尔曼滤波增益更新获取第k+1帧的后验状态矩阵并基于所述第k+1帧的后验状态矩阵/>对所述协方差矩阵进行更新得到后验协方差矩阵，完成首次迭代并设定迭代计数器，将所述迭代计数器迭代次数加一；并重复上述操作，记录所述迭代次数；

所述第k+1帧的后验状态矩阵为：

所述后验协方差矩阵为：

步骤S107：根据所述第k+1帧的后验状态矩阵与所述第k+1帧的状态先验值计算获取先后变化量E；预设先后变化量最大阈值E'与最大迭代次数阈值；在所述先后变化量E大于或等于所述先后变化量最大阈值E'时或所述迭代次数达到最大迭代次数阈值时停止迭代操作，获取当前迭代次数对应的第k+1帧的后验状态矩阵/>为激光雷达位姿；

需要说明的是，上述本申请通过不断的迭代操作，使后延状态矩阵不断趋近于真实水平，并利用预设的最大迭代次数阈值防止过耦合；

上述本申请实施例通过不断迭代调整得到最终的激光雷达位姿，从而利用激光雷达位姿对每一帧的激光点云数据进行拼接从而得到点云地图，可提供更加精准完整的点云地图。

具体地，如图8所示，在步骤S30中，较佳的，所述根据所述投影点将所述激光点云数据进行分类得到激光点对，包括如下操作步骤：

步骤S31：判断所述投影点p_k坐标是否为负或者处于所述针孔相机模型图像的边缘；

步骤S32：若是，则将所述激光点云数据筛除；若否，则将所述激光点云数据分为左3d激光点对ζ_l与右3d激光点对ζ_r；并根据所述左3d激光点对ζ_l与右3d激光点对ζ_r对应的投影点p_k通过光流跟踪获取下一帧对应的投影点p_k。

较佳的，所述投影点p_k包括左鱼眼相机投影点p_kl与右鱼眼相机投影点p_kr；

其中，所述左鱼眼相机投影点p_kl为：

所述右鱼眼相机投影点p_kr为：

式中，表示左鱼眼相机投影点；/>表示右鱼眼相机投影点；K为相机内参；^aT_b表示a坐标系到b坐标系的坐标系转换矩阵；P_k为点云地图中的激光点云数据；n为归一化因子；

需要说明的是，上述本实施例对3d点(即上述点云地图中的激光点云数据)进行投影，如果当投影到左鱼眼相机得到的像素坐标为负或者在边界时则尝试投影到另一个相机，如果两个相机均投影失败，则该激光点认为不在相机视野内。最终将3d激光点分成左3d激光点对ζ_l和右3d激光点对ζ_r。然后对ζ_l和ζ_r中的2d点进行光流跟踪得到下一帧对应特征点的像素坐标(即上述投影点p_k)。

具体地，如图9所示，在步骤S40中，根据所述激光点对以及所述激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息，包括如下操作步骤：

步骤S41：根据所述激光点对计算获取重投影误差；

所述重投影误差为：

式中，^IT_c代表IMU到左鱼眼相机与右鱼眼相机的坐标系转换矩阵；

需要说明的是，在上述本申请实施例的重投影误差计算方式中，^IT_c根据左鱼眼相机与右鱼眼相机对进行判断，如果是左鱼眼相机，则为左鱼眼相机的外参，右鱼眼相机则为右鱼眼相机的外参；

为了计算方便，将上式进行分解。首先将3d点由世界坐标G系转换到相机坐标系c中，转换方法如下：

上式中，c_p根据左右目相机表示成c_l和c_R；Ps为第s个激光点云数据；

然后可以计算视觉残差：

同理，根据视觉残差趋近0的原则，可以得到残差线性化：

(x_k+1为当前设备真实的状态，后面是我们通过传感器估计得到的状态)

其中，

其中，针对右鱼眼相机和左鱼眼相机外参不同的情况，根据坐标转换矩阵可知IMU和右鱼眼相机的外参矩阵：

其中，表示IMU和右鱼眼相机的外参矩阵，/>为左鱼眼相机和右鱼眼相机的外参矩阵，/>为左鱼眼相机和IMU的外参矩阵；由于是全景相机，左鱼眼相机和右鱼眼相机的外参矩阵可以认为是：

步骤S42：基于所述重投影误差进行重投影误差作为观测，进行迭代误差卡尔曼滤波估计视觉里程计，得到视觉里程计的最优状态信息；

需要说明的是，上述本实施例使用卡尔曼滤波增益计算公式以及卡尔曼滤波后验公式进行状态更新迭代估计最优状态信息。

然后将重投影误差优化后的状态进行视觉第二轮优化，使用光度误差进行优化，即根据投影到图像上的颜色和地图点的颜色进行做差得到光度残差，进行同重投影误差类似的优化估计最优状态信息，即为鱼眼相机的最优位姿状态信息，保障点云地图中每一帧激光点云数据都处在鱼眼相机的拍摄范围内，提高后续着色效果。

综上所述，本发明实例提出的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，首先利用激光雷达实时采集每一帧的激光点云数据，同时利用陀螺仪采集每一帧对应的IMU数据；

然后对激光点云数据进行运动畸变去除操作得到目标激光点云数据，提高激光雷达提高激光点云数据的定位精度，并可改善建图质量，同时可以降低后续算法的计算复杂度；

进而根据每一帧的目标激光雷达点云数据的坐标进行分析得到每一帧下的平面点，利用当前帧的各个平面点分别计算得到与前一帧的平面点中距离最短的四个平面点(即上述对照平面点)，从而得到对照平面以及对照平面的中心点，进而对当前帧的平面点进行构建总激光点云残差约束r，用来衡量匹配的质量和准确性的指标；

进一步根据当前帧的IMU数据和下一帧的IMU数据进行计算得到下一帧的IMU位姿信息；

通过迭代误差扩展卡尔曼滤波算法可以保障位姿解算速度快，并且其解算位姿的结果精准；进而通过激光雷达外参和IMU外参对IMU数据进行解算得到每一帧对应的激光雷达位姿，将激光点云数据根据激光雷达位姿进行拼接从而得到点云地图；使得到的点云地图更加精准；

进一步获取每一帧对应的双目鱼眼图像，根据双目鱼眼图像对双目鱼眼相机进行标定，得到相机内参(即相机畸变参数)和相机的坐标转换矩阵，可为后续点云地图的颜色处理提供处理的依据；

进而根据相机内参对双目鱼眼图像进行校正得到针孔相机模型图像后，将点云地图中的激光点云数据投影到针孔相机模型图像中，得到2d图像(即将3d的激光点云数据投影到针孔相机模型图像中得到每个3d的激光点云数据对应的2d像素点(即为投影点)，从而由所有的2d像素点得到的整个2d图像)；根据2d像素点的投影位置从而将激光点云数据分类为激光点对，并同时根据投影点的位置通过光流得到下一帧的投影点；

进一步使用卡尔曼滤波增益计算公式以及卡尔曼滤波后验公式进行状态更新迭代估计最优状态信息。

然后将重投影误差优化后的状态进行视觉第二轮优化，使用光度误差进行优化，即根据投影到图像上的颜色和地图点的颜色进行做差得到光度残差，进行同重投影误差类似的优化估计最优状态信息，即为鱼眼相机的最优位姿状态信息，保障点云地图中每一帧激光点云数据都处在鱼眼相机的拍摄范围内，提高后续着色效果

最终使用贝叶斯公式对激光点云进行赋色，从而得到有纹理信息点云地图。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；本领域的普通技术人员可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (10)

1.一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，包括如下操作步骤：

根据实时采集得到的每一帧的激光点云数据和每一帧的IMU数据通过迭代误差扩展卡尔曼滤波算法融合解算得到IMU位姿之后，将所述激光点云数据根据通过根据所述IMU位姿和激光雷达外参、IMU外参计算得到的激光雷达位姿进行拼接生成点云地图；

根据实时获取得到的每一帧的双目鱼眼图像使用相机畸变模型对双目鱼眼相机进行标定得到所述双目鱼眼相机的内参及双目鱼眼相机的外参；

根据所述双目鱼眼相机的内参对双目鱼眼图像进行校正得到针孔相机模型图像后将所述激光点云数据进行投影，获取每个激光点云数据对应的投影点；根据所述投影点将所述激光点云数据进行分类得到激光点对；

根据所述激光点对以及所述激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息；

根据所述最优状态信息及所述针孔相机模型图像对所述点云地图中的各个所述激光点云数据进行着色，得到各个所述激光点云数据对应的颜色信息。

2.根据权利要求1所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述双目鱼眼相机包括左鱼眼相机与右鱼眼相机；所述双目鱼眼相机的内参为相机畸变参数；所述双目鱼眼相机的外参为相机的坐标转换矩阵。

3.根据权利要求2所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述根据实时采集得到的每一帧的激光点云数据和每一帧的IMU数据通过迭代误差扩展卡尔曼滤波算法融合解算得到IMU位姿，包括如下操作步骤：

基于所述IMU数据对所述激光点云数据分别进行运动畸变去除操作，得到多个目标激光点云数据；基于所有所述目标激光点云数据构建目标激光点云数据集合；

根据所述目标激光点云数据集合获取每一帧对应的点云数据集合；遍历各帧对应的点云数据集合，根据各个所述目标激光点云数据的坐标，计算获取平面距离中距离最短对应的四个对照平面点构建对照平面；并获取所述对照平面的中心点pj；对当前所述帧k中的各个平面点pk构建总激光点云残差约束r；

根据当前帧k的第一IMU数据及下一帧k+1的第二IMU数据进行积分解算，得到在第k+1帧的IMU位姿信息。

4.根据权利要求3所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述第k+1帧的IMU位姿信息包括第k+1帧的IMU姿态第k+1帧的IMU速度^wv_k+1、第k+1帧的IMU位置^wp_k+1。

5.根据权利要求4所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述平面距离为当前帧k中的平面点pk至前一帧k-1中平面激光点云集合中各个平面点的距离。

6.根据权利要求5所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述点云数据集合包括平面激光点云集合；

所述平面激光点云数据集合包括多个平面点；所述平面点为目标激光点云数据集合中的目标激光点云数据。

7.根据权利要求6所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述根据所述IMU位姿和激光雷达外参、IMU外参计算得到的激光雷达位姿，包括如下操作步骤：

根据第k帧的状态量x_k进行先验推导得到第k+1帧的状态量x_k+1；并根据所述第k帧的状态量x_k及所述第k+1帧的状态量x_k+1计算获取第k帧的状态误差量δx_k；

根据所述第k帧的状态误差量δx_i通过状态递推方程计算获取第k+1帧的状态先验值根据所述第k+1帧的状态先验值/>计算获取先验协方差矩阵/>

根据所述状态递推方程及所述先验协方差矩阵/>根据测量方程进行计算获取后验状态误差；

根据所述先验协方差矩阵计算获取测量协方差矩阵R；

根据所述状态观测矩阵H及所述测量协方差矩阵R以及所述先验协方差矩阵计算获取卡尔曼滤波增益；

利用所述卡尔曼滤波增益更新获取第k+1帧的后验状态矩阵并基于所述第k+1帧的后验状态矩阵/>对所述协方差矩阵进行更新得到后验协方差矩阵，完成首次迭代并设定迭代计数器，将所述迭代计数器迭代次数加一；并重复上述操作，记录所述迭代次数；

根据所述第k+1帧的后验状态矩阵与所述第k+1帧的状态先验值/>计算获取先后变化量E；根据预设的先后变化量最大阈值E'与最大迭代次数阈值通过重复上述步骤进行迭代操作获取激光雷达位姿。

8.根据权利要求7所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述根据所述投影点将所述激光点云数据进行分类得到激光点对，包括如下操作步骤：

判断所述投影点p_k坐标是否为负或者处于所述针孔相机模型图像的边缘；

若是，则将所述激光点云数据筛除；若否，则将所述激光点云数据分为左3d激光点对ζ_l与右3d激光点对ζ_r；并根据所述左3d激光点对ζ_l与右3d激光点对ζ_r对应的投影点p_k通过光流跟踪获取下一帧对应的投影点p_k。

9.根据权利要求8所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，所述投影点p_k包括左鱼眼相机投影点p_kl与右鱼眼相机投影点p_kr。

10.根据权利要求9所述的一种基于全景相机和激光雷达融合生成纹理信息点云地图方法，其特征在于，根据所述激光点对以及所述激光雷达位姿进行视觉里程估计，得到视觉里程计的最优状态信息，包括如下操作步骤：

根据所述激光点对计算获取重投影误差；

基于所述重投影误差进行重投影误差作为观测，进行迭代误差卡尔曼滤波估计视觉里程计，得到视觉里程计的最优状态信息。