许秀峰， 蒲家坤， 周爱国， 杨思静， 魏榕慧

(同济大学 机械与能源工程学院，上海 201804)

智能驾驶汽车需要高精度的稳定定位信息，以满足各场景的应用需求，如碰撞预警、并线等[1-2]。除了常见的室外定位场景外，自动驾驶要求在室内场景(如地下车库、隧道等)也能得到精确的车辆位置，从而实现全方位的高精度定位。无线定位利用无线通信和测量参数(幅度、传输时间、相位角和到达角等)确定车辆位置，主要用于室内定位。常见的无线定位技术包括超声波定位、蓝牙定位、射频识别定位、WiFi定位、ZigBee定位和超宽带(Ultra Wideband，UWB)定位等。

UWB是一种以极低的功率在短距离内进行高速传输的无线技术，具有速率高、功耗低、测距精度高等特点[3]。发送机在发射无线信号时，将分散信号加载到宽频带中，然后接收机接收并还原这些信号，同时产生扩频增益。UWB信号自身抗干扰性强且不会干扰其他设备上的无线信号。

UWB定位技术在国外的发展较早，且已有不少学者对其进行了深入的研究。Maalek等[4]研究了UWB定位技术在施工现场次优条件下的定位性能，进行了一系列的实验，通过控制变量来模拟每种场景对UWB定位性能的影响。Zhou等[5]提出了一种高精度的远距离UWB定位算法，以Cramer-Rao下界为设计工具解决了测距信息融合的问题，并研究了到达时间差(Time Difference of Arrival,TDOA)定位系统中基于相关测距信息的锚点最佳几何分布算法。我国从1999年开始研究UWB技术的应用，2003年国家“863”计划开启了东南大学、清华大学、中国科技大学等国内高校的自主研发高潮[6]。王志铠[7]针对传统导航方式在无人搬运车(Automated Guided Vehicle，AGV)机器人上的不足，提出了一种基于UWB的室内AGV定位算法。

UWB定位系统工作时，首先建立UWB模块间的通信机制，然后通过基站和移动站之间的通信信息获得定位所需的必要参数，建立相应的系统估算模型，最后解算出移动站的位置。UWB的定位方式有4种：基于接收信号强度、基于达到角度、基于到达时间和基于到达时间差(TDOA)。其中基于TDOA的定位方式只需保证基站之间的时钟严格同步，且单程测距即可获得所需信息，降低了基站的硬件成本和定位系统的通信容量，是一种优良的定位方式。

在实际的定位过程中，多径效应和非视距环境会带来很大的误差。多径效应是指在复杂的室内环境下，UWB信号在传播过程中会发生反射、折射、衍射或散射等现象，信号会通过多条路经到达UWB基站。非视距(Non Line of Slight，NLOS)是指当UWB基站和移动站之间存在障碍物时，UWB信号传播过程中就不存在直达路径。一方面，UWB会通过反射、折射或衍射等方式到达UWB基站，造成多径效应；另一方面，UWB信号在穿透障碍物时，传播速度小于在空气中的传播速度，造成传播时间上的正偏差，从而影响测距精度。

为了满足智能车在室内的高精度定位要求，解决多径效应和非视距产生的测量误差对定位精度的影响，针对室内的伪三维定位场景，提出了一种基于UWB的LSM-Taylor级联车辆定位算法。该算法以TDOA为定位方式，以多基站LSM定位算法的计算结果为初始值，通过Taylor级数迭代估计车辆的精确位置，为车辆提供精确的定位信息。

1 定位算法方案与框架

1.1 UWB信号分析

在无干扰的静止环境下，实验采集TDOA信息，数据结果如图1所示。由图1可知，TDOA的测量值在真值±20 cm的范围内波动，其误差服从高斯分布，均值为0，标准差为6.5 cm。

图1 TDOA误差分布示意图

1.2 定位算法方案与框架设计

传统的TDOA定位算法有Fang算法[8]、Chan算法[9]等。Fang算法通过线性化双曲线方程来求解位置，但其只能应用于二维平面的定位，且只能利用3个基站的TDOA信息，不能综合多基站的TDOA信息，因此精确度较低。Chan算法采用两次迭代的求解方法，使用多基站求解三维空间下移动站坐标，但NLOS等因素造成的较大测距误差对Chan算法的定位精度影响很大。

