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

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

背景技术：

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

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

技术实现思路

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

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

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

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

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

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

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

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

9、

10、

11、

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

13、γs表示观测投影点；

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

15、σs表示跟随时间变化的协方差变化。

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

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

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

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

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

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

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

23、r＝ut(pk-pj)；

24、式中，ut表示拟合平面的法向量；

25、pk为当前帧k中的一个平面点；

26、pj为对照平面的中心点。

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

28、较佳的，所述第k+1帧的imu位姿信息包括第k+1帧的imu姿态wrik+1、第k+1帧的imu速度wvk+1、第k+1帧的imu位置wpk+1；

29、其中，所述第k+1帧的imu姿态wrik+1、第k+1帧的imu速度wvk+1、第k+1帧的imu位置wpk+1分别表示为：

30、

31、

32、

33、式中，代表k+1帧的imu姿态信息；表示k帧的imu姿态信息；表示k帧imu的角速度信息；bgk表示陀螺仪随机游走误差信息；表示陀螺仪测量白噪声误差信息；wvk+1表示第k+1帧的imu速度信息；wvk表示第k帧的imu速度信息；wg表示imu重力加速度信息；表示k帧imu加速度测量值；表示imu加速度计的随机游走噪声；表示k帧的加速度计白噪声；wpk表示k帧的imu位置信息；

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

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

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

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

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

39、所述第k帧的状态量xi为：

40、

41、式中，表示第k帧的imu姿态信息，表示k帧的imu位置信息，gvkt表示第k帧的imu速度信息，分别表示陀螺仪和加速度在第k帧的随机游走误差信息，ggt表示重力加速度信息；分别表示imu坐标系到相机坐标系的转换矩阵以及传感器采集时间差，φ表示相机内参fx、fy、cx以及cy；

42、所述第k+1帧的状态量xk+1为：

43、

44、其中，

45、

46、

47、所述第k帧的状态误差量δxk为：

48、

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

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

51、

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

53、所述先验协方差矩阵为：

54、

55、其中q代表噪声协方差矩阵，和代表状态递推矩阵；

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

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

58、

59、式中，h为状态观测矩阵；所述状态观测矩阵为：

60、

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

62、所述测量协方差矩阵为：

63、

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

65、所述卡尔曼滤波增益为：

66、k＝(htr-1h+p-1)-1htr-1；

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

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

69、

70、所述后验协方差矩阵为：

71、

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

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

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

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

76、较佳的，所述投影点pk包括左鱼眼相机投影点pkl与右鱼眼相机投影点pkr；

77、其中，所述左鱼眼相机投影点pkl为：

78、

79、所述右鱼眼相机投影点pkr为：

80、

81、式中，表示左鱼眼相机投影点；表示右鱼眼相机投影点；k为相机内参；atb表示a坐标系到b坐标系的坐标系转换矩阵；pk为点云地图中的激光点云数据；n为归一化因子；

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

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

84、所述重投影误差为：

85、式中，itc代表imu到左鱼眼相机与右鱼眼相机的坐标系转换矩阵；

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

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

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

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

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

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

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