[发明专利]一种交通信号灯在先验地图中的标注方法

权利要求书

1.一种交通信号灯在先验地图中的标注方法，其特征在于，具体包括如下步骤：

步骤1，在自动驾驶车辆上安装IMU，激光雷达，相机和BDS/GPS接收机；

步骤2，对IMU、激光雷达和相机进行联合标定，从而实现激光雷达和相机坐标系的统一；

步骤3：对BDS/GPS接收机、激光雷达和相机进行联合标定；

步骤4：将自动驾驶车辆设置在距离交通信号灯x米范围内，控制自动驾驶车辆朝交通信号灯行驶，直至交通信号灯消失在激光雷达的视野中；

步骤5：基于激光雷达第t次采集的激光雷达点云数据的时间标签，将与该时间标签最近的相机采集的图像，与该时间标签最近的IMU采集的数据以及与该时间标签最近的BDS/GPS接收机采集的数据作为一组数据组；t＝1，2，…T；T为激光雷达采样的总次数；

步骤6：将激光雷达第t次采集到的激光雷达点云数据投影至第t组数据组的图像中，通过手工粗分割从图像中提取交通信号灯区域，并将交通信号灯区域中的雷达点云数据恢复至三维；从而得到与交通信号灯区域对应的雷达点云数据；

步骤7：将步骤6中的与交通信号灯区域对应的雷达点云数据存入第t组数据组中，得到第t个交通信号灯数据包；

步骤8：再次将第t个交通信号灯数据包中的激光雷达点云数据投影至该交通信号灯数据包中的图像中，利用矩形框提取图像中的交通信号灯区域，将位于矩形框四个顶点上的激光雷达点云作为目标点云，并计算目标点云的经度、纬度和高度；将目标点云的经度、纬度和高度作为矩形框四个顶点的经度、纬度和高度；从而得到该矩形框的尺寸以及该矩形框中心点的经度、纬度和高度；

步骤9：根据每个矩形的尺寸和每个矩形框中心点的经度、纬度和高度，计算交通信号灯的经度、纬度，高度和尺寸，并在地图中进行标注。

2.根据权利要求1所述的一种交通信号灯在先验地图中的标注方法,其特征在于，所述激光雷达沿自动驾驶车辆的中轴线水平安装在车顶上，所述相机沿自动驾驶车辆的中轴线水平安装在前挡风玻璃上。

3.根据权利要求1所述的一种交通信号灯在先验地图中的标注方法,其特征在于，所述步骤8计算位于矩形框四个顶点上的激光雷达点云的经度、纬度和高度具体为：针对任意一个矩形框，根据如下公式计算激光雷达的姿态，经度，纬度和高度：

其中λ，β，H，θ，γ，φ分别表示激光雷达的经度、纬度、高度、俯仰角、横滚角和航向角，为天线坐标系到地理坐标系的转换矩阵，所述天线为BDS/GPS接收机中的天线，x_b，y_b，z_b分别为激光雷达在天线坐标系中的坐标；θ_IMU，γ_IMU，分别为IMU输出的车辆的俯仰角、横滚角和航向角；λ_ant，β_ant，H_ant分别为BDS/GPS输出的天线的经度、纬度和高度；为IMU坐标系到激光雷达坐标系的转移矩阵，为IMU的姿态到天线姿态的旋转矩阵；ΔC为激光雷达在天线坐标系中的坐标到天线与激光雷达之间的经度差、纬度差和高度差的转换矩阵；

根据雷达的姿态计算雷达坐标系到地理坐标系的转换矩阵：

根据转换矩阵将位于矩形框左上、左下、右上、右下四个顶点的雷达激光点云在雷达坐标系下的坐标均转换为在地理坐标系下的坐标(x_LU,y_LU,z_LU)，(x_LD,y_LD,z_LD)，(x_RU,y_RU,z_RU),(x_RD,y_RD,z_RD)；

基于如下公式计算每个目标点云与激光雷达的经度差ΔL和纬度差Δλ：

其中，R_e为地球半径，x_Y＝x_LU，x_LD，x_RU或者x_RD；y_Y＝y_LU，y_LD，y_RU或者y_RD；基于每个目标点云与激光雷达的经度差ΔL和纬度差Δλ得到该目标点云的经纬度，将z_LU作为矩形左上顶点的高度，z_LD作为矩形左下顶点的高度，z_RU作为矩形右上顶点的高度，z_RD作为矩形右下顶点的高度。