基于UWB 的自动驾驶编队车辆相对位姿计算

陈守刚，赵波，廖明明

（201600 上海市 上海工程技术大学 机械与汽车工程学院）

0 引言

近年来，自动驾驶物流车编队行驶成为智能交通中的重要环节，智能网联车辆将成为国内汽车产业发展新的突破点[1]。自动驾驶物流车编队需要安装各种各样的传感器用以获取车辆之间与周围环境的信息，是编队行驶做出适当规划决策的基础[2]。随着科学与技术的飞速进步，车辆定位技术使用的传感器种类更加丰富多样，如激光雷达、GNSS系统、惯导系统等。激光雷达成本高，计算量大，在编队行驶车队中成本更高；GNSS 系统在不开阔的场景下容易失效；惯导系统的数据易产生漂移[3]。

针对上述问题，本文提出基于UWB 的相对位姿估计，计算量小、数据不易漂移，而且在使用场景没有限制，数据更新频率快。将UWB 技术应用于自动驾驶编队中，提出了基于UWB 的位姿计算方法，以及基于卡尔曼滤波的数据去噪方法。

1 UWB 技术特性分析

1.1 UWB 优势分析

超宽带（Ultra Wide Band，UWB）技术是无线载波通信技术的重要组成部分[4]。UWB 技术没有使用正弦载波作为传输载体，而是使用非正弦波窄脉冲信号进行数据传输，所以UWB 技术所能使用的频谱范围极其宽泛，使得UWB 系统结构的实现比较简单，并且可以实现高速数据传播。UWB 系统使用持续时间很短的脉冲信号进行数据发送，占空比很低，系统耗电很低，在能耗方面比传统无线通信设备有非常大的优势。由于UWB 信号在极宽的频带范围之内，对于传统的通讯设备而言，UWB 信号与白噪声信号相似，UWB 信号的功率谱密度低于自然的电子噪声的功率谱密度，因此UWB 技术安全性很高。传统的无线通信设备的射频信号大多为连续信号，或者延续时间大于多径传播的时间，多径传播效应明显，但是UWB 技术发射的是持续时间很短、占空比很小的单周期脉冲信号。UWB 多径信号在时间序列上可进行分离，因此多径分辨能力强。冲激脉冲信号具有很高的定位精度，UWB 技术很容易将定位需求与通信需求合二为一，但是传统无线电通信设备无法做到这一点。UWB 可以满足室内和地下精确定位的需求，而GPS 只能在定位卫星的可视范围之内使用。UWB技术可以进行相对定位，并且定位精度可以达到厘米级别。在实际工程中，UWB 技术比传统无线通信设备要简单得多，可通过全数字化实现[5]。

综上所述，UWB 相对于传统的定位技术，具有系统构造复杂度低、高速的数据传播、能耗低、对信道衰落鲁棒、发射信号功率谱密度低、被截获的概率极低、可以提供厘米级别定位精度、工程造价低等一系列优点。

1.2 UWB 定位原理

UWB 高精度定位技术主要分为同步式 UWB定位系统和异步式 UWB 定位系统[6]。

（1）同步式 UWB 定位系统

原理如图1 所示。基于 TDOA 算法，定位精度可以达到 10 cm CEP，刷新频率＞50 Hz。但成本很高，基站之间需光纤连接，施工比较复杂，算法复杂。需要定位服务器，定位信息需从服务器无线发送至车载终端，延迟较高。

图1 同步式 UWB 定位系统Fig.1 Synchronous UWB positioning system

（2）异步式 UWB 定位系统

原理如图2 所示。基于 TOF 算法，目前定位精度为 20 cm CEP，需要控制收发时序。成本较低，基站之间无需光纤连接。该算法简单，可由DSP 车载终端完成位置计算，然后直接发送到车辆CAN 总线。

本文采用异步式UWB 定位技术，在车上安装十分方便。异步式 UWB 定位系统三边测量原理，图2 中，分别以A、B、C 三点为圆心画圆，并建立对应的坐标系，A、B、C 的坐标分别是（Xa，Ya）、（Xb，Yb）、（Xc，Yc），D为标签，以A、B、C为圆心的圆相交于标签D。

