CN113673386A - 一种交通信号灯在先验地图中的标注方法 - Google Patents

一种交通信号灯在先验地图中的标注方法 Download PDF

Info

Publication number
CN113673386A
CN113673386A CN202110900620.0A CN202110900620A CN113673386A CN 113673386 A CN113673386 A CN 113673386A CN 202110900620 A CN202110900620 A CN 202110900620A CN 113673386 A CN113673386 A CN 113673386A
Authority
CN
China
Prior art keywords
traffic signal
signal lamp
laser radar
latitude
longitude
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110900620.0A
Other languages
English (en)
Inventor
李子月
曾庆化
刘建业
杨闯
赖际舟
吕品
刘玉超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202110900620.0A priority Critical patent/CN113673386A/zh
Publication of CN113673386A publication Critical patent/CN113673386A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种交通信号灯在先验地图中的标注方法，首先，利用自动驾驶车辆硬件平台完成多传感器在线采集；然后，对多传感器采集的位置、姿态、图像、点云、数据进行匹配和对齐；最后，通过数据融合获取交通信号灯在先验地图中的位置和尺寸信息。本发明可以更加具体和精确地标注和描述交通信号灯信息，为自动驾驶图像在线识别算法提供辅助。

Description

一种交通信号灯在先验地图中的标注方法
技术领域
本发明属于信息感知与识别技术领域。
背景技术
随着DARPA重启无人车技术研究，并先后于2004年、2005年和2007年举办了三届智能车挑战赛，谷歌大力投入自动驾驶技术研究，近年来全球范围内掀起了自动驾驶技术研究的热潮。参与者既有谷歌、苹果、Uber、BAT等顶尖互联网企业，也有特斯拉、奔驰、宝马、通用、凯迪拉克等传统车企，同时，英特尔、英伟达、博世、Mobileye等零部件供应商也参与其中，另外自动驾驶领域创业公司也如雨后春笋般应运而生。
自动驾驶技术经过十多年发展，逐渐从实验室研究、到封闭环境技术竞赛、再到固定场景示范应用，虽然取得了长足的进展，但距离实际应用仍有较大距离。大型车企普遍采用的由低级辅助驾驶到高级辅助驾驶，再到限定场景自动驾驶，最后到达全自动驾驶的技术路径，然而却并不十分顺利。例如，特斯拉公司车辆配备的高级辅助驾驶***，至今仍事故频出。谷歌、BAT互联网公司及创业公司则采用的由低速、封闭场景逐渐向外延伸的技术路线，发展也并不理想，目前仍然只能在封闭场景低速驾驶状态下执行简单确定性任务，并且可靠性较低。
交通信号灯是城市道路的重要组成部分，然而，基于视觉的交通信号灯识别却一直以来都是个技术难题。尤其是夜晚、极端天气环境下，交叉路口、多信号灯等复杂路况场景应用，这一问题将变得更加困难。先验地图辅助提高交通信号灯识别准确率的有效方法，但是，目前的高精度地图更加注重位置、坡度、朝向等路面信息，对交通信号灯的描述不够具体，难以满足实用要求。
发明内容
发明目的：为了解决上述现有技术存在的问题，本发明提供了一种交通信号灯在先验地图中的标注方法。
技术方案：本发明提供了一种交通信号灯在先验地图中的标注方法，具体包括如下步骤：
步骤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：根据每个矩形的尺寸和每个矩形框中心点的经度、纬度和高度，计算交通信号灯的经度、纬度，高度和尺寸，并在地图中进行标注。
进一步的，所述激光雷达沿自动驾驶车辆的中轴线水平安装在车顶上，所述相机沿自动驾驶车辆的中轴线水平安装在前挡风玻璃上。
进一步的，所述步骤8计算位于矩形框四个顶点上的激光雷达点云的经度、纬度和高度具体为：
针对任意一个矩形框，根据如下公式计算激光雷达的姿态，经度，纬度和高度：
Figure BDA0003199660140000031
其中λ，β，H，θ，γ，φ分别表示激光雷达的经度、纬度、高度、俯仰角、横滚角和航向角，
Figure BDA0003199660140000032
为天线坐标系到地理坐标系的转换矩阵，所述天线为BDS/GPS接收机中的天线，xb，yb，zb分别为激光雷达在天线坐标系中的坐标；θIMU，γIMU，
Figure BDA0003199660140000033
分别为IMU输出的车辆的俯仰角、横滚角和航向角；λant，βant，Hant分别为BDS/GPS输出的天线的经度、纬度和高度；
Figure BDA0003199660140000034
为IMU坐标系到激光雷达坐标系的转移矩阵，
Figure BDA0003199660140000035
为IMU的姿态到天线姿态的旋转矩阵；ΔC为激光雷达在天线坐标系中的坐标到天线与激光雷达之间的经度差、纬度差和高度差的转换矩阵；
根据雷达的姿态计算雷达坐标系到地理坐标系的转换矩阵：
Figure BDA0003199660140000036
根据转换矩阵
Figure BDA0003199660140000037
将位于矩形框左上、左下、右上、右下四个顶点的雷达激光点云在雷达坐标系下的坐标均转换为在地理坐标系下的坐标(xLU,yLU,zLU)，(xLD,yLD,zLD)，(xRU,yRU,zRU),(xRD,yRD,zRD)；
基于如下公式计算每个目标点云与激光雷达的经度差ΔL和纬度差Δλ：
Figure BDA0003199660140000038
其中，Re为地球半径，xY＝xLU，xLD，xRU或者xRD；yY＝yLU，yLD，yRU或者yRD；基于每个目标点云与激光雷达的经度差ΔL和纬度差Δλ得到该目标点云的经纬度，将zLU作为矩形左上顶点的高度，zLD作为矩形左下顶点的高度，zRU作为矩形右上顶点的高度，zRD作为矩形右下顶点的高度。
进一步的，该方法还包括交通信号灯朝向的标注，具体为：
步骤A：根据第t个交通信号灯数据包中IMU和BDS/GPS接收机输出的信息得到自动驾车辆的航向Hvihicle，并通过激光雷达的安装参数，将Hvihicle转换为激光雷达坐标系Y轴的方向角HLiDAR；计算T个激光雷达坐标系Y轴的方向角的均值得到HLiDAR；
步骤B：根据第t个矩形框的左上、左下、右上、右下四个顶点在激光雷达坐标系下的坐标值，计算交通信号灯和激光雷达坐标系Y轴之间的相对角度αt：
Figure BDA0003199660140000041
其中(xlu,ylu,zlu)、(xld,yld,zld)、(xru,yru,zru)、(xrd,yrd,zrd)分别为第t个矩形框的左上、左下、右上、右下四个顶点在激光雷达坐标系下的坐标值；
步骤C：计算T个相对角度的平均值，并将该平均值与HLiDAR相加得到交通信号灯方向角参数HTL，并将HTL标注在地图中。
进一步的，所述步骤9具体为：对矩形框的尺寸和矩形框的中心点的经度，纬度和高度分别求平均值，将经度的平均值作为交通信号灯的经度，纬度的平均值作为交通信号灯的纬度，高度的平均值作为交通信号灯的高度，尺寸的平均值作为交通信号灯的尺寸。
有益效果：相比于现有技术，本发明中提供的标注方法更加精确，详细，尤其是将交通信号灯的尺寸、中心点、朝向进行详细标注。自动驾驶车辆在行驶过程中，可以通过本发明的多传感器融合技术获取交通信号灯在图像中的兴趣区域，本发明极大地提高交通信号灯视觉识别的准确度和计算效率，促进自动驾驶技术的产业化。
附图说明
图1为本发明的方法流程图；
图2为本发明中计算交通信号灯的经度、纬度和高度的方法流程图。
具体实施方式
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
如图1所示，本实施例提供一种交通信号灯在先验地图中的标注方法，具体包括：
1、传感器联合标定；
2、多传感器数据采集和融合预处理；
3、交通信号灯融合信息提取和标注。
传感器联合标定
本发明利用自动驾驶车辆作为数据采集平台，主要搭载激光雷达、IMU、BDS/GPS接收机、视觉传感器，通过对多传感器数据融合和预处理，获取交通信号灯基本数据。其中，传感器标定工作是数据采集的基础，本发明的标定方法具体如下。
本发明中的传感器联合标定主要包括：首先对IMU、激光雷达和相机进行联合标定，实现传感器的姿态统一；然后对BDS/GPS接收机、激光雷达和相机进行联合标定，实现传感器的位置统一。
IMU、激光雷达和视觉联合标定的主要目的，是将激光雷达坐标***一到以相机中心为原点的笛卡尔坐标系(本实施例中选择载体坐标系)，具体方法分为：
(1)激光雷达和相机分别沿车辆中轴线，水平安装于车顶和前挡风玻璃；
(2)将自动驾驶车辆放置于水平地面，并通过水平仪对激光雷达和相机的俯仰、横滚安装角度进行调节，使得激光雷达和相机均保持水平；
(3)在车辆正前方3米处，居中放置垂直于地面的柱状标识物(高度为2米，宽度不小于5厘米，作为激光雷达和相机航向角调节和对准的标志物)对激光雷达和相机的航向角进行调节，使得激光雷达、相机的Y轴和车辆中轴线重合，指向车辆正前方。
(4)在以相机为原点的载体坐标系中，测量激光雷达的坐标(x,y,z)，并以此作为激光雷达坐标系的平移向量。
(5)激光雷达、相机调整完成之后，读取IMU输出的姿态和航向数据。
BDS/GPS接收机、激光雷达和相机进行联合标定的主要目的，是通过BDS/GPS接收机的接收机天线、激光雷达和相机的相对位置关系，实时将高精度BDS/GPS接收机的定位数据转化为激光雷达和相机的经度、纬度和高度信息。
以激光雷达为例，通过IMU、BDS/GPS接收机输出的姿态、航向、位置数据，激光雷达的位置和姿态参数的计算如式(1)所示。
Figure BDA0003199660140000061
其中λ，β，H，θ，γ，φ分别表示激光雷达的经度、纬度、高度、俯仰角、横滚角和航向角，
Figure BDA0003199660140000062
为天线坐标系到地理坐标系的转换矩阵，所述天线为BDS/GPS接收机中的接收天线，天线坐标系定义为：天线中心为原心，沿车头方向为Y轴，横向为X轴，纵向为z轴；xb，yb，zb分别为激光雷达在天线坐标系中的坐标；θIMU，γIMU，
Figure BDA0003199660140000063
分别为IMU输出的车辆的俯仰角、横滚角和航向角；λant，βant，Hant分别为BDS/GPS输出的天线的经度、纬度和高度；
Figure BDA0003199660140000064
为IMU坐标系到激光雷达坐标系的转移矩阵，
Figure BDA0003199660140000065
为IMU的姿态到天线姿态的旋转矩阵；ΔC为为激光雷达在天线坐标系中的坐标到天线与激光雷达之间的经度差、纬度差和高度差的转换矩阵。
多传感器数据采集和融合预处理
在传感器联合标定的基础之上，在人工驾驶模式下，利用自动驾驶车辆进行交通信号灯道路数据采集，然后对激光雷达点云、图像、IMU、定位数据进行融合预处理，具体过程如下。
(1)人工驾驶自动驾驶车辆，选择市区道路，在正常光照条件下，利用相机采集真实道路上交通信号灯x米范围内的图像信息(本实施例中x＝100)；激光雷达采集道路点云数据；IMU和BDS/GPS接收机采集车体的姿态、航向、位置数据；
(2)以5Hz更新频率的64线激光雷达数据帧起始时间标签为基准，选取和雷达点云时间标签最接近的图像、姿态、航向、定位数据，组成一组数据组；
(3)将激光雷达点云投影到图像上，通过手工初步粗分割图像中的交通信号灯区域，获取和交通信号灯区域对应的激光雷达点云，将该些激光雷达点云由二维数据恢复至三维，将三维的和交通信号灯区域对应的激光雷达点云放入相应的数据组，将改数据组作为交通信号灯数据包。
(4)从距离交通信号灯100米开始，直到交通信号灯消失在激光雷达的视野中，将每一次采集的激光雷达数据点云均按照上述步骤(2)，(3)步骤处理，得到T个交通信号灯数据包，T为激光雷达在某个道路上的总采样次数，按照道路名称和时间标签对每个交通信号灯数据包进行编号。
交通信号灯融合信息提取和标注
在对多传感器数据融合处理的基础之上，本发明对交通信号灯数据包进行信息提取，创新性地通过矩形框及其中心点坐标表示交通信号灯位置，并且通过多传感器数据对交通信号灯的朝向进行计算，在此基础之上，增加交通信号灯的尺寸、语义、属性信息，形成格式化的交通信号灯数据，具体计算过程如下。
交通信号灯矩形框、中心点及尺寸计算
激光雷达点云测量的是交通信号灯的三维外形特征，而视觉图像测量的是二维外形和色彩特征。因此，激光雷达和视觉数据融合不仅需要坐标***一、时间标签对齐，再次将交通信号灯数据包中的三维激光雷达点云数据投影到交通信号灯数据包中图像中。经过投影后，根据图像中的交通信号灯色彩特征，利用矩形框对图像进行划分。从而得到矩形框的左上、左下、右上、右下四个顶点对应的激光雷达点云。
根据IMU和BDS/GPS接收机可以实时计算得到激光雷达的经度，纬度，高度，姿态和航向数据，从而计算交通信号灯的经度、纬度、高度，如图2所示，具体方法如下：
(1)根据雷达的姿态计算雷达坐标系到地理坐标系的转换矩阵：
Figure BDA0003199660140000071
通过激光雷达坐标系到地理坐标系的姿态转移矩阵
Figure BDA0003199660140000072
可以将位于矩形框四个顶点的激光雷达点云在在激光雷达坐标系上的坐标转化为在地理坐标系下的坐标(xLU,yLU,zLU)，(xLD,yLD,zLD)，(xRU,yRU,zRU),(xRD,yRD,zRD)；本实施例将位于矩形框四个顶点的激光雷达点云作为目标点云。
通过地球半径参数和如下公式计算每个目标点云与激光雷达相对位置关系位置差，所述相对位置关系位置差包括经度差ΔL和纬度差Δλ：
Figure BDA0003199660140000073
其中，Re为地球半径，xY＝xLU，xLD，xRU或者xRD；yY＝yLU，yLD，yRU或者yRD；基于每个目标点云与激光雷达的经度差ΔL和纬度差Δλ得到每个顶点的经纬度。
通过激光雷达绝对位置和每个目标点云与激光雷达相对位置关系位置差，计算四个目标点云的经度和纬度，并将四个目标点云的经度和纬度作为矩形框中相应顶点的经度和纬度，将zLU作为左上顶点的高度，zLD作为左下顶点的高度，zRU作为右上顶点的高度，zRD作为右下顶点的高度。根据矩形框四个顶点的经度，纬度和高度可以计算得到该矩形框中心点的经度、纬度、高度和尺寸。
根据每个矩形框的中心点的经度、纬度、高度和尺寸，计算交通信号灯的经度、纬度，高度和尺寸，并在地图中进行标注。具体计算交通信号灯的经度、纬度，高度和尺寸的方法为：
对T个矩形框中心点的经度，纬度，高度和尺寸分别求平均值，将经度的平均值作为交通信号灯的经度，纬度的平均值作为交通信号灯的纬度，高度的平均值作为交通信号灯的高度，尺寸的平均值作为交通信号灯的尺寸。
于融合信息的交通信号灯朝向计算
本发明根据激光雷达点云、IMU和BDS/GPS接收机融合数据，获取交通信号灯的朝向信息。在实际道路上行驶，车辆和交通信号灯相向而行，因此，交通信号灯背面的朝向和车头保持一致，本发明中的交通信号灯方向为背面方向。交通信号灯朝向计算的主要原理如下：
利用第t个交通信号灯数据包中IMU和BDS/GPS接收机输出的信息，可以获得车辆航向Hvihicle，并通过激光雷达的安装参数，可以将Hvihicle转化为激光雷达坐标系Y轴的方向角HLiDAR；计算T个激光雷达坐标系Y轴的方向角的均值
Figure BDA0003199660140000081
激光雷达坐标系中，交通信号灯区域的左上、左下、右上、右下四个顶点对应的坐标为(xlu,ylu,zlu)、(xld,yld,zld)、(xru,yru,zru)、(xrd,yrd,zrd)，交通信号灯和激光雷达Y轴之间的相对角度的计算方法为：
Figure BDA0003199660140000082
αt为T个交通信号灯数据包中第t个交通信号灯数据包对应的相对角度；
计算T个相对角度的平均值，并将该平均值与
Figure BDA0003199660140000091
相加得到交通信号灯方向角参数HTL，并将HTL标注在地图中。
(3)其他信息提取和标注
提取交通信号灯路段的BDS/GPS接收机的工作状态(RTK实时动态差分状态、PDOP位置精度因子值)、道路状态(直行、T型路口、Y型路口、十字交叉路口、立交桥、隧道)以及信号灯语义(直行、左转、直行左转组合、直行左转右转组合)。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

