CN110208842A - 一种车联网环境下车辆高精度定位方法 - Google Patents

Abstract

本发明公开了一种车联网环境下车辆高精度定位方法，待定位车辆上配备激光雷达和雷达两种类型的传感器，采用扩展卡尔曼滤波将两种测量数据进行融合，并通过与周围配备GPS+IMU组合导航***的车辆进行V2V通信，来对自身定位结果进行改进得到相对准确的定位；本发明方法，利用其他车辆共享的原始GPS数据来为自动驾驶汽车定位，使得车辆在一些无GPS信号或者GPS信号弱的地区也能进行相对准确的定位。另外，多传感器和V2V通信的融合也使得自动驾驶车辆的定位方案比仅使用高质量传感器的方法更具成本效益。

Description

一种车联网环境下车辆高精度定位方法

技术领域

本发明属于无人驾驶车辆技术领域，具体设计一种车联网环境下车辆高精度定位方法。

背景技术

无人驾驶汽车是一种通过车载传感***感知道路环境，自动规划行车路线并控制车辆到达预定目标的智能汽车。它根据感知得到的道路、车辆位置和障碍物信息，控制车辆的行驶，从而使车辆能够安全、可靠地在道路上行驶。无人驾驶汽车集自动控制、体系结构、人工智能、视觉计算等众多技术于一体，是计算机科学、模式识别和智能控制技术高度发展的产物，也是衡量一个国家科研实力和工业水平的一个重要标志，在国防和国民经济领域具有广阔的应用前景。

定位精度的高低直接影响了无人驾驶汽车的发展和使用。无人驾驶车辆通过将GPS数据与其他传感器的数据进行融合，并通过DSRC实现V2I和V2V通信，以此来校正偏差，提高自身定位精度，有助于帮助无人驾驶车辆自动规划行车路线，选择合适的车道，处理各种交通突发情况，增强车辆行驶的智能性和安全性。

传统的车辆定位通常使用GPS与惯性导航***组成。GPS信号在高架桥下、隧道中、树木茂密的郊区或乡村道路以及高楼林立的城市路段中易被遮挡或者存在多径效应。而基于惯性***的定位方法则易造成累计误差，难以满足无人驾驶车辆长距离行驶对定位***的要求。

公开号为CN106767853A的专利(申请号201611261781.5)提供了一种基于多信息融合的无人驾驶车辆的高精度定位方法。该方法包括地图模块、相机处理模块和雷达处理模块，地图模块根据实时定位信息绘制以车辆为中心，一定范围内的局部地图，供相机处理模块和雷达处理模块使用；相机处理模块使用相机采集车辆行驶过程中的路面原始数据，提取车道线，用于地图匹配和校正；雷达处理模块使用激光雷达采集车辆周围环境信息，提取道路边界，用于地图匹配和校正。由于该专利要求使用离线地图，且地图信息存在偏差，所以应用范围有限，定位精度不足。

采用车联网环境下的车辆高精度定位方案，车辆通过DSRC技术进行V2V通信，可以使用自身融合后的雷达和激光雷达两种传感器的测量数据，再借助其他车辆共享的GPS数据，得到相对准确的定位，并且使得车辆可以在一些无GPS信号或者GPS信号弱的地区也能进行相对准确的定位。另外，多传感器和V2V通信的融合也使得自动驾驶车辆的定位方案比仅使用高质量传感器的方法更具成本效益。

发明内容

为了克服现有技术存在的缺陷，本发明提供了一种网联环境下的车辆高精度定位方法，可以使用自身融合后的雷达和激光雷达两种传感器的测量数据，再借助其他车辆共享的GPS数据，得到相对准确的定位，并且使得车辆可以在一些无GPS信号或者GPS信号弱的地区也能进行相对准确的定位。

为了实现上述任务，本发明采用以下技术方案：

一种车联网环境下车辆高精度定位方法，包括以下步骤：

步骤1，在接入车联网的车辆中，确定待定位车辆，在待定位车辆上配备激光雷达、毫米波雷达以及车载单元OBU，在其余的M个车辆上均配备车载GPS、IMU设备以及车载单元；

步骤2，建立车辆的运动参考模型，所述的运动参考模型为恒定转率和速度模型；

步骤3，交通信息采集与预处理

通过DSRC技术将车联网中的各个车辆上的车载单元OBU相连接，以便于进行实时的数据交互与处理；在其他车辆上，通过车载GPS、IMU设备进行定位信息的采集和融合，得到融合后的定位信息；待定位车辆通过车载单元OBU采集其他车辆发来的位置信息，同时通过待定位车辆上安装的激光雷达和毫米波雷达采集待定位车辆与其他车辆的相对位置、距离和速度信息；

步骤4，传感器数据融合

将待定位车辆上的激光雷达和毫米波雷达采集的数据进行融合，得到待定位车辆的位置坐标；

步骤5，根据待定位车辆接收到的其他车辆的数据，联合待定位车辆测量的相对于其他车辆的相对距离向量和方位角，解算出待定位车辆的多个理论位置，然后联合步骤4传感器数据融合得到的待定位车辆位置坐标，计算出待定位车辆的最终位置。

进一步地，步骤2中所述的运动参考模型中车辆的状态向量为：

其中，(x,y)为车辆在局部坐标系下的坐标，其中局部坐标系是以车辆的初始位置为坐标系的原点，车正前方为x轴，车的左侧为y轴建立的坐标系；v为待定位车辆行驶速度，θ为偏航角，ω是偏航角速度；

运动参考模型的状态转移函数为：

其中，t表示时间，Δt表示时间的变化量，x(t)、y(t)表示t时刻的坐标。

进一步地，步骤3的具体过程包括：

步骤3.1，其他车辆上的车载单元OBU通过车载GPS、IMU设备来采集自身的定位信息并进行融合，得到融合后的定位信息，包括经度L、纬度B和高度H；

步骤3.2，待定位车辆通过OBU采集周围第i辆其他车辆发送来的GPS数据，并使用激光雷达检测该车辆的位置，得到该车辆在局部坐标系下的坐标位置(p_x,p_y,p_z)；利用毫米波雷达采集的各个其他车辆相对于待定位车辆的位置、距离和速度信息，包括：相对距离向量ρ，方位角φ—向量ρ与x轴方向的夹角，速度变化量速度信息，包括局部坐标系下的速度v_x和v_y；航向信息，包括偏航角θ，偏航角速度ω。

进一步地，所述的步骤5具体包括：

步骤5.1，将每个其他车辆发来的各自车载GPS、IMU设备采集定位数据的融合结果转换到直角坐标系下，得到每个其他车辆i在直角坐标系下的坐标；

步骤5.2，将步骤5.1转换后的坐标转换到东北天坐标系下，得到每个其他车辆i在东北天坐标系下的坐标；

步骤5.3，将步骤5.2转换后的坐标转换到局部坐标系下，得到每个其他车辆i在局部坐标系下的坐标；

步骤5.4，分别将每个其他车辆i在局部坐标系下的坐标联合毫米波雷达测量的数据进行解算，得到待定位车辆的M个理论位置：

其中，(a_i,b_i,c_i)表示第待定位车辆的i个理论位置，i＝1,2,…M，M为其他车辆的总数；ρ_i为待定位车辆相对于第i个其他车辆的相对距离向量，φ_r为待定位车辆的方位角；

步骤5.5，误差数据剔除，具体计算公式为：

根据设定的阈值σ₁,σ₂,σ₃，若或或则判定根据第i个其他车辆计算出的待定位车辆的第i个理论位置(a_i,b_i,c_i)误差过大，将其剔除；设剔除后剩余M′个理论位置；

步骤5.6，待定位车辆最终位置的确定

联合步骤4融合后得到的待定位车辆的位置坐标(p_x,p_y,p_z)，得到待定位车辆的最终位置(a,b,c)：