图2 异步式 UWB 定位系统Fig.2 Asynchronous UWB positioning system

圆A、B、C 的半径分别是da、db、dc，假设D的坐标为（X，Y），根据最小二乘法可以列出公式：

可以得到标签D 的坐标为

所以，只需要知道基站A、B、C 的坐标及到标签的距离，就可以得到D 的坐标，从而定位到D 的位置。该方法适用于室内的定位，在基站位置确定的情况下，使用该方法可以快速进行标签的决定位置定位。

1.3 基于TOF 算法的UWB 技术

在UWB 定位中，使用信号时间戳计算基站从发射信号到接受信号所用时间，即飞行时间TOF（Time of Flight）法，根据位移时间公式计算得到信号经过的位移距离，这个距离是测量距离的2 倍（信号发送与接收）。位移时间公式为：

评价TOF 测距方法的指标就是精度。单边双向测距方法是常用方法之一，但是由于该方法精度不高，误差与所测量的距离成指数关系，特别在本文的动态测量定位案例中，会导致误差更大，不符合实际情况。为了减小误差，本文使用双边双向测距[7]。使用基站与标签正反两个方向的测量方法，可以有效减少时钟偏移所导致的测距误差。基站首先标签发送测量信息，标签接收测量信息之后进行回复，基站接受信号后向标签回复信息。这样，通过求取飞行时间的平均值，可以减少基站与标签两者之间的时钟偏移，从而改善测量精度。原理如图3 所示。

图3 双边双向测距原理Fig.3 Principle of bilateral two-way ranging

2 车与车之间的相对位姿计算

2.1 车车相对位姿数据获取

在自动驾驶物流车辆直行行驶编队中，因为路径规划和底盘线控的需求，需要获取跟随车相对于牵引车的距离、方位角与姿态角，使得编队驾驶更稳定协调。图4（a）所示为实际车辆模型，图4（b）所示为数据测量模型。图4（b）中：两车之间的距离L 为牵引车尾部的中心点与跟随车头部的中心点之间的距离；方位角θ为跟随车与L 所在方向之间的夹角；姿态角β为跟随车轴线与牵引车轴线的夹角。图4 中定义的进行方向，顺时针为正，逆时针为负，因此θ为正方向，β为负方向。

图4 车辆实际模型与测量原理Fig.4 The actual vehicle model and measurement principle

因为本文提出的方法所用UWB 设备安装在动态运动的车辆上，所以基站位置不固定。本文方法不需要绝对的角度信息，只需要相对的距离信息。标签与基站之间的距离分别为OA、OB、OD、DA、DB，标签O 与基站D 和基站A 与基站B 的距离与车身宽度相等，即AB=OD 。OA、OB、DA、DB、OD 的距离可以通过UWB 测量得到。

需要建立相应的坐标系确定基站与标签之间的相对关系。以标签O 为坐标原点，OA 方向为x轴，经过原点且垂直于x 的直线为y 轴。则各坐标点为O（0，0），A（XA，YA），B（XB，YB），D（XD，YD），并且可以得到∠AOD 与∠BOD 的角度值，设∠AOD=α，∠BOD=λ。

则A 点的坐标为（OA·sinα，OA·cosα），B 点的坐标为（OB·sinλ，OB·cosλ），D 点的坐标为（OD，0）。将数据代入可得A 点为（c·tOA·sinα，c·tOA·cosα），B 点为（c·tOB·sinλ，c·tOB·cosλ），D 点为（c·tOD，0）。因此可以计算得L、θ、β：

根据式（5）—式（7），可以得到相关的数据。但是实际测距时，由于环境复杂多变，加之车辆自身抖动，测得的数据中有噪声，即得到的L、θ、β数据不平滑，无法应用于高速运动的实际场景中。为了提高测试数据的稳定性和精度，需要对原始数据进行平滑处理，因此引入卡尔曼滤波。卡尔曼滤波能够从一系列包含噪声的测量中估计动态系统的状态。

2.2 基于卡尔曼滤波的数据平滑

本文提出的方法中，卡尔曼滤波系统[8]的输入状态量为x=（L，θ，β）T，即通过UWB 测量得到的原始数据值。首先建立数据的计算模型：