Claims (5)

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计算位于矩形框四个顶点上的激光雷达点云的经度、纬度和高度具体为：针对任意一个矩形框，根据如下公式计算激光雷达的姿态，经度，纬度和高度：
Figure FDA0003199660130000021
其中λ，β，H，θ，γ，φ分别表示激光雷达的经度、纬度、高度、俯仰角、横滚角和航向角，
Figure FDA0003199660130000022
为天线坐标系到地理坐标系的转换矩阵，所述天线为BDS/GPS接收机中的天线，xb，yb，zb分别为激光雷达在天线坐标系中的坐标；θIMU，γIMU，
Figure FDA0003199660130000023
分别为IMU输出的车辆的俯仰角、横滚角和航向角；λant，βant，Hant分别为BDS/GPS输出的天线的经度、纬度和高度；
Figure FDA0003199660130000024
为IMU坐标系到激光雷达坐标系的转移矩阵，
Figure FDA0003199660130000025
为IMU的姿态到天线姿态的旋转矩阵；ΔC为激光雷达在天线坐标系中的坐标到天线与激光雷达之间的经度差、纬度差和高度差的转换矩阵；
根据雷达的姿态计算雷达坐标系到地理坐标系的转换矩阵：
Figure FDA0003199660130000026
根据转换矩阵
Figure FDA0003199660130000027
将位于矩形框左上、左下、右上、右下四个顶点的雷达激光点云在雷达坐标系下的坐标均转换为在地理坐标系下的坐标(xLU,yLU,zLU)，(xLD,yLD,zLD)，(xRU,yRU,zRU),(xRD,yRD,zRD)；
基于如下公式计算每个目标点云与激光雷达的经度差ΔL和纬度差Δλ：
Figure FDA0003199660130000028
其中，Re为地球半径，xY＝xLU，xLD，xRU或者xRD；yY＝yLU，yLD，yRU或者yRD；基于每个目标点云与激光雷达的经度差ΔL和纬度差Δλ得到该目标点云的经纬度，将zLU作为矩形左上顶点的高度，zLD作为矩形左下顶点的高度，zRU作为矩形右上顶点的高度，zRD作为矩形右下顶点的高度。
4.根据权利要求1所述的一种交通信号灯在先验地图中的标注方法,其特征在于，该方法还包括交通信号灯朝向的标注，具体为：
步骤A：根据第t个交通信号灯数据包中IMU和BDS/GPS接收机输出的信息得到自动驾车辆的航向Hvihicle，并通过激光雷达的安装参数，将Hvihicle转换为激光雷达坐标系Y轴的方向角HLiDAR；计算T个激光雷达坐标系Y轴的方向角的均值得到
Figure FDA0003199660130000031
步骤B：根据第t个矩形框的左上、左下、右上、右下四个顶点在激光雷达坐标系下的坐标值，计算交通信号灯和激光雷达坐标系Y轴之间的相对角度αt：
Figure FDA0003199660130000032
其中(xlu,ylu,zlu)、(xld,yld,zld)、(xru,yru,zru)、(xrd,yrd,zrd)分别为第t个矩形框的左上、左下、右上、右下四个顶点在激光雷达坐标系下的坐标值；
步骤C：计算T个交通信号灯和激光雷达坐标系Y轴之间的相对角度的平均值，并将该平均值与
Figure FDA0003199660130000033
相加得到交通信号灯方向角参数HTL，并将HTL标注在地图中。
5.根据权利要求1所述的一种交通信号灯在先验地图中的标注方法,其特征在于，所述步骤9具体为：对矩形框的尺寸和矩形框的中心点的经度，纬度和高度分别求平均值，将经度的平均值作为交通信号灯的经度，纬度的平均值作为交通信号灯的纬度，高度的平均值作为交通信号灯的高度，尺寸的平均值作为交通信号灯的尺寸。
CN202110900620.0A 2021-08-06 2021-08-06 一种交通信号灯在先验地图中的标注方法 Pending CN113673386A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110900620.0A CN113673386A (zh) 2021-08-06 2021-08-06 一种交通信号灯在先验地图中的标注方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110900620.0A CN113673386A (zh) 2021-08-06 2021-08-06 一种交通信号灯在先验地图中的标注方法

