CN114333297A - 一种基于交通雷达的车辆所属弯道车道估计方法 - Google Patents

Abstract

本发明公开了一种基于交通雷达的车辆所属弯道车道估计方法，该方法应用场景为高速公路的直线段或曲线段，雷达波束覆盖所测车道。首先，选取来向和去向道路边线地图信息，共四条车道线地图信息点集，将地图点集经纬高坐标先转换为东北天坐标，再将东北天坐标转换为雷达本地坐标，获得雷达本地坐标系下的道路高精度地图信息，然后结合目标所处道路在雷达本地坐标系中的偏转角，使用该偏转角将雷达坐标系旋转到新坐标系，得到新坐标系下目标车辆坐标和地图点集坐标，在新坐标系下利用线性插值找到车辆的道路边界定位点坐标，最后利用车辆坐标和边界定位点坐标计算归属车道。本发明具有高实时性、高精度的优点，可实现对车辆归属车道的精准估计。

Description

一种基于交通雷达的车辆所属弯道车道估计方法

技术领域

本发明属于交通雷达应用技术领域，具体涉及一种基于交通雷达的车辆所属弯道车道估计方法。

背景技术

随着智能交通系统的发展，人们对于汽车道路安全信息关注度不断提高。车辆在高速上行驶的过程中，探测雷达精确捕获每个车辆的归属车道是有实际意义的，这有利于提高雷达对车辆的追踪精度，也可以获取单个车道的车流量。因此作为交通雷达需要获取的路况信息，精确计算每个目标车辆的归属车道成为一个亟待解决的问题。

现有技术中，毫米波雷达一般用于对十字路口上车辆进行归属车道估计，十字路口道路是直道，车道划分是通过对雷达本地坐标系中的X坐标值判定归属车道，但是该方法不适用于高速上的弯道。

发明内容

有鉴于此，本发明的目的是提供一种基于交通雷达的车辆所属弯道车道估计方法，可实时输出车辆的归属车道，计算复杂度低，实时性强。

一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，包括如下步骤：

S0、建立雷达本地笛卡尔坐标系X₀Y₀Z₀，交通雷达所处位置为坐标原点，以雷达阵面法线为Y₀轴，使Y₀轴尽量与道路方向平行；

S1：获取交通雷达的经纬度信息和偏北角θ；其中，偏北角θ指的是雷达本地坐标系X₀Y₀Z₀的Y₀轴相对于东北天坐标系ENU中N轴的夹角；

S2：获得待测路段地图点集的经纬高信息；

S3：将道路地图点集的经纬度和雷达位置的经纬度坐标转换到东北天坐标系ENU中；

S4：将道路地图点集和雷达位置的经纬度从东北天坐标系ENU转换为雷达本地笛卡尔坐标系X₀Y₀Z₀中；

S5：对车道信息进行提取，包括车道宽度，车道数量；

S6：结合目标在雷达本地笛卡尔坐标系X₀Y₀Z₀的位置和车道线信息，得到目标车辆的归属车，具体为：

根据交通雷达返回的数据，提取目标车辆的位置信息和速度信息；

由目标车辆的位置信息中Y轴坐标值y，在目标车辆所在道路左边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标定义为(x_N,Y_N)和(x_N-1,Y_N-1)；在目标车辆所在道路右边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点集，其坐标定义为(x_M,Y_M)和(x_M-1,Y_M-1)；计算出目标车辆位置处车道左边界线偏转角和右边界线偏转角，其中左边界线偏转角为两地图点集(x_N,Y_N)和(x_N-1,Y_N-1)连线与Y₀轴夹角

右边界线偏转角为两地图点集(x_M,Y_M)和(x_M-1,Y_M-1)连线与Y₀轴夹角

最后求出两偏转角均值，作为该目标运动方向相对于Y₀轴的偏转角

使用该偏转角

将雷达坐标系X₀Y₀Z₀旋转到新坐标系XYZ：

即

由车辆位置数据中在新坐标系XYZ中的y值，在目标车辆所在道路左边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标定义为(x_N′,Y_N′)和(x_N′-1,Y_N′-1)；同理，在目标车辆所在道路右边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标定义为(x_M′,Y_M′)和(x_M′-1,Y_M′-1)，则车辆与道路左边界和右边界的距离x_{_left}和x_{_right}值可以由相邻的两个地图点集坐标(x_N′,Y_N′)和(x_N′-1,Y_N′-1)，(x_M′,Y_M′)和(x_M′-1,Y_M′-1)进行线性插值得到，插值公式表示为：