式中：k，k-1——当前状态与前一状态；A∈Rn×n——系统的转移矩阵；u∈Rl——可控制的输入；B∈Rn×l——控制输入的转移矩阵；w——过程噪声。由于本文提出的方法不需要使用u控制输入，因此可以得到更为简洁的表达形式：

卡尔曼滤波系统的数据观测模型为

式中：x∈Rn——要估计的状态；z∈Rm——实际测量值；H∈Rm×n——当前状态实际测量值的转移矩阵；v——观测噪声。

卡尔曼滤波的增益矩阵为

式中：Pk|k-1——预测协方差矩阵；Pk——观测被观测的噪声；H∈Rm×n——当前状态实际测量值的转移矩阵。当Pk|k-1增大，卡尔曼滤波的增益Kk相应增大，噪声Rk增大，卡尔曼滤波的增益Kk相应减小。

3 实验结果对比

3.1 实验内容

3.1.1 实验设备

为了检验本文所提出方法的有效性与合理性，对该理论进行实验验证。实验设备为2 辆自动挡解放车，使用该车型的目的就是为了检测UWB 在大型车辆上的可行性与稳定性。实验设备如图5 所示。

图5 实验设备Fig.5 Experimental equipment

3.1.2 测试方案

2 辆解放车进行直行跟随行驶，匀速跟随，速度为30 km/h，跟随距离2～ 60 m。然后跟随车静止，前车沿直线向前行驶至60 m 处，目的是为了模拟车间距从小距离变化到大距离的情况。

3.2 实验结果

直线跟随距离精度测试以及经卡尔曼滤波平滑之后的对比如图6 所示。图6 中直线表示跟车距离从2～60 m 的真实值，图6（a）中折线表示跟车距离的测量值，可见波动明显；图6（b）中折线表示经过卡尔曼滤波之后的跟车距离。

图6 直线距离测试Fig.6 Straight-line distance test

直线跟随方位角精度测试以及经过卡尔曼滤波平滑之后的对比如图7 所示。

图7 方位角测试Fig.7 Azimuth angle test

图7 中的直线表示跟车距离2～60 m 过程中的方位角，图7（a）中折线表示根据方位角的测量值，可见波动明显，波峰可达15°；图7（b）中折线表示经过卡尔曼滤波之后的跟车方位角，可以看出波动范围明显减小，滤波后的波峰小于6°。

直线跟随姿态角精度的测试以及经过卡尔曼滤波平滑之后的对比如图8 所示。图8 中的直线表示跟车距离从2～60 m 过程中的姿态角，图8（a）中折线表示跟车姿态角的测量值，可见波动明显，波峰可达10°；图8（b）中折线表示经卡尔曼滤波后的跟车姿态角，波动明显减小，波峰小于5°。

图8 姿态角测试Fig.8 Attitude angle test

表1 是UWB 测量数据滤波前后的误差统计。从表1 可以看出，经过滤波之后，直线距离的平均误差从16.3%降到5.2%；方位角的平均误差从19.7%降为9.4%；姿态角的平均误差从17.4%降为8.9%。说明卡尔曼滤波很有效。距离的精度最高，说明UWB 完全适用。但是角度误差较大，可能是因为基站A 和基站B 与标签O 和基站D 之间的距离与车辆等宽，在车距变远时，对角度的获取有限制。但是经过平滑之后的数据是满足车辆编队行驶需求的，说明UWB 技术应用在自动驾驶编队中有效且便利。

表1 UWB 数据误差前后对比Tab.1 Comparison of UWB data error before and after filtering

4 结语

本文利用UWB 定位精度高的特性，将UWB应用到自动驾驶车辆中。利用UWB 计算自动驾驶编队直行时车辆间的相对位姿，包括距离、方位角与姿态角，然后使用卡尔曼滤波对原始测量得到的数据进行平滑滤波，结果提高了距离、方位角、姿态角在直行时的精度，验证了低成本传感器UWB应用在自动驾驶行业的可能性是极大的，为相对位姿的获取提供了新的思路和方法。下一步可以将惯导器件与UWB 进行多传感器融合，改进编队车辆在左转和右转时的精度。