融合 UWB 测距信息的室外高精度定位算法

Posted 2022-08-29 爱是与世界平行

0 引言

智能驾驶汽车需要高精度的稳定定位信息，以满足各场景的应用需求，如碰撞预警、并线等[1-2]。

目前的智能驾驶汽车定位系统高度依赖于差分定位的全球卫星导航系统（global navigation satellitesystem，GNSS），不仅价格昂贵而且在城市环境中极易因信号遮挡而引起误差，造成车载定位系统在城市环境下定位误差的增大和稳定性的降低[3]。

惯性导航系统（inertial navigation system, INS）利用惯性敏感元件来推算载体的位置、速度和姿态，可实现自主定位，但其误差会随时间累积以致定位精度随时间发散。作为 GNSS 定位技术的 1 种，单点定位模式的全球定位系统（global positioning system, GPS）定位精度较低，但能够提供全方位、全天候的定位信息且价格较低。基于扩展卡尔曼滤波的 GPS/INS 组合定位算法结合 GPS 和 INS 的误差特性保证定位信号的稳定输出，但其定位误差难以达到分米级[4]。

地图匹配是 1 种软件纠错技术，将车辆行驶轨迹与高精度电子地图进行匹配，从而确定车辆在地图中的位置[5]。但现有地图匹配技术存在鲁棒性差、可用路标有限等缺点，且大部分地图匹配技术主要修正沿车道方向的纵向定位误差，而对相对于车道的横向定位研究还较少[6]。实际上，横向定位精度对车辆的行驶安全起着至关重要的作用，如超车和并道过程中的 2 车横向间距一般不超过1 m。考虑到车辆沿车道行驶时，行驶轨迹基本平行于车道线，且与车道线距离保持稳定，因此可以通过车辆的历史轨迹推算出车辆的未来行驶轨迹，即航迹。本文将提出 1 种推算航迹的方法，并融合航迹对车辆进行横向定位。

在室内定位领域，超宽带（ultra wideband, UWB）技术作为 1 种无线电技术，将信号传输时间转换为距离信息，可以实现厘米级的定位精度，但主流的基于到达时间（time of arrival, TOA）和基于达到时间差（time difference of arrival, TDOA）的定位技术都至少需要设置 3 个基站，硬件成本较高且系统复杂[7]。在立交桥、树荫遮挡等复杂城市道路环境下，GNSS 定位精度严重下降，本文将利用 UWB 技术辅助 GPS/INS 组合定位系统，既降低成本又实现高精度的室外定位。

综上所述，本文在高精度车道线数据库和低精度 GPS/INS 组合定位的基础上提出 1 种融合 UWB测距信息的室外高精度定位算法。

以 GPS/INS 组合定位信号作为稳定原始输出，针对沿车道行驶和路口转弯 2 种情况分别提出单基站航迹匹配和扩展卡尔曼滤波（extended Kalman filter, EKF）组合定位算法。

1 定位算法方案与框架

UWB 是 1 种无载波通信技术，利用微秒至纳秒级的非正弦波窄脉冲传输数据，可达到厘米级的测距精度，并且还具有处理增益高、输出功率低、抗干扰能力强、传输距离远等特点，因此可将其应用于室外定位中。融合 UWB 测距信息的室外高精度定位算法方案如图 1 所示。

为了保证稳定输出，将 GPS/INS 组合定位信号作为原始输出，其中包括位置、速度和航向角。

因为车辆在路口转弯时不存在起引导作用的车道线，因此其定位方案与沿车道行驶时有所不同。实车实验中的航向角变化率如图 2 所示。

由实验结果可知，当车辆沿车道行驶时，大部分行驶时间内航向角变化率稳定在 2(°)/s 以内，因此可将其作为判断车辆行驶状态的基准。当航向角保持稳定或微小变化时，可判定车辆处于沿车道行驶阶段，通过车辆行驶历史轨迹计算车辆在车道上的横向偏移量，根据车道线数据库确定车辆行驶航迹，最后结合 UWB 测距信息进行单基站航迹匹配[8]；当航向角发生较大变化时，可判定车辆处于路口转弯阶段，由于 UWB 测距信息与车辆的位置存在非线性关系，因此通过EKF 结合原始定位数据和 UWB 测距信息进行组合定位[9]。

另外，为了保证 UWB 的测距精度，应将 UWB基站布置在相对空旷的环境下，抑制其非视距误差。

2 单基站航迹匹配

