臧利国，唐一鸣，沈业辉，王孜洲

（南京工程学院 a.汽车与轨道交通学院；b.自动化学院，南京 211167）

随着人工智能的高速发展和车辆技术的不断提高，智能车的研究与应用越来越成为生产生活的重要方面。作为现代社会的新产物，在智能车上开发出来的各种功能，如自动泊车、防碰撞预警、辅助驾驶系统等，可以保证行驶的安全性，进一步提高智能化水平。2019年9月，特斯拉推出了一项名为“Smart Summon（智能召车）”的新功能，通过该功能，用户只需在车停放的任意位置周围操控手机，车便可自动行驶到车主旁，无需人为干预。其结合了u-blox公司的NEO-M8L汽车惯性导航模块定位［1］和自身强大的视觉处理能力，但此功能有60.96 m的距离限制和1 m的定位误差。

实现智能召车，如何使车辆在非结构化环境中获取自身状态是关键。在高精度定位方面，牛群峰等［2］提出了一种基于TW_TOF的UWB信号定位系统，利用三边测量法实现定位，采用IMM卡尔曼滤波优化定位数据，试验显示该系统有效减小了定位数据的误差；王川阳等［3］提出基于双向测距的TOA定位方法，利用Chan算法和高斯－牛顿迭代算法解算定位结果，提高了系统的测距和定位精度；张浩等［4］提出一种基于单基站六角单极定向天线阵列的到达角（AOA）估计方法，解决了多基站系统的部署困难问题，可提供较精确的定位精度。

针对定位精度问题，结合智能召车应用场景，设计了一种基于超宽带技术的导航定位系统，利用加权三边测量法实现定位，通过改进无迹卡尔曼滤波算法减小噪声干扰，有效提高了定位精度，保证了召车的安全性。

1 UWB定位

定位一般有如下几种方法：GPS、超声波、WIFI、UWB、RFID等。其中GPS易受天气、地球自转、卫星运行、云层流动等因素的影响［5］，且在室内信号强度较弱，精度无法满足需求；超声波易产生多径效应和NLOS误差，且频率受多普勒效应和温度影响；WIFI是通过无线接入点组成的无线局域网络，但其定位误差较大，且易受干扰［6］；RFID可以在几毫秒内实现厘米级定位，但其作用距离近，且不具通信能力。不同室内定位方式的评价指标如表1所示。

表1 常见室内定位方式的评价指标

UWB是近年来新兴的一项无载波通信技术，其工作时间短、穿透能力强、定位精确［7］。利用纳秒级的非正弦波窄脉冲传输数据，以较高的时间分辨力、抗多径效应等特点，成为目前室内定位最佳物理层技术［8－17］。

1.1 基于UWB的智能召车定位模型

如图1所示，将UWB基站A0、A1、A2分别固定于车身上，以A1A2中点O为原点建立空间坐标系。其中M为车主手持标签，M′为M在基站平面的投影，定义OM′与Y轴夹角α为车辆姿态角。UWB测得标签M距基站A0、A1、A2的距离分别为d0、d1、d2，根据基站位置和d0、d1、d2，实现对标签位置的定位。

图1 UWB定位车主示意图

以标签M的位置和速度作为状态向量Xk＝［xkykvx，kvy，k］，其中xk、yk分别为标签的平面坐标，vx、vy分别表示标签在方向上的速度。由于应用场景中只需考虑标签在平面中的移动情况，故只考虑二维位置信息。

状态方程

式中：F为状态转移矩阵；ΔT为UWB采样间隔；ωk为k时刻系统噪声向量。

UWB量测方程

式中：Yk＝［r1，kr2，kr3，k］T为量测信息；h（Xk）＝［d1，kd2，kd3，k］T，dn，k为第n个基站与标签的实际距离；Vk为k时刻量测噪声。

为衡量车辆的位置与姿态，引入机器人正运动学概念。以基站A0、A1、A2为基准建立如图1空间坐标系，设A0（0，－b，0），A1（－a，0，0），A2（a，0，0），M（x，y，z），已知基站A0、A1、A2距目标M的距离分别为d0、d1、d2，列出如下方程，

解标签M坐标（x，y，z），对坐标轴XYZ进行变换，使得

1）O点到达目标点M′（x，y，0）

2）变换后的X轴垂直于线段OA变换矩阵

令

式中，矩阵P1、P2、…、Pn为召车过程中车辆经历的所有位姿变换；P为总变换矩阵，P的确定为智能召车的路径规划奠定了基础。