根据车辆所在与道路左边界和右边界的距离x_{_left}和x_{_right}，结合道路总宽度以及各车道宽度确定车辆所在车道。

进一步的，步骤S6中，利用x_{_left}和x_{_right}计算车辆位置x所占车道比例rate：

根据rate的值来判断车辆归属车道：假设高速公路为双向N车道，那么单向车道总宽：

其中，I_∑为单向车道总宽；

所属车道判定规则为：

当

判定为1车道；当

判定为2车道；当

判定为h车道；其中h∈N。

进一步的，所述S6中，假如目标落入绿化带内，rate满足α≤rate<0，α是设定的临界值，则将该目标归属车道判断为1车道。

进一步的，所述S6中，假如目标落到车道之外，即1≤rate<1+β，β是设定的临界值，则将该目标归属车道判断为应急车道。

较佳的，所述交通雷达安装在路中或路旁。

较佳的，所述交通雷达的经纬度信息和偏北角θ通过GPS设备获取。

较佳的，所述S3中，使用地理信息标准库的转换公式，将道路地图点集的经纬度和雷达位置的经纬度坐标转换到东北天坐标系ENU中。

较佳的，所述S4中，将道路地图点集和雷达位置的经纬度从东北天坐标系ENU转换为雷达本地笛卡尔坐标系X₀Y₀Z₀中，转换公式如下：

即

其中x₀，y₀，z₀为雷达本地笛卡尔坐标系XYZ中的坐标，E，N，U为东北天坐标系ENU中坐标，L为转换矩阵，ψ为偏转角且数值等于-θ。

本发明具有如下有益效果：

本发明公开了一种基于交通雷达的车辆所属弯道车道估计方法，该方法应用场景为高速公路的直线段或曲线段，雷达波束覆盖所测车道。首先，选取来向和去向道路边线地图信息，共四条车道线地图信息点集，将地图点集经纬高坐标先转换为东北天坐标，再将东北天坐标转换为雷达本地坐标，获得雷达本地坐标系下的道路高精度地图信息，然后结合目标所处道路在雷达本地坐标系中的偏转角，使用该偏转角将雷达坐标系旋转到新坐标系，得到新坐标系下目标车辆坐标和地图点集坐标，在新坐标系下利用线性插值找到车辆的道路边界定位点坐标，最后利用车辆坐标和边界定位点坐标计算归属车道。本发明具有高实时性、高精度的优点，可实现对车辆归属车道的精准估计。

附图说明

图1是本发明的东北天坐标系与雷达本地坐标系以及两者转换关系示意图；

图2是目标车辆归属车道计算示意图；

图3为本发明偏转角计算示意图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

如图1所示，地图点集的选取是由单向车道的左右护栏决定的，两两地图点集沿道路方向间隔10m(可以根据实际需要调整间隔距离)。雷达的波束指向要求覆盖所测道路，其位置安装在路中和路旁均可。其中东北天坐标系ENU的N轴指北；雷达本地笛卡尔坐标系X₀Y₀Z₀，雷达所处位置为坐标原点，以雷达的前方或侧前方为Y₀轴，使Y₀轴尽量与道路方向平行。为了简洁，图中均省去了高度坐标轴。地图点集按照地理信息标准库中的转换关系，由经纬高坐标系转换为东北天坐标系ENU，再由坐标系ENU转换为雷达本地笛卡尔坐标系X₀Y₀Z₀。有了道路地图点集在雷达本地坐标系X₀Y₀Z₀的位置信息后，才能对车辆进行车道归属估计。

图2所示是高速双向车道目标车辆归属车道估计建模。两向车道被绿化带分开，将雷达置于绿化带上，此时功率波束会更好的覆盖双向车道，使探测目标的接收信噪比更加稳定。车道为双向8车道，最外围车道是应急车道，绿化带宽度大于车道宽度。此时的坐标系是雷达坐标系X₀Y₀Z₀和雷达坐标系经过旋转后的坐标系XYZ，估计方法具体步骤如下：

S1：获取交通雷达的经纬度信息和偏北角θ(可通过GPS设备获取)；其中，偏北角θ指的是雷达本地坐标系X₀Y₀Z₀的Y₀轴相对于东北天坐标系ENU中N轴的夹角。

S2：获得待测高速路段地图点集的经纬高(经度，纬度，高度)信息，经纬高地图点集数据范围为雷达测量范围，地图点集间隔为10m(距离可调)；

