导航系统中立体车库地图的构建方法

技术领域

本发明涉及自动导航技术领域，具体涉及一种导航系统中立体车库地图的构建方法。

背景技术

实时定位与地图构建是自动导航技术的关键步骤，它能够在未曾探索的环境中利用传感器收集环境信息，从而定位自身在环境中的位姿并构建环境地图；该技术能够识别认知障碍物与自身的距离情况，为路径规划、行驶避障提供帮助。实时定位与地图构建技术在立体车库中进行运用时，能让车辆转运小车按照规划路径行驶，在不需要人工驾驶汽车的情况下，完成汽车的取停，使用方便，但是现有的车辆转运小车实时定位与地图构建方法尚有不足，定位和建图的可靠性不太高。

公开号为CN112627611A的发明专利公开了一种智能AGV搬运机器人及其搬运方法，该搬运方式使用激光SLAM和二维码扫描仪：利用激光SLAM系统定位自身位姿，核心控制单元根据位姿控制运动模块驱动电机前往预定位置并调整自身姿态，预定位置处的二维码内存放下一阶段路径和路径终点位姿信息，由二维码扫描仪实现读取并交由核心控制单元解析内容计算当前位姿偏差，此后进入二次定位状态计算并获得微调整角点坐标，后续在原路径基础上规划新的曲线路径，最后回到由激光SLAM定位的路径移动过程。

该方案中定位方法有局限，仅依靠激光SLAM估算位姿，未采用惯性测量单元与其做交叉验证。其次，二维码中存放的路径信息固定，后期也是在该路径上修改规划，在多车交汇时容易出现路径重叠的弊端，若前车报错停车阻碍后车读取二维码，则会导致整个车库无法运转，自动智能的属性受到约束。再次激光SLAM方式仅截取雷达高度上障碍特征，通过飞行时间法计算距离，使用线特征替代平面特征的方法实现二维地图的绘制，大量空间信息与特征不被考虑和计算。最后，由于环境改变引起的二维码位置和码内信息更改需要人工重新设定，在应变性和时效性上稍欠考虑。因此，如何提供一种包含了地面车位线信息，精度高的立体车库地图构建方法，就需要进一步进行考虑。

发明内容

针对上述现有技术的不足，本发明所要解决的技术问题是：如何提供一种包含了地面车位线信息，定位精度高的导航系统中立体车库地图的构建方法。

为了解决上述技术问题，本发明采用的技术方案：

一种导航系统中立体车库地图的构建方法，包括以下步骤：

(1)对具有自动导航功能的小车上的传感器进行标定工作，传感器包括相机、激光雷达和惯性测量单元，相机能够获取小车前方的图像，通过对传感器的标定工作得到相机的内参矩阵和外参矩阵，以及激光雷达与惯性测量单元的联合数据转换矩阵；每个相机的内参矩阵和外参矩阵不同，通过标定的工作确定其中的具体参数，其目的是用于实现相机数据到融合点云的数据转换时保障精确性，每个激光雷达的测量效果不同，通过标定得到的联合数据转换矩阵能够补偿测量值，使之接近真实值，在激光雷达数据到点云子图的数据转换时效果更好；

(2)通过惯性测量单元获取小车的位姿转变数据，对位姿转变数据进行扩展卡尔曼滤波处理，得到矫正后小车在立体车库地图中的估计位姿；估计位姿是后续点云子图进行匹配工作时的初值，通过扩展卡尔曼滤波处理后的估计位姿相比于惯性测量单元输出的原始位姿信息更准确，通过扩展卡尔曼处理后估计位姿内部的噪声信息更少，位姿偏移量更少；

(3)通过激光雷达获取小车的环境点云数据，根据联合数据转换矩阵将环境点云数据转换成点云子图，将估计位姿作为初值并使用该时刻点云子图与前一时刻点云子图进行匹配，得到基于激光雷达的匹配位姿；在估计位姿的基础上使用激光雷达的信息对位姿进行矫正，得到匹配位姿是最接近真实位姿的值，根据匹配位姿得到的导航系统地区更加精确；

(4)通过相机获取小车前方的图像数据和深度数据后，利用内参矩阵和外参矩阵对图像数据和深度数据进行分割并投影到同一时间戳的点云子图中，得到带多传感器的融合点云子图；该步骤将相机数据添加到点云子图中，形成多传感器融合的点云子图，使融合点云子图包含更多的特征；