1.2 测距方式

目前室内定位大多使用飞行时间（TOF）测距原理，但其忽略了设备的响应时间（1 ns会导致0.3 m的测距误差），且现实中不可避免地存在因时钟振荡器不同引起的时钟漂移问题。为提高测距精度，本研究采用双向测距方法（TW_TOF）。TW_TOF工作示意图见图2。

图2 TW_TOF工作示意图

标签与基站分别生成2条独立的时间戳，TSP时刻标签向基站发送一条请求性质的信号，基站开始接收，并记下此时的时刻TRP。TSR时刻基站回复信息，标签开始接收，记录此时的时刻，并在TSF时刻将数据包P3（TSP，TRR，TSF）发送给基站，进行如下计算：

因此，TW_TOF测距法因时钟漂移带来的误差仅为毫米级。

图3为标签与基站的TW_TOF法测距试验数据，其真实距离为5 m，可看出误差均在0.1 m之内。

图3 标签与基站测距试验数据

1.3 改进Trilateration三边测量法

标准三边测量法根据标签与基站距离，以基站为圆心、距离为半径作圆，圆的交点即为标签位置。设3个基站的坐标分别为（x0，y0），（x1，y1），（x2，y2），标签距3个基站的距离分别为d0，d1，d2，交点坐标为（x，y），根据毕达哥拉斯定理［10］：

求解上述公式得交点坐标

但由于UWB基站功耗的不同，现实情况中测距不可避免地存在误差，三边法中3个圆不可能理想化地交于同一点，且多径效应的存在大大提高了定位的不确定性，为此对标准三边测量法进行了改进。

绝大多数情况下三边法会相交于如图4所示的一片区域，设3个圆交点分别为（xa，ya），（xb，yb），（xc，yc），选择3个点所围成的三角形的显然3个圆所围成的区域面积大小对标签的位置准确性具有较大影响，且所围区域面积越小，准确度越高［11］。

图4 改进的三边测量示意图

所以面积可以作为权值衡量定位点的可信程度。但由于所围区域面积的求解过于繁琐，若直接使用面积计算权值，则会占用大量的CPU资源，导致定位的滞后。

图5为三圆相交面积与3个交点所围成三角形面积关系的Matlab仿真结果。由图5可知，三圆相交面积和三角形面积呈正相关，即可用三角形面积与定位精确程度的关系近似3个圆相交面积与定位精确程度的关系。围成的三角形面积越小，所求取到的定位点更准确，其权值也更大。

图5 三圆相交面积和交点围成的三角形面积关系

设3个圆分别为O1，O2，O3，其方程为：三角形面积：

假设UWB每0.2 s获取n组有效数据，每组数据下所求取的交点围成的三角形面积为S，可以定义以下的权重函数，表示位置的可信程度

标准化处理，求得位置最优值：

这样，考虑多次估算未知点位置，利用各个位置的权值确定最终定位。不仅解决了标准三边测量法无法交于一点的问题，也一定程度上克服了多径效应、NLOS误差对定位的影响，增加了定位的可信度。

2 误差分析与修正

2.1 理论模型误差

2.1.1 标准时间偏差

标准时间偏差en包括UWB信号在标签与基站之间传播的固定时延误差、器件误差、时钟漂移误差，还有温度、气压等外界环境引起的误差，可表示为：

式中：en为标准时间偏差引起的测距误差；cn为器件误差、固定时延误差、时钟漂移误差和外界环境所引起的常值误差；φ（d）为不同UWB基站测距形成的误差，与距离d有关［12］；Dn为量测噪声。对UWB进行数据标定，通过大量测距数据进行线性拟合可确定标准时间偏差，从而减小误差。

2.1.2 信号传播误差

UWB传播误差主要来源于2个方面：一是多径效应，多径传播是UWB的典型信道环境，多径信号受信道干扰和噪声等影响，会引起直达单径分量DP被误检或延迟检测，带来多径误差；二是NLOS误差，NLOS环境中存在障碍物遮挡，造成NDDP（non-dominant direct path）和UDP（undetectable direct path）情形［13］，导致DP延迟或无法检测。当误差较大时，会导致量测信息偏离真实状态，甚至引起滤波发散。

2.2 实际测量误差

为分析测距误差特性，建立误差模型，随机选择距离进行了10 000次测距试验（已进行过数据标定，剔除标准时间偏差），其误差分布情况如图6所示。

图6 误差分布