在实际的智能车室内定位场景下，安装在车上的UWB移动站高度变化很小，可将其近似为常值，并且高度方向上的位移对车辆导航的意义不大。因此，可将移动站的运动简化为高度未知的水平二维运动，定位系统通常只输出车辆的水平二维坐标。考虑到定位解算的准确性和简便性，通常将UWB基站布置在同一高度上。

在三维立体空间内，每两个基站的TDOA信息可以构建出一个非线性方程，本文提出的UWB定位算法为忽略高度的伪三维定位，但由于基站和移动站之间存在高度差，因此仍需要3组TDOA信息才能求解位置，即需要4个基站。在实际情况中，UWB测距信息存在一定的误差和噪声，还需要更多的基站，利用冗余信息对UWB的定位结果进行优化。笔者提出的基于TDOA的LSM-Taylor级联定位算法，首先利用最小二乘法(Least Square Method,LSM)得到非理想状态下的移动站位置初始值，再通过Taylor展开递归获取精确的位置估计，直至收敛。算法流程图如图2所示。

图2 级联定位算法流程图

2 多基站LSM定位算法

理想情况下，伪三维定位系统需要4个基站才能获得移动站的位置坐标，但测量过程中存在NLOS、多径效应或温漂等引起的误差，因此四基站定位系统并不能准确求解出移动站的位置。TDOA定位方式为变量耦合程度较高的非线性系统，本文采用多基站LSM定位算法，将非线性方程求解问题转换为非线性系统的最优估计问题[10]，其中基站数量≥4。

为了简化定位系统模型，假设定位系统中包含一个高度为h的移动站(x,y,h)和N个高度为0，三维坐标已知的基站(x1,y1,0;x2,y2,0;…;xN,yN,0),第i个基站(xi,yi,0)到移动站的距离di可以由UWB信号飞行时间τi计算得到：

(1)

式中，c为UWB信号传播速度，近似于光速，可取c=299792485 m/s。

Lij表示移动站到第i个和第j个基站之间的TDOA值，即

Lij=di-dj=c×(τi-τj)

(2)

