[发明专利]一种基于激光雷达辅助视觉的地图构建方法、装置和系统

权利要求书

1.一种基于激光雷达辅助视觉的地图构建方法，其特征在于，包括以下步骤：

对观测同一个观测对象的激光雷达和视觉相机进行标定，得到雷达坐标系与相机坐标系之间的标定转换参数；

获取所述激光雷达采集的雷达点云数据序列，采用ICP点云匹配方法，根据所述标定转换参数对所述雷达点云数据序列进行匹配求解，得到所述雷达点云数据序列对应的中间转换参数序列；

获取所述视觉相机采集的相机图像数据序列，根据所述中间转换参数序列对所述相机图像数据序列进行三角化处理，得到优化图像特征序列；

采用最小二乘法，根据所述雷达点云数据序列和所述优化图像特征序列对所述中间转换参数序列进行优化，得到目标转换参数序列；

根据所述目标转换参数序列和所述相机图像数据序列进行定位和建图，完成地图构建。

2.根据权利要求1所述的基于激光雷达辅助视觉的地图构建方法，其特征在于，所述标定转换参数包括所述雷达坐标系与所述相机坐标系之间的第一旋转矩阵和第一平移向量；

则得到所述标定转换参数，具体包括以下步骤：

对所述激光雷达和所述视觉相机进行外参粗标定，得到所述雷达坐标系与所述相机坐标系之间的初始旋转矩阵和初始平移向量；

采用预设标定工具分别获取所述观测对象在所述相机坐标系下的第一已知特征向量集合以及在所述雷达坐标系下的第二已知特征向量集合；

采用所述最小二乘法，根据所述第一已知特征向量集合和所述第二已知特征向量集合，构建第一优化求解函数；

所述第一优化求解函数的表达式具体为：

其中，f₁为所述第一优化求解函数，R₁和T₁分别为所述第一旋转矩阵和所述第一平移向量，y_k为所述第一已知特征向量集合中的第k个第一已知特征向量，x_k为所述第二已知特征向量集合中的第k个第二已知特征向量，第k个第一已知特征向量与第k个第二已知特征向量对应，第一已知特征向量的总数和第二已知特征向量的总数均与所述观测对象的特征点总数相等，均为K；

将所述初始旋转矩阵和所述初始平移向量作为初始值，并输入至所述优化求解函数中，求解得到所述第一旋转矩阵和所述第一平移向量。

3.根据权利要求2所述的基于激光雷达辅助视觉的地图构建方法，其特征在于，所述雷达点云数据序列的帧数大于或等于2；

得到所述中间转换参数序列，具体包括以下步骤：

选取所述雷达点云数据序列中的第j帧点云数据与第j-1帧点云数据，根据第j帧点云数据与第j-1帧点云数据中的所有共同特征点，分别提取第j帧点云数据中至少一个第一特征坐标向量以及第j-1帧点云数据中与每个第一坐标特征向量一一对应的至少一个第二特征坐标向量；

根据所述第一旋转矩阵和所述第一平移向量，将所有第一特征坐标向量分别转化为在所述相机坐标系下的第三特征坐标向量，并将所有第二特征坐标向量分别转化为在所述相机坐标系下的第四特征坐标向量；

采用所述ICP点云匹配方法，根据所有第三特征坐标向量和所有第四特征坐标向量，构建选取的两帧点云数据之间的匹配求解函数；

第j帧点云数据与第j-1帧点云数据之间的匹配求解函数的表达式具体为：

其中，为第j帧点云数据与第j-1帧点云数据之间的匹配求解函数，和分别为第j帧点云数据与第j-1帧点云数据之间的第二旋转矩阵和第二平移向量，为第j帧点云数据在所述相机坐标系下的第i个第三特征坐标向量，为第j-1帧点云数据在所述相机坐标系下的第i个第四特征坐标向量；第三特征坐标向量的数量和第四特征坐标向量的数量相等，均为n；

求解选取的两帧点云数据之间的匹配求解函数，得到选取的两帧点云数据之间的第二旋转矩阵和第二平移向量；

遍历所述雷达点云数据序列中的每相邻两帧点云数据，分别得到每相邻两帧点云数据之间的第二旋转矩阵和第二平移向量；并根据所有第二旋转矩阵和所有第二平移向量，得到所述中间转换参数序列。