此试验表明，UWB测距误差集中分布在－0.015～0.015 m，随着误差绝对值的增大概率明显减小，且其分布图形与高斯分布的钟形较为贴合，在滤波处理时可将误差模型近似为高斯分布。

2.3 UKF算法

在数字滤波方面，卡尔曼滤波作为经典的算法，其可以利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计［14］。但其只适用于高斯线性模型，为此有学者提出了扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）。

对于EKF，首先必须求出非线性函数的Jacobi矩阵，对于模型复杂的系统，计算复杂且容易出错；其次，必须满足小扰动假设，只适用于弱非线性系统，对于强非线性系统容易导致精度下降［15］。UKF使用确定性采样方式，通过一种非线性变换——UT变换来进行模型状态与协方差的更新，其估计均值与协方差均达到了Taylor级数的4阶精度，过程如下：

步骤1设置滤波初始值

步骤2计算k－1时刻的Sigma样本点，对Sigma点进行非线性变换

式中：n为状态维数（本文研究对象为UWB定位模型，考虑取n＝2）；κ为尺度参数，对其进行调整可以提高逼近的精度。用这组采样点χik－1为2n＋1个Sigma点构成的列向量，可以近似表示状态的高斯分布。

步骤3计算均值和方差

α、λ和β是计算过程中所需的3个参数，α通常取e－4≤α＜1；λ为比例因子，设置为0或3－n；β表示状态分布，对于高斯分布其最优值为2；κ＝α2（n＋λ）－n；适当调节α、λ可以提高估计均值的精度，调节β可以提高所求方差的精度。

步骤4计算Sigma样本点，预测k时刻的测量信息。

步骤5测量更新

计算更新后的增益矩阵，求出状态向量、协方差矩阵最优值

2.4 算法简化

标准UKF算法需要计算2次Sigma点，预测k时刻的状态值时需进行如下计算，

需要以xk和Pk作为输出值，计算5个Sigma点，从而预测状态和协方差值。但式（1）观测模型为线性方程，预测时使用UT变换不一定能提高精度，反而一定会降低计算效率［16］，因此将式（35）～（37）的预测方程改为KF的预测方程：

这样，不仅保证了模型的定位精度，也减少了算法的运算量，提高了近40%的计算效率，使其可以应用于UWB定位的数据处理。

3 试验与分析

为验证定位与算法的合理性，分别进行了静态试验和动态试验。试验在10 m×10 m的LOS环境中进行，采用Decawave公司生产的DWM1000射频模块。其典型带宽500 MHz，发射功率调整范围为－62～－35 dBm／MHz，选择110 kb／s的通信速率。

3.1 静态试验

试验模拟智能召车过程中的UWB定位，将车上的3个基站分别设置于（500，0，1 000），（0，－300，1 000），（－500，0，1 000），标签固定于（3 000，4 000，1 000）处，试验中标签与基站使用TW_TOF法测距，利用加权优化后的三边测量法初步定位标签位置，其位置再经改进的无迹卡尔曼滤波处理得到最终定位。试验共分为2组，图7为对标签的定位结果，其中改进组（红点）使用本文算法，未改进组（蓝点）使用标准三边测量法。

图7 静态定位

与理想点（3 000，4 000）对比，未改进组中x方向平均误差70.91 mm，最大误差172.08 mm，y方向平均误差89.47 mm，最大误差239.87 mm；改进组中x方向平均误差55.66 mm，最大误差102.25 mm，y方向平均误差68.17 mm，最大误差180.38 mm，其RMSE（均方根误差）与MaE（平均绝对误差）值如表2所示。

表2 试验结果

3.2 动态试验

为验证此导航系统对动态目标的定位精度，进行了动态试验。试验中，车主手持标签进行图8红线所示的S型移动，结果表明，动态定位中x方向最大偏离123.65 mm，y方向最大偏离150.27 mm，绝大多数点误差在100 mm之内，且轨迹波动不大，与理想轨迹基本一致。

图8 动态定位轨迹

4 结论

分析了TW_TOF的测距原理与精度，构建UWB定位模型，并对三边测量的结果进行加权优化，有效抑制了多径效应和NLOS误差对定位的影响，提高了定位可信度；根据误差分析，改进无迹卡尔曼滤波算法，在保证精度的同时提高了计算效率；最后，根据定位数据计算车辆位姿变换矩阵，为智能召车路径规划奠定了研究基础。试验结果表明：该导航定位系统能有效地对动、静态目标进行高精度定位，为智能召车定位提供了一种可行的解决方案。