(5)对融合点云子图进行预处理，按点云高度将融合点云子图中的地面点云进行剥离，对地面点云做图像处理得到仅车位线的地面点云，再更新至融合点云子图中；立体车库的车位线信息是地图信息的重要部分，通过图像处理提取这部分特征进行保存，使更新后的融合点云子图信息更丰富；

(6)通过基于激光雷达的匹配位姿在小车的导航系统地图中补充更新后的融合点云子图，构建立体车库的地图；使用融合点云子图构建导航系统地图，将上述各种传感器信息全部包含其中，并且使用匹配位姿作为参考，更接近真实地图的样貌，尽可能地减小了误差。

作为优化，在步骤(1)中，所述相机的标定工作包括使用matlab工具箱中的CameraCalibrator对进行张正友标定，在cameraParameter属性中得到相机的内参矩阵和外参矩阵，内参矩阵中包括焦距信息f

所述激光雷达和所述惯性测量单元的标定工作包括使用lidar-align功能包进行标定，将所述惯性测量单元获取的位姿转变数据与所述激光雷达获取的环境点云数据输入后，使用最小二乘法计算得到使距离平均最小的转换矩阵。

作为优化，所述步骤(2)中，所述扩展卡尔曼滤波方式包括：

根据前一时刻所述小车的位姿p

/>