S3：使用地理信息标准库的转换公式，将道路地图点集和雷达位置的经纬度(WGS84)坐标转换到东北天坐标系ENU中；

S4：将道路地图点集和雷达位置的经纬度从东北天坐标系ENU转换为雷达本地笛卡尔坐标系X₀Y₀Z₀中，转换公式如下：

即

其中x₀，y₀，z₀为雷达本地笛卡尔坐标系XYZ中的坐标，E，N，U为东北天坐标系ENU中坐标，L为转换矩阵，ψ为偏转角且数值等于-θ。

S5：对车道信息进行提取，包括车道宽度，车道数量；

S6：结合目标在雷达本地笛卡尔坐标系XYZ的位置和车道线信息，得到目标车辆的归属车，具体为：

根据交通雷达返回的数据，提取目标车辆的位置信息和速度信息；

如附图3所示，由目标车辆的位置信息中Y轴坐标值y，在目标车辆所在道路左边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，如图3所示，该两个地图点即为沿左边界上与车辆前、后两个地图点集1和2，其坐标可以定义为(x_N,Y_N)和(x_N-1,Y_N-1)；同理，在目标车辆所在道路右边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点集3和4，其坐标可以定义为(x_M,Y_M)和(x_M-1,Y_M-1)；由于车辆所在车道不是直道而是弯道，根据y值确定的道路两侧边界的最接近地图点集并非是真正的点集，因此需要计算出目标车辆位置处车道左边界线偏转角和右边界线偏转角，其中左边界线偏转角为两地图点集(x_N,Y_N)和(x_N-1,Y_N-1)连线与Y₀轴夹角

右边界线偏转角为两地图点集(x_M,Y_M)和(x_M-1,Y_M-1)连线与Y₀轴夹角

最后求出两偏转角均值，作为该目标运动方向相对于Y₀轴的偏转角

使用该偏转角

将雷达坐标系X₀Y₀Z₀旋转到新坐标系XYZ：

即

由车辆位置数据中在新坐标系XYZ中的y值，在目标车辆所在道路左边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标可以定义为(x_N′,Y_N′)和(x_N′-1,Y_N′-1)；同理，在目标车辆所在道路右边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标可以定义为(x_M′,Y_M′)和(x_M′-1,Y_M′-1)，则车辆与道路左边界和右边界的距离x_{_left}和x_{_right}值可以由相邻的两个地图点集坐标(x_N′,Y_N′)和(x_N′-1,Y_N′-1)，(x_M′,Y_M′)和(x_M′-1,Y_M′-1)进行线性插值得到，插值公式表示为：

至此，由于计算得到了车辆所在与道路左边界和右边界的距离x_{_left}和x_{_right}，根据道路总宽度以及各车道宽度即可确定车辆所在车道。

为了简化计算过程，本发明采用如下方法估计车辆所在车道，具体为：

利用x_{_left}和x_{_right}计算车辆位置x所占车道比例rate：

根据rate的值来判断车辆归属车道。高速公路为双向N车道，那么单向车道总宽：

其中，I_∑为单向车道总宽。

以图2为例，假设车道1、2、3、4和5的道路间距分别为I₁、I₂、I₃、I₄和I₅，各车道判定规则为：

(1)

判定为1车道。

(2)

判定为2车道。

(3)

判定为3车道。

(4)

判定为4车道。

(5)

判定为应急车道。

当车道数量N≠5时，同样使用图2示例的方法判定。

为了减少计算量，在对地图点集进行坐标系转换时，可以只转换目标车辆附近的地图点集的坐标值。转换的地图点集取值范围由目标车辆在雷达坐标系中的Y₀值确定：由Y₀值找到左右边界定位点所处于在道路地图点集的区间，该区间即为需要转换的地图点集范围。

由于目标距离较远，雷达测量精度下降，可能导致滤波航迹偏入绿化带或车道外，在这里说明如何处理这些特殊情况：

(1)假如目标落入绿化带内，rate满足α≤rate<0，α是设定的临界值，由道路信息决定，且目标的速度和上行道方向相符或者目标的速度和下行道相符，则将该目标归属车道判断为1车道。

(2)假如目标落到车道之外，即1<rate≤1+β，β是设定的临界值，由道路信息决定，且目标的速度和上行道方向相符或者目标的速度和下行道相符，则将该目标归属车道判断为5车道，也就是应急车道。