Publications (1)

Publication Number Publication Date
CN113673386A true CN113673386A (zh) 2021-11-19

Family

ID=78541740

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110900620.0A Pending CN113673386A (zh) 2021-08-06 2021-08-06 一种交通信号灯在先验地图中的标注方法

Country Status (1)

Country Link
CN (1) CN113673386A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114166137A (zh) * 2021-11-26 2022-03-11 沪东中华造船(集团)有限公司 一种船对船加注间距智能检测***及方法
GB2613400A (en) * 2021-12-01 2023-06-07 Motional Ad Llc Automatically detecting traffic signals using sensor data

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114166137A (zh) * 2021-11-26 2022-03-11 沪东中华造船(集团)有限公司 一种船对船加注间距智能检测***及方法
GB2613400A (en) * 2021-12-01 2023-06-07 Motional Ad Llc Automatically detecting traffic signals using sensor data
GB2613400B (en) * 2021-12-01 2024-01-10 Motional Ad Llc Automatically detecting traffic signals using sensor data

Similar Documents

Publication Publication Date Title
CN109945858B (zh) 用于低速泊车驾驶场景的多传感融合定位方法
US11802769B2 (en) Lane line positioning method and apparatus, and storage medium thereof
CN108802785B (zh) 基于高精度矢量地图和单目视觉传感器的车辆自定位方法
CN107246868B (zh) 一种协同导航定位***及导航定位方法
CN109099901B (zh) 基于多源数据融合的全自动压路机定位方法
CN106441319B (zh) 一种无人驾驶车辆车道级导航地图的生成***及方法
US10860871B2 (en) Integrated sensor calibration in natural scenes
CN106651953B (zh) 一种基于交通指示牌的车辆位姿估计方法
CN104865578B (zh) 一种室内停车场高精细地图生成装置及方法
CN109446973B (zh) 一种基于深度神经网络图像识别的车辆定位方法
CN109901580A (zh) 一种无人机与无人地面机器人协作循径避障***及其方法
CN105588563A (zh) 一种自动驾驶中双目相机与惯性导航单元联合标定方法
CN111006655A (zh) 机场巡检机器人多场景自主导航定位方法
CN112904395B (zh) 一种矿用车辆定位***及方法
CN113673386A (zh) 一种交通信号灯在先验地图中的标注方法
EP4124829A1 (en) Map construction method, apparatus, device and storage medium
CN111426320A (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN102967311A (zh) 基于天空偏振分布模型匹配的导航定位方法
CN114509065A (zh) 地图构建方法、***、车辆终端、服务器端及存储介质
CN113419235A (zh) 一种基于毫米波雷达的无人机定位方法
CN115027482A (zh) 智能驾驶中的融合定位方法
CN114485654A (zh) 一种基于高精地图的多传感器融合定位方法及装置
CN112446915B (zh) 一种基于图像组的建图方法及装置
CN115265493A (zh) 一种基于非标定相机的车道级定位方法及装置
CN112577499B (zh) 一种vslam特征地图尺度恢复方法及***

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination