基于GRI的多车协同定位研究∗

孙 宁，闫梦如，倪 捷，葛如海，秦洪懋，唐翊铭，高行健

(1.江苏大学汽车与交通工程学院，镇江 212013； 2.北京航空航天大学，车路协同与安全控制重点实验室，北京 100191)

前言

随着汽车自动驾驶技术的发展，车辆对环境感知的要求也越来越高。获取实时、准确的自身和周围车辆位置信息(如经度、纬度、高度等)是环境感知内容的重要组成部分，也相应面临着更加严格的精度要求。如自动紧急制动系统，对前方车辆的位置误差要求就在1m以内。因此，如何提高车辆位置信息的定位精度[1]，是目前国内外汽车领域学者面临的一项新课题。高精度车辆定位方法的研究，对发展汽车自动驾驶技术具有十分重要的意义。

鉴于经济成本因素，目前单一应用传统GPS或毫米波雷达，都无法满足上述车辆位置定位的高精度要求。而通信技术的发展则为高精度车辆定位提供了一种新的途径。在车车通信、车路通信的支持下，车辆间可相互交换丰富的信息，同时，车辆自身获取周围环境信息的能力大大提升，从而可以计算获得更加准确的位置信息[2-3]。本文中在传统GPS和毫米波雷达获取初步车辆定位信息的基础上，参照文献[4]中的思路，提出一种基于GPS/雷达集成(GPS/radar integration，GRI)的多车协同定位方法，为获取高精度的车辆位置信息提供一种新思路。

1 多车协同定位的总体框架设计

本文中提出的多车协同定位系统由GPS、毫米波雷达和车车通信3大模块组成。其中GPS数据已经通过计算转换为局部区域的平面坐标。车车通信模块为LTE-V/DSRC。

系统中在基于GRI的多车协同定位过程中，对定位的估计分为两阶段。第一阶段，进行基于本车位置信息的粗估计，即通过GPS获取含噪声的车辆的绝对位置信息，经卡尔曼滤波器处理，再通过雷达得到周围车辆位置的粗估计；第二阶段，将粗估计的结果通过车车通信进行分享，进行综合多车位置信息的融合估计，得出更加准确的本车和周围车辆的绝对位置。协同定位的总体工作流程如图1所示。

2 基于GPS与雷达的车辆位置信息估计

GPS模块获得的是带有噪声的车辆绝对位置信息，通过设计滤波器对该噪声信号进行处理，可实现基于本车GPS观测位置信息粗估计。本文中采用扩展卡尔曼滤波[5-6]对GPS信号进行消噪处理。

2.1 基于GPS本车观测定位信息估计

对于车辆而言，当侧偏角为零时，将t时刻的线速度用vt表示，角速度用ωt表示，则控制向量设正的ωt产生一个逆时针旋转，正的vt产生一个向前移动。速度运动模型的状态向量为

式中：xt′，yt为直角坐标系下的位置坐标；θt为车辆航向角。

在无噪声影响的理想情况下，设初始状态向量为(x，y，θ)T，控制向量为(v，ω)T，在 Δt时间后的状态向量为(x′，y′，θ′)T，则有

式中为该 Δt时间内运动的圆心。

经简化，在 Δt时间后的状态向量(x′，y′，θ′)T可以表示为

实际上，获取的车辆运动信息含有噪声，假设该噪声来自控制量。首先，设真实运动的线速度和角速度为和则有

式中：εv和εω分别为真实控制量与理想控制量的误差。

仅在εv和εω的作用下，车辆的真实运动和理想运动之间的误差有两个自由度，只能描述圆周运动，这与车辆真实运动的情况并不相符。因此须引入一个误差参数，以使理想运动与运动误差叠加后的真实状态空间与建立的模型中所有可能的状态组合的状态空间等价。为此，引入虚拟控制量γ＝0，且有

此时，新的控制向量为(v，ω，γ)T，其中γ≡0，εγ为虚拟控制量误差，这样的引入只是为使得对运动误差的建模变得完整，使：

式中εθ表示车辆航向角的噪声。

此外，假设控制向量中各误差均服从正态分布：

因此，车辆的真实运动模型表示为

因为车辆运动模型为非线性模型，故应用扩展卡尔曼滤波对车辆状态进行估计。

基于扩展卡尔曼滤波的位置估计具体步骤如下[7]。

将t时刻真实状态向量记为V t＝(xt，yt，θt)T，滤波器对状态的预测(无观测信息)记为获取观测信息后修正的状态估计记为

控制向量记为其中γ≡0，观测向量记为预测状态协方差矩阵记为，获得观测信息后估计协方差矩阵记为Σt，记状态预测误差为，记状态估计误差为有

观测向量由GPS采集的经纬度与航向角获得，控制向量中的速度由GPS采集，横摆角速度根据车辆转向盘转角和速度转换得到。

通过状态转移方程预测t时刻状态：

对状态转移方程求μt-1的1阶偏导的雅可比矩阵：

状态转移方程求u t的1阶偏导的雅可比矩阵：

t时刻预测状态协方差矩阵更新为

其中Q为控制噪声协方差矩阵：

记t时刻控制误差为有

状态观测方程：

对状态观测方程求的雅可比矩阵：

计算卡尔曼增益：

其中R为观测噪声协方差矩阵：

记t时刻GPS观测误差为

根据观测信息修正对状态的估计：

更新t时刻估计状态协方差矩阵为

式中I为与μt同维数的单位矩阵。μt和Σt即为t时刻车辆状态估计的输出结果。

2.2 基于雷达的目标绝对位置计算

采用毫米波雷达对周围车辆进行观测。毫米波雷达的特点为对目标的纵向(y向)距离测量精度较高，而横向(x向)距离测量精度较低[8-9]。记t时刻雷达对第j个目标相对位置的测量误差假设

将GPS本车粗估计绝对位置与雷达探测目标相对位置进行叠加，得到雷达第j个目标的绝对位置：

由于自身粗估计误差与雷达探测相对位置误差均服从多维正态分布，由正态分布性质可得的协方差矩阵为

上述和即为基于雷达观测的目标绝对位置信息。

3 多车定位信息融合

3.1 基于车车通信的本车定位估计

如上所述，对GPS定位信息进行滤波后的本车位置估计结果，通过车车通信发送给周围车辆。同时，车辆通过毫米波雷达可探知周围车辆相对于本车的位置。由于周围车辆状态中的航向角θ项在相对测距中不起作用，故记t时刻周围车辆i通信发送的本车位置估计为状态估计协方差矩阵为

记t时刻本车雷达探测到车辆i的相对位置为换算到直角坐标系为其中为i车与本车的距离为i车与本车连线和水平x轴的夹角如图 2所示。

图2 雷达探测参数示意

则根据车辆i与本车相对定位得到的本车位置估计为

假设t时刻雷达对相对位置的测量误差向量为在x向和y向上均服从正态分布且互不相关，即为雷达相对测距的协方差矩阵，则根据车辆i与本车相对定位得到的本车位置估计的协方差矩阵为

?和即为本车与车辆i相对定位得到的本车位置估计的输出结果。

3.2 本车定位信息最优融合估计

如果本车在时刻t有附近的Nt辆车的绝对位置信息及其与本车的相对位置信息，则可通过上节的方法得到Nt个通过相对定位得到的本车位置估计，加上本车自车通过扩展卡尔曼滤波得到的位置估计，共有Nt+1个对本车的位置估计结果。文中基于数据融合理论[1，10]对这Nt+1个估计结果进行综合。

记t时刻车辆i的绝对位置观测误差为控制误差为本车对车辆i的雷达观测误差为本文假设：

则有

式中：δpq为克罗内克函数另外，假设不同车辆的初始状态估计误差不相关：

记本车与车辆i相对定位得到的本车位置估计误差为基于上述条件，不难证明：

基于上述推论，基于局部估计误差不相关假设下的最优融合估计对所有本车位置估计进行融合：

式中：Nt为t时刻本车附近获知绝对位置与雷达相对位置的车辆数。

融合后的估计协方差矩阵为

μ′t和Σ′t即为多车信息融合得到的本车位置估计。

4 仿真试验与测试

4.1 仿真参数设置

为验证基于GRI的多车协同定位算法的有效性，通过MATLAB中建立了多车运动场景，并进行对上述各部分算法的仿真。仿真时间为30s，时间步长0.1s。

本文中多车运动仿真场景分为直线运动场景与换道运动场景两种。

(1)直线运动场景

该场景中有2个车道，车道宽3.75m，每车道5辆车，均沿各自车道的中心线直线行驶。左侧车道为快车道，5辆车初始车速均为 50km/h，车间距80m。右侧车道为慢车道，5辆车初始车速均为30km/h，车距50m，如图3所示。仿真开始后t在0～10s时间段内，两车道的头车均按照如图4所示的加速度曲线运动。t＞10s后，两头车保持匀速直线运动。其他8辆车根据线性跟驰模型对前车的速度波动产生响应：

式中：xr(t)和xf(t)为t时刻后车和前车沿车道方向到原点的距离，参数取λ＝0.3，T＝1s。

图3 直线运动场景示意图

(2)换道运动场景

该场景中有2个车道，车道宽3.75m，每车道各有5辆车，初始时均沿各自车道的中心线直线行驶。两车道上车辆的初始车速均为30km/h，4号与5号车之间、6号与7号车之间前后车间距100m，其他车辆前后车间距50m，如图5所示。

开始仿真后t在3～8s时间段内，2号车和9号

图4 头车加速度曲线(部分)

图5 换道运动场景示意图

车进行换道操作，其他车辆保持匀速直线运动。换道操作的转向角模型如图6所示。又根据本文中车辆运动仿真模型，得到的换道位置曲线如图7所示。

图6 车辆换道转向角模型

图7 换道运动横向位置曲线

本文中的仿真试验对雷达、GPS与车车通信等各模块的功能参数设置如下。

雷达：可探测到以本车位置为圆心，半径100m范围内的周围车辆与本车的相对位置，要求周围车辆与本车之间无遮挡。一般的毫米波雷达对纵向位置探测精度比横向位置探测精度高，误差均满足正态分布，标准差σrx＝0.2m，σry＝0.1m。 每隔0.1s更新一次。

GPS：可提供本车的绝对位置和速度信息，横向、纵向和航向角误差均满足正态分布，标准差σzx＝σzy＝3.33m，σzθ＝π/180，σzv＝1m/s。 每隔0.1s更新一次。

车车通信：可接收到以本车位置为圆心，半径100m范围内的周围车辆发送的自身位置信息。为测试在通信不稳定的情况下基于GRI的多车协同定位算法的表现，设在t＝20～25s时间段内车车通信失效，所有车辆无法获得周围车辆的绝对位置信息。在其他时间内，每隔0.1s更新一次车辆发送和接收的通信信息。

另外，假设横摆角速度测量误差满足正态分布，标准差σzω＝π/180s-1。每隔0.1s更新一次。

4.2 仿真结果分析

在两个场景下，对基于GRI的多车协同定位算法进行仿真，结果如图8、图9和表1所示。其中图8和图9分别为直行场景和换道场景下协同定位算法x向和y向的均方根误差随时间的变化曲线，而表1则为按通信正常和通信失效两个阶段分别计算的两种场景平均的x向、y向和总的均方根误差。皆为除头车(5#车和10#车)外其余8辆车的平均结果。

图8 直线场景协同定位算法的均方根误差的时间历程

由表1可见，协同定位的误差水平总体保持在0.6m以下。这说明本文中的方法科学有效，能高精度地对车辆进行定位。从表中还可看出，在通信失效情况下定位误差较大，这体现了车车通信在基于GRI的多车协同定位中的重要性。

图9 换道场景协同定位算法的均方根误差的时间历程

表1 本车定位误差统计

5 结论

本文中提出了基于GRI的多车协同定位的方法，并通过MATLAB软件进行了模拟仿真，通过大量重复试验取均方根值分析各种定位算法的误差，发现最优融合的协同定位算法在通信理想条件下误差极小，即便短时间通信失效，平均误差水平也控制在0.6m以内，这表明该方法合理有效，能高精度地对周围行驶车辆进行定位。但是由于本文中速度运动模型有一定的理想假设，实际的车辆转向过程中侧偏角广泛存在，与车辆真实运动的描述有一定偏差，对该因素的忽略可能导致本车位置的滤波跟踪效果减弱。

参考文献

[1]QURESHIK N，ABDULLAH A H.Localization-based system challenges in vehicular Ad Hoc networks：Survey[J].Smart Computing Review，2014，4(6)：515-528.

[2]ALAM N，DEMPSTER A.Cooperative positioning for vehicular networks：Facts and future[J].IEEE Transactions on Intelligent Transportation Systems，2013，14(4).

[3]EFATMANESHNIK M，ALAM N，KEALY A，et al.A fast multidimensional scaling filter for vehicular cooperative positioning[J].Journal of Navigation，2012，65(2)：223-243.

[4]SHERER T P，MARTIN SM，et al.Radar probabilistic data association filter with GPSaiding for target selection and relative position determination[C].Proceedings of the ION 2017 Pacific PNT Meeting， Honolulu， Hawaii，2017：419-428.

[5]LIU K，LIM H B.Positioning accuracy improvement via distributed location estimate in cooperative vehicular networks[C].Conference Record-IEEE Conference on Intelligent Transportation Systems，2012：1549-1554.

[6]GOLESTAN K， SATTAR F， KARRAY F， et al.Localization in vehicular ad hoc networks using data fusion and V2V communication[J].Computer Communications，2015，71：61-72.

[7]刘江，蔡伯根，王云鹏.基于GNSS/DSRC融合的协同车辆定位方法[J].交通运输工程学报，2014(4)：116-126.

[8]AKCAN H，EVRENDILEK C.GPS-free directional localization via dual wireless radios[J].Computer Communications，2012，35(9)：1151-1163.

[9]YAN T，ZHANG W，WANG G.A grid-based on-road localization system in VANET with linear error propagation[J].IEEE Transactions on Wireless Communications，2014，13(2)：861-875.

[10]BHAWIYUGA A，NGUYEN H H，JEONG H Y.An accurate vehicle positioning algorithm based on vehicle-to-vehicle(V2V)communications[C].IEEE International Conference on Vehicular Electronics and Safety，IEEE，2012：273-278.