本文根据 UWB 的高精度测距特性提出了 1 种应用于室外定位的单基站航迹匹配算法，主要包括融合横向位移的航迹推算和融合纵向位移的圆交点求解。

2.1 航迹推算

由驾驶员驾驶习惯可知，沿车道行驶时其行驶轨迹平行于车道线，且与车道线的横向距离保持稳定，因此本算法设计在每个路口放置 1 个 UWB基站，同时在车辆顶部安装 UWB 移动站，通过计算车辆在进入车道后稳定行驶 1 段距离的平均横向距离推算出未来行驶的航迹。

为了提高航迹推算的简便性和精确性，根据行驶策略将处于交叉路口的车辆分为 2 种情况：

直行（如路径 1）和路口转弯（如路径 2），如图3 所示。由 UWB 基站的经纬度坐标与车道线数据库坐标点集计算 UWB 基站与车道线之间的距离 d2。

2.1.1 直行时的航迹推算

以图 3 中路径 1 为例，车辆直行通过路口时，车辆进入推算区域 1 后稳定行驶，其 UWB 基站与车辆 UWB 移动站之间的距离特征曲线如图 4所示。可取距离特征曲线的最低点作为行驶航迹与 UWB 基站之间的距离 d1，则行驶航迹与车道中心线之间的距离为 d=d2*-**d*1（设定车辆在车道中心线以右时 d 为正值）。最后将车道中心线向右横向平移距离 d，即可得到行驶航迹，即路径 1（虚线部分）。

2.1.2 路口转弯后的航迹推算

以图 3 中路径 2 为例，车辆转弯通过路口后，车辆进入推算区域 2 后稳定行驶。此时，UWB 基站、行驶航迹和车辆之间的几何关系可以简化为图 5 所示几何关系。

根据勾股定理可得

2.2 圆交点求解

车辆确定行驶航迹后，即可通过圆交点求解确定其纵向位移。如图 2 所示，在 t 时刻，以 UWB基站坐标( x_uwb , y_uwb)为圆点，UWB 的测距特征 L(t)为半径作圆，与行驶航迹的交点即为车辆位置。联立方程为

通常情况下，上述方法将求得多个圆交点。根据 d 的正负性判断车辆在车道中心线的左侧还是右侧。在匹配的初始化阶段，将靠近 GPS/INS 定位坐标的交点作为匹配起始点；在后续的匹配过程中，则采用与上 1 次匹配位置最近的交点。

3 EKF 组合定位算法

4 实验与结果分析

为了验证本文提出的定位算法的合理性及其高定位精度，在 4 个路口各安装 1 个 UWB 基站且安装高度与车载 UWB 移动站一致，围绕同济大学机械学院进行实车实验。其中，GPS 接收机为 u-blox LEA-6H，MEMS 传感器为 MPU6050，UWB 模块为 DW1000。实验时以 RT3000 高精度定位模块的定位数据作为车辆的参考定位轨迹，进而比较融合本文定位算法的定位轨迹与标定轨迹的偏差。

图 6 和图 7 分别为 GPS/INS 组合定位算法和本文定位算法的横向和纵向误差曲线。GPS/INS 组合定位算法的横向误差和纵向误差平均值分别为1.07 和 4.73 m。而本文定位算法大部分横向误差和纵向误差均在 1 m 以内，且平均误差分别为 0.24 和0.4 m，满足智能驾驶中的分米级高精度定位要求。

但在弯道处有较大的偏差，这是因为在沿车道行驶时，车辆的定位精度高度依赖于 UWB 的厘米级测距精度和高精度车道线数据库，所以最终的精度也能保证在 40 cm 以下。而在邻近弯道处车辆的定位精度由低精度的 GPS/INS 组合定位信号和UWB 模块共同决定，因此相对于沿车道行驶的定位精度较低。

实车实验轨迹如图 8 所示，其局部放大图如图 9 所示。GPS/INS 组合定位算法与标定轨迹存在较大偏差，融合 UWB 测距信息的室外高精度定位轨迹与轨迹基本重合，证明了本文定位算法的优越性和可靠性。

5 结束语

本文在 GPS/INS 组合定位和 UWB 技术的基础上提出了 1 种融合 UWB 测距信息的室外高精度定位算法。在沿车道线行驶情况下，综合驾驶员的驾驶习惯和 UWB 的高精度测距特性提出了单基站航迹匹配；在弯道情况下，利用 EKF 融合GPS/INS 组合定位原始输出和 UWB 的测距信息。

实车实验结果表明，该算法准确可靠，能广泛应用于智能驾驶的导航定位中。