S7：需要对每一个目标车辆重复一遍S6步骤。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，包括如下步骤：

S0、建立雷达本地笛卡尔坐标系X₀Y₀Z₀，交通雷达所处位置为坐标原点，以雷达阵面法线为Y₀轴，使Y₀轴尽量与道路方向平行；

S1：获取交通雷达的经纬度信息和偏北角θ；其中，偏北角θ指的是雷达本地坐标系X₀Y₀Z₀的Y₀轴相对于东北天坐标系ENU中N轴的夹角；

S2：获得待测路段地图点集的经纬高信息；

S3：将道路地图点集的经纬度和雷达位置的经纬度坐标转换到东北天坐标系ENU中；

S4：将道路地图点集和雷达位置的经纬度从东北天坐标系ENU转换为雷达本地笛卡尔坐标系X₀Y₀Z₀中；

S5：对车道信息进行提取，包括车道宽度，车道数量；

S6：结合目标在雷达本地笛卡尔坐标系X₀Y₀Z₀的位置和车道线信息，得到目标车辆的归属车，具体为：

根据交通雷达返回的数据，提取目标车辆的位置信息和速度信息；

由目标车辆的位置信息中Y轴坐标值y，在目标车辆所在道路左边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标定义为(x_N,Y_N)和(x_N-1,Y_N-1)；在目标车辆所在道路右边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点集，其坐标定义为(x_M,Y_M)和(x_M-1,Y_M-1)；计算出目标车辆位置处车道左边界线偏转角和右边界线偏转角，其中左边界线偏转角为两地图点集(x_N,Y_N)和(x_N-1,Y_N-1)连线与Y₀轴夹角

右边界线偏转角为两地图点集(x_M,Y_M)和(x_M-1,Y_M-1)连线与Y₀轴夹角

最后求出两偏转角均值，作为该目标运动方向相对于Y₀轴的偏转角

使用该偏转角

将雷达坐标系X₀Y₀Z₀旋转到新坐标系XYZ：

即

由车辆位置数据中在新坐标系XYZ中的y值，在目标车辆所在道路左边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标定义为(x_N′,Y_N′)和(x_N′-1,Y_N′-1)；同理，在目标车辆所在道路右边界上地图点集中，找到与Y轴坐标值与y值最接近的两个地图点，其坐标定义为(x_M′,Y_M′)和(x_M′-1,Y_M′-1)，则车辆与道路左边界和右边界的距离x_{_left}和x_{_right}值可以由相邻的两个地图点集坐标(x_N′,Y_N′)和(x_N′-1,Y_N′-1)，(x_M′,Y_M′)和(x_M′-1,Y_M′-1)进行线性插值得到，插值公式表示为：

根据车辆所在与道路左边界和右边界的距离x_{_left}和x_{_right}，结合道路总宽度以及各车道宽度确定车辆所在车道。

2.如权利要求1所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述步骤S6中，利用x_{_left}和x_{_right}计算车辆位置x所占车道比例rate：

根据rate的值来判断车辆归属车道：假设高速公路为双向N车道，那么单向车道总宽：

其中，I_∑为单向车道总宽；

所属车道判定规则为：

当

判定为1车道；当

判定为2车道；当

判定为h车道；其中h∈N。

3.如权利要求2所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述S6中，假如目标落入绿化带内，rate满足α≤rate<0，α是设定的临界值，则将该目标归属车道判断为1车道。

4.如权利要求2所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述S6中，假如目标落到车道之外，即1≤rate<1+β，β是设定的临界值，则将该目标归属车道判断为应急车道。

5.如权利要求1、2、3或4所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述交通雷达安装在路中或路旁。

6.如权利要求1、2、3或4所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述交通雷达的经纬度信息和偏北角θ通过GPS设备获取。

7.如权利要求1、2、3或4所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述S3中，使用地理信息标准库的转换公式，将道路地图点集的经纬度和雷达位置的经纬度坐标转换到东北天坐标系ENU中。

8.如权利要求1、2、3或4所述的一种基于交通雷达的车辆所属弯道车道估计方法，其特征在于，所述S4中，将道路地图点集和雷达位置的经纬度从东北天坐标系ENU转换为雷达本地笛卡尔坐标系X₀Y₀Z₀中，转换公式如下：

即

其中x₀，y₀，z₀为雷达本地笛卡尔坐标系XYZ中的坐标，E，N，U为东北天坐标系ENU中坐标，L为转换矩阵，ψ为偏转角且数值等于-θ。

Images