其中，(a₁,b₁,c₁)…(a_M′,b_M′,c_M′)分别为经过步骤5.5剔除误差数据后，剩余的M′个理论位置。

本发明具有以下技术特点：

1.本发明方法中的待定位车辆上配备激光雷达和雷达两种类型的传感器，采用扩展卡尔曼滤波将两种测量数据进行融合，并通过与周围配备GPS+IMU组合导航***的车辆进行V2V通信，来对自身定位结果进行改进得到相对准确的定位。

2.本发明方法利用其他车辆共享的原始GPS数据来为自动驾驶汽车定位，使得车辆在一些无GPS信号或者GPS信号弱的地区也能进行相对准确的定位。另外，多传感器和V2V通信的融合也使得自动驾驶车辆的定位方案比仅使用高质量传感器的方法更具成本效益。

附图说明

图1为本发明的结构框架示意图。

具体实施方式

本发明公开了一种车联网环境下车辆高精度定位方法，该方法中所用到的模块有：GPS-IMU模块、激光雷达-雷达模块、无线传输模块和坐标系转换模块：

GPS-IMU模块，通过车载GPS和IMU设备，获取车辆当前的位置信息；

激光雷达-雷达模块，使用雷达和激光雷达采集车辆周围的环境信息，用于定位的校正与改进；

无线传输模块，使用专用短距离通信技术DSRC，通过无线通信实现车与车之间的实时信息交互；其中，车载单元OBU，将GPS采集的车辆位置信息，通过无线传输模块与其他车辆的OBU进行GPS数据共享；

坐标系转换模块，将OBU收到的N个其他车辆发送来的各自的GPS-IMU定位结果进行坐标系转换，从原本的地球坐标系转到东北天坐标系下，再转换到激光雷达-雷达的局部坐标系下，便于进行下一步的运算。

本发明方法的具体步骤如下：

一种车联网环境下车辆高精度定位方法，包括以下步骤：

步骤1，在接入车联网的车辆中，确定待定位车辆，在待定位车辆上配备激光雷达、毫米波雷达以及车载单元OBU，在其余的M个车辆上均配备车载GPS、IMU设备以及车载单元。

本实施例中，车载单元OBU采用WB-L20B设备，车载GPS、IMU设备分别采用RT3000惯性和GNSS导航***，激光雷达的型号为velodyne HDL32，毫米波雷达的型号为ARS408-2177GHz。

步骤2，建立车辆的运动参考模型

本方案中，为待定位车辆和其他车辆建立运动参考模型，运动参考模型为恒定转率和速度模型CTRV，所有车辆的行驶符合该参考模型；后续步骤3中采集的相关数据均是车辆在该参考模型下收集到的信息，而步骤4中传感器数据融合过程、噪声的处理问题也由于该模型的引入而得到简化。所述的参考模型中车辆的状态向量为：

以待定位车辆为例，其中，(x,y)为待定位车辆在局部坐标系下的坐标，其中局部坐标系是以待定位车辆的初始位置为坐标系的原点，车正前方为x轴，车的左侧为y轴建立的坐标系；v为待定位车辆行驶速度，θ为偏航角，即待定位车辆在局部坐标系下与x轴的夹角，逆时针方向为正，取值范围是[0,2π)，ω是偏航角速度。

参考模型的状态转移函数为：

其中，t表示时间，Δt表示时间的变化量，x(t)、y(t)表示t时刻的坐标。

当ω＝0时，状态转移公式中的(x,y)将变成无穷大。因此，考察当ω＝0的情况，此时车辆实际上时直线行驶的，所以(x,y)的计算公式就变成了：

x(t+Δt)＝vcos(θ)Δt+x(t)

y(t+Δt)＝vsin(θ)Δt+y(t)

步骤3，交通信息采集与预处理

通过DSRC技术将车联网中的各个车辆上的车载单元OBU相连接，以便于进行实时的数据交互与处理；在其他车辆上，通过车载GPS、IMU设备进行定位信息的采集和融合，得到融合后的定位信息；待定位车辆通过车载单元OBU采集其他车辆发来的位置信息，同时通过待定位车辆上安装的激光雷达和毫米波雷达采集待定位车辆与其他车辆的相对位置、距离和速度信息。

步骤3.1，其他车辆上的车载单元OBU通过车载GPS、IMU设备来采集自身的定位信息并进行融合，得到融合后的定位信息，包括经度L、纬度B和高度H；该数据融合过程属现有技术，在此不赘述。

步骤3.2，待定位车辆通过OBU采集周围第i辆其他车辆发送来的GPS数据，并使用激光雷达检测该车辆的位置，得到该车辆在局部坐标系下的坐标位置(p_x,p_y,p_z)；利用毫米波雷达采集的各个其他车辆相对于待定位车辆的位置、距离和速度信息，包括：相对距离向量ρ，方位角φ—向量ρ与x轴方向的夹角，速度变化量速度信息(包括局部坐标系下的速度v_x和v_y)、航向信息(包括偏航角θ，偏航角速度ω)。

步骤4，传感器数据融合

将待定位车辆上的激光雷达和毫米波雷达采集的数据进行融合，得到更新后的状态向量，该状态向量中包含待定位车辆的位置坐标(p_x,p_y,p_z)；该融合过程属现有技术，其具体步骤为：

步骤4.1，状态向量初始化，在激光雷达和雷达收到第一个测量值后，对状态向量x进行初始化；

激光雷达的初始化：

(p_x,p_y)为待定位车辆使用激光雷达所探测到的其他车辆的中心在局部坐标系下的坐标，(v_x,v_y)为其他车辆在局部坐标系下的x轴和y轴上的速度分量；一般情况下，p_z＝0，因此后续计算中忽略该参数。

毫米波雷达的初始化：

ρ为其他车辆相对待定位车辆的距离向量，φ为向量ρ与x轴方向的夹角，为其他车辆相对待定位车辆的速度变化量。

步骤4.2，状态预测，根据当前的状态向量，对下一时刻的状态进行预测。其中激光雷达和毫米波雷达的预测过程是完全相同的，两者的预测交替进行，因此以下进行统一说明；

首先进行状态的预测：

x'＝Fx

其中，x为当前时刻的状态向量，x'为下一时刻的状态向量，F为和激光雷达与毫米波雷达测量***相关的状态转移矩阵；

其中，F为：

该步骤完成后，得到了预测状态向量x'；

然后进行误差矩阵的预测：

P'＝FPF^T+Q

其中，P为误差矩阵，P'为下一时刻的误差矩阵，Q为预测噪声协方差矩阵。P用x的协方差来表示，根据设备的实际情况来指定；

Q表明了其他外界干扰的影响，车辆的速度v服从高斯分布N(0,Q)。在CTRV模型中，外界干扰为加速度，假定x轴方向上的加速度满足均值为0，方差为σ_ax的高斯分布，y轴方向上的加速度满足均值为0，方差为σ_ay的高斯分布，因此

该步骤完成后，得到了预测误差矩阵P'；

步骤4.3，状态修正，根据激光雷达毫米波雷达的测量值，来对上一步中的状态预测进行修正，激光雷达和雷达的修正过程交替进行；

以下对激光雷达和毫米波雷达的修正过程分别进行说明：

对于激光雷达：

K＝PH^T(HPH^T+R)^-1

x'＝x+K(z-Hx)

P'＝(I-KH)P

其中，K为卡尔曼增益，H为观测矩阵，z为观测值，R为测量噪声协方差矩阵，I为单位矩阵；

其中，H为：

R由所用激光雷达的厂家提供；

激光雷达的修正过程完成后，得到了新的状态向量x'和误差矩阵P'；

对于毫米波雷达：

x'＝x+K(z-f(x))

P'＝(I-KH_j)P

其中，f(x)为状态空间到测量空间的非线性映射，H_j为非线性映射线性化后的Jacob矩阵，R_r为雷达的测量噪声协方差矩阵；

其中，f(x)为：

非线性映射化后的Jacob矩阵H_j为：

R_r由毫米波雷达的厂家提供；

毫米波雷达的修正过程完成后，再次更新状态向量x'和误差矩阵P'，这就是当前时刻的融合结果。

步骤5，待定位车辆位置的确定

步骤5.1，将每个其他车辆发来的各自车载GPS、IMU设备采集定位数据的融合结果转换到直角坐标系下，得到每个其他车辆i在直角坐标系(X,Y,Z)下的坐标：

其中，车载GPS、IMU设备采集了车辆所在的经度L、纬度B和高度H，N为车辆所在点的卯酉圈曲率半径，e为对应参考椭球的第一偏心率；i表示第i个其他车辆，参数下标i表示第i个其他车辆的该参数，下同；i＝1,2,…,M。

步骤5.2，将步骤5.1转换后的坐标转换到东北天坐标系下，得到每个其他车辆i在东北天坐标系(x,y,z)下的坐标：

其中(x_i,y_i,z_i)为第i个其他车辆在东北天坐标系下的坐标。

步骤5.3，将步骤5.2转换后的坐标转换到局部坐标系下，得到每个其他车辆i在局部坐标系(x',y',z')下的坐标：

其中，(x′_i,y′_i,z′_i)为第i个其他车辆在局部坐标系下的坐标，ψ为航向角，是待定位车辆纵轴在所行驶路面上的投影线与地理北向的夹角，取北偏东为正；θ为俯仰角，是待定位车辆纵轴与水平投影线之间的夹角，待定位车辆抬头时角度定义为正；γ为横滚角，是待定位车辆立轴与纵轴所在铅垂面之间的夹角，待定位车辆向右倾斜时角度定义为正。

步骤5.4，分别将每个其他车辆i在局部坐标系下的坐标联合毫米波雷达测量的数据进行解算，得到待定位车辆的M个理论位置：

其中，(a_i,b_i,c_i)表示第待定位车辆的i个理论位置，i＝1,2,…M，M为其他车辆的总数；ρ_i为待定位车辆相对于第i个其他车辆的相对距离向量，φ_r为待定位车辆的方位角；

步骤5.5，误差数据剔除，具体计算公式为：

根据设定的阈值σ₁,σ₂,σ₃，若或或则判定根据第i个其他车辆计算出的待定位车辆的第i个理论位置(a_i,b_i,c_i)误差过大，将其剔除；设剔除后剩余M′个理论位置；其中三个阈值根据实际需要的定位精度来设置。

步骤5.6，待定位车辆最终位置的确定

联合步骤4融合后得到的状态向量中待定位车辆的位置坐标(p_x,p_y,p_z)，得到待定位车辆的最终位置(a,b,c)：

其中，(a₁,b₁,c₁)…(a_M′,b_M′,c_M′)分别为经过步骤5.5剔除误差数据后，剩余的M′个理论位置。

本发明方法利用其他车辆共享的原始GPS数据来为自动驾驶汽车定位，使得车辆在一些无GPS信号或者GPS信号弱的地区也能进行相对准确的定位。在环境和路况较为恶劣致使获取GPS信号受影响的情况下，也能够使用其他车辆的GPS数据来准确定位。另外，多传感器和V2V通信的融合也使得自动驾驶车辆的定位方案比仅使用高质量传感器的方法更具成本效益。

Claims (4)

1.一种车联网环境下车辆高精度定位方法，其特征在于，包括以下步骤：

步骤1，在接入车联网的车辆中，确定待定位车辆，在待定位车辆上配备激光雷达、毫米波雷达以及车载单元OBU，在其余的M个车辆上均配备车载GPS、IMU设备以及车载单元；

步骤2，建立车辆的运动参考模型，所述的运动参考模型为恒定转率和速度模型；

步骤3，交通信息采集与预处理

通过DSRC技术将车联网中的各个车辆上的车载单元OBU相连接，以便于进行实时的数据交互与处理；在其他车辆上，通过车载GPS、IMU设备进行定位信息的采集和融合，得到融合后的定位信息；待定位车辆通过车载单元OBU采集其他车辆发来的位置信息，同时通过待定位车辆上安装的激光雷达和毫米波雷达采集待定位车辆与其他车辆的相对位置、距离和速度信息；

步骤4，传感器数据融合

将待定位车辆上的激光雷达和毫米波雷达采集的数据进行融合，得到待定位车辆的位置坐标；

步骤5，根据待定位车辆接收到的其他车辆的数据，联合待定位车辆测量的相对于其他车辆的相对距离向量和方位角，解算出待定位车辆的多个理论位置，然后联合步骤4传感器数据融合得到的待定位车辆位置坐标，计算出待定位车辆的最终位置。

2.如权利要求1所述的车联网环境下车辆高精度定位方法，其特征在于，步骤2中所述的运动参考模型中车辆的状态向量为：

其中，(x,y)为车辆在局部坐标系下的坐标，其中局部坐标系是以车辆的初始位置为坐标系的原点，车正前方为x轴，车的左侧为y轴建立的坐标系；v为待定位车辆行驶速度，θ为偏航角，ω是偏航角速度；

运动参考模型的状态转移函数为：

其中，t表示时间，Δt表示时间的变化量，x(t)、y(t)表示t时刻的坐标。

3.如权利要求1所述的车联网环境下车辆高精度定位方法，其特征在于，步骤3的具体过程包括：

步骤3.1，其他车辆上的车载单元OBU通过车载GPS、IMU设备来采集自身的定位信息并进行融合，得到融合后的定位信息，包括经度L、纬度B和高度H；

步骤3.2，待定位车辆通过OBU采集周围第i辆其他车辆发送来的GPS数据，并使用激光雷达检测该车辆的位置，得到该车辆在局部坐标系下的坐标位置(p_x,p_y,p_z)；利用毫米波雷达采集的各个其他车辆相对于待定位车辆的位置、距离和速度信息，包括：相对距离向量ρ，方位角φ—向量ρ与x轴方向的夹角，速度变化量速度信息，包括局部坐标系下的速度v_x和v_y；航向信息，包括偏航角θ，偏航角速度ω。

4.如权利要求1所述的车联网环境下车辆高精度定位方法，其特征在于，所述的步骤5具体包括：

步骤5.1，将每个其他车辆发来的各自车载GPS、IMU设备采集定位数据的融合结果转换到直角坐标系下，得到每个其他车辆i在直角坐标系下的坐标；

步骤5.2，将步骤5.1转换后的坐标转换到东北天坐标系下，得到每个其他车辆i在东北天坐标系下的坐标；

步骤5.3，将步骤5.2转换后的坐标转换到局部坐标系下，得到每个其他车辆i在局部坐标系下的坐标；

步骤5.4，分别将每个其他车辆i在局部坐标系下的坐标联合毫米波雷达测量的数据进行解算，得到待定位车辆的M个理论位置：

其中，(a_i,b_i,c_i)表示第待定位车辆的i个理论位置，i＝1,2,…M，M为其他车辆的总数；ρ_i为待定位车辆相对于第i个其他车辆的相对距离向量，φ_r为待定位车辆的方位角；

步骤5.5，误差数据剔除，具体计算公式为：

根据设定的阈值σ₁,σ₂,σ₃，若或或则判定根据第i个其他车辆计算出的待定位车辆的第i个理论位置(a_i,b_i,c_i)误差过大，将其剔除；设剔除后剩余M′个理论位置；

步骤5.6，待定位车辆最终位置的确定

联合步骤4融合后得到的待定位车辆的位置坐标(p_x,p_y,p_z)，得到待定位车辆的最终位置(a,b,c)：

其中，(a₁,b₁,c₁)…(a_M′,b_M′,c_M′)分别为经过步骤5.5剔除误差数据后，剩余的M′个理论位置。