一种融合环视图像与激光雷达的自动泊车感知方法与流程

1.本发明涉及自动泊车技术领域，尤其是涉及一种融合环视图像与激光雷达的自动泊车感知方法。


背景技术：

2.现有的自动泊车方案的感知信息来源大多基于环视图像、环视图像与毫米波雷达融合或者环视图像与超声波传感器融合，环视图像用于检测车位，检测方法包括传统视觉的方法和深度学习方法，通过毫米波雷达、超声波传感器判断目标停车位是否有车，获取障碍物的距离，且现有的自动泊车方案多采用自身车辆的运动学信息进行定位，再用定位去跟踪停车位的坐标。
3.只依靠环视图像的泊车方案无法感知障碍物，超声波传感器的感知距离较短，毫米波雷达的感知角度范围具有局限性，存在感知盲区，上述传感器一般用于判断车位是否有车，对于障碍物只能做到停让；另外，只依赖车辆运动状态的定位方法会造成累积误差。因此，对于自动泊车功能，现有方法对环境的感知不够全面，一方面限制了自动泊车的智能程度，另一方面也难以保证自动泊车过程中的安全性，存在较大的隐患。


技术实现要素：

4.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种融合环视图像与激光雷达的自动泊车感知方法。
5.本发明的目的可以通过以下技术方案来实现：
6.一种融合环视图像与激光雷达的自动泊车感知方法，该方法包括以下步骤：
7.步骤1：通过安装在车身上的四个鱼眼摄像头获取原始图像，并对原始图像进行畸变纠正；
8.步骤2：通过选取四个已知地面坐标的标记点，分别对畸变纠正后的原始图像进行投影变换，得到投影到地面的四张俯瞰图；
9.步骤3：基于标记点的空间位置一致性对四张俯瞰图进行拼接，得到环视图像；
10.步骤4：将环视图像输入车位检测模型进行车位检测，得到停车位的像素坐标；
11.步骤5：根据停车位的像素坐标获取停车位的全局坐标，以进行自动泊车路径规划。
12.所述的步骤1中，四个鱼眼摄像头分别安装于车身的前方、后方、左侧和右侧，且车身左侧的鱼眼摄像头和车身右侧的鱼眼摄像头分别安装在车顶左右两侧的正中间，以提高投影变换后的环视图像的清晰度。
13.所述的步骤1中，对原始图像进行畸变纠正的过程具体为：
14.对原始图像进行畸变纠正，即进行去畸变，采用opencv的鱼眼摄像头模型对四个鱼眼摄像头分别进行内参标定，得到内参矩阵a和畸变系数组k，由内参矩阵a和畸变系数组k计算出图像的畸变纠正变换，并以映射形式表示畸变纠正变换的结果，根据映射形式的畸
变纠正变换的结果对每一帧输入的原始图像进行重映射，得到四个鱼眼摄像头对应的图像：
[0015][0016]
k＝(k1,k2,k3,k4)
[0017]
其中，a为内参矩阵，f
x
和fy分别为x方向的焦距参数和y方向的焦距参数，c
x
为x方向的光轴和像素原点的偏移，cy为y方向的光轴和像素原点的偏移，k为畸变系数组，k1、k2、k3和k4均为畸变系数。
[0018]
所述的步骤2中，得到投影到地面的四张俯瞰图的过程具体为：
[0019]
分别在每两个鱼眼摄像头的图像视野重叠处选择两个标记点，总共八个标记点，分别为a1、a2、b1、b2、c1、c2、d1和d2，标记点的地面坐标通过实地测量得到，标记点的像素坐标在原始图像上手动选取得到，每个鱼眼摄像头的图像通过对应的四个标记点进行投影变换。
[0020]
所述的步骤3中，得到环视图像的过程具体为：
[0021]
由两张相邻俯瞰图的视野重叠处的标记点确定一条直线，以直线作为相邻两张俯瞰图的拼接线，并采用掩膜的方式拼接四张俯视图像，以拼接线为边界把最终的环视图像分为四部分，分别制作四个图像的掩膜，并对掩膜的边缘进行高斯模糊处理，进而使环视图像之间的接缝处过渡自然，对四张图像的色彩进行平衡，消除环境光的影响，拼接完成后得到一张600*600像素的环视图像。
[0022]
所述的步骤4中，得到停车位的像素坐标的过程具体包括以下步骤：
[0023]
步骤401：采用基于yolov3的车位检测模型进行车位检测，得到环视图像的特征图，并将环视图像的特征图划分多个网格；
[0024]
步骤402：每个网格的特征图回归出对应范围内的停车位角点；
[0025]
步骤403：根据停车位角点的相对位置关系推理出有效的停车位的位置，通过计算和筛选得到有效的停车位完整的四个停车位角点的像素坐标，即一个停车位对应四个停车位角点，检测到两个停车位角点，经过计算和筛选之后，再根据先验信息推理出其余两个停车位角点。
[0026]
所述的步骤5中，获取停车位的全局坐标的过程具体包括以下步骤：
[0027]
步骤501：采用基于环视图像与激光雷达的快速标定法进行标定，获取停车位在激光雷达坐标系下的停车位消息slots_msg_lidar，并实时感知周围环境的障碍物信息；
[0028]
步骤502：采用激光雷达在点云地图上进行车辆定位，得到自车定位的全局坐标，进而形成自车定位的车辆位姿消息odometry；
[0029]
步骤503：将自车定位的车辆位姿消息odometry和停车位消息slots_msg_lidar的时间戳同步，从车辆位姿消息odometry中得到激光雷达坐标系相对于全局地图的旋转平移变换矩阵，将停车位角点在激光雷达坐标系下的雷达坐标转化为在全局地图下的全局坐标，得到停车位的全局坐标；
[0030]
步骤504：对于不同帧得到的停车位的全局坐标，分别计算不同帧的两个全局坐标的距离，判断距离是否小于设定的阈值，若是，则判定两者属于同一停车位，并计算同一停
车位不同帧的全局坐标的平均值，将其作为最终的检测结果；
[0031]
步骤505：通过激光雷达采集的雷达点云和障碍物信息实时构建障碍物栅格地图，选取检测结果中自动泊车的目标停车位，得到路径规划终点的全局位姿，进而在栅格地图上进行实时路径规划。
[0032]
所述的步骤501中，采用基于环视图像与激光雷达的快速标定法进行标定的过程具体包括以下步骤：
[0033]
步骤5011：在自动泊车的过程中，假设地面为水平面，忽略地面的雷达点云的高度坐标，简化为两个平面的坐标标定：
[0034]
步骤5012：采用一个3*3的单应性矩阵h描述像素坐标系到激光雷达坐标系的映射关系，映射关系的表达式为：
[0035][0036]
其中，(x1,y1)为停车位角点在像素坐标系下对应的像素坐标，(x2,y2)为停车位角点在激光雷达坐标系下对应的雷达坐标，h为单应性矩阵，h
11
、h
12
、h
13
、h
21
、h
22
、h
23
、h
31
、h
32
和h
33
均为单应性矩阵的元素，
∝
表示正比关系；
[0037]
步骤5013：由于转换的是齐次坐标，通过使单应性矩阵的元素h
33
＝1进行归一化：
[0038][0039]
步骤5014：选取标定点对，分别在雷达点云和环视图像中选取标定点对，通过在地面粘贴反光贴的方式标记标定点，在rviz中通过显示雷达点云的反射强度找到标记点，并获得该标记点在激光雷达坐标系下的雷达坐标，通过图像处理软件获得标记点在环视图像的像素坐标；
[0040]
步骤5015：根据采集到的标定点对求解环视图像平面和激光雷达地面平面之间的映射关系，即得到单应性矩阵h，并使得重投影误差最小化；
[0041]
步骤5016：标定完成后，在激光雷达的线束扫描到地面的位置放置锥形桶，将雷达点云投影到环视图像上进行验证，激光雷达点云的投影与图像对应；
[0042]
步骤5017：采用得到的单应性矩阵h将停车位角点的像素坐标转化为激光雷达坐标系下的雷达坐标，得到停车位的雷达坐标，并发布为停车位消息slots_msg_lidar。
[0043]
所述的步骤5015中，重投影误差最小化的计算公式为：
[0044][0045]
其中，ε表示重投影误差。
[0046]
所述的步骤5017中，将停车位角点的像素坐标转化为激光雷达坐标系下的雷达坐标的计算公式为：
[0047][0048]
其中，x2为停车位角点在激光雷达坐标系下的横坐标，y2为停车位角点在激光雷达坐标系下的纵坐标。
[0049]
与现有技术相比，本发明具有以下优点：
[0050]
1、本发明提出的一种融合环视图像与激光雷达的自动泊车感知方法，建立了环视图像和点云感知信息融合的桥梁，填补了激光雷达传感器在自动泊车应用中的技术空白，实现更智能的自动泊车功能；
[0051]
2、本发明将侧面摄像头安装在车顶，提高了环视感知的视野范围；
[0052]
3、本发明简化了标定的过程，是环视图像和激光雷达的感知信息融合的基础；
[0053]
4、将车位标记到全局地图进行规划的方式，简化了停车位的跟踪算法，融合了激光雷达信息，对环境的感知更加全面，提高了路径规划的智能性和鲁棒性，同时也保障了自动泊车过程中的安全。
附图说明
[0054]
图1为本发明的结构框架图。
[0055]
图2为本发明的鱼眼摄像头安装位置示意图。
[0056]
图3为本发明的环视图像拼接示意图。
[0057]
图4为环视图像激光雷达标定效果图。
[0058]
图5为检测结果示意图。
具体实施方式
[0059]
下面结合附图和具体实施例对本发明进行详细说明。
[0060]
实施例
[0061]
本发明公开了一种融合环视图像与激光雷达的自动泊车感知方法，该方法包括以下步骤：
[0062]
步骤1：通过安装在车上的四个鱼眼摄像头获取原始图像，并对原始图像进行畸变纠正；
[0063]
步骤2：通过选取四个已知地面坐标的标记点，分别对畸变纠正后的原始图像进行投影变换，得到投影到地面的四张俯瞰图；
[0064]
步骤3：基于标记点的空间位置一致性对四张俯瞰图进行拼接，得到环视图像；
[0065]
步骤4：将环视图像输入车位检测模型进行车位检测，得到停车位的像素坐标；
[0066]
步骤5：根据停车位的像素坐标获取停车位的全局坐标，以进行自动泊车路径规划。
[0067]
如图1所示，本发明采用四个视角为190度的鱼眼摄像头获取原始图像，四个鱼眼
摄像头分别安装于车身的前方、后方、左侧和右侧，且左侧鱼眼摄像头和右侧鱼眼摄像头分别安装在车顶左右两侧正中间，环视视野范围达到12
×
12m2，同时提高了投影变换之后的环视图像的清晰度，鱼眼摄像头的安装位置如图2所示。
[0068]
在步骤1中，对鱼眼摄像头获取的原始图像进行畸变纠正，即去畸变，采用opencv的鱼眼摄像头模型对四个鱼眼摄像头进行标定，得到内参矩阵a和畸变系数组k，计算原始环视图像的畸变纠正变换，并以映射形式表示畸变纠正变换的结果，基于映射形式的畸变纠正变换的结果进行重映射变换，对之后每一帧输入的图像都进行相同的重映射变换，得到四张图像，以提高图像处理的速度。
[0069][0070]
k＝(k1,k2,k3,k4)
[0071]
其中，a为内参矩阵，f
x
和fy分别为x方向的焦距参数和y方向的焦距参数，c
x
为x方向的光轴和像素原点的偏移，cy为y方向的光轴和像素原点的偏移，k为畸变系数组，k1、k2、k3和k4均为畸变系数。
[0072]
如图3所示，在步骤2中，选取已知地面坐标的四个标记点进行俯视投影变换，每个鱼眼摄像头的图像需要四个标记点进行投影变换，因此分别在每两个摄像头的图像视野重叠处选择两个标记点，总共八个标记点，分别为a1、a2、b1、b2、c1、c2、d1和d2，标记点的地面坐标通过实地测量得到，标记点的像素坐标在图像上手动选取得到。
[0073]
在步骤3中，由两张相邻俯瞰图的视野重叠处的标记点确定一条直线，以直线作为相邻两张俯瞰图的拼接线，并采用掩膜的方式拼接四张俯视图像，以拼接线为边界把最终的环视图像分为四部分，分别制作四个图像的掩膜，并对掩膜的边缘进行高斯模糊处理，进而使环视图像之间的接缝处过渡自然，对四张图像的色彩进行平衡，消除环境光的影响，拼接完成后得到一张600*600像素的环视图像。
[0074]
在步骤4中，采用基于yolov3的车位检测模型进行车位检测，得到环视图像的特征图，将环视图像的特征图划分网格，每个网格的特征图回归出对应范围内的停车位角点，再根据停车位角点的相对位置关系，推理出有效的停车位的位置，进而计算出停车位完整的四个停车位角点的像素坐标，得到停车位的像素坐标。
[0075]
车位检测的输出结果为停车位的像素坐标，进行自动泊车路径规划时，需要获取停车位在全局坐标系(即全局坐标系)下的全局坐标，并实时感知周围环境的障碍物信息，采用激光雷达(激光slam)在点云地图上进行车辆定位，得到自车定位的全局坐标，并形成自车定位的车辆位姿消息odometry，基于激光雷达采集的雷达点云实时构建障碍物栅格地图，结合自车定位和停车位角点的全局坐标，在栅格地图上进行实时路径规划。
[0076]
计算停车位在全局地图下的全局坐标：
[0077]
本发明首先标定了像素坐标系和激光雷达坐标系，即环视图像2d坐标系和激光雷达3d坐标系，得到停车位角点在激光雷达3d坐标系下的雷达坐标，对于环视图像2d坐标系和激光雷达3d坐标系的标定，由于环视图像不是由相机直接获取的，不存在相机内参等参数，无法通过求解pnp(perspective-n-point)的方法进行标定，若将单个鱼眼相机分别与激光雷达坐标系标定，再转换到环视图像2d坐标系，则会使标定流程变得复杂繁琐，因此，
本发明采用基于环视图像与激光雷达的快速标定方法进行标定，由于车位检测只需要关注环视图像中地面的图像信息，在标定的过程中只关注激光雷达扫描到地面的雷达点云，在自动泊车的过程中，假设地面为水平面，忽略地面雷达点云的高度坐标，简化为两个平面坐标的标定，因此采用一个3*3的单应性矩阵h描述像素坐标到雷达坐标的映射关系，设(x1,y1)为停车位角点在像素坐标系下对应的像素坐标，(x2,y2)为停车位角点在激光雷达坐标系下对应的雷达坐标，则有
[0078][0079]
其中，(x1,y1)为停车位角点在像素坐标系下对应的像素坐标，(x2,y2)为停车位角点在激光雷达坐标系下对应的雷达坐标，h为单应性矩阵，h
11
、h
12
、h
13
、h
21
、h
22
、h
23
、h
31
、h
32
和h
33
均为单应性矩阵的元素，
∝
表示正比关系；
[0080]
由于转换的是齐次坐标，使h
33
＝1进行归一化：
[0081][0082]
一共有8个未知数，因此求解单应性矩阵h需要8个方程，需要至少选取4组标定点对，分别在雷达点云和环视图像中选取标定点对，通过在地面粘贴反光贴的方式标记标定点，在rviz通过显示雷达点云的反射强度找到标记点并获得标记点在激光雷达坐标系下的雷达坐标，通过图像处理软件获得标记点在环视图像的像素坐标，采集到的标定点对用以求解环视图像平面和激光雷达地面平面之间的透视变换关系，即得到单应性矩阵h，并使得重投影误差最小化，重投影误差最小化的计算公式为：
[0083][0084]
其中，ε表示重投影误差。
[0085]
如图4所示，标定完成后，在激光雷达线束扫描到地面的位置放置锥形桶，将雷达点云投影到环视图像上进行验证，激光雷达点云的投影与图像吻合度高。
[0086]
采用标定过程中得到的单应性矩阵h，将停车位角点的像素坐标转化为激光雷达坐标系下的雷达坐标，得到停车位的雷达坐标，并发布为停车位消息slots_msg_lidar：
[0087][0088]
其中，x2为停车位角点在激光雷达坐标系下的横坐标，y2为停车位角点在激光雷达
坐标系下的纵坐标。
[0089]
获取自车定位的车辆位姿消息odometry和停车位消息slots_msg_lidar，同步两者的时间戳，从车辆位姿消息odometry中得到激光雷达坐标系相对于全局地图的旋转平移变换矩阵，将停车位角点在激光雷达坐标系下的雷达坐标转化为在全局地图下的全局坐标，得到停车位的全局坐标。
[0090]
如图5所示，对于不同帧得到的停车位的全局坐标，分别计算不同帧的两个全局坐标的距离，判断距离是否小于设定的阈值，若是，则判定两者属于同一停车位，对于同一停车位不同帧得到的全局坐标，计算不同帧的全局坐标的平均值，并将其作为最终的检测结果，选取检测结果作为自动泊车的目标停车位，得到路径规划终点的全局位姿，再进行路径规划。
[0091]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。