CN110745140B - 一种基于连续图像约束位姿估计的车辆换道预警方法 - Google Patents

一种基于连续图像约束位姿估计的车辆换道预警方法 Download PDF

Info

Publication number
CN110745140B
CN110745140B CN201911030362.4A CN201911030362A CN110745140B CN 110745140 B CN110745140 B CN 110745140B CN 201911030362 A CN201911030362 A CN 201911030362A CN 110745140 B CN110745140 B CN 110745140B
Authority
CN
China
Prior art keywords
vehicle
target vehicle
coordinate system
target
point
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.)
Active
Application number
CN201911030362.4A
Other languages
English (en)
Other versions
CN110745140A (zh
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.)
Tsinghua University
Original Assignee
Tsinghua University
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 Tsinghua University filed Critical Tsinghua University
Priority to CN201911030362.4A priority Critical patent/CN110745140B/zh
Publication of CN110745140A publication Critical patent/CN110745140A/zh
Application granted granted Critical
Publication of CN110745140B publication Critical patent/CN110745140B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W50/08—Interaction between the driver and the control system
    • B60W50/14—Means for informing the driver, warning the driver or prompting a driver intervention
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/06—Direction of travel
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2520/00—Input parameters relating to overall vehicle dynamics
    • B60W2520/10—Longitudinal speed
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30256—Lane; Road marking

Abstract

本发明提出的一种基于连续图像约束位姿估计的车辆换道预警方法，包括：建立目标车辆车身坐标系、自车车身坐标系和像素坐标系；目标车辆图像采集与处理及自车状态测量；利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束；利用连续多帧图像建立同一目标车辆位置运动约束；联立各约束，求解目标车辆在自车车身坐标系中的位置和方位，即确定目标车辆的三维边界框；根据确定的三维边界框，确定目标车辆与自车的横纵向相对位置关系；评估目标车辆对自车换道的碰撞风险。本发明使用相机采集连续多帧侧后方车辆图像，高精度地估计他车位姿，从而更精确的计算他车与自车的横纵向距离，进行车辆换道预警。

Description

