本发明提出的一种基于连续图像约束位姿估计的车辆换道预警方法，包括：建立目标车辆车身坐标系、自车车身坐标系和像素坐标系；目标车辆图像采集与处理及自车状态测量；利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束；利用连续多帧图像建立同一目标车辆位置运动约束；联立各约束，求解目标车辆在自车车身坐标系中的位置和方位，即确定目标车辆的三维边界框；根据确定的三维边界框，确定目标车辆与自车的横纵向相对位置关系；评估目标车辆对自车换道的碰撞风险。本发明使用相机采集连续多帧侧后方车辆图像，高精度地估计他车位姿，从而更精确的计算他车与自车的横纵向距离，进行车辆换道预警。
1.一种基于连续图像约束位姿估计的车辆换道预警方法，在自车车顶中心固定有相机，自车尾部中心搭载有毫米波雷达、全球定位系统GPS和惯性导航单元IMU；其特征在于，所述车辆换道预警方法包括以下步骤：S1建立目标车辆车身坐标系、自车车身坐标系和像素坐标系
将目标车辆三维边界框简化为一个长方体，以车辆前部中心接地点T为原点、向目标车辆后侧为Xt轴正向、向目标车辆右侧为Yt轴正向、向上为Zt轴正向，建立与目标车辆固定的目标车辆车身坐标系T-XtYtZt；在所述目标车辆车身坐标系中，设三维边界框的8个角点的坐标分别为：其中，dx，dy，dz为目标车辆三维边界框沿Xt轴，Yt轴和Zt轴方向的长度；
以相机中心Oe为原点，向自车后侧为Xe轴正向、向车辆左侧为Ye轴正向、向上为Ze轴正向，建立与自车固定的自车车身坐标系Oe-XeYeZe；设目标车辆三维边界框前部的中心接地点T在自车车身坐标系中的坐标为：T＝[tx，ty，0]T；设目标车辆三维边界框在自车坐标系中的方位为R＝[θ，0，0]，其中θ为目标车辆的朝向角，即目标车辆纵轴与Ye轴间的夹角；
以相机采集图像的左上角Op为原点、向右为U轴正向、向下为V轴正向，建立像素坐标系Op-UV；
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满足以下变换关系：其中，K为相机的内部参数矩阵，维度为3×3；R’为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由目标车辆的方位R确定，维度为3×3；
S3.2根据步骤S2.3中目标车辆二维边界框的各特征点与目标车辆三维边界框各角点的对应关系，利用当前帧图像以及前N-1帧图像建立共计6N个投影约束，表达式如下：其中，*表示该处为任意值，没有约束；k＝1，2...N对应图像的帧数，k＝N代表当前帧，每一帧图像中目标车辆的二维边界框位置不同，用 表示第k帧图像中二维边界框的第p特征点在像素坐标系中的坐标，通过步骤S2.3得到，p＝1，2，3，4，5；每一帧图像中目标车辆头部中心接地点T在自车车身坐标系中的坐标位置不同，用Tk＝[txk，tyk，0]表示；规定短时间内各帧图像中目标车辆在自车车身坐标系中的方位相同，均用R＝[θ，0，0]表示，R′为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由R确定；
设连续两帧图像中的目标车辆车头前部中心接地点在自车车身坐标系中的坐标为：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作直线，直线方程为：该直线与车道线相交，记各交点分别为Cj，记各交点在自车车身坐标系下的Ye轴坐标分别为CjY，j＝1，2，…，J；
S6.3将各车道线与目标车辆三维边界框前部直线交点Cj的Ye轴坐标CjY和目标车辆三维边界框第一角点和第二角点的Ye轴坐标进行升序排列，通过比较交点坐标和目标车辆三维边界点框前部角点坐标的大小，确定目标车辆相对于车道线的位置；
S6.4过自车车身坐标系原点作任意一条与车道线非平行的直线，利用步骤S6.3所述方法判断自车所在车道，以此确定目标车辆与自车的横向相对位置关系；所述目标车辆与自车的横向相对位置分为左侧相邻、右侧相邻和不相邻；
S7.1根据步骤S6.4中得到的目标车辆和自车横向相对位置关系，判断目标车辆是否为潜在危险车辆：若目标车辆在自车的相邻车道，则目标车辆为换道预警系统的潜在危险车辆，执行步骤S7.2；若目标车辆不在自车相邻车道，则换道预警系统不关注该车辆，本轮车辆换道预警结束，返回步骤S2，执行下一轮车辆换道预警；
S7.2在换道过程中，规定目标车辆沿当前车道匀加速运动；设步骤S5确定的目标车辆三维边界框的前部中心接地点当前时刻t0在自车车身坐标系中的坐标为[x0，y0]，目标车辆运动速度为[vx0，vy0]，加速度为[ax0，ay0]，则此后任意时刻t目标车辆的坐标为：S7.3对于换道预警系统的潜在危险车辆，根据如下规则进行换道风险评估：
1)若当前时刻目标车辆三维边界框前部距离自车尾部距离小于设定阈值，且目标车辆在换道预警系统反应时间ts之后的预测位置x(ts)至自车尾部的距离小于自车后视镜盲区范围D，则换道预警系统发出盲区预警信号，直至当目标车辆三维边界框前部越过自车前部时，盲区预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警；
2)若当前时刻目标车辆前部至自车尾部的距离大于自车后视镜盲区范围D，计算目标车辆与自车的纵向距离碰撞时间TTC，若距离碰撞时间大于零且小于等于3.5秒，则换道预警系统发出车辆快速接近预警信号，直至当目标车辆三维边界框前部越过自车前部时，取消车辆快速接近预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警，其中，距离碰撞时间定义为：其中，Lr为自车车身坐标系原点到自车尾部的距离；ve为自车纵向车速。
2.根据权利要求1所述的车辆换道预警方法，其特征在于，所采用的连续图像帧数N为2或3。
3.根据权利要求1所述的车辆换道预警方法，其特征在于，步骤S2.4中得到的各车道线方程均为三次多项式形式。
一种基于连续图像约束位姿估计的车辆换道预警方法
技术领域
[0001] 本发明涉及高级辅助驾驶系统环境感知技术领域，具体涉及一种基于连续图像约束位姿估计的车辆换道预警方法。
背景技术
[0002] 车辆行驶环境感知是换道预警系统的核心内容，为了评估换道风险，车载传感器系统需要对周围其他车辆的位置、速度大小与方向等进行感知。现有换道预警研究中多采用相机、雷达等相对廉价的感知方案，如宴晓娟采用相机采集侧后方图像信息检测目标二维边界框进行测距测速(晏晓娟.基于机器视觉的侧后方目标检测算法及换道预警系统研究[D].江苏大学,2017)，朱愿采用毫米波雷达采集侧后方车辆的速度与位置信息(朱愿.基于视觉和雷达的智能车辆自主换道决策机制与控制研究[D].中国人民解放军军事医学科学院,2014)。但相机的二维边界框不能表达深度及姿态信息，而雷达传感器通常将其他车辆聚类为一个目标点，因此无法精确获取目标车辆的三维尺寸与方位等信息。
[0003] 在换道预警系统中，判断目标车辆与自车的相对位置，尤其是横向相对位置，是评估换道风险的关键。相对于目标点，三维的目标边界框可以提高车辆横向相对位置关系识别的准确性，减少横向位置关系识别不准确造成的虚警。
[0004] 交通场景中的三维目标检测通常依赖激光雷达点云或双目相机深度点云，这些传感器价格昂贵，且精确的三维检测算法尚不成熟。Mousavian等人提出首先采用成熟的二维检测算法估计目标的三维尺寸，然后利用三维边界框与二维边界框的投影约束关系估计目标三维边界框的位姿(Mousavian A,Anguelov D,Flynn J,et al.3D Bounding Box Estimation Using Deep Learning and Geometry[J].2016.)。Mousavian等人首先训练了一个深度卷积神经网络(CNN)来回归目标物的三维尺寸，然后根据三维边界框和二维边界框的投影约束得到了三维边界框的转换矩阵，最终确定了物体的三维位姿。Mousavian等的方法针对随机图像，目标物的类别和方位具有多样性，需要附加额外的约束确定目标方位。此外图像往往只有单帧，因此该方法只使用一帧图像进行目标三维尺寸及位姿估计，无法利用目标物多帧信息进行联合优化，因此该方法获得三维边界框的精度还有待提高。
发明内容
[0005] 本发明目的是为了克服已有技术的不足之处，针对换道预警情景中目标车辆在自车的侧后方姿态相对固定，且可以对同一目标连续多帧跟踪观测的特点，提出一种基于连续图像约束位姿估计的车辆换道预警方法。
[0006] 为了实现上述目的，本发明采用如下技术方案：
[0007] 本发明提出的一种基于连续图像约束位姿估计的车辆换道预警方法，在自车车顶中心固定有相机，自车尾部中心搭载有毫米波雷达、全球定位系统GPS和惯性导航单元IMU；其特征在于，所述车辆换道预警方法包括以下步骤：
[0008] S1建立目标车辆车身坐标系、自车车身坐标系和像素坐标系
[0009] S1.1建立目标车辆车身坐标系
[0010] 将目标车辆三维边界框简化为一个长方体，以车辆前部中心接地点T为原点、向目标车辆后侧为Xt轴正向、向目标车辆右侧为Yt轴正向、向上为Zt轴正向，建立与目标车辆固定的目标车辆车身坐标系T-XtYtZt；在所述目标车辆车身坐标系中，设三维边界框的8个角点的坐标分别为：
[0011]
[0012]
[0013] 其中，dx,dy,dz为目标车辆三维边界框沿Xt轴，Yt轴和Zt轴方向的长度；
[0014] S1.2建立自车车身坐标系
[0015] 以相机中心Oe为原点，向自车后侧为Xe轴正向、向车辆左侧为Ye轴正向、向上为Ze轴正向，建立与自车固定的自车车身坐标系Oe-XeYeZe；设目标车辆三维边界框前部的中心接地点T在自车车身坐标系中的坐标为：T＝[tx,ty,0]T；设目标车辆三维边界框在自车坐标系中的方位为R＝[θ,0,0]，其中θ为目标车辆的朝向角，即目标车辆纵轴与Ye轴间的夹角；
[0016] S1.3建立像素坐标系
[0017] 以相机采集图像的左上角Op为原点、向右为U轴正向、向下为V轴正向，建立像素坐标系Op-UV；
[0018] S2目标车辆图像采集与处理及自车状态测量
[0019] S2.1采用相机采集当前时刻自车后方的图像作为当前帧图像；
[0020] S2.2通过CNN网络模型对采集的当前帧图像进行回归处理得到目标车辆三维边界框的尺寸dx,dy,dz；
[0021] S2.3通过车辆目标检测算法得到当前帧图像中目标车辆在像素坐标系下的矩形二维边界框并计算其上5个特征点的坐标；其中，第一特征点、第三特征点作为二维边界框的上边线两端点且分别与三维边界框的第八角点、第六角点相对应，第二特征点位于二维边界框的上边线上且与三维边界框的第七角点相对应，第四特征点位于二维边界框的左边线上且与三维边界框的第四角点相对应，第五特征点位于二维边界框的下边线上且与三维边界框的第一角点相对应；
[0022] S2.4对当前帧图像通过车道线检测算法得到像素坐标系下的各车道线方程；
[0023] S2.5采用毫米波雷达测量得到当前时刻目标车辆在自车车身坐标系中的速度；
[0024] S2.6采用GPS和IMU测量当前时刻自车的位置、速度和朝向角；
[0025] S3利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束[0026] S3.1根据目标车辆车身坐标系和像素坐标系的投影关系，目标车辆车身坐标系中任意点X0＝[X,Y,Z]T与像素坐标系中对应点x0＝[u,v]T满足以下变换关系：
[0027]
[0028] 其中，K为相机的内部参数矩阵，维度为3×3；R’为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由目标车辆的方位R确定，维度为3×3；
[0029] S3.2根据步骤S2.3中目标车辆二维边界框的各特征点与目标车辆三维边界框各角点的对应关系，利用当前帧图像以及前N-1帧图像建立共计6N个投影约束，表达式如下：
[0030]
[0031]
[0032]
[0033]
[0034]
[0035] 其中，*表示该处为任意值，没有约束；k＝1,2…N对应图像的帧数，k＝N代表当前帧，每一帧图像中目标车辆的二维边界框位置不同，用 表示第k帧图像中二维边界框的第p特征点在像素坐标系中的坐标，通过步骤S2.3得到，p＝1,2,3,4,5；每一帧图像中目标车辆头部中心接地点T在自车车身坐标系中的坐标位置不同，用Tk＝[txk,tyk,0]表示；规定短时间内各帧图像中目标车辆在自车车身坐标系中的方位相同，均用R＝[θ,0,0]表示，R′为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由R确定；
[0036] S4利用连续N帧图像建立同一目标车辆位置运动约束
[0037] 设连续两帧图像中的目标车辆车头前部中心接地点在自车车身坐标系中的坐标为：T(tk-1)＝[tx(k-1),ty(k-1),0],T(tk)＝[txk,tyk,0]，tk,tk-1是连续两帧图像的采集时间；
[0038] 对于车辆前部中心接地点坐标位置根据当前帧图像以及前N-1帧图像建立共计2(N-1)个运动约束：
[0039] txk＝tx(k-1)+vxk(tk-tk-1) (7)
[0040] tyk＝ty(k-1)+vyk(tk-tk-1) (8)
[0041] 其中，vxk,vyk分别为第k帧图像对应时刻的自车车身坐标系中目标车辆的纵向和横向速度；
[0042] S5多约束求解目标车辆方位并得到目标车辆三维边界框
[0043] 联立公式(2)～(8)，通过最小二乘法求解，得到当前时刻目标车辆在自车车身坐标系中的位置Tk和方位R，即确定当前时刻目标车辆的三维边界框；
[0044] S6根据当前时刻目标车辆三维边界框，确定目标车辆与自车的横纵向相对位置关系
[0045] S6.1记当前时刻目标车辆三维边界框的第一角点和第二角点在自车车身坐标系中的坐标分别为X1(x1,y1),X2(x2,y2)；同时将步骤S2得到的像素坐标系下的各车道线方程投影在自车车身坐标系中，得到自车车身坐标系下车道线方程y＝f(x)，共含有J条车道线；
[0046] S6.2沿目标车辆三维边界框的的第一角点和第二角点X1,X2作直线，直线方程为：
[0047]
[0048] 该直线与车道线相交，记各交点分别为Cj，记各交点在自车车身坐标系下的Ye轴坐标分别为CjY，j＝1,2,…,J；
[0049] S6.3将各车道线与目标车辆三维边界框前部直线交点Cj的Ye轴坐标CjY和目标车辆三维边界框第一角点和第二角点的Ye轴坐标进行升序排列，通过比较交点坐标和目标车辆三维边界点框前部角点坐标的大小，确定目标车辆相对于车道线的位置；
[0050] S6.4过自车车身坐标系原点作任意一条与车道线非平行的直线，利用步骤S6.3所述方法判断自车所在车道，以此确定目标车辆与自车的横向相对位置关系；所述目标车辆与自车的横向相对位置分为左侧相邻、右侧相邻和不相邻；
[0051] S7评估目标车辆对自车换道的碰撞风险
[0052] S7.1根据步骤S6.4中得到的目标车辆和自车横向相对位置关系，判断目标车辆是否为潜在危险车辆：若目标车辆在自车的相邻车道，则目标车辆为换道预警系统的潜在危险车辆，执行步骤S7.2；若目标车辆不在自车相邻车道，则换道预警系统不关注该车辆，本轮车辆换道预警结束，返回步骤S2，执行下一轮车辆换道预警；
[0053] S7.2在换道过程中，规定目标车辆沿当前车道匀加速运动；设步骤S5确定的目标车辆三维边界框的前部中心接地点当前时刻t0在自车车身坐标系中的坐标为[x0,y0]，目标车辆运动速度为[vx0,vy0]，加速度为[ax0,ay0]，则此后任意时刻t目标车辆的坐标为：
[0054]
[0055]
[0056] S7.3对于换道预警系统的潜在危险车辆，根据如下规则进行换道风险评估：
[0057] 1)若当前时刻目标车辆三维边界框前部距离自车尾部距离小于设定阈值，且目标车辆在换道预警系统反应时间ts之后的预测位置x(ts)至自车尾部的距离小于自车后视镜盲区范围D，则换道预警系统发出盲区预警信号，直至当目标车辆三维边界框前部越过自车前部时，盲区预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警；
[0058] 2)若当前时刻目标车辆前部至自车尾部的距离大于自车后视镜盲区范围D，计算目标车辆与自车的纵向距离碰撞时间TTC，若距离碰撞时间大于零且小于等于3.5秒，则换道预警系统发出车辆快速接近预警信号，直至当目标车辆三维边界框前部越过自车前部时，取消车辆快速接近预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警，其中，距离碰撞时间定义为：
[0059]
[0060] 其中，Lr为自车车身坐标系原点到自车尾部的距离；ve为自车纵向车速。
[0061] 本发明的特点及有益效果：
[0062] 本发明针对车辆换道预警场景，使用相机采集连续多帧侧后方车辆图像。通过检测车辆二维边框和车辆三维尺寸，利用相机几何投影约束及多帧图像中车辆的运动约束，高精度地估计他车位姿，从而更精确的计算他车与自车的横纵向距离，提高了车辆换道预警的准确率。
附图说明
[0063] 图1为本发明实施例中建立的目标车辆车身坐标系及自车车身坐标系。
[0064] 图2为本发明实施例中建立的像素坐标系及目标车辆二维边界框示意图。
[0065] 图3为本发明实施例中涉及的目标车辆多帧位置运动约束示意图。
[0066] 图4为本发明实施例中涉及的目标车辆车道位置判断示意图。
具体实施方式
[0067] 以下结合附图及实施例对本发明的技术方案进一步详细说明如下：
[0068] 本发明实施例的一种基于连续图像约束位姿估计的车辆换道预警方法，在自车车顶中心固定有相机，自车尾部中心还搭载有毫米波雷达、全球定位系统GPS和惯性导航单元IMU，各器件均按相同频率(本实施例取采集频率为10Hz)采集相应信息。本方法具体包括以下步骤：
[0069] S1建立目标车辆车身坐标系、自车车身坐标系和像素坐标系，参见图1，具体包括：
[0070] S1.1建立目标车辆车身坐标系
[0071] 将目标车辆三维边界框简化为一个长方体，以车辆前部中心接地点T为原点、向目标车辆后侧为Xt轴正向、向目标车辆右侧为Yt轴正向、向上为Zt轴正向，建立与目标车辆固定的目标车辆车身坐标系T-XtYtZt。在目标车辆车身坐标系中，设车辆长方体的8个角点的坐标分别为：
[0072]
[0073]
[0074] 其中，dx,dy,dz为目标车辆三维边界框沿Xt轴，Yt轴和Zt轴方向的长度。
[0075] S1.2建立自车车身坐标系
[0076] 以相机中心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均为待求量。
[0077] S1.3建立像素坐标系
[0078] 以相机采集图像的左上角Op为原点、向右为U轴正向、向下为V轴正向，建立像素坐标系Op-UV，如图2所示。
[0079] S2目标车辆图像采集与处理及自车状态测量
[0080] 采集当前时刻自车后方的图像并使用已有成熟算法处理图像得到后方目标车辆的三维边界框的长宽高、二维边界框和车道线信息；采用车载毫米波雷达测量当前时刻目标车速度；采用车载GPS和惯性导航单元IMU测量当前时刻自车的速度和位姿。
[0081] S2.1采用固定在自车顶部的相机采集当前时刻自车后方的图像作为当前帧图像。
[0082] S2.2通过CNN网络模型对采集的当前帧图像进行回归处理得到目标车辆三维边界框的尺寸dx,dy,dz(具体可参见Mousavian A,Anguelov D,Flynn J,et al.3D Bounding Box Estimation Using Deep Learning and Geometry[J].2016.)。
[0083] S2.3通过基于统一多尺度深卷积神经网络的车辆目标检测算法得到当前帧图像中目标车辆在像素坐标系下的矩形二维边界框并计算其上5个特征点的坐标，参见图2，其中，第一特征点、第三特征点作为二维边界框的上边线两端点且分别与三维边界框的第八角点、第六角点相对应，第二特征点位于二维边界框的上边线上且与三维边界框的第七角点相对应，第四特征点位于二维边界框的左边线上且与三维边界框的第四角点相对应，第五特征点位于二维边界框的下边线上且与三维边界框的第一角点相对应(具体可参见Cai Z,Fan Q,Feris R S,et al.A Unified Multi-scale Deep Convolutional Neural Network for Fast Object Detection[J].2016.)。
[0084] S2.4对当前帧图像通过基于自适应阈值和车道分类的车道线检测算法得到像素坐标系下的各车道线方程，本实施例中采用3次多项式形式的车道线方程(Son Y,Lee E S,Kum D.Robust multi-lane detection and tracking using adaptive threshold and lane classification[J].Machine Vision and Applications,2018,30(8):111-124.)。
[0085] S2.5采用车载后向毫米波雷达测量得到当前时刻目标车辆在自车车身坐标系中的速度。
[0086] S2.6采用车载GPS和IMU测量当前时刻自车的位置、速度和朝向角。
[0087] S3利用连续N帧图像建立目标车辆三维边界框与二维边界框的投影约束[0088] S3.1根据目标车辆车身坐标系和像素坐标系的投影关系，目标车辆车身坐标系中任意点X0＝[X,Y,Z]T与像素坐标系中对应点x0＝[u,v]T满足以下变换关系：
[0089]
[0090] 其中，K为相机的内部参数矩阵(维度为3×3)，根据所选用的相机型号确定，为已知量；R’为目标车辆车身坐标系到自车车身坐标系的旋转矩阵(维度为3×3)，由目标车辆的方位R确定；
[0091] 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个投影约束，表达式如下：
[0092]
[0093]
[0094]
[0095]
[0096]
[0097] 其中，*表示该处为任意值，没有约束。k＝1,2…N对应图像的帧数，k＝N(N优选2或3)代表当前帧，每一帧图像中目标车辆的二维边界框位置不同，用 表示第k帧图像中二维边界框的第p特征点在像素坐标系中的坐标，通过步骤S2.3得到，p＝1,2,3,4,5。每一帧图像中目标车辆头部中心接地点T在自车车身坐标系中的坐标位置不同，用Tk＝[txk,tyk,0]表示(为待求量)。规定短时间内各帧图像中目标车辆在自车车身坐标系中的方位相同，均用R＝[θ,0,0]表示，R′为目标车辆车身坐标系到自车车身坐标系的旋转矩阵，由R确定。Tk,R均为待求量。
[0098] S4利用连续N帧图像建立同一目标车辆位置运动约束
[0099] 如图3所示，设连续两帧图像中的目标车辆车头前部中心接地点在自车车身坐标系中的坐标为：T(tk-1)＝[tx(k-1),ty(k-1),0],T(tk)＝[txk,tyk,0]，tk,tk-1是连续两帧图像的采集时间。
[0100] 在短时间内，车辆运动的朝向角可假设为恒定，车辆运动速度大小和方向保持不变，因此对于车辆前部中心接地点坐标位置根据当前帧图像以及前N-1帧图像建立共计2(N-1)个运动约束：
[0101] txk＝tx(k-1)+vxk(tk-tk-1) (7)
[0102] tyk＝ty(k-1)+vyk(tk-tk-1) (8)
[0103] 其中，vxk,vyk分别为第k帧图像对应时刻的自车车身坐标系中目标车辆的纵向和横向速度，通过车载后向毫米波雷达获取。tk-tk-1为连续两帧图像间的时间间隔，由相机的采集频率确定，本实施例中取为0.1s。
[0104] S5多约束求解目标车辆方位并得到目标车辆三维边界框
[0105] 联立公式(2)～(8)，得到(8N-2)个约束方程，其中有(2N+1)个待求解参数，约束方程数大于未知数数量，使用最小二乘法求解可得到未知参数，得到当前时刻目标车辆在自k车车身坐标系中的位置T和方位R，即确定当前时刻目标车辆的三维边界框。
[0106] S6根据当前时刻目标车辆三维边界框，确定目标车辆与自车的横纵向相对位置关系。
[0107] S6.1记当前时刻目标车辆三维边界框的第一角点和第二角点在自车车身坐标系中的坐标分别为X1(x1,y1),X2(x2,y2)；同时将步骤S2得到的像素坐标系下的各车道线方程投影在自车车身坐标系中，得到自车车身坐标系下车道线方程y＝f(x)，共含有J条车道线，如图4中包括J＝3条车道线。
[0108] S6.2沿目标车辆三维边界框的的第一角点和第二角点X1,X2作直线，直线方程为：
[0109]
[0110] 该直线与车道线相交，记各交点分别为Cj，记各交点在自车车身坐标系下的Ye轴坐标分别为CjY，j＝1,2,…,J，J为车道线总数。
[0111] S6.3将各车道线与目标车辆三维边界框前部直线交点Cj的Ye轴坐标CjY和目标车辆三维边界框第一角点和第二角点的Ye轴坐标进行升序排列，通过比较交点坐标和目标车辆三维边界点框前部角点坐标的大小，确定目标车辆相对于车道线的位置，具体如下：
[0112] 若CjY
[0113] S6.4过自车车身坐标系原点(即自车中心)作任意一条与车道线非平行的直线，利用步骤S6.3所述方法判断自车所在车道，最终确定目标车辆与自车的横向相对位置关系。目标车辆与自车的横向相对位置分为左侧相邻、右侧相邻和不相邻。
[0114] S7评估目标车辆对自车换道的碰撞风险
[0115] S7.1根据步骤S6.4中得到的目标车辆和自车横向相对位置关系，判断目标车辆是否为潜在危险车辆。若目标车辆在自车的相邻车道，则目标车辆为换道预警系统的潜在危险车辆，执行步骤S7.2；若目标车辆不在自车相邻车道，则换道预警系统不关注该车辆，本轮车辆换道预警结束，返回步骤S2，执行下一轮车辆换道预警。
[0116] S7.2在换道过程中，规定目标车辆沿当前车道匀加速运动，预测目标车辆未来任意时刻的位置。设步骤S5确定的目标车辆三维边界框的前部中心接地点当前时刻t0在自车车身坐标系中的坐标为[x0,y0]，目标车辆运动速度为[vx0,vy0]，加速度为[ax0,ay0]，则此后任意时刻t目标车辆的坐标为：
[0117]
[0118]
[0119] S7.3对于换道预警系统潜在危险车辆，则根据如下规则进行换道风险评估：
[0120] 1)若当前时刻目标车辆三维边界框前部距离自车尾部距离小于设定阈值(一般取30～50米，本实施例取30米)，且目标车辆在换道预警系统反应时间ts之后的预测位置x(ts)至自车尾部的距离小于自车后视镜盲区范围D(本实施例D＝3米)，则换道预警系统发出盲区预警信号，直至当目标车辆三维边界框前部越过自车前部时，盲区预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警。根据国标ISO 17387-2008Intelligent transport systems-Lane change decision aid systems(LCDAS)要求系统反应时间不大于300毫秒，本实施例取换道预警系统的反应时间为200毫秒。即：
[0121] |x(ts)|-Lr
[0122] 时给出盲区预警，其中Lr为自车车身坐标系原点(即相机中心)到自车尾部的距离，本例中Lr＝1.37m。
[0123] 2)若当前时刻目标车辆前部至自车尾部的距离大于自车后视镜盲区范围D(本实施例D＝3米)，计算目标车辆与自车的纵向距离碰撞时间TTC，若距离碰撞时间大于零且小于等于3.5秒，则换道预警系统发出车辆快速接近预警信号，直至当目标车辆三维边界框前部越过自车前部时，取消车辆快速接近预警信号取消，本轮预警结束，返回步骤S2，执行下一轮车辆换道预警，其中，距离碰撞时间定义为：
[0124]
[0125] 其中ve为自车纵向车速。
[0126] 以上所述仅为本发明的实施例，并非因此限制本发明的保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的保护范围内。