其中g(u

作为优化，在步骤(3)中，所述点云子图转换方式包括：

首先将所述激光雷达采样数据z

设立位姿为ξ＝(x

作为优化，在所述步骤(3)中，该时刻所述点云子图与前一时刻所述点云子图匹配方式包括：

使用Scan-Match方法寻求最优(x

其中M

作为优化，在步骤(4)中，所述融合点云子图的生成方式包括：

使用ROS功能包中的depthimage_to_laserscan对所述相机获取的图像数据进行分割成等大小的子图像，子图像附带的深度信息转换为距焦点的长度，通过所述内参矩阵和所述外参矩阵将子图像在所述点云子图中的位置算出,,相机坐标点(u,v)与空间坐标点(X,Y,Z)计算公式为：

其中f

作为优化，在步骤(5)中，所述地面点云的剥离方式包括：

设置地面高度阈值为d，将所述融合点云子图所有Z轴高度绝对值小于d的点放置于集合S中，该集合S为地面信息特征候选集合，计算集合S内点的平滑度，剔除离群值后即可认为集合S为被剥离的地面信息点云。

作为优化，在步骤(5)中，所述地面点云进行剥离的方式包括：

抽取所述相机获取的图像数据关键帧，然后将RGB图像转换为HSV图像，进行高斯滤波和图像增强后转回RGB图像以提高图像差异性，再将图像灰度化并二值化以突出图像内车位线边缘信息，之后使用连通区域标记加强边缘信息之间的联系，再利用边缘检测将车位线轮廓信息提取，最后使用膨胀腐蚀处理以轮廓构建完整车位线信息。

作为优化，在步骤(6)中，使用更新后的所述融合点云子图对所述小车的导航系统地图进行补充的方式包括：

设置链表保存每个时刻的所述点云子图和所述匹配位姿，当某时刻之后的连续帧成功完成步骤(3)的匹配，则实现前后帧依赖，根据关键帧之间的时间差进行位姿修正。

综上所述，本发明的有益效果在于：

1、使用深度相机和激光雷达作为环境信息传感器，激光雷达获取360度的同高度地图信息，深度相机获取小车正前方的图像信息，实现范围广但信息片面的激光雷达与信息量大但范围小的深度相机优势互补，两者共同获取的融合点云子图相比于单一传感器的更加详细，构建出的立体车库地图特征点也更多，便于后续定位和导航工作，在立体车库中的可靠性更好；

2、使用关键帧依赖的方式将前后两帧中小车的位姿和融合点云子图排序，在闭环检测时将最后时刻帧与前某时刻重合帧位置重合，再从后往前依次对两帧中间的每个关键帧进行修正，实现闭环工作的完善；

3、惯性测量单元记录的位姿转变数据会使用扩展卡尔曼滤波进行处理，传感器的检测值与实际值之间存在随机游走，并且该帧游走值会随着关键帧依赖传递至下一帧，这种误差累积的现象需要使用滤波方法削弱，同时传感器在未检测出数据值时并未返回零值，而是在靠近零的一定阈值内变化，这种零偏现象也在滤波过程中削弱；

4、提供的一系列图像处理技术能够实现地面上的车位线信息的提取，将其余无关信息从地面内容中剔除，仅保留后续小车实现停取功能的车位线信息，使得其信息分明、相互独立、地图简洁。

附图说明

为了使发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：

图1为本发明的流程图；

图2为本发明中图像处理的流程图。

具体实施方式

如图1和图2所示，本具体实施方式中的导航系统中立体车库地图的构建方法，包括以下步骤：

(1)对具有自动导航功能的小车上的传感器进行标定工作，传感器包括相机、激光雷达和惯性测量单元，相机能够获取小车前方的图像，通过对传感器的标定工作得到相机的内参矩阵和外参矩阵，以及激光雷达与惯性测量单元的联合数据转换矩阵；

(2)通过惯性测量单元获取小车的位姿转变数据，对位姿转变数据进行扩展卡尔曼滤波处理，得到矫正后小车在立体车库地图中的估计位姿；

(3)通过激光雷达获取小车的环境点云数据，根据联合数据转换矩阵将环境点云数据转换成点云子图，将估计位姿作为初值并使用该时刻点云子图与前一时刻点云子图进行匹配，得到基于激光雷达的匹配位姿；

(4)通过相机获取小车前方的图像数据和深度数据后，利用内参矩阵和外参矩阵对图像数据和深度数据进行分割并投影到同一时间戳的点云子图中，得到带多传感器的融合点云子图；

(5)对融合点云子图进行预处理，按点云高度将融合点云子图中的地面点云进行剥离，对地面点云做图像处理得到仅车位线的地面点云，再更新至融合点云子图中；

(6)通过基于激光雷达的匹配位姿在小车的导航系统地图中补充更新后的融合点云子图，构建立体车库的地图。

本具体实施方式中，在步骤(1)中，所述相机的标定工作包括使用matlab工具箱中的CameraCalibrator对进行张正友标定，在cameraParameter属性中得到相机的内参矩阵和外参矩阵，内参矩阵中包括焦距信息f

所述激光雷达和所述惯性测量单元的标定工作包括使用lidar-align功能包进行标定，将所述惯性测量单元获取的位姿转变数据与所述激光雷达获取的环境点云数据输入后，使用最小二乘法计算得到使距离平均最小的转换矩阵。

本具体实施方式中，所述步骤(2)中，所述扩展卡尔曼滤波方式包括：

根据前一时刻所述小车的位姿p

其中g(u

本具体实施方式中，在步骤(3)中，所述点云子图转换方式包括：

首先将所述激光雷达采样数据z

设立位姿为ξ＝(x

本具体实施方式中，在所述步骤(3)中，该时刻所述点云子图与前一时刻所述点云子图匹配方式包括：

使用Scan-Match方法寻求最优(x

其中M

本具体实施方式中，在步骤(4)中，所述融合点云子图的生成方式包括：

使用ROS功能包中的depthimage_to_laserscan对所述相机获取的图像数据进行分割成等大小的子图像，子图像附带的深度信息转换为距焦点的长度，通过所述内参矩阵和所述外参矩阵将子图像在所述点云子图中的位置算出,相机坐标点(u,v)与空间坐标点(X,Y,Z)计算公式为：

其中f

本具体实施方式中，在步骤(5)中，所述地面点云的剥离方式包括：

设置地面高度阈值为d，将所述融合点云子图所有Z轴高度绝对值小于d的点放置于集合S中，该集合S为地面信息特征候选集合，计算集合S内点的平滑度，剔除离群值后即可认为集合S为被剥离的地面信息点云。

本具体实施方式中，在步骤(5)中，所述地面点云进行剥离的方式包括：

抽取所述相机获取的图像数据关键帧，然后将RGB图像转换为HSV图像，进行高斯滤波和图像增强后转回RGB图像以提高图像差异性，再将图像灰度化并二值化以突出图像内车位线边缘信息，之后使用连通区域标记加强边缘信息之间的联系，再利用边缘检测将车位线轮廓信息提取，最后使用膨胀腐蚀处理以轮廓构建完整车位线信息。

本具体实施方式中，在步骤(6)中，使用更新后的所述融合点云子图对所述小车的导航系统地图进行补充的方式包括：

设置链表保存每个时刻的所述点云子图和所述匹配位姿，当某时刻之后的连续帧成功完成步骤(3)的匹配，则实现前后帧依赖，根据关键帧之间的时间差进行位姿修正。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管通过参照本发明的优选实施例已经对本发明进行了描述，但本领域的普通技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离所附权利要求书所限定的本发明的精神和范围。