一种基于连续图像约束位姿估计的车辆换道预警方法
技术领域
本发明涉及高级辅助驾驶系统环境感知技术领域，具体涉及一种基于连续图像约束位姿估计的车辆换道预警方法。
背景技术
车辆行驶环境感知是换道预警系统的核心内容，为了评估换道风险，车载传感器系统需要对周围其他车辆的位置、速度大小与方向等进行感知。现有换道预警研究中多采用相机、雷达等相对廉价的感知方案，如宴晓娟采用相机采集侧后方图像信息检测目标二维边界框进行测距测速(晏晓娟.基于机器视觉的侧后方目标检测算法及换道预警系统研究[D].江苏大学,2017)，朱愿采用毫米波雷达采集侧后方车辆的速度与位置信息(朱愿.基于视觉和雷达的智能车辆自主换道决策机制与控制研究[D].中国人民解放军军事医学科学院,2014)。但相机的二维边界框不能表达深度及姿态信息，而雷达传感器通常将其他车辆聚类为一个目标点，因此无法精确获取目标车辆的三维尺寸与方位等信息。
在换道预警系统中，判断目标车辆与自车的相对位置，尤其是横向相对位置，是评估换道风险的关键。相对于目标点，三维的目标边界框可以提高车辆横向相对位置关系识别的准确性，减少横向位置关系识别不准确造成的虚警。
交通场景中的三维目标检测通常依赖激光雷达点云或双目相机深度点云，这些传感器价格昂贵，且精确的三维检测算法尚不成熟。Mousavian等人提出首先采用成熟的二维检测算法估计目标的三维尺寸，然后利用三维边界框与二维边界框的投影约束关系估计目标三维边界框的位姿(Mousavian A,Anguelov D,Flynn J,et al.3D Bounding BoxEstimation Using Deep Learning and Geometry[J].2016.)。Mousavian等人首先训练了一个深度卷积神经网络(CNN)来回归目标物的三维尺寸，然后根据三维边界框和二维边界框的投影约束得到了三维边界框的转换矩阵，最终确定了物体的三维位姿。Mousavian等的方法针对随机图像，目标物的类别和方位具有多样性，需要附加额外的约束确定目标方位。此外图像往往只有单帧，因此该方法只使用一帧图像进行目标三维尺寸及位姿估计，无法利用目标物多帧信息进行联合优化，因此该方法获得三维边界框的精度还有待提高。
发明内容
本发明目的是为了克服已有技术的不足之处，针对换道预警情景中目标车辆在自车的侧后方姿态相对固定，且可以对同一目标连续多帧跟踪观测的特点，提出一种基于连续图像约束位姿估计的车辆换道预警方法。
为了实现上述目的，本发明采用如下技术方案：
本发明提出的一种基于连续图像约束位姿估计的车辆换道预警方法，在自车车顶中心固定有相机，自车尾部中心搭载有毫米波雷达、全球定位系统GPS和惯性导航单元IMU；其特征在于，所述车辆换道预警方法包括以下步骤：
S1建立目标车辆车身坐标系、自车车身坐标系和像素坐标系
S1.1建立目标车辆车身坐标系
将目标车辆三维边界框简化为一个长方体，以车辆前部中心接地点T为原点、向目标车辆后侧为Xt轴正向、向目标车辆右侧为Yt轴正向、向上为Zt轴正向，建立与目标车辆固定的目标车辆车身坐标系T-XtYtZt；在所述目标车辆车身坐标系中，设三维边界框的8个角点的坐标分别为：
Figure BDA0002249979190000021
Figure BDA0002249979190000022
其中，dx,dy,dz为目标车辆三维边界框沿Xt轴，Yt轴和Zt轴方向的长度；
S1.2建立自车车身坐标系
以相机中心Oe为原点，向自车后侧为Xe轴正向、向车辆左侧为Ye轴正向、向上为Ze轴正向，建立与自车固定的自车车身坐标系Oe-XeYeZe；设目标车辆三维边界框前部的中心接地点T在自车车身坐标系中的坐标为：T＝[tx,ty,0]T；设目标车辆三维边界框在自车坐标系中的方位为R＝[θ,0,0]，其中θ为目标车辆的朝向角，即目标车辆纵轴与Ye轴间的夹角；
S1.3建立像素坐标系
以相机采集图像的左上角Op为原点、向右为U轴正向、向下为V轴正向，建立像素坐标系Op-UV；
S2目标车辆图像采集与处理及自车状态测量
S2.1采用相机采集当前时刻自车后方的图像作为当前帧图像；
S2.2通过CNN网络模型对采集的当前帧图像进行回归处理得到目标车辆三维边界框的尺寸dx,dy,dz；
S2.3通过车辆目标检测算法得到当前帧图像中目标车辆在像素坐标系下的矩形二维边界框并计算其上5个特征点的坐标；其中，第一特征点、第三特征点作为二维边界框的上边线两端点且分别与三维边界框的第八角点、第六角点相对应，第二特征点位于二维边界框的上边线上且与三维边界框的第七角点相对应，第四特征点位于二维边界框的左边线上且与三维边界框的第四角点相对应，第五特征点位于二维边界框的下边线上且与三维边界框的第一角点相对应；
S2.4对当前帧图像通过车道线检测算法得到像素坐标系下的各车道线方程；
S2.5采用毫米波雷达测量得到当前时刻目标车辆在自车车身坐标系中的速度；
S2.6采用GPS和IMU测量当前时刻自车的位置、速度和朝向角；
S3利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束
S3.1根据目标车辆车身坐标系和像素坐标系的投影关系，目标车辆车身坐标系中任意点X0＝[X,Y,Z]T与像素坐标系中对应点x0＝[u,v]T满足以下变换关系：
Figure BDA0002249979190000031
其中，K为相机的内部参数矩阵，维度为3×3；R’为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由目标车辆的方位R确定，维度为3×3；
Figure BDA0002249979190000032
S3.2根据步骤S2.3中目标车辆二维边界框的各特征点与目标车辆三维边界框各角点的对应关系，利用当前帧图像以及前N-1帧图像建立共计6N个投影约束，表达式如下：
Figure BDA0002249979190000033
Figure BDA0002249979190000034
Figure BDA0002249979190000035
Figure BDA0002249979190000036
Figure BDA0002249979190000037
其中，*表示该处为任意值，没有约束；k＝1,2…N对应图像的帧数，k＝N代表当前帧，每一帧图像中目标车辆的二维边界框位置不同，用
Figure BDA0002249979190000038
表示第k帧图像中二维边界框的第p特征点在像素坐标系中的坐标，通过步骤S2.3得到，p＝1,2,3,4,5；每一帧图像中目标车辆头部中心接地点T在自车车身坐标系中的坐标位置不同，用Tk＝[txk,tyk,0]表示；规定短时间内各帧图像中目标车辆在自车车身坐标系中的方位相同，均用R＝[θ,0,0]表示，R′为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由R确定；
S4利用连续N帧图像建立同一目标车辆位置运动约束
设连续两帧图像中的目标车辆车头前部中心接地点在自车车身坐标系中的坐标为：T(tk-1)＝[tx(k-1),ty(k-1),0],T(tk)＝[txk,tyk,0]，tk,tk-1是连续两帧图像的采集时间；
对于车辆前部中心接地点坐标位置根据当前帧图像以及前N-1帧图像建立共计2(N-1)个运动约束：
txk＝tx(k-1)+vxk(tk-tk-1) (7)
tyk＝ty(k-1)+vyk(tk-tk-1) (8)
其中，vxk,vyk分别为第k帧图像对应时刻的自车车身坐标系中目标车辆的纵向和横向速度；
S5多约束求解目标车辆方位并得到目标车辆三维边界框
联立公式(2)～(8)，通过最小二乘法求解，得到当前时刻目标车辆在自车车身坐标系中的位置Tk和方位R，即确定当前时刻目标车辆的三维边界框；
S6根据当前时刻目标车辆三维边界框，确定目标车辆与自车的横纵向相对位置关系
S6.1记当前时刻目标车辆三维边界框的第一角点和第二角点在自车车身坐标系中的坐标分别为X1(x1,y1),X2(x2,y2)；同时将步骤S2得到的像素坐标系下的各车道线方程投影在自车车身坐标系中，得到自车车身坐标系下车道线方程y＝f(x)，共含有J条车道线；
S6.2沿目标车辆三维边界框的的第一角点和第二角点X1,X2作直线，直线方程为：
Figure BDA0002249979190000041
该直线与车道线相交，记各交点分别为Cj，记各交点在自车车身坐标系下的Ye轴坐标分别为CjY，j＝1,2,…,J；
S6.3将各车道线与目标车辆三维边界框前部直线交点Cj的Ye轴坐标CjY和目标车辆三维边界框第一角点和第二角点的Ye轴坐标进行升序排列，通过比较交点坐标和目标车辆三维边界点框前部角点坐标的大小，确定目标车辆相对于车道线的位置；
S6.4过自车车身坐标系原点作任意一条与车道线非平行的直线，利用步骤S6.3所述方法判断自车所在车道，以此确定目标车辆与自车的横向相对位置关系；所述目标车辆与自车的横向相对位置分为左侧相邻、右侧相邻和不相邻；
S7评估目标车辆对自车换道的碰撞风险
S7.1根据步骤S6.4中得到的目标车辆和自车横向相对位置关系，判断目标车辆是否为潜在危险车辆：若目标车辆在自车的相邻车道，则目标车辆为换道预警系统的潜在危险车辆，执行步骤S7.2；若目标车辆不在自车相邻车道，则换道预警系统不关注该车辆，本轮车辆换道预警结束，返回步骤S2，执行下一轮车辆换道预警；
S7.2在换道过程中，规定目标车辆沿当前车道匀加速运动；设步骤S5确定的目标车辆三维边界框的前部中心接地点当前时刻t0在自车车身坐标系中的坐标为[x0,y0]，目标车辆运动速度为[vx0,vy0]，加速度为[ax0,ay0]，则此后任意时刻t目标车辆的坐标为：
Figure BDA0002249979190000051
Figure BDA0002249979190000052
S7.3对于换道预警系统的潜在危险车辆，根据如下规则进行换道风险评估：
1)若当前时刻目标车辆三维边界框前部距离自车尾部距离小于设定阈值，且目标车辆在换道预警系统反应时间ts之后的预测位置x(ts)至自车尾部的距离小于自车后视镜盲区范围D，则换道预警系统发出盲区预警信号，直至当目标车辆三维边界框前部越过自车前部时，盲区预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警；
2)若当前时刻目标车辆前部至自车尾部的距离大于自车后视镜盲区范围D，计算目标车辆与自车的纵向距离碰撞时间TTC，若距离碰撞时间大于零且小于等于3.5秒，则换道预警系统发出车辆快速接近预警信号，直至当目标车辆三维边界框前部越过自车前部时，取消车辆快速接近预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警，其中，距离碰撞时间定义为：
Figure BDA0002249979190000053
其中，Lr为自车车身坐标系原点到自车尾部的距离；ve为自车纵向车速。
本发明的特点及有益效果：
本发明针对车辆换道预警场景，使用相机采集连续多帧侧后方车辆图像。通过检测车辆二维边框和车辆三维尺寸，利用相机几何投影约束及多帧图像中车辆的运动约束，高精度地估计他车位姿，从而更精确的计算他车与自车的横纵向距离，提高了车辆换道预警的准确率。
附图说明
图1为本发明实施例中建立的目标车辆车身坐标系及自车车身坐标系。
图2为本发明实施例中建立的像素坐标系及目标车辆二维边界框示意图。
图3为本发明实施例中涉及的目标车辆多帧位置运动约束示意图。
图4为本发明实施例中涉及的目标车辆车道位置判断示意图。
具体实施方式
以下结合附图及实施例对本发明的技术方案进一步详细说明如下：
本发明实施例的一种基于连续图像约束位姿估计的车辆换道预警方法，在自车车顶中心固定有相机，自车尾部中心还搭载有毫米波雷达、全球定位系统GPS和惯性导航单元IMU，各器件均按相同频率(本实施例取采集频率为10Hz)采集相应信息。本方法具体包括以下步骤：
S1建立目标车辆车身坐标系、自车车身坐标系和像素坐标系，参见图1，具体包括：
S1.1建立目标车辆车身坐标系
将目标车辆三维边界框简化为一个长方体，以车辆前部中心接地点T为原点、向目标车辆后侧为Xt轴正向、向目标车辆右侧为Yt轴正向、向上为Zt轴正向，建立与目标车辆固定的目标车辆车身坐标系T-XtYtZt。在目标车辆车身坐标系中，设车辆长方体的8个角点的坐标分别为：
Figure BDA0002249979190000061
Figure BDA0002249979190000062
其中，dx,dy,dz为目标车辆三维边界框沿Xt轴，Yt轴和Zt轴方向的长度。
S1.2建立自车车身坐标系
以相机中心Oe为原点，向自车后侧为Xe轴正向、向车辆左侧为Ye轴正向、向上为Ze轴正向，建立与自车固定的自车车身坐标系Oe-XeYeZe。设目标车辆三维边界框前部的中心接地点T在自车车身坐标系中的坐标为：T＝[tx,ty,tz]T，该中心接地点T在Ze轴的投影为0，即tz＝0，T＝[tx,ty,0]T；设目标车辆三维边界框在自车坐标系中的方位为R＝[θ,φ,α]，其中θ为目标车辆的朝向角，即目标车辆纵轴与Ye轴间的夹角，φ为目标车辆的俯仰角，α为目标车辆的侧倾角，由于车辆正常行驶中俯仰角和侧倾角较小，取俯仰角和侧倾角为零(φ＝0,α＝0)，只考虑目标车辆的方位角θ，即R＝[θ,0,0]，T和R均为待求量。
S1.3建立像素坐标系
以相机采集图像的左上角Op为原点、向右为U轴正向、向下为V轴正向，建立像素坐标系Op-UV，如图2所示。
S2目标车辆图像采集与处理及自车状态测量
采集当前时刻自车后方的图像并使用已有成熟算法处理图像得到后方目标车辆的三维边界框的长宽高、二维边界框和车道线信息；采用车载毫米波雷达测量当前时刻目标车速度；采用车载GPS和惯性导航单元IMU测量当前时刻自车的速度和位姿。
S2.1采用固定在自车顶部的相机采集当前时刻自车后方的图像作为当前帧图像。
S2.2通过CNN网络模型对采集的当前帧图像进行回归处理得到目标车辆三维边界框的尺寸dx,dy,dz(具体可参见Mousavian A,Anguelov D,Flynn J,et al.3D BoundingBox Estimation Using Deep Learning and Geometry[J].2016.)。
S2.3通过基于统一多尺度深卷积神经网络的车辆目标检测算法得到当前帧图像中目标车辆在像素坐标系下的矩形二维边界框并计算其上5个特征点的坐标，参见图2，其中，第一特征点、第三特征点作为二维边界框的上边线两端点且分别与三维边界框的第八角点、第六角点相对应，第二特征点位于二维边界框的上边线上且与三维边界框的第七角点相对应，第四特征点位于二维边界框的左边线上且与三维边界框的第四角点相对应，第五特征点位于二维边界框的下边线上且与三维边界框的第一角点相对应(具体可参见CaiZ,Fan Q,Feris R S,et al.A Unified Multi-scale Deep Convolutional NeuralNetwork for Fast Object Detection[J].2016.)。
S2.4对当前帧图像通过基于自适应阈值和车道分类的车道线检测算法得到像素坐标系下的各车道线方程，本实施例中采用3次多项式形式的车道线方程(Son Y,Lee E S,Kum D.Robust multi-lane detection and tracking using adaptive threshold andlane classification[J].Machine Vision and Applications,2018,30(8):111-124.)。
S2.5采用车载后向毫米波雷达测量得到当前时刻目标车辆在自车车身坐标系中的速度。
S2.6采用车载GPS和IMU测量当前时刻自车的位置、速度和朝向角。
S3利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束
S3.1根据目标车辆车身坐标系和像素坐标系的投影关系，目标车辆车身坐标系中任意点X0＝[X,Y,Z]T与像素坐标系中对应点x0＝[u,v]T满足以下变换关系：
Figure BDA0002249979190000071
其中，K为相机的内部参数矩阵(维度为3×3)，根据所选用的相机型号确定，为已知量；R’为目标车辆车身坐标系到自车车身坐标系的旋转矩阵(维度为3×3)，由目标车辆的方位R确定；
Figure BDA0002249979190000072
S3.2由于目标车辆在自车侧后方，因此车辆的大致方位已知，可以确定目标车辆二维边界框和三维边界框的投影约束关系即步骤S2.3中目标车辆二维边界框的各特征点与目标车辆三维边界框各角点的对应关系，以图2中位于自车左后侧的目标车辆为例，图中1号点(即第一特征点)为目标车辆二维边界框左上角，对应目标车辆三维边界框的第八角点X8，1号点在像素坐标系中的u和v坐标已知，提供了2个约束；图中2号点(即第二特征点)为车辆边界框的上边线中的一点，对应目标车辆三维边界框的第七角点X7，由于2号点只能确定图像中v坐标，因此只有1个约束。类似的，图中3号点(即第三特征点)提供了目标车辆三维边界框第六角点X6的1个约束，图中4号点(即第四特征点)提供了目标车辆三维边界框第四角点X4的1个约束，图中5号点(即第五特征点)提供了目标车辆三维边界框第一角点X1的1个约束。根据当前帧图像以及前N-1帧图像建立共计6N个投影约束，表达式如下：
Figure BDA0002249979190000081
Figure BDA0002249979190000082
Figure BDA0002249979190000083
Figure BDA0002249979190000084
Figure BDA0002249979190000085
其中，*表示该处为任意值，没有约束。k＝1,2…N对应图像的帧数，k＝N(N优选2或3)代表当前帧，每一帧图像中目标车辆的二维边界框位置不同，用
Figure BDA0002249979190000086
表示第k帧图像中二维边界框的第p特征点在像素坐标系中的坐标，通过步骤S2.3得到，p＝1,2,3,4,5。每一帧图像中目标车辆头部中心接地点T在自车车身坐标系中的坐标位置不同，用Tk＝[txk,tyk,0]表示(为待求量)。规定短时间内各帧图像中目标车辆在自车车身坐标系中的方位相同，均用R＝[θ,0,0]表示，R′为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由R确定。Tk,R均为待求量。
S4利用连续N帧图像建立同一目标车辆位置运动约束
如图3所示，设连续两帧图像中的目标车辆车头前部中心接地点在自车车身坐标系中的坐标为：T(tk-1)＝[tx(k-1),ty(k-1),0],T(tk)＝[txk,tyk,0]，tk,tk-1是连续两帧图像的采集时间。
在短时间内，车辆运动的朝向角可假设为恒定，车辆运动速度大小和方向保持不变，因此对于车辆前部中心接地点坐标位置根据当前帧图像以及前N-1帧图像建立共计2(N-1)个运动约束：
txk＝tx(k-1)+vxk(tk-tk-1) (7)
tyk＝ty(k-1)+vyk(tk-tk-1) (8)
其中，vxk,vyk分别为第k帧图像对应时刻的自车车身坐标系中目标车辆的纵向和横向速度，通过车载后向毫米波雷达获取。tk-tk-1为连续两帧图像间的时间间隔，由相机的采集频率确定，本实施例中取为0.1s。
S5多约束求解目标车辆方位并得到目标车辆三维边界框
联立公式(2)～(8)，得到(8N-2)个约束方程，其中有(2N+1)个待求解参数，约束方程数大于未知数数量，使用最小二乘法求解可得到未知参数，得到当前时刻目标车辆在自车车身坐标系中的位置Tk和方位R，即确定当前时刻目标车辆的三维边界框。
S6根据当前时刻目标车辆三维边界框，确定目标车辆与自车的横纵向相对位置关系。
S6.1记当前时刻目标车辆三维边界框的第一角点和第二角点在自车车身坐标系中的坐标分别为X1(x1,y1),X2(x2,y2)；同时将步骤S2得到的像素坐标系下的各车道线方程投影在自车车身坐标系中，得到自车车身坐标系下车道线方程y＝f(x)，共含有J条车道线，如图4中包括J＝3条车道线。
S6.2沿目标车辆三维边界框的的第一角点和第二角点X1,X2作直线，直线方程为：
Figure BDA0002249979190000091
该直线与车道线相交，记各交点分别为Cj，记各交点在自车车身坐标系下的Ye轴坐标分别为CjY，j＝1,2,…,J，J为车道线总数。
S6.3将各车道线与目标车辆三维边界框前部直线交点Cj的Ye轴坐标CjY和目标车辆三维边界框第一角点和第二角点的Ye轴坐标进行升序排列，通过比较交点坐标和目标车辆三维边界点框前部角点坐标的大小，确定目标车辆相对于车道线的位置，具体如下：
若CjY<yCj<yC(j+1)，则表明目标车辆在第j条车道线右侧；若yCj<yC(j+1)<CjY，则表明目标车辆在第j条车道线左侧；若yCj<CjY<yC(j+1)，则表明目标车辆压在第j条车道线上。图4中目标车辆的位置对应C1Y<C2Y<yC1<C3Y，说明车辆在车道线1和车道线2的右侧，在车道线3的左侧，即车辆在车道线2和车道线3之间。
S6.4过自车车身坐标系原点(即自车中心)作任意一条与车道线非平行的直线，利用步骤S6.3所述方法判断自车所在车道，最终确定目标车辆与自车的横向相对位置关系。目标车辆与自车的横向相对位置分为左侧相邻、右侧相邻和不相邻。
S7评估目标车辆对自车换道的碰撞风险
S7.1根据步骤S6.4中得到的目标车辆和自车横向相对位置关系，判断目标车辆是否为潜在危险车辆。若目标车辆在自车的相邻车道，则目标车辆为换道预警系统的潜在危险车辆，执行步骤S7.2；若目标车辆不在自车相邻车道，则换道预警系统不关注该车辆，本轮车辆换道预警结束，返回步骤S2，执行下一轮车辆换道预警。
S7.2在换道过程中，规定目标车辆沿当前车道匀加速运动，预测目标车辆未来任意时刻的位置。设步骤S5确定的目标车辆三维边界框的前部中心接地点当前时刻t0在自车车身坐标系中的坐标为[x0,y0]，目标车辆运动速度为[vx0,vy0]，加速度为[ax0,ay0]，则此后任意时刻t目标车辆的坐标为：
Figure BDA0002249979190000101
Figure BDA0002249979190000102
S7.3对于换道预警系统潜在危险车辆，则根据如下规则进行换道风险评估：
1)若当前时刻目标车辆三维边界框前部距离自车尾部距离小于设定阈值(一般取30～50米，本实施例取30米)，且目标车辆在换道预警系统反应时间ts之后的预测位置x(ts)至自车尾部的距离小于自车后视镜盲区范围D(本实施例D＝3米)，则换道预警系统发出盲区预警信号，直至当目标车辆三维边界框前部越过自车前部时，盲区预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警。根据国标ISO 17387-2008Intelligenttransport systems-Lane change decision aid systems(LCDAS)要求系统反应时间不大于300毫秒，本实施例取换道预警系统的反应时间为200毫秒。即：
|x(ts)|-Lr<D (12)
时给出盲区预警，其中Lr为自车车身坐标系原点(即相机中心)到自车尾部的距离，本例中Lr＝1.37m。
2)若当前时刻目标车辆前部至自车尾部的距离大于自车后视镜盲区范围D(本实施例D＝3米)，计算目标车辆与自车的纵向距离碰撞时间TTC，若距离碰撞时间大于零且小于等于3.5秒，则换道预警系统发出车辆快速接近预警信号，直至当目标车辆三维边界框前部越过自车前部时，取消车辆快速接近预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警，其中，距离碰撞时间定义为：
Figure BDA0002249979190000103
其中ve为自车纵向车速。
以上所述仅为本发明的实施例，并非因此限制本发明的保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的保护范围内。

Claims (3)

1.一种基于连续图像约束位姿估计的车辆换道预警方法，在自车车顶中心固定有相机，自车尾部中心搭载有毫米波雷达、全球定位系统GPS和惯性导航单元IMU；其特征在于，所述车辆换道预警方法包括以下步骤：
S1建立目标车辆车身坐标系、自车车身坐标系和像素坐标系
S1.1建立目标车辆车身坐标系
将目标车辆三维边界框简化为一个长方体，以车辆前部中心接地点T为原点、向目标车辆后侧为Xt轴正向、向目标车辆右侧为Yt轴正向、向上为Zt轴正向，建立与目标车辆固定的目标车辆车身坐标系T-XtYtZt；在所述目标车辆车身坐标系中，设三维边界框的8个角点的坐标分别为：
Figure FDA0002627389310000011
Figure FDA0002627389310000012
其中，dx，dy，dz为目标车辆三维边界框沿Xt轴，Yt轴和Zt轴方向的长度；
S1.2建立自车车身坐标系
以相机中心Oe为原点，向自车后侧为Xe轴正向、向车辆左侧为Ye轴正向、向上为Ze轴正向，建立与自车固定的自车车身坐标系Oe-XeYeZe；设目标车辆三维边界框前部的中心接地点T在自车车身坐标系中的坐标为：T＝[tx，ty，0]T；设目标车辆三维边界框在自车坐标系中的方位为R＝[θ，0，0]，其中θ为目标车辆的朝向角，即目标车辆纵轴与Ye轴间的夹角；
S1.3建立像素坐标系
以相机采集图像的左上角Op为原点、向右为U轴正向、向下为V轴正向，建立像素坐标系Op-UV；
S2目标车辆图像采集与处理及自车状态测量
S2.1采用相机采集当前时刻自车后方的图像作为当前帧图像；
S2.2通过CNN网络模型对采集的当前帧图像进行回归处理得到目标车辆三维边界框的尺寸dx，dy，dz；
S2.3通过车辆目标检测算法得到当前帧图像中目标车辆在像素坐标系下的矩形二维边界框并计算其上5个特征点的坐标；其中，第一特征点、第三特征点作为二维边界框的上边线两端点且分别与三维边界框的第八角点、第六角点相对应，第二特征点位于二维边界框的上边线上且与三维边界框的第七角点相对应，第四特征点位于二维边界框的左边线上且与三维边界框的第四角点相对应，第五特征点位于二维边界框的下边线上且与三维边界框的第一角点相对应；
S2.4对当前帧图像通过车道线检测算法得到像素坐标系下的各车道线方程；
S2.5采用毫米波雷达测量得到当前时刻目标车辆在自车车身坐标系中的速度；
S2.6采用全球定位系统GPS和惯性导航单元IMU测量当前时刻自车的位置、速度和朝向角；
S3利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束
S3.1根据目标车辆车身坐标系和像素坐标系的投影关系，目标车辆车身坐标系中任意点X0＝[X，Y，Z]T与像素坐标系中对应点x0＝[u，v]T满足以下变换关系：
Figure FDA0002627389310000021
其中，K为相机的内部参数矩阵，维度为3×3；R’为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由目标车辆的方位R确定，维度为3×3；
Figure FDA0002627389310000022
S3.2根据步骤S2.3中目标车辆二维边界框的各特征点与目标车辆三维边界框各角点的对应关系，利用当前帧图像以及前N-1帧图像建立共计6N个投影约束，表达式如下：
Figure FDA0002627389310000023
Figure FDA0002627389310000024
Figure FDA0002627389310000025
Figure FDA0002627389310000026
Figure FDA0002627389310000027
其中，*表示该处为任意值，没有约束；k＝1，2...N对应图像的帧数，k＝N代表当前帧，每一帧图像中目标车辆的二维边界框位置不同，用
Figure FDA0002627389310000028
表示第k帧图像中二维边界框的第p特征点在像素坐标系中的坐标，通过步骤S2.3得到，p＝1，2，3，4，5；每一帧图像中目标车辆头部中心接地点T在自车车身坐标系中的坐标位置不同，用Tk＝[txk，tyk，0]表示；规定短时间内各帧图像中目标车辆在自车车身坐标系中的方位相同，均用R＝[θ，0，0]表示，R′为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由R确定；
S4利用连续N帧图像建立同一目标车辆位置运动约束
设连续两帧图像中的目标车辆车头前部中心接地点在自车车身坐标系中的坐标为：T(tk-1)＝[tx(k-1)，ty(k-1)，0]，T(tk)＝[txk，tyk，0]，tk，tk-1是连续两帧图像的采集时间；
对于车辆前部中心接地点坐标位置根据当前帧图像以及前N-1帧图像建立共计2(N-1)个运动约束：
txk＝tx(k-1)+vxk(tk-tk-1) (7)
tyk＝ty(k-1)+vyk(tk-tk-1) (8)
其中，vxk，vyk分别为第k帧图像对应时刻的自车车身坐标系中目标车辆的纵向和横向速度；
S5多约束求解目标车辆方位并得到目标车辆三维边界框
联立公式(2)～(8)，通过最小二乘法求解，得到当前时刻目标车辆在自车车身坐标系中的位置Tk和方位R，即确定当前时刻目标车辆的三维边界框；
S6根据当前时刻目标车辆三维边界框，确定目标车辆与自车的横纵向相对位置关系
S6.1记当前时刻目标车辆三维边界框的第一角点和第二角点在自车车身坐标系中的坐标分别为X1(x1，y1)，X2(x2，y2)；同时将步骤S2得到的像素坐标系下的各车道线方程投影在自车车身坐标系中，得到自车车身坐标系下车道线方程y＝f(x)，共含有J条车道线；
S6.2沿目标车辆三维边界框的的第一角点和第二角点X1，X2作直线，直线方程为：
Figure FDA0002627389310000031
该直线与车道线相交，记各交点分别为Cj，记各交点在自车车身坐标系下的Ye轴坐标分别为CjY，j＝1，2，…，J；
S6.3将各车道线与目标车辆三维边界框前部直线交点Cj的Ye轴坐标CjY和目标车辆三维边界框第一角点和第二角点的Ye轴坐标进行升序排列，通过比较交点坐标和目标车辆三维边界点框前部角点坐标的大小，确定目标车辆相对于车道线的位置；
S6.4过自车车身坐标系原点作任意一条与车道线非平行的直线，利用步骤S6.3所述方法判断自车所在车道，以此确定目标车辆与自车的横向相对位置关系；所述目标车辆与自车的横向相对位置分为左侧相邻、右侧相邻和不相邻；
S7评估目标车辆对自车换道的碰撞风险
S7.1根据步骤S6.4中得到的目标车辆和自车横向相对位置关系，判断目标车辆是否为潜在危险车辆：若目标车辆在自车的相邻车道，则目标车辆为换道预警系统的潜在危险车辆，执行步骤S7.2；若目标车辆不在自车相邻车道，则换道预警系统不关注该车辆，本轮车辆换道预警结束，返回步骤S2，执行下一轮车辆换道预警；
S7.2在换道过程中，规定目标车辆沿当前车道匀加速运动；设步骤S5确定的目标车辆三维边界框的前部中心接地点当前时刻t0在自车车身坐标系中的坐标为[x0，y0]，目标车辆运动速度为[vx0，vy0]，加速度为[ax0，ay0]，则此后任意时刻t目标车辆的坐标为：
Figure FDA0002627389310000041
Figure FDA0002627389310000042
S7.3对于换道预警系统的潜在危险车辆，根据如下规则进行换道风险评估：
1)若当前时刻目标车辆三维边界框前部距离自车尾部距离小于设定阈值，且目标车辆在换道预警系统反应时间ts之后的预测位置x(ts)至自车尾部的距离小于自车后视镜盲区范围D，则换道预警系统发出盲区预警信号，直至当目标车辆三维边界框前部越过自车前部时，盲区预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警；
2)若当前时刻目标车辆前部至自车尾部的距离大于自车后视镜盲区范围D，计算目标车辆与自车的纵向距离碰撞时间TTC，若距离碰撞时间大于零且小于等于3.5秒，则换道预警系统发出车辆快速接近预警信号，直至当目标车辆三维边界框前部越过自车前部时，取消车辆快速接近预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警，其中，距离碰撞时间定义为：
Figure FDA0002627389310000043
其中，Lr为自车车身坐标系原点到自车尾部的距离；ve为自车纵向车速。
2.根据权利要求1所述的车辆换道预警方法，其特征在于，所采用的连续图像帧数N为2或3。
3.根据权利要求1所述的车辆换道预警方法，其特征在于，步骤S2.4中得到的各车道线方程均为三次多项式形式。
CN201911030362.4A 2019-10-28 2019-10-28 一种基于连续图像约束位姿估计的车辆换道预警方法 Active CN110745140B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911030362.4A CN110745140B (zh) 2019-10-28 2019-10-28 一种基于连续图像约束位姿估计的车辆换道预警方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911030362.4A CN110745140B (zh) 2019-10-28 2019-10-28 一种基于连续图像约束位姿估计的车辆换道预警方法