假定UWB定位系统的原始输入数据为{L21,L32,…,LN(N-1),L1N}，可由UWB模块测得。序号相连的3个基站组成一个位置解算子单元，设基站序号分别为i，j和k(i

(3)

将式(3)线性化可得

(4)

aijkx+bijky=dijk

(5)

式中，

(6)

由式(5)可知，每个位置解算子单元可以得到两组非线性方程，由此可以得到一个包含移动站的直线约束。若想求出移动站的位置，需要更多的位置解算子单元来提供直线约束，最少直线约束数为2。

将所有的定位解算子单元和TDOA测量值代入式(5)，可得

Ap=d

(7)

式中，

(8)

当N≥4时，通过最小二乘法得到移动站的估计位置：

(9)

式(5)并不包含高度项h，说明基站LSM定位算法可以在高度未知的情况下求解移动站的水平位置坐标。与传统的Fang算法相比较，多基站LSM定位算法结合多个基站的TDOA信息联合估计移动站的位置初始值，降低了个别误差较大的TDOA测量值对定位结果的影响，提高了定位系统的稳定性和准确性。

3 Taylor级数定位算法

多基站LSM定位算法计算量小且精度较高，但得到的定位结果并非最优解，还需通过Taylor级数定位算法得到精确解。Taylor级数定位算法是一种迭代的求解方法[11]。每次迭代中，对误差项在估计值附近进行Taylor展开，并忽略高阶项，通过局部的最小二乘法来更新移动站的位置。定位算法的迭代次数和定位精度很大程度上取决于位置初始值的计算。当初始值偏差较大时，迭代极易陷入局部最优中，定位算法将难以收敛至准确值。

(10)

(11)

式中，Δx和Δy为修正量；

(12)

整合式(11)可得

(13)

式中，

(14)

忽略e，并通过最小二乘法得到位置校正量的估计值：

(15)

式中，Φ为TDOA测量值的协方差矩阵。利用校正值更新估计量：

(16)

重复上述过程直至收敛，收敛条件为

|Δx|+|Δy|<ε

(17)

式中，ε为修正量的最小阈值。

4 UWB定位算法仿真分析

为了验证上述定位算法的准确性和稳定性，下面以Matlab(CPU为Intel i7-8565U)为仿真环境对算法进行仿真分析。

4.1 仿真方案设计

首先，假设TDOA测量误差的标准差σUWB为3 cm，利用LSM-Taylor级联定位算法和多基站LSM定位算法分别进行两次仿真，每次仿真包含100组数据。

4.2 仿真结果

仿真结果俯视图如图3和图4所示，图3和图4中每个点的横纵坐标代表了定位算法得到的位置。由图3和图4可知，两种算法的移动站位置都聚集在真实值附近，且LSM-Taylor级联定位算法的结果分布更加紧密，定位精度更高。多基站LSM定位算法解算每组数据的时间为12 μs，而LSM-Taylor级联定位算法采用多次迭代的方式，定位精度较高但计算量会相对较大，其解算每组数据的时间为58 μs。由此可见，两种算法都能满足UWB实时定位的要求。

图3 定位点(1 m,3 m,0)仿真结果俯视图

图4 定位点(5 m,2 m,0)仿真结果俯视图

4.3 定位精度的评估

均方根误差(Root Mean Squared Error，RMSE)可以评价数据的变化程度，RMSE的值越小，说明定位系统的精确度越高。其计算公式为

(18)

接下来，针对定位点(1 m,3 m,0)，以RMSE为评估标准，定量分析TDOA测量误差的标准差σUWB对两种算法定位精度的影响，其结果如图5所示。由图5可知，随着误差标准差σUWB的增大，两种算法的定位误差均逐渐增大，但LSM-Taylor级联定位算法的误差较小，保持在10 cm以内，且增大的幅度也较小，受误差标准差σUWB的影响较小，具有更高的精度和鲁棒性。

图5 TDOA测量误差的标准差对定位精度的影响

5 UWB定位算法实验与分析

5.1 实验环境设计

定位元件选择DW1000芯片，它是由Decawave公司推出的超宽带定位芯片，符合IEEE 802.15.4-2011超宽带标准。DW1000作为一款定位芯片，其功耗低，输出频率为100 Hz，可双向测距和定位，测距噪声标准差为6 cm，最大测距距离可达200 m，定位精度达到cm级。

实验场地设置在同济大学机械与能源工程学院地下车库。为了减小室内物体遮挡造成的NLOS误差，将UWB基站的高度设置为3 m。如图6所示，4个UWB基站分别布置在正方形的4个顶点上，其坐标依次为 (0,0,3 m)，(0,10 m,3 m)，(10 m,10 m,3 m)，(10 m,0,3 m)。7个静态定位点布置在高度为2 m的水平面内，其中4个均匀分布在正方形区域内，另外4个分别布置在正方形区域的中心、边上、顶点附近和外部。

图6 静态定位实验UWB布置俯视图

5.2 实验结果与分析

UWB模块的输出频率为100 Hz，在每个定位点静止采集10 s的TDOA信息，并利用LSM-Taylor级联定位算法对其解算，以距离真实位置的最大偏差和RMSE为定位精度的评估标准。定位结果统计如表1所示。

表1 静态定位实验统计结果

由表1可知，1#～4#定位点的最大偏差和RMSE近似，且明显大于5#定位点，小于6#、7#和8#定位点。随着定位点与正方形中心的距离变大，定位点的定位精度下降。当定位点距离中心较近时，其RMSE在10 cm以内，符合室内定位的精度要求；当定位点位于顶点附近或正方形外部时，其精度大幅度下降。因此，移动站的运动范围应尽量处于UWB基站多边形区域的内部，从而保证室内定位的高精度。

6 结束语

以TDOA为定位方式，提出了一种基于UWB的LSM-Taylor级联车辆定位算法。首先利用TDOA构造非线性方程组，然后通过LSM求解车辆的估计位置，最后通过Taylor级数迭代获得车辆的精确位置。在仿真结果中，相比LSM定位算法，LSM-Taylor级联定位算法的定位结果分布更加紧密，定位精度更高。实际测试结果表明，该定位算法的RMSE在10 cm以下，能够满足智能驾驶中的室内定位要求，验证了该方法的有效性，能广泛应用于智能驾驶的室内定位中。