编队行驶系统中基于Kalman 滤波的相对定位算法研究

尹立强,张鹤杨,霍俊臣,陈文浩,刘亚飞,朱文成

（河南科技学院信息工程学院,河南 新乡 453003）

“编队行驶”作为车联网的三大主要应用场景之一,也称为“列队行驶”,是指三辆及以上车辆,在同一路段排序行驶,后续车辆在头部车辆的引领下,无需驾驶员操控便可自动行驶[1-3].车辆精确定位作为车联网的核心技术之一,为车联网编队行驶应用提供了最基本的位置信息.车联网系统中车辆定位包含两个方面：领航车辆定位和随行车辆定位.领航车辆承担着整个车队的导航任务,其定位过程要求具有绝对位置坐标,可以采用卫星定位和基站定位联合工作方式,彼此取长补短,提高定位精度.行驶过程中,随行车辆跟随领航车辆,领航车辆需要获取随行车辆的位置下达相应的行驶指令,随行车辆之间也需要保持一定的安全行驶距离,可以采用相对定位技术实现上述功能.随行车辆之间采用相对定位技术,不仅可以消除绝对定位初始化时间长、成本高的缺点,还可以有效减小定位误差的累积,提高定位精度.本文着重分析探讨编队车辆之间的相对定位技术.

1 编队行驶原理

本系统采用leading-following 模式进行编队,每辆车在行驶过程中自动保持车间距离,并且带头车辆无论是加减速、转向还是刹车,跟随的车辆都会实时同步完成.编队行驶示意图如图1 所示.

图1 编队行驶示意图Fig.1 Formation driving diagram

在编队车辆行驶过程中,如果有其他车辆要求加入编队或者有车辆要脱离编队,首先向领航车辆发出申请,领航车辆同意后,以广播形式通知其它随行车辆,新加入车辆跟在编队队尾或者根据需要插入车队中间,其它车辆要实时调整速度以保持安全距离和编队队形.本系统以领航车辆作为中央处理器,领航车辆安装GPS 和车路协同感知系统,领航车辆将自身定位信息和路面状况及时以广播形式通知随行车辆.随行车辆需将自身的相对测距信息告知领航车辆.车与车之间的通信采用C-V2V 技术.系统原理框图如图2 所示.

图2 系统整体原理框图Fig.2 System overall principle block diagram

2 编队行驶车辆定位方法

2.1 领航车辆定位

领航车辆安装有GPS 和5G 通信模块,因为领航车辆需要进行行驶路径导航,需要获取领航车辆的绝对地理位置坐标,故此领航车辆可以采用GPS 和基站联合定位方式.

2.1.1 GPS 定位GPS 定位系统主要由卫星、控制部分、地面接收即用户部分组成[4].GPS 系统共有24 颗卫星, 每颗GPS 工作卫星都发出用于导航定位的信号,GPS 用户正是利用这些信号来进行工作的.GPS的控制部分由分布在全球的由若干个跟踪站所组成的监控系统所构成,根据其作用的不同,这些跟踪站又被分为主控站、监控站和注入站.GPS 的用户部分由GPS 接收机、数据处理软件及相应的用户设备如计算机气象仪器等所组成.它的作用是接收GPS 卫星所发出的信号,利用这些信号进行导航定位等工作.

终端设备中的GPS 导航软件能够实时提供游客所在位置的经度、纬度与高程,定位精度小于20 m,并以标准形式显示.GPS 系统能够连续实时隐蔽低定位,一次定位时间仅几秒到十几秒,用户无需发射任何电磁信号,只需接受卫星导航信号即可定位,可全天候昼夜作业,隐蔽性好.

GPS 定位方法有直接定位和差分定位两种,差分定位又分为位置差分、伪距差分、载波相位差分.目前多采用伪距差分算法进行定位.

2.1.2 基站定位 基站定位一般应用于移动终端用户,基站定位服务又叫做移动位置服务LBS（Location Based Service）,它是通过电信移动运营商的网络获取移动终端用户的位置信息（经、纬度坐标）.移动终端测量不同基站的下行导频信号,得到不同基站下行导频的TOA（Time of Arrival,到达时刻）或TDOA（Time Difference of Arrival,到达时间差）,根据该测量结果并结合基站的坐标,一般采用三角公式估计算法,就能够计算出移动终端的位置.实际的位置估计算法需要考虑多基站（3 个或3 个以上）定位的情况,因此算法要复杂很多.3GPP 的R16 版本中还提出了基于增强小区ID（E-CID）的定位方法[5],基站与终端互发参考信号测量Rx-Tx 时间差以得到RTT,确定终端所在的圆,结合DL-AoD（下行离开角）或UL-AoA（上行离开角）得到角度信息,它与圆的交点即为终端位置.这种定位方法的一个显著的优点是仅靠单站即可完成定位,不受基站之间同步精度的影响.一般而言,移动终端测量的基站数目越多,测量精度越高,定位性能改善越明显.但是精度很大程度依赖于基站的分布及覆盖范围的大小,有时误差会超过一公里.

随着5G 网络的广泛覆盖, 其定位精度也得到了很大提升.5G 相对于4G 在定位技术方面具备一些天然的优势,比如大带宽（时间分辨率高）以及采用Massive MIMO 大规模天线技术,具有更高分辨率的波束,可以实现更高精度的测距和测角,上述定位方法既可以单独使用,也可以结合使用,以此来满足不同场景的精度需求.

2.2 随行车辆定位

在车联网编队行驶系统中,随行车辆之间时刻需要保持合理的距离,车辆之间的距离定位尤为重要.目前,随行车辆定位主要通过GPS、北斗等全球卫星定位系统获得绝对位置信息,然后利用位置坐标计算车辆之间的距离.但是民用GPS 标准位置服务可提供10m 左右的定位精度[6],无法满足编队行驶的精确定位应用要求,除此以外,在密集建筑群、森林、隧道、立交桥下、地下停车场等区域时,卫星信号受到影响会造成较大的定位误差,甚至定位失败,所以基于卫星定位的绝对坐标定位方式就不适用.车辆定位和位置感知作为编队行驶的必需条件, 其突出特点是车联网编队行驶系统只需要知道车辆间的相对距离而不需要车辆的绝对位置信息.为了克服卫星定位存在的定位初始时间长、定位精度差、覆盖受限等缺点,赵海涛等采用了伪距双差的方式进行车辆相对距离的测量[7],定位精度得到了提升,但是每辆车需要安装较多传感器,成本较高.

本系统着重研究随行车辆定位方式,采用了一种不依赖于精确初始坐标的车联网相对定位技术,随行车辆安装雷达传感器,并采用Kalman 滤波算法对其测量过程进行处理,该方法不仅可以有效减小定位误差的累积效应,并且进一步提高了定位精度,完善了车辆防碰撞机制,提高编队车辆行驶的安全性.

3 编队随行车辆相对定位实现

3.1 雷达传感器

雷达即Radar（Radio Detection and Ranging）,意为“无线电探测和测距”,即用无线电方法发现目标并测定它们在空间的位置.典型的雷达系统主要由天线、收发转换开关、发射机、接收机、定时器、显示器、电源等部分组成[8].发射机在定时器控制下,产生高频大功率的脉冲串,通过收发开关到达定向天线,以电磁波形式向外辐射.在天线控制设备的控制下,天线波束按照指定方向在空间扫描,电磁波在传播过程中遇到目标时,目标受到激励而产生二次辐射,二次辐射中的一小部分电磁波返回雷达,为天线所收集,称为回波信号.接收机将回波信号放大和变换后,送到显示器上显示,从而探测到目标的存在.为了使雷达能够在各个方向的广阔空域内搜索、发现和跟踪目标,天线的定向波束以一定的方式在空间扫描.定时器用于控制雷达各个部分保持同步工作.收发转换开关可使同一副天线兼作发射和接收之用.电源供给雷达各部分需要的电能.雷达工作原理如图3 所示.

图3 雷达工作原理图Fig.3 Schematic diagram of radar operation

考虑到除了测距功能外,编队行驶车辆还需要进行车道识别、测速、测方位角度以及车身周边的道路情况,本系统采用毫米波雷达.毫米波雷达兼具有微波雷达和光电雷达的一些优点,并且具有体积小、易集成和空间分辨率高的特点.车载毫米波雷达的工作频率为一般为24 GHz 和77 GHz[9],可以同时对多个目标进行测距、测速以及方位测量.其测速是根据多普勒效应,而方位测量（包括水平角度和垂直角度）是通过天线的阵列方式来实现的.

3.2 TOA 测距方式

TOA 为基于电磁波空中传播时间换算距离的点对点测距方法,算法易实现,在成本和可靠性上优势显著,因而受到广泛关注.在TOA 点对点的距离测量方法中,典型的有单边双向测距（SS-TWR）、双边双向测距（DS-TWR）方法[10-11].SS-TWR 是由设备发送一条消息并接收应答,根据电磁波传播时间换算成距离的测量方法.SS-TWR 测距方法中,交互信息帧少,总响应时间短,错误帧数量少,传输过程稳定.刘鹏媛[12]运用SS-TWR 对高速弹丸测定距离,仿真结果显示测距误差在40~60 cm.TOA 单边双向测距原理如图4 所示.

图4 TOA 单边双向测距原理图Fig.4 TOA unilateral bidirectional ranging schematic diagram

根据发送设备发送“询问”帧与收到“应答”帧的时间差,即可计算出发送设备和应答设备之间的距离.设发送设备发送“询问”帧的时刻为ts,收到“应答”帧的时刻为tr,电磁波传播速度为c（3×108m/s）,则发送设备和接收设备之间的距离为：

3.3 随行车辆测距模型

本系统中编队车辆采用相对定位技术,每辆随行车辆均在车头与车尾安装测距模块,通过测量车头与相邻前车车尾之间的距离确定自身的相对位置,并将定位信息通过通信模块传输给领航车辆,原理图如图5 所示.领航车辆根据车辆编号与定位距离可计算出该车辆的绝对位置.这种相对定位技术算法简单便捷、延时短,定位测距模式采用TOA 方式,测距时间短,精确度高,可有效保证车辆之间的安全距离.

图5 相对定位测距示意图Fig.5 Schematic diagram of relative positioning and ranging

领航车辆在尾部安装雷达,随行车辆车头与车尾分别安装雷达系统.随行车辆进行编号,其向领行车辆发送信息时携带有车辆编号.领航车辆尾部安装雷达,编号为1 的随行车辆向领航车辆发送雷达信号,领航车辆收到信号后向编号为1 的随行车辆回送信号,编号为1 的随行车辆根据发送信号与收到信号的时间差计算出两者之间的距离, 采用这种双向传输测距方式可有效减小单向传输信号的不稳定性造成的误差,使测量结果更为可靠.

当编号i+1 随行车辆的车头的测距雷达向编号i 随行车辆的车尾的测距雷达发出电磁波信号时,编号i 随行车辆收到此信号即向编号i+1 随行车辆回送响应信号,编号i+1 随行车辆根据发射信号与接收到信号的时间差来计算其与编号i 随行车辆之间的距离,设光速为c,发射信号与接收信号的时间差为Δt,则两车之间的距离为di+1：

4 仿真分析

4.1 卡尔曼（Kalman）滤波算法

Kalman 滤波是一种时域滤波方法,采用状态空间方法描述系统[14-15],算法采用递推形式,数据存储量小,不仅可以处理平稳随机过程,还可以处理多维和非平稳随机过程,其作为一种非常重要的最优估计理论被广泛应用于导航、定位、目标跟踪、通信与信号处理等领域.

Kalnam 滤波算法中,首先建立两个状态空间模型描述的动态线性系统方程

式（1）为状态方程,式（2）为观测方程.其中：k 为离散时间,X（k）为系统在k 时刻的状态,Y（k）为对应状态的观测信号;Φ 为状态转移矩阵,Γ 为噪声驱动矩阵,H 为观测矩阵;W（k）为输入的白噪声,V（k）为观测噪声.

在式（3）对应的性能指标下,根据递推射影定理可得

K(k+1)为Kalman 滤波器增益.

ε(k+1)为新息序列,定义为

在假设1 和假设2 的条件下,对式（1）两边取射影,应用射影公式可得

同理对式（2）两边取射影,应用射影公式可得

新息序列表达式此时可写为

Kalman 滤波器和预报估值误差为

方差阵为

更新方差阵为

4.2 搭建仿真模型

Kalman 滤波中蕴含递推估计思想,通过系统输入与输出的观测数据,对系统状态进行最优估计[16],利用k 时刻推测k+1 时刻的估计值X（k+1）,并使估计值均方差最小.

车辆行驶过程中获取的车辆之间相对距离在任何时刻都是有噪声的,卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计.这个估计可以是对当前目标位置的估计,也可以是对于将来位置的估计,还可以是对过去位置的估计.卡尔曼滤波就是这样一个根据当前时刻目标的检测状态,预测估计目标下一时刻目标检测状态的一个动态迭代循环过程.本系统采用Kalman滤波对车辆相对距离d 进行预测,校准测距.假设测量过程中,真实值会受到加性噪声V（k）的干扰.再假定任意2 个不同时刻的噪声互不相关,根据Kalman 滤波理论,建立系统状态估计.根据推荐性国家标准《智能网联汽车自动驾驶功能测试方法及要求第三部分列队跟驰功能》公开验证试验标准,车速提升至60 km/h 的编队行驶要求跟车距离为15 m±20%.仿真过程中,各车辆行驶轨迹为匀速直线运动,设两车之间的预期距离为15 m,初始距离为14 m,初始测量距离为13.7 m.过程噪声方差Q=0.01,测量引入噪声方差R=0.04.Φ=1,Γ=1,H=1.则系统状态方程和观测方程分别为

4.3 仿真结果分析

仿真结果如图6 和图7 所示.由图6 可以看出,不管是真实值还是观测值,甚至是Kalman 滤波值,其结果都小于预期值,这就表明在系统测量过程中,两车之间的相对距离预设要尽可能的大,以保证两车之间的安全行驶.由图7 可以看出,Kalman 滤波与雷达直接测量的值相比,大大降低了偏差,虽然Kalman 滤波误差没有完全消失,但它使状态尽可能地逼近真实值.另外仿真计算结果表明,滤波前误差平均值为0.141 2,经过Kalman 滤波后,误差平均值为0.087 8,误差减小了37.82%.可见采用卡尔曼滤波后可有效减小误差,提高定位精度.

图6 相对距离仿真结果Fig.6 Relative distance simulation results

图7 相对距离误差分析Fig.7 Relative distance error analysis

5 小结

本文针对车联网编队行驶系统中的车辆定位进行了分析,搭建了一种基于相对定位的测距模型,引入了雷达的TOA 测距方式,在此基础上采用了Kalman 滤波算法进行数据降噪处理,从而对编队中相邻车辆之间的距离进行测算.仿真结果表明：

（1）基于相对定位方式能够快速测量相邻车辆之间的距离,降低了时延,并且避免了绝对坐标定位系统覆盖盲区的弊端.

（2）基于Kalman 滤波算法能够有效降低测量过程中误差的影响,提升了最终的目标距离定位结果,降低了定位误差.