Publications (2)

Publication Number Publication Date
CN110745140A CN110745140A (zh) 2020-02-04
CN110745140B true CN110745140B (zh) 2021-01-01

Family

ID=69280397

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911030362.4A Active CN110745140B (zh) 2019-10-28 2019-10-28 一种基于连续图像约束位姿估计的车辆换道预警方法

Country Status (1)

Country Link
CN (1) CN110745140B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113470110A (zh) * 2020-03-30 2021-10-01 北京四维图新科技股份有限公司 测距方法和装置
CN113643355B (zh) * 2020-04-24 2024-03-29 广州汽车集团股份有限公司 一种目标车辆位置和朝向的检测方法、系统及存储介质
CN112668428A (zh) * 2020-12-21 2021-04-16 北京百度网讯科技有限公司 车辆变道的检测方法、路侧设备、云控平台和程序产品
CN112489496A (zh) * 2020-12-25 2021-03-12 深圳市爱培科技术股份有限公司 一种基于视频识别的车辆安全预警方法及装置
CN112818834B (zh) * 2021-01-29 2022-11-25 山东大学 一种交叉口针对应急车辆的避让判定方法、设备及介质
CN113033441A (zh) * 2021-03-31 2021-06-25 广州敏视数码科技有限公司 一种基于广角成像的行人碰撞预警方法
US11854255B2 (en) 2021-07-27 2023-12-26 Ubkang (Qingdao) Technology Co., Ltd. Human-object scene recognition method, device and computer-readable storage medium
CN113619578A (zh) * 2021-07-28 2021-11-09 东风汽车集团股份有限公司 一种车辆防碰撞方法、防碰撞系统和计算机可读存储介质
CN113792634B (zh) * 2021-09-07 2022-04-15 北京易航远智科技有限公司 一种基于车载相机的目标相似性评分计算方法及系统
CN113954838B (zh) * 2021-11-24 2023-04-07 上海安亭地平线智能交通技术有限公司 车辆换道的控制方法和装置、电子设备和存储介质
CN114604175B (zh) * 2021-11-26 2023-10-13 中科云谷科技有限公司 用于确定工程车辆的方法、处理器、装置及系统
CN115472005B (zh) * 2022-08-09 2023-12-19 东软睿驰汽车技术（上海）有限公司 车辆碰撞预警方法、装置、设备及存储介质
CN115861975B (zh) * 2023-02-28 2023-05-12 杭州枕石智能科技有限公司 障碍物车辆位姿估计方法及设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101089917A (zh) * 2007-06-01 2007-12-19 清华大学 一种目标车换道工况下的快速识别方法
CN101870293A (zh) * 2009-04-24 2010-10-27 南京理工大学 基于切道行为检测的车辆驾驶状态评价方法
US9365162B2 (en) * 2012-08-20 2016-06-14 Magna Electronics Inc. Method of obtaining data relating to a driver assistance system of a vehicle
CN108106627A (zh) * 2017-12-20 2018-06-01 清华大学 一种基于特征点在线动态标定的单目视觉车辆定位方法
CN110097785A (zh) * 2019-05-30 2019-08-06 长安大学 一种前车切入或紧急换道识别预警装置及预警方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101089917A (zh) * 2007-06-01 2007-12-19 清华大学 一种目标车换道工况下的快速识别方法
CN101870293A (zh) * 2009-04-24 2010-10-27 南京理工大学 基于切道行为检测的车辆驾驶状态评价方法
US9365162B2 (en) * 2012-08-20 2016-06-14 Magna Electronics Inc. Method of obtaining data relating to a driver assistance system of a vehicle
CN108106627A (zh) * 2017-12-20 2018-06-01 清华大学 一种基于特征点在线动态标定的单目视觉车辆定位方法
CN110097785A (zh) * 2019-05-30 2019-08-06 长安大学 一种前车切入或紧急换道识别预警装置及预警方法

Also Published As

Publication number Publication date
CN110745140A (zh) 2020-02-04

Similar Documents

Publication Publication Date Title
CN110745140B (zh) 一种基于连续图像约束位姿估计的车辆换道预警方法
EP3436879B1 (en) An autonomous vehicle with improved visual detection ability
CN107389026B (zh) 一种基于固定点射影变换的单目视觉测距方法
CN110065494B (zh) 一种基于车轮检测的车辆防碰撞方法
CN111126269B (zh) 三维目标检测方法、装置以及存储介质
EP2461305B1 (en) Road shape recognition device
CA2987373C (en) Position estimation device and position estimation method
CN108638999B (zh) 一种基于360度环视输入的防碰撞预警系统及方法
EP3792660B1 (en) Method, apparatus and system for measuring distance
CN107972662A (zh) 一种基于深度学习的车辆前向碰撞预警方法
CN113819914A (zh) 一种地图构建方法及装置
US10363940B2 (en) Vehicle position attitude-angle estimation device and vehicle position attitude-angle estimation method
CN113850102B (zh) 基于毫米波雷达辅助的车载视觉检测方法及系统
DE102018103473A1 (de) Wirksamer rollradius
CN114442101B (zh) 基于成像毫米波雷达的车辆导航方法、装置、设备及介质
CN109900490B (zh) 基于自主式和协同式传感器的车辆运动状态检测方法及系统
CN108021899A (zh) 基于双目相机的车载智能前车防撞预警方法
CN110969064A (zh) 一种基于单目视觉的图像检测方法、装置及存储设备
CN113819905A (zh) 一种基于多传感器融合的里程计方法及装置
CN113252051A (zh) 一种地图构建方法及装置
Qi et al. Distance estimation of monocular based on vehicle pose information
CN114295099B (zh) 基于单目摄像头的测距方法、车载测距设备以及存储介质
US20230060542A1 (en) Method and Apparatus for Evaluating Maps for Autonomous Driving and Vehicle
Li et al. Pitch angle estimation using a Vehicle-Mounted monocular camera for range measurement
CN106885523B (zh) 一种车辆路径跟随误差视觉测量优化方法

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
GR01 Patent grant
GR